void Physics::Update(double delta_time)
{
last_physics_update += delta_time;
for(std::list<RigidBody*>::iterator it = register_queue.begin(); it != register_queue.end();)
physics_objects.push_back(*(it++));
register_queue.clear();
for(std::list<RigidBody*>::iterator it = unregister_queue.begin(); it != unregister_queue.end();)
physics_objects.erase(std::remove(physics_objects.begin(), physics_objects.end(), *(it++)), physics_objects.end());
unregister_queue.clear();
if(last_physics_update < update_time)
return;
for(unsigned int i = 0; i < physics_objects.size(); i++ )
for(unsigned int j = i+1; j < physics_objects.size(); j++ )
{
if(IntersectAABB(&physics_objects[i]->aabb, &physics_objects[j]->aabb))
{
vec3 contact_normal;
vec3 contact_point;
if(IntersectConvexShape(physics_objects[i]->shape, physics_objects[i]->model, physics_objects[j]->shape, physics_objects[j]->model, contact_point, contact_normal))
{
if(physics_objects[i]->state == PO_STATIC && physics_objects[j]->state != PO_STATIC)
physics_objects[j]->model.position -= contact_normal;
else if(physics_objects[j]->state == PO_STATIC && physics_objects[i]->state != PO_STATIC)
physics_objects[i]->model.position += contact_normal;
else if(physics_objects[j]->state != PO_STATIC && physics_objects[i]->state != PO_STATIC)
{
if(abs(physics_objects[i]->anti_mass + physics_objects[j]->anti_mass) < math_epsilon)
{
physics_objects[j]->model.position -= contact_normal/2.0f;
physics_objects[i]->model.position += contact_normal/2.0f;
}
else
{
float summ_antimass = (float)(physics_objects[j]->anti_mass + physics_objects[i]->anti_mass);
physics_objects[j]->model.position -= contact_normal * (float)(physics_objects[j]->anti_mass / summ_antimass);
physics_objects[i]->model.position += contact_normal * (float)(physics_objects[i]->anti_mass / summ_antimass);
}
}
double impulse = CalculateImpulse(contact_normal, physics_objects[j], physics_objects[i], contact_point);
if(physics_objects[i]->state != PO_STATIC)
physics_objects[i]->ApplyImpulse(contact_point, contact_normal, -impulse);
if(physics_objects[j]->state != PO_STATIC)
physics_objects[j]->ApplyImpulse(contact_point, contact_normal, impulse);
physics_objects[i]->OnCollide(physics_objects[j]);
physics_objects[j]->OnCollide(physics_objects[i]);
}
}
}
for(unsigned int i = 0; i < physics_objects.size(); i++)
physics_objects[i]->Tick(last_physics_update);
last_physics_update = 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::ResolveCollision()
{
dMatrix matrix;
NewtonWorldConvexCastReturnInfo info[D_MAX_ROWS];
NewtonWorld* const world = m_manager->GetWorld();
NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
NewtonCollision* const shape = NewtonBodyGetCollision(m_kinematicBody);
int contactCount = NewtonWorldCollide(world, &matrix[0][0], shape, this, PrefilterCallback, info, 4, 0);
if (!contactCount) {
return;
}
dFloat maxPenetration = 0.0f;
for (int i = 0; i < contactCount; i ++) {
maxPenetration = dMax (info[i].m_penetration, maxPenetration);
}
if (maxPenetration > D_MAX_COLLISION_PENETRATION) {
ResolveInterpenetrations(contactCount, info);
NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
}
int rowCount = 0;
dVector zero(0.0f);
dMatrix invInertia;
dVector com(0.0f);
dVector veloc(0.0f);
dComplementaritySolver::dJacobian jt[D_MAX_ROWS];
dFloat rhs[D_MAX_ROWS];
dFloat low[D_MAX_ROWS];
dFloat high[D_MAX_ROWS];
dFloat impulseMag[D_MAX_ROWS];
int normalIndex[D_MAX_ROWS];
NewtonBodyGetVelocity(m_kinematicBody, &veloc[0]);
NewtonBodyGetCentreOfMass(m_kinematicBody, &com[0]);
NewtonBodyGetInvInertiaMatrix(m_kinematicBody, &invInertia[0][0]);
// const dMatrix localFrame (dPitchMatrix(m_headingAngle) * m_localFrame * matrix);
const dMatrix localFrame (m_localFrame * matrix);
com = matrix.TransformVector(com);
com.m_w = 0.0f;
for (int i = 0; i < contactCount; i ++) {
NewtonWorldConvexCastReturnInfo& contact = info[i];
dVector point (contact.m_point[0], contact.m_point[1], contact.m_point[2], 0.0f);
dVector normal (contact.m_normal[0], contact.m_normal[1], contact.m_normal[2], 0.0f);
jt[rowCount].m_linear = normal;
jt[rowCount].m_angular = (point - com).CrossProduct(normal);
low[rowCount] = 0.0f;
high[rowCount] = 1.0e12f;
normalIndex[rowCount] = 0;
dVector tmp (veloc * jt[rowCount].m_linear.Scale (1.001f));
rhs[rowCount] = - (tmp.m_x + tmp.m_y + tmp.m_z);
rowCount ++;
dAssert (rowCount < (D_MAX_ROWS - 3));
//dFloat updir = localFrame.m_front.DotProduct3(normal);
dFloat friction = m_manager->ContactFriction(this, point, normal, contact.m_hitBody);
if (friction > 0.0f)
{
// add lateral traction friction
dVector sideDir (localFrame.m_up.CrossProduct(normal).Normalize());
jt[rowCount].m_linear = sideDir;
jt[rowCount].m_angular = (point - com).CrossProduct(sideDir);
low[rowCount] = -friction;
high[rowCount] = friction;
normalIndex[rowCount] = -1;
dVector tmp1 (veloc * jt[rowCount].m_linear);
rhs[rowCount] = -m_lateralSpeed - (tmp1.m_x + tmp1.m_y + tmp1.m_z);
rowCount++;
dAssert (rowCount < (D_MAX_ROWS - 3));
// add longitudinal traction friction
dVector frontDir (normal.CrossProduct(sideDir));
jt[rowCount].m_linear = frontDir;
jt[rowCount].m_angular = (point - com).CrossProduct(frontDir);
low[rowCount] = -friction;
high[rowCount] = friction;
normalIndex[rowCount] = -2;
dVector tmp2 (veloc * jt[rowCount].m_linear);
rhs[rowCount] = -m_forwardSpeed - (tmp2.m_x + tmp2.m_y + tmp2.m_z);
rowCount++;
dAssert(rowCount < (D_MAX_ROWS - 3));
}
}
for (int i = 0; i < 3; i++) {
jt[rowCount].m_linear = zero;
jt[rowCount].m_angular = zero;
jt[rowCount].m_angular[i] = dFloat(1.0f);
rhs[rowCount] = 0.0f;
impulseMag[rowCount] = 0;
low[rowCount] = -1.0e12f;
high[rowCount] = 1.0e12f;
normalIndex[rowCount] = 0;
rowCount ++;
dAssert (rowCount < D_MAX_ROWS);
}
dVector impulse (veloc.Scale (m_mass) + CalculateImpulse(rowCount, rhs, low, high, normalIndex, jt));
veloc = impulse.Scale(m_invMass);
NewtonBodySetVelocity(m_kinematicBody, &veloc[0]);
}
void 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]);
}
}
