void dCustomPlayerController::PreUpdate(dFloat timestep)
{
dFloat timeLeft = timestep;
const dFloat timeEpsilon = timestep * (1.0f / 16.0f);
m_impulse = dVector(0.0f);
m_manager->ApplyMove(this, timestep);
//SetForwardSpeed(1.0f);
//SetLateralSpeed(0.0f);
//SetHeadingAngle(45.0f*dDegreeToRad);
// set player orientation
dMatrix matrix(dYawMatrix(GetHeadingAngle()));
NewtonBodyGetPosition(m_kinematicBody, &matrix.m_posit[0]);
NewtonBodySetMatrix(m_kinematicBody, &matrix[0][0]);
// set play desired velocity
dVector veloc(GetVelocity() + m_impulse.Scale(m_invMass));
NewtonBodySetVelocity(m_kinematicBody, &veloc[0]);
// determine if player has to step over obstacles lower than step hight
ResolveStep(timestep);
// advance player until it hit a collision point, until there is not more time left
for (int i = 0; (i < D_DESCRETE_MOTION_STEPS) && (timeLeft > timeEpsilon); i++) {
if (timeLeft > timeEpsilon) {
ResolveCollision();
}
dFloat predicetdTime = PredictTimestep(timestep);
NewtonBodyIntegrateVelocity(m_kinematicBody, predicetdTime);
timeLeft -= predicetdTime;
}
}
void SpawnRandomProp(const dMatrix& location) const
{
NewtonWorld* const world = GetWorld();
DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData(world);
//scene->SetCurrent();
static PrimitiveType proSelection[] = {_SPHERE_PRIMITIVE, _BOX_PRIMITIVE, _CAPSULE_PRIMITIVE, _CYLINDER_PRIMITIVE, _CONE_PRIMITIVE,
_CHAMFER_CYLINDER_PRIMITIVE, _RANDOM_CONVEX_HULL_PRIMITIVE, _REGULAR_CONVEX_HULL_PRIMITIVE};
PrimitiveType type = PrimitiveType (dRand() % (sizeof (proSelection) / sizeof (proSelection[0])));
dVector size (0.35f, 0.25f, 0.25f, 0.0f);
NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), size, type, 0);
DemoMesh* const geometry = new DemoMesh("prop", collision, "smilli.tga", "smilli.tga", "smilli.tga");
dMatrix matrix (location);
matrix.m_posit.m_y += 0.5f;
NewtonBody* const prop = CreateSimpleSolid (scene, geometry, 30.0f, matrix, collision, 0);
NewtonDestroyCollision(collision);
geometry->Release();
dFloat initialSpeed = 20.0f;
dVector veloc (matrix.m_front.Scale (initialSpeed));
NewtonBodySetVelocity(prop, &veloc[0]);
NewtonBodySetLinearDamping(prop, 0);
}
virtual void SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix;
dVector com;
const dFloat speed = 3.0f;
NewtonBody* const body = GetBody0();
NewtonBodyGetCentreOfMass(body, &com[0]);
NewtonBodyGetMatrix(body, &matrix[0][0]);
com = matrix.TransformVector(com);
switch (m_state)
{
case m_stop:
{
SetTargetPosit (com);
break;
}
case m_driving:
{
dVector veloc (m_target - com);
veloc = veloc.Scale (speed / dSqrt (veloc % veloc));
dVector target = com + veloc.Scale(timestep);
SetTargetPosit (target);
break;
}
default:;
dAssert (0);
}
CustomKinematicController::SubmitConstraints (timestep, threadIndex);
}
void CustomPlayerController::SetPlayerVelocity (dFloat forwardSpeed, dFloat lateralSpeed, dFloat verticalSpeed, dFloat headingAngle, const dVector& gravity, dFloat timestep)
{
dVector omega (CalculateDesiredOmega (headingAngle, timestep));
dVector veloc (CalculateDesiredVelocity (forwardSpeed, lateralSpeed, verticalSpeed, gravity, timestep));
NewtonBodySetOmega(m_body, &omega[0]);
NewtonBodySetVelocity(m_body, &veloc[0]);
if ((verticalSpeed > 0.0f)) {
m_isJumping = true;
}
}
// create a mesh using the NewtonMesh low lever interface
static NewtonBody* CreateSimpleBox_NewtonMesh (DemoEntityManager* const scene, const dVector& origin, const dVector& scale, dFloat mass)
{
dBigVector array[8];
dBigVector scale1 (scale);
for (int i = 0; i < 8; i ++) {
dBigVector p(&BoxPoints[i * 4]);
array[i] = scale1 * p;
}
NewtonMeshVertexFormat vertexFormat;
NewtonMeshClearVertexFormat(&vertexFormat);
vertexFormat.m_faceCount = 10;
vertexFormat.m_faceIndexCount = faceIndexList;
vertexFormat.m_faceMaterial = faceMateriaIndexList;
vertexFormat.m_vertex.m_data = &array[0][0];
vertexFormat.m_vertex.m_indexList = BoxIndices;
vertexFormat.m_vertex.m_strideInBytes = sizeof (dBigVector);
vertexFormat.m_normal.m_data = normal;
vertexFormat.m_normal.m_indexList = faceNormalIndex;
vertexFormat.m_normal.m_strideInBytes = 3 * sizeof (dFloat);
// all channel are now optionals so we not longer has to pass default values
// vertexFormat.m_uv0.m_data = uv0;
// vertexFormat.m_uv0.m_indexList = uv0_indexList;
// vertexFormat.m_uv0.m_strideInBytes = 2 * sizeof (dFloat);
// now we create and empty mesh
NewtonMesh* const newtonMesh = NewtonMeshCreate(scene->GetNewton());
NewtonMeshBuildFromVertexListIndexList(newtonMesh, &vertexFormat);
// now we can use this mesh for lot of stuff, we can apply UV, we can decompose into convex,
NewtonCollision* const collision = NewtonCreateConvexHullFromMesh(scene->GetNewton(), newtonMesh, 0.001f, 0);
// for now we will simple make simple Box, make a visual Mesh
DemoMesh* const visualMesh = new DemoMesh (newtonMesh);
dMatrix matrix (dGetIdentityMatrix());
matrix.m_posit = origin;
matrix.m_posit.m_w = 1.0f;
NewtonBody* const body = CreateSimpleSolid(scene, visualMesh, mass, matrix, collision, 0);
dVector veloc(1, 0, 2, 0);
NewtonBodySetVelocity(body, &veloc[0]);
visualMesh->Release();
NewtonDestroyCollision(collision);
NewtonMeshDestroy (newtonMesh);
return body;
}
void DemoSoundListener::PostUpdate (const NewtonWorld* const world, dFloat timestep)
{
DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData(world);
DemoCamera* const camera = scene->GetCamera();
dMatrix matrix0 (camera->GetCurrentMatrix ());
dMatrix matrix1 (camera->GetNextMatrix ());
dVector veloc ((matrix1.m_posit - matrix0.m_posit).Scale (1.0f / timestep));
//printf ("%f %f %f %f\n", veloc.m_x, veloc.m_y, veloc.m_z, timestepInSecunds);
UpdateListener (matrix1.m_posit, veloc, matrix0.m_front, matrix0.m_up);
dSoundManager::Update();
}
dVector CustomPlayerController::CalculateDesiredVelocity (dFloat forwardSpeed, dFloat lateralSpeed, dFloat verticalSpeed, const dVector& gravity, dFloat timestep) const
{
dMatrix matrix;
NewtonBodyGetMatrix(m_body, &matrix[0][0]);
dVector updir (matrix.RotateVector(m_upVector));
dVector frontDir (matrix.RotateVector(m_frontVector));
dVector rightDir (frontDir * updir);
dVector veloc (0.0f, 0.0f, 0.0f, 0.0f);
if ((verticalSpeed <= 0.0f) && (m_groundPlane % m_groundPlane) > 0.0f) {
// plane is supported by a ground plane, apply the player input velocity
if ((m_groundPlane % updir) >= m_maxSlope) {
// player is in a legal slope, he is in full control of his movement
dVector bodyVeloc;
NewtonBodyGetVelocity(m_body, &bodyVeloc[0]);
veloc = updir.Scale(bodyVeloc % updir) + gravity.Scale (timestep) + frontDir.Scale (forwardSpeed) + rightDir.Scale (lateralSpeed) + updir.Scale(verticalSpeed);
veloc += (m_groundVelocity - updir.Scale (updir % m_groundVelocity));
dFloat speedLimitMag2 = forwardSpeed * forwardSpeed + lateralSpeed * lateralSpeed + verticalSpeed * verticalSpeed + m_groundVelocity % m_groundVelocity + 0.1f;
dFloat speedMag2 = veloc % veloc;
if (speedMag2 > speedLimitMag2) {
veloc = veloc.Scale (dSqrt (speedLimitMag2 / speedMag2));
}
dFloat normalVeloc = m_groundPlane % (veloc - m_groundVelocity);
if (normalVeloc < 0.0f) {
veloc -= m_groundPlane.Scale (normalVeloc);
}
} else {
// player is in an illegal ramp, he slides down hill an loses control of his movement
NewtonBodyGetVelocity(m_body, &veloc[0]);
veloc += updir.Scale(verticalSpeed);
veloc += gravity.Scale (timestep);
dFloat normalVeloc = m_groundPlane % (veloc - m_groundVelocity);
if (normalVeloc < 0.0f) {
veloc -= m_groundPlane.Scale (normalVeloc);
}
}
} else {
// player is on free fall, only apply the gravity
NewtonBodyGetVelocity(m_body, &veloc[0]);
veloc += updir.Scale(verticalSpeed);
veloc += gravity.Scale (timestep);
}
return veloc;
}
void SetKinematicPose(NewtonBody* const body, const dMatrix& matrix1, dFloat timestep)
{
dMatrix matrix0;
const dFloat OneOverDt = 1.0f / timestep;
NewtonBodyGetMatrix(body, &matrix0[0][0]);
dQuaternion q0(matrix0);
dQuaternion q1(matrix1);
dVector omega(q0.CalcAverageOmega(q1, OneOverDt));
// const dFloat maxOmega = 10.0f;
// dFloat mag2 = omega.DotProduct3(omega);
// if (mag2 > maxOmega) {
// omega = omega.Normalize().Scale(maxOmega);
// }
dVector veloc ((matrix1.m_posit - matrix0.m_posit).Scale (OneOverDt));
NewtonBodySetVelocity(body, &veloc[0]);
NewtonBodySetOmega(body, &omega[0]);
NewtonBodyIntegrateVelocity(body, timestep);
}
void dgWorldDynamicUpdate::ResolveClusterForces(dgBodyCluster* const cluster, dgInt32 threadID, dgFloat32 timestep) const
{
if (cluster->m_activeJointCount) {
SortClusters(cluster, timestep, threadID);
}
if (!cluster->m_isContinueCollision) {
if (cluster->m_activeJointCount) {
BuildJacobianMatrix (cluster, threadID, timestep);
CalculateClusterReactionForces(cluster, threadID, timestep, DG_SOLVER_MAX_ERROR);
//CalculateClusterReactionForces_1(cluster, threadID, timestep, DG_SOLVER_MAX_ERROR);
} else {
IntegrateExternalForce(cluster, timestep, threadID);
}
IntegrateVelocity (cluster, DG_SOLVER_MAX_ERROR, timestep, threadID);
} else {
// calculate reaction forces and new velocities
BuildJacobianMatrix (cluster, threadID, timestep);
IntegrateReactionsForces (cluster, threadID, timestep, DG_SOLVER_MAX_ERROR);
// see if the island goes to sleep
bool isAutoSleep = true;
bool stackSleeping = true;
dgInt32 sleepCounter = 10000;
dgWorld* const world = (dgWorld*) this;
const dgInt32 bodyCount = cluster->m_bodyCount;
dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0];
dgBodyInfo* const bodyArray = &bodyArrayPtr[cluster->m_bodyStart];
const dgFloat32 forceDamp = DG_FREEZZING_VELOCITY_DRAG;
dgFloat32 maxAccel = dgFloat32 (0.0f);
dgFloat32 maxAlpha = dgFloat32 (0.0f);
dgFloat32 maxSpeed = dgFloat32 (0.0f);
dgFloat32 maxOmega = dgFloat32 (0.0f);
const dgFloat32 speedFreeze = world->m_freezeSpeed2;
const dgFloat32 accelFreeze = world->m_freezeAccel2;
const dgVector forceDampVect (forceDamp, forceDamp, forceDamp, dgFloat32 (0.0f));
for (dgInt32 i = 1; i < bodyCount; i ++) {
dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body;
if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
dgAssert (body->m_invMass.m_w);
const dgFloat32 accel2 = body->m_accel.DotProduct3(body->m_accel);
const dgFloat32 alpha2 = body->m_alpha.DotProduct3(body->m_alpha);
const dgFloat32 speed2 = body->m_veloc.DotProduct3(body->m_veloc);
const dgFloat32 omega2 = body->m_omega.DotProduct3(body->m_omega);
maxAccel = dgMax (maxAccel, accel2);
maxAlpha = dgMax (maxAlpha, alpha2);
maxSpeed = dgMax (maxSpeed, speed2);
maxOmega = dgMax (maxOmega, omega2);
bool equilibrium = (accel2 < accelFreeze) && (alpha2 < accelFreeze) && (speed2 < speedFreeze) && (omega2 < speedFreeze);
if (equilibrium) {
dgVector veloc (body->m_veloc * forceDampVect);
dgVector omega = body->m_omega * forceDampVect;
body->m_veloc = (dgVector (veloc.DotProduct4(veloc)) > m_velocTol) & veloc;
body->m_omega = (dgVector (omega.DotProduct4(omega)) > m_velocTol) & omega;
}
body->m_equilibrium = dgUnsigned32 (equilibrium);
stackSleeping &= equilibrium;
isAutoSleep &= body->m_autoSleep;
sleepCounter = dgMin (sleepCounter, body->m_sleepingCounter);
}
// clear accel and angular acceleration
body->m_accel = dgVector::m_zero;
body->m_alpha = dgVector::m_zero;
}
if (isAutoSleep) {
if (stackSleeping) {
// the island went to sleep mode,
for (dgInt32 i = 1; i < bodyCount; i ++) {
dgBody* const body = bodyArray[i].m_body;
dgAssert (body->IsRTTIType (dgBody::m_dynamicBodyRTTI) || body->IsRTTIType (dgBody::m_kinematicBodyRTTI));
body->m_accel = dgVector::m_zero;
body->m_alpha = dgVector::m_zero;
body->m_veloc = dgVector::m_zero;
body->m_omega = dgVector::m_zero;
}
} else {
// island is not sleeping but may be resting with small residual velocity for a long time
// see if we can force to go to sleep
if ((maxAccel > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAccel) ||
(maxAlpha > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAlpha) ||
(maxSpeed > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxVeloc) ||
(maxOmega > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxOmega)) {
for (dgInt32 i = 1; i < bodyCount; i ++) {
dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body;
if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
body->m_sleepingCounter = 0;
}
}
} else {
dgInt32 index = 0;
for (dgInt32 i = 0; i < DG_SLEEP_ENTRIES; i ++) {
if ((maxAccel <= world->m_sleepTable[i].m_maxAccel) &&
(maxAlpha <= world->m_sleepTable[i].m_maxAlpha) &&
(maxSpeed <= world->m_sleepTable[i].m_maxVeloc) &&
(maxOmega <= world->m_sleepTable[i].m_maxOmega)) {
index = i;
break;
}
}
dgInt32 timeScaleSleepCount = dgInt32 (dgFloat32 (60.0f) * sleepCounter * timestep);
if (timeScaleSleepCount > world->m_sleepTable[index].m_steps) {
// force island to sleep
stackSleeping = true;
for (dgInt32 i = 1; i < bodyCount; i ++) {
dgBody* const body = bodyArray[i].m_body;
dgAssert (body->IsRTTIType (dgBody::m_dynamicBodyRTTI) || body->IsRTTIType (dgBody::m_kinematicBodyRTTI));
body->m_accel = dgVector::m_zero;
body->m_alpha = dgVector::m_zero;
body->m_veloc = dgVector::m_zero;
body->m_omega = dgVector::m_zero;
body->m_equilibrium = true;
}
} else {
sleepCounter ++;
for (dgInt32 i = 1; i < bodyCount; i ++) {
dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body;
if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
body->m_sleepingCounter = sleepCounter;
}
}
}
}
}
}
if (!(isAutoSleep & stackSleeping)) {
// island is not sleeping, need to integrate island velocity
const dgUnsigned32 lru = world->GetBroadPhase()->m_lru;
const dgInt32 jointCount = cluster->m_jointCount;
dgJointInfo* const constraintArrayPtr = (dgJointInfo*) &world->m_jointsMemory[0];
dgJointInfo* const constraintArray = &constraintArrayPtr[cluster->m_jointStart];
dgFloat32 timeRemaining = timestep;
const dgFloat32 timeTol = dgFloat32 (0.01f) * timestep;
for (dgInt32 i = 0; (i < DG_MAX_CONTINUE_COLLISON_STEPS) && (timeRemaining > timeTol); i ++) {
// calculate the closest time to impact
dgFloat32 timeToImpact = timeRemaining;
for (dgInt32 j = 0; (j < jointCount) && (timeToImpact > timeTol); j ++) {
dgContact* const contact = (dgContact*) constraintArray[j].m_joint;
if (contact->GetId() == dgConstraint::m_contactConstraint) {
dgDynamicBody* const body0 = (dgDynamicBody*)contact->m_body0;
dgDynamicBody* const body1 = (dgDynamicBody*)contact->m_body1;
if (body0->m_continueCollisionMode | body1->m_continueCollisionMode) {
dgVector p;
dgVector q;
dgVector normal;
timeToImpact = dgMin (timeToImpact, world->CalculateTimeToImpact (contact, timeToImpact, threadID, p, q, normal, dgFloat32 (-1.0f / 256.0f)));
}
}
}
if (timeToImpact > timeTol) {
timeRemaining -= timeToImpact;
for (dgInt32 j = 1; j < bodyCount; j ++) {
dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body;
if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
body->IntegrateVelocity(timeToImpact);
body->UpdateWorlCollisionMatrix();
}
}
} else {
if (timeToImpact >= dgFloat32 (-1.0e-5f)) {
for (dgInt32 j = 1; j < bodyCount; j++) {
dgDynamicBody* const body = (dgDynamicBody*)bodyArray[j].m_body;
if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
body->IntegrateVelocity(timeToImpact);
body->UpdateWorlCollisionMatrix();
}
}
}
CalculateClusterContacts (cluster, timeRemaining, lru, threadID);
BuildJacobianMatrix (cluster, threadID, 0.0f);
IntegrateReactionsForces (cluster, threadID, 0.0f, DG_SOLVER_MAX_ERROR);
bool clusterReceding = true;
const dgFloat32 step = timestep * dgFloat32 (1.0f / DG_MAX_CONTINUE_COLLISON_STEPS);
for (dgInt32 k = 0; (k < DG_MAX_CONTINUE_COLLISON_STEPS) && clusterReceding; k ++) {
dgFloat32 smallTimeStep = dgMin (step, timeRemaining);
timeRemaining -= smallTimeStep;
for (dgInt32 j = 1; j < bodyCount; j ++) {
dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body;
if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
body->IntegrateVelocity (smallTimeStep);
body->UpdateWorlCollisionMatrix();
}
}
clusterReceding = false;
if (timeRemaining > timeTol) {
CalculateClusterContacts (cluster, timeRemaining, lru, threadID);
bool isColliding = false;
for (dgInt32 j = 0; (j < jointCount) && !isColliding; j ++) {
dgContact* const contact = (dgContact*) constraintArray[j].m_joint;
if (contact->GetId() == dgConstraint::m_contactConstraint) {
const dgBody* const body0 = contact->m_body0;
const dgBody* const body1 = contact->m_body1;
const dgVector& veloc0 = body0->m_veloc;
const dgVector& veloc1 = body1->m_veloc;
const dgVector& omega0 = body0->m_omega;
const dgVector& omega1 = body1->m_omega;
const dgVector& com0 = body0->m_globalCentreOfMass;
const dgVector& com1 = body1->m_globalCentreOfMass;
for (dgList<dgContactMaterial>::dgListNode* node = contact->GetFirst(); node; node = node->GetNext()) {
const dgContactMaterial* const contactMaterial = &node->GetInfo();
dgVector vel0 (veloc0 + omega0.CrossProduct3(contactMaterial->m_point - com0));
dgVector vel1 (veloc1 + omega1.CrossProduct3(contactMaterial->m_point - com1));
dgVector vRel (vel0 - vel1);
dgAssert (contactMaterial->m_normal.m_w == dgFloat32 (0.0f));
dgFloat32 speed = vRel.DotProduct4(contactMaterial->m_normal).m_w;
isColliding |= (speed < dgFloat32 (0.0f));
}
}
}
clusterReceding = !isColliding;
}
}
}
}
if (timeRemaining > dgFloat32 (0.0)) {
for (dgInt32 j = 1; j < bodyCount; j ++) {
dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body;
if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
body->IntegrateVelocity(timeRemaining);
body->UpdateCollisionMatrix (timeRemaining, threadID);
}
}
} else {
for (dgInt32 j = 1; j < bodyCount; j ++) {
dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body;
if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
body->UpdateCollisionMatrix (timestep, threadID);
}
}
}
}
}
}
void SimulationPostListener(DemoEntityManager* const scene, DemoEntityManager::dListNode* const mynode, dFloat timeStep)
{
// see if the net force on the body comes fr a high impact collision
dFloat breakImpact = 0.0f;
for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(m_myBody); joint; joint = NewtonBodyGetNextContactJoint(m_myBody, joint)) {
for (void* contact = NewtonContactJointGetFirstContact(joint); contact; contact = NewtonContactJointGetNextContact(joint, contact)) {
dVector contactForce;
NewtonMaterial* const material = NewtonContactGetMaterial(contact);
dFloat impulseImpact = NewtonMaterialGetContactMaxNormalImpact(material);
if (impulseImpact > breakImpact) {
breakImpact = impulseImpact;
}
}
}
// if the force is bigger than N time Gravities, It is considered a collision force
breakImpact *= m_myMassInverse;
// breakImpact = 1000.0f;
if (breakImpact > BREAK_IMPACT_IN_METERS_PER_SECONDS) {
NewtonWorld* const world = NewtonBodyGetWorld(m_myBody);
dMatrix bodyMatrix;
dVector com(0.0f);
dVector veloc(0.0f);
dVector omega(0.0f);
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetVelocity(m_myBody, &veloc[0]);
NewtonBodyGetOmega(m_myBody, &omega[0]);
NewtonBodyGetCentreOfMass(m_myBody, &com[0]);
NewtonBodyGetMatrix(m_myBody, &bodyMatrix[0][0]);
NewtonBodyGetMass(m_myBody, &mass, &Ixx, &Iyy, &Izz);
com = bodyMatrix.TransformVector(com);
dMatrix matrix(GetCurrentMatrix());
dQuaternion rotation(matrix);
// we need to lock the world before creation a bunch of bodies
scene->Lock(m_lock);
for (FractureEffect::dListNode* node = m_effect.GetFirst(); node; node = node->GetNext()) {
FractureAtom& atom = node->GetInfo();
DemoEntity* const entity = new DemoEntity(dMatrix(rotation, matrix.m_posit), NULL);
entity->SetMesh(atom.m_mesh, dGetIdentityMatrix());
scene->Append(entity);
int materialId = 0;
dFloat debriMass = mass * atom.m_massFraction;
//create the rigid body
NewtonBody* const rigidBody = NewtonCreateDynamicBody(world, atom.m_collision, &matrix[0][0]);
// calculate debris initial velocity
dVector center(matrix.TransformVector(atom.m_centerOfMass));
dVector v(veloc + omega.CrossProduct(center - com));
// set initial velocity
NewtonBodySetVelocity(rigidBody, &v[0]);
NewtonBodySetOmega(rigidBody, &omega[0]);
// set the debris mass properties, mass, center of mass, and inertia
NewtonBodySetMassProperties(rigidBody, debriMass, atom.m_collision);
// save the pointer to the graphic object with the body.
NewtonBodySetUserData(rigidBody, entity);
// assign the wood id
NewtonBodySetMaterialGroupID(rigidBody, materialId);
// set continuous collision mode
// NewtonBodySetContinuousCollisionMode (rigidBody, continueCollisionMode);
// set a destructor for this rigid body
NewtonBodySetDestructorCallback(rigidBody, PhysicsBodyDestructor);
// set the transform call back function
NewtonBodySetTransformCallback(rigidBody, DemoEntity::TransformCallback);
// set the force and torque call back function
NewtonBodySetForceAndTorqueCallback(rigidBody, PhysicsApplyGravityForce);
}
NewtonDestroyBody(m_myBody);
scene->RemoveEntity(mynode);
// unlock the work after done with the effect
scene->Unlock(m_lock);
}
}
void CalculatePickForceAndTorque (const NewtonBody* const body, const dVector& pointOnBodyInGlobalSpace, const dVector& targetPositionInGlobalSpace, dFloat timestep)
{
dVector com;
dMatrix matrix;
dVector omega0;
dVector veloc0;
dVector omega1;
dVector veloc1;
dVector pointVeloc;
const dFloat stiffness = 0.3f;
const dFloat angularDamp = 0.95f;
dFloat invTimeStep = 1.0f / timestep;
NewtonWorld* const world = NewtonBodyGetWorld (body);
NewtonWorldCriticalSectionLock (world, 0);
// calculate the desired impulse
NewtonBodyGetMatrix(body, &matrix[0][0]);
NewtonBodyGetOmega (body, &omega0[0]);
NewtonBodyGetVelocity (body, &veloc0[0]);
NewtonBodyGetPointVelocity (body, &pointOnBodyInGlobalSpace[0], &pointVeloc[0]);
dVector deltaVeloc (targetPositionInGlobalSpace - pointOnBodyInGlobalSpace);
deltaVeloc = deltaVeloc.Scale (stiffness * invTimeStep) - pointVeloc;
for (int i = 0; i < 3; i ++) {
dVector veloc (0.0f, 0.0f, 0.0f, 0.0f);
veloc[i] = deltaVeloc[i];
NewtonBodyAddImpulse (body, &veloc[0], &pointOnBodyInGlobalSpace[0]);
}
// damp angular velocity
NewtonBodyGetOmega (body, &omega1[0]);
NewtonBodyGetVelocity (body, &veloc1[0]);
omega1 = omega1.Scale (angularDamp);
// restore body velocity and angular velocity
NewtonBodySetOmega (body, &omega0[0]);
NewtonBodySetVelocity(body, &veloc0[0]);
// convert the delta velocity change to a external force and torque
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
dVector angularMomentum (Ixx, Iyy, Izz);
angularMomentum = matrix.RotateVector (angularMomentum.CompProduct(matrix.UnrotateVector(omega1 - omega0)));
dVector force ((veloc1 - veloc0).Scale (mass * invTimeStep));
dVector torque (angularMomentum.Scale(invTimeStep));
NewtonBodyAddForce(body, &force[0]);
NewtonBodyAddTorque(body, &torque[0]);
// make sure the body is unfrozen, if it is picked
NewtonBodySetSleepState (body, 0);
NewtonWorldCriticalSectionUnlock (world);
}
dVector dCustomPlayerController::GetVelocity() const
{
dVector veloc(0.0);
NewtonBodyGetVelocity(m_kinematicBody, &veloc[0]);
return veloc;
}
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]);
}
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 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]);
}
}
// create a mesh using the dNewtonMesh wrapper
static NewtonBody* CreateSimpledBox_dNetwonMesh(DemoEntityManager* const scene, const dVector& origin, const dVector& scale, dFloat mass)
{
dVector array[8];
dVector scale1(scale);
for (int i = 0; i < 8; i++) {
dVector p(&BoxPoints[i * 4]);
array[i] = scale1 * p;
}
dNewtonMesh buildMesh(scene->GetNewton());
// start build a mesh
buildMesh.BeginBuild();
// add faces one at a time
int index = 0;
const int faceCount = sizeof (faceIndexList)/sizeof (faceIndexList[0]);
for (int i = 0; i < faceCount; i ++) {
const int indexCount = faceIndexList[i];
const int faceMaterail = faceMateriaIndexList[i];
buildMesh.BeginPolygon();
for (int j = 0; j < indexCount; j ++) {
// add a mesh point
int k = BoxIndices[index + j];
buildMesh.AddPoint(array[k].m_x, array[k].m_y, array[k].m_z);
// add vertex normal
int n = faceNormalIndex[index + j];
buildMesh.AddNormal (dFloat32 (normal[n * 3 + 0]), dFloat32 (normal[n * 3 + 1]), dFloat32 (normal[n * 3 + 2]));
// add face material index
buildMesh.AddMaterial(faceMaterail);
// continue adding more vertex attributes of you want, uv, binormal, weights, etc
}
buildMesh.EndPolygon();
index += indexCount;
}
buildMesh.EndBuild();
// get the newtonMesh from the dNewtonMesh class and use it to build a render mesh, collision or anything
NewtonMesh* const newtonMesh = buildMesh.GetMesh();
// test collision tree
//NewtonCollision* const collisionTree = NewtonCreateTreeCollisionFromMesh (scene->GetNewton(), newtonMesh, 0);
//NewtonDestroyCollision(collisionTree);
// now we can use this mesh for lot of stuff, we can apply UV, we can decompose into convex,
NewtonCollision* const collision = NewtonCreateConvexHullFromMesh(scene->GetNewton(), newtonMesh, 0.001f, 0);
// for now we will simple make simple Box, make a visual Mesh
DemoMesh* const visualMesh = new DemoMesh(newtonMesh);
dMatrix matrix(dGetIdentityMatrix());
matrix.m_posit = origin;
matrix.m_posit.m_w = 1.0f;
NewtonBody* const body = CreateSimpleSolid(scene, visualMesh, mass, matrix, collision, 0);
dVector veloc (1, 0, 2, 0);
NewtonBodySetVelocity(body, &veloc[0]);
visualMesh->Release();
NewtonDestroyCollision(collision);
return body;
}
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 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);
*/
}
}
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy)
{
dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI));
dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));
dgAssert(this == proxy.m_instance1->GetChildShape());
dgAssert(m_count);
dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));
const dgBody* const body0 = proxy.m_body0;
const dgBody* const body1 = proxy.m_body1;
dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity());
dgVector relativeVelocity (body0->m_veloc - body1->m_veloc);
if (m_normal.DotProduct4(relativeVelocity).GetScalar() >= 0.0f) {
return 0;
}
dgFloat32 den = dgFloat32 (1.0f) / (relativeVelocity % m_normal);
if (den > dgFloat32 (1.0e-5f)) {
// this can actually happens
dgAssert(0);
return 0;
}
dgContact* const contactJoint = proxy.m_contactJoint;
contactJoint->m_closestDistance = dgFloat32(1.0e10f);
dgMatrix polygonMatrix;
dgVector right (m_localPoly[1] - m_localPoly[0]);
polygonMatrix[0] = right.CompProduct4(right.InvMagSqrt());
polygonMatrix[1] = m_normal;
polygonMatrix[2] = polygonMatrix[0] * m_normal;
polygonMatrix[3] = dgVector::m_wOne;
dgAssert (polygonMatrix.TestOrthogonal());
dgVector polyBoxP0(dgFloat32(1.0e15f));
dgVector polyBoxP1(dgFloat32(-1.0e15f));
for (dgInt32 i = 0; i < m_count; i++) {
dgVector point (polygonMatrix.UnrotateVector(m_localPoly[i]));
polyBoxP0 = polyBoxP0.GetMin(point);
polyBoxP1 = polyBoxP1.GetMax(point);
}
dgVector hullBoxP0;
dgVector hullBoxP1;
dgMatrix hullMatrix (polygonMatrix * proxy.m_instance0->m_globalMatrix);
proxy.m_instance0->CalcAABB(hullMatrix, hullBoxP0, hullBoxP1);
dgVector minBox(polyBoxP0 - hullBoxP1);
dgVector maxBox(polyBoxP1 - hullBoxP0);
dgVector veloc (polygonMatrix.UnrotateVector (relativeVelocity));
dgFastRayTest ray(dgVector(dgFloat32(0.0f)), veloc);
dgFloat32 distance = ray.BoxIntersect(minBox, maxBox);
dgInt32 count = 0;
if (distance < dgFloat32(1.0f)) {
bool inside = false;
dgVector boxSize((hullBoxP1 - hullBoxP0).CompProduct4(dgVector::m_half));
dgVector sphereMag2 (boxSize.DotProduct4(boxSize));
boxSize = sphereMag2.Sqrt();
dgVector pointInPlane (polygonMatrix.RotateVector(hullBoxP1 + hullBoxP0).CompProduct4(dgVector::m_half));
dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal;
dgFloat32 timeToPlane0 = (distToPlane + boxSize.GetScalar()) * den;
dgFloat32 timeToPlane1 = (distToPlane - boxSize.GetScalar()) * den;
dgVector boxOrigin0 (pointInPlane + relativeVelocity.Scale4(timeToPlane0));
dgVector boxOrigin1 (pointInPlane + relativeVelocity.Scale4(timeToPlane1));
dgVector boxOrigin ((boxOrigin0 + boxOrigin1).CompProduct4(dgVector::m_half));
dgVector boxProjectSize (((boxOrigin0 - boxOrigin1).CompProduct4(dgVector::m_half)));
sphereMag2 = boxProjectSize.DotProduct4(boxProjectSize);
boxSize = sphereMag2.Sqrt();
dgAssert (boxOrigin.m_w == 0.0f);
boxOrigin = boxOrigin | dgVector::m_wOne;
if (!proxy.m_intersectionTestOnly) {
inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i++) {
dgVector e(m_localPoly[i] - m_localPoly[i0]);
dgVector n(m_normal * e & dgVector::m_triplexMask);
dgFloat32 param = dgSqrt (sphereMag2.GetScalar() / (n.DotProduct4(n)).GetScalar());
dgPlane plane(n, -(m_localPoly[i0] % n));
dgVector p0 (boxOrigin + n.Scale4 (param));
dgVector p1 (boxOrigin - n.Scale4 (param));
dgFloat32 size0 = (plane.DotProduct4 (p0)).GetScalar();
dgFloat32 size1 = (plane.DotProduct4 (p1)).GetScalar();
if ((size0 < 0.0f) && (size1 < 0.0f)) {
return 0;
}
if ((size0 * size1) < 0.0f) {
inside = false;
break;
}
i0 = i;
}
}
dgFloat32 convexSphapeUmbra = dgMax (proxy.m_instance0->GetUmbraClipSize(), boxSize.GetScalar());
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping(boxOrigin, convexSphapeUmbra);
m_faceClipSize = proxy.m_instance0->m_childShape->GetBoxMaxRadius();
}
const dgInt32 hullId = proxy.m_instance0->GetUserDataID();
if (inside & !proxy.m_intersectionTestOnly) {
const dgMatrix& matrixInstance0 = proxy.m_instance0->m_globalMatrix;
dgVector normalInHull(matrixInstance0.UnrotateVector(m_normal.Scale4(dgFloat32(-1.0f))));
dgVector pointInHull(proxy.m_instance0->SupportVertex(normalInHull, NULL));
dgVector p0 (matrixInstance0.TransformVector(pointInHull));
dgFloat32 timetoImpact = dgFloat32(0.0f);
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32(0.0f)) {
timetoImpact = penetration / (relativeVelocity % m_normal);
dgAssert(timetoImpact >= dgFloat32(0.0f));
}
if (timetoImpact <= proxy.m_timestep) {
dgVector contactPoints[64];
contactJoint->m_closestDistance = penetration;
proxy.m_timestep = timetoImpact;
proxy.m_normal = m_normal;
proxy.m_closestPointBody0 = p0;
proxy.m_closestPointBody1 = p0 + m_normal.Scale4(penetration);
if (!proxy.m_intersectionTestOnly) {
pointInHull -= normalInHull.Scale4 (DG_ROBUST_PLANE_CLIP);
count = proxy.m_instance0->CalculatePlaneIntersection(normalInHull, pointInHull, contactPoints);
dgVector step(relativeVelocity.Scale4(timetoImpact));
penetration = dgMax(penetration, dgFloat32(0.0f));
dgContactPoint* const contactsOut = proxy.m_contacts;
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_point = matrixInstance0.TransformVector(contactPoints[i]) + step;
contactsOut[i].m_normal = m_normal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
}
}
} else {
m_vertexCount = dgUnsigned16 (m_count);
count = world->CalculateConvexToConvexContacts(proxy);
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
return count;
}
static void AddPathFollow (DemoEntityManager* const scene, const dVector& origin)
{
// create a Bezier Spline path for AI car to drive
DemoEntity* const rollerCosterPath = new DemoEntity(dGetIdentityMatrix(), NULL);
scene->Append(rollerCosterPath);
dBezierSpline spline;
dFloat64 knots[] = {0.0f, 1.0f / 5.0f, 2.0f / 5.0f, 3.0f / 5.0f, 4.0f / 5.0f, 1.0f};
dBigVector o (origin[0], origin[1], origin[2], 0.0f);
dBigVector control[] =
{
dBigVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 1.0f) + o,
dBigVector(150.0f - 100.0f, 10.0f, 150.0f - 250.0f, 1.0f) + o,
dBigVector(175.0f - 100.0f, 30.0f, 250.0f - 250.0f, 1.0f) + o,
dBigVector(200.0f - 100.0f, 70.0f, 250.0f - 250.0f, 1.0f) + o,
dBigVector(215.0f - 100.0f, 20.0f, 250.0f - 250.0f, 1.0f) + o,
dBigVector(150.0f - 100.0f, 50.0f, 350.0f - 250.0f, 1.0f) + o,
dBigVector( 50.0f - 100.0f, 30.0f, 250.0f - 250.0f, 1.0f) + o,
dBigVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 1.0f) + o,
};
spline.CreateFromKnotVectorAndControlPoints(3, sizeof (knots) / sizeof (knots[0]), knots, control);
DemoBezierCurve* const mesh = new DemoBezierCurve (spline);
rollerCosterPath->SetMesh(mesh, dGetIdentityMatrix());
mesh->SetVisible(true);
mesh->SetRenderResolution(500);
mesh->Release();
const int count = 32;
NewtonBody* bodies[count];
dBigVector point0;
dVector positions[count + 1];
dFloat64 knot = spline.FindClosestKnot(point0, origin + dVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 0.0f), 4);
positions[0] = dVector (point0.m_x, point0.m_y, point0.m_z, 0.0);
dFloat average = 0.0f;
for (int i = 0; i < count; i ++) {
dBigVector point1;
average += positions[i].m_y;
dBigVector tangent(spline.CurveDerivative(knot));
tangent = tangent.Scale (1.0 / sqrt (tangent % tangent));
knot = spline.FindClosestKnot(point1, dBigVector (point0 + tangent.Scale (2.0f)), 4);
point0 = point1;
positions[i + 1] = dVector (point0.m_x, point0.m_y, point0.m_z, 0.0);
}
average /= count;
for (int i = 0; i < count + 1; i ++) {
positions[i].m_y = average;
}
dFloat attachmentOffset = 0.8f;
for (int i = 0; i < count; i ++) {
dMatrix matrix;
dVector location0 (positions[i].m_x, positions[i].m_y, positions[i].m_z, 0.0);
bodies[i] = CreateWheel(scene, location0, 1.0f, 0.5f);
NewtonBodySetLinearDamping(bodies[i], 0.0f);
NewtonBody* const box = bodies[i];
NewtonBodyGetMatrix(box, &matrix[0][0]);
dVector location1 (positions[i + 1].m_x, positions[i + 1].m_y, positions[i + 1].m_z, 0.0);
dVector dir (location1 - location0);
matrix.m_front = dir.Scale (1.0f / dSqrt (dir % dir));
matrix.m_right = matrix.m_front * matrix.m_up;
dMatrix matrix1 (dYawMatrix(0.5f * 3.141692f) * matrix);
NewtonBodySetMatrix(box, &matrix1[0][0]);
matrix.m_posit.m_y += attachmentOffset;
new MyPathFollow(matrix, box, rollerCosterPath);
dVector veloc (dir.Scale (20.0f));
NewtonBodySetVelocity(box, &veloc[0]);
}
for (int i = 1; i < count; i ++) {
NewtonBody* const box0 = bodies[i - 1];
NewtonBody* const box1 = bodies[i];
dMatrix matrix0;
dMatrix matrix1;
NewtonBodyGetMatrix(box0, &matrix0[0][0]);
NewtonBodyGetMatrix(box1, &matrix1[0][0]);
matrix0.m_posit.m_y += attachmentOffset;
matrix1.m_posit.m_y += attachmentOffset;
new CustomDistanceRope (matrix1.m_posit, matrix0.m_posit, box1, box0);
}
void* const aggregate = NewtonCollisionAggregateCreate (scene->GetNewton());
for (int i = 0; i < count; i ++) {
NewtonCollisionAggregateAddBody(aggregate, bodies[i]);
}
NewtonCollisionAggregateSetSelfCollision (aggregate, false);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), bodies[0], NULL);
for (int i = 1; i < count; i++) {
NewtonSkeletonContainerAttachBone(skeleton, bodies[i], bodies[i - 1]);
}
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
