PlaformEntityEntity (DemoEntityManager* const scene, DemoEntity* const source, NewtonBody* const triggerPort0, NewtonBody* const triggerPort1)
:DemoEntity (source->GetNextMatrix(), NULL)
{
scene->Append(this);
DemoMesh* const mesh = (DemoMesh*)source->GetMesh();
dAssert (mesh->IsType(DemoMesh::GetRttiType()));
SetMesh(mesh, source->GetMeshMatrix());
const dFloat mass = 100.0f;
dMatrix matrix (source->GetNextMatrix()) ;
NewtonWorld* const world = scene->GetNewton();
// note: because the mesh matrix can have scale, for simplicity just apply the local mesh matrix to the vertex cloud
dVector pool[128];
const dMatrix& meshMatrix = GetMeshMatrix();
meshMatrix.TransformTriplex(&pool[0].m_x, sizeof (dVector), mesh->m_vertex, 3 * sizeof (dFloat), mesh->m_vertexCount);
NewtonCollision* const collision = NewtonCreateConvexHull(world, mesh->m_vertexCount, &pool[0].m_x, sizeof (dVector), 0, 0, NULL);
NewtonBody* body = CreateSimpleBody (world, this, 100, matrix, collision, 0);
NewtonDestroyCollision(collision);
// attach a kinematic joint controller joint to move this body
dVector pivot;
NewtonBodyGetCentreOfMass (body, &pivot[0]);
pivot = matrix.TransformVector(pivot);
m_driver = new FerryDriver (body, pivot, triggerPort0, triggerPort1);
m_driver->SetMaxLinearFriction (50.0f * dAbs (mass * DEMO_GRAVITY));
m_driver->SetMaxAngularFriction(50.0f * dAbs (mass * DEMO_GRAVITY));
}
CustomVehicleControllerComponentEngine::dGearBox::dGearState* CustomVehicleControllerComponentEngine::dGearBox::dGearState::Update(CustomVehicleController* const vehicle)
{
const CustomVehicleControllerComponentEngine* const engine = vehicle->GetEngine();
dFloat param = engine->GetParam();
dFloat speed = engine->GetSpeed();
dFloat normalrpm = engine->GetRPM() / engine->GetRedLineRPM();
if ((normalrpm > m_shiftUp) && (speed > 1.0f)) {
return m_next;
} else if (normalrpm < m_shiftDown) {
if ((dAbs (speed) < 0.5f) && (param < dFloat (1.0e-3f))) {
return m_neutral;
}
if ((dAbs (speed) < 2.0f) && (param < dFloat (-1.0e-3f))) {
return m_reverse;
}
if (param > dFloat (-1.0e-3f))
dAssert (m_prev != m_neutral);
dAssert (m_prev != m_reverse);
return m_prev;
} else if (param < 0.0f) {
dAssert (0);
/*
if (speed < 1.0f) {
return m_reverse;
}
*/
}
return this;
}
CustomVehicleControllerComponentTrackSkidSteering::CustomVehicleControllerComponentTrackSkidSteering (CustomVehicleController* const controller, dFloat steeringRPM, dFloat teeringTorque)
:CustomVehicleControllerComponentSteering (controller, 0.0f)
,m_steeringRPM (dAbs(steeringRPM))
,m_steeringTorque (dAbs(teeringTorque))
{
m_differencialTurnRate = m_steeringTorque / (m_steeringRPM * m_steeringRPM);
}
void Custom6DOF::SetAngularLimits (const dVector& minAngularLimits, const dVector& maxAngularLimits)
{
for (int i = 0; i < 3; i ++) {
m_minAngularLimits[i] = (dAbs (minAngularLimits[i]) < dFloat (1.0e-5f)) ? 0.0f : minAngularLimits[i];
m_maxAngularLimits[i] = (dAbs (maxAngularLimits[i]) < dFloat (1.0e-5f)) ? 0.0f : maxAngularLimits[i];
}
}
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);
}
dQuaternion::dQuaternion (const dMatrix &matrix)
{
enum QUAT_INDEX
{
X_INDEX=0,
Y_INDEX=1,
Z_INDEX=2
};
static QUAT_INDEX QIndex [] = {Y_INDEX, Z_INDEX, X_INDEX};
dFloat trace = matrix[0][0] + matrix[1][1] + matrix[2][2];
dAssert (((matrix[0] * matrix[1]) % matrix[2]) > 0.0f);
if (trace > dFloat(0.0f)) {
trace = dSqrt (trace + dFloat(1.0f));
m_q0 = dFloat (0.5f) * trace;
trace = dFloat (0.5f) / trace;
m_q1 = (matrix[1][2] - matrix[2][1]) * trace;
m_q2 = (matrix[2][0] - matrix[0][2]) * trace;
m_q3 = (matrix[0][1] - matrix[1][0]) * trace;
} else {
QUAT_INDEX i = X_INDEX;
if (matrix[Y_INDEX][Y_INDEX] > matrix[X_INDEX][X_INDEX]) {
i = Y_INDEX;
}
if (matrix[Z_INDEX][Z_INDEX] > matrix[i][i]) {
i = Z_INDEX;
}
QUAT_INDEX j = QIndex [i];
QUAT_INDEX k = QIndex [j];
trace = dFloat(1.0f) + matrix[i][i] - matrix[j][j] - matrix[k][k];
trace = dSqrt (trace);
dFloat* const ptr = &m_q1;
ptr[i] = dFloat (0.5f) * trace;
trace = dFloat (0.5f) / trace;
m_q0 = (matrix[j][k] - matrix[k][j]) * trace;
ptr[j] = (matrix[i][j] + matrix[j][i]) * trace;
ptr[k] = (matrix[i][k] + matrix[k][i]) * trace;
}
#if _DEBUG
dMatrix tmp (*this, matrix.m_posit);
dMatrix unitMatrix (tmp * matrix.Inverse());
for (int i = 0; i < 4; i ++) {
dFloat err = dAbs (unitMatrix[i][i] - dFloat(1.0f));
dAssert (err < dFloat (1.0e-3f));
}
dFloat err = dAbs (DotProduct(*this) - dFloat(1.0f));
dAssert (err < dFloat(1.0e-3f));
#endif
}
bool dMatrix::TestIdentity() const
{
const dMatrix & matrix = *this;
const dMatrix & identity = dGetIdentityMatrix();
bool isIdentity = true;
for (int i = 0; isIdentity && (i < 3); i ++)
{
isIdentity &= dAbs (matrix[3][i]) < 1.0e-4f;
for (int j = i; isIdentity && (j < 3); j ++)
isIdentity &= dAbs (matrix[i][j] - identity[i][j]) < 1.0e-4f;
}
return isIdentity;
}
dVector FindFloor (const NewtonWorld* world, const dVector& origin, dFloat dist)
{
// shot a vertical ray from a high altitude and collect the intersection parameter.
dVector p0 (origin);
dVector p1 (origin - dVector (0.0f, dAbs (dist), 0.0f, 0.0f));
dFloat parameter = 1.2f;
NewtonWorldRayCast (world, &p0[0], &p1[0], RayCastPlacement, ¶meter, RayPrefilter, 0);
if (parameter < 1.0f) {
p0 -= dVector (0.0f, dAbs (dist) * parameter, 0.0f, 0.0f);
}
return p0;
}
dFloat CustomDGRayCastCar::CalculateLongitudinalForce (int tireIndex, dFloat hubSpeed, dFloat tireLoad) const
{
dFloat force = 0.0f;
Tire& tire = m_tires[tireIndex];
//hubSpeed = 10.0;
//tire.m_torque = 500;
dFloat velocAbs = dAbs (hubSpeed);
if (velocAbs > 1.0e-2f) {
dFloat den = 1.0f / velocAbs;
float omega0 = hubSpeed;
float maxSlip = m_normalizedLongitudinalForce.GetMaxValue ();
float omega1 = velocAbs * maxSlip + hubSpeed;
if (tire.m_torque < 0.0f) {
//maxSlip *= -1.0f;
omega1 = velocAbs * maxSlip - hubSpeed;
}
// float omega1 = velocAbs * maxSlip + hubSpeed;
//dFloat slipRatioCoef = (dAbs (axelLinearSpeed) > 1.e-3f) ? ((tireRotationSpeed - axelLinearSpeed) / dAbs (axelLinearSpeed)) : 0.0f;
dFloat omega = 0.0f;
dFloat slipRatio = 0.0f;
for (int i = 0; i < 32; i ++) {
omega = (omega0 + omega1) * 0.5f;
slipRatio = den * (omega - hubSpeed);
force = m_normalizedLongitudinalForce.GetValue (slipRatio) * tireLoad;
dFloat torque = tire.m_torque - tire.m_radius * force;
if (torque > 1.0e-1f) {
omega0 = omega;
} else if (torque < -1.0e-1f) {
omega1 = omega;
} else {
break;
}
}
tire.m_angularVelocity = -omega / tire.m_radius;
} else {
if (dAbs (tire.m_torque) > 0.1f) {
_ASSERTE (0);
}
//dFloat slipRatioCoef = (dAbs (axelLinearSpeed) > 1.e-3f) ? ((tireRotationSpeed - axelLinearSpeed) / dAbs (axelLinearSpeed)) : 0.0f;
}
return force;
}
bool dMatrix::SanityCheck() const
{
dVector right (m_front * m_up);
if (dAbs (right % m_right) < 0.9999f)
return false;
if (dAbs (m_right.m_w) > 0.0f)
return false;
if (dAbs (m_up.m_w) > 0.0f)
return false;
if (dAbs (m_right.m_w) > 0.0f)
return false;
if (dAbs (m_posit.m_w) != 1.0f)
return false;
return true;
}
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;
}
dMatrix dMatrix::Inverse4x4 () const
{
const dFloat tol = 1.0e-4f;
dMatrix tmp (*this);
dMatrix inv (dGetIdentityMatrix());
for (int i = 0; i < 4; i ++)
{
dFloat diag = tmp[i][i];
if (dAbs (diag) < tol)
{
int j = 0;
for (j = i + 1; j < 4; j ++)
{
dFloat val = tmp[j][i];
if (dAbs (val) > tol)
break;
}
dAssert (j < 4);
for (int k = 0; k < 4; k ++)
{
tmp[i][k] += tmp[j][k];
inv[i][k] += inv[j][k];
}
diag = tmp[i][i];
}
dFloat invDiag = 1.0f / diag;
for (int j = 0; j < 4; j ++)
{
tmp[i][j] *= invDiag;
inv[i][j] *= invDiag;
}
tmp[i][i] = 1.0f;
for (int j = 0; j < 4; j ++)
{
if (j != i)
{
dFloat pivot = tmp[j][i];
for (int k = 0; k < 4; k ++)
{
tmp[j][k] -= pivot * tmp[i][k];
inv[j][k] -= pivot * inv[i][k];
}
tmp[j][i] = 0.0f;
}
}
}
return inv;
}
void dCustomTireSpringDG::SubmitConstraints(dFloat timestep, int threadIndex)
{
NewtonBody* BodyAttach;
//NewtonBody* BodyFrame;
//
dVector tireOmega = dVector(0.0f, 0.0f, 0.0f);
//BodyFrame = GetBody0();
BodyAttach = GetBody1();
//
SteeringController(timestep);
//
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(mChassisPivotMatrix, mTirePivotMatrix);
//
NewtonBodyGetOmega(BodyAttach, &tireOmega[0]);
//
mRealOmega = dAbs(tireOmega.DotProduct3(mChassisPivotMatrix.m_front));
//
TireCenterPin(timestep);
//
TireCenterBolt(timestep);
//
SuspenssionSpringLimits(timestep);
//
TireBreakAction(BodyAttach, timestep);
}
CustomVehicleControllerComponentSteering::CustomVehicleControllerComponentSteering (CustomVehicleController* const controller, dFloat maxAngleInRadians)
:CustomVehicleControllerComponent (controller)
,m_maxAngle (dAbs (maxAngleInRadians))
,m_akermanWheelBaseWidth(0.0f)
,m_akermanAxelSeparation(0.0f)
{
}
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));
}
}
dVector FindFloor (const NewtonWorld* world, const dVector& origin, dFloat dist, dVector* const normal)
{
// shot a vertical ray from a high altitude and collect the intersection parameter.
dVector p0 (origin);
dVector p1 (origin - dVector (0.0f, dAbs (dist), 0.0f, 0.0f));
RayCastPlacementData parameter;
NewtonWorldRayCast (world, &p0[0], &p1[0], RayCastPlacement, ¶meter, RayPrefilter, 0);
if (parameter.m_param < 1.0f) {
p0 -= dVector (0.0f, dAbs (dist) * parameter.m_param, 0.0f, 0.0f);
if (normal) {
*normal = parameter.m_normal;
}
}
return p0;
}
static dErr JakoSIAVelocity(VHTCase scase,dReal b,dReal h,dReal dh[2],dReal z,dScalar u[3])
{
struct VHTRheology *rheo = &scase->rheo;
const dReal
p = rheo->pe,
n = 1 ? 1 : 1./(p-1),
B = rheo->B0 * pow(2*rheo->gamma0,(n-1)/(2*n)), // reduce to Stress = B |Du|^{1/n}
A = pow(B,-n);
const dScalar
slope = dSqrt(dSqr(dh[0]) + dSqr(dh[1])),
sliding = 0,
hmz = z > h ? 0 : (z < b ? h-b : h-z),
// The strain rate is: Du = A tau^n
// where: tau = rho*grav*(h-z)*dh
int_bz = -1. / (n+1) * (pow(hmz,n+1) - pow(h-b,n+1)), // \int_b^z (h-z)^n
//bstress = rheo->rhoi * dAbs(rheo->gravity[2]) * (h-b) * slope, // diagnostic
//rstress = dUnitNonDimensionalizeSI(scase->utable.Pressure,1e5),
siaspeed = sliding + A * pow(rheo->rhoi * dAbs(rheo->gravity[2]) * slope, n) * int_bz, // Integrate strain rate from the bottom
speed = siaspeed * (5 + 0 * (1 + tanh((z-b)/100))/2);
u[0] = -speed / slope * dh[0];
u[1] = -speed / slope * dh[1];
u[2] = 0; // Would need another derivative to evaluate this and it should not be a big deal for this stationary computation
//printf("u0 %g u1 %g rho %g grav %g stress %g 1bar %g dh[2] %g %g A %g B %g;\n",u[0],u[1],rheo->rhoi,rheo->gravity[2],bstress,rstress,dh[0],dh[1],A,B);
return 0;
}
bool dMatrix::TestOrthogonal() const
{
dVector n (m_front * m_up);
dFloat a = m_right % m_right;
dFloat b = m_up % m_up;
dFloat c = m_front % m_front;
dFloat d = n % m_right;
return (m_front[3] == dFloat (0.0f)) &
(m_up[3] == dFloat (0.0f)) &
(m_right[3] == dFloat (0.0f)) &
(m_posit[3] == dFloat (1.0f)) &
(dAbs (a - dFloat (1.0f)) < dFloat (1.0e-4f)) &
(dAbs (b - dFloat (1.0f)) < dFloat (1.0e-4f)) &
(dAbs (c - dFloat (1.0f)) < dFloat (1.0e-4f)) &
(dAbs (d - dFloat (1.0f)) < dFloat (1.0e-4f));
}
dFloat dPolygonRayCast (const dVector& l0, const dVector& l1, int indexCount, const dFloat* const vertex, int strideInBytes, const int* const indices)
{
int stride = strideInBytes / sizeof (dFloat);
dBigVector line0 (l0);
dBigVector line1(l1);
dBigVector segment (line1 - line0);
dBigVector normal (dPolygonNormal (indexCount, vertex, strideInBytes, indices));
double den = normal % segment;
if (dAbs(den) < 1.0e-6) {
return 1.2f;
}
double sign = (den < 0.0f) ? 1.0f : -1.0f;
int index = indices[indexCount - 1] * stride;
dBigVector v0 (vertex[index], vertex[index + 1], vertex[index + 2], 0.0f);
dBigVector p0v0 (v0 - line0);
for (int i = 0; i < indexCount; i ++) {
index = indices[i] * stride;
dBigVector v1 (vertex[index], vertex[index + 1], vertex[index + 2], 0.0f);
dBigVector p0v1 (v1 - line0);
double alpha = sign * ((p0v1 * p0v0) % segment);
if (alpha < 1.0e-3f) {
return 1.2f;
}
p0v0 = p0v1;
}
double t = - ((line0 - v0) % normal) / den;
if ((t < 0.0f) || (t > 1.0f)) {
return 1.2f;
}
return dFloat (t);
}
void dCustomCorkScrew::SubmitAngularRow(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
const dFloat angleError = GetMaxAngleError();
dFloat angle0 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_up);
NewtonUserJointAddAngularRow(m_joint, angle0, &matrix1.m_up[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
if (dAbs(angle0) > angleError) {
const dFloat alpha = NewtonUserJointCalculateRowZeroAcceleration(m_joint) + dFloat(0.25f) * angle0 / (timestep * timestep);
NewtonUserJointSetRowAcceleration(m_joint, alpha);
}
dFloat angle1 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right);
NewtonUserJointAddAngularRow(m_joint, angle1, &matrix1.m_right[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
if (dAbs(angle1) > angleError) {
const dFloat alpha = NewtonUserJointCalculateRowZeroAcceleration(m_joint) + dFloat(0.25f) * angle1 / (timestep * timestep);
NewtonUserJointSetRowAcceleration(m_joint, alpha);
}
// the joint angle can be determined by getting the angle between any two non parallel vectors
m_curJointAngle.Update(-CalculateAngle(matrix0.m_up, matrix1.m_up, matrix1.m_front));
// 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);
}
}
void SetSteerAngle (dFloat angle)
{
if (dAbs (angle - m_steeAngle) > 1.0e-4f) {
m_steeAngle = angle;
dMatrix rotation (dYawMatrix(m_steeAngle));
m_chassisLocalMatrix = rotation * m_refChassisLocalMatrix;
}
}
void CustomDGRayCastCar::ApplyDeceleration (Tire& tire)
{
if ( dAbs( tire.m_torque ) < 1.0e-3f ){
dVector cvel = m_chassisVelocity;
cvel = cvel.Scale( m_fixDeceleration );
cvel.m_y = m_chassisVelocity.m_y;
NewtonBodySetVelocity( m_body0, &cvel.m_x );
}
}
static dErr JakoGradient2(VHTCase scase,const dReal field[],dInt i,dInt j,dReal grad[2])
{
VHTCase_Jako *jako = scase->data;
const dReal dx = jako->mygeo[1],dy = jako->mygeo[5];
const dInt nx = jako->nx,ny = jako->ny;
dReal fx0,fx1,fy0,fy1;
dFunctionBegin;
if (i+1 >= nx) dERROR(PETSC_COMM_SELF,PETSC_ERR_ARG_OUTOFRANGE,"X slope would evaluate point (%D,%D) outside of valid range %Dx%D",i+1,j,nx,ny);
if (j-1 < 0) dERROR(PETSC_COMM_SELF,PETSC_ERR_ARG_OUTOFRANGE,"Y slope would evaluate point (%D,%D) outside of valid range %Dx%D",i,j-1,nx,ny);
fx0 = field[j*nx + i + 0] - field[j*nx + i - 1];
fx1 = field[j*nx + i + 1] - field[j*nx + i + 0];
grad[0] = (dAbs(fx0) < dAbs(fx1) ? fx0 : fx1) / dx;
// The raster has Y axis flipped
fy0 = field[(j+0)*nx + i] - field[(j+1)*nx + i];
fy1 = field[(j-1)*nx + i] - field[(j+0)*nx + i];
grad[1] = (dAbs(fy0) < dAbs(fy1) ? fy0 : fy1) / dy;
dFunctionReturn(0);
}
void dCustomHingeActuator::SubmitConstraintsFreeDof (dFloat timestep, const dMatrix& matrix0, const dMatrix& matrix1)
{
if (m_actuatorFlag) {
dFloat jointangle = GetActuatorAngle();
dFloat relAngle = jointangle - m_angle;
dFloat currentSpeed = GetJointOmega();
dFloat step = dFloat(2.0f) * m_angularRate * timestep;
dFloat desiredSpeed = (dAbs(relAngle) > dAbs(step)) ? -dSign(relAngle) * m_angularRate : -dFloat(0.1f) * relAngle / timestep;
dFloat accel = (desiredSpeed - currentSpeed) / timestep;
NewtonUserJointAddAngularRow(m_joint, relAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration(m_joint, accel);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxForce);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxForce);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
} else {
dCustomHinge::SubmitConstraintsFreeDof (timestep, matrix0, matrix1);
}
}
static void AddShatterEntity (DemoEntityManager* const scene, DemoMesh* const visualMesh, NewtonCollision* const collision, const ShatterEffect& shatterEffect, dVector location)
{
dQuaternion rotation;
SimpleShatterEffectEntity* const entity = new SimpleShatterEffectEntity (visualMesh, shatterEffect);
entity->SetMatrix(*scene, rotation, location);
entity->InterpolateMatrix (*scene, 1.0f);
scene->Append(entity);
dVector origin;
dVector inertia;
NewtonConvexCollisionCalculateInertialMatrix (collision, &inertia[0], &origin[0]);
float mass = 10.0f;
int materialId = 0;
dFloat Ixx = mass * inertia[0];
dFloat Iyy = mass * inertia[1];
dFloat Izz = mass * inertia[2];
//create the rigid body
dMatrix matrix (GetIdentityMatrix());
matrix.m_posit = location;
NewtonWorld* const world = scene->GetNewton();
NewtonBody* const rigidBody = NewtonCreateBody (world, collision, &matrix[0][0]);
entity->m_myBody = rigidBody;
entity->m_myweight = dAbs (mass * DEMO_GRAVITY);
// set the correct center of gravity for this body
NewtonBodySetCentreOfMass (rigidBody, &origin[0]);
// set the mass matrix
NewtonBodySetMassMatrix (rigidBody, mass, Ixx, Iyy, Izz);
// activate
// NewtonBodyCoriolisForcesMode (blockBoxBody, 1);
// save the pointer to the graphic object with the body.
NewtonBodySetUserData (rigidBody, entity);
// assign the wood id
NewtonBodySetMaterialGroupID (rigidBody, materialId);
// set continue collision mode
// NewtonBodySetContinuousCollisionMode (rigidBody, continueCollisionMode);
// set a destructor for this rigid body
NewtonBodySetDestructorCallback (rigidBody, PhysicsBodyDestructor);
// set the transform call back function
NewtonBodySetTransformCallback (rigidBody, DemoEntity::SetTransformCallback);
// set the force and torque call back function
NewtonBodySetForceAndTorqueCallback (rigidBody, PhysicsApplyGravityForce);
}
void CustomKinematicController::SetMaxLinearFriction(dFloat accel)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass (m_body0, &mass, &Ixx, &Iyy, &Izz);
m_maxLinearFriction = dAbs (accel) * mass;
}
void CustomPickBody::SetMaxLinearFriction(dFloat accel)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMassMatrix (m_body0, &mass, &Ixx, &Iyy, &Izz);
m_maxLinearFriction = dAbs (accel) * mass;
}
int dPackVertexArray (dFloat* const vertexList, int compareElements, int strideInBytes, int vertexCount, int* const indexListOut)
{
int stride = strideInBytes / sizeof (dFloat);
dFloat errorTol = dFloat (1.0e-4f);
dFloat* const array = new dFloat[(stride + 2) * vertexCount];
for (int i = 0; i < vertexCount; i ++) {
memcpy (&array[i * (stride + 2)], &vertexList[i * stride], strideInBytes);
array[i * (stride + 2) + stride + 0] = dFloat(i);
array[i * (stride + 2) + stride + 1] = 0.0f;
}
qsort(array, vertexCount, (stride + 2) * sizeof (dFloat), SortVertexArray);
int indexCount = 0;
for (int i = 0; i < vertexCount; i ++) {
int index = i * (stride + 2);
if (array[index + stride + 1] == 0.0f) {
dFloat window = array[index] + errorTol;
for (int j = i + 1; j < vertexCount; j ++) {
int index2 = j * (stride + 2);
if (array[index2] >= window) {
break;
}
if (array[index2 + stride + 1] == 0.0f) {
int k;
for (k = 0; k < compareElements; k ++) {
dFloat error;
error = array[index + k] - array[index2+ k];
if (dAbs (error) >= errorTol) {
break;
}
}
if (k >= compareElements) {
int m = int (array[index2 + stride]);
memcpy (&array[indexCount * (stride + 2)], &array[index2], sizeof (dFloat) * stride);
indexListOut[m] = indexCount;
array[index2 + stride + 1] = 1.0f;
}
}
}
int m = int (array[index + stride]);
memcpy (&array[indexCount * (stride + 2)], &array[index], sizeof (dFloat) * stride);
indexListOut[m] = indexCount;
array[indexCount * (stride + 2) + stride + 1] = 1.0f;
indexCount ++;
}
}
for (int i = 0; i < indexCount; i ++) {
memcpy (&vertexList[i * stride], &array[i * (stride + 2)], sizeof (dFloat) * stride);
}
delete[] array;
return indexCount;
}
void CustomUniversalActuator::SubmitConstraints (dFloat timestep, int threadIndex)
{
CustomUniversal::SubmitConstraints (timestep, threadIndex);
if (m_flag0 | m_flag1){
dMatrix matrix0;
dMatrix matrix1;
CalculateGlobalMatrix (matrix0, matrix1);
if (m_flag0) {
dFloat jointAngle = GetJointAngle_0();
dFloat relAngle = jointAngle - m_angle0;
NewtonUserJointAddAngularRow (m_joint, -relAngle, &matrix0.m_front[0]);
dFloat step = m_angularRate0 * timestep;
if (dAbs (relAngle) > 2.0f * dAbs (step)) {
dFloat desiredSpeed = dSign(relAngle) * m_angularRate0;
dFloat currentSpeed = GetJointOmega_0 ();
dFloat accel = (desiredSpeed - currentSpeed) / timestep;
NewtonUserJointSetRowAcceleration (m_joint, accel);
}
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxForce0);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxForce0);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
}
if (m_flag1) {
dFloat jointAngle = GetJointAngle_1();
dFloat relAngle = jointAngle - m_angle1;
NewtonUserJointAddAngularRow (m_joint, -relAngle, &matrix1.m_up[0]);
dFloat step = m_angularRate1 * timestep;
if (dAbs (relAngle) > 2.0f * dAbs (step)) {
dFloat desiredSpeed = dSign(relAngle) * m_angularRate1;
dFloat currentSpeed = GetJointOmega_1 ();
dFloat accel = (desiredSpeed - currentSpeed) / timestep;
NewtonUserJointSetRowAcceleration (m_joint, accel);
}
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxForce1);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxForce1);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
}
}
}
//void dMatrix::PolarDecomposition (dMatrix& orthogonal, dMatrix& symetric) const
void dMatrix::PolarDecomposition (dMatrix & transformMatrix, dVector & scale, dMatrix & stretchAxis, const dMatrix & initialStretchAxis) const
{
// a polar decomposition decompose matrix A = O * S
// where S = sqrt (transpose (L) * L)
// calculate transpose (L) * L
dMatrix LL ((*this) * Transpose());
// check is this si a pure uniformScale * rotation * translation
dFloat det2 = (LL[0][0] + LL[1][1] + LL[2][2]) * (1.0f / 3.0f);
dFloat invdet2 = 1.0f / det2;
dMatrix pureRotation (LL);
pureRotation[0] = pureRotation[0].Scale (invdet2);
pureRotation[1] = pureRotation[1].Scale (invdet2);
pureRotation[2] = pureRotation[2].Scale (invdet2);
//dFloat soureSign = ((*this)[0] * (*this)[1]) % (*this)[2];
dFloat sign = ((((*this)[0] * (*this)[1]) % (*this)[2]) > 0.0f) ? 1.0f : -1.0f;
dFloat det = (pureRotation[0] * pureRotation[1]) % pureRotation[2];
if (dAbs (det - 1.0f) < 1.e-5f)
{
// this is a pure scale * rotation * translation
det = sign * dSqrt (det2);
scale[0] = det;
scale[1] = det;
scale[2] = det;
scale[3] = 1.0f;
det = 1.0f / det;
transformMatrix.m_front = m_front.Scale (det);
transformMatrix.m_up = m_up.Scale (det);
transformMatrix.m_right = m_right.Scale (det);
transformMatrix[0][3] = 0.0f;
transformMatrix[1][3] = 0.0f;
transformMatrix[2][3] = 0.0f;
transformMatrix.m_posit = m_posit;
stretchAxis = dGetIdentityMatrix();
}
else
{
stretchAxis = LL.JacobiDiagonalization (scale, initialStretchAxis);
// I need to deal with buy seeing of some of the Scale are duplicated
// do this later (maybe by a given rotation around the non uniform axis but I do not know if it will work)
// for now just us the matrix
scale[0] = sign * dSqrt (scale[0]);
scale[1] = sign * dSqrt (scale[1]);
scale[2] = sign * dSqrt (scale[2]);
scale[3] = 1.0f;
dMatrix scaledAxis;
scaledAxis[0] = stretchAxis[0].Scale (1.0f / scale[0]);
scaledAxis[1] = stretchAxis[1].Scale (1.0f / scale[1]);
scaledAxis[2] = stretchAxis[2].Scale (1.0f / scale[2]);
scaledAxis[3] = stretchAxis[3];
dMatrix symetricInv (stretchAxis.Transpose() * scaledAxis);
transformMatrix = symetricInv * (*this);
transformMatrix.m_posit = m_posit;
}
}
