|
|
发表于 2009-8-5 10:35:00
|
显示全部楼层
Re:求根据变换矩阵求三分量（平移 旋转(四元数) 缩放）方
microsoft matrix.decompose() source code(c#) in xna framework:
public unsafe bool Decompose(out Vector3 scale, out Quaternion rotation, out Vector3 translation)
{
bool flag = true;
fixed (float* numRef = &scale.X)
{
uint num;
uint num3;
uint num4;
VectorBasis basis2;
Vector3** vectorPtr = (Vector3**) &basis2;
Matrix identity = Identity;
CanonicalBasis basis = new CanonicalBasis();
Vector3* vectorPtr2 = &basis.Row0;
basis.Row0 = new Vector3(1f, 0f, 0f);
basis.Row1 = new Vector3(0f, 1f, 0f);
basis.Row2 = new Vector3(0f, 0f, 1f);
translation.X = this.M41;
translation.Y = this.M42;
translation.Z = this.M43;
*((IntPtr*) (vectorPtr + 1)) = (IntPtr) &identity.M11;
*((IntPtr*) (vectorPtr + 1)) = (IntPtr) &identity.M21;
*((IntPtr*) (vectorPtr + 2)) = (IntPtr) &identity.M31;
*(*((IntPtr*) (vectorPtr + 1))) = new Vector3(this.M11, this.M12, this.M13);
*(*((IntPtr*) (vectorPtr + 1))) = new Vector3(this.M21, this.M22, this.M23);
*(*((IntPtr*) (vectorPtr + 2))) = new Vector3(this.M31, this.M32, this.M33);
scale.X = *(((IntPtr*) (vectorPtr + 1))).Length();
scale.Y = *(((IntPtr*) (vectorPtr + 1))).Length();
scale.Z = *(((IntPtr*) (vectorPtr + 2))).Length();
float num11 = numRef[0];
float num10 = numRef[4];
float num7 = numRef[8];
if (num11 < num10)
{
if (num10 < num7)
{
num = 2;
num3 = 1;
num4 = 0;
}
else
{
num = 1;
if (num11 < num7)
{
num3 = 2;
num4 = 0;
}
else
{
num3 = 0;
num4 = 2;
}
}
}
else if (num11 < num7)
{
num = 2;
num3 = 0;
num4 = 1;
}
else
{
num = 0;
if (num10 < num7)
{
num3 = 2;
num4 = 1;
}
else
{
num3 = 1;
num4 = 2;
}
}
if (numRef[num * 4] < 0.0001f)
{
*(*((IntPtr*) (vectorPtr + (num * sizeof(Vector3*))))) = vectorPtr2[(int) (num * sizeof(Vector3))];
}
*(((IntPtr*) (vectorPtr + (num * sizeof(Vector3*))))).Normalize();
if (numRef[num3 * 4] < 0.0001f)
{
uint num5;
float num9 = Math.Abs(*(((IntPtr*) (vectorPtr + (num * sizeof(Vector3*))))).X);
float num8 = Math.Abs(*(((IntPtr*) (vectorPtr + (num * sizeof(Vector3*))))).Y);
float num6 = Math.Abs(*(((IntPtr*) (vectorPtr + (num * sizeof(Vector3*))))).Z);
if (num9 < num8)
{
if (num8 < num6)
{
num5 = 0;
}
else if (num9 < num6)
{
num5 = 0;
}
else
{
num5 = 2;
}
}
else if (num9 < num6)
{
num5 = 1;
}
else if (num8 < num6)
{
num5 = 1;
}
else
{
num5 = 2;
}
Vector3.Cross(ref (Vector3) ref *(((IntPtr*) (vectorPtr + (num3 * sizeof(Vector3*))))), ref (Vector3) ref *(((IntPtr*) (vectorPtr + (num * sizeof(Vector3*))))), out (Vector3) ref (vectorPtr2 + (num5 * sizeof(Vector3))));
}
*(((IntPtr*) (vectorPtr + (num3 * sizeof(Vector3*))))).Normalize();
if (numRef[num4 * 4] < 0.0001f)
{
Vector3.Cross(ref (Vector3) ref *(((IntPtr*) (vectorPtr + (num4 * sizeof(Vector3*))))), ref (Vector3) ref *(((IntPtr*) (vectorPtr + (num * sizeof(Vector3*))))), out (Vector3) ref *(((IntPtr*) (vectorPtr + (num3 * sizeof(Vector3*))))));
}
*(((IntPtr*) (vectorPtr + (num4 * sizeof(Vector3*))))).Normalize();
float num2 = identity.Determinant();
if (num2 < 0f)
{
numRef[num * 4] = -numRef[num * 4];
*(*((IntPtr*) (vectorPtr + (num * sizeof(Vector3*))))) = Vector3.op_UnaryNegation(*((Vector3*) *(((IntPtr*) (vectorPtr + (num * sizeof(Vector3*)))))));
num2 = -num2;
}
num2--;
num2 *= num2;
if (0.0001f < num2)
{
rotation = Quaternion.Identity;
flag = false;
}
else
{
Quaternion.CreateFromRotationMatrix(ref identity, out rotation);
}
}
return flag;
}
|
|
楼主