void dCustomTireSpringDG::SubmitConstraints(dFloat timestep, int threadIndex) { NewtonBody* BodyAttach; //NewtonBody* BodyFrame; // dVector tireOmega = dVector(0.0f, 0.0f, 0.0f); //BodyFrame = GetBody0(); BodyAttach = GetBody1(); // SteeringController(timestep); // // calculate the position of the pivot point and the Jacobian direction vectors, in global space. CalculateGlobalMatrix(mChassisPivotMatrix, mTirePivotMatrix); // NewtonBodyGetOmega(BodyAttach, &tireOmega[0]); // mRealOmega = dAbs(tireOmega.DotProduct3(mChassisPivotMatrix.m_front)); // TireCenterPin(timestep); // TireCenterBolt(timestep); // SuspenssionSpringLimits(timestep); // TireBreakAction(BodyAttach, timestep); }
void 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); dVector p0(matrix0.m_posit); dVector p1(matrix1.m_posit); dVector dir(p1 - p0); dFloat mag2 = dir % dir; dir = dir.Scale(1.0f / dSqrt(mag2)); dMatrix matrix(dGrammSchmidt(dir)); dFloat x = dSqrt(mag2) - m_distance; dVector com0; dVector com1; dVector veloc0; dVector veloc1; dMatrix body0Matrix; dMatrix body1Matrix; NewtonBody* const body0 = GetBody0(); NewtonBody* const body1 = GetBody1(); NewtonBodyGetCentreOfMass(body0, &com0[0]); NewtonBodyGetMatrix(body0, &body0Matrix[0][0]); NewtonBodyGetPointVelocity(body0, &p0[0], &veloc0[0]); NewtonBodyGetCentreOfMass(body1, &com1[0]); NewtonBodyGetMatrix(body1, &body1Matrix[0][0]); NewtonBodyGetPointVelocity(body1, &p1[0], &veloc1[0]); dFloat v((veloc0 - veloc1) % dir); dFloat a = (x - v * timestep) / (timestep * timestep); dVector r0((p0 - body0Matrix.TransformVector(com0)) * matrix.m_front); dVector r1((p1 - body1Matrix.TransformVector(com1)) * matrix.m_front); dFloat jacobian0[6]; dFloat jacobian1[6]; jacobian0[0] = matrix[0][0]; jacobian0[1] = matrix[0][1]; jacobian0[2] = matrix[0][2]; jacobian0[3] = r0[0]; jacobian0[4] = r0[1]; jacobian0[5] = r0[2]; jacobian1[0] = -matrix[0][0]; jacobian1[1] = -matrix[0][1]; jacobian1[2] = -matrix[0][2]; jacobian1[3] = -r1[0]; jacobian1[4] = -r1[1]; jacobian1[5] = -r1[2]; NewtonUserJointAddGeneralRow(m_joint, jacobian0, jacobian1); NewtonUserJointSetRowAcceleration(m_joint, a); }
void dgConstraint::InitInfo (dgConstraintInfo* const info) const { info->m_attachBody_0 = GetBody0(); dgAssert (info->m_attachBody_0); dgWorld* const world = info->m_attachBody_0->GetWorld(); if (info->m_attachBody_0 == (dgBody*)world->GetSentinelBody()) { info->m_attachBody_0 = NULL; } info->m_attachBody_1 = GetBody1(); if (info->m_attachBody_1 == (dgBody*)world->GetSentinelBody()) { info->m_attachBody_1 = NULL; } info->m_attachMatrix_0 = dgGetIdentityMatrix(); info->m_attachMatrix_1 = dgGetIdentityMatrix(); info->m_discriptionType[0] = 0; }
void dCustomTireSpringDG::TireMatrixProjection() { NewtonBody* tire; NewtonBody* chassis; // dMatrix tireMatrix; dMatrix chassisMatrix; dMatrix tireMatrixInGlobalSpace; dMatrix chassisMatrixInGlobalSpace; dMatrix projectedChildMatrixInGlobalSpace; // dVector projectDist; dVector projTireOmega; dVector chassisOmega; dVector tireOmega; dVector tireOmegaCorrection; // // D.G: Thanks Julio for the helps in the past for this part of code. // D.G: This code come from a pretty old vehicle implementation. tire = GetBody1(); chassis = GetBody0(); // NewtonBodyGetMatrix(tire, &tireMatrix[0][0]); NewtonBodyGetMatrix(chassis, &chassisMatrix[0][0]); // // project the tire matrix to the right space. tireMatrixInGlobalSpace = (m_localMatrix1 * tireMatrix); chassisMatrixInGlobalSpace = (m_localMatrix0 * chassisMatrix); // projectDist = (tireMatrixInGlobalSpace.m_posit - chassisMatrixInGlobalSpace.m_posit).DotProduct3(chassisMatrixInGlobalSpace.m_up); chassisMatrixInGlobalSpace.m_posit = chassisMatrixInGlobalSpace.m_posit + (chassisMatrixInGlobalSpace.m_up * projectDist); // chassisMatrixInGlobalSpace.m_up = tireMatrixInGlobalSpace.m_right.CrossProduct(chassisMatrixInGlobalSpace.m_front); chassisMatrixInGlobalSpace.m_up = chassisMatrixInGlobalSpace.m_up * (1.0f / dSqrt(chassisMatrixInGlobalSpace.m_up.DotProduct3(chassisMatrixInGlobalSpace.m_up))); chassisMatrixInGlobalSpace.m_right = chassisMatrixInGlobalSpace.m_front.CrossProduct(chassisMatrixInGlobalSpace.m_up); chassisMatrixInGlobalSpace.m_up.m_w = 0.0; chassisMatrixInGlobalSpace.m_right.m_w = 0.0; // projectedChildMatrixInGlobalSpace = (m_localMatrix1.Inverse() * chassisMatrixInGlobalSpace); NewtonBodySetMatrix(tire, &projectedChildMatrixInGlobalSpace[0][0]); // NewtonBodyGetOmega(chassis, &chassisOmega[0]); // D.G: Temporary disabled. // D.G: The result is a lot better without this part. // D.G: Causing problems, something is calculed wrong and the result give some bad bounce force on the wheels. // /* NewtonBodyGetCentreOfMass(chassis, @chassisCom.V[0]); NewtonBodyGetMatrix(chassis, @rlmat.V[0].V[0]); chassisCom: = OXTransformVector(chassisCom, rlmat); chassisVeloc: = VectorAdd(chassisVeloc, VectorCrossProduct(chassisOmega, VectorSubtract(chassisMatrixInGlobalSpace.m_posit, chassisCom))); projTireVeloc: = VectorSubtract{ VectorAdd }(chassisVeloc, VectorScale(chassisMatrixInGlobalSpace.m_up, VectorDotProduct(chassisVeloc, chassisMatrixInGlobalSpace.m_up))); projTireVeloc: = VectorAdd{ VectorSubtract }(projTireVeloc, VectorScale(chassisMatrixInGlobalSpace.m_up, VectorDotProduct(tireVeloc, chassisMatrixInGlobalSpace.m_up))); //NegateVector(projTireVeloc); NewtonBodySetVelocity(tire, @projTireVeloc.V[0]); */ // project angular velocity NewtonBodyGetOmega(tire, &tireOmega[0]); tireOmegaCorrection = tireOmega; // if (GetVecLength(tireOmegaCorrection) > mFpsRequest) { // I need to use this omega correction fix when NewtonBodySetLinearDamping,NewtonBodySetAngularDamping is set to zero. // Because the tire rotation overpass the physics rotation limit in time, and it can give bad result without this fix. // I prefered to have the body totally free rolling, because of this I need this fix. // If you don't use this fix, Don't set the NewtonBodySetLinearDamping,NewtonBodySetAngularDamping to zero value, just don't call this both functions on the body at all. // //printf("Omega tire correction, Fix a Rotation limitation. \n"); tireOmegaCorrection = (tireOmegaCorrection * mTireOmegaCorrection); NewtonBodySetOmega(tire, &tireOmegaCorrection[0]); } // projTireOmega = chassisOmega - chassisMatrixInGlobalSpace.m_front * (chassisOmega.DotProduct3(chassisMatrixInGlobalSpace.m_front)); projTireOmega = projTireOmega + chassisMatrixInGlobalSpace.m_front * (tireOmegaCorrection.DotProduct3(chassisMatrixInGlobalSpace.m_front)); NewtonBodySetOmega(tire, &projTireOmega[0]); }
void SubmitConstraints (dFloat timestep, int threadIndex) { dMatrix tirePivotMatrix; dMatrix chassisPivotMatrix; ProjectTireMatrix(); // calculate the position of the pivot point and the Jacobian direction vectors, in global space. CalculateGlobalMatrix (m_chassisLocalMatrix, m_tireLocalMatrix, chassisPivotMatrix, tirePivotMatrix); // Restrict the movement on the pivot point along all two orthonormal direction dVector centerInTire (tirePivotMatrix.m_posit); dVector centerInChassis (chassisPivotMatrix.m_posit + chassisPivotMatrix.m_up.Scale ((centerInTire - chassisPivotMatrix.m_posit) % chassisPivotMatrix.m_up)); NewtonUserJointAddLinearRow (m_joint, ¢erInChassis[0], ¢erInTire[0], &chassisPivotMatrix.m_front[0]); NewtonUserJointAddLinearRow (m_joint, ¢erInChassis[0], ¢erInTire[0], &chassisPivotMatrix.m_right[0]); // get a point along the pin axis at some reasonable large distance from the pivot dVector pointInPinInTire (centerInChassis + chassisPivotMatrix.m_front.Scale(MIN_JOINT_PIN_LENGTH)); dVector pointInPinInChassis (centerInTire + tirePivotMatrix.m_front.Scale(MIN_JOINT_PIN_LENGTH)); NewtonUserJointAddLinearRow (m_joint, &pointInPinInTire[0], &pointInPinInChassis[0], &chassisPivotMatrix.m_right[0]); NewtonUserJointAddLinearRow (m_joint, &pointInPinInTire[0], &pointInPinInChassis[0], &chassisPivotMatrix.m_up[0]); //calculate the suspension spring and damper force dFloat dist; dFloat speed; dFloat force; dVector tireVeloc; dVector chassisVeloc; dVector chassisOmega; dVector chassisCom; dMatrix chassisMatrix; const NewtonBody* tire; const NewtonBody* chassis; // calculate the velocity of tire attachments point on the car chassis tire = GetBody1(); chassis = GetBody0(); NewtonBodyGetVelocity (tire, &tireVeloc[0]); NewtonBodyGetVelocity (chassis, &chassisVeloc[0]); NewtonBodyGetOmega (chassis, &chassisOmega[0]); NewtonBodyGetMatrix (chassis, &chassisMatrix[0][0]); NewtonBodyGetCentreOfMass (chassis, &chassisCom[0]); chassisCom = chassisMatrix.TransformVector(chassisCom); chassisVeloc += chassisOmega * (centerInChassis - chassisCom); // get the spring damper parameters speed = (chassisVeloc - tireVeloc) % chassisPivotMatrix.m_up; dist = (chassisPivotMatrix.m_posit - tirePivotMatrix.m_posit) % chassisPivotMatrix.m_up; // check if the suspension pass the bumpers limits if (-dist > m_suspenstionSpan* 0.5f) { // if it hit the bumpers then speed is zero speed = 0; NewtonUserJointAddLinearRow (m_joint, ¢erInChassis[0], ¢erInChassis[0], &chassisPivotMatrix.m_up[0]); NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f); } else if (dist > 0.0f) { // if it hit the bumpers then speed is zero speed = 0; NewtonUserJointAddLinearRow (m_joint, ¢erInChassis[0], ¢erInChassis[0], &chassisPivotMatrix.m_up[0]); NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f); } // calculate magnitude of suspension force force = NewtonCalculateSpringDamperAcceleration (timestep, m_spring, dist, m_damper, speed) * m_effectiveSpringMass; dVector chassisForce (chassisMatrix.m_up.Scale (force)); dVector chassisTorque ((centerInChassis - chassisCom) * chassisForce); NewtonBodyAddForce (chassis, &chassisForce[0]); NewtonBodyAddTorque (chassis, &chassisTorque[0]); dVector tireForce (chassisForce.Scale (-1.0f)); NewtonBodyAddForce(tire, &tireForce[0]); // apply the engine torque to tire torque dFloat relOmega; dMatrix tireMatrix; dVector tireOmega; NewtonBodyGetOmega(tire, &tireOmega[0]); NewtonBodyGetMatrix (tire, &tireMatrix[0][0]); relOmega = ((tireOmega - chassisOmega) % tireMatrix.m_front); // apply engine torque plus some tire angular drag dVector tireTorque (tireMatrix.m_front.Scale (m_enginetorque - relOmega * m_Ixx * m_angularDragCoef)); NewtonBodyAddTorque (tire, &tireTorque[0]); dVector chassisReationTorque (chassisMatrix.m_right.Scale (- m_enginetorque)); NewtonBodyAddTorque(chassis, &chassisTorque[0]); m_enginetorque = 0.0f; // add the brake torque row if (dAbs(m_brakeToque) > 1.0e-3f) { relOmega /= timestep; NewtonUserJointAddAngularRow (m_joint, 0.0f, &tireMatrix.m_front[0]); NewtonUserJointSetRowAcceleration(m_joint, relOmega); NewtonUserJointSetRowMaximumFriction(m_joint, m_brakeToque); NewtonUserJointSetRowMinimumFriction(m_joint, -m_brakeToque); } m_brakeToque = 0.0f; }