void dNewtonCollision::SetScale(dFloat scaleX, dFloat scaleY, dFloat scaleZ)
{
scaleX = dMax(0.01f, dAbs(scaleX));
scaleY = dMax(0.01f, dAbs(scaleY));
scaleZ = dMax(0.01f, dAbs(scaleZ));
NewtonCollisionSetScale(m_shape, scaleX, scaleY, scaleZ);
}
void dMeshNodeInfo::CalcutateAABB (dVector& p0, dVector& p1) const
{
// int strideInBytes = NewtonMeshGetVertexStrideInByte(m_mesh);
// dFloat64* const vertexList = NewtonMeshGetVertexArray(m_mesh);
// dFloat t = 1.2f;
// for (void* face = NewtonMeshGetFirstFace (m_mesh); face; face = NewtonMeshGetNextFace (m_mesh, face)) {
// if (!NewtonMeshIsFaceOpen (m_mesh, face)) {
// dMatrix matrix (GetIdentityMatrix());
// NewtonMeshCalculateOOBB(const NewtonMesh* const mesh, dFloat* const matrix, dFloat* const x, dFloat* const y, dFloat* const z);
p0 = dVector (1.0e10f, 1.0e10f, 1.0e10f, 0.0f);
p1 = dVector (-1.0e10f, -1.0e10f, -1.0e10f, 0.0f);
int strideInBytes = NewtonMeshGetVertexStrideInByte(m_mesh);
int stride = strideInBytes / sizeof (dFloat64) ;
const dFloat64* const vertexList = NewtonMeshGetVertexArray(m_mesh);
for (void* ptr = NewtonMeshGetFirstVertex(m_mesh); ptr; ptr = NewtonMeshGetNextVertex(m_mesh, ptr)) {
int index = NewtonMeshGetVertexIndex (m_mesh, ptr);
dFloat x = dFloat (vertexList[index * stride + 0]);
dFloat y = dFloat (vertexList[index * stride + 1]);
dFloat z = dFloat (vertexList[index * stride + 2]);
dVector v (m_matrix.TransformVector(dVector (x, y, z, 0.0f)));
p0[0] = dMin(v[0], p0[0]);
p0[1] = dMin(v[1], p0[1]);
p0[2] = dMin(v[2], p0[2]);
p1[0] = dMax(v[0], p1[0]);
p1[1] = dMax(v[1], p1[1]);
p1[2] = dMax(v[2], p1[2]);
}
}
static void PlaneCollisionCollideCallbackDescrete (NewtonUserMeshCollisionCollideDesc* const collideDesc)
{
dInfinitePlane* const me = (dInfinitePlane*) collideDesc->m_userData;
dVector p0 (collideDesc->m_boxP0[0], collideDesc->m_boxP0[1], collideDesc->m_boxP0[2], 0.0f);
dVector p1 (collideDesc->m_boxP1[0], collideDesc->m_boxP1[1], collideDesc->m_boxP1[2], 0.0f);
dVector suportVertex ((me->m_plane.m_x > 0.0f) ? p0.m_x : p1.m_x, (me->m_plane.m_y > 0.0f) ? p0.m_y : p1.m_y, (me->m_plane.m_z > 0.0f) ? p0.m_z : p1.m_z);
dFloat dist = me->m_plane.DotProduct3(suportVertex) + me->m_plane.m_w;
if (dist < 0.25f) {
// calculate the aabb center
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));
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 LoadLumberYardMesh(DemoEntityManager* const scene, const dVector& location, int shapeid)
{
DemoEntity* const entity = DemoEntity::LoadNGD_mesh ("lumber.ngd", scene->GetNewton(), scene->GetShaderCache());
dTree<NewtonCollision*, DemoMesh*> filter;
NewtonWorld* const world = scene->GetNewton();
dFloat density = 15.0f;
int defaultMaterialID = NewtonMaterialGetDefaultGroupID(scene->GetNewton());
for (DemoEntity* child = entity->GetFirst(); child; child = child->GetNext()) {
DemoMesh* const mesh = (DemoMesh*)child->GetMesh();
if (mesh) {
dAssert(mesh->IsType(DemoMesh::GetRttiType()));
dTree<NewtonCollision*, DemoMesh*>::dTreeNode* node = filter.Find(mesh);
if (!node) {
// make a collision shape only for and instance
dFloat* const array = mesh->m_vertex;
dVector minBox(1.0e10f, 1.0e10f, 1.0e10f, 1.0f);
dVector maxBox(-1.0e10f, -1.0e10f, -1.0e10f, 1.0f);
for (int i = 0; i < mesh->m_vertexCount; i++) {
dVector p(array[i * 3 + 0], array[i * 3 + 1], array[i * 3 + 2], 1.0f);
minBox.m_x = dMin(p.m_x, minBox.m_x);
minBox.m_y = dMin(p.m_y, minBox.m_y);
minBox.m_z = dMin(p.m_z, minBox.m_z);
maxBox.m_x = dMax(p.m_x, maxBox.m_x);
maxBox.m_y = dMax(p.m_y, maxBox.m_y);
maxBox.m_z = dMax(p.m_z, maxBox.m_z);
}
dVector size(maxBox - minBox);
dMatrix offset(dGetIdentityMatrix());
offset.m_posit = (maxBox + minBox).Scale(0.5f);
NewtonCollision* const shape = NewtonCreateBox(world, size.m_x, size.m_y, size.m_z, shapeid, &offset[0][0]);
node = filter.Insert(shape, mesh);
}
// create a body and add to the world
NewtonCollision* const shape = node->GetInfo();
dMatrix matrix(child->GetMeshMatrix() * child->CalculateGlobalMatrix());
matrix.m_posit += location;
dFloat mass = density * NewtonConvexCollisionCalculateVolume(shape);
CreateSimpleSolid(scene, mesh, mass, matrix, shape, defaultMaterialID);
}
}
// destroy all shapes
while (filter.GetRoot()) {
NewtonCollision* const shape = filter.GetRoot()->GetInfo();
NewtonDestroyCollision(shape);
filter.Remove(filter.GetRoot());
}
delete entity;
}
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 CustomVehicleControllerComponentEngine::Update (dFloat timestep)
{
CustomVehicleControllerBodyStateChassis& chassis = m_controller->m_chassisState;
CustomVehicleControllerComponentEngine::dGearBox* const gearBox = GetGearBox();
gearBox->Update (timestep);
int gear = gearBox->GetGear();
#ifdef __TEST_VEHICLE_XXX__
if (gear > (CustomVehicleControllerComponentEngine::dGearBox::m_firstGear))
{
gearBox->SetGear (CustomVehicleControllerComponentEngine::dGearBox::m_firstGear);
// gearBox->SetGear (CustomVehicleControllerComponentEngine::dGearBox::m_reverseGear);
gear = gearBox->GetGear();
}
#endif
if (m_engineSwitch) {
if (gear == CustomVehicleControllerComponentEngine::dGearBox::m_newtralGear) {
dFloat param = dMax (GetParam(), 0.05f);
m_engineToque = GetTorque (m_engineRPS) * param - m_engineIdleResistance1 * m_engineRPS - m_engineIdleResistance2 * m_engineRPS * m_engineRPS;
dFloat alpha = m_engineIdleInvInertia * m_engineToque;
m_engineRPS = dMin (dMax (m_engineRPS + alpha * timestep, 0.0f), m_radiansPerSecundsAtRedLine);
} else {
dFloat gearRatio = gearBox->GetGearRatio(gear);
dFloat shaftOmega = m_differencial->GetShaftOmega();
m_engineRPS = shaftOmega * gearRatio;
dFloat torqueResistance = - m_engineInternalFriction * m_engineRPS;
dFloat param = GetParam();
dFloat nominalTorque = -GetTorque (dMax (-m_engineRPS, 0.0f)) * param;
dFloat engineTorque = nominalTorque + torqueResistance;
m_engineToque = m_engineToque + (engineTorque - m_engineToque) * timestep / m_clutchTorqueCouplingTime;
dFloat shaftTorque = m_engineToque * gearRatio;
m_differencial->ApplyTireTorque(shaftTorque, shaftOmega);
}
} else {
m_engineRPS = m_engineRPS * 0.95f;
if (m_engineRPS < 0.001f) {
m_engineRPS = 0.0f;
}
m_engineToque = 0.0f;
}
dVector front (chassis.m_matrix.RotateVector(chassis.m_localFrame[0]));
m_speedMPS = chassis.m_veloc % front;
}
void CustomVehicleControllerComponentBrake::Update (dFloat timestep)
{
for (dList<dList<CustomVehicleControllerBodyStateTire>::dListNode*>::dListNode* node = m_brakeTires.GetFirst(); node; node = node->GetNext()) {
CustomVehicleControllerBodyStateTire& tire = node->GetInfo()->GetInfo();
tire.m_brakeTorque = dMax (tire.m_brakeTorque, dAbs (m_maxBrakeTorque * m_param));
}
}
dErr dFSGetBoundingBox(dFS fs,dReal bbox[3][2])
{
dErr err;
Vec X;
const dScalar *x;
dInt n;
dFunctionBegin;
for (dInt i=0; i<3; i++) {
bbox[i][0] = PETSC_MAX_REAL;
bbox[i][1] = PETSC_MIN_REAL;
}
err = dFSGetGeometryVectorExpanded(fs,&X);
dCHK(err);
err = VecGetLocalSize(X,&n);
dCHK(err);
err = VecGetArrayRead(X,&x);
dCHK(err);
for (dInt i=0; i<n; i++) {
dInt j = i%3;
bbox[j][0] = dMin(bbox[j][0],x[i]);
bbox[j][1] = dMax(bbox[j][1],x[i]);
}
err = VecRestoreArrayRead(X,&x);
dCHK(err);
dFunctionReturn(0);
}
void SubmitConstraints(dFloat timestep, int threadIndex)
{
CustomBallAndSocket::SubmitConstraints(timestep, threadIndex);
float invTimestep = 1.0f / timestep;
dMatrix matrix0;
dMatrix matrix1;
CalculateGlobalMatrix(matrix0, matrix1);
if (m_anim_speed != 0.0f) // some animation to illustrate purpose
{
m_anim_time += timestep * m_anim_speed;
float a0 = sin(m_anim_time);
float a1 = m_anim_offset * 3.14f;
dVector axis(sin(a1), 0.0f, cos(a1));
//dVector axis (1,0,0);
m_target = dQuaternion(axis, a0 * 0.5f);
}
// measure error
dQuaternion q0(matrix0);
dQuaternion q1(matrix1);
dQuaternion qt0 = m_target * q1;
dQuaternion qErr = ((q0.DotProduct(qt0) < 0.0f) ? dQuaternion(-q0.m_q0, q0.m_q1, q0.m_q2, q0.m_q3) : dQuaternion(q0.m_q0, -q0.m_q1, -q0.m_q2, -q0.m_q3)) * qt0;
float errorAngle = 2.0f * acos(dMax(-1.0f, dMin(1.0f, qErr.m_q0)));
dVector errorAngVel(0, 0, 0);
dMatrix basis;
if (errorAngle > 1.0e-10f) {
dVector errorAxis(qErr.m_q1, qErr.m_q2, qErr.m_q3, 0.0f);
errorAxis = errorAxis.Scale(1.0f / dSqrt(errorAxis % errorAxis));
errorAngVel = errorAxis.Scale(errorAngle * invTimestep);
basis = dGrammSchmidt(errorAxis);
} else {
basis = dMatrix(qt0, dVector(0.0f, 0.0f, 0.0f, 1.0f));
}
dVector angVel0, angVel1;
NewtonBodyGetOmega(m_body0, (float*)&angVel0);
NewtonBodyGetOmega(m_body1, (float*)&angVel1);
dVector angAcc = (errorAngVel.Scale(m_reduceError) - (angVel0 - angVel1)).Scale(invTimestep);
// motor
for (int n = 0; n < 3; n++) {
// calculate the desired acceleration
dVector &axis = basis[n];
float relAccel = angAcc % axis;
NewtonUserJointAddAngularRow(m_joint, 0.0f, &axis[0]);
NewtonUserJointSetRowAcceleration(m_joint, relAccel);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
}
}
void dNewtonJointDoubleHinge::SetLimits_1(bool enable, dFloat minVal, dFloat maxAngle)
{
dCustomDoubleHinge* const joint = (dCustomDoubleHinge*)m_joint;
joint->EnableLimits1(enable);
if (enable) {
joint->SetLimits1(dMin(minVal * DEGREES_TO_RAD, 0.0f), dMax(maxAngle * DEGREES_TO_RAD, 0.0f));
}
}
double analysis::getMax(double *d, int n)
{
double v = 0;
for (int i = 0; i < n; i++)
v = dMax(d[i], v);
return v;
}
void CustomVehicleControllerComponentEngine::dGearBox::SetOptimalShiftLimits (dFloat minShift, dFloat maxShift)
{
minShift = dMax (minShift - 0.01f, 0.1f);
maxShift = dMin (maxShift + 0.01f, 0.9f);
for (int i = m_firstGear; i < m_gearsCount; i ++) {
dGearState* const state = m_gears[i];
state->m_shiftUp = maxShift;
state->m_shiftDown = minShift;
}
}
dFloat64 dSymmetricBiconjugateGradientSolve::Solve (int size, dFloat64 tolerance, dFloat64* const x, const dFloat64* const b) const
{
dFloat64* const r0 = new dFloat64 [size];
dFloat64* const p0 = new dFloat64 [size];
dFloat64* const MinvR0 = new dFloat64 [size];
dFloat64* const matrixP0 = new dFloat64 [size];
MatrixTimeVector (matrixP0, x);
Sub(size, r0, b, matrixP0);
bool continueExecution = InversePrecoditionerTimeVector (p0, r0);
int iter = 0;
dFloat64 num = DotProduct (size, r0, p0);
dFloat64 error2 = num;
for (int j = 0; (j < size) && (error2 > tolerance) && continueExecution; j ++) {
MatrixTimeVector (matrixP0, p0);
dFloat64 den = DotProduct (size, p0, matrixP0);
dAssert (fabs(den) > dFloat64 (0.0f));
dFloat64 alpha = num / den;
ScaleAdd (size, x, x, alpha, p0);
if ((j % 50) != 49) {
ScaleAdd (size, r0, r0, -alpha, matrixP0);
} else {
MatrixTimeVector (matrixP0, x);
Sub(size, r0, b, matrixP0);
}
continueExecution = InversePrecoditionerTimeVector (MinvR0, r0);
dFloat64 num1 = DotProduct (size, r0, MinvR0);
dFloat64 beta = num1 / num;
ScaleAdd (size, p0, MinvR0, beta, p0);
num = DotProduct (size, r0, MinvR0);
iter ++;
error2 = num;
if (j > 10) {
error2 = dFloat64 (0.0f);
for (int i = 0; i < size; i ++) {
error2 = dMax (error2, r0[i] * r0[i]);
}
}
}
delete[] matrixP0;
delete[] MinvR0;
delete[] p0;
delete[] r0;
dAssert (iter <= size);
return num;
}
static dErr JakoInternalEnergy_Sharp2(VHTCase scase,dReal b,dReal h,const dReal x[],dScalar *e)
{
struct VHTRheology *rheo = &scase->rheo;
dReal z,t,T;
dFunctionBegin;
z = (x[2] - b) / (h-b); // Normalize to [0,1] plus possible fuzz
z = dMax(0,dMin(1,z)); // Clip to [0,1]
t = z < 0.3 ? 0 : (z < 0.7 ? 2 : 1);
T = rheo->T3 - (rheo->T3 - rheo->T0)*t; // Maximum value is at the bed and equal to T3, extends past T0
*e = rheo->c_i * (T - rheo->T0); // energy/mass
dFunctionReturn(0);
}
static dErr JakoInternalEnergy_Smooth(VHTCase scase,dReal b,dReal h,const dReal x[],dScalar *e)
{
struct VHTRheology *rheo = &scase->rheo;
dReal z,t,T;
dFunctionBegin;
z = (x[2] - b) / (h-b); // Normalize to [0,1] plus possible fuzz
z = dMax(0,dMin(1,z)); // Clip to [0,1]
t = 90*dSqr(z)*exp(-5*z); // hump with maximum value close to 2
T = rheo->T3 - (rheo->T3 - rheo->T0)*t; // Maximum value is at the bed and equal to T3, extends past T0
*e = rheo->c_i * (T - rheo->T0); // energy/mass
dFunctionReturn(0);
}
static dErr JakoInternalEnergy_Sharp1(VHTCase scase,dReal b,dReal h,const dReal x[],dScalar *e)
{
struct VHTRheology *rheo = &scase->rheo;
dReal z,t,T;
dFunctionBegin;
z = (x[2] - b) / (h-b); // Normalize to [0,1] plus possible fuzz
z = dMax(0,dMin(1,z)); // Clip to [0,1]
t = floor(5 * z) * 0.2;
T = rheo->T3 - (rheo->T3 - rheo->T0)*t; // Maximum value is at the bed and equal to T3, extends past T0
*e = rheo->c_i * (T - rheo->T0); // energy/mass
//printf("b %g h %g H %g z %g t %g T %g e %g\n",b,h,h-b,z,t,T,e[0]);
dFunctionReturn(0);
}
dCustomPlayerController* dCustomPlayerControllerManager::CreateController(const dMatrix& location, const dMatrix& localAxis, dFloat mass, dFloat radius, dFloat height, dFloat stepHeight)
{
NewtonWorld* const world = GetWorld();
dMatrix shapeMatrix(localAxis);
shapeMatrix.m_posit = shapeMatrix.m_front.Scale (height * 0.5f);
shapeMatrix.m_posit.m_w = 1.0f;
dFloat scale = 3.0f;
height = dMax(height - 2.0f * radius / scale, dFloat(0.1f));
NewtonCollision* const bodyCapsule = NewtonCreateCapsule(world, radius / scale, radius / scale, height, 0, &shapeMatrix[0][0]);
NewtonCollisionSetScale(bodyCapsule, 1.0f, scale, scale);
// create the kinematic body
NewtonBody* const body = NewtonCreateKinematicBody(world, bodyCapsule, &location[0][0]);
// players must have weight, otherwise they are infinitely strong when they collide
NewtonCollision* const shape = NewtonBodyGetCollision(body);
NewtonBodySetMassProperties(body, mass, shape);
// make the body collidable with other dynamics bodies, by default
NewtonBodySetCollidable(body, 1);
NewtonDestroyCollision(bodyCapsule);
dCustomPlayerController& controller = m_playerList.Append()->GetInfo();
controller.m_localFrame = localAxis;
controller.m_mass = mass;
controller.m_invMass = 1.0f / mass;
controller.m_manager = this;
controller.m_kinematicBody = body;
controller.m_contactPatch = radius / scale;
controller.m_stepHeight = dMax (stepHeight, controller.m_contactPatch * 2.0f);
return &controller;
}
dFloat dCustomPlayerController::PredictTimestep(dFloat timestep)
{
dMatrix matrix;
dMatrix predicMatrix;
NewtonWorld* const world = m_manager->GetWorld();
NewtonWorldConvexCastReturnInfo info[16];
NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
NewtonCollision* const shape = NewtonBodyGetCollision(m_kinematicBody);
NewtonBodyIntegrateVelocity(m_kinematicBody, timestep);
NewtonBodyGetMatrix(m_kinematicBody, &predicMatrix[0][0]);
int contactCount = NewtonWorldCollide(world, &predicMatrix[0][0], shape, this, PrefilterCallback, info, 4, 0);
NewtonBodySetMatrix(m_kinematicBody, &matrix[0][0]);
if (contactCount) {
dFloat t0 = 0.0f;
dFloat t1 = timestep;
dFloat dt = (t1 + t0) * 0.5f;
timestep = dt;
for (int i = 0; i < D_MAX_COLLIONSION_STEPS; i++) {
NewtonBodyIntegrateVelocity(m_kinematicBody, timestep);
NewtonBodyGetMatrix(m_kinematicBody, &predicMatrix[0][0]);
contactCount = NewtonWorldCollide(world, &predicMatrix[0][0], shape, this, PrefilterCallback, info, 4, 0);
NewtonBodySetMatrix(m_kinematicBody, &matrix[0][0]);
dt *= 0.5f;
if (contactCount) {
dFloat penetration = 0.0f;
for (int j = 0; j < contactCount; j++) {
penetration = dMax(penetration, info[j].m_penetration);
}
if (penetration < D_MAX_COLLISION_PENETRATION) {
break;
}
timestep -= dt;
} else {
timestep += dt;
}
}
}
return timestep;
}
void dComplemtaritySolver::dFrictionLessContactJoint::JacobianDerivative (dParamInfo * const constraintParams)
{
for (int i = 0; i < m_count; i ++)
{
dPointDerivativeParam pointData;
InitPointParam (pointData, m_contacts[i].m_point);
CalculatePointDerivative (constraintParams, m_contacts[i].m_normal, pointData);
dVector velocError (pointData.m_veloc1 - pointData.m_veloc0);
//dFloat restitution = 0.05f;
dFloat relVelocErr = velocError % m_contacts[i].m_normal;
dFloat penetration = 0.0f;
dFloat penetrationStiffness = 0.0f;
dFloat penetrationVeloc = penetration * penetrationStiffness;
if (relVelocErr > dFloat (1.0e-3f))
relVelocErr *= (m_restitution + dFloat (1.0f));
constraintParams->m_jointLowFriction[i] = dFloat (0.0f);
constraintParams->m_jointAccel[i] = dMax (dFloat (-4.0f), relVelocErr + penetrationVeloc) * constraintParams->m_timestepInv;
}
}
static dErr JakoInternalEnergy_Sharp3(VHTCase scase,dReal b,dReal h,const dReal x[],dScalar *e)
{
struct VHTRheology *rheo = &scase->rheo;
const dReal origin[] = {5920,76765},normal[] = {1,0.4};
dReal xx,yy,z,t,T;
dFunctionBegin;
xx = (x[0] - origin[0])*normal[0] + (x[1] - origin[1])*normal[1];
yy = -(x[0] - origin[0])*normal[1] + (x[1] - origin[1])*normal[0];
z = (x[2] - b) / (h-b); // Normalize to [0,1] plus possible fuzz
z = dMax(0,dMin(1,z)); // Clip to [0,1]
t = xx + 0*yy > -30
? -2.0
: (z < 0.3
? 0
: (z < 0.7 ? -2 : -1));
T = rheo->T3 + (rheo->T3 - rheo->T0)*t; // Maximum value is at the bed and equal to T3, extends past T0
*e = rheo->c_i * (T - rheo->T0); // energy/mass
dFunctionReturn(0);
}
static void UserContactRestitution (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
{
// call the basic call back
GenericContactProcess (contactJoint, timestep, threadIndex);
const NewtonBody* const body0 = NewtonJointGetBody0(contactJoint);
const NewtonBody* const body1 = NewtonJointGetBody1(contactJoint);
//now core 3.14 can have per collision user data
const NewtonCollision* const collision0 = NewtonBodyGetCollision(body0);
const NewtonCollision* const collision1 = NewtonBodyGetCollision(body1);
NewtonCollisionMaterial material0;
NewtonCollisionMaterial material1;
NewtonCollisionGetMaterial(collision0, &material0);
NewtonCollisionGetMaterial(collision1, &material1);
dAssert((material0.m_userId == 1) || (material1.m_userId == 1));
dFloat restitution = dMax(material0.m_userParam[0], material1.m_userParam[0]);
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
NewtonMaterial* const material = NewtonContactGetMaterial (contact);
NewtonMaterialSetContactElasticity (material, restitution);
}
}
void dNewtonJointDoubleHingeActuator::SetTargetAngle0(dFloat angle, dFloat minLimit, dFloat maxLimit)
{
dCustomDoubleHingeActuator* const joint = (dCustomDoubleHingeActuator*) m_joint;
joint->SetLimits(dMin(minLimit * DEGREES_TO_RAD, dFloat(0.0f)), dMax(maxLimit * DEGREES_TO_RAD, dFloat(0.0f)));
joint->SetTargetAngle0(dClamp (angle * DEGREES_TO_RAD, joint->GetMinAngularLimit0(), joint->GetMaxAngularLimit0()));
}
void dCustomPlayerController::ResolveStep(dFloat timestep)
{
dMatrix matrix;
dMatrix stepMatrix;
dVector veloc(0.0f);
dVector zero(0.0f);
NewtonWorldConvexCastReturnInfo info[16];
NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
NewtonBodyGetVelocity(m_kinematicBody, &veloc[0]);
dMatrix coodinateMatrix (m_localFrame * matrix);
dComplementaritySolver::dJacobian jt[3];
dFloat rhs[3];
dFloat low[3];
dFloat high[3];
int normalIndex[3];
jt[0].m_linear = coodinateMatrix[0];
jt[0].m_angular = zero;
low[0] = 0.0f;
high[0] = 1.0e12f;
normalIndex[0] = 0;
rhs[0] = -m_impulse.DotProduct3(jt[0].m_linear) * m_invMass;
// add lateral traction friction
jt[1].m_linear = coodinateMatrix[1];
jt[1].m_angular = zero;
low[1] = -m_friction;
high[1] = m_friction;
normalIndex[1] = -1;
dVector tmp1(veloc * jt[1].m_linear);
rhs[1] = -m_lateralSpeed - (tmp1.m_x + tmp1.m_y + tmp1.m_z);
// add longitudinal traction friction
jt[2].m_linear = coodinateMatrix[2];
jt[2].m_angular = zero;
low[2] = -m_friction;
high[2] = m_friction;
normalIndex[2] = -2;
dVector tmp2(veloc * jt[2].m_linear);
rhs[2] = -m_forwardSpeed - (tmp2.m_x + tmp2.m_y + tmp2.m_z);
dVector impulse(veloc.Scale(m_mass) + CalculateImpulse(3, rhs, low, high, normalIndex, jt));
impulse = impulse.Scale(m_invMass);
NewtonBodySetVelocity(m_kinematicBody, &impulse[0]);
NewtonBodyIntegrateVelocity(m_kinematicBody, timestep);
NewtonWorld* const world = m_manager->GetWorld();
NewtonCollision* const shape = NewtonBodyGetCollision(m_kinematicBody);
NewtonBodyGetMatrix(m_kinematicBody, &stepMatrix[0][0]);
int contactCount = NewtonWorldCollide(world, &stepMatrix[0][0], shape, this, PrefilterCallback, info, 4, 0);
NewtonBodySetMatrix(m_kinematicBody, &matrix[0][0]);
NewtonBodySetVelocity(m_kinematicBody, &veloc[0]);
dFloat maxHigh = 0.0f;
for (int i = 0; i < contactCount; i++) {
NewtonWorldConvexCastReturnInfo& contact = info[i];
dVector point(contact.m_point[0], contact.m_point[1], contact.m_point[2], 0.0f);
point = m_localFrame.UntransformVector (stepMatrix.UntransformVector(point));
maxHigh = dMax (point.m_x, maxHigh);
}
if ((maxHigh < m_stepHeight) && (maxHigh > m_contactPatch)) {
dVector step (stepMatrix.RotateVector(m_localFrame.RotateVector (dVector(maxHigh, dFloat(0.0f), dFloat(0.0f), dFloat(0.0f)))));
matrix.m_posit += step;
NewtonBodySetMatrix(m_kinematicBody, &matrix[0][0]);
}
}
void dVehicleChassis::CalculateSuspensionForces(dFloat timestep)
{
const int maxSize = 64;
dComplementaritySolver::dJacobianPair m_jt[maxSize];
dComplementaritySolver::dJacobianPair m_jInvMass[maxSize];
dVehicleVirtualTire* tires[maxSize];
dFloat massMatrix[maxSize * maxSize];
dFloat accel[maxSize];
dComplementaritySolver::dBodyState* const chassisBody = m_vehicle->GetBody();
const dMatrix& chassisMatrix = chassisBody->GetMatrix();
const dMatrix& chassisInvInertia = chassisBody->GetInvInertia();
dVector chassisOrigin (chassisMatrix.TransformVector (chassisBody->GetCOM()));
dFloat chassisInvMass = chassisBody->GetInvMass();
int tireCount = 0;
const dList<dVehicleNode*>& children = m_vehicle->GetChildren();
for (dList<dVehicleNode*>::dListNode* tireNode = children.GetFirst(); tireNode; tireNode = tireNode->GetNext()) {
dVehicleVirtualTire* const tire = (dVehicleVirtualTire*) tireNode->GetInfo()->GetAsTire();
if (tire) {
const dVehicleVirtualTire::dTireInfo& info = tire->m_info;
tires[tireCount] = tire;
dFloat x = tire->m_position;
dFloat v = tire->m_speed;
dFloat weight = 1.0f;
/*
switch (tire->m_suspentionType)
{
case m_offroad:
weight = 0.9f;
break;
case m_confort:
weight = 1.0f;
break;
case m_race:
weight = 1.1f;
break;
}
*/
//x = 0.1f;
//v = 10.0f;
dComplementaritySolver::dBodyState* const tireBody = tire->GetBody();
const dFloat invMass = tireBody->GetInvMass();
const dFloat kv = info.m_dampingRatio * invMass;
const dFloat ks = info.m_springStiffness * invMass;
accel[tireCount] = -NewtonCalculateSpringDamperAcceleration(timestep, ks * weight, x, kv, v);
const dMatrix& tireMatrix = tireBody->GetMatrix();
const dMatrix& tireInvInertia = tireBody->GetInvInertia();
dFloat tireMass = tireBody->GetInvMass();
m_jt[tireCount].m_jacobian_J01.m_linear = chassisMatrix.m_up.Scale(-1.0f);
m_jt[tireCount].m_jacobian_J01.m_angular = dVector(0.0f);
m_jt[tireCount].m_jacobian_J10.m_linear = chassisMatrix.m_up;
m_jt[tireCount].m_jacobian_J10.m_angular = (tireMatrix.m_posit - chassisOrigin).CrossProduct(chassisMatrix.m_up);
m_jInvMass[tireCount].m_jacobian_J01.m_linear = m_jt[tireCount].m_jacobian_J01.m_linear.Scale(tireMass);
m_jInvMass[tireCount].m_jacobian_J01.m_angular = tireInvInertia.RotateVector(m_jt[tireCount].m_jacobian_J01.m_angular);
m_jInvMass[tireCount].m_jacobian_J10.m_linear = m_jt[tireCount].m_jacobian_J10.m_linear.Scale(chassisInvMass);
m_jInvMass[tireCount].m_jacobian_J10.m_angular = chassisInvInertia.RotateVector(m_jt[tireCount].m_jacobian_J10.m_angular);
tireCount++;
}
}
for (int i = 0; i < tireCount; i++) {
dFloat* const row = &massMatrix[i * tireCount];
dFloat aii = m_jInvMass[i].m_jacobian_J01.m_linear.DotProduct3(m_jt[i].m_jacobian_J01.m_linear) + m_jInvMass[i].m_jacobian_J01.m_angular.DotProduct3(m_jt[i].m_jacobian_J01.m_angular) +
m_jInvMass[i].m_jacobian_J10.m_linear.DotProduct3(m_jt[i].m_jacobian_J10.m_linear) + m_jInvMass[i].m_jacobian_J10.m_angular.DotProduct3(m_jt[i].m_jacobian_J10.m_angular);
row[i] = aii * 1.0001f;
for (int j = i + 1; j < tireCount; j++) {
dFloat aij = m_jInvMass[i].m_jacobian_J10.m_linear.DotProduct3(m_jt[j].m_jacobian_J10.m_linear) + m_jInvMass[i].m_jacobian_J10.m_angular.DotProduct3(m_jt[j].m_jacobian_J10.m_angular);
row[j] = aij;
massMatrix[j * tireCount + i] = aij;
}
}
dCholeskyFactorization(tireCount, massMatrix);
dCholeskySolve(tireCount, tireCount, massMatrix, accel);
dVector chassisForce(0.0f);
dVector chassisTorque(0.0f);
for (int i = 0; i < tireCount; i++) {
dVehicleVirtualTire* const tire = tires[i];
dComplementaritySolver::dBodyState* const tireBody = tire->GetBody();
tires[i]->m_tireLoad = dMax(dFloat(1.0f), accel[i]);
dVector tireForce(m_jt[i].m_jacobian_J01.m_linear.Scale(accel[i]));
tireBody->SetForce(tireBody->GetForce() + tireForce);
chassisForce += m_jt[i].m_jacobian_J10.m_linear.Scale(accel[i]);
chassisTorque += m_jt[i].m_jacobian_J10.m_angular.Scale(accel[i]);
}
chassisBody->SetForce(chassisBody->GetForce() + chassisForce);
chassisBody->SetTorque(chassisBody->GetTorque() + chassisTorque);
}
int dCustomPlayerController::ResolveInterpenetrations(int contactCount, NewtonWorldConvexCastReturnInfo* const contactArray)
{
dVector zero (0.0f);
dVector veloc (0.0f);
dVector savedVeloc (0.0f);
NewtonBodyGetVelocity(m_kinematicBody, &savedVeloc[0]);
NewtonBodySetVelocity(m_kinematicBody, &veloc[0]);
dFloat timestep = 0.1f;
dFloat invTimestep = 1.0f / timestep;
dComplementaritySolver::dJacobian jt[D_MAX_ROWS];
dFloat rhs[D_MAX_ROWS];
dFloat low[D_MAX_ROWS];
dFloat high[D_MAX_ROWS];
int normalIndex[D_MAX_ROWS];
NewtonWorld* const world = m_manager->GetWorld();
for (int i = 0; i < 3; i++) {
jt[i].m_linear = zero;
jt[i].m_angular = zero;
jt[i].m_angular[i] = dFloat(1.0f);
rhs[i] = 0.0f;
low[i] = -1.0e12f;
high[i] = 1.0e12f;
normalIndex[i] = 0;
}
dFloat penetration = D_MAX_COLLISION_PENETRATION * 10.0f;
for (int j = 0; (j < 8) && (penetration > D_MAX_COLLISION_PENETRATION) ; j ++) {
dMatrix matrix;
dVector com(0.0f);
NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
NewtonBodyGetCentreOfMass(m_kinematicBody, &com[0]);
com = matrix.TransformVector(com);
com.m_w = 0.0f;
int rowCount = 3;
for (int i = 0; i < contactCount; i++) {
NewtonWorldConvexCastReturnInfo& contact = contactArray[i];
dVector point(contact.m_point[0], contact.m_point[1], contact.m_point[2], 0.0f);
dVector normal(contact.m_normal[0], contact.m_normal[1], contact.m_normal[2], 0.0f);
jt[rowCount].m_linear = normal;
jt[rowCount].m_angular = (point - com).CrossProduct(normal);
low[rowCount] = 0.0f;
high[rowCount] = 1.0e12f;
normalIndex[rowCount] = 0;
penetration = dClamp(contact.m_penetration - D_MAX_COLLISION_PENETRATION * 0.5f, dFloat(0.0f), dFloat(0.5f));
rhs[rowCount] = penetration * invTimestep;
rowCount++;
}
dVector impulse (CalculateImpulse(rowCount, rhs, low, high, normalIndex, jt));
impulse = impulse.Scale(m_invMass);
NewtonBodySetVelocity(m_kinematicBody, &impulse[0]);
NewtonBodyIntegrateVelocity(m_kinematicBody, timestep);
NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
NewtonCollision* const shape = NewtonBodyGetCollision(m_kinematicBody);
contactCount = NewtonWorldCollide(world, &matrix[0][0], shape, this, PrefilterCallback, contactArray, 4, 0);
penetration = 0.0f;
for (int i = 0; i < contactCount; i++) {
penetration = dMax(contactArray[i].m_penetration, penetration);
}
}
NewtonBodySetVelocity(m_kinematicBody, &savedVeloc[0]);
return contactCount;
}
void SerializeMesh (const NewtonMesh* const mesh, TiXmlElement* const rootNode)
{
TiXmlElement* pointElement = new TiXmlElement ("points");
rootNode->LinkEndChild(pointElement);
int bufferCount = dMax (NewtonMeshGetVertexCount(mesh), NewtonMeshGetPointCount(mesh));
int bufferSizeInBytes = bufferCount * sizeof (dFloat) * 4 * 12;
char* const buffer = new char[bufferSizeInBytes];
dFloat* const packVertex = new dFloat [4 * bufferCount];
int vertexCount = NewtonMeshGetVertexCount (mesh);
int vertexStride = NewtonMeshGetVertexStrideInByte(mesh) / sizeof (dFloat64);
const dFloat64* const vertex = NewtonMeshGetVertexArray(mesh);
// pack the vertex Array
int* const vertexIndexList = new int [vertexCount];
for (int i = 0; i < vertexCount; i ++) {
packVertex[i * 4 + 0] = dFloat(vertex[i * vertexStride + 0]);
packVertex[i * 4 + 1] = dFloat(vertex[i * vertexStride + 1]);
packVertex[i * 4 + 2] = dFloat(vertex[i * vertexStride + 2]);
packVertex[i * 4 + 3] = dFloat(vertex[i * vertexStride + 3]);
vertexIndexList[i] = i;
}
dFloatArrayToString (packVertex, vertexCount * 4, buffer, bufferSizeInBytes);
TiXmlElement* const position = new TiXmlElement ("position");
pointElement->LinkEndChild(position);
position->SetAttribute("float4", vertexCount);
position->SetAttribute("floats", buffer);
// pack the normal array
int pointCount = NewtonMeshGetPointCount (mesh);
int pointStride = NewtonMeshGetPointStrideInByte(mesh) / sizeof (dFloat64);
const dFloat64* const normals = NewtonMeshGetNormalArray(mesh);
int* const normalIndexList = new int [pointCount];
for (int i = 0; i < pointCount; i ++) {
packVertex[i * 3 + 0] = dFloat(normals[i * pointStride + 0]);
packVertex[i * 3 + 1] = dFloat(normals[i * pointStride + 1]);
packVertex[i * 3 + 2] = dFloat(normals[i * pointStride + 2]);
}
int count = dPackVertexArray (packVertex, 3, 3 * sizeof (dFloat), pointCount, normalIndexList);
dFloatArrayToString (packVertex, count * 3, buffer, bufferSizeInBytes);
TiXmlElement* const normal = new TiXmlElement ("normal");
pointElement->LinkEndChild(normal);
normal->SetAttribute("float3", count);
normal->SetAttribute("floats", buffer);
// pack the uv0 array
int* const uv0IndexList = new int [pointCount];
const dFloat64* const uv0s = NewtonMeshGetUV0Array(mesh);
for (int i = 0; i < pointCount; i ++) {
packVertex[i * 3 + 0] = dFloat(uv0s[i * pointStride + 0]);
packVertex[i * 3 + 1] = dFloat(uv0s[i * pointStride + 1]);
packVertex[i * 3 + 2] = 0.0f;
}
count = dPackVertexArray (packVertex, 3, 3 * sizeof (dFloat), pointCount, uv0IndexList);
for (int i = 0; i < pointCount; i ++) {
packVertex[i * 2 + 0] = packVertex[i * 3 + 0];
packVertex[i * 2 + 1] = packVertex[i * 3 + 1];
}
dFloatArrayToString (packVertex, count * 2, buffer, bufferSizeInBytes);
TiXmlElement* const uv0 = new TiXmlElement ("uv0");
pointElement->LinkEndChild(uv0);
uv0->SetAttribute("float2", count);
uv0->SetAttribute("floats", buffer);
// pack the uv1 array
int* const uv1IndexList = new int [pointCount];
const dFloat64* const uv1s = NewtonMeshGetUV1Array(mesh);
for (int i = 0; i < pointCount; i ++) {
packVertex[i * 3 + 0] = dFloat(uv1s[i * pointStride + 0]);
packVertex[i * 3 + 1] = dFloat(uv1s[i * pointStride + 1]);
packVertex[i * 3 + 2] = 0.0f;
}
count = dPackVertexArray (packVertex, 3, 3 * sizeof (dFloat), pointCount, uv1IndexList);
for (int i = 0; i < pointCount; i ++) {
packVertex[i * 2 + 0] = packVertex[i * 3 + 0];
packVertex[i * 2 + 1] = packVertex[i * 3 + 1];
}
dFloatArrayToString (packVertex, count * 2, buffer, bufferSizeInBytes);
TiXmlElement* const uv1 = new TiXmlElement ("uv1");
pointElement->LinkEndChild(uv1);
uv1->SetAttribute("float2", count);
uv1->SetAttribute("floats", buffer);
// save the polygon array
int faceCount = NewtonMeshGetTotalFaceCount (mesh);
int indexCount = NewtonMeshGetTotalIndexCount (mesh);
int* const faceArray = new int [faceCount];
void** const indexArray = new void* [indexCount];
int* const materialIndexArray = new int [faceCount];
int* const remapedIndexArray = new int [indexCount];
NewtonMeshGetFaces (mesh, faceArray, materialIndexArray, indexArray);
// save the faces vertex Count
dIntArrayToString (faceArray, faceCount, buffer, bufferSizeInBytes);
TiXmlElement* const polygons = new TiXmlElement ("polygons");
rootNode->LinkEndChild(polygons);
polygons->SetAttribute("count", faceCount);
polygons->SetAttribute("faceIndexCount", buffer);
dIntArrayToString (materialIndexArray, faceCount, buffer, bufferSizeInBytes);
TiXmlElement* const faceMaterial = new TiXmlElement ("faceMaterial");
polygons->LinkEndChild(faceMaterial);
faceMaterial->SetAttribute("index", buffer);
for (int i = 0; i < indexCount; i ++) {
int index = NewtonMeshGetVertexIndex (mesh, indexArray[i]);
remapedIndexArray[i] = vertexIndexList[index];
}
dIntArrayToString (remapedIndexArray, indexCount, buffer, bufferSizeInBytes);
TiXmlElement* const positionIndex = new TiXmlElement ("position");
polygons->LinkEndChild(positionIndex);
positionIndex->SetAttribute("index", buffer);
for (int i = 0; i < indexCount; i ++) {
int index = NewtonMeshGetPointIndex(mesh, indexArray[i]);
remapedIndexArray[i] = normalIndexList[index];
}
dIntArrayToString (remapedIndexArray, indexCount, buffer, bufferSizeInBytes);
TiXmlElement* const normalIndex = new TiXmlElement ("normal");
polygons->LinkEndChild(normalIndex);
normalIndex->SetAttribute("index", buffer);
for (int i = 0; i < indexCount; i ++) {
int index = NewtonMeshGetPointIndex(mesh, indexArray[i]);
remapedIndexArray[i] = uv0IndexList[index];
}
dIntArrayToString (remapedIndexArray, indexCount, buffer, bufferSizeInBytes);
TiXmlElement* const uv0Index = new TiXmlElement ("uv0");
polygons->LinkEndChild(uv0Index);
uv0Index->SetAttribute("index", buffer);
for (int i = 0; i < indexCount; i ++) {
int index = NewtonMeshGetPointIndex(mesh, indexArray[i]);
remapedIndexArray[i] = uv1IndexList[index];
}
dIntArrayToString (remapedIndexArray, indexCount, buffer, bufferSizeInBytes);
TiXmlElement* const uv1Index = new TiXmlElement ("uv1");
polygons->LinkEndChild(uv1Index);
uv1Index->SetAttribute("index", buffer);
delete[] remapedIndexArray;
delete[] faceArray;
delete[] indexArray;
delete[] materialIndexArray;
delete[] uv1IndexList;
delete[] uv0IndexList;
delete[] normalIndexList;
delete[] vertexIndexList;
delete[] packVertex;
delete[] buffer;
}
void dCustomPlayerController::ResolveCollision()
{
dMatrix matrix;
NewtonWorldConvexCastReturnInfo info[D_MAX_ROWS];
NewtonWorld* const world = m_manager->GetWorld();
NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
NewtonCollision* const shape = NewtonBodyGetCollision(m_kinematicBody);
int contactCount = NewtonWorldCollide(world, &matrix[0][0], shape, this, PrefilterCallback, info, 4, 0);
if (!contactCount) {
return;
}
dFloat maxPenetration = 0.0f;
for (int i = 0; i < contactCount; i ++) {
maxPenetration = dMax (info[i].m_penetration, maxPenetration);
}
if (maxPenetration > D_MAX_COLLISION_PENETRATION) {
ResolveInterpenetrations(contactCount, info);
NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
}
int rowCount = 0;
dVector zero(0.0f);
dMatrix invInertia;
dVector com(0.0f);
dVector veloc(0.0f);
dComplementaritySolver::dJacobian jt[D_MAX_ROWS];
dFloat rhs[D_MAX_ROWS];
dFloat low[D_MAX_ROWS];
dFloat high[D_MAX_ROWS];
dFloat impulseMag[D_MAX_ROWS];
int normalIndex[D_MAX_ROWS];
NewtonBodyGetVelocity(m_kinematicBody, &veloc[0]);
NewtonBodyGetCentreOfMass(m_kinematicBody, &com[0]);
NewtonBodyGetInvInertiaMatrix(m_kinematicBody, &invInertia[0][0]);
// const dMatrix localFrame (dPitchMatrix(m_headingAngle) * m_localFrame * matrix);
const dMatrix localFrame (m_localFrame * matrix);
com = matrix.TransformVector(com);
com.m_w = 0.0f;
for (int i = 0; i < contactCount; i ++) {
NewtonWorldConvexCastReturnInfo& contact = info[i];
dVector point (contact.m_point[0], contact.m_point[1], contact.m_point[2], 0.0f);
dVector normal (contact.m_normal[0], contact.m_normal[1], contact.m_normal[2], 0.0f);
jt[rowCount].m_linear = normal;
jt[rowCount].m_angular = (point - com).CrossProduct(normal);
low[rowCount] = 0.0f;
high[rowCount] = 1.0e12f;
normalIndex[rowCount] = 0;
dVector tmp (veloc * jt[rowCount].m_linear.Scale (1.001f));
rhs[rowCount] = - (tmp.m_x + tmp.m_y + tmp.m_z);
rowCount ++;
dAssert (rowCount < (D_MAX_ROWS - 3));
//dFloat updir = localFrame.m_front.DotProduct3(normal);
dFloat friction = m_manager->ContactFriction(this, point, normal, contact.m_hitBody);
if (friction > 0.0f)
{
// add lateral traction friction
dVector sideDir (localFrame.m_up.CrossProduct(normal).Normalize());
jt[rowCount].m_linear = sideDir;
jt[rowCount].m_angular = (point - com).CrossProduct(sideDir);
low[rowCount] = -friction;
high[rowCount] = friction;
normalIndex[rowCount] = -1;
dVector tmp1 (veloc * jt[rowCount].m_linear);
rhs[rowCount] = -m_lateralSpeed - (tmp1.m_x + tmp1.m_y + tmp1.m_z);
rowCount++;
dAssert (rowCount < (D_MAX_ROWS - 3));
// add longitudinal traction friction
dVector frontDir (normal.CrossProduct(sideDir));
jt[rowCount].m_linear = frontDir;
jt[rowCount].m_angular = (point - com).CrossProduct(frontDir);
low[rowCount] = -friction;
high[rowCount] = friction;
normalIndex[rowCount] = -2;
dVector tmp2 (veloc * jt[rowCount].m_linear);
rhs[rowCount] = -m_forwardSpeed - (tmp2.m_x + tmp2.m_y + tmp2.m_z);
rowCount++;
dAssert(rowCount < (D_MAX_ROWS - 3));
}
}
for (int i = 0; i < 3; i++) {
jt[rowCount].m_linear = zero;
jt[rowCount].m_angular = zero;
jt[rowCount].m_angular[i] = dFloat(1.0f);
rhs[rowCount] = 0.0f;
impulseMag[rowCount] = 0;
low[rowCount] = -1.0e12f;
high[rowCount] = 1.0e12f;
normalIndex[rowCount] = 0;
rowCount ++;
dAssert (rowCount < D_MAX_ROWS);
}
dVector impulse (veloc.Scale (m_mass) + CalculateImpulse(rowCount, rhs, low, high, normalIndex, jt));
veloc = impulse.Scale(m_invMass);
NewtonBodySetVelocity(m_kinematicBody, &veloc[0]);
}
void CustomVehicleController::DrawSchematic (dFloat scale) const
{
dVector array [32];
dMatrix projectionMatrix (dGetIdentityMatrix());
projectionMatrix[0][0] = scale;
projectionMatrix[1][1] = 0.0f;
projectionMatrix[2][1] = scale;
projectionMatrix[2][2] = 0.0f;
CustomVehicleControllerManager* const manager = (CustomVehicleControllerManager*)GetManager();
const dMatrix& chassisMatrix = m_chassisState.GetMatrix();
const dMatrix& chassisFrameMatrix = m_chassisState.GetLocalMatrix();
dMatrix worldToComMatrix ((chassisFrameMatrix * chassisMatrix).Inverse() * projectionMatrix);
{
// draw vehicle chassis
dVector p0 (1.0e10f, 1.0e10f, 1.0e10f, 0.0f);
dVector p1 (-1.0e10f, -1.0e10f, -1.0e10f, 0.0f);
for (dList<CustomVehicleControllerBodyStateTire>::dListNode* node = m_tireList.GetFirst(); node; node = node->GetNext()) {
CustomVehicleControllerBodyStateTire* const tire = &node->GetInfo();
dMatrix matrix (tire->CalculateSteeringMatrix() * m_chassisState.GetMatrix());
dVector p (worldToComMatrix.TransformVector(matrix.m_posit));
p0 = dVector (dMin (p.m_x, p0.m_x), dMin (p.m_y, p0.m_y), dMin (p.m_z, p0.m_z), 1.0f);
p1 = dVector (dMax (p.m_x, p1.m_x), dMax (p.m_y, p1.m_y), dMax (p.m_z, p1.m_z), 1.0f);
}
array[0] = dVector (p0.m_x, p0.m_y, p0.m_z, 1.0f);
array[1] = dVector (p1.m_x, p0.m_y, p0.m_z, 1.0f);
array[2] = dVector (p1.m_x, p1.m_y, p0.m_z, 1.0f);
array[3] = dVector (p0.m_x, p1.m_y, p0.m_z, 1.0f);
manager->DrawSchematicCallback(this, "chassis", 0, 4, array);
}
{
// draw vehicle tires
for (dList<CustomVehicleControllerBodyStateTire>::dListNode* node = m_tireList.GetFirst(); node; node = node->GetNext()) {
CustomVehicleControllerBodyStateTire* const tire = &node->GetInfo();
dFloat width = tire->m_width * 0.5f;
dFloat radio = tire->m_radio;
dMatrix matrix (tire->CalculateSteeringMatrix() * m_chassisState.GetMatrix());
array[0] = worldToComMatrix.TransformVector(matrix.TransformVector(dVector ( width, 0.0f, radio, 0.0f)));
array[1] = worldToComMatrix.TransformVector(matrix.TransformVector(dVector ( width, 0.0f, -radio, 0.0f)));
array[2] = worldToComMatrix.TransformVector(matrix.TransformVector(dVector (-width, 0.0f, -radio, 0.0f)));
array[3] = worldToComMatrix.TransformVector(matrix.TransformVector(dVector (-width, 0.0f, radio, 0.0f)));
manager->DrawSchematicCallback(this, "tire", 0, 4, array);
}
}
{
// draw vehicle velocity
//dVector veloc1;
//NewtonBodyGetVelocity(GetBody(), &veloc[0]);
dVector veloc (m_chassisState.GetVelocity());
//dVector xxx (veloc1 - veloc);
//dAssert (dAbs(xxx % xxx) < 1.0e-3f);
dVector localVelocity (chassisFrameMatrix.UnrotateVector (chassisMatrix.UnrotateVector (veloc)));
localVelocity.m_y = 0.0f;
localVelocity = projectionMatrix.RotateVector(localVelocity);
array[0] = dVector (0.0f, 0.0f, 0.0f, 0.0f);
array[1] = localVelocity.Scale (0.25f);
manager->DrawSchematicCallback(this, "velocity", 0, 2, array);
}
{
dFloat scale (2.0f / (m_chassisState.GetMass() * m_chassisState.m_gravityMag));
// draw vehicle forces
for (dList<CustomVehicleControllerBodyStateTire>::dListNode* node = m_tireList.GetFirst(); node; node = node->GetNext()) {
CustomVehicleControllerBodyStateTire* const tire = &node->GetInfo();
//dVector p0 (tire->GetCenterOfMass());
dMatrix matrix (tire->CalculateSteeringMatrix() * m_chassisState.GetMatrix());
//dTrace (("(%f %f %f) (%f %f %f)\n", p0.m_x, p0.m_y, p0.m_z, matrix.m_posit.m_x, matrix.m_posit.m_y, matrix.m_posit.m_z ));
dVector origin (worldToComMatrix.TransformVector(matrix.m_posit));
dVector lateralForce (chassisFrameMatrix.UnrotateVector(chassisMatrix.UnrotateVector(tire->GetLateralForce())));
lateralForce = lateralForce.Scale (-scale);
lateralForce = projectionMatrix.RotateVector (lateralForce);
//dTrace (("(%f %f %f)\n", lateralForce.m_x, lateralForce.m_y, lateralForce.m_z ));
array[0] = origin;
array[1] = origin + lateralForce;
manager->DrawSchematicCallback(this, "lateralForce", 0, 2, array);
dVector longitudinalForce (chassisFrameMatrix.UnrotateVector(chassisMatrix.UnrotateVector(tire->GetLongitudinalForce())));
longitudinalForce = longitudinalForce.Scale (-scale);
longitudinalForce = projectionMatrix.RotateVector (longitudinalForce);
//dTrace (("(%f %f %f)\n", lateralForce.m_x, lateralForce.m_y, lateralForce.m_z ));
array[0] = origin;
array[1] = origin + longitudinalForce;
manager->DrawSchematicCallback(this, "longitudinalForce", 0, 2, array);
// dVector p2 (p0 - tire->GetLateralForce().Scale (scale));
/*
// offset the origin of of tire force so that they are visible
const dMatrix& tireMatrix = tire.GetLocalMatrix ();
p0 += chassis.GetMatrix()[2].Scale ((tireMatrix.m_posit.m_z > 0.0f ? 1.0f : -1.0f) * 0.25f);
// draw the tire load
dVector p1 (p0 + tire.GetTireLoad().Scale (scale));
glColor3f (0.0f, 0.0f, 1.0f);
glVertex3f (p0.m_x, p0.m_y, p0.m_z);
glVertex3f (p1.m_x, p1.m_y, p1.m_z);
// show tire lateral force
dVector p2 (p0 - tire.GetLateralForce().Scale (scale));
glColor3f(1.0f, 0.0f, 0.0f);
glVertex3f (p0.m_x, p0.m_y, p0.m_z);
glVertex3f (p2.m_x, p2.m_y, p2.m_z);
// show tire longitudinal force
dVector p3 (p0 - tire.GetLongitudinalForce().Scale (scale));
glColor3f(0.0f, 1.0f, 0.0f);
glVertex3f (p0.m_x, p0.m_y, p0.m_z);
glVertex3f (p3.m_x, p3.m_y, p3.m_z);
*/
}
}
}
void dComplentaritySolver::CalculateReactionsForces (int bodyCount, dBodyState** const bodyArray, int jointCount, dBilateralJoint** const jointArray, dFloat timestepSrc, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray)
{
dJacobian stateVeloc[COMPLEMENTARITY_STACK_ENTRIES];
dJacobian internalForces [COMPLEMENTARITY_STACK_ENTRIES];
int stateIndex = 0;
dVector zero(dFloat (0.0f), dFloat (0.0f), dFloat (0.0f), dFloat (0.0f));
for (int i = 0; i < bodyCount; i ++) {
dBodyState* const state = bodyArray[i];
stateVeloc[stateIndex].m_linear = state->m_veloc;
stateVeloc[stateIndex].m_angular = state->m_omega;
internalForces[stateIndex].m_linear = zero;
internalForces[stateIndex].m_angular = zero;
state->m_myIndex = stateIndex;
stateIndex ++;
dAssert (stateIndex < int (sizeof (stateVeloc)/sizeof (stateVeloc[0])));
}
for (int i = 0; i < jointCount; i ++) {
dJacobian y0;
dJacobian y1;
y0.m_linear = zero;
y0.m_angular = zero;
y1.m_linear = zero;
y1.m_angular = zero;
dBilateralJoint* const constraint = jointArray[i];
int first = constraint->m_start;
int count = constraint->m_count;
for (int j = 0; j < count; j ++) {
dJacobianPair* const row = &jacobianArray[j + first];
const dJacobianColum* const col = &jacobianColumnArray[j + first];
dFloat val = col->m_force;
y0.m_linear += row->m_jacobian_IM0.m_linear.Scale(val);
y0.m_angular += row->m_jacobian_IM0.m_angular.Scale(val);
y1.m_linear += row->m_jacobian_IM1.m_linear.Scale(val);
y1.m_angular += row->m_jacobian_IM1.m_angular.Scale(val);
}
int m0 = constraint->m_state0->m_myIndex;
int m1 = constraint->m_state1->m_myIndex;
internalForces[m0].m_linear += y0.m_linear;
internalForces[m0].m_angular += y0.m_angular;
internalForces[m1].m_linear += y1.m_linear;
internalForces[m1].m_angular += y1.m_angular;
}
dFloat invTimestepSrc = dFloat (1.0f) / timestepSrc;
dFloat invStep = dFloat (0.25f);
dFloat timestep = timestepSrc * invStep;
dFloat invTimestep = invTimestepSrc * dFloat (4.0f);
int maxPasses = 5;
dFloat firstPassCoef = dFloat (0.0f);
dFloat maxAccNorm = dFloat (1.0e-2f);
for (int step = 0; step < 4; step ++) {
dJointAccelerationDecriptor joindDesc;
joindDesc.m_timeStep = timestep;
joindDesc.m_invTimeStep = invTimestep;
joindDesc.m_firstPassCoefFlag = firstPassCoef;
for (int i = 0; i < jointCount; i ++) {
dBilateralJoint* const constraint = jointArray[i];
joindDesc.m_rowsCount = constraint->m_count;
joindDesc.m_rowMatrix = &jacobianArray[constraint->m_start];
joindDesc.m_colMatrix = &jacobianColumnArray[constraint->m_start];
constraint->JointAccelerations (&joindDesc);
}
firstPassCoef = dFloat (1.0f);
dFloat accNorm = dFloat (1.0e10f);
for (int passes = 0; (passes < maxPasses) && (accNorm > maxAccNorm); passes ++) {
accNorm = dFloat (0.0f);
for (int i = 0; i < jointCount; i ++) {
dBilateralJoint* const constraint = jointArray[i];
int index = constraint->m_start;
int rowsCount = constraint->m_count;
int m0 = constraint->m_state0->m_myIndex;
int m1 = constraint->m_state1->m_myIndex;
dVector linearM0 (internalForces[m0].m_linear);
dVector angularM0 (internalForces[m0].m_angular);
dVector linearM1 (internalForces[m1].m_linear);
dVector angularM1 (internalForces[m1].m_angular);
dBodyState* const state0 = constraint->m_state0;
dBodyState* const state1 = constraint->m_state1;
const dMatrix& invInertia0 = state0->m_invInertia;
const dMatrix& invInertia1 = state1->m_invInertia;
dFloat invMass0 = state0->m_invMass;
dFloat invMass1 = state1->m_invMass;
for (int k = 0; k < rowsCount; k ++) {
dJacobianPair* const row = &jacobianArray[index];
dJacobianColum* const col = &jacobianColumnArray[index];
dVector JMinvIM0linear (row->m_jacobian_IM0.m_linear.Scale (invMass0));
dVector JMinvIM1linear (row->m_jacobian_IM1.m_linear.Scale (invMass1));
dVector JMinvIM0angular = invInertia0.UnrotateVector(row->m_jacobian_IM0.m_angular);
dVector JMinvIM1angular = invInertia1.UnrotateVector(row->m_jacobian_IM1.m_angular);
dVector acc (JMinvIM0linear.CompProduct(linearM0) + JMinvIM0angular.CompProduct(angularM0) + JMinvIM1linear.CompProduct(linearM1) + JMinvIM1angular.CompProduct(angularM1));
dFloat a = col->m_coordenateAccel - acc.m_x - acc.m_y - acc.m_z - col->m_force * col->m_diagDamp;
dFloat f = col->m_force + col->m_invDJMinvJt * a;
dFloat lowerFrictionForce = col->m_jointLowFriction;
dFloat upperFrictionForce = col->m_jointHighFriction;
if (f > upperFrictionForce) {
a = dFloat (0.0f);
f = upperFrictionForce;
} else if (f < lowerFrictionForce) {
a = dFloat (0.0f);
f = lowerFrictionForce;
}
accNorm = dMax (accNorm, dAbs (a));
dFloat prevValue = f - col->m_force;
col->m_force = f;
linearM0 += row->m_jacobian_IM0.m_linear.Scale (prevValue);
angularM0 += row->m_jacobian_IM0.m_angular.Scale (prevValue);
linearM1 += row->m_jacobian_IM1.m_linear.Scale (prevValue);
angularM1 += row->m_jacobian_IM1.m_angular.Scale (prevValue);
index ++;
}
internalForces[m0].m_linear = linearM0;
internalForces[m0].m_angular = angularM0;
internalForces[m1].m_linear = linearM1;
internalForces[m1].m_angular = angularM1;
}
}
for (int i = 0; i < bodyCount; i ++) {
dBodyState* const state = bodyArray[i];
//int index = state->m_myIndex;
dAssert (state->m_myIndex == i);
dVector force (state->m_externalForce + internalForces[i].m_linear);
dVector torque (state->m_externalTorque + internalForces[i].m_angular);
state->IntegrateForce(timestep, force, torque);
}
}
for (int i = 0; i < jointCount; i ++) {
dBilateralJoint* const constraint = jointArray[i];
int first = constraint->m_start;
int count = constraint->m_count;
for (int j = 0; j < count; j ++) {
const dJacobianColum* const col = &jacobianColumnArray[j + first];
dFloat val = col->m_force;
constraint->m_jointFeebackForce[j] = val;
}
}
for (int i = 0; i < jointCount; i ++) {
dBilateralJoint* const constraint = jointArray[i];
constraint->UpdateSolverForces (jacobianArray);
}
for (int i = 0; i < bodyCount; i ++) {
dBodyState* const state = bodyArray[i];
dAssert (state->m_myIndex == i);
state->ApplyNetForceAndTorque (invTimestepSrc, stateVeloc[i].m_linear, stateVeloc[i].m_angular);
}
}
dFloat CustomVehicleControllerComponentEngine::GetRPM () const
{
return dMax (-m_engineRPS * m_crownGearRatio * 9.55f, 0.0f);
}
