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]);
}
}
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;
}
const dMaterialPairManager::dMaterialPair* dMaterialPairManager::GetPair (int materialId_0, int materialId_1, int threadIndex) const
{
unsigned key = MakeKey (materialId_0, materialId_1);
if (m_cachedKeys[threadIndex] != key) {
m_cachedKeys[threadIndex] = key;
int index0 = 0;
int index2 = m_entryCount - 1;
unsigned key0 = m_keys[index0];
unsigned key2 = m_keys[index2];
while ((index2 - index0) > 4) {
int index1 = (index0 + index2) >> 1;
unsigned key1 = m_keys[index1];
if (key < key1) {
index2 = index1;
key2 = key1;
} else {
index0 = index1;
key0 = key1;
}
}
index2 = dMin (m_entryCount, index2 + 4);
for (int i = index0; (i <= index2) && (m_keys[i] <= key); i ++) {
if (m_keys[i] == key) {
m_cachedMaterial[threadIndex] = &m_pool[i];
return m_cachedMaterial[threadIndex];
}
}
m_cachedMaterial[threadIndex] = &m_default;
}
void dComplentaritySolver::dFrictionLessContactJoint::JointAccelerations (dJointAccelerationDecriptor* const params)
{
dJacobianPair* const rowMatrix = params->m_rowMatrix;
dJacobianColum* const jacobianColElements = params->m_colMatrix;
const dVector& bodyVeloc0 = m_state0->GetVelocity();
const dVector& bodyOmega0 = m_state0->GetOmega();
const dVector& bodyVeloc1 = m_state1->GetVelocity();
const dVector& bodyOmega1 = m_state1->GetOmega();
int count = params->m_rowsCount;
dAssert (params->m_timeStep > dFloat (0.0f));
for (int k = 0; k < count; k ++) {
const dJacobianPair& Jt = rowMatrix[k];
dJacobianColum& element = jacobianColElements[k];
dVector relVeloc (Jt.m_jacobian_IM0.m_linear.CompProduct(bodyVeloc0) + Jt.m_jacobian_IM0.m_angular.CompProduct(bodyOmega0) + Jt.m_jacobian_IM1.m_linear.CompProduct(bodyVeloc1) + Jt.m_jacobian_IM1.m_angular.CompProduct(bodyOmega1));
dFloat vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
dFloat aRel = element.m_deltaAccel;
//dFloat restitution = (vRel <= 0.0f) ? 1.05f : 1.0f;
dFloat restitution = (vRel <= 0.0f) ? (dFloat (1.0f) + m_restitution) : dFloat(1.0f);
vRel *= restitution;
vRel = dMin (dFloat (4.0f), vRel);
element.m_coordenateAccel = (aRel - vRel * params->m_invTimeStep);
}
}
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::getMin(double *d, int n)
{
double v = 0;
for (int i = 0; i < n; i++)
v = dMin(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;
}
}
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);
}
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;
}
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);
}
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);
}
dNewtonLuaCompiler::dUserVariable dNewtonLuaCompiler::EmitAssigmentStatement(const dUserVariable& nameList, const dUserVariable& expresionList)
{
//dList<dCIL::dListNode*>::dListNode* nameListNode = nameList.m_nodeList.GetFirst();
dList<dString>::dListNode* nameListNode = nameList.m_tokenList.GetFirst();
dList<dCIL::dListNode*>::dListNode* expressionListNode = expresionList.m_nodeList.GetFirst();
dAssert(nameList.m_tokenList.GetCount() >= 1);
dAssert(expresionList.m_nodeList.GetCount() >= 1);
int count = dMin (nameList.m_tokenList.GetCount(), expresionList.m_nodeList.GetCount());
for (int i = 0; i < count; i ++) {
//dCILSingleArgInstr* const dst = nameListNode->GetInfo();
const dString& dstName = nameListNode->GetInfo();
/*
if (dest->GetAsLocal()) {
dCILInstrLocal* const dstIntruction = dest->GetAsLocal();
const dCILInstr::dArg& argName = dstIntruction->GetArg0();
for (dList<dCILInstrLocal*>::dListNode* node = m_currentClosure->m_localVariables.GetFirst(); node; node = node->GetNext()) {
dCILInstrLocal* const intruction = node->GetInfo()->GetAsLocal();
dAssert(intruction);
if (argName.m_label == intruction->GetArg0().m_label) {
destArg = argName;
break;
}
}
}
*/
dCILSingleArgInstr* const src = expressionListNode->GetInfo()->GetInfo()->GetAsSingleArg();
dAssert(src);
//dAssert(dst);
//const dCILInstr::dArg& dstArg = dst->GetArg0();
const dCILInstr::dArg& srcArg = src->GetArg0();
dCILInstrMove* const move = new dCILInstrMove(*m_currentClosure, dstName, srcArg.GetType(), srcArg.m_label, srcArg.GetType());
TRACE_INSTRUCTION(move);
nameListNode = nameListNode->GetNext();
expressionListNode = expressionListNode->GetNext();
}
return dUserVariable();
}
void CustomPlayerController::PostUpdate(dFloat timestep, int threadIndex)
{
dMatrix matrix;
dQuaternion bodyRotation;
dVector veloc(0.0f, 0.0f, 0.0f, 0.0f);
dVector omega(0.0f, 0.0f, 0.0f, 0.0f);
CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*) GetManager();
NewtonWorld* const world = manager->GetWorld();
// apply the player motion, by calculation the desired plane linear and angular velocity
manager->ApplyPlayerMove (this, timestep);
// get the body motion state
NewtonBodyGetMatrix(m_body, &matrix[0][0]);
NewtonBodyGetVelocity(m_body, &veloc[0]);
NewtonBodyGetOmega(m_body, &omega[0]);
// integrate body angular velocity
NewtonBodyGetRotation (m_body, &bodyRotation.m_q0);
bodyRotation = bodyRotation.IntegrateOmega(omega, timestep);
matrix = dMatrix (bodyRotation, matrix.m_posit);
// integrate linear velocity
dFloat normalizedTimeLeft = 1.0f;
dFloat step = timestep * dSqrt (veloc % veloc) ;
dFloat descreteTimeStep = timestep * (1.0f / D_DESCRETE_MOTION_STEPS);
int prevContactCount = 0;
CustomControllerConvexCastPreFilter castFilterData (m_body);
NewtonWorldConvexCastReturnInfo prevInfo[PLAYER_CONTROLLER_MAX_CONTACTS];
dVector updir (matrix.RotateVector(m_upVector));
dVector scale;
NewtonCollisionGetScale (m_upperBodyShape, &scale.m_x, &scale.m_y, &scale.m_z);
//const dFloat radio = m_outerRadio * 4.0f;
const dFloat radio = (m_outerRadio + m_restrainingDistance) * 4.0f;
NewtonCollisionSetScale (m_upperBodyShape, m_height - m_stairStep, radio, radio);
NewtonWorldConvexCastReturnInfo upConstratint;
memset (&upConstratint, 0, sizeof (upConstratint));
upConstratint.m_normal[0] = m_upVector.m_x;
upConstratint.m_normal[1] = m_upVector.m_y;
upConstratint.m_normal[2] = m_upVector.m_z;
upConstratint.m_normal[3] = m_upVector.m_w;
for (int j = 0; (j < D_PLAYER_MAX_INTERGRATION_STEPS) && (normalizedTimeLeft > 1.0e-5f); j ++ ) {
if ((veloc % veloc) < 1.0e-6f) {
break;
}
dFloat timetoImpact;
NewtonWorldConvexCastReturnInfo info[PLAYER_CONTROLLER_MAX_CONTACTS];
dVector destPosit (matrix.m_posit + veloc.Scale (timestep));
int contactCount = NewtonWorldConvexCast (world, &matrix[0][0], &destPosit[0], m_upperBodyShape, &timetoImpact, &castFilterData, CustomControllerConvexCastPreFilter::Prefilter, info, sizeof (info) / sizeof (info[0]), threadIndex);
if (contactCount) {
contactCount = manager->ProcessContacts (this, info, contactCount);
}
if (contactCount) {
matrix.m_posit += veloc.Scale (timetoImpact * timestep);
if (timetoImpact > 0.0f) {
matrix.m_posit -= veloc.Scale (D_PLAYER_CONTACT_SKIN_THICKNESS / dSqrt (veloc % veloc)) ;
}
normalizedTimeLeft -= timetoImpact;
dFloat speed[PLAYER_CONTROLLER_MAX_CONTACTS * 2];
dFloat bounceSpeed[PLAYER_CONTROLLER_MAX_CONTACTS * 2];
dVector bounceNormal[PLAYER_CONTROLLER_MAX_CONTACTS * 2];
for (int i = 1; i < contactCount; i ++) {
dVector n0 (info[i-1].m_normal);
for (int j = 0; j < i; j ++) {
dVector n1 (info[j].m_normal);
if ((n0 % n1) > 0.9999f) {
info[i] = info[contactCount - 1];
i --;
contactCount --;
break;
}
}
}
int count = 0;
if (!m_isJumping) {
upConstratint.m_point[0] = matrix.m_posit.m_x;
upConstratint.m_point[1] = matrix.m_posit.m_y;
upConstratint.m_point[2] = matrix.m_posit.m_z;
upConstratint.m_point[3] = matrix.m_posit.m_w;
speed[count] = 0.0f;
bounceNormal[count] = dVector (upConstratint.m_normal);
bounceSpeed[count] = CalculateContactKinematics(veloc, &upConstratint);
count ++;
}
for (int i = 0; i < contactCount; i ++) {
speed[count] = 0.0f;
bounceNormal[count] = dVector (info[i].m_normal);
bounceSpeed[count] = CalculateContactKinematics(veloc, &info[i]);
count ++;
}
for (int i = 0; i < prevContactCount; i ++) {
speed[count] = 0.0f;
bounceNormal[count] = dVector (prevInfo[i].m_normal);
bounceSpeed[count] = CalculateContactKinematics(veloc, &prevInfo[i]);
count ++;
}
dFloat residual = 10.0f;
dVector auxBounceVeloc (0.0f, 0.0f, 0.0f, 0.0f);
for (int i = 0; (i < D_PLAYER_MAX_SOLVER_ITERATIONS) && (residual > 1.0e-3f); i ++) {
residual = 0.0f;
for (int k = 0; k < count; k ++) {
dVector normal (bounceNormal[k]);
dFloat v = bounceSpeed[k] - normal % auxBounceVeloc;
dFloat x = speed[k] + v;
if (x < 0.0f) {
v = 0.0f;
x = 0.0f;
}
if (dAbs (v) > residual) {
residual = dAbs (v);
}
auxBounceVeloc += normal.Scale (x - speed[k]);
speed[k] = x;
}
}
dVector velocStep (0.0f, 0.0f, 0.0f, 0.0f);
for (int i = 0; i < count; i ++) {
dVector normal (bounceNormal[i]);
velocStep += normal.Scale (speed[i]);
}
veloc += velocStep;
dFloat velocMag2 = velocStep % velocStep;
if (velocMag2 < 1.0e-6f) {
dFloat advanceTime = dMin (descreteTimeStep, normalizedTimeLeft * timestep);
matrix.m_posit += veloc.Scale (advanceTime);
normalizedTimeLeft -= advanceTime / timestep;
}
prevContactCount = contactCount;
memcpy (prevInfo, info, prevContactCount * sizeof (NewtonWorldConvexCastReturnInfo));
} else {
matrix.m_posit = destPosit;
matrix.m_posit.m_w = 1.0f;
break;
}
}
NewtonCollisionSetScale (m_upperBodyShape, scale.m_x, scale.m_y, scale.m_z);
// determine if player is standing on some plane
dMatrix supportMatrix (matrix);
supportMatrix.m_posit += updir.Scale (m_sphereCastOrigin);
if (m_isJumping) {
dVector dst (matrix.m_posit);
UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex);
} else {
step = dAbs (updir % veloc.Scale (timestep));
dFloat castDist = ((m_groundPlane % m_groundPlane) > 0.0f) ? m_stairStep : step;
dVector dst (matrix.m_posit - updir.Scale (castDist * 2.0f));
UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex);
}
// set player velocity, position and orientation
NewtonBodySetVelocity(m_body, &veloc[0]);
NewtonBodySetMatrix (m_body, &matrix[0][0]);
}
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 dCustomBallAndSocket::SetConeLimits(dFloat maxAngle)
{
m_maxConeAngle = dMin (dAbs (maxAngle), dFloat (160.0f * dDegreeToRad));
}
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 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()));
}
