dQuaternion dQuaternion::Slerp (const dQuaternion &QB, dFloat t) const
{
dFloat dot;
dFloat ang;
dFloat Sclp;
dFloat Sclq;
dFloat den;
dFloat sinAng;
dQuaternion Q;
dot = DotProduct (QB);
_ASSERTE (dot >= 0.0f);
if ((dot + dFloat(1.0f)) > dFloat(1.0e-5f)) {
if (dot < dFloat(0.995f)) {
ang = dAcos (dot);
sinAng = dSin (ang);
den = dFloat(1.0f) / sinAng;
Sclp = dSin ((dFloat(1.0f) - t ) * ang) * den;
Sclq = dSin (t * ang) * den;
} else {
Sclp = dFloat(1.0f) - t;
Sclq = t;
}
Q.m_q0 = m_q0 * Sclp + QB.m_q0 * Sclq;
Q.m_q1 = m_q1 * Sclp + QB.m_q1 * Sclq;
Q.m_q2 = m_q2 * Sclp + QB.m_q2 * Sclq;
Q.m_q3 = m_q3 * Sclp + QB.m_q3 * Sclq;
} else {
Q.m_q0 = m_q3;
Q.m_q1 = -m_q2;
Q.m_q2 = m_q1;
Q.m_q3 = m_q0;
Sclp = dSin ((dFloat(1.0f) - t) * dFloat (3.141592f *0.5f));
Sclq = dSin (t * dFloat (3.141592f * 0.5f));
Q.m_q0 = m_q0 * Sclp + Q.m_q0 * Sclq;
Q.m_q1 = m_q1 * Sclp + Q.m_q1 * Sclq;
Q.m_q2 = m_q2 * Sclp + Q.m_q2 * Sclq;
Q.m_q3 = m_q3 * Sclp + Q.m_q3 * Sclq;
}
dot = Q.DotProduct (Q);
if ((dot) < (1.0f - 1.0e-4f) ) {
dot = dFloat(1.0f) / dSqrt (dot);
//dot = dgRsqrt (dot);
Q.m_q0 *= dot;
Q.m_q1 *= dot;
Q.m_q2 *= dot;
Q.m_q3 *= dot;
}
return Q;
}
void DynamicRagDollJoint::SetConeAngle(dFloat angle)
{
m_coneAngle = angle;
m_coneAngleCos = dCos(angle);
m_coneAngleSin = dSin(angle);
m_coneAngleHalfCos = dCos(angle * 0.5f);
m_coneAngleHalfSin = dSin(angle * 0.5f);
}
void CustomLimitBallAndSocket::SetConeAngle (dFloat angle)
{
m_coneAngle = angle;
m_coneAngleCos = dCos (angle);
m_coneAngleSin = dSin (angle);
m_coneAngleHalfCos = dCos (angle * 0.5f);
m_coneAngleHalfSin = dSin (angle * 0.5f);
}
// this calculate the desired position and orientation in the path
dMatrix BuildMatrix (dFloat timestep)
{
dMatrix matrix (dPitchMatrix(-m_angle) * dRollMatrix(-m_angle));
matrix.m_posit = m_origin;
matrix.m_posit.m_z += radius * dSin (m_angle);
matrix.m_posit.m_y += radius * dCos (m_angle);
return matrix;
}
static inline dReal sinc (dReal x)
{
// if |x| < 1e-4 then use a taylor series expansion. this two term expansion
// is actually accurate to one LS bit within this range if double precision
// is being used - so don't worry!
if (dFabs(x) < 1.0e-4) return REAL(1.0) - x*x*REAL(0.166666666666666666667);
else return dSin(x)/x;
}
void AddCollisionHeightFieldMesh(DemoEntityManager* const scene)
{
int size = 101;
dFloat* const elevation = new dFloat[size * size];
//y = a * sin(x)* (sin (z)) ^2
for (int i = 0; i < size; i ++) {
dFloat z = 3.1416f * i / (size - 1);
dFloat z2 = dSin (z);
z2 = z2 * z2 * 5.0f;
for (int j = 0; j < size; j ++) {
dFloat x = 3.1416f * j / (size - 1);
dFloat y = z2 * dSin(x);
elevation[j * size + i] = y;
}
}
dFloat cellsize = 0.25f;
DemoMesh* const mesh = new DemoMesh ("terrain", elevation, size, cellsize, 1.0f/16.0f, size - 1);
int width = size;
int height = size;
char* const attibutes = new char [size * size];
memset (attibutes, 0, width * height * sizeof (char));
NewtonCollision* const collision = NewtonCreateHeightFieldCollision (scene->GetNewton(), width, height, 1, 0, elevation, attibutes, 1.0f, cellsize, 0);
void* const proxy = NewtonSceneCollisionAddSubCollision (m_sceneCollision, collision);
NewtonDestroyCollision(collision);
// set the parameter on the added collision share
dMatrix matrix (dGetIdentityMatrix());
matrix.m_posit.m_y = -2.0f;
matrix.m_posit.m_x = -15.0f;
matrix.m_posit.m_z = -10.0f;
NewtonCollision* const terrainCollision = NewtonSceneCollisionGetCollisionFromNode (m_sceneCollision, proxy );
NewtonSceneCollisionSetSubCollisionMatrix (m_sceneCollision, proxy, &matrix[0][0]);
NewtonCollisionSetUserData(terrainCollision, mesh);
delete[] elevation;
delete[] attibutes;
}
dMatrix dRollMatrix (dFloat ang)
{
dFloat cosAng;
dFloat sinAng;
sinAng = dSin (ang);
cosAng = dCos (ang);
return dMatrix (dVector ( cosAng, sinAng, 0.0f, 0.0f),
dVector (-sinAng, cosAng, 0.0f, 0.0f),
dVector ( 0.0f, 0.0f, 1.0f, 0.0f),
dVector ( 0.0f, 0.0f, 0.0f, 1.0f));
}
void CustomDGRayCastCar::SetTireSteerAngleForce (int index, dFloat angle, dFloat turnforce)
{
m_tires[index].m_steerAngle = angle;
m_tires[index].m_localAxis.m_z = dCos (angle);
m_tires[index].m_localAxis.m_x = dSin (angle);
// if (m_tiresRollSide==0) {
// m_tires[index].m_turnforce = turnforce;
// } else {
// m_tires[index].m_turnforce = -turnforce;
// }
}
EXPORT_C void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi)
{
dReal sphi,cphi,stheta,ctheta,spsi,cpsi;
sphi = dSin(phi);
cphi = dCos(phi);
stheta = dSin(theta);
ctheta = dCos(theta);
spsi = dSin(psi);
cpsi = dCos(psi);
_R(0,0) = dMUL(cpsi,ctheta);
_R(0,1) = dMUL(spsi,ctheta);
_R(0,2) =-stheta;
_R(0,3) = REAL(0.0);
_R(1,0) = dMUL(cpsi,dMUL(stheta,sphi)) - dMUL(spsi,cphi);
_R(1,1) = dMUL(spsi,dMUL(stheta,sphi)) + dMUL(cpsi,cphi);
_R(1,2) = dMUL(ctheta,sphi);
_R(1,3) = REAL(0.0);
_R(2,0) = dMUL(cpsi,dMUL(stheta,cphi)) + dMUL(spsi,sphi);
_R(2,1) = dMUL(spsi,dMUL(stheta,cphi)) - dMUL(cpsi,sphi);
_R(2,2) = dMUL(ctheta,cphi);
_R(2,3) = REAL(0.0);
}
void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi)
{
dReal sphi,cphi,stheta,ctheta,spsi,cpsi;
dAASSERT (R);
sphi = dSin(phi);
cphi = dCos(phi);
stheta = dSin(theta);
ctheta = dCos(theta);
spsi = dSin(psi);
cpsi = dCos(psi);
_R(0,0) = cpsi*ctheta;
_R(0,1) = spsi*ctheta;
_R(0,2) =-stheta;
_R(0,3) = REAL(0.0);
_R(1,0) = cpsi*stheta*sphi - spsi*cphi;
_R(1,1) = spsi*stheta*sphi + cpsi*cphi;
_R(1,2) = ctheta*sphi;
_R(1,3) = REAL(0.0);
_R(2,0) = cpsi*stheta*cphi + spsi*sphi;
_R(2,1) = spsi*stheta*cphi - cpsi*sphi;
_R(2,2) = ctheta*cphi;
_R(2,3) = REAL(0.0);
}
dQuaternion::dQuaternion (const dVector &unitAxis, dFloat angle)
{
angle *= dFloat (0.5f);
m_q0 = dCos (angle);
dFloat sinAng = dSin (angle);
#ifdef _DEBUG
if (dAbs (angle) > dFloat(1.0e-6f)) {
dAssert (dAbs (dFloat(1.0f) - unitAxis % unitAxis) < dFloat(1.0e-3f));
}
#endif
m_q1 = unitAxis.m_x * sinAng;
m_q2 = unitAxis.m_y * sinAng;
m_q3 = unitAxis.m_z * sinAng;
}
void _InitCylinderTrimeshData(sData& cData)
{
// get cylinder information
// Rotation
const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder);
dMatrix3Copy(pRotCyc,cData.mCylinderRot);
dGeomGetQuaternion(cData.gCylinder,cData.qCylinderRot);
// Position
const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder);
dVector3Copy(*pPosCyc,cData.vCylinderPos);
// Cylinder axis
dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis);
// get cylinder radius and size
dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize);
// get trimesh position and orientation
const dReal* pRotTris = dGeomGetRotation(cData.gTrimesh);
dMatrix3Copy(pRotTris,cData.mTrimeshRot);
dGeomGetQuaternion(cData.gTrimesh,cData.qTrimeshRot);
// Position
const dVector3* pPosTris = (const dVector3*)dGeomGetPosition(cData.gTrimesh);
dVector3Copy(*pPosTris,cData.vTrimeshPos);
// calculate basic angle for 8-gon
dReal fAngle = M_PI / nCYLINDER_CIRCLE_SEGMENTS;
// calculate angle increment
dReal fAngleIncrement = fAngle*REAL(2.0);
// calculate plane normals
// axis dependant code
for(int i=0; i<nCYLINDER_CIRCLE_SEGMENTS; i++)
{
cData.avCylinderNormals[i][0] = -dCos(fAngle);
cData.avCylinderNormals[i][1] = -dSin(fAngle);
cData.avCylinderNormals[i][2] = REAL(0.0);
fAngle += fAngleIncrement;
}
dSetZero(cData.vBestPoint,4);
// reset best depth
cData.fBestCenter = REAL(0.0);
}
dEffectorWalkPoseGenerator(NewtonBody* const rootBody)
:dEffectorTreeFixPose(rootBody)
,m_acc(0.0f)
,m_amplitud_x(0.35f)
,m_amplitud_y(0.1f)
,m_period (1.0f)
,m_cycle()
{
m_sequence[0] = 0;
m_sequence[3] = 0;
m_sequence[4] = 0;
m_sequence[1] = 1;
m_sequence[2] = 1;
m_sequence[5] = 1;
// make left walk cycle
const int size = 11;
const int splite = (size - 1) / 2 - 1;
dFloat64 knots[size];
dBigVector leftControlPoints[size + 2];
for (int i = 0; i < size; i ++) {
knots[i] = dFloat (i) / (size - 1);
}
memset (leftControlPoints, 0, sizeof (leftControlPoints));
dFloat x = -m_amplitud_x / 2.0f;
dFloat step_x = m_amplitud_x / splite;
for (int i = 0; i <= splite; i ++) {
leftControlPoints[i + 1].m_y = m_amplitud_y * dSin (dPi * dFloat (i) / splite);
leftControlPoints[i + 1].m_x = x;
x += step_x;
}
x = m_amplitud_x / 2.0f;
step_x = -m_amplitud_x / (size - splite - 1);
for (int i = splite; i < size; i++) {
leftControlPoints[i + 1].m_x = x;
x += step_x;
}
leftControlPoints[0].m_x = leftControlPoints[1].m_x;
leftControlPoints[size + 1].m_x = leftControlPoints[size].m_x;
// cycle.CreateFromKnotVectorAndControlPoints(3, size, knots, leftControlPoints);
m_cycle.CreateFromKnotVectorAndControlPoints(1, size, knots, &leftControlPoints[1]);
}
void dCustomCorkScrew::SubmitAngularRow(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
dMatrix localMatrix(matrix0 * matrix1.Inverse());
dVector euler0;
dVector euler1;
localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw);
dVector rollPin(dSin(euler0[1]), dFloat(0.0f), dCos(euler0[1]), dFloat(0.0f));
rollPin = matrix1.RotateVector(rollPin);
NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddAngularRow(m_joint, -euler0[2], &rollPin[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
// the joint angle can be determined by getting the angle between any two non parallel vectors
m_curJointAngle.Update(euler0.m_x);
// save the current joint Omega
dVector omega0(0.0f);
dVector omega1(0.0f);
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
m_angularOmega = (omega0 - omega1).DotProduct3(matrix1.m_front);
if (m_options.m_option2) {
if (m_options.m_option3) {
dCustomCorkScrew::SubmitConstraintLimitSpringDamper(matrix0, matrix1, timestep);
} else {
dCustomCorkScrew::SubmitConstraintLimits(matrix0, matrix1, timestep);
}
} else if (m_options.m_option3) {
dCustomCorkScrew::SubmitConstraintSpringDamper(matrix0, matrix1, timestep);
} else if (m_angularFriction != 0.0f) {
NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowAcceleration(m_joint, -m_angularOmega / timestep);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
}
}
EXPORT_C void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az,
dReal angle)
{
dReal l = dMUL(ax,ax) + dMUL(ay,ay) + dMUL(az,az);
if (l > REAL(0.0)) {
angle = dMUL(angle,REAL(0.5));
q[0] = dCos (angle);
l = dMUL(dReal(dSin(angle)),dRecipSqrt(l));
q[1] = dMUL(ax,l);
q[2] = dMUL(ay,l);
q[3] = dMUL(az,l);
}
else {
q[0] = REAL(1.0);
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
}
void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az,
dReal angle)
{
dAASSERT (q);
dReal l = ax*ax + ay*ay + az*az;
if (l > REAL(0.0)) {
angle *= REAL(0.5);
q[0] = dCos (angle);
l = dSin(angle) * dRecipSqrt(l);
q[1] = ax*l;
q[2] = ay*l;
q[3] = az*l;
}
else {
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
}
dFloat AngularIntegration::update(dFloat angle) {
return update(dCos(angle), dSin(angle));
}
void dCustomBallAndSocket::Debug(dDebugDisplay* const debugDisplay) const
{
dMatrix matrix0;
dMatrix matrix1;
dCustomJoint::Debug(debugDisplay);
CalculateGlobalMatrix(matrix0, matrix1);
const dVector& coneDir0 = matrix0.m_front;
const dVector& coneDir1 = matrix1.m_front;
dFloat cosAngleCos = coneDir0.DotProduct3(coneDir1);
dMatrix coneRotation(dGetIdentityMatrix());
if (cosAngleCos < 0.9999f) {
dVector lateralDir(coneDir1.CrossProduct(coneDir0));
dFloat mag2 = lateralDir.DotProduct3(lateralDir);
//dAssert(mag2 > 1.0e-4f);
if (mag2 > 1.0e-4f) {
lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2));
coneRotation = dMatrix(dQuaternion(lateralDir, dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)))), matrix1.m_posit);
} else {
lateralDir = matrix0.m_up.Scale(-1.0f);
coneRotation = dMatrix(dQuaternion(matrix0.m_up, 180 * dDegreeToRad), matrix1.m_posit);
}
} else if (cosAngleCos < -0.9999f) {
coneRotation[0][0] = -1.0f;
coneRotation[1][1] = -1.0f;
}
const int subdiv = 18;
const dFloat radius = debugDisplay->m_debugScale;
dVector arch[subdiv + 1];
// show twist angle limits
if (m_options.m_option0 && ((m_maxTwistAngle - m_minTwistAngle) > dFloat(1.0e-3f))) {
dMatrix pitchMatrix(matrix1 * coneRotation);
pitchMatrix.m_posit = matrix1.m_posit;
dVector point(dFloat(0.0f), dFloat(radius), dFloat(0.0f), dFloat(0.0f));
dFloat angleStep = dMin (m_maxTwistAngle - m_minTwistAngle, dFloat (2.0f * dPi)) / subdiv;
dFloat angle0 = m_minTwistAngle;
debugDisplay->SetColor(dVector(0.6f, 0.2f, 0.0f, 0.0f));
for (int i = 0; i <= subdiv; i++) {
arch[i] = pitchMatrix.TransformVector(dPitchMatrix(angle0).RotateVector(point));
debugDisplay->DrawLine(pitchMatrix.m_posit, arch[i]);
angle0 += angleStep;
}
for (int i = 0; i < subdiv; i++) {
debugDisplay->DrawLine(arch[i], arch[i + 1]);
}
}
// show cone angle limits
if (m_options.m_option2) {
dVector point(radius * dCos(m_maxConeAngle), radius * dSin(m_maxConeAngle), 0.0f, 0.0f);
dFloat angleStep = dPi * 2.0f / subdiv;
dFloat angle0 = 0.0f;
debugDisplay->SetColor(dVector(0.3f, 0.8f, 0.0f, 0.0f));
for (int i = 0; i <= subdiv; i++) {
dVector conePoint(dPitchMatrix(angle0).RotateVector(point));
dVector p(matrix1.TransformVector(conePoint));
arch[i] = p;
debugDisplay->DrawLine(matrix1.m_posit, p);
angle0 += angleStep;
}
for (int i = 0; i < subdiv; i++) {
debugDisplay->DrawLine(arch[i], arch[i + 1]);
}
}
}
void CustomDGRayCastCar::AddSingleSuspensionTire (
void *userData,
const dVector& localPosition,
dFloat mass,
dFloat radius,
dFloat width,
dFloat friction,
dFloat suspensionLenght,
dFloat springConst,
dFloat springDamper,
int castMode)
{
dVector relTirePos = localPosition;
suspensionLenght = dAbs ( suspensionLenght );
dMatrix chassisMatrix;
NewtonBodyGetMatrix (m_body0, &chassisMatrix[0][0]);
chassisMatrix = chassisMatrix * chassisMatrix;
relTirePos += chassisMatrix.m_up.Scale ( suspensionLenght );
m_tires[m_tiresCount].m_harpoint = m_localFrame.UntransformVector( relTirePos );
m_tires[m_tiresCount].m_localAxis = dVector (0.0f, 0.0f, 1.0f, 0.0f);
m_tires[m_tiresCount].m_tireAxelPosit = dVector (0.0f, 0.0f, 0.0f, 1.0f);
m_tires[m_tiresCount].m_tireAxelVeloc = dVector (0.0f, 0.0f, 0.0f, 1.0f);
m_tires[m_tiresCount].m_lateralPin = dVector (0.0f, 0.0f, 0.0f, 1.0f);
m_tires[m_tiresCount].m_longitudinalPin = dVector (0.0f, 0.0f, 0.0f, 1.0f);
m_tires[m_tiresCount].m_hitBodyPointVelocity = dVector (0.0f, 0.0f, 0.0f, 1.0f);
m_tires[m_tiresCount].m_HitBody = NULL;
m_tires[m_tiresCount].m_userData = userData;
m_tires[m_tiresCount].m_spinAngle = 0.0f;
m_tires[m_tiresCount].m_steerAngle = 0.0f;
m_tires[m_tiresCount].m_tireLoad = 0.0f;
m_tires[m_tiresCount].m_posit = suspensionLenght;
m_tires[m_tiresCount].m_tireIsOnAir = 1;
m_tires[m_tiresCount].m_tireUseConvexCastMode = castMode;
m_tires[m_tiresCount].m_springConst = springConst;
m_tires[m_tiresCount].m_springDamper = springDamper;
m_tires[m_tiresCount].m_suspensionLenght = suspensionLenght;
m_tires[m_tiresCount].m_angularVelocity = 0.0f;
m_tires[m_tiresCount].m_breakForce = 0.0f;
m_tires[m_tiresCount].m_torque = 0.0f;
m_tires[m_tiresCount].m_groundFriction = friction;
m_tires[m_tiresCount].m_tireIsConstrained = 0;
m_tires[m_tiresCount].m_mass = mass;
m_tires[m_tiresCount].m_width = width;
m_tires[m_tiresCount].m_radius = radius;
m_tires[m_tiresCount].m_Ixx = mass * radius * radius / 2.0f;
m_tires[m_tiresCount].m_IxxInv = 1.0f / m_tires[m_tiresCount].m_Ixx;
#define TIRE_SHAPE_SIZE 12
dVector shapePoints[TIRE_SHAPE_SIZE * 2];
for ( int i = 0; i < TIRE_SHAPE_SIZE; i ++ ) {
shapePoints[i].m_x = -width * 0.5f;
shapePoints[i].m_y = radius * dCos ( 2.0f * 3.14159265f * dFloat( i )/ dFloat( TIRE_SHAPE_SIZE ) );
shapePoints[i].m_z = radius * dSin ( 2.0f * 3.14159265f * dFloat( i )/ dFloat( TIRE_SHAPE_SIZE ) );
shapePoints[i + TIRE_SHAPE_SIZE].m_x = -shapePoints[i].m_x;
shapePoints[i + TIRE_SHAPE_SIZE].m_y = shapePoints[i].m_y;
shapePoints[i + TIRE_SHAPE_SIZE].m_z = shapePoints[i].m_z;
}
m_tires[m_tiresCount].m_shape = NewtonCreateConvexHull ( m_world, TIRE_SHAPE_SIZE * 2, &shapePoints[0].m_x, sizeof (dVector), 0.0f, 0, NULL );
// NewtonCreateChamferCylinder(m_world,radius,width,NULL);
// NewtonCreateSphere(m_world,radius,radius,radius,&offmat[0][0]);
// NewtonCreateCone(m_world,radius,width,NULL);
// NewtonCreateCapsule(m_world,radius,width,NULL);
// NewtonCreateChamferCylinder(m_world,radius,width,NULL);
// NewtonCreateCylinder(m_world,radius*2,width*2,NULL);
// NewtonCreateBox(m_world,radius*2,radius*2,radius*2,NULL);
// NewtonCreateConvexHull (m_world, TIRE_SHAPE_SIZE * 2, &shapePoints[0].m_x, sizeof (dVector), 0.0f, NULL);
m_tiresCount ++;
}
// initialize collision data
void _cldInitCylinderBox(sCylinderBoxData& cData)
{
// get cylinder position, orientation
const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder);
dMatrix3Copy(pRotCyc,cData.mCylinderRot);
const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder);
dVector3Copy(*pPosCyc,cData.vCylinderPos);
dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis);
// get cylinder radius and size
dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize);
// get box position, orientation, size
const dReal* pRotBox = dGeomGetRotation(cData.gBox);
dMatrix3Copy(pRotBox,cData.mBoxRot);
const dVector3* pPosBox = (const dVector3*)dGeomGetPosition(cData.gBox);
dVector3Copy(*pPosBox,cData.vBoxPos);
dGeomBoxGetLengths(cData.gBox, cData.vBoxHalfSize);
cData.vBoxHalfSize[0] *= REAL(0.5);
cData.vBoxHalfSize[1] *= REAL(0.5);
cData.vBoxHalfSize[2] *= REAL(0.5);
// vertex 0
cData.avBoxVertices[0][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[0][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[0][2] = -cData.vBoxHalfSize[2];
// vertex 1
cData.avBoxVertices[1][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[1][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[1][2] = -cData.vBoxHalfSize[2];
// vertex 2
cData.avBoxVertices[2][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[2][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[2][2] = -cData.vBoxHalfSize[2];
// vertex 3
cData.avBoxVertices[3][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[3][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[3][2] = -cData.vBoxHalfSize[2];
// vertex 4
cData.avBoxVertices[4][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[4][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[4][2] = cData.vBoxHalfSize[2];
// vertex 5
cData.avBoxVertices[5][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[5][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[5][2] = cData.vBoxHalfSize[2];
// vertex 6
cData.avBoxVertices[6][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[6][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[6][2] = cData.vBoxHalfSize[2];
// vertex 7
cData.avBoxVertices[7][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[7][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[7][2] = cData.vBoxHalfSize[2];
// temp index
int i = 0;
dVector3 vTempBoxVertices[8];
// transform vertices in absolute space
for(i=0; i < 8; i++)
{
dMultiplyMat3Vec3(cData.mBoxRot,cData.avBoxVertices[i], vTempBoxVertices[i]);
dVector3Add(vTempBoxVertices[i], cData.vBoxPos, cData.avBoxVertices[i]);
}
// find relative position
dVector3Subtract(cData.vCylinderPos,cData.vBoxPos,cData.vDiff);
cData.fBestDepth = MAX_FLOAT;
cData.vNormal[0] = REAL(0.0);
cData.vNormal[1] = REAL(0.0);
cData.vNormal[2] = REAL(0.0);
// calculate basic angle for nCYLINDER_SEGMENT-gon
dReal fAngle = M_PI/nCYLINDER_SEGMENT;
// calculate angle increment
dReal fAngleIncrement = fAngle * REAL(2.0);
// calculate nCYLINDER_SEGMENT-gon points
for(i = 0; i < nCYLINDER_SEGMENT; i++)
{
cData.avCylinderNormals[i][0] = -dCos(fAngle);
cData.avCylinderNormals[i][1] = -dSin(fAngle);
cData.avCylinderNormals[i][2] = 0;
fAngle += fAngleIncrement;
}
cData.fBestrb = 0;
cData.fBestrc = 0;
cData.iBestAxis = 0;
cData.nContacts = 0;
}
// initialize collision data
void sCylinderBoxData::_cldInitCylinderBox()
{
// get cylinder position, orientation
const dReal* pRotCyc = dGeomGetRotation(m_gCylinder);
dMatrix3Copy(pRotCyc,m_mCylinderRot);
const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(m_gCylinder);
dVector3Copy(*pPosCyc,m_vCylinderPos);
dMat3GetCol(m_mCylinderRot,nCYLINDER_AXIS,m_vCylinderAxis);
// get cylinder radius and size
dGeomCylinderGetParams(m_gCylinder,&m_fCylinderRadius,&m_fCylinderSize);
// get box position, orientation, size
const dReal* pRotBox = dGeomGetRotation(m_gBox);
dMatrix3Copy(pRotBox,m_mBoxRot);
const dVector3* pPosBox = (const dVector3*)dGeomGetPosition(m_gBox);
dVector3Copy(*pPosBox,m_vBoxPos);
dGeomBoxGetLengths(m_gBox, m_vBoxHalfSize);
m_vBoxHalfSize[0] *= REAL(0.5);
m_vBoxHalfSize[1] *= REAL(0.5);
m_vBoxHalfSize[2] *= REAL(0.5);
// vertex 0
m_avBoxVertices[0][0] = -m_vBoxHalfSize[0];
m_avBoxVertices[0][1] = m_vBoxHalfSize[1];
m_avBoxVertices[0][2] = -m_vBoxHalfSize[2];
// vertex 1
m_avBoxVertices[1][0] = m_vBoxHalfSize[0];
m_avBoxVertices[1][1] = m_vBoxHalfSize[1];
m_avBoxVertices[1][2] = -m_vBoxHalfSize[2];
// vertex 2
m_avBoxVertices[2][0] = -m_vBoxHalfSize[0];
m_avBoxVertices[2][1] = -m_vBoxHalfSize[1];
m_avBoxVertices[2][2] = -m_vBoxHalfSize[2];
// vertex 3
m_avBoxVertices[3][0] = m_vBoxHalfSize[0];
m_avBoxVertices[3][1] = -m_vBoxHalfSize[1];
m_avBoxVertices[3][2] = -m_vBoxHalfSize[2];
// vertex 4
m_avBoxVertices[4][0] = m_vBoxHalfSize[0];
m_avBoxVertices[4][1] = m_vBoxHalfSize[1];
m_avBoxVertices[4][2] = m_vBoxHalfSize[2];
// vertex 5
m_avBoxVertices[5][0] = m_vBoxHalfSize[0];
m_avBoxVertices[5][1] = -m_vBoxHalfSize[1];
m_avBoxVertices[5][2] = m_vBoxHalfSize[2];
// vertex 6
m_avBoxVertices[6][0] = -m_vBoxHalfSize[0];
m_avBoxVertices[6][1] = -m_vBoxHalfSize[1];
m_avBoxVertices[6][2] = m_vBoxHalfSize[2];
// vertex 7
m_avBoxVertices[7][0] = -m_vBoxHalfSize[0];
m_avBoxVertices[7][1] = m_vBoxHalfSize[1];
m_avBoxVertices[7][2] = m_vBoxHalfSize[2];
// temp index
int i = 0;
dVector3 vTempBoxVertices[8];
// transform vertices in absolute space
for(i=0; i < 8; i++)
{
dMultiplyMat3Vec3(m_mBoxRot,m_avBoxVertices[i], vTempBoxVertices[i]);
dVector3Add(vTempBoxVertices[i], m_vBoxPos, m_avBoxVertices[i]);
}
// find relative position
dVector3Subtract(m_vCylinderPos,m_vBoxPos,m_vDiff);
m_fBestDepth = MAX_FLOAT;
m_vNormal[0] = REAL(0.0);
m_vNormal[1] = REAL(0.0);
m_vNormal[2] = REAL(0.0);
// calculate basic angle for nCYLINDER_SEGMENT-gon
dReal fAngle = (dReal) (M_PI/nCYLINDER_SEGMENT);
// calculate angle increment
dReal fAngleIncrement = fAngle * REAL(2.0);
// calculate nCYLINDER_SEGMENT-gon points
for(i = 0; i < nCYLINDER_SEGMENT; i++)
{
m_avCylinderNormals[i][0] = -dCos(fAngle);
m_avCylinderNormals[i][1] = -dSin(fAngle);
m_avCylinderNormals[i][2] = 0;
fAngle += fAngleIncrement;
}
m_fBestrb = 0;
m_fBestrc = 0;
m_iBestAxis = 0;
m_nContacts = 0;
}
void AngularIntegration::set_angle(dFloat angle) {
m_angle = angle;
m_sin_angle = dSin(angle);
m_cos_angle = dCos(angle);
}
void CustomDGRayCastCar::SetTireSteerAngleForce (int index, dFloat angle, dFloat turnforce)
{
m_tires[index].m_steerAngle = angle;
m_tires[index].m_localAxisInJointSpace.m_z = dCos (angle);
m_tires[index].m_localAxisInJointSpace.m_x = dSin (angle);
}
