481 lines
20 KiB
C++
481 lines
20 KiB
C++
/********************************************************************************
|
|
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
|
|
* Copyright (c) 2010-2022 Daniel Chappuis *
|
|
*********************************************************************************
|
|
* *
|
|
* This software is provided 'as-is', without any express or implied warranty. *
|
|
* In no event will the authors be held liable for any damages arising from the *
|
|
* use of this software. *
|
|
* *
|
|
* Permission is granted to anyone to use this software for any purpose, *
|
|
* including commercial applications, and to alter it and redistribute it *
|
|
* freely, subject to the following restrictions: *
|
|
* *
|
|
* 1. The origin of this software must not be misrepresented; you must not claim *
|
|
* that you wrote the original software. If you use this software in a *
|
|
* product, an acknowledgment in the product documentation would be *
|
|
* appreciated but is not required. *
|
|
* *
|
|
* 2. Altered source versions must be plainly marked as such, and must not be *
|
|
* misrepresented as being the original software. *
|
|
* *
|
|
* 3. This notice may not be removed or altered from any source distribution. *
|
|
* *
|
|
********************************************************************************/
|
|
|
|
// Libraries
|
|
#include <reactphysics3d/utils/DebugRenderer.h>
|
|
#include <reactphysics3d/memory/MemoryManager.h>
|
|
#include <cassert>
|
|
#include <reactphysics3d/collision/shapes/ConvexMeshShape.h>
|
|
#include <reactphysics3d/collision/shapes/ConcaveMeshShape.h>
|
|
#include <reactphysics3d/collision/shapes/HeightFieldShape.h>
|
|
#include <reactphysics3d/collision/shapes/BoxShape.h>
|
|
#include <reactphysics3d/collision/shapes/SphereShape.h>
|
|
#include <reactphysics3d/collision/shapes/CapsuleShape.h>
|
|
#include <reactphysics3d/collision/Collider.h>
|
|
#include <reactphysics3d/engine/PhysicsWorld.h>
|
|
#include <reactphysics3d/containers/Pair.h>
|
|
|
|
using namespace reactphysics3d;
|
|
|
|
// Constructor
|
|
DebugRenderer::DebugRenderer(MemoryAllocator& allocator)
|
|
:mAllocator(allocator), mLines(allocator), mTriangles(allocator), mDisplayedDebugItems(0), mMapDebugItemWithColor(allocator),
|
|
mContactPointSphereRadius(DEFAULT_CONTACT_POINT_SPHERE_RADIUS), mContactNormalLength(DEFAULT_CONTACT_NORMAL_LENGTH) {
|
|
|
|
mMapDebugItemWithColor.add(Pair<DebugItem, uint32>(DebugItem::COLLIDER_AABB, static_cast<uint32>(DebugColor::MAGENTA)));
|
|
mMapDebugItemWithColor.add(Pair<DebugItem, uint32>(DebugItem::COLLIDER_BROADPHASE_AABB, static_cast<uint32>(DebugColor::YELLOW)));
|
|
mMapDebugItemWithColor.add(Pair<DebugItem, uint32>(DebugItem::COLLISION_SHAPE, static_cast<uint32>(DebugColor::GREEN)));
|
|
mMapDebugItemWithColor.add(Pair<DebugItem, uint32>(DebugItem::CONTACT_POINT, static_cast<uint32>(DebugColor::RED)));
|
|
mMapDebugItemWithColor.add(Pair<DebugItem, uint32>(DebugItem::CONTACT_NORMAL, static_cast<uint32>(DebugColor::WHITE)));
|
|
}
|
|
|
|
// Destructor
|
|
DebugRenderer::~DebugRenderer() {
|
|
|
|
}
|
|
|
|
// Clear all the debugging primitives (points, lines, triangles, ...)
|
|
void DebugRenderer::reset() {
|
|
|
|
mLines.clear();
|
|
mTriangles.clear();
|
|
}
|
|
|
|
// Draw an AABB
|
|
void DebugRenderer::drawAABB(const AABB& aabb, uint32 color) {
|
|
|
|
const Vector3& min = aabb.getMin();
|
|
const Vector3& max = aabb.getMax();
|
|
|
|
// Bottom edges
|
|
mLines.add(DebugLine(Vector3(min.x, min.y, max.z), Vector3(max.x, min.y, max.z), color));
|
|
mLines.add(DebugLine(Vector3(max.x, min.y, max.z), Vector3(max.x, min.y, min.z), color));
|
|
mLines.add(DebugLine(Vector3(max.x, min.y, min.z), Vector3(min.x, min.y, min.z), color));
|
|
mLines.add(DebugLine(Vector3(min.x, min.y, min.z), Vector3(min.x, min.y, max.z), color));
|
|
|
|
// Top edges
|
|
mLines.add(DebugLine(Vector3(min.x, max.y, max.z), Vector3(max.x, max.y, max.z), color));
|
|
mLines.add(DebugLine(Vector3(max.x, max.y, max.z), Vector3(max.x, max.y, min.z), color));
|
|
mLines.add(DebugLine(Vector3(max.x, max.y, min.z), Vector3(min.x, max.y, min.z), color));
|
|
mLines.add(DebugLine(Vector3(min.x, max.y, min.z), Vector3(min.x, max.y, max.z), color));
|
|
|
|
// Side edges
|
|
mLines.add(DebugLine(Vector3(min.x, min.y, max.z), Vector3(min.x, max.y, max.z), color));
|
|
mLines.add(DebugLine(Vector3(max.x, min.y, max.z), Vector3(max.x, max.y, max.z), color));
|
|
mLines.add(DebugLine(Vector3(max.x, min.y, min.z), Vector3(max.x, max.y, min.z), color));
|
|
mLines.add(DebugLine(Vector3(min.x, min.y, min.z), Vector3(min.x, max.y, min.z), color));
|
|
}
|
|
|
|
// Draw a box
|
|
void DebugRenderer::drawBox(const Transform& transform, const Vector3& halfExtents, uint32 color) {
|
|
|
|
Vector3 vertices[8];
|
|
|
|
// Vertices
|
|
vertices[0] = transform * Vector3(-halfExtents.x, -halfExtents.y, halfExtents.z);
|
|
vertices[1] = transform * Vector3(halfExtents.x, -halfExtents.y, halfExtents.z);
|
|
vertices[2] = transform * Vector3(halfExtents.x, -halfExtents.y, -halfExtents.z);
|
|
vertices[3] = transform * Vector3(-halfExtents.x, -halfExtents.y, -halfExtents.z);
|
|
vertices[4] = transform * Vector3(-halfExtents.x, halfExtents.y, halfExtents.z);
|
|
vertices[5] = transform * Vector3(halfExtents.x, halfExtents.y, halfExtents.z);
|
|
vertices[6] = transform * Vector3(halfExtents.x, halfExtents.y, -halfExtents.z);
|
|
vertices[7] = transform * Vector3(-halfExtents.x, halfExtents.y, -halfExtents.z);
|
|
|
|
// Triangle faces
|
|
mTriangles.add(DebugTriangle(vertices[0], vertices[1], vertices[5], color));
|
|
mTriangles.add(DebugTriangle(vertices[0], vertices[5], vertices[4], color));
|
|
mTriangles.add(DebugTriangle(vertices[1], vertices[2], vertices[6], color));
|
|
mTriangles.add(DebugTriangle(vertices[1], vertices[6], vertices[5], color));
|
|
mTriangles.add(DebugTriangle(vertices[2], vertices[3], vertices[6], color));
|
|
mTriangles.add(DebugTriangle(vertices[3], vertices[7], vertices[6], color));
|
|
mTriangles.add(DebugTriangle(vertices[0], vertices[7], vertices[3], color));
|
|
mTriangles.add(DebugTriangle(vertices[0], vertices[4], vertices[7], color));
|
|
mTriangles.add(DebugTriangle(vertices[0], vertices[2], vertices[1], color));
|
|
mTriangles.add(DebugTriangle(vertices[0], vertices[3], vertices[2], color));
|
|
mTriangles.add(DebugTriangle(vertices[5], vertices[6], vertices[4], color));
|
|
mTriangles.add(DebugTriangle(vertices[4], vertices[6], vertices[7], color));
|
|
}
|
|
|
|
/// Draw a sphere
|
|
void DebugRenderer::drawSphere(const Vector3& position, decimal radius, uint32 color) {
|
|
|
|
Vector3 vertices[(NB_SECTORS_SPHERE + 1) * (NB_STACKS_SPHERE + 1) + (NB_SECTORS_SPHERE + 1)];
|
|
|
|
// Vertices
|
|
const decimal sectorStep = 2 * PI_RP3D / NB_SECTORS_SPHERE;
|
|
const decimal stackStep = PI_RP3D / NB_STACKS_SPHERE;
|
|
|
|
for (uint32 i = 0; i <= NB_STACKS_SPHERE; i++) {
|
|
|
|
const decimal stackAngle = PI_RP3D / 2 - i * stackStep;
|
|
const decimal radiusCosStackAngle = radius * std::cos(stackAngle);
|
|
const decimal z = radius * std::sin(stackAngle);
|
|
|
|
for (uint32 j = 0; j <= NB_SECTORS_SPHERE; j++) {
|
|
|
|
const decimal sectorAngle = j * sectorStep;
|
|
const decimal x = radiusCosStackAngle * std::cos(sectorAngle);
|
|
const decimal y = radiusCosStackAngle * std::sin(sectorAngle);
|
|
|
|
vertices[i * (NB_SECTORS_SPHERE + 1) + j] = position + Vector3(x, y, z);
|
|
}
|
|
}
|
|
|
|
// Faces
|
|
for (uint32 i = 0; i < NB_STACKS_SPHERE; i++) {
|
|
|
|
uint32 a1 = i * (NB_SECTORS_SPHERE + 1);
|
|
uint32 a2 = a1 + NB_SECTORS_SPHERE + 1;
|
|
|
|
for (uint32 j = 0; j < NB_SECTORS_SPHERE; j++, a1++, a2++) {
|
|
|
|
// 2 triangles per sector except for the first and last stacks
|
|
|
|
if (i != 0) {
|
|
|
|
mTriangles.add(DebugTriangle(vertices[a1], vertices[a2], vertices[a1 + 1], color));
|
|
}
|
|
|
|
if (i != (NB_STACKS_SPHERE - 1)) {
|
|
|
|
mTriangles.add(DebugTriangle(vertices[a1 + 1], vertices[a2], vertices[a2 + 1], color));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Draw a capsule
|
|
void DebugRenderer::drawCapsule(const Transform& transform, decimal radius, decimal height, uint32 color) {
|
|
|
|
Vector3 vertices[(NB_SECTORS_SPHERE + 1) * (NB_STACKS_SPHERE + 1) + (NB_SECTORS_SPHERE + 1)];
|
|
|
|
const decimal halfHeight = decimal(0.5) * height;
|
|
|
|
// Use an even number of stacks
|
|
const uint32 nbStacks = NB_STACKS_SPHERE % 2 == 0 ? NB_STACKS_SPHERE : NB_STACKS_SPHERE - 1;
|
|
const uint32 nbHalfStacks = nbStacks / 2;
|
|
|
|
// Vertices
|
|
const decimal sectorStep = 2 * PI_RP3D / NB_SECTORS_SPHERE;
|
|
const decimal stackStep = PI_RP3D / nbStacks;
|
|
|
|
uint32 vertexIndex = 0;
|
|
|
|
// Top cap sphere vertices
|
|
for (uint32 i = 0; i <= nbHalfStacks; i++) {
|
|
|
|
const decimal stackAngle = PI_RP3D / 2 - i * stackStep;
|
|
const decimal radiusCosStackAngle = radius * std::cos(stackAngle);
|
|
const decimal y = radius * std::sin(stackAngle);
|
|
|
|
for (uint32 j = 0; j <= NB_SECTORS_SPHERE; j++) {
|
|
|
|
const decimal sectorAngle = j * sectorStep;
|
|
const decimal x = radiusCosStackAngle * std::sin(sectorAngle);
|
|
const decimal z = radiusCosStackAngle * std::cos(sectorAngle);
|
|
|
|
assert(vertexIndex < (NB_SECTORS_SPHERE + 1) * (nbStacks + 1) + (NB_SECTORS_SPHERE + 1));
|
|
vertices[vertexIndex] = transform * Vector3(x, y + halfHeight, z);
|
|
|
|
vertexIndex++;
|
|
}
|
|
}
|
|
|
|
// Bottom cap sphere vertices
|
|
for (uint32 i = 0; i <= nbHalfStacks; i++) {
|
|
|
|
const decimal stackAngle = PI_RP3D / 2 - (nbHalfStacks + i) * stackStep;
|
|
const decimal radiusCosStackAngle = radius * std::cos(stackAngle);
|
|
const decimal y = radius * std::sin(stackAngle);
|
|
|
|
for (uint32 j = 0; j <= NB_SECTORS_SPHERE; j++) {
|
|
|
|
const decimal sectorAngle = j * sectorStep;
|
|
const decimal x = radiusCosStackAngle * std::sin(sectorAngle);
|
|
const decimal z = radiusCosStackAngle * std::cos(sectorAngle);
|
|
|
|
assert(vertexIndex < (NB_SECTORS_SPHERE + 1) * (nbStacks + 1) + (NB_SECTORS_SPHERE + 1));
|
|
vertices[vertexIndex] = transform * Vector3(x, y - halfHeight, z);
|
|
|
|
vertexIndex++;
|
|
}
|
|
}
|
|
|
|
// Faces of the top cap sphere
|
|
for (uint32 i = 0; i < nbHalfStacks; i++) {
|
|
|
|
uint32 a1 = i * (NB_SECTORS_SPHERE + 1);
|
|
uint32 a2 = a1 + NB_SECTORS_SPHERE + 1;
|
|
|
|
for (uint32 j = 0; j < NB_SECTORS_SPHERE; j++, a1++, a2++) {
|
|
|
|
// 2 triangles per sector except for the first stack
|
|
|
|
if (i != 0) {
|
|
|
|
mTriangles.add(DebugTriangle(vertices[a1], vertices[a2], vertices[a1 + 1], color));
|
|
}
|
|
|
|
mTriangles.add(DebugTriangle(vertices[a1 + 1], vertices[a2], vertices[a2 + 1], color));
|
|
}
|
|
}
|
|
|
|
// Faces of the bottom cap sphere
|
|
for (uint32 i = 0; i < nbHalfStacks; i++) {
|
|
|
|
uint32 a1 = (nbHalfStacks + 1) * (NB_SECTORS_SPHERE + 1) + i * (NB_SECTORS_SPHERE + 1);
|
|
uint32 a2 = a1 + NB_SECTORS_SPHERE + 1;
|
|
|
|
for (uint32 j = 0; j < NB_SECTORS_SPHERE; j++, a1++, a2++) {
|
|
|
|
// 2 triangles per sector except for the last stack
|
|
|
|
mTriangles.add(DebugTriangle(vertices[a1], vertices[a2], vertices[a1 + 1], color));
|
|
|
|
if (i != (nbHalfStacks - 1)) {
|
|
|
|
mTriangles.add(DebugTriangle(vertices[a1 + 1], vertices[a2], vertices[a2 + 1], color));
|
|
}
|
|
}
|
|
}
|
|
|
|
// Faces of the cylinder between the two spheres
|
|
uint32 a1 = nbHalfStacks * (NB_SECTORS_SPHERE + 1);
|
|
uint32 a2 = a1 + NB_SECTORS_SPHERE + 1;
|
|
for (uint32 i = 0; i < NB_SECTORS_SPHERE; i++, a1++, a2++) {
|
|
|
|
mTriangles.add(DebugTriangle(vertices[a1 + 1], vertices[a2], vertices[a2 + 1], color));
|
|
mTriangles.add(DebugTriangle(vertices[a1], vertices[a2], vertices[a1 + 1], color));
|
|
}
|
|
}
|
|
|
|
// Draw a convex mesh
|
|
void DebugRenderer::drawConvexMesh(const Transform& transform, const ConvexMeshShape* convexMesh, uint32 color) {
|
|
|
|
// For each face of the convex mesh
|
|
for (uint32 f = 0; f < convexMesh->getNbFaces(); f++) {
|
|
|
|
const HalfEdgeStructure::Face& face = convexMesh->getFace(f);
|
|
assert(face.faceVertices.size() >= 3);
|
|
|
|
// Perform a fan triangulation of the convex polygon face
|
|
const uint32 nbFaceVertices = static_cast<uint32>(face.faceVertices.size());
|
|
for (uint32 v = 2; v < nbFaceVertices; v++) {
|
|
|
|
uint32 v1Index = face.faceVertices[v - 2];
|
|
uint32 v2Index = face.faceVertices[v - 1];
|
|
uint32 v3Index = face.faceVertices[v];
|
|
|
|
Vector3 v1 = convexMesh->getVertexPosition(v1Index);
|
|
Vector3 v2 = convexMesh->getVertexPosition(v2Index);
|
|
Vector3 v3 = convexMesh->getVertexPosition(v3Index);
|
|
|
|
v1 = transform * v1;
|
|
v2 = transform * v2;
|
|
v3 = transform * v3;
|
|
|
|
mTriangles.add(DebugTriangle(v1, v2, v3, color));
|
|
}
|
|
}
|
|
}
|
|
|
|
// Draw a concave mesh shape
|
|
void DebugRenderer::drawConcaveMeshShape(const Transform& transform, const ConcaveMeshShape* concaveMeshShape, uint32 color) {
|
|
|
|
// For each sub-part of the mesh
|
|
for (uint32 p = 0; p < concaveMeshShape->getNbSubparts(); p++) {
|
|
|
|
// For each triangle of the sub-part
|
|
for (uint32 t = 0; t < concaveMeshShape->getNbTriangles(p); t++) {
|
|
|
|
Vector3 triangleVertices[3];
|
|
concaveMeshShape->getTriangleVertices(p, t, triangleVertices);
|
|
|
|
triangleVertices[0] = transform * triangleVertices[0];
|
|
triangleVertices[1] = transform * triangleVertices[1];
|
|
triangleVertices[2] = transform * triangleVertices[2];
|
|
|
|
mTriangles.add(DebugTriangle(triangleVertices[0], triangleVertices[1], triangleVertices[2], color));
|
|
}
|
|
}
|
|
}
|
|
|
|
// Draw a height field shape
|
|
void DebugRenderer::drawHeightFieldShape(const Transform& transform, const HeightFieldShape* heightFieldShape, uint32 color) {
|
|
|
|
// For each sub-grid points (except the last ones one each dimension)
|
|
for (int i = 0; i < heightFieldShape->getNbColumns() - 1; i++) {
|
|
for (int j = 0; j < heightFieldShape->getNbRows() - 1; j++) {
|
|
|
|
// Compute the four point of the current quad
|
|
Vector3 p1 = heightFieldShape->getVertexAt(i, j);
|
|
Vector3 p2 = heightFieldShape->getVertexAt(i, j + 1);
|
|
Vector3 p3 = heightFieldShape->getVertexAt(i + 1, j);
|
|
Vector3 p4 = heightFieldShape->getVertexAt(i + 1, j + 1);
|
|
|
|
p1 = transform * p1;
|
|
p2 = transform * p2;
|
|
p3 = transform * p3;
|
|
p4 = transform * p4;
|
|
|
|
mTriangles.add(DebugTriangle(p1, p2, p3, color));
|
|
mTriangles.add(DebugTriangle(p3, p2, p4, color));
|
|
}
|
|
}
|
|
}
|
|
|
|
// Draw the collision shape of a collider
|
|
void DebugRenderer::drawCollisionShapeOfCollider(const Collider* collider, uint32 color) {
|
|
|
|
switch (collider->getCollisionShape()->getName()) {
|
|
|
|
case CollisionShapeName::BOX:
|
|
{
|
|
const BoxShape* boxShape = static_cast<const BoxShape*>(collider->getCollisionShape());
|
|
drawBox(collider->getLocalToWorldTransform(), boxShape->getHalfExtents(), color);
|
|
break;
|
|
}
|
|
case CollisionShapeName::SPHERE:
|
|
{
|
|
const SphereShape* sphereShape = static_cast<const SphereShape*>(collider->getCollisionShape());
|
|
drawSphere(collider->getLocalToWorldTransform().getPosition(), sphereShape->getRadius(), color);
|
|
break;
|
|
}
|
|
case CollisionShapeName::CAPSULE:
|
|
{
|
|
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(collider->getCollisionShape());
|
|
drawCapsule(collider->getLocalToWorldTransform(), capsuleShape->getRadius(), capsuleShape->getHeight(), color);
|
|
break;
|
|
}
|
|
case CollisionShapeName::CONVEX_MESH:
|
|
{
|
|
const ConvexMeshShape* convexMeshShape = static_cast<const ConvexMeshShape*>(collider->getCollisionShape());
|
|
drawConvexMesh(collider->getLocalToWorldTransform(), convexMeshShape, color);
|
|
break;
|
|
}
|
|
case CollisionShapeName::TRIANGLE_MESH:
|
|
{
|
|
const ConcaveMeshShape* concaveMeshShape = static_cast<const ConcaveMeshShape*>(collider->getCollisionShape());
|
|
drawConcaveMeshShape(collider->getLocalToWorldTransform(), concaveMeshShape, color);
|
|
break;
|
|
}
|
|
case CollisionShapeName::HEIGHTFIELD:
|
|
{
|
|
const HeightFieldShape* heighFieldShape = static_cast<const HeightFieldShape*>(collider->getCollisionShape());
|
|
drawHeightFieldShape(collider->getLocalToWorldTransform(), heighFieldShape, color);
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
assert(false);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Generate the rendering primitives (triangles, lines, ...) of a physics world
|
|
void DebugRenderer::computeDebugRenderingPrimitives(const PhysicsWorld& world) {
|
|
|
|
const bool drawColliderAABB = getIsDebugItemDisplayed(DebugItem::COLLIDER_AABB);
|
|
const bool drawColliderBroadphaseAABB = getIsDebugItemDisplayed(DebugItem::COLLIDER_BROADPHASE_AABB);
|
|
const bool drawCollisionShape = getIsDebugItemDisplayed(DebugItem::COLLISION_SHAPE);
|
|
|
|
const uint32 nbCollisionBodies = world.getNbCollisionBodies();
|
|
const uint32 nbRigidBodies = world.getNbRigidBodies();
|
|
|
|
// For each body of the world
|
|
for (uint32 b = 0; b < nbCollisionBodies + nbRigidBodies; b++) {
|
|
|
|
// Get a body
|
|
const CollisionBody* body = b < nbCollisionBodies ? world.getCollisionBody(b) : world.getRigidBody(b - nbCollisionBodies);
|
|
|
|
if (body->isActive() && body->isDebugEnabled()) {
|
|
|
|
// For each collider of the body
|
|
for (uint32 c = 0; c < body->getNbColliders(); c++) {
|
|
|
|
// Get a collider
|
|
const Collider* collider = body->getCollider(c);
|
|
|
|
// If we need to draw the collider AABB
|
|
if (drawColliderAABB) {
|
|
|
|
drawAABB(collider->getWorldAABB(), mMapDebugItemWithColor[DebugItem::COLLIDER_AABB]);
|
|
}
|
|
|
|
// If we need to draw the collider broad-phase AABB
|
|
if (drawColliderBroadphaseAABB) {
|
|
|
|
if (collider->getBroadPhaseId() != -1) {
|
|
drawAABB(world.mCollisionDetection.mBroadPhaseSystem.getFatAABB(collider->getBroadPhaseId()), mMapDebugItemWithColor[DebugItem::COLLIDER_BROADPHASE_AABB]);
|
|
}
|
|
}
|
|
|
|
// If we need to draw the collision shape
|
|
if (drawCollisionShape) {
|
|
|
|
drawCollisionShapeOfCollider(collider, mMapDebugItemWithColor[DebugItem::COLLISION_SHAPE]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Called when some contacts occur
|
|
void DebugRenderer::onContact(const CollisionCallback::CallbackData& callbackData) {
|
|
|
|
// If we need to draw contact points
|
|
if (getIsDebugItemDisplayed(DebugItem::CONTACT_POINT) || getIsDebugItemDisplayed(DebugItem::CONTACT_NORMAL)) {
|
|
|
|
// For each contact pair
|
|
for (uint32 p = 0; p < callbackData.getNbContactPairs(); p++) {
|
|
|
|
CollisionCallback::ContactPair contactPair = callbackData.getContactPair(p);
|
|
|
|
if (contactPair.getEventType() != CollisionCallback::ContactPair::EventType::ContactExit) {
|
|
|
|
// For each contact point of the contact pair
|
|
for (uint32 c = 0; c < contactPair.getNbContactPoints(); c++) {
|
|
|
|
CollisionCallback::ContactPoint contactPoint = contactPair.getContactPoint(c);
|
|
|
|
Vector3 point = contactPair.getCollider1()->getLocalToWorldTransform() * contactPoint.getLocalPointOnCollider1();
|
|
|
|
if (getIsDebugItemDisplayed(DebugItem::CONTACT_POINT)) {
|
|
|
|
// Contact point
|
|
drawSphere(point, mContactPointSphereRadius, mMapDebugItemWithColor[DebugItem::CONTACT_POINT]);
|
|
}
|
|
|
|
if (getIsDebugItemDisplayed(DebugItem::CONTACT_NORMAL)) {
|
|
|
|
// Contact normal
|
|
mLines.add(DebugLine(point, point + contactPoint.getWorldNormal() * mContactNormalLength, mMapDebugItemWithColor[DebugItem::CONTACT_NORMAL]));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|