Code:

void RenderScene::ProcessContact(Contact* contact, float& Pn, Vector3&vel1, Vector3& ang_vel1, Vector3&vel2, Vector3& ang_vel2){const float k_allowedPenetration = 0.01f;float BAUMGARTE = 0.2f;//BAUMGARTEfloat bias = -BAUMGARTE * 1.f/this->physics_timestep * std::min(0.0f, -contact->penetration + k_allowedPenetration);RigidBody* ent1 = contact->one;RigidBody* ent2 = contact->two;const float e = std::min(ent1->restitution, ent2->restitution);Vector3 contactPoint = contact->position;Vector3 contactNormal = contact->normal;Vector3 rA = contactPoint - ent1->position;Vector3 rB = contactPoint - ent2->position;Vector3 kA = Vector3::Cross(rA, contactNormal);Vector3 kB = Vector3::Cross(rB, contactNormal);Vector3 uA = ent1->inverse_inertia_tensor_world_space*kA;Vector3 uB = ent2->inverse_inertia_tensor_world_space*kB;float normalMass = ent1->inverse_mass + ent2->inverse_mass;if (!ent1->is_kinematic){normalMass += Vector3::Dot(kA, uA);}if (!ent2->is_kinematic){normalMass += Vector3::Dot(kB, uB);}// Relative velocity at contactVector3 relativeVel = (ent2->velocity + Vector3::Cross(ent2->angular_velocity, rB)) - (ent1->velocity + Vector3::Cross(ent1->angular_velocity, rA));float numer = -(1 + e)*Vector3::Dot(relativeVel, contactNormal) + bias;float f = numer / normalMass;float Pn0 = contact->accumulated_impulse;contact->accumulated_impulse = std::fmax(Pn0 + f, 0.0f);f = contact->accumulated_impulse - Pn0;Vector3 impulse = f*contactNormal;contact->one->velocity -= impulse*ent1->inverse_mass;contact->two->velocity += impulse*ent2->inverse_mass;contact->one->angular_velocity -= f*uA;contact->two->angular_velocity += f*uB;}

Code:

std::vector<Contact> contacts;this->ClipFaceToSidePlane(incident_face, -1 * normal1, neg_offset1, contacts);this->ClipFaceToSidePlane(incident_face, normal1, pos_offset1, contacts);this->ClipFaceToSidePlane(incident_face, -1 * normal2, neg_offset2, contacts);this->ClipFaceToSidePlane(incident_face, normal2, pos_offset2, contacts);

Code:

void RenderScene::ClipFaceToSidePlane(const Vector3* face, const Vector3& normal, float plane_offset, std::vector<Contact> &contacts){Vector3 Vertex1 = face[3]; //start edge forms from endvertex->startvertexfloat Distance1 = Vector3::Dot(Vertex1, normal) - plane_offset;for (int i = 0; i < 4; i++){Vector3 Vertex2 = face[i];float Distance2 = Vector3::Dot(Vertex2, normal) - plane_offset;if (Distance1 <= 0.0f && Distance2 <= 0.0f){// Both vertices are behind the plane - keep vertex2Contact contact1;contact1.position = Vertex2;contacts.push_back(contact1);}else if (Distance1 <= 0.0f && Distance2 > 0.0f){// Vertex1 is behind the plane, vertex2 is in front -> intersection pointfloat Fraction = Distance1 / (Distance1 - Distance2);Vector3 Position = Vertex1 + Fraction * (Vertex2 - Vertex1);// Keep intersection pointContact contact1;contact1.position = Position;contacts.push_back(contact1);}else if (Distance2 <= 0.0f && Distance1 > 0){// Vertex2 is behind the plane, vertex1 is in front -> intersection pointfloat Fraction = Distance1 / (Distance1 - Distance2);Vector3 Position = Vertex1 + Fraction * (Vertex2 - Vertex1);// Keep intersection point Contact contact1;contact1.position = Position;contacts.push_back(contact1);// And also keep vertex2Contact contact2;contact2.position = Vertex2;contacts.push_back(contact2);}// Keep vertex2 as starting vertex for next edgeVertex1 = Vertex2;Distance1 = Distance2;}}

Code:

void RenderScene::CreateReferenceSidePlanes(RigidBody* reference_body, int &axis_number, Vector3& normal1, Vector3& normal2, float& neg_offset1, float& pos_offset1, float& neg_offset2, float& pos_offset2){switch (axis_number){case 0: //Reference face is on the x axis so we create side planes for the other axes{normal1 = reference_body->obb->rot[1];normal2 = reference_body->obb->rot[2];float y = Vector3::Dot(reference_body->obb->pos, normal1);float z = Vector3::Dot(reference_body->obb->pos, normal2);neg_offset1 = -y + reference_body->obb->half_extent[1];pos_offset1 = y + reference_body->obb->half_extent[1];neg_offset2 = -z + reference_body->obb->half_extent[2];pos_offset2 = z + reference_body->obb->half_extent[2];this->DrawSidePlanes(normal1, normal2, reference_body->obb->pos, 1, 2, reference_body->obb->half_extent);}break;case 1: //Reference face is on the y axis so we create side planes for the other axes{normal1 = reference_body->obb->rot[0];normal2 = reference_body->obb->rot[2];float x = Vector3::Dot(reference_body->obb->pos, normal1);float z = Vector3::Dot(reference_body->obb->pos, normal2);neg_offset1 = -x + reference_body->obb->half_extent[0];pos_offset1 = x + reference_body->obb->half_extent[0];neg_offset2 = -z + reference_body->obb->half_extent[2];pos_offset2 = z + reference_body->obb->half_extent[2];this->DrawSidePlanes(normal1, normal2, reference_body->obb->pos, 0, 2, reference_body->obb->half_extent);}break;case 2: //Reference face is on the z axis so we create side planes for the other axes{normal1 = reference_body->obb->rot[0];normal2 = reference_body->obb->rot[1];float x = Vector3::Dot(reference_body->obb->pos, normal1);float y = Vector3::Dot(reference_body->obb->pos, normal2);neg_offset1 = -x + reference_body->obb->half_extent[0];pos_offset1 = x + reference_body->obb->half_extent[0];neg_offset2 = -y + reference_body->obb->half_extent[1];pos_offset2 = y + reference_body->obb->half_extent[1];this->DrawSidePlanes(normal1, normal2, reference_body->obb->pos, 0, 1, reference_body->obb->half_extent);}break;}}