void ConvexApproximationObject::AddPolygonFromObject(INode* const node, ObjectState* const os, NewtonMesh* const meshOut, const dMatrix& matrix)
{
BOOL needDel = FALSE;
PolyObject* const poly = GetPolyObject (os, needDel);
if (poly) {
float polygon[32][12];
memset (polygon, 0, sizeof (polygon));
MNMesh& maxMesh = poly->GetMesh();
int facesCount = maxMesh.FNum();
int vertexCount = maxMesh.VNum();
if (facesCount && vertexCount) {
for (int i = 0; i < facesCount; i ++) {
MNFace* const face = maxMesh.F(i);
for (int j = 0; j < face->deg; j ++) {
int index = face->vtx[j];
Point3 p (maxMesh.P(index));
dVector v (matrix.TransformVector(dVector (p.x, p.y, p.z, 0.0f)));
polygon[j][0] = v.m_x;
polygon[j][1] = v.m_y;
polygon[j][2] = v.m_z;
}
NewtonMeshAddFace(meshOut, face->deg, &polygon[0][0], 12 * sizeof (float), 0);
}
}
}
if (needDel) {
delete poly;
}
}
void CalculateAABB (const NewtonCollision* const collision, const dMatrix& matrix, dVector& minP, dVector& maxP)
{
for (int i = 0; i < 3; i ++) {
dVector support(0.0f);
dVector dir (0.0f);
dir[i] = 1.0f;
dVector localDir (matrix.UnrotateVector (dir));
NewtonCollisionSupportVertex (collision, &localDir[0], &support[0]);
support = matrix.TransformVector (support);
maxP[i] = support[i];
localDir = localDir.Scale (-1.0f);
NewtonCollisionSupportVertex (collision, &localDir[0], &support[0]);
support = matrix.TransformVector (support);
minP[i] = support[i];
}
}
void CalculateAABB (const NewtonCollision* const collision, const dMatrix& matrix, dVector& minP, dVector& maxP)
{
dFloat skinThickness = NewtonCollisionGetSkinThickness (collision) * 0.125f;
for (int i = 0; i < 3; i ++) {
dVector support;
dVector dir (0.0f, 0.0f, 0.0f, 0.0f);
dir[i] = 1.0f;
dVector localDir (matrix.UnrotateVector (dir));
NewtonCollisionSupportVertex (collision, &localDir[0], &support[0]);
support = matrix.TransformVector (support);
maxP[i] = support[i] - skinThickness;
localDir = localDir.Scale (-1.0f);
NewtonCollisionSupportVertex (collision, &localDir[0], &support[0]);
support = matrix.TransformVector (support);
minP[i] = support[i] + skinThickness;
}
}
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);
}
void ApplySuspesionForce (
dFloat timestep,
const NewtonBody* thread, const dVector& threadPointLocal, const dMatrix& threadMatrix, const dVector& threadCOM, const dVector& threadVeloc, const dVector& threadOmega,
const NewtonBody* parent, const dVector& parentPointLocal, const dMatrix& parentMatrix, const dVector& parentCOM, const dVector& parentVeloc, const dVector& parentOmega)
{
dFloat dist;
dFloat speed;
dFloat forceMag;
// calculate separation and speed of hard points
dVector threadPoint (threadMatrix.TransformVector(threadPointLocal));
dVector parentPoint (parentMatrix.TransformVector(parentPointLocal));
dist = (parentPoint - threadPoint) % parentMatrix.m_up;
speed = ((parentVeloc + parentOmega * (parentPoint - parentCOM) -
threadVeloc - threadOmega * (threadPoint - threadCOM)) % parentMatrix.m_up);
if (dist > MAX_COMPRESION_DIST) {
NewtonUserJointAddLinearRow (m_joint, &threadPoint[0], &threadPoint[0], &parentMatrix.m_up[0]);
NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
} else if (dist < MIN_EXPANSION_DIST) {
// submit a contact constraint to prevent the body
NewtonUserJointAddLinearRow (m_joint, &threadPoint[0], &threadPoint[0], &parentMatrix.m_up[0]);
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
}
// apply the spring force
forceMag = NewtonCalculateSpringDamperAcceleration (timestep, SPRING_CONST, dist, DAMPER_CONST, speed) * m_massScale;
dVector forceParent (parentMatrix.m_up.Scale (forceMag));
dVector torqueParent ((parentPoint - parentCOM) * forceParent);
NewtonBodyAddForce(m_body1, &forceParent[0]);
NewtonBodyAddTorque(m_body1, &torqueParent[0]);
dVector forceThread (forceParent.Scale (-1.0f));
dVector torqueThread ((threadPoint - threadCOM) * forceThread);
NewtonBodyAddForce(m_body0, &forceThread[0]);
NewtonBodyAddTorque(m_body0, &torqueThread[0]);
}
void CustomDGRayCastCar::CalculateTireCollision (Tire& tire, const dMatrix& suspensionMatrix, int threadIndex) const
{
int floorcontact = 0;
tire.m_HitBody = NULL;
tire.m_posit = tire.m_suspensionLenght;
if ( tire.m_tireUseConvexCastMode ) {
dFloat hitParam;
NewtonWorldConvexCastReturnInfo info;
// tire.m_rayDestination = tire.m_suspensionMatrix.TransformVector (m_localFrame.m_up.Scale ( -tire.m_suspensionLenght ));
dVector rayDestination (suspensionMatrix.TransformVector (m_localFrame.m_up.Scale ( -tire.m_suspensionLenght)));
if ( NewtonWorldConvexCast ( m_world, &suspensionMatrix[0][0], &rayDestination[0], tire.m_shape, &hitParam, (void*)m_body0, ConvexCastPrefilter, &info, 1, threadIndex ) ){
tire.m_posit = hitParam * tire.m_suspensionLenght;
tire.m_contactPoint = info.m_point;
tire.m_contactNormal = info.m_normal;
tire.m_HitBody = (NewtonBody*)info.m_hitBody;
floorcontact = 1;
}
} else {
struct RayCastInfo
{
RayCastInfo (const NewtonBody* body)
{
m_param = 1.0f;
m_me = body;
m_hitBody = NULL;
m_contactID = 0;
m_normal = dVector (0.0f, 0.0f, 0.0f, 1.0f);
}
static dFloat RayCast (const NewtonBody* body, const dFloat* normal, int collisionID, void* userData, dFloat intersetParam)
{
RayCastInfo& caster = *( (RayCastInfo*) userData );
// if this body is not the vehicle, see if a close hit
if ( body != caster.m_me ) {
if ( intersetParam < caster.m_param) {
// this is a close hit, record the information.
caster.m_param = intersetParam;
caster.m_hitBody = body;
caster.m_contactID = collisionID;
caster.m_normal = dVector (normal[0], normal[1], normal[2], 1.0f);
}
}
return intersetParam;
}
dFloat m_param;
dVector m_normal;
const NewtonBody* m_me;
const NewtonBody* m_hitBody;
int m_contactID;
};
RayCastInfo info (m_body0);
_ASSERTE (0);
// extend the ray by the radius of the tire
dFloat dist ( tire.m_suspensionLenght + tire.m_radius );
dVector rayDestination (suspensionMatrix.TransformVector (m_localFrame.m_up.Scale ( -dist )));
// cast a ray to the world ConvexCastPrefilter
NewtonWorldRayCast( m_world, &suspensionMatrix.m_posit[0], &rayDestination[0], RayCastInfo::RayCast, &info, &ConvexCastPrefilter );
// if the ray hit something, it means the tire has some traction
if ( info.m_hitBody ) {
dFloat intesectionDist;
tire.m_HitBody = (NewtonBody*)info.m_hitBody;
tire.m_contactPoint = suspensionMatrix.m_posit + (rayDestination - suspensionMatrix.m_posit ).Scale ( info.m_param );
tire.m_contactNormal = info.m_normal;
// TO DO: get the material properties for tire frictions on different roads
intesectionDist = ( dist * info.m_param - tire.m_radius );
if ( intesectionDist < 0.0f ) {
intesectionDist = 0.0f;
} else if ( intesectionDist > tire.m_suspensionLenght ) {
intesectionDist = tire.m_suspensionLenght;
}
tire.m_posit = intesectionDist;
}
}
}
