CustomLimitBallAndSocket::CustomLimitBallAndSocket(const dMatrix& childPinAndPivotFrame, NewtonBody* const child, const dMatrix& parentPinAndPivotFrame, NewtonBody* const parent)
:CustomBallAndSocket(childPinAndPivotFrame, child, parent)
,m_rotationOffset(childPinAndPivotFrame * parentPinAndPivotFrame.Inverse())
{
SetConeAngle (0.0f);
SetTwistAngle (0.0f, 0.0f);
dMatrix matrix;
CalculateLocalMatrix (parentPinAndPivotFrame, matrix, m_localMatrix1);
}
dQuaternion::dQuaternion (const dMatrix &matrix)
{
enum QUAT_INDEX
{
X_INDEX=0,
Y_INDEX=1,
Z_INDEX=2
};
static QUAT_INDEX QIndex [] = {Y_INDEX, Z_INDEX, X_INDEX};
dFloat trace = matrix[0][0] + matrix[1][1] + matrix[2][2];
dAssert (((matrix[0] * matrix[1]) % matrix[2]) > 0.0f);
if (trace > dFloat(0.0f)) {
trace = dSqrt (trace + dFloat(1.0f));
m_q0 = dFloat (0.5f) * trace;
trace = dFloat (0.5f) / trace;
m_q1 = (matrix[1][2] - matrix[2][1]) * trace;
m_q2 = (matrix[2][0] - matrix[0][2]) * trace;
m_q3 = (matrix[0][1] - matrix[1][0]) * trace;
} else {
QUAT_INDEX i = X_INDEX;
if (matrix[Y_INDEX][Y_INDEX] > matrix[X_INDEX][X_INDEX]) {
i = Y_INDEX;
}
if (matrix[Z_INDEX][Z_INDEX] > matrix[i][i]) {
i = Z_INDEX;
}
QUAT_INDEX j = QIndex [i];
QUAT_INDEX k = QIndex [j];
trace = dFloat(1.0f) + matrix[i][i] - matrix[j][j] - matrix[k][k];
trace = dSqrt (trace);
dFloat* const ptr = &m_q1;
ptr[i] = dFloat (0.5f) * trace;
trace = dFloat (0.5f) / trace;
m_q0 = (matrix[j][k] - matrix[k][j]) * trace;
ptr[j] = (matrix[i][j] + matrix[j][i]) * trace;
ptr[k] = (matrix[i][k] + matrix[k][i]) * trace;
}
#if _DEBUG
dMatrix tmp (*this, matrix.m_posit);
dMatrix unitMatrix (tmp * matrix.Inverse());
for (int i = 0; i < 4; i ++) {
dFloat err = dAbs (unitMatrix[i][i] - dFloat(1.0f));
dAssert (err < dFloat (1.0e-3f));
}
dFloat err = dAbs (DotProduct(*this) - dFloat(1.0f));
dAssert (err < dFloat(1.0e-3f));
#endif
}
void ConvexApproximationObject::LoadGeometries (NewtonMesh* const meshOut, const dMatrix& matrix)
{
INode* stack[1024];
ConvexApproximationClassDesc* const desc = (ConvexApproximationClassDesc*) ConvexApproximationClassDesc::GetDescriptor();
INode* const sourceNode = desc->m_sourceNode;
_ASSERTE (sourceNode);
stack[0] = sourceNode;
int stackIndex = 1;
dMatrix orthogonalRootTransform (matrix.Inverse());
while (stackIndex) {
stackIndex --;
INode* const node = stack[stackIndex];
// Get Transformation matrix at frame 0
//dMatrix matrix (GetMatrixFromMaxMatrix (node->GetNodeTM (0)) * orthogonalRootTransform);
dMatrix matrix (GetMatrixFromMaxMatrix (node->GetObjectTM(0)) * orthogonalRootTransform);
//nodeMap.Insert(sceneNode, node);
ObjectState os (node->EvalWorldState(0));
// The obj member of ObjectState is the actual object we will export.
if (os.obj) {
// We look at the super class ID to determine the type of the object.
switch(os.obj->SuperClassID())
{
case GEOMOBJECT_CLASS_ID:
{
if (!node->GetBoneNodeOnOff()) {
AddPolygonFromObject(node, &os, meshOut, matrix);
}
break;
}
}
}
for (int i = 0; i < node->NumberOfChildren(); i ++) {
stack[stackIndex] = node->GetChildNode(i);
stackIndex ++;
_ASSERTE (stackIndex * sizeof (INode*) < sizeof (stack));
}
}
}
void dCustomCorkScrew::SubmitAngularRow(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
dMatrix localMatrix(matrix0 * matrix1.Inverse());
dVector euler0;
dVector euler1;
localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw);
dVector rollPin(dSin(euler0[1]), dFloat(0.0f), dCos(euler0[1]), dFloat(0.0f));
rollPin = matrix1.RotateVector(rollPin);
NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddAngularRow(m_joint, -euler0[2], &rollPin[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
// the joint angle can be determined by getting the angle between any two non parallel vectors
m_curJointAngle.Update(euler0.m_x);
// save the current joint Omega
dVector omega0(0.0f);
dVector omega1(0.0f);
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
m_angularOmega = (omega0 - omega1).DotProduct3(matrix1.m_front);
if (m_options.m_option2) {
if (m_options.m_option3) {
dCustomCorkScrew::SubmitConstraintLimitSpringDamper(matrix0, matrix1, timestep);
} else {
dCustomCorkScrew::SubmitConstraintLimits(matrix0, matrix1, timestep);
}
} else if (m_options.m_option3) {
dCustomCorkScrew::SubmitConstraintSpringDamper(matrix0, matrix1, timestep);
} else if (m_angularFriction != 0.0f) {
NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowAcceleration(m_joint, -m_angularOmega / timestep);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
}
}
