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]); } }