trMatrix33d enMissionFunctionMgr::FindRotMatrix(trVector3d &from, trVector3d &to)
{

	from.Normalize();
	to.Normalize();

	trVector3d vs = Cross(from, to);

	trVector3d v(vs);
	v.Normalize();

	double ca = from.Dot(to);

	trVector3d vt(v*(1.0 - ca));

	trMatrix33d rotM;
	rotM.Set(0, vt.x * v.x + ca);
	rotM.Set(4, vt.y * v.y + ca);
	rotM.Set(8, vt.z * v.z + ca);

	vt.x *= v.y;
	vt.z *= v.x;
	vt.y *= v.z;
	
	rotM.Set(1, vt.x - vs.z);
	rotM.Set(2, vt.z + vs.y);
	rotM.Set(3, vt.x + vs.z);
	rotM.Set(5, vt.y - vs.x);
	rotM.Set(6, vt.z - vs.y);
	rotM.Set(7, vt.y + vs.x);

	return rotM;
}

trVector3d PolyNorm = trVector3d(normalX, normalY, normalZ);trMatrix33d currMatZ;currMatZ.RotationZ(-entityYaw);//create orientation of terrain contact planetrMatrix33d orient;PolyNorm = currMatZ * PolyNorm;orient = FindRotMatrix(trVector3d(0.0, 0.0, 1.0), PolyNorm);entityPitch = asin(orient(0,2));entityRoll = atan2( orient(1,2), orient(2,2));

// Optimized version from Stan Melax (GPG):inline Quat4 rotationArcMelax(const Vec3 & a,const Vec3 & b) {  Vec3 c  = a.cross(b);  flt  ss = (1.f+a.dot(b))*2.f;  flt  rs = sqrtInverse(ss);  flt  s  = ss*rs; // s = sqrt(ss).  return Quat4(c.x*rs,c.y*rs,c.z*rs,s*.5f);} // rotationArcMelax