//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
CustomGear::CustomGear(
dFloat gearRatio,
const dVector& childPin,
const dVector& parentPin,
NewtonBody* child,
NewtonBody* parent)
:NewtonCustomJoint(1, child, parent)
{
m_gearRatio = gearRatio;
// calculate the two local matrix of the pivot point
// dVector pivot (0.0f, 0.0f, 0.0f);
dMatrix dommyMatrix;
// calculate the local matrix for body body0
dMatrix pinAndPivot0 (dgGrammSchmidt(childPin));
// CalculateLocalMatrix (pivot, childPin, m_localMatrix0, dommyMatrix);
CalculateLocalMatrix (pinAndPivot0, m_localMatrix0, dommyMatrix);
// calculate the local matrix for body body1
dMatrix pinAndPivot1 (dgGrammSchmidt(parentPin));
// CalculateLocalMatrix (pivot, parentPin, dommyMatrix, m_localMatrix1);
CalculateLocalMatrix (pinAndPivot1, dommyMatrix, m_localMatrix1);
}
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
CustomWormGear::CustomWormGear(
dFloat gearRatio,
const dVector& rotationalPin,
const dVector& linearPin,
NewtonBody* rotationalBody,
NewtonBody* linearBody)
:NewtonCustomJoint(1, rotationalBody, linearBody)
{
m_gearRatio = gearRatio;
// calculate the two local matrix of the pivot point
// dVector pivot (0.0f, 0.0f, 0.0f);
dMatrix dommyMatrix;
// calculate the local matrix for body body0
dMatrix pinAndPivot0 (dgGrammSchmidt (rotationalPin));
// CalculateLocalMatrix (pivot, rotationalPin, m_localMatrix0, dommyMatrix);
CalculateLocalMatrix (pinAndPivot0, m_localMatrix0, dommyMatrix);
// calculate the local matrix for body body1
// _ASSERTE (0);
dMatrix pinAndPivot1 (dgGrammSchmidt(linearPin));
// CalculateLocalMatrix (pivot, linearPin, dommyMatrix, m_localMatrix1);
CalculateLocalMatrix (pinAndPivot1, dommyMatrix, m_localMatrix1);
}
void ShowMousePicking (const dVector& p0, const dVector& p1)
{
dFloat radius = 0.25f;
// set the color of the cube's surface
GLfloat cubeColor[] = { 1.0f, 1.0f, 1.0f, 1.0 };
glMaterialfv(GL_FRONT, GL_SPECULAR, cubeColor);
glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, cubeColor);
glMaterialf(GL_FRONT, GL_SHININESS, 50.0);
// set up the cube's texture
glDisable(GL_TEXTURE_2D);
GLUquadricObj *pObj;
glPushMatrix();
dMatrix matrix (GetIdentityMatrix());
matrix.m_posit = p0;
glMultMatrix(&matrix[0][0]);
// Get a new Quadric off the stack
pObj = gluNewQuadric();
gluQuadricTexture(pObj, true);
gluSphere(pObj, radius, 10, 10);
glPopMatrix();
glPushMatrix();
matrix.m_posit = p1;
glMultMatrix(&matrix[0][0]);
gluSphere(pObj, radius, 10, 10);
gluDeleteQuadric(pObj);
glPopMatrix();
dVector dir (p1 - p0);
dFloat lenght (dir % dir);
if (lenght > 1.0e-2f) {
glPushMatrix();
lenght = sqrt (lenght);
dMatrix align (dgYawMatrix(0.5f * 3.1426f));
align.m_posit.m_x = -lenght * 0.5f;
matrix = align * dgGrammSchmidt(dir);
matrix.m_posit += (p1 + p0).Scale (0.5f);
glMultMatrix(&matrix[0][0]);
// Get a new Quadric off the stack
pObj = gluNewQuadric();
gluCylinder(pObj, radius * 0.5f, radius * 0.5f, lenght, 10, 2);
gluDeleteQuadric(pObj);
glPopMatrix();
}
}
void NewtonCustomJoint::CalculateLocalMatrix (const dVector& pivot, const dVector& dir, dMatrix& localMatrix0, dMatrix& localMatrix1) const
{
dMatrix matrix0;
// Get the global matrices of each rigid body.
NewtonBodyGetMatrix(m_body0, &matrix0[0][0]);
dMatrix matrix1 (GetIdentityMatrix());
if (m_body1) {
NewtonBodyGetMatrix(m_body1, &matrix1[0][0]);
}
// create a global matrix at the pivot point with front vector aligned to the pin vector
dMatrix pinAndPivotMatrix (dgGrammSchmidt(dir));
pinAndPivotMatrix.m_posit = pivot;
pinAndPivotMatrix.m_posit.m_w = 1.0f;
// calculate the relative matrix of the pin and pivot on each body
localMatrix0 = pinAndPivotMatrix * matrix0.Inverse();
localMatrix1 = pinAndPivotMatrix * matrix1.Inverse();
}
void CustomPickBody::SubmitConstrainst (dFloat timestep, int threadIndex)
{
dVector v;
dVector w;
dVector cg;
dMatrix matrix0;
dFloat invTimestep;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
NewtonBodyGetOmega (m_body0, &w[0]);
NewtonBodyGetVelocity (m_body0, &v[0]);
NewtonBodyGetCentreOfMass (m_body0, &cg[0]);
NewtonBodyGetMatrix (m_body0, &matrix0[0][0]);
invTimestep = 1.0f / timestep;
dVector p0 (matrix0.TransformVector (m_localHandle));
dVector pointVeloc = v + w * matrix0.RotateVector (m_localHandle - cg);
dVector relPosit (m_targetPosit - p0);
dVector relVeloc (relPosit.Scale (invTimestep) - pointVeloc);
dVector relAccel (relVeloc.Scale (invTimestep * 0.3f));
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_front);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxLinearFriction);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_up[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_up);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxLinearFriction);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_right[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_right);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxLinearFriction);
if (m_pickMode) {
dFloat mag;
dQuaternion rotation;
NewtonBodyGetRotation (m_body0, &rotation.m_q0);
if (m_targetRot.DotProduct (rotation) < 0.0f) {
rotation.m_q0 *= -1.0f;
rotation.m_q1 *= -1.0f;
rotation.m_q2 *= -1.0f;
rotation.m_q3 *= -1.0f;
}
dVector relOmega (rotation.CalcAverageOmega (m_targetRot, timestep) - w);
mag = relOmega % relOmega;
if (mag > 1.0e-6f) {
dFloat relAlpha;
dFloat relSpeed;
dVector pin (relOmega.Scale (1.0f / mag));
dMatrix basis (dgGrammSchmidt (pin));
relSpeed = dSqrt (relOmega % relOmega);
relAlpha = relSpeed * invTimestep;
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_up[0]);
NewtonUserJointSetRowAcceleration (m_joint, 0.0f);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_right[0]);
NewtonUserJointSetRowAcceleration (m_joint, 0.0f);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
} else {
dVector relAlpha = w.Scale (-invTimestep);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
}
} else {
// this is the single handle pick mode, add soem angular frition
dVector relAlpha = w.Scale (-invTimestep);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction * 0.025f);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction * 0.025f);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction * 0.025f);
}
}
static void DestroyThisBodyCallback (const NewtonBody* body, const NewtonJoint* contactJoint)
{
NewtonWorld* world;
NewtonMesh* topMesh;
NewtonMesh* bottomMesh;
NewtonMesh* effectMesh;
RenderPrimitive* srcPrimitive;
dMatrix matrix;
dFloat maxForce;
dVector point;
dVector dir0;
dVector dir1;
// Get the world;
world = NewtonBodyGetWorld (body);
// find a the strongest force
maxForce = 0.0f;
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
dVector force;
NewtonMaterial* material;
material = NewtonContactGetMaterial (contact);
NewtonMaterialGetContactForce (material, &force.m_x);
if (force.m_x > maxForce) {
dVector normal;
NewtonMaterialGetContactPositionAndNormal(material, &point[0], &normal[0]);
NewtonMaterialGetContactTangentDirections (material, &dir0[0], &dir0[0]);
}
}
// get the visual primitive
srcPrimitive = (RenderPrimitive*) NewtonBodyGetUserData (body);
// get the effect mesh that is use to create the debris pieces
effectMesh = srcPrimitive->m_specialEffect;
// calculate the cut plane plane
NewtonBodyGetMatrix (body, &matrix[0][0]);
dMatrix clipMatrix (dgGrammSchmidt(dir0) *
dYawMatrix(30.0f * 3.1416f/180.0f * RandomVariable(1.0f)) *
dRollMatrix(30.0f * 3.1416f/180.0f * RandomVariable(1.0f)));
clipMatrix.m_posit = point;
clipMatrix.m_posit.m_w = 1.0f;
clipMatrix = clipMatrix * matrix.Inverse();
// break the mesh into two pieces
NewtonMeshClip (effectMesh, meshClipper, &clipMatrix[0][0], &topMesh, &bottomMesh);
if (topMesh && bottomMesh) {
dFloat volume;
NewtonMesh* meshPartA = NULL;
NewtonMesh* meshPartB = NULL;
volume = NewtonConvexCollisionCalculateVolume (NewtonBodyGetCollision(body));
// the clipper was able to make a cut now we can create new debris piece for replacement
dMatrix clipMatrix1 (dgGrammSchmidt(dir1) *
dYawMatrix(30.0f * 3.1416f/180.0f * RandomVariable(1.0f)) *
dRollMatrix(30.0f * 3.1416f/180.0f * RandomVariable(1.0f)));
NewtonMeshClip (bottomMesh, meshClipper, &clipMatrix1[0][0], &meshPartA, &meshPartB);
if (meshPartA && meshPartB) {
// creat another split (you can make as many depend of the FPS)
CreateDebriPiece (body, meshPartA, volume);
CreateDebriPiece (body, meshPartB, volume);
NewtonMeshDestroy(meshPartA);
NewtonMeshDestroy(meshPartB);
} else {
CreateDebriPiece (body, bottomMesh, volume);
}
NewtonMeshDestroy(bottomMesh);
dMatrix clipMatrix2 (dgGrammSchmidt(dir1) *
dYawMatrix(30.0f * 3.1416f/180.0f * RandomVariable(1.0f)) *
dRollMatrix(30.0f * 3.1416f/180.0f * RandomVariable(1.0f)));
NewtonMeshClip (topMesh, meshClipper, &clipMatrix2[0][0], &meshPartA, &meshPartB);
if (meshPartA && meshPartB) {
// creat another split (you can make as many depend of the FPS)
CreateDebriPiece (body, meshPartA, volume);
CreateDebriPiece (body, meshPartB, volume);
NewtonMeshDestroy(meshPartA);
NewtonMeshDestroy(meshPartB);
} else {
CreateDebriPiece (body, topMesh, volume);
}
NewtonMeshDestroy(topMesh);
// remove the old visual from graphics world
SceneManager* system = (SceneManager*) NewtonWorldGetUserData(world);
delete srcPrimitive;
system->Remove(srcPrimitive);
// finally destroy this body;
NewtonDestroyBody(world, body);
}
}
