void MSNewton::Servo::adjust_pin_matrix_proc(JointData* joint_data, dMatrix& pin_matrix) {
dMatrix matrix;
dVector centre;
NewtonBodyGetMatrix(joint_data->child, &matrix[0][0]);
NewtonBodyGetCentreOfMass(joint_data->child, ¢re[0]);
centre = matrix.TransformVector(centre);
centre = pin_matrix.UntransformVector(centre);
dVector point(0.0f, 0.0f, centre.m_z);
pin_matrix.m_posit = pin_matrix.TransformVector(point);
}
dMatrix CustomDGRayCastCar::CalculateTireMatrix (int tireIndex) const
{
const Tire& tire = m_tires[tireIndex];
// calculate the rotation angle matrix
dMatrix angleMatrix ( dPitchMatrix( tire.m_spinAngle ) );
// get the tire body matrix
dMatrix bodyMatrix;
NewtonBodyGetMatrix (m_body0, &bodyMatrix[0][0]);
return angleMatrix * CalculateSuspensionMatrix (tireIndex, tire.m_posit) * m_localFrame * bodyMatrix;
}
void CustomDGRayCastCar::ApplyChassisTorque (const dVector& vForce, const dVector& vPoint)
{
dVector Torque;
dMatrix M;
dVector com;
NewtonBodyGetCentreOfMass( m_body0, &com[0] );
NewtonBodyGetMatrix( m_body0, &M[0][0] );
Torque = ( vPoint - M.TransformVector( dVector( com.m_x, com.m_y, com.m_z, 1.0f ) ) ) * vForce;
NewtonBodyAddTorque( m_body0, &Torque[0] );
}
VALUE MSNewton::Bodies::get_closest_points(VALUE self, VALUE v_body1, VALUE v_body2) {
const NewtonBody* body1 = Util::value_to_body(v_body1);
const NewtonBody* body2 = Util::value_to_body(v_body2);
Util::validate_two_bodies(body1, body2);
const NewtonWorld* world = NewtonBodyGetWorld(body1);
WorldData* world_data = (WorldData*)NewtonWorldGetUserData(world);
NewtonCollision* colA = NewtonBodyGetCollision(body1);
NewtonCollision* colB = NewtonBodyGetCollision(body2);
dMatrix matrixA;
dMatrix matrixB;
NewtonBodyGetMatrix(body1, &matrixA[0][0]);
NewtonBodyGetMatrix(body2, &matrixB[0][0]);
dVector pointA;
dVector pointB;
dVector normalAB;
if (NewtonCollisionClosestPoint(world, colA, &matrixA[0][0], colB, &matrixB[0][0], &pointA[0], &pointB[0], &normalAB[0], 0) == 0)
return Qnil;
return rb_ary_new3(2, Util::point_to_value(pointA, world_data->inverse_scale), Util::point_to_value(pointB, world_data->inverse_scale));
}
dVector BodyGetPointVelocity(const NewtonBody* const body, const dVector &point)
{
dVector v, w, c;
NewtonBodyGetVelocity(body, &v[0]);
NewtonBodyGetOmega(body, &w[0]);
dMatrix matrix;
NewtonBodyGetMatrix(body, &matrix[0][0]);
c = matrix.m_posit; // TODO: Does not handle COM offset !!!
return v + w * (point - c);
}
static CustomCorkScrew* AddCylindricalWheel (DemoEntityManager* const scene, const dVector& origin, dFloat radius, dFloat height, NewtonBody* const parent)
{
NewtonBody* const wheel = CreateWheel (scene, origin, height, radius);
// the joint pin is the first row of the matrix
dMatrix matrix;
NewtonBodyGetMatrix (wheel, &matrix[0][0]);
return new CustomCorkScrew (matrix, wheel, parent);
}
dVector CalculateToqueAtPoint (const NewtonBody* body, const dVector& point, const dVector& force)
{
dVector com;
dMatrix matrix;
NewtonBodyGetMatrix (body, &matrix[0][0]);
NewtonBodyGetCentreOfMass (body, &matrix[0][0]);
com = matrix.TransformVector (com);
return (point - com) * force;
}
void AddPulley (DemoEntityManager* const scene, const dVector& origin)
{
NewtonBody* const reel0 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 2.0f), dVector(4.0f, 0.25f, 0.25f));
// this is just for show
NewtonBody* const reel1 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 0.0f), dVector(4.0f, 0.25f, 0.25f));
NewtonBodySetMassMatrix (reel0, 0.0f, 0.0f, 0.0f, 0.0f);
NewtonBodySetMassMatrix (reel1, 0.0f, 0.0f, 0.0f, 0.0f);
dMatrix matrix;
CustomSlider* const slider0 = AddSliderWheel (scene, origin + dVector (0.0f, 4.0f, 2.0f), 0.5f, 1.0f, reel0);
CustomSlider* const slider1 = AddSliderWheel (scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel0);
slider0->EnableLimits(true);
slider0->SetLimits (-2.0f, 2.0f);
NewtonBody* const body0 = slider0->GetBody0();
NewtonBody* const body1 = slider1->GetBody0();
dMatrix matrix0;
dMatrix matrix1;
NewtonBodyGetMatrix (body0, &matrix0[0][0]);
NewtonBodyGetMatrix (body1, &matrix1[0][0]);
dVector pin0 (matrix0.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
dVector pin1 (matrix1.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
new CustomPulley (4.0f, pin0, pin1, body0, body1);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), reel0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, slider0->GetBody0(), reel0);
NewtonSkeletonContainerAttachBone(skeleton, slider1->GetBody0(), reel0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
// make an aggregate for disabling collisions
void* const aggregate = NewtonCollisionAggregateCreate (scene->GetNewton());
NewtonCollisionAggregateSetSelfCollision (aggregate, 0);
NewtonCollisionAggregateAddBody (aggregate, reel0);
NewtonCollisionAggregateAddBody (aggregate, reel1);
NewtonCollisionAggregateAddBody (aggregate, body0);
NewtonCollisionAggregateAddBody (aggregate, body1);
}
FrictionTrankThreaSuspention(const dMatrix& pinsAndPivoFrame, const NewtonBody* child, const NewtonBody* parent)
: CustomSlidingContact (pinsAndPivoFrame, child, parent)
{
dMatrix childMatrix;
dMatrix parentMatrix;
dVector maxPointFront;
dVector minPointFront;
NewtonCollision* collision;
collision = NewtonBodyGetCollision(child);
NewtonBodyGetMatrix(child, &childMatrix[0][0]);
NewtonBodyGetMatrix(parent, &parentMatrix[0][0]);
// find the the extreme front and rear point, this is used to calculate the position of the suspension points
dVector frontDir (childMatrix.UnrotateVector(parentMatrix.m_front));
NewtonCollisionSupportVertex(collision, &frontDir[0], &maxPointFront[0]);
dVector rearDir (frontDir.Scale (-1.0f));
NewtonCollisionSupportVertex(collision, &rearDir[0], &minPointFront[0]);
// calculate the front suspension points
dVector frontHardPoint (childMatrix.m_posit + childMatrix.RotateVector(frontDir.Scale (maxPointFront % frontDir)));
m_frontHarpointOnParent = parentMatrix.UntransformVector(frontHardPoint);
m_frontHarpointOnThread = childMatrix.UntransformVector(frontHardPoint);
// calculate the front rear suspension points
dVector rearHardPoint (childMatrix.m_posit + childMatrix.RotateVector(rearDir.Scale (minPointFront % rearDir)));
m_rearHarpointOnParent = parentMatrix.UntransformVector(rearHardPoint);
m_rearHarpointOnThread = childMatrix.UntransformVector(rearHardPoint);
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass0;
dFloat mass1;
NewtonBodyGetMassMatrix(child, &mass0, &Ixx, &Iyy, &Izz);
NewtonBodyGetMassMatrix(parent, &mass1, &Ixx, &Iyy, &Izz);
m_massScale = (mass0 * mass1) / (mass0 + mass1);
}
static void AddLimitedBallAndSocket (DemoEntityManager* const scene, const dVector& origin)
{
dVector size(1.0f, 1.0f, 1.0f);
NewtonBody* const box0 = CreateCapule(scene, origin + dVector(0.0f, 5.0f, 0.0f, 0.0f), size);
NewtonBody* const box1 = CreateCapule(scene, origin + dVector(0.0f, 5.0 - size.m_y * 2.0f, 0.0f, 0.0f), size);
NewtonBody* const box2 = CreateCapule(scene, origin + dVector(0.0f, 5.0 - size.m_y * 4.0f, 0.0f, 0.0f), size);
dMatrix pinMatrix(dGrammSchmidt(dVector(0.0f, -1.0f, 0.0f, 0.0f)));
// connect first box to the world
dMatrix matrix;
NewtonBodyGetMatrix(box0, &matrix[0][0]);
pinMatrix.m_posit = matrix.m_posit + dVector(0.0f, size.m_y, 0.0f, 0.0f);
CustomLimitBallAndSocket* const joint0 = new CustomLimitBallAndSocket(pinMatrix, box0, NULL);
joint0->SetConeAngle (30.0f * 3.141592f / 180.0f);
joint0->SetTwistAngle (-30.0f * 3.141592f / 180.0f, 30.0f * 3.141592f / 180.0f);
// connect first box1 to box0 the world
NewtonBodyGetMatrix(box1, &matrix[0][0]);
pinMatrix.m_posit = matrix.m_posit + dVector(0.0f, size.m_y, 0.0f, 0.0f);
CustomLimitBallAndSocket* const joint1 = new CustomLimitBallAndSocket(pinMatrix, box1, box0);
joint1->SetConeAngle(30.0f * 3.141592f / 180.0f);
joint1->SetTwistAngle(-30.0f * 3.141592f / 180.0f, 30.0f * 3.141592f / 180.0f);
// connect first box2 to box1 the world
NewtonBodyGetMatrix(box2, &matrix[0][0]);
pinMatrix.m_posit = matrix.m_posit + dVector(0.0f, size.m_y, 0.0f, 0.0f);
CustomLimitBallAndSocket* const joint2 = new CustomLimitBallAndSocket(pinMatrix, box2, box1);
joint2->SetConeAngle(30.0f * 3.141592f / 180.0f);
joint2->SetTwistAngle(-30.0f * 3.141592f / 180.0f, 30.0f * 3.141592f / 180.0f);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), NULL, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerAttachBone(skeleton, box2, box1);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
void OgreNewtonSceneBody::EndAddRemoveCollision()
{
dNewtonCollisionScene* const scene = (dNewtonCollisionScene*) GetCollision();
scene->EndAddRemoveCollision();
// need to update the aabb in the broad phase, for this we call set matrix
dMatrix matrix;
NewtonBody* const body = GetNewtonBody();
NewtonBodyGetMatrix(body, &matrix[0][0]);
NewtonBodySetMatrix(body, &matrix[0][0]);
}
void DemoCameraListener::RenderPickedTarget () const
{
if (m_targetPicked) {
dMatrix matrix;
NewtonBodyGetMatrix(m_targetPicked, &matrix[0][0]);
dVector p0 (matrix.TransformVector(m_pickedBodyLocalAtachmentPoint));
dVector p1 (p0 + matrix.RotateVector (m_pickedBodyLocalAtachmentNormal.Scale (0.5f)));
ShowMousePicking (p0, p1);
}
}
void CustomVehicleController::PostUpdate(dFloat timestep, int threadIndex)
{
if (m_finalized) {
NewtonBody* const body = GetBody();
NewtonBodyGetMatrix(body, &m_chassisState.m_matrix[0][0]);
for (dTireList::dListNode* node = m_tireList.GetFirst(); node; node = node->GetNext()) {
CustomVehicleControllerBodyStateTire* const tire = &node->GetInfo();
tire->UpdateTransform();
}
}
}
static void PlaneCollisionCollideCallbackConstinue (NewtonUserMeshCollisionCollideDesc* const collideDesc, const void* const continueCollisionHandle)
{
dInfinitePlane* const me = (dInfinitePlane*) collideDesc->m_userData;
// build that aabb of each face and submit only the one that pass the test.
if (NewtonUserMeshCollisionContinuousOverlapTest (collideDesc, continueCollisionHandle, &me->m_minBox[0], &me->m_maxBox[0])) {
const dVector& p0 = me->m_minBox;
const dVector& p1 = me->m_maxBox;
dVector centre ((p1 + p0).Scale (0.5f));
//find the projection of center point over the plane
dFloat t = - (me->m_plane.DotProduct3(centre) + me->m_plane.m_w);
centre += me->m_plane.Scale (t);
//know calculate the scale factor
dVector size (p1 - p0);
dFloat s = dMax(size.m_x, dMax (size.m_y, size.m_z)) * 0.5f;
dInt32 threadNumber = collideDesc->m_threadNumber;
// initialize the callback data structure
#ifdef PASS_A_QUAD
collideDesc->m_faceCount = 1;
#else
collideDesc->m_faceCount = 2;
#endif
collideDesc->m_vertexStrideInBytes = sizeof (dVector);
collideDesc->m_faceIndexCount = &me->m_faceIndices[threadNumber][0];
collideDesc->m_faceVertexIndex = &me->m_indexArray[threadNumber][0];
collideDesc->m_vertex = &me->m_collisionVertex[threadNumber][0][0];
dVector* const polygon = &me->m_collisionVertex[threadNumber][0];
for (int i = 0; i < 4; i ++) {
polygon[i] = centre + me->m_unitSphape[i].Scale (s);
}
// save face normal
polygon[4] = me->m_plane;
// show debug display info
if (DebugDisplayOn()) {
dMatrix matrix;
dVector face[64];
NewtonBodyGetMatrix (collideDesc->m_polySoupBody, &matrix[0][0]);
matrix.TransformTriplex (&face[0].m_x, sizeof (dVector), &polygon[0].m_x, sizeof (dVector), 4);
NewtonWorld* const world = NewtonBodyGetWorld (collideDesc->m_polySoupBody);
// critical section lock
NewtonWorldCriticalSectionLock (world, threadNumber);
//DebugDrawPolygon (4, &face[0]);
// unlock the critical section
NewtonWorldCriticalSectionUnlock (world);
}
}
}
void SubmitConstraints (dFloat timestep, int threadIndex)
{
// calculate suspension bumpers and forces
dMatrix threadMatrix;
dMatrix parentMatrix;
dVector threadCOM;
dVector parentCOM;
dVector threadVeloc;
dVector parentVeloc;
dVector threadOmega;
dVector parentOmega;
// get the physics body state;
NewtonBodyGetOmega(m_body0, &threadOmega[0]);
NewtonBodyGetOmega(m_body1, &parentOmega[0]);
NewtonBodyGetVelocity(m_body0, &threadVeloc[0]);
NewtonBodyGetVelocity(m_body1, &parentVeloc[0]);
NewtonBodyGetCentreOfMass(m_body0, &threadCOM[0]);
NewtonBodyGetCentreOfMass(m_body1, &parentCOM[0]);
NewtonBodyGetMatrix(m_body0, &threadMatrix[0][0]);
NewtonBodyGetMatrix(m_body1, &parentMatrix[0][0]);
threadCOM = threadMatrix.TransformVector(threadCOM);
parentCOM = parentMatrix.TransformVector(parentCOM);
ApplySuspesionForce (timestep,
m_body0, m_rearHarpointOnThread, threadMatrix, threadCOM, threadVeloc, threadOmega,
m_body1, m_rearHarpointOnParent, parentMatrix, parentCOM, parentVeloc, parentOmega);
ApplySuspesionForce (timestep,
m_body0, m_frontHarpointOnThread, threadMatrix, threadCOM, threadVeloc, threadOmega,
m_body1, m_frontHarpointOnParent, parentMatrix, parentCOM, parentVeloc, parentOmega);
CustomSlidingContact::SubmitConstraints (timestep, threadIndex);
}
dCustomUpVector::dCustomUpVector(const dVector& pin, NewtonBody* child)
:dCustomJoint(2, child, NULL)
{
dMatrix pivot;
NewtonBodyGetMatrix(child, &pivot[0][0]);
dMatrix matrix (dGrammSchmidt(pin));
matrix.m_posit = pivot.m_posit;
CalculateLocalMatrix (matrix, m_localMatrix0, m_localMatrix1);
}
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();
}
static void AddMerryGoRound(DemoEntityManager* const scene, const dVector& location)
{
NewtonBody* const pole = CreateCylinder(scene, location, 0.0f, 0.2f, 3.0f);
dVector platformPosit(location);
platformPosit.m_y += 0.2f;
NewtonBody* const platform = CreateCylinder(scene, platformPosit, 200.0f, 10.0f, 0.2f);
dMatrix pivot;
NewtonBodyGetMatrix(platform, &pivot[0][0]);
dCustomHinge* const hinge = new dCustomHinge(pivot, platform, pole);
hinge;
}
dNewtonJointDoubleHinge::dNewtonJointDoubleHinge(const dMatrix pintAndPivotMatrix, void* const body0, void* const body1)
:dNewtonJoint()
{
dMatrix bodyMatrix;
NewtonBody* const netwonBody0 = (NewtonBody*)body0;
NewtonBody* const netwonBody1 = (NewtonBody*)body1;
NewtonBodyGetMatrix(netwonBody0, &bodyMatrix[0][0]);
dMatrix matrix(pintAndPivotMatrix * bodyMatrix);
dCustomDoubleHinge* const joint = new dCustomDoubleHinge(matrix, netwonBody0, netwonBody1);
SetJoint(joint);
}
void CustomPathFollow::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
// Get the global matrices of each rigid body.
NewtonBodyGetMatrix(m_body0, &matrix0[0][0]);
matrix0 = m_localMatrix0 * matrix0;
dMatrix matrix1 (EvalueCurve (matrix0.m_posit));
// Restrict the movement on the pivot point along all tree the normal and binormal of the path
const dVector& p0 = matrix0.m_posit;
const dVector& p1 = matrix1.m_posit;
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_right[0]);
// get a point along the ping axis at some reasonable large distance from the pivot
dVector q0 (p0 + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
dVector q1 (p1 + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH));
// two more constraints rows to inforce the normal and binormal
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_right[0]);
// Julio: at this point the path follow will maintaing the pivot fixed to the path and will allow it to slide freelly
// it will also spin around the path tangent, if more control is needed then more constraion rows need to be submited
// for example friction along the oath tangent be added to make more realistic behavior
// or anothet point contration along the binormal to make no spin around the path tangent
/*
// example of ading friction
dVector veloc0;
dVector omega0;
NewtonBodyGetVelocity(m_body0, &veloc0[0]);
NewtonBodyGetOmega(m_body0, &omega0[0]);
veloc0 += omega0 * (matrix0.m_posit - p0);
dFloat relAccel;
relAccel = - (veloc0 % matrix0.m_front) / timestep;
#define MaxFriction 10.0f
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p0[0], &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
NewtonUserJointSetRowMinimumFriction (m_joint, -MaxFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, MaxFriction);
*/
}
void cPhysicsBodyNewton::RenderDebugGeometry(iLowLevelGraphics *a_pLowLevel, const cColor &a_Color)
{
//cPhysicsWorldNewton *pPhysicsWorld = static_cast<cPhysicsWorldNewton*>(NewtonWorldGetUserData(m_pNewtonWorld));
//pPhysicsWorld->GetWorld3D()->gets
NewtonCollision *pCollision = NewtonBodyGetCollision(m_pNewtonBody);
float matrix[4][4];
NewtonBodyGetMatrix(m_pNewtonBody, &matrix[0][0]);
g_pLowLevelGraphics = a_pLowLevel;
g_DebugColor = a_Color;
NewtonCollision *pNewtonCollision = NewtonBodyGetCollision(m_pNewtonBody);
NewtonCollisionForEachPolygonDo(pNewtonCollision,
m_mtxLocalTransform.GetTranspose().m[0], RenderDebugPolygon, this);
}
dNewtonJointDoubleHingeActuator::dNewtonJointDoubleHingeActuator(const dMatrix pintAndPivotMatrix, void* const body0, void* const body1)
:dNewtonJoint()
{
dMatrix bodyMatrix;
NewtonBody* const netwonBody0 = (NewtonBody*)body0;
NewtonBody* const netwonBody1 = (NewtonBody*)body1;
NewtonBodyGetMatrix(netwonBody0, &bodyMatrix[0][0]);
dMatrix matrix(pintAndPivotMatrix * bodyMatrix);
dCustomDoubleHingeActuator* const joint = new dCustomDoubleHingeActuator(matrix, netwonBody0, netwonBody1);
SetJoint(joint);
//joint->SetEnableFlag0(true);
//joint->SetEnableFlag1(true);
}
dAnimationJoint* CreateChildNode(NewtonBody* const boneBody, dAnimationJoint* const parent, const dJointDefinition& definition) const
{
dMatrix matrix;
NewtonBodyGetMatrix(boneBody, &matrix[0][0]);
dJointDefinition::dFrameMatrix frameAngle(definition.m_frameBasics);
dMatrix pinAndPivotInGlobalSpace(dPitchMatrix(frameAngle.m_pitch * dDegreeToRad) * dYawMatrix(frameAngle.m_yaw * dDegreeToRad) * dRollMatrix(frameAngle.m_roll * dDegreeToRad));
pinAndPivotInGlobalSpace = pinAndPivotInGlobalSpace * matrix;
//dMatrix bindMatrix(entity->GetParent()->CalculateGlobalMatrix((DemoEntity*)NewtonBodyGetUserData(parentBody)).Inverse());
dMatrix bindMatrix(dGetIdentityMatrix());
dAnimationJoint* const joint = new dAnimationRagdollJoint(definition.m_type, pinAndPivotInGlobalSpace, boneBody, bindMatrix, parent);
return joint;
}
static CustomSlider* AddSliderWheel (DemoEntityManager* const scene, const dVector& origin, dFloat radius, dFloat height, NewtonBody* const parent)
{
NewtonBody* const wheel = CreateWheel (scene, origin, height, radius);
// the joint pin is the first row of the matrix
//dMatrix localPin (dRollMatrix(90.0f * 3.141592f / 180.0f));
dMatrix localPin (dGetIdentityMatrix());
dMatrix matrix;
NewtonBodyGetMatrix (wheel, & matrix[0][0]);
matrix = localPin * matrix;
// connect first box to the world
return new CustomSlider (matrix, wheel, parent);
}
static void AddGear (DemoEntityManager* const scene, const dVector& origin)
{
NewtonBody* const box0 = CreateCylinder(scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.25f, 4.0f);
dMatrix matrix;
//connect the box0 to the base by a fix joint (a hinge with zero limit)
NewtonBodyGetMatrix(box0, &matrix[0][0]);
CustomHinge* const hinge = new CustomHinge(matrix, box0, NULL);
hinge->EnableLimits(true);
hinge->SetLimis(0.0f, 0.0f);
// connect two bodies with a hinge
CustomHinge* const hinge0 = AddHingeWheel (scene, origin + dVector (-1.0f, 4.0f, 0.0f), 0.5f, 1.0f, box0);
CustomHinge* const hinge1 = AddHingeWheel (scene, origin + dVector ( 1.0f, 4.0f, 0.0f), 0.5f, 1.0f, box0);
NewtonBody* const body0 = hinge0->GetBody0();
NewtonBody* const body1 = hinge1->GetBody0();
dMatrix matrix0;
dMatrix matrix1;
NewtonBodyGetMatrix (body0, &matrix0[0][0]);
NewtonBodyGetMatrix (body1, &matrix1[0][0]);
// relate the two body motion with a gear joint
dVector pin0 (matrix0.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
dVector pin1 (matrix1.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
new CustomGear (4.0f, pin0, pin1, body0, body1);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), NULL, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, hinge0->GetBody0(), box0);
NewtonSkeletonContainerAttachBone(skeleton, hinge1->GetBody0(), box0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
void
PhysicsActor::getOrientation(Math::Matrix4& _orient)
{
Math::Matrix4 matrix;
NewtonBodyGetMatrix(m_pActor, matrix.m_array);
// ensure a zero offset for the returned orientation matrix:
matrix.setPosition(0.0f, 0.0f, 0.0f);
// transfer values to the return matrix:
for (int i = 0; i < 16; i++)
{
_orient.m_array[i] = matrix.m_array[i];
}
}
CustomUniversal::CustomUniversal(const dMatrix& pinAndPivotFrame, NewtonBody* child, NewtonBody* parent)
:CustomJoint(6, child, parent)
,m_curJointAngle_0()
,m_curJointAngle_1()
{
dMatrix matrix0;
m_limit_0_On = true;
m_angularMotor_0_On = false;
m_angularDamp_0 = 0.5f;
m_angularAccel_0 = -4.0f;
m_jointOmega_0 = 0.0f;
m_minAngle_0 = -45.0f * 3.141592f / 180.0f;
m_maxAngle_0 = 45.0f * 3.141592f / 180.0f;
m_limit_1_On = true;
m_angularMotor_1_On = false;
m_angularDamp_1 = 0.3f;
m_angularAccel_1 = -4.0f;
m_jointOmega_1 = 0.0f;
m_minAngle_1 = -45.0f * 3.141592f / 180.0f;
m_maxAngle_1 = 45.0f * 3.141592f / 180.0f;
// Get the global matrices of each rigid body.
NewtonBodyGetMatrix(m_body0, &matrix0[0][0]);
dMatrix matrix1 (dGetIdentityMatrix());
if (m_body1) {
NewtonBodyGetMatrix(m_body1, &matrix1[0][0]);
}
// calculate the relative matrix of the pin and pivot on each body
m_localMatrix0 = pinAndPivotFrame * matrix0.Inverse();
m_localMatrix1 = pinAndPivotFrame * matrix1.Inverse();
}
void MoveToTarget (NewtonBody* const targetBody)
{
dAssert ((targetBody == m_triggerPort0) || (targetBody == m_triggerPort1));
if (targetBody != m_currentPort) {
m_currentPort = targetBody;
// get the location of next target
dMatrix targetMatrix;
NewtonBodyGetMatrix(m_currentPort, &targetMatrix[0][0]);
m_target = targetMatrix.m_posit;
// the body might be sleep we need to activate the body by sett the sleep stet off
NewtonBodySetSleepState(GetBody0(), 0);
m_state = m_driving;
}
}
void Init (dFloat location_x, dFloat location_z, PrimitiveType shapeType, int materialID, PrimitiveType castingShapeType)
{
m_pith = dGaussianRandom (3.1416f * 2.0f);
m_yaw = dGaussianRandom (3.1416f * 2.0f);
m_roll = dGaussianRandom (3.1416f * 2.0f);
m_step = 15.0f * (dAbs (dGaussianRandom (0.25f)) + 0.0001f) * 3.1416f/180.0f;
CreatCasterBody(location_x, location_z, shapeType, materialID);
NewtonWorld* const world = ((dCustomControllerManager<dClosestDistanceRecord>*)GetManager())->GetWorld();
DemoEntityManager* const scene = (DemoEntityManager*)NewtonWorldGetUserData(world);
dMatrix matrix;
NewtonBodyGetMatrix(m_body, &matrix[0][0]);
matrix.m_posit.m_y += 10.0f;
m_castingVisualEntity = new ClosestDistanceEntity (scene, matrix, materialID, castingShapeType);
}
dVector CustomPlayerController::CalculateDesiredVelocity (dFloat forwardSpeed, dFloat lateralSpeed, dFloat verticalSpeed, const dVector& gravity, dFloat timestep) const
{
dMatrix matrix;
NewtonBodyGetMatrix(m_body, &matrix[0][0]);
dVector updir (matrix.RotateVector(m_upVector));
dVector frontDir (matrix.RotateVector(m_frontVector));
dVector rightDir (frontDir * updir);
dVector veloc (0.0f, 0.0f, 0.0f, 0.0f);
if ((verticalSpeed <= 0.0f) && (m_groundPlane % m_groundPlane) > 0.0f) {
// plane is supported by a ground plane, apply the player input velocity
if ((m_groundPlane % updir) >= m_maxSlope) {
// player is in a legal slope, he is in full control of his movement
dVector bodyVeloc;
NewtonBodyGetVelocity(m_body, &bodyVeloc[0]);
veloc = updir.Scale(bodyVeloc % updir) + gravity.Scale (timestep) + frontDir.Scale (forwardSpeed) + rightDir.Scale (lateralSpeed) + updir.Scale(verticalSpeed);
veloc += (m_groundVelocity - updir.Scale (updir % m_groundVelocity));
dFloat speedLimitMag2 = forwardSpeed * forwardSpeed + lateralSpeed * lateralSpeed + verticalSpeed * verticalSpeed + m_groundVelocity % m_groundVelocity + 0.1f;
dFloat speedMag2 = veloc % veloc;
if (speedMag2 > speedLimitMag2) {
veloc = veloc.Scale (dSqrt (speedLimitMag2 / speedMag2));
}
dFloat normalVeloc = m_groundPlane % (veloc - m_groundVelocity);
if (normalVeloc < 0.0f) {
veloc -= m_groundPlane.Scale (normalVeloc);
}
} else {
// player is in an illegal ramp, he slides down hill an loses control of his movement
NewtonBodyGetVelocity(m_body, &veloc[0]);
veloc += updir.Scale(verticalSpeed);
veloc += gravity.Scale (timestep);
dFloat normalVeloc = m_groundPlane % (veloc - m_groundVelocity);
if (normalVeloc < 0.0f) {
veloc -= m_groundPlane.Scale (normalVeloc);
}
}
} else {
// player is on free fall, only apply the gravity
NewtonBodyGetVelocity(m_body, &veloc[0]);
veloc += updir.Scale(verticalSpeed);
veloc += gravity.Scale (timestep);
}
return veloc;
}
