bool MousePick (NewtonWorld* nWorld, const dMOUSE_POINT& mouse1, dInt32 mouseLeftKey1, dFloat witdh, dFloat length)
{
static int mouseLeftKey0;
static dMOUSE_POINT mouse0;
static bool mousePickMode = false;
dMatrix matrix;
witdh;
if (mouseLeftKey1) {
if (!mouseLeftKey0) {
dVector p0 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 0.0f, 0.0f)));
dVector p1 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 1.0f, 0.0f)));
pickedBody = NULL;
pickedParam = 1.1f;
isPickedBodyDynamics = false;
NewtonWorldRayCast(nWorld, &p0[0], &p1[0], RayCastFilter, NULL, RayCastPrefilter);
if (pickedBody) {
mousePickMode = true;
//NewtonBodySetFreezeState (pickedBody, 0);
NewtonBodyGetMatrix(pickedBody, &matrix[0][0]);
dVector p (p0 + (p1 - p0).Scale (pickedParam));
// save point local to th body matrix
attachmentPoint = matrix.UntransformVector (p);
// convert normal to local space
rayLocalNormal = matrix.UnrotateVector(rayLocalNormal);
// save the transform call back
chainForceCallback = NewtonBodyGetForceAndTorqueCallback (pickedBody);
dAssert (chainForceCallback != PhysicsApplyPickForce);
// set a new call back
NewtonBodySetForceAndTorqueCallback (pickedBody, PhysicsApplyPickForce);
}
}
if (mousePickMode) {
// init pick mode
dMatrix matrix;
NewtonBodyGetMatrix(pickedBody, &matrix[0][0]);
dVector p0 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 0.0f, 0.0f)));
dVector p1 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 1.0f, 0.0f)));
dVector p2 (matrix.TransformVector (attachmentPoint));
dVector p (p0 + (p1 - p0).Scale (pickedParam));
pickedForce = p - p2;
dFloat mag2 = pickedForce % pickedForce;
if (mag2 > dFloat (20 * 20)) {
pickedForce = pickedForce.Scale (20.0f / dSqrt (pickedForce % pickedForce));
}
// rotate normal to global space
rayWorldNormal = matrix.RotateVector(rayLocalNormal);
// rayWorldOrigin = p2;
// show the pick points
//ShowMousePicking (p, p2, witdh);
ShowMousePicking (p, p2);
//ShowMousePicking (p2, p2 + rayWorldNormal.Scale (length), witdh);
ShowMousePicking (p2, p2 + rayWorldNormal.Scale (length));
}
} else {
mousePickMode = false;
if (pickedBody) {
//dAssert (chainForceCallback != NewtonBodyGetForceAndTorqueCallback (pickedBody));
dAssert (chainForceCallback != PhysicsApplyPickForce);
NewtonBodySetForceAndTorqueCallback (pickedBody, chainForceCallback);
pickedBody = NULL;
chainForceCallback = NULL;
}
}
mouse0 = mouse1;
mouseLeftKey0 = mouseLeftKey1;
bool retState;
retState = isPickedBodyDynamics;
return retState;
}
int CustomMultiBodyVehicle::AddSingleSuspensionTire (
void* userData,
const dVector& localPosition,
dFloat mass,
dFloat radius,
dFloat width,
dFloat suspensionLength,
dFloat springConst,
dFloat springDamper)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dMatrix carMatrix;
NewtonBody* tire;
NewtonWorld* world;
NewtonCollision *collision;
world = NewtonBodyGetWorld(GetBody0());
// create the tire RogidBody
collision = NewtonCreateChamferCylinder(world, radius, width, 0, NULL);
//create the rigid body
tire = NewtonCreateBody (world, collision);
// release the collision
NewtonReleaseCollision (world, collision);
// save the user data
NewtonBodySetUserData (tire, userData);
// set the material group id for vehicle
NewtonBodySetMaterialGroupID (tire, 0);
// NewtonBodySetMaterialGroupID (tire, woodID);
// set the force and torque call back function
NewtonBodySetForceAndTorqueCallback (tire, NewtonBodyGetForceAndTorqueCallback (GetBody0()));
// body part do not collision
NewtonBodySetJointRecursiveCollision (tire, 0);
// calculate the moment of inertia and the relative center of mass of the solid
dVector origin;
dVector inertia;
NewtonConvexCollisionCalculateInertialMatrix (collision, &inertia[0], &origin[0]);
Ixx = mass * inertia[0];
Iyy = mass * inertia[1];
Izz = mass * inertia[2];
// set the mass matrix
NewtonBodySetMassMatrix (tire, mass, Ixx, Iyy, Izz);
// calculate the tire local base pose matrix
dMatrix tireMatrix;
tireMatrix.m_front = m_localFrame.m_right;
tireMatrix.m_up = m_localFrame.m_up;
tireMatrix.m_right = tireMatrix.m_front * tireMatrix.m_up;
tireMatrix.m_posit = localPosition;
NewtonBodyGetMatrix(GetBody0(), &carMatrix[0][0]);
tireMatrix = tireMatrix * carMatrix;
// set the matrix for both the rigid body and the graphic body
NewtonBodySetMatrix (tire, &tireMatrix[0][0]);
// add a single tire
m_tires[m_tiresCount] = new CustomMultiBodyVehicleTire (GetBody0(), tire, suspensionLength, springConst, springDamper, radius);
m_tiresCount ++;
return m_tiresCount - 1;
}
