public static warp_Matrix quaternionMatrix( warp_Quaternion quat )
{
warp_Matrix m = new warp_Matrix();
float xx = quat.X * quat.X;
float xy = quat.X * quat.Y;
float xz = quat.X * quat.Z;
float xw = quat.X * quat.W;
float yy = quat.Y * quat.Y;
float yz = quat.Y * quat.Z;
float yw = quat.Y * quat.W;
float zz = quat.Z * quat.Z;
float zw = quat.Z * quat.W;
m.m00 = 1 - 2 * ( yy + zz );
m.m01 = 2 * ( xy - zw) ;
m.m02 = 2*(xz + yw);
m.m10 = 2*(xy + zw);
m.m11 = 1 - 2* ( xx + zz );
m.m12 = 2*(yz - xw);
m.m20 = 2*(xz - yw);
m.m21 = 2*(yz + xw);
m.m22 = 1 - 2 * ( xx + yy );
m.m03 = m.m13 = m.m23 = m.m30 = m.m31 = m.m32 = 0;
m.m33 = 1;
return m;
}
public static warp_Matrix quaternionMatrix(warp_Quaternion quat)
{
warp_Matrix m = new warp_Matrix();
float xx = quat.X * quat.X;
float xy = quat.X * quat.Y;
float xz = quat.X * quat.Z;
float xw = quat.X * quat.W;
float yy = quat.Y * quat.Y;
float yz = quat.Y * quat.Z;
float yw = quat.Y * quat.W;
float zz = quat.Z * quat.Z;
float zw = quat.Z * quat.W;
m.m00 = 1 - 2 * (yy + zz);
m.m01 = 2 * (xy - zw);
m.m02 = 2 * (xz + yw);
m.m10 = 2 * (xy + zw);
m.m11 = 1 - 2 * (xx + zz);
m.m12 = 2 * (yz - xw);
m.m20 = 2 * (xz - yw);
m.m21 = 2 * (yz + xw);
m.m22 = 1 - 2 * (xx + yy);
m.m03 = m.m13 = m.m23 = m.m30 = m.m31 = m.m32 = 0;
m.m33 = 1;
return(m);
}
public warp_Matrix rotateMatrix(warp_Quaternion quat)
{
reset();
warp_Matrix temp = warp_Matrix.quaternionMatrix(quat);
warp_Matrix result = warp_Matrix.multiply(this, temp);
return(result);
}
public static warp_Quaternion Ode2WarpQuaternion( Ode.Quaternion odeQ )
{
warp_Quaternion q = new warp_Quaternion(
( float )odeQ.X,
( float )odeQ.Y,
( float )odeQ.Z,
( float )odeQ.W );
return q;
}
public static Ode.Quaternion Warp2OdeQuaternion( warp_Quaternion q )
{
Ode.Quaternion odeQ = new Ode.Quaternion();
odeQ.W = q.W;
odeQ.X = q.X;
odeQ.Y = q.Y;
odeQ.Z = q.Z;
return odeQ;
}
static public warp_Quaternion matrix(warp_Matrix xfrm)
{
warp_Quaternion quat = new warp_Quaternion();
// Check the sum of the diagonal
float tr = xfrm[0, 0] + xfrm[1, 1] + xfrm[2, 2];
if (tr > 0.0f)
{
// The sum is positive
// 4 muls, 1 div, 6 adds, 1 trig function call
float s = ( float )Math.Sqrt(tr + 1.0f);
quat.W = s * 0.5f;
s = 0.5f / s;
quat.X = (xfrm[1, 2] - xfrm[2, 1]) * s;
quat.Y = (xfrm[2, 0] - xfrm[0, 2]) * s;
quat.Z = (xfrm[0, 1] - xfrm[1, 0]) * s;
}
else
{
// The sum is negative
// 4 muls, 1 div, 8 adds, 1 trig function call
int[] nIndex = { 1, 2, 0 };
int i, j, k;
i = 0;
if (xfrm[1, 1] > xfrm[i, i])
{
i = 1;
}
if (xfrm[2, 2] > xfrm[i, i])
{
i = 2;
}
j = nIndex[i];
k = nIndex[j];
float s = ( float )Math.Sqrt((xfrm[i, i] - (xfrm[j, j] + xfrm[k, k])) + 1.0f);
quat[i] = s * 0.5f;
if (s != 0.0)
{
s = 0.5f / s;
}
quat[j] = (xfrm[i, j] + xfrm[j, i]) * s;
quat[k] = (xfrm[i, k] + xfrm[k, i]) * s;
quat[3] = (xfrm[j, k] - xfrm[k, j]) * s;
}
return(quat);
}
static public warp_Quaternion matrix(warp_Matrix xfrm)
{
warp_Quaternion quat = new warp_Quaternion();
// Check the sum of the diagonal
float tr = xfrm[0, 0] + xfrm[1, 1] + xfrm[2, 2];
if (tr > 0.0f)
{
// The sum is positive
// 4 muls, 1 div, 6 adds, 1 trig function call
float s = (float)Math.Sqrt(tr + 1.0f);
quat.W = s * 0.5f;
s = 0.5f / s;
quat.X = (xfrm[1, 2] - xfrm[2, 1]) * s;
quat.Y = (xfrm[2, 0] - xfrm[0, 2]) * s;
quat.Z = (xfrm[0, 1] - xfrm[1, 0]) * s;
}
else
{
// The sum is negative
// 4 muls, 1 div, 8 adds, 1 trig function call
int[] nIndex = { 1, 2, 0 };
int i, j, k;
i = 0;
if (xfrm[1, 1] > xfrm[i, i])
i = 1;
if (xfrm[2, 2] > xfrm[i, i])
i = 2;
j = nIndex[i];
k = nIndex[j];
float s = (float)Math.Sqrt((xfrm[i, i] - (xfrm[j, j] + xfrm[k, k])) + 1.0f);
quat[i] = s * 0.5f;
if (s != 0.0)
{
s = 0.5f / s;
}
quat[j] = (xfrm[i, j] + xfrm[j, i]) * s;
quat[k] = (xfrm[i, k] + xfrm[k, i]) * s;
quat[3] = (xfrm[j, k] - xfrm[k, j]) * s;
}
return quat;
}
public void rotate( warp_Quaternion quat, float x, float y, float z )
{
matrix.rotate( quat , x, y, z);
normalmatrix.rotate( quat , x, y, z);
}
public void rotate( warp_Quaternion quat, float x, float y, float z )
{
transform( rotateMatrix( quat ) );
}
public bool RotateSelf( string name, warp_Quaternion quat )
{
if ( _scene == null )
{
return false;
}
warp_Object o = _scene.sceneobject( name );
if ( o == null )
{
return false;
}
o.rotateSelf( quat );
return true;
}
public void rotateSelf(warp_Quaternion quat)
{
matrix.rotateSelf(quat);
normalmatrix.rotateSelf(quat);
}
public warp_Matrix rotateMatrix( warp_Quaternion quat )
{
reset();
warp_Matrix temp = warp_Matrix.quaternionMatrix( quat );
warp_Matrix result = warp_Matrix.multiply( this, temp );
return result;
}
public void rotate(warp_Quaternion quat)
{
transform(rotateMatrix(quat));
}
public void rotate(warp_Quaternion quat, float x, float y, float z)
{
transform(rotateMatrix(quat));
}
public void rotateSelf( warp_Quaternion quat )
{
matrix.rotateSelf( quat );
normalmatrix.rotateSelf( quat );
}
public void rotateSelf(warp_Quaternion quat)
{
preTransform(rotateMatrix(quat));
}
public bool RotateScene( warp_Quaternion quat )
{
if ( _scene == null )
{
return false;
}
_scene.rotate( quat );
return true;
}
public void rotate( warp_Quaternion quat)
{
transform( rotateMatrix( quat ) );
}
public bool RotateScene( warp_Quaternion quat, float x, float y, float z )
{
if ( _scene == null )
{
return false;
}
_scene.rotate( quat, x, y, z );
return true;
}
public void rotateSelf( warp_Quaternion quat )
{
preTransform(rotateMatrix( quat) );
}
public void rotate(warp_Quaternion quat, float x, float y, float z)
{
matrix.rotate(quat, x, y, z);
normalmatrix.rotate(quat, x, y, z);
}
