////////////////////////////////////////////////////////////////////// // 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); } }