/* vectomat function obtained from constrain.c and modified to work with MOTO library */
static MT_Matrix3x3 vectomat(MT_Vector3 vec, short axis, short upflag, short threedimup)
{
MT_Matrix3x3 mat;
MT_Vector3 y(MT_Scalar(0.0f), MT_Scalar(1.0f), MT_Scalar(0.0f));
MT_Vector3 z(MT_Scalar(0.0f), MT_Scalar(0.0f), MT_Scalar(1.0f)); /* world Z axis is the global up axis */
MT_Vector3 proj;
MT_Vector3 right;
MT_Scalar mul;
int right_index;
/* Normalized Vec vector*/
vec = vec.safe_normalized_vec(z);
/* if 2D doesn't move the up vector */
if (!threedimup) {
vec.setValue(MT_Scalar(vec[0]), MT_Scalar(vec[1]), MT_Scalar(0.0f));
vec = (vec - z.dot(vec)*z).safe_normalized_vec(z);
}
if (axis > 2)
axis -= 3;
else
vec = -vec;
/* project the up vector onto the plane specified by vec */
/* first z onto vec... */
mul = z.dot(vec) / vec.dot(vec);
proj = vec * mul;
/* then onto the plane */
proj = z - proj;
/* proj specifies the transformation of the up axis */
proj = proj.safe_normalized_vec(y);
/* Normalized cross product of vec and proj specifies transformation of the right axis */
right = proj.cross(vec);
right.normalize();
if (axis != upflag) {
right_index = 3 - axis - upflag;
/* account for up direction, track direction */
right = right * basis_cross(axis, upflag);
mat.setRow(right_index, right);
mat.setRow(upflag, proj);
mat.setRow(axis, vec);
mat = mat.inverse();
}
/* identity matrix - don't do anything if the two axes are the same */
else {
mat.setIdentity();
}
return mat;
}
static basis inertia_contrib(point o, point com)
{
basis a;
a = basis_cross(point_sub(o, com));
return basis_cat(a, a);
}