xiaoqi
/
SHADE_Y3
1
0
Fork 0
Code Issues 12 Pull Requests Packages Projects Releases 9 Wiki Activity

Contacts are solved locally on each body

This commit is contained in:
Diren D Bharwani 2023-03-01 00:55:44 +08:00
parent 9d8d1ee19d
commit abdf614083
12 changed files with 838 additions and 668 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
Assets/Scenes/PhysicsSandbox.shade
View File

@ -182,9 +182,9 @@
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: 0, y: 7, z: 0}
Rotate: {x: 0, y: 0, z: 0.785398185}
Scale: {x: 0.999990404, y: 0.999994457, z: 0.999985337}
Translate: {x: 0, y: 2, z: 3}
Rotate: {x: 0, y: 0.785398185, z: 0}
Scale: {x: 0.999988496, y: 0.999994099, z: 0.999984443}
IsActive: true
RigidBody Component:
Type: Dynamic
@ -229,7 +229,7 @@
Transform Component:
Translate: {x: 0, y: 0, z: 3}
Rotate: {x: -0, y: 0, z: -0}
Scale: {x: 1, y: 1, z: 1}
Scale: {x: 5, y: 1, z: 5}
IsActive: true
Collider Component:
DrawColliders: false

82
Assets/Scenes/SS_Playground.shade Normal file
View File

@ -0,0 +1,82 @@
- EID: 0
Name: Default
IsActive: true
NumberOfChildren: 0
Components:
Camera Component:
Position: {x: 0, y: 2, z: 7}
Pitch: 0
Yaw: 0
Roll: 0
Width: 1920
Height: 1080
Near: 0.00999999978
Far: 10000
Perspective: true
IsActive: true
Scripts: ~
- EID: 1
Name: Default
IsActive: true
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: 0, y: 0, z: 0}
Rotate: {x: 0, y: 0, z: 0}
Scale: {x: 1, y: 1, z: 1}
IsActive: true
Collider Component:
DrawColliders: true
Colliders:
- Is Trigger: false
Collision Tag: 1
Type: Sphere
Radius: 2
Friction: 0.400000006
Bounciness: 0
Density: 1
Position Offset: {x: 0, y: 0, z: 0}
Rotation Offset: {x: 0, y: 0, z: 0}
IsActive: true
Scripts: ~
- EID: 2
Name: Default
IsActive: true
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: 0, y: 3, z: 0}
Rotate: {x: 0, y: 0, z: 0}
Scale: {x: 1, y: 1, z: 1}
IsActive: true
RigidBody Component:
Type: Dynamic
Auto Mass: false
Mass: 1
Drag: 0.00999999978
Angular Drag: 0.00999999978
Use Gravity: true
Gravity Scale: 1
Interpolate: true
Sleeping Enabled: true
Freeze Position X: false
Freeze Position Y: false
Freeze Position Z: false
Freeze Rotation X: false
Freeze Rotation Y: false
Freeze Rotation Z: false
IsActive: true
Collider Component:
DrawColliders: true
Colliders:
- Is Trigger: false
Collision Tag: 1
Type: Sphere
Radius: 1
Friction: 0.400000006
Bounciness: 0
Density: 1
Position Offset: {x: 0, y: 0, z: 0}
Rotation Offset: {x: 0, y: 0, z: 0}
IsActive: true
Scripts: ~

3
Assets/Scenes/SS_Playground.shade.shmeta Normal file
View File

@ -0,0 +1,3 @@
Name: SS_Playground
ID: 92914350
Type: 5

4
SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h
View File

@ -62,7 +62,9 @@ namespace SHADE
float tangentImpulse[NUM_TANGENTS] = { 0.0f }; // Accumulated tangent impulses
float tangentMass[NUM_TANGENTS] = { 0.0f }; // Effective masses along the tangents
SHVec3 position;
// We store points locally for each contact
SHVec3 localPointA;
SHVec3 localPointB;
SHVec3 rA; // Vector from COM of A to the contact
SHVec3 rB; // Vector from COM of B to the contact
SHContactFeatures featurePair;

148
SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h
View File

@ -14,7 +14,6 @@
#include "Math/Geometry/SHPlane.h"
#include "Physics/Collision/Contacts/SHManifold.h"
#include "Physics/Collision/Contacts/SHCollisionKey.h"
#include "Physics/Collision/Shapes/SHHalfEdgeStructure.h"
namespace SHADE
@ -68,155 +67,10 @@ namespace SHADE
[[nodiscard]] static bool ConvexVsConvex (SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept;
private:
/*---------------------------------------------------------------------------------*/
/* Type Definitions */
/*---------------------------------------------------------------------------------*/
struct FaceQuery
{
bool colliding = false; // Allows for early out
int32_t closestFace = -1;
float bestDistance = std::numeric_limits<float>::lowest();
};
struct EdgeQuery
{
int32_t halfEdgeA = -1;
int32_t halfEdgeB = -1;
int32_t axis = -1;
float bestDistance = std::numeric_limits<float>::lowest();
};
struct ClipVertex
{
SHVec3 position;
SHContactFeatures featurePair;
};
/*---------------------------------------------------------------------------------*/
/* Member Functions */
/*---------------------------------------------------------------------------------*/
// Sphere VS Convex
static FaceQuery findClosestFace
(
const SHSphere& sphere
, const SHConvexPolyhedron& polyhedron
) noexcept;
static int32_t findClosestPoint
(
const SHSphere& sphere
, const SHConvexPolyhedron& polyhedron
, int32_t faceIndex
) noexcept;
static int32_t findVoronoiRegion
(
const SHSphere& sphere
, const SHVec3& faceVertex
, const SHVec3& faceNormal
, const SHVec3& tangent1
, const SHVec3& tangent2
) noexcept;
// Capsule VS Convex
// TODO: Capsule VS Convex uses the same gauss map optimisation as convex vs convex
// Convex VS Convex
/*
* ! References
* https://ia801303.us.archive.org/30/items/GDC2013Gregorius/GDC2013-Gregorius.pdf
* https://github.com/RandyGaul/qu3e/blob/master/src/collision/q3Collide.cpp
*/
static FaceQuery queryFaceDirections
(
const SHConvexPolyhedron& A
, const SHConvexPolyhedron& B
) noexcept;
static EdgeQuery queryEdgeDirections
(
const SHConvexPolyhedron& A
, const SHConvexPolyhedron& B
) noexcept;
static bool buildMinkowskiFace
(
const SHConvexPolyhedron& A
, const SHConvexPolyhedron& B
, int32_t edgeA
, int32_t edgeB
) noexcept;
static bool isMinkowskiFace
(
const SHVec3& a
, const SHVec3& b
, const SHVec3& c
, const SHVec3& d
) noexcept;
static float distanceBetweenEdges
(
const SHConvexPolyhedron& A
, const SHConvexPolyhedron& B
, int32_t edgeA
, int32_t edgeB
) noexcept;
static SHVec3 findClosestPointBetweenEdges
(
const SHConvexPolyhedron& A
, const SHConvexPolyhedron& B
, int32_t edgeA
, int32_t edgeB
) noexcept;
static int32_t findIncidentFace
(
const SHConvexPolyhedron& poly
, const SHVec3& normal
) noexcept;
static bool findFaceContacts
(
SHManifold& manifold
, const SHConvexPolyhedron& incPoly
, int32_t incFace
, const SHConvexPolyhedron& refPoly
, int32_t refFace
, bool flip
) noexcept;
static std::vector<ClipVertex> clipPolygonWithPlane
(
const std::vector<ClipVertex>& in
, int32_t numIn
, const SHPlane& plane
, int32_t planeIdx
) noexcept;
static std::vector<int32_t> reduceContacts
(
const std::vector<SHContact>& in
, const SHVec3& faceNormal
) noexcept;
// Cached Convex VS Convex
static bool cachedConvexVSConvex
(
SHManifold& manifold
, const SHSATInfo& cachedInfo
, const SHConvexPolyhedron& A
, const SHConvexPolyhedron& B
) noexcept;
static bool cachedConvexVSConvex(SHManifold& manifold, const SHSATInfo& cachedInfo, const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
};
} // namespace SHADE

38
SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionUtils.cpp Normal file
View File

@ -0,0 +1,38 @@
/****************************************************************************************
* \file SHCollisionUtils.cpp
* \author Diren D Bharwani, diren.dbharwani, 390002520
* \brief Implementation for some objects to assist with collision detection
*
* \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or
* disclosure of this file or its contents without the prior written consent
* of DigiPen Institute of Technology is prohibited.
****************************************************************************************/
#include <SHpch.h>
// Primary Header
#include "SHCollisionUtils.h"
namespace SHADE
{
/*-----------------------------------------------------------------------------------*/
/* Operator Overload Definitions */
/*-----------------------------------------------------------------------------------*/
SHVec3 SHCollisionUtils::ShapeTransform::operator*(const SHVec3& rhs) const noexcept
{
return SHVec3::Rotate(rhs, orientation) + position;
}
/*-----------------------------------------------------------------------------------*/
/* Public Member Functions Definitions */
/*-----------------------------------------------------------------------------------*/
void SHCollisionUtils::ShapeTransform::Invert() noexcept
{
orientation.Invert();
position = SHVec3::Rotate(-position, orientation);
}
} // namespace SHADE

108
SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionUtils.h Normal file
View File

@ -0,0 +1,108 @@
/****************************************************************************************
* \file SHCollisionUtils.h
* \author Diren D Bharwani, diren.dbharwani, 390002520
* \brief Interface for some objects to assist with collision detection
*
* \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or
* disclosure of this file or its contents without the prior written consent
* of DigiPen Institute of Technology is prohibited.
****************************************************************************************/
#pragma once
// Project Headers
#include "Math/Vector/SHVec3.h"
#include "Physics/Collision/Contacts/SHContact.h"
namespace SHADE
{
/*-----------------------------------------------------------------------------------*/
/* Type Definitions */
/*-----------------------------------------------------------------------------------*/
/**
* @brief
* Defines a bunch of helper objects for collision detection.
*/
class SHCollisionUtils
{
public:
/*---------------------------------------------------------------------------------*/
/* Type Definitions */
/*---------------------------------------------------------------------------------*/
struct ShapeTransform
{
public:
/*-------------------------------------------------------- ----------------------*/
/* Data Members */
/*-------------------------------------------------------- ----------------------*/
SHVec3 position;
SHQuaternion orientation;
/*-------------------------------------------------------- ----------------------*/
/* Operator Overloads */
/*-------------------------------------------------------- ----------------------*/
SHVec3 operator* (const SHVec3& rhs) const noexcept;
/*-------------------------------------------------------- ----------------------*/
/* Member Functions */
/*-------------------------------------------------------- ----------------------*/
void Invert() noexcept;
};
struct FaceQuery
{
public:
/*-------------------------------------------------------- ----------------------*/
/* Data Members */
/*-------------------------------------------------------- ----------------------*/
bool colliding = false; // Allows for early out
int32_t closestFace = -1;
float bestDistance = std::numeric_limits<float>::lowest();
};
struct EdgeQuery
{
public:
/*-------------------------------------------------------- ----------------------*/
/* Data Members */
/*-------------------------------------------------------- ----------------------*/
int32_t halfEdgeA = -1;
int32_t halfEdgeB = -1;
int32_t axis = -1;
float bestDistance = std::numeric_limits<float>::lowest();
};
struct EdgeContacts
{
public:
/*-------------------------------------------------------- ----------------------*/
/* Data Members */
/*-------------------------------------------------------- ----------------------*/
SHVec3 closestPointA;
SHVec3 closestPointB;
};
struct ClipVertex
{
public:
/*-------------------------------------------------------- ----------------------*/
/* Data Members */
/*-------------------------------------------------------- ----------------------*/
SHVec3 position;
SHContactFeatures featurePair;
};
using ClipVertices = std::vector<ClipVertex>;
};
} // namespace SHAD

894
SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp
View File

File diff suppressed because it is too large Load Diff

179
SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp
View File

@ -15,14 +15,27 @@
#include "SHCollision.h"
// Project Headers
#include "SHCollisionUtils.h"
#include "Math/Geometry/SHPlane.h"
#include "Math/SHMathHelpers.h"
#include "Physics/Collision/Shapes/SHSphere.h"
#include "Physics/Collision/Shapes/SHConvexPolyhedron.h"
#include "Physics/SHPhysicsConstants.h"
namespace SHADE
{
/*-----------------------------------------------------------------------------------*/
/* Local Functions Declarations */
/*-----------------------------------------------------------------------------------*/
SHCollisionUtils::FaceQuery findClosestFace (const SHSphere&, const SHConvexPolyhedron&) noexcept;
int32_t findClosestPoint (const SHSphere&, const SHConvexPolyhedron&, int32_t faceIndex) noexcept;
int32_t findVoronoiRegion (const SHSphere&, const SHVec3& faceVertex, const SHVec3& faceNormal, const SHVec3& t1, const SHVec3& t2) noexcept;
/*-----------------------------------------------------------------------------------*/
/* Public Member Functions Definitions */
/*-----------------------------------------------------------------------------------*/
@ -36,7 +49,7 @@ namespace SHADE
const float RADIUS = SPHERE.GetWorldRadius();
// Find closest face
const FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON);
const SHCollisionUtils::FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON);
if (!FACE_QUERY.colliding)
return false;
@ -77,104 +90,106 @@ namespace SHADE
{
// Convert to underlying types
// For the convex, we only need the convex polyhedron shape since the get vertex is pure virtual.
const SHSphere& SPHERE = dynamic_cast<const SHSphere&>(A);
const SHConvexPolyhedron& POLYHEDRON = dynamic_cast<const SHConvexPolyhedron&>(B);
//const SHSphere& SPHERE = dynamic_cast<const SHSphere&>(A);
//const SHConvexPolyhedron& POLYHEDRON = dynamic_cast<const SHConvexPolyhedron&>(B);
const SHVec3 CENTER = SPHERE.Center;
const float RADIUS = SPHERE.GetWorldRadius();
//const SHVec3 CENTER = SPHERE.Center;
//const float RADIUS = SPHERE.GetWorldRadius();
const FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON);
if (!FACE_QUERY.colliding)
return false;
//const SHCollisionUtils::FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON);
//if (!FACE_QUERY.colliding)
// return false;
uint32_t numContacts = 0;
const float PENETRATION = RADIUS - FACE_QUERY.bestDistance;
//uint32_t numContacts = 0;
//const float PENETRATION = RADIUS - FACE_QUERY.bestDistance;
SHContact contact;
contact.featurePair.key = 0;
//SHContact contact;
//contact.featurePair.key = 0;
// Check if center is inside polyhedron (below the face)
if (FACE_QUERY.bestDistance < SHMath::EPSILON)
{
manifold.normal = -POLYHEDRON.GetNormal(FACE_QUERY.closestFace);
//// Check if center is inside polyhedron (below the face)
//if (FACE_QUERY.bestDistance < SHMath::EPSILON)
//{
// manifold.normal = -POLYHEDRON.GetNormal(FACE_QUERY.closestFace);
contact.penetration = PENETRATION;
contact.position = CENTER;
// contact.penetration = PENETRATION;
// contact.position = CENTER;
manifold.contacts[numContacts++] = contact;
manifold.numContacts = numContacts;
// manifold.contacts[numContacts++] = contact;
// manifold.numContacts = numContacts;
return true;
}
// return true;
//}
// Find closest face of polygon to circle
const int32_t CLOSEST_POINT = findClosestPoint(SPHERE, POLYHEDRON, FACE_QUERY.closestFace);
//// Find closest face of polygon to circle
//const int32_t CLOSEST_POINT = findClosestPoint(SPHERE, POLYHEDRON, FACE_QUERY.closestFace);
const auto& FACE = POLYHEDRON.GetFace(FACE_QUERY.closestFace);
const SHVec3& FACE_NORMAL = POLYHEDRON.GetNormal(FACE_QUERY.closestFace);
const int32_t NUM_VERTICES = static_cast<int32_t>(FACE.vertexIndices.size());
//const auto& FACE = POLYHEDRON.GetFace(FACE_QUERY.closestFace);
//const SHVec3& FACE_NORMAL = POLYHEDRON.GetNormal(FACE_QUERY.closestFace);
//const int32_t NUM_VERTICES = static_cast<int32_t>(FACE.vertexIndices.size());
// Get points and build tangents
const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES;
const int32_t P3_INDEX = CLOSEST_POINT == 0 ? NUM_VERTICES - 1 : CLOSEST_POINT - 1;
//// Get points and build tangents
//const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES;
//const int32_t P3_INDEX = CLOSEST_POINT == 0 ? NUM_VERTICES - 1 : CLOSEST_POINT - 1;
const SHVec3 P1 = POLYHEDRON.GetVertex(FACE.vertexIndices[CLOSEST_POINT].index);
const SHVec3 P2 = POLYHEDRON.GetVertex(FACE.vertexIndices[P2_INDEX].index);
const SHVec3 P3 = POLYHEDRON.GetVertex(FACE.vertexIndices[P3_INDEX].index);
//const SHVec3 P1 = POLYHEDRON.GetVertex(FACE.vertexIndices[CLOSEST_POINT].index);
//const SHVec3 P2 = POLYHEDRON.GetVertex(FACE.vertexIndices[P2_INDEX].index);
//const SHVec3 P3 = POLYHEDRON.GetVertex(FACE.vertexIndices[P3_INDEX].index);
const SHVec3 TANGENT_1 = SHVec3::Normalise(P2 - P1);
const SHVec3 TANGENT_2 = SHVec3::Normalise(P3 - P1);
//const SHVec3 TANGENT_1 = SHVec3::Normalise(P2 - P1);
//const SHVec3 TANGENT_2 = SHVec3::Normalise(P3 - P1);
// Get the voronoi region it belongs in
const int32_t REGION = findVoronoiRegion(SPHERE, P1, FACE_NORMAL, TANGENT_1, TANGENT_2);
if (REGION == 0)
return false;
//// Get the voronoi region it belongs in
//const int32_t REGION = findVoronoiRegion(SPHERE, P1, FACE_NORMAL, TANGENT_1, TANGENT_2);
//if (REGION == 0)
// return false;
// Create contact information based on region
//// Create contact information based on region
const SHVec3 P1_TO_CENTER = CENTER - P1;
switch (REGION)
{
case 1: // Region A
case 2: // Region B
{
// Find closest point
const SHVec3& TANGENT = REGION == 1 ? TANGENT_1 : TANGENT_2;
const SHVec3 CP = P1 + TANGENT * SHVec3::Dot(P1_TO_CENTER, TANGENT);
const SHVec3 CP_TO_CENTER = CENTER - CP;
//const SHVec3 P1_TO_CENTER = CENTER - P1;
//switch (REGION)
//{
// case 1: // Region A
// case 2: // Region B
// {
// // Find closest point
// const SHVec3& TANGENT = REGION == 1 ? TANGENT_1 : TANGENT_2;
// const SHVec3 CP = P1 + TANGENT * SHVec3::Dot(P1_TO_CENTER, TANGENT);
// const SHVec3 CP_TO_CENTER = CENTER - CP;
manifold.normal = -SHVec3::Normalise(CP_TO_CENTER);
// manifold.normal = -SHVec3::Normalise(CP_TO_CENTER);
contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal);
contact.position = CP;
// contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal);
// contact.position = CP;
break;
}
case 3: // Region C
{
manifold.normal = -SHVec3::Normalise(P1_TO_CENTER);
// break;
// }
// case 3: // Region C
// {
// manifold.normal = -SHVec3::Normalise(P1_TO_CENTER);
contact.penetration = RADIUS - P1_TO_CENTER.Length();
contact.position = P1;
// contact.penetration = RADIUS - P1_TO_CENTER.Length();
// contact.position = P1;
break;
}
case 4: // Region D
{
manifold.normal = -FACE_NORMAL;
// break;
// }
// case 4: // Region D
// {
// manifold.normal = -FACE_NORMAL;
contact.penetration = PENETRATION;
contact.position = CENTER - FACE_NORMAL * RADIUS;
// contact.penetration = PENETRATION;
// contact.position = CENTER - FACE_NORMAL * RADIUS;
break;
}
default: return false; // Should never happen
}
// break;
// }
// default: return false; // Should never happen
//}
manifold.contacts[numContacts++] = contact;
manifold.numContacts = numContacts;
//manifold.contacts[numContacts++] = contact;
//manifold.numContacts = numContacts;
return true;
//return true;
return false;
}
bool SHCollision::ConvexVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
@ -192,15 +207,15 @@ namespace SHADE
/* Private Member Functions Definitions */
/*-----------------------------------------------------------------------------------*/
SHCollision::FaceQuery SHCollision::findClosestFace
SHCollisionUtils::FaceQuery findClosestFace
(
const SHSphere& sphere
, const SHConvexPolyhedron& polyhedron
) noexcept
{
FaceQuery faceQuery;
SHCollisionUtils::FaceQuery faceQuery;
const SHVec3 CENTER = sphere.Center;
const SHVec3 CENTER = sphere.GetWorldCentroid();
const float RADIUS = sphere.GetWorldRadius();
/*
@ -238,7 +253,7 @@ namespace SHADE
return faceQuery;
}
int32_t SHCollision::findClosestPoint
int32_t findClosestPoint
(
const SHSphere& sphere
, const SHConvexPolyhedron& polyhedron
@ -248,7 +263,7 @@ namespace SHADE
// Find closest point on face
int32_t closestPointIndex = -1;
const SHVec3 CENTER = sphere.Center;
const SHVec3 CENTER = sphere.GetWorldCentroid();
const SHHalfEdgeStructure::Face& FACE = polyhedron.GetFace(faceIndex);
const int32_t NUM_VERITICES = static_cast<int32_t>(FACE.vertexIndices.size());
@ -269,7 +284,7 @@ namespace SHADE
return closestPointIndex;
}
int32_t SHCollision::findVoronoiRegion
int32_t findVoronoiRegion
(
const SHSphere& sphere
, const SHVec3& faceVertex
@ -294,8 +309,8 @@ namespace SHADE
* / / regionC
*
*/
const SHVec3 CENTER = sphere.Center;
const SHVec3 CENTER = sphere.GetWorldCentroid();
const float RADIUS = sphere.GetWorldRadius();
const SHVec3 TANGENTS [NUM_TANGENTS] { tangent1, tangent2 };

20
SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp
View File

@ -14,6 +14,7 @@
#include "SHCollision.h"
// Project Headers
#include "SHCollisionUtils.h"
#include "Math/SHMathHelpers.h"
#include "Physics/Collision/Shapes/SHSphere.h"
@ -23,7 +24,7 @@ namespace SHADE
/* Public Member Functions Definitions */
/*-----------------------------------------------------------------------------------*/
bool SHCollision::SphereVsSphere(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
bool SHCollision::SphereVsSphere(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
{
const SHSphere& SPHERE_A = dynamic_cast<const SHSphere&>(A);
const SHSphere& SPHERE_B = dynamic_cast<const SHSphere&>(B);
@ -65,26 +66,35 @@ namespace SHADE
if (DISTANCE_BETWEEN_CENTERS_SQUARED > COMBINED_RADIUS_SQUARED)
return false;
SHCollisionUtils::ShapeTransform inverseTransformA = { SPHERE_A.GetWorldCentroid(), SPHERE_A.GetWorldOrientation() };
SHCollisionUtils::ShapeTransform inverseTransformB = { SPHERE_B.GetWorldCentroid(), SPHERE_B.GetWorldOrientation() };
inverseTransformA.Invert();
inverseTransformB.Invert();
// Only populate the manifold if there is a collision
uint32_t numContacts = 0;
SHContact contact;
contact.featurePair.key = 0;
contact.penetration = COMBINED_RADIUS - A_TO_B.Length();
// Degenerate case
if (SHMath::CompareFloat(DISTANCE_BETWEEN_CENTERS_SQUARED, 0.0f))
{
manifold.normal = SHVec3::UnitY;
contact.position = CENTER_A;
contact.penetration = RADIUS_B;
contact.localPointA = RADIUS_A * SHVec3::Rotate(manifold.normal, inverseTransformA.orientation);
contact.localPointB = RADIUS_B * SHVec3::Rotate(manifold.normal, inverseTransformB.orientation);
manifold.contacts[numContacts++] = contact;
}
else
{
manifold.normal = SHVec3::Normalise(A_TO_B);
contact.position = CENTER_B - manifold.normal * RADIUS_B;
contact.penetration = COMBINED_RADIUS - A_TO_B.Length();
contact.localPointA = RADIUS_A * SHVec3::Normalise(inverseTransformA * CENTER_B);
contact.localPointB = RADIUS_B * SHVec3::Normalise(inverseTransformB * CENTER_A);
manifold.contacts[numContacts++] = contact;
}

14
SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp
View File

@ -17,6 +17,7 @@
// Project Headers
#include "Math/SHMathHelpers.h"
#include "Physics/Collision/Narrowphase/SHCollisionDispatch.h"
#include "Physics/Collision/Narrowphase/SHCollisionUtils.h"
namespace SHADE
{
@ -51,8 +52,12 @@ namespace SHADE
, .normal = manifold.normal
};
const auto* SHAPE_A = manifold.shapeA;
const SHCollisionUtils::ShapeTransform TF_A = { SHAPE_A->GetWorldCentroid(), SHAPE_A->GetWorldOrientation() };
for (uint32_t i = 0; i < manifold.numContacts; ++i)
collisionEvent.contactPoints[i] = manifold.contacts[i].position;
collisionEvent.contactPoints[i] = TF_A * manifold.contacts[i].localPointA;
collisionEvents.emplace_back(collisionEvent);
}
@ -71,11 +76,16 @@ namespace SHADE
if (manifold.state == SHCollisionState::EXIT)
continue;
const auto* SHAPE_A = manifold.shapeA;
const SHCollisionUtils::ShapeTransform TF_A = { SHAPE_A->GetWorldCentroid(), SHAPE_A->GetWorldOrientation() };
for (uint32_t i = 0; i < manifold.numContacts; ++i)
{
// Contact position will be the world position of localPointA
const ContactInfo INFO
{
.position = manifold.contacts[i].position
.position = TF_A * manifold.contacts[i].localPointA
, .normal = manifold.normal
};

8
SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp
View File

@ -16,6 +16,7 @@
// Project Headers
#include "Math/SHMathHelpers.h"
#include "Physics/SHPhysicsConstants.h"
#include "Physics/Collision/Narrowphase/SHCollisionUtils.h"
namespace SHADE
{
@ -108,11 +109,14 @@ namespace SHADE
memcpy_s(newConstraint.tangents, TANGENTS_SIZE, manifold.tangents, TANGENTS_SIZE);
memcpy_s(newConstraint.contacts, CONTACTS_SIZE, manifold.contacts, CONTACTS_SIZE);
const SHCollisionUtils::ShapeTransform TF_A = { SHAPE_A->GetWorldCentroid(), SHAPE_A->GetWorldOrientation() };
const SHCollisionUtils::ShapeTransform TF_B = { SHAPE_B->GetWorldCentroid(), SHAPE_B->GetWorldOrientation() };
// Compute rA & rB for contacts
for (uint32_t i = 0; i < newConstraint.numContacts; ++i)
{
newConstraint.contacts[i].rA = newConstraint.contacts[i].position - newConstraint.centerOfMassA;
newConstraint.contacts[i].rB = newConstraint.contacts[i].position - newConstraint.centerOfMassB;
newConstraint.contacts[i].rA = (TF_A * newConstraint.contacts[i].localPointA) - newConstraint.centerOfMassA;
newConstraint.contacts[i].rB = (TF_B * newConstraint.contacts[i].localPointB) - newConstraint.centerOfMassB;
}
}