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 NumberOfChildren: 0
Components: Components:
Transform Component: Transform Component:
Translate: {x: 0, y: 7, z: 0} Translate: {x: 0, y: 2, z: 3}
Rotate: {x: 0, y: 0, z: 0.785398185} Rotate: {x: 0, y: 0.785398185, z: 0}
Scale: {x: 0.999990404, y: 0.999994457, z: 0.999985337} Scale: {x: 0.999988496, y: 0.999994099, z: 0.999984443}
IsActive: true IsActive: true
RigidBody Component: RigidBody Component:
Type: Dynamic Type: Dynamic
@ -229,7 +229,7 @@
Transform Component: Transform Component:
Translate: {x: 0, y: 0, z: 3} Translate: {x: 0, y: 0, z: 3}
Rotate: {x: -0, y: 0, z: -0} Rotate: {x: -0, y: 0, z: -0}
Scale: {x: 1, y: 1, z: 1} Scale: {x: 5, y: 1, z: 5}
IsActive: true IsActive: true
Collider Component: Collider Component:
DrawColliders: false 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 tangentImpulse[NUM_TANGENTS] = { 0.0f }; // Accumulated tangent impulses
float tangentMass[NUM_TANGENTS] = { 0.0f }; // Effective masses along the tangents 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 rA; // Vector from COM of A to the contact
SHVec3 rB; // Vector from COM of B to the contact SHVec3 rB; // Vector from COM of B to the contact
SHContactFeatures featurePair; SHContactFeatures featurePair;

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

@ -14,7 +14,6 @@
#include "Math/Geometry/SHPlane.h" #include "Math/Geometry/SHPlane.h"
#include "Physics/Collision/Contacts/SHManifold.h" #include "Physics/Collision/Contacts/SHManifold.h"
#include "Physics/Collision/Contacts/SHCollisionKey.h" #include "Physics/Collision/Contacts/SHCollisionKey.h"
#include "Physics/Collision/Shapes/SHHalfEdgeStructure.h"
namespace SHADE namespace SHADE
@ -68,155 +67,10 @@ namespace SHADE
[[nodiscard]] static bool ConvexVsConvex (SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept; [[nodiscard]] static bool ConvexVsConvex (SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept;
private: 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 */ /* Member Functions */
/*---------------------------------------------------------------------------------*/ /*---------------------------------------------------------------------------------*/
// Sphere VS Convex static bool cachedConvexVSConvex(SHManifold& manifold, const SHSATInfo& cachedInfo, const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
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;
}; };
} // namespace SHADE } // 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" #include "SHCollision.h"
// Project Headers // Project Headers
#include "SHCollisionUtils.h"
#include "Math/Geometry/SHPlane.h" #include "Math/Geometry/SHPlane.h"
#include "Math/SHMathHelpers.h" #include "Math/SHMathHelpers.h"
#include "Physics/Collision/Shapes/SHSphere.h" #include "Physics/Collision/Shapes/SHSphere.h"
#include "Physics/Collision/Shapes/SHConvexPolyhedron.h" #include "Physics/Collision/Shapes/SHConvexPolyhedron.h"
#include "Physics/SHPhysicsConstants.h" #include "Physics/SHPhysicsConstants.h"
namespace SHADE 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 */ /* Public Member Functions Definitions */
/*-----------------------------------------------------------------------------------*/ /*-----------------------------------------------------------------------------------*/
@ -36,7 +49,7 @@ namespace SHADE
const float RADIUS = SPHERE.GetWorldRadius(); const float RADIUS = SPHERE.GetWorldRadius();
// Find closest face // Find closest face
const FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON); const SHCollisionUtils::FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON);
if (!FACE_QUERY.colliding) if (!FACE_QUERY.colliding)
return false; return false;
@ -77,104 +90,106 @@ namespace SHADE
{ {
// Convert to underlying types // Convert to underlying types
// For the convex, we only need the convex polyhedron shape since the get vertex is pure virtual. // 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 SHSphere& SPHERE = dynamic_cast<const SHSphere&>(A);
const SHConvexPolyhedron& POLYHEDRON = dynamic_cast<const SHConvexPolyhedron&>(B); //const SHConvexPolyhedron& POLYHEDRON = dynamic_cast<const SHConvexPolyhedron&>(B);
const SHVec3 CENTER = SPHERE.Center; //const SHVec3 CENTER = SPHERE.Center;
const float RADIUS = SPHERE.GetWorldRadius(); //const float RADIUS = SPHERE.GetWorldRadius();
const FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON); //const SHCollisionUtils::FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON);
if (!FACE_QUERY.colliding) //if (!FACE_QUERY.colliding)
return false; // return false;
uint32_t numContacts = 0; //uint32_t numContacts = 0;
const float PENETRATION = RADIUS - FACE_QUERY.bestDistance; //const float PENETRATION = RADIUS - FACE_QUERY.bestDistance;
SHContact contact; //SHContact contact;
contact.featurePair.key = 0; //contact.featurePair.key = 0;
// Check if center is inside polyhedron (below the face) //// Check if center is inside polyhedron (below the face)
if (FACE_QUERY.bestDistance < SHMath::EPSILON) //if (FACE_QUERY.bestDistance < SHMath::EPSILON)
{ //{
manifold.normal = -POLYHEDRON.GetNormal(FACE_QUERY.closestFace); // manifold.normal = -POLYHEDRON.GetNormal(FACE_QUERY.closestFace);
contact.penetration = PENETRATION; // contact.penetration = PENETRATION;
contact.position = CENTER; // contact.position = CENTER;
manifold.contacts[numContacts++] = contact; // manifold.contacts[numContacts++] = contact;
manifold.numContacts = numContacts; // manifold.numContacts = numContacts;
return true; // return true;
} //}
// Find closest face of polygon to circle //// Find closest face of polygon to circle
const int32_t CLOSEST_POINT = findClosestPoint(SPHERE, POLYHEDRON, FACE_QUERY.closestFace); //const int32_t CLOSEST_POINT = findClosestPoint(SPHERE, POLYHEDRON, FACE_QUERY.closestFace);
const auto& FACE = POLYHEDRON.GetFace(FACE_QUERY.closestFace); //const auto& FACE = POLYHEDRON.GetFace(FACE_QUERY.closestFace);
const SHVec3& FACE_NORMAL = POLYHEDRON.GetNormal(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 int32_t NUM_VERTICES = static_cast<int32_t>(FACE.vertexIndices.size());
// Get points and build tangents //// Get points and build tangents
const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES; //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 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 P1 = POLYHEDRON.GetVertex(FACE.vertexIndices[CLOSEST_POINT].index);
const SHVec3 P2 = POLYHEDRON.GetVertex(FACE.vertexIndices[P2_INDEX].index); //const SHVec3 P2 = POLYHEDRON.GetVertex(FACE.vertexIndices[P2_INDEX].index);
const SHVec3 P3 = POLYHEDRON.GetVertex(FACE.vertexIndices[P3_INDEX].index); //const SHVec3 P3 = POLYHEDRON.GetVertex(FACE.vertexIndices[P3_INDEX].index);
const SHVec3 TANGENT_1 = SHVec3::Normalise(P2 - P1); //const SHVec3 TANGENT_1 = SHVec3::Normalise(P2 - P1);
const SHVec3 TANGENT_2 = SHVec3::Normalise(P3 - P1); //const SHVec3 TANGENT_2 = SHVec3::Normalise(P3 - P1);
// Get the voronoi region it belongs in //// Get the voronoi region it belongs in
const int32_t REGION = findVoronoiRegion(SPHERE, P1, FACE_NORMAL, TANGENT_1, TANGENT_2); //const int32_t REGION = findVoronoiRegion(SPHERE, P1, FACE_NORMAL, TANGENT_1, TANGENT_2);
if (REGION == 0) //if (REGION == 0)
return false; // return false;
// Create contact information based on region //// Create contact information based on region
const SHVec3 P1_TO_CENTER = CENTER - P1; //const SHVec3 P1_TO_CENTER = CENTER - P1;
switch (REGION) //switch (REGION)
{ //{
case 1: // Region A // case 1: // Region A
case 2: // Region B // case 2: // Region B
{ // {
// Find closest point // // Find closest point
const SHVec3& TANGENT = REGION == 1 ? TANGENT_1 : TANGENT_2; // const SHVec3& TANGENT = REGION == 1 ? TANGENT_1 : TANGENT_2;
const SHVec3 CP = P1 + TANGENT * SHVec3::Dot(P1_TO_CENTER, TANGENT); // const SHVec3 CP = P1 + TANGENT * SHVec3::Dot(P1_TO_CENTER, TANGENT);
const SHVec3 CP_TO_CENTER = CENTER - CP; // 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.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal);
contact.position = CP; // contact.position = CP;
break; // break;
} // }
case 3: // Region C // case 3: // Region C
{ // {
manifold.normal = -SHVec3::Normalise(P1_TO_CENTER); // manifold.normal = -SHVec3::Normalise(P1_TO_CENTER);
contact.penetration = RADIUS - P1_TO_CENTER.Length(); // contact.penetration = RADIUS - P1_TO_CENTER.Length();
contact.position = P1; // contact.position = P1;
break; // break;
} // }
case 4: // Region D // case 4: // Region D
{ // {
manifold.normal = -FACE_NORMAL; // manifold.normal = -FACE_NORMAL;
contact.penetration = PENETRATION; // contact.penetration = PENETRATION;
contact.position = CENTER - FACE_NORMAL * RADIUS; // contact.position = CENTER - FACE_NORMAL * RADIUS;
break; // break;
} // }
default: return false; // Should never happen // default: return false; // Should never happen
} //}
manifold.contacts[numContacts++] = contact; //manifold.contacts[numContacts++] = contact;
manifold.numContacts = numContacts; //manifold.numContacts = numContacts;
return true; //return true;
return false;
} }
bool SHCollision::ConvexVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept bool SHCollision::ConvexVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
@ -192,15 +207,15 @@ namespace SHADE
/* Private Member Functions Definitions */ /* Private Member Functions Definitions */
/*-----------------------------------------------------------------------------------*/ /*-----------------------------------------------------------------------------------*/
SHCollision::FaceQuery SHCollision::findClosestFace SHCollisionUtils::FaceQuery findClosestFace
( (
const SHSphere& sphere const SHSphere& sphere
, const SHConvexPolyhedron& polyhedron , const SHConvexPolyhedron& polyhedron
) noexcept ) noexcept
{ {
FaceQuery faceQuery; SHCollisionUtils::FaceQuery faceQuery;
const SHVec3 CENTER = sphere.Center; const SHVec3 CENTER = sphere.GetWorldCentroid();
const float RADIUS = sphere.GetWorldRadius(); const float RADIUS = sphere.GetWorldRadius();
/* /*
@ -238,7 +253,7 @@ namespace SHADE
return faceQuery; return faceQuery;
} }
int32_t SHCollision::findClosestPoint int32_t findClosestPoint
( (
const SHSphere& sphere const SHSphere& sphere
, const SHConvexPolyhedron& polyhedron , const SHConvexPolyhedron& polyhedron
@ -248,7 +263,7 @@ namespace SHADE
// Find closest point on face // Find closest point on face
int32_t closestPointIndex = -1; int32_t closestPointIndex = -1;
const SHVec3 CENTER = sphere.Center; const SHVec3 CENTER = sphere.GetWorldCentroid();
const SHHalfEdgeStructure::Face& FACE = polyhedron.GetFace(faceIndex); const SHHalfEdgeStructure::Face& FACE = polyhedron.GetFace(faceIndex);
const int32_t NUM_VERITICES = static_cast<int32_t>(FACE.vertexIndices.size()); const int32_t NUM_VERITICES = static_cast<int32_t>(FACE.vertexIndices.size());
@ -269,7 +284,7 @@ namespace SHADE
return closestPointIndex; return closestPointIndex;
} }
int32_t SHCollision::findVoronoiRegion int32_t findVoronoiRegion
( (
const SHSphere& sphere const SHSphere& sphere
, const SHVec3& faceVertex , const SHVec3& faceVertex
@ -294,8 +309,8 @@ namespace SHADE
* / / regionC * / / regionC
* *
*/ */
const SHVec3 CENTER = sphere.Center; const SHVec3 CENTER = sphere.GetWorldCentroid();
const float RADIUS = sphere.GetWorldRadius(); const float RADIUS = sphere.GetWorldRadius();
const SHVec3 TANGENTS [NUM_TANGENTS] { tangent1, tangent2 }; 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" #include "SHCollision.h"
// Project Headers // Project Headers
#include "SHCollisionUtils.h"
#include "Math/SHMathHelpers.h" #include "Math/SHMathHelpers.h"
#include "Physics/Collision/Shapes/SHSphere.h" #include "Physics/Collision/Shapes/SHSphere.h"
@ -23,7 +24,7 @@ namespace SHADE
/* Public Member Functions Definitions */ /* 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_A = dynamic_cast<const SHSphere&>(A);
const SHSphere& SPHERE_B = dynamic_cast<const SHSphere&>(B); const SHSphere& SPHERE_B = dynamic_cast<const SHSphere&>(B);
@ -65,26 +66,35 @@ namespace SHADE
if (DISTANCE_BETWEEN_CENTERS_SQUARED > COMBINED_RADIUS_SQUARED) if (DISTANCE_BETWEEN_CENTERS_SQUARED > COMBINED_RADIUS_SQUARED)
return false; 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 // Only populate the manifold if there is a collision
uint32_t numContacts = 0; uint32_t numContacts = 0;
SHContact contact; SHContact contact;
contact.featurePair.key = 0; contact.featurePair.key = 0;
contact.penetration = COMBINED_RADIUS - A_TO_B.Length();
// Degenerate case
if (SHMath::CompareFloat(DISTANCE_BETWEEN_CENTERS_SQUARED, 0.0f)) if (SHMath::CompareFloat(DISTANCE_BETWEEN_CENTERS_SQUARED, 0.0f))
{ {
manifold.normal = SHVec3::UnitY; manifold.normal = SHVec3::UnitY;
contact.position = CENTER_A; contact.localPointA = RADIUS_A * SHVec3::Rotate(manifold.normal, inverseTransformA.orientation);
contact.penetration = RADIUS_B; contact.localPointB = RADIUS_B * SHVec3::Rotate(manifold.normal, inverseTransformB.orientation);
manifold.contacts[numContacts++] = contact; manifold.contacts[numContacts++] = contact;
} }
else else
{ {
manifold.normal = SHVec3::Normalise(A_TO_B); manifold.normal = SHVec3::Normalise(A_TO_B);
contact.position = CENTER_B - manifold.normal * RADIUS_B; contact.localPointA = RADIUS_A * SHVec3::Normalise(inverseTransformA * CENTER_B);
contact.penetration = COMBINED_RADIUS - A_TO_B.Length(); contact.localPointB = RADIUS_B * SHVec3::Normalise(inverseTransformB * CENTER_A);
manifold.contacts[numContacts++] = contact; manifold.contacts[numContacts++] = contact;
} }

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

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

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

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