static void BuildJenga (DemoEntityManager* const scene, dFloat mass, const dVector& origin, const dVector& size, int count) { // build a standard block stack of 20 * 3 boxes for a total of 60 NewtonWorld* const world = scene->GetNewton(); dVector blockBoxSize (0.8f, 0.5f, 0.8f * 3.0f); blockBoxSize = blockBoxSize.Scale (1.0f); // create the stack dMatrix baseMatrix (dGetIdentityMatrix()); // for the elevation of the floor at the stack position baseMatrix.m_posit.m_x = origin.m_x; baseMatrix.m_posit.m_z = origin.m_z; dFloat startElevation = 100.0f; dVector floor (FindFloor (world, dVector (baseMatrix.m_posit.m_x, startElevation, baseMatrix.m_posit.m_z, 0.0f), 2.0f * startElevation)); baseMatrix.m_posit.m_y = floor.m_y + blockBoxSize.m_y / 2.0f; // set realistic mass and inertia matrix for each block mass = 5.0f; // create a 90 degree rotation matrix dMatrix rotMatrix (dYawMatrix (3.141592f * 0.5f)); // create a material to control collision with this objects int defaultMaterialID; defaultMaterialID = NewtonMaterialGetDefaultGroupID (world); // separate a bit the block alone the horizontal direction dFloat gap = 0.01f; // create the shape and visual mesh as a common data to be re used NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), blockBoxSize, _BOX_PRIMITIVE, defaultMaterialID); DemoMesh* const geometry = new DemoMesh("box", collision, "wood_0.tga", "wood_0.tga", "wood_0.tga"); for (int i = 0; i < count; i ++) { dMatrix matrix(baseMatrix); matrix.m_posit -= matrix.m_front.Scale (blockBoxSize.m_x - gap); for (int j = 0; j < 3; j ++) { CreateSimpleSolid (scene, geometry, mass, matrix, collision, defaultMaterialID); matrix.m_posit += matrix.m_front.Scale (blockBoxSize.m_x + gap); } baseMatrix = rotMatrix * baseMatrix; baseMatrix.m_posit += matrix.m_up.Scale (blockBoxSize.m_y * 0.99f); } // do not forget to release the assets geometry->Release(); NewtonDestroyCollision (collision); }
dCustomInverseDynamicsEffector* AddLeg(DemoEntityManager* const scene, void* const rootNode, const dMatrix& matrix, dFloat partMass, dFloat limbLenght) { NewtonBody* const parent = NewtonInverseDynamicsGetBody(m_kinematicSolver, rootNode); dFloat inertiaScale = 4.0f; // make limb base dMatrix baseMatrix(dRollMatrix(dPi * 0.5f)); dMatrix cylinderMatrix(baseMatrix * matrix); NewtonBody* const base = CreateCylinder(scene, cylinderMatrix, partMass, inertiaScale, 0.2f, 0.1f); dCustomInverseDynamics* const baseHinge = new dCustomInverseDynamics(cylinderMatrix, base, parent); baseHinge->SetJointTorque(1000.0f); baseHinge->SetTwistAngle(-0.5f * dPi, 0.5f * dPi); void* const baseHingeNode = NewtonInverseDynamicsAddChildNode(m_kinematicSolver, rootNode, baseHinge->GetJoint()); //make limb forward arm dMatrix forwardArmMatrix(dPitchMatrix(-30.0f * dDegreeToRad)); dVector forwardArmSize(limbLenght * 0.25f, limbLenght * 0.25f, limbLenght, 0.0f); forwardArmMatrix.m_posit += forwardArmMatrix.m_right.Scale(forwardArmSize.m_z * 0.5f); NewtonBody* const forwardArm = CreateBox(scene, forwardArmMatrix * matrix, forwardArmSize, partMass, inertiaScale); dMatrix forwardArmPivot(forwardArmMatrix); forwardArmPivot.m_posit -= forwardArmMatrix.m_right.Scale(forwardArmSize.m_z * 0.5f); dCustomInverseDynamics* const forwardArmHinge = new dCustomInverseDynamics(forwardArmPivot * matrix, forwardArm, base); forwardArmHinge->SetJointTorque(1000.0f); forwardArmHinge->SetTwistAngle(-0.5f * dPi, 0.5f * dPi); void* const forwardArmHingeNode = NewtonInverseDynamicsAddChildNode(m_kinematicSolver, baseHingeNode, forwardArmHinge->GetJoint()); //make limb forward arm dMatrix armMatrix(dPitchMatrix(-90.0f * dDegreeToRad)); dFloat armSize = limbLenght * 1.25f; armMatrix.m_posit += forwardArmMatrix.m_right.Scale(limbLenght); armMatrix.m_posit.m_y -= armSize * 0.5f; NewtonBody* const arm = CreateCapsule(scene, armMatrix * matrix, partMass, inertiaScale, armSize * 0.2f, armSize); dMatrix armPivot(armMatrix); armPivot.m_posit.m_y += armSize * 0.5f; dCustomInverseDynamics* const armHinge = new dCustomInverseDynamics(armPivot * matrix, arm, forwardArm); armHinge->SetJointTorque(1000.0f); armHinge->SetTwistAngle(-0.5f * dPi, 0.5f * dPi); void* const armHingeNode = NewtonInverseDynamicsAddChildNode(m_kinematicSolver, forwardArmHingeNode, armHinge->GetJoint()); dMatrix effectorMatrix(dGetIdentityMatrix()); effectorMatrix.m_posit = armPivot.m_posit; effectorMatrix.m_posit.m_y -= armSize; dHexapodEffector* const effector = new dHexapodEffector(m_kinematicSolver, armHingeNode, parent, effectorMatrix * matrix); effector->SetAsThreedof(); effector->SetMaxLinearFriction(partMass * DEMO_GRAVITY * 10.0f); effector->SetMaxAngularFriction(partMass * DEMO_GRAVITY * 10.0f); return effector; }
void Custom6DOF::SubmitConstraints (dFloat timestep, int threadIndex) { dMatrix matrix0; dMatrix matrix1; // calculate the position of the pivot point and the Jacobian direction vectors, in global space. CalculateGlobalMatrix (matrix0, matrix1); // add the linear limits const dVector& p0 = matrix0.m_posit; const dVector& p1 = matrix1.m_posit; dVector dp (p0 - p1); for (int i = 0; i < 3; i ++) { if ((m_minLinearLimits[i] == 0.0f) && (m_maxLinearLimits[i] == 0.0f)) { NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0[i][0]); NewtonUserJointSetRowStiffness (m_joint, 1.0f); } else { // it is a limited linear dof, check if it pass the limits dFloat dist = dp.DotProduct3(matrix1[i]); if (dist > m_maxLinearLimits[i]) { dVector q1 (p1 + matrix1[i].Scale (m_maxLinearLimits[i])); // clamp the error, so the not too much energy is added when constraint violation occurs dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]); if (maxDist > D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) { q1 = p0 - matrix1[i].Scale(D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION); } NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]); NewtonUserJointSetRowStiffness (m_joint, 1.0f); // allow the object to return but not to kick going forward NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f); } else if (dist < m_minLinearLimits[i]) { dVector q1 (p1 + matrix1[i].Scale (m_minLinearLimits[i])); // clamp the error, so the not too much energy is added when constraint violation occurs dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]); if (maxDist < -D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) { q1 = p0 - matrix1[i].Scale(-D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION); } NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]); NewtonUserJointSetRowStiffness (m_joint, 1.0f); // allow the object to return but not to kick going forward NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f); } } } dVector euler0(0.0f); dVector euler1(0.0f); dMatrix localMatrix (matrix0 * matrix1.Inverse()); localMatrix.GetEulerAngles(euler0, euler1); AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch); AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch); if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) { euler0 = euler1; } dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f); //dTrace (("(%f %f %f) (%f %f %f)\n", m_pitch.m_angle * 180.0f / 3.141592f, m_yaw.m_angle * 180.0f / 3.141592f, m_roll.m_angle * 180.0f / 3.141592f, euler0.m_x * 180.0f / 3.141592f, euler0.m_y * 180.0f / 3.141592f, euler0.m_z * 180.0f / 3.141592f)); bool limitViolation = false; for (int i = 0; i < 3; i ++) { if (euler[i] < m_minAngularLimits[i]) { limitViolation = true; euler[i] = m_minAngularLimits[i]; } else if (euler[i] > m_maxAngularLimits[i]) { limitViolation = true; euler[i] = m_maxAngularLimits[i]; } } if (limitViolation) { //dMatrix pyr (dPitchMatrix(m_pitch.m_angle) * dYawMatrix(m_yaw.m_angle) * dRollMatrix(m_roll.m_angle)); dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2])); dMatrix baseMatrix (p0y0r0 * matrix1); dMatrix rotation (matrix0.Inverse() * baseMatrix); dQuaternion quat (rotation); if (quat.m_q0 > dFloat (0.99995f)) { //dVector p0 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH)); //dVector p1 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH)); //NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &baseMatrix[2][0]); //NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f); //dVector q0 (matrix0[3] + baseMatrix[0].Scale (MIN_JOINT_PIN_LENGTH)); //NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[1][0]); //NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[2][0]); } else { dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f))); dVector q0 (matrix0[3] + basis[1].Scale (MIN_JOINT_PIN_LENGTH)); dVector q1 (matrix0[3] + rotation.RotateVector(basis[1].Scale (MIN_JOINT_PIN_LENGTH))); NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &basis[2][0]); NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f); //dVector q0 (matrix0[3] + basis[0].Scale (MIN_JOINT_PIN_LENGTH)); //NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[1][0]); //NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[2][0]); } } }
void CustomControlledBallAndSocket::SubmitConstraints (dFloat timestep, int threadIndex) { dMatrix matrix0; dMatrix matrix1; // calculate the position of the pivot point and the Jacobian direction vectors, in global space. CalculateGlobalMatrix (matrix0, matrix1); // Restrict the movement on the pivot point along all tree orthonormal direction NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]); NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]); NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]); #if 0 dVector euler0; dVector euler1; dMatrix localMatrix (matrix0 * matrix1.Inverse()); localMatrix.GetEulerAngles(euler0, euler1); AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch); AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch); if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) { euler0 = euler1; } dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f); for (int i = 0; i < 3; i ++) { dFloat error = m_targetAngles[i] - euler[i]; if (dAbs (error) > (0.125f * 3.14159213f / 180.0f) ) { dFloat angularStep = dSign(error) * m_angulaSpeed * timestep; if (angularStep > 0.0f) { if (angularStep > error) { angularStep = error * 0.5f; } } else { if (angularStep < error) { angularStep = error * 0.5f; } } euler[i] = euler[i] + angularStep; } } dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2])); dMatrix baseMatrix (p0y0r0 * matrix1); dMatrix rotation (matrix0.Inverse() * baseMatrix); dQuaternion quat (rotation); if (quat.m_q0 > dFloat (0.99995f)) { dVector euler0; dVector euler1; rotation.GetEulerAngles(euler0, euler1); NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]); NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]); NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]); } else { dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f))); NewtonUserJointAddAngularRow (m_joint, 2.0f * dAcos (quat.m_q0), &basis[0][0]); NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[1][0]); NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[2][0]); } #else matrix1 = m_targetRotation * matrix1; dQuaternion localRotation(matrix1 * matrix0.Inverse()); if (localRotation.DotProduct(m_targetRotation) < 0.0f) { localRotation.Scale(-1.0f); } dFloat angle = 2.0f * dAcos(localRotation.m_q0); dFloat angleStep = m_angulaSpeed * timestep; if (angleStep < angle) { dVector axis(dVector(localRotation.m_q1, localRotation.m_q2, localRotation.m_q3, 0.0f)); axis = axis.Scale(1.0f / dSqrt(axis % axis)); // localRotation = dQuaternion(axis, angleStep); } dVector axis (matrix1.m_front * matrix1.m_front); dVector axis1 (matrix1.m_front * matrix1.m_front); //dFloat sinAngle; //dFloat cosAngle; //CalculatePitchAngle (matrix0, matrix1, sinAngle, cosAngle); //float xxxx = dAtan2(sinAngle, cosAngle); //float xxxx1 = dAtan2(sinAngle, cosAngle); dQuaternion quat(localRotation); if (quat.m_q0 > dFloat(0.99995f)) { // dAssert (0); /* dVector euler0; dVector euler1; rotation.GetEulerAngles(euler0, euler1); NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]); NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]); NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]); */ } else { dMatrix basis(dGrammSchmidt(dVector(quat.m_q1, quat.m_q2, quat.m_q3, 0.0f))); NewtonUserJointAddAngularRow(m_joint, -2.0f * dAcos(quat.m_q0), &basis[0][0]); NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[1][0]); NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[2][0]); } #endif }
//static void UnstableStruture (SceneManager& system, dVector location, int high) static void BreakableStruture (SceneManager& system, dVector location, int high) { dFloat plankMass; dFloat columnMass; // ///////////////////////////////////////////////////////////////////// // // Build a parking lot type structure dVector columnBoxSize (3.0f, 1.0f, 1.0f); // dVector columnBoxSize (3.0f, 3.0f, 1.0f); dVector plankBoxSize (6.0f, 1.0f, 6.0f); // create the stack dMatrix baseMatrix (GetIdentityMatrix()); // get the floor position in from o the camera baseMatrix.m_posit = location; baseMatrix.m_posit.m_y += columnBoxSize.m_x; // set realistic (extremes in this case for 24 bits precision) masses for the different components // note how the newton engine handle different masses ratios without compromising stability, // we recommend the application keep this ration under 100 for contacts and 50 for joints columnMass = 1.0f; plankMass = 20.0f; // plankMass = 1.0f; // create a material carrier to to cou collision wit ethsi obejted int defaultMaterialID; defaultMaterialID = NewtonMaterialGetDefaultGroupID (system.m_world); dMatrix columAlignment (dRollMatrix(3.1416f * 0.5f)); for (int i = 0; i < high; i ++) { NewtonBody* body; RenderPrimitive* primitive; dMatrix matrix(columAlignment * baseMatrix); // add the 4 column matrix.m_posit.m_x -= (columnBoxSize.m_z - plankBoxSize.m_x) * 0.5f; matrix.m_posit.m_z -= (columnBoxSize.m_z - plankBoxSize.m_z) * 0.5f; body = CreateGenericSolid (system.m_world, &system, columnMass, matrix, columnBoxSize, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID); primitive = (RenderPrimitive*) NewtonBodyGetUserData (body); SetBreakValue (body, 50.0f); ConvexCastPlacement (body); matrix.m_posit.m_x += columnBoxSize.m_z - plankBoxSize.m_x; body = CreateGenericSolid (system.m_world, &system, columnMass, matrix, columnBoxSize, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID); primitive = (RenderPrimitive*) NewtonBodyGetUserData (body); SetBreakValue (body, 30.0f); ConvexCastPlacement (body); matrix.m_posit.m_z += columnBoxSize.m_z - plankBoxSize.m_z; body = CreateGenericSolid (system.m_world, &system, columnMass, matrix, columnBoxSize, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID); primitive = (RenderPrimitive*) NewtonBodyGetUserData (body); SetBreakValue (body, 30.0f); ConvexCastPlacement (body); matrix.m_posit.m_x -= columnBoxSize.m_z - plankBoxSize.m_x; body = CreateGenericSolid (system.m_world, &system, columnMass, matrix, columnBoxSize, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID); primitive = (RenderPrimitive*) NewtonBodyGetUserData (body); SetBreakValue (body, 30.0f); ConvexCastPlacement (body); // add a plank dVector size (plankBoxSize); size.m_x *= 0.85f; size.m_z *= 0.85f; body = CreateGenericSolid (system.m_world, &system, plankMass, baseMatrix, size, _BOX_PRIMITIVE, defaultMaterialID); primitive = (RenderPrimitive*) NewtonBodyGetUserData (body); SetBreakValue (body, 1.0f); ConvexCastPlacement (body); // set up for another level baseMatrix.m_posit.m_y += (columnBoxSize.m_x + plankBoxSize.m_y); } dFloat mass; NewtonBody* body; RenderPrimitive* primitive; PrimitiveType type = _BOX_PRIMITIVE; dVector size (1.0f, 2.0f, 1.0f); dMatrix matrix (GetIdentityMatrix()); mass = 10.0f; matrix.m_posit = location; matrix.m_posit.m_y = FindFloor (system.m_world, matrix.m_posit.m_x, matrix.m_posit.m_z) + baseMatrix.m_posit.m_y + 35.0f; body = CreateGenericSolid (system.m_world, &system, mass, matrix, size, type, defaultMaterialID); primitive = (RenderPrimitive*) NewtonBodyGetUserData (body); }
void MakeHexapod(DemoEntityManager* const scene, const dMatrix& location) { dFloat mass = 30.0f; // make the kinematic solver m_kinematicSolver = NewtonCreateInverseDynamics(scene->GetNewton()); // make the root body dMatrix baseMatrix(dGetIdentityMatrix()); baseMatrix.m_posit.m_y += 0.35f; dVector size (1.3f, 0.31f, 0.5f, 0.0f); NewtonBody* const hexaBody = CreateBox(scene, baseMatrix * location, size, mass, 1.0f); void* const hexaBodyNode = NewtonInverseDynamicsAddRoot(m_kinematicSolver, hexaBody); int legEffectorCount = 0; dCustomInverseDynamicsEffector* legEffectors[32]; baseMatrix.m_posit.m_y -= 0.06f; // make the hexapod six limbs for (int i = 0; i < 3; i ++) { dMatrix rightLocation (baseMatrix); rightLocation.m_posit += rightLocation.m_right.Scale (size.m_z * 0.65f); rightLocation.m_posit += rightLocation.m_front.Scale (size.m_x * 0.3f - size.m_x * i / 3.0f); legEffectors[legEffectorCount] = AddLeg (scene, hexaBodyNode, rightLocation * location, mass * 0.1f, 0.3f); legEffectorCount ++; dMatrix similarTransform (dGetIdentityMatrix()); similarTransform.m_posit.m_x = rightLocation.m_posit.m_x; similarTransform.m_posit.m_y = rightLocation.m_posit.m_y; dMatrix leftLocation (rightLocation * similarTransform.Inverse() * dYawMatrix(dPi) * similarTransform); legEffectors[legEffectorCount] = AddLeg (scene, hexaBodyNode, leftLocation * location, mass * 0.1f, 0.3f); legEffectorCount ++; } // finalize inverse dynamics solver NewtonInverseDynamicsEndBuild(m_kinematicSolver); // create a fix pose frame generator dEffectorTreeFixPose* const idlePose = new dEffectorTreeFixPose(hexaBody); dEffectorTreeFixPose* const walkPoseGenerator = new dEffectorWalkPoseGenerator(hexaBody); m_walkIdleBlender = new dEffectorBlendIdleWalk (hexaBody, idlePose, walkPoseGenerator); m_postureModifier = new dAnimationHipController(m_walkIdleBlender); m_animTreeNode = new dEffectorTreeRoot(hexaBody, m_postureModifier); dMatrix rootMatrix; NewtonBodyGetMatrix (hexaBody, &rootMatrix[0][0]); rootMatrix = rootMatrix.Inverse(); for (int i = 0; i < legEffectorCount; i++) { dEffectorTreeInterface::dEffectorTransform frame; dCustomInverseDynamicsEffector* const effector = legEffectors[i]; dMatrix effectorMatrix(effector->GetBodyMatrix()); dMatrix poseMatrix(effectorMatrix * rootMatrix); frame.m_effector = effector; frame.m_posit = poseMatrix.m_posit; frame.m_rotation = dQuaternion(poseMatrix); idlePose->GetPose().Append(frame); walkPoseGenerator->GetPose().Append(frame); m_animTreeNode->GetPose().Append(frame); } }