Program Listing for File physics_statics.hpp¶
↰ Return to documentation for file (/home/runner/work/Legion-Engine/Legion-Engine/legion/engine/physics/physics_statics.hpp
)
#pragma once
#include <core/core.hpp>
#include <physics/colliders/convexcollider.hpp>
#include <physics/data/pointer_encapsulator.hpp>
#include <physics/data/contact_vertex.hpp>
#include <Voro++/voro++.hh>
#include <rendering/debugrendering.hpp>
#include <physics/data/convex_convex_collision_info.hpp>
namespace legion::physics
{
typedef std::shared_ptr<PhysicsCollider> PhysicsColliderPtr;
struct HalfEdgeFace;
class PhysicsStatics
{
public:
//TODO move implementation to seperate cpp file
//---------------------------------------------------------------- Collision Detection ----------------------------------------------------------------------------//
static void DetectConvexConvexCollision(ConvexCollider* convexA, ConvexCollider* convexB
, const math::mat4& transformA, const math::mat4& transformB,
ConvexConvexCollisionInfo& outCollisionInfo, physics_manifold& manifold);
static void GetSupportPoint(const math::vec3& planePosition, const math::vec3& direction, ConvexCollider* collider, const math::mat4& colliderTransform
, math::vec3& worldSupportPoint)
{
float largestDistanceInDirection = std::numeric_limits<float>::lowest();
for (const auto& vert : collider->GetVertices())
{
math::vec3 transformedVert = colliderTransform * math::vec4(vert, 1);
float dotResult = math::dot(transformedVert - planePosition, direction);
if (dotResult > largestDistanceInDirection)
{
largestDistanceInDirection = dotResult;
worldSupportPoint = transformedVert;
}
}
}
static void GetSupportPointNoTransform( math::vec3 planePosition, math::vec3 direction, ConvexCollider* collider, const math::mat4& colliderTransform
, math::vec3& worldSupportPoint)
{
float largestDistanceInDirection = std::numeric_limits<float>::lowest();
planePosition = math::inverse(colliderTransform) * math::vec4(planePosition, 1);
direction = math::inverse(colliderTransform) * math::vec4(direction, 0);
for (const auto& vert : collider->GetVertices())
{
math::vec3 transformedVert = math::vec4(vert, 1);
float dotResult = math::dot(transformedVert - planePosition, direction);
if (dotResult > largestDistanceInDirection)
{
largestDistanceInDirection = dotResult;
worldSupportPoint = transformedVert;
}
}
worldSupportPoint = colliderTransform * math::vec4(worldSupportPoint, 1);
}
static float GetSupportPoint(const std::vector<math::vec3>& vertices, const math::vec3& direction, math::vec3& outVec);
static bool FindSeperatingAxisByExtremePointProjection(ConvexCollider* convexA
, ConvexCollider* convexB, const math::mat4& transformA, const math::mat4& transformB, PointerEncapsulator<HalfEdgeFace>&refFace, float& maximumSeperation,bool shouldDebug = false)
{
//shouldDebug = false;
float currentMaximumSeperation = std::numeric_limits<float>::lowest();
for (auto face : convexB->GetHalfEdgeFaces())
{
//log::debug("face->normal {} ", math::to_string( face->normal));
//get inverse normal
math::vec3 seperatingAxis = math::normalize(transformB * math::vec4((face->normal), 0));
math::vec3 transformedPositionB = transformB * math::vec4(face->centroid, 1);
//get extreme point of other face in normal direction
math::vec3 worldSupportPoint;
GetSupportPoint(transformedPositionB, -seperatingAxis,
convexA, transformA, worldSupportPoint);
float seperation = math::dot(worldSupportPoint - transformedPositionB, seperatingAxis);
if (seperation > currentMaximumSeperation)
{
currentMaximumSeperation = seperation;
refFace.ptr = face;
}
if (seperation > 0)
{
//we have found a seperating axis, we can exit early
maximumSeperation = currentMaximumSeperation;
return true;
}
}
//no seperating axis was found
maximumSeperation = currentMaximumSeperation;
return false;
}
static bool FindSeperatingAxisByGaussMapEdgeCheck(ConvexCollider* convexA, ConvexCollider* convexB,
const math::mat4& transformA, const math::mat4& transformB, PointerEncapsulator<HalfEdgeEdge>& refEdge, PointerEncapsulator<HalfEdgeEdge>& incEdge,
math::vec3& seperatingAxisFound, float& maximumSeperation, bool shouldDebug = false);
static bool DetectConvexSphereCollision(ConvexCollider* convexA, const math::mat4& transformA, math::vec3 sphereWorldPosition, float sphereRadius,
float& maximumSeperation);
static std::pair< math::vec3,math::vec3> ConstructAABBFromPhysicsComponentWithTransform
(ecs::component_handle<physicsComponent> physicsComponentToUse, const math::mat4& transform);
static float GetPhysicsComponentSupportPointAtDirection(math::vec3 direction,physicsComponent& physicsComponentToUse);
static std::pair< math::vec3, math::vec3> ConstructAABBFromVertices(const std::vector<math::vec3>& vertices);
static std::pair< math::vec3, math::vec3> ConstructAABBFromTransformedVertices(const std::vector<math::vec3>& vertices,const math::mat4& transform);
static std::pair<math::vec3, math::vec3> CombineAABB(const std::pair<math::vec3, math::vec3>& first, const std::pair<math::vec3, math::vec3>& second);
//---------------------------------------------------------- Polyhedron Clipping ----------------------------------------------------------------------------//
static void SutherlandHodgmanFaceClip( math::vec3& planeNormal, math::vec3& planePosition,
std::vector<ContactVertex>& inputList, std::vector<ContactVertex>& outputList, HalfEdgeEdge* clippingEdge)
{
for (size_t i = 0; i < inputList.size(); i++)
{
ContactVertex& currentVertex = inputList.at(i);
ContactVertex& nextVertex = i + 1 >= inputList.size() ? inputList.at(0) : inputList.at(i + 1);
float currentVertDistFromPlane = PointDistanceToPlane(planeNormal, planePosition, currentVertex.position);
float nextVertDistFromPlane = PointDistanceToPlane(planeNormal, planePosition, nextVertex.position);
//at a certain threshold, the vertices can be considered to be on the plane for numerical robustness reasons
if (math::abs(currentVertDistFromPlane) < constants::sutherlandHodgmanClippingThreshold
|| math::abs(nextVertDistFromPlane) < constants::sutherlandHodgmanClippingThreshold)
{
outputList.push_back(nextVertex);
continue;
}
bool isCurrentVertexUnderPlane = currentVertDistFromPlane < 0.0f;
bool isNextVertexUnderPlane = nextVertDistFromPlane < 0.0f;
//we always check clipping with a line that goes from the point below the plane to the point outside the plane.
//We do this mostly for numerical robustness reasons.
const math::vec3& pointAbovePlane = isCurrentVertexUnderPlane ? nextVertex.position : currentVertex.position;
const math::vec3& pointBelowPlane = isCurrentVertexUnderPlane ? currentVertex.position : nextVertex.position;
if (isCurrentVertexUnderPlane && isNextVertexUnderPlane)
{
//send next vertex to outputlist
outputList.push_back(nextVertex);
}
else if (!isCurrentVertexUnderPlane && isNextVertexUnderPlane)
{
//send next vertex to outputlist and the intersection between the line and the plane
math::vec3 intersectionPoint;
if (FindLineToPlaneIntersectionPoint(planeNormal, planePosition,
pointBelowPlane, pointAbovePlane, intersectionPoint))
{
auto currentVertexLabel = currentVertex.label;
auto nextVertexLabel = clippingEdge->label;
EdgeLabel label(currentVertexLabel.firstEdge, nextVertexLabel.nextEdge);
outputList.push_back(ContactVertex(intersectionPoint, label));
}
outputList.push_back(nextVertex);
}
else if (isCurrentVertexUnderPlane && !isNextVertexUnderPlane)
{
//send intersection between the line and the plane
math::vec3 intersectionPoint;
if (FindLineToPlaneIntersectionPoint(planeNormal, planePosition,
pointBelowPlane, pointAbovePlane, intersectionPoint))
{
auto currentVertexLabel = currentVertex.label;
auto nextVertexLabel = clippingEdge->label;
EdgeLabel label(currentVertexLabel.firstEdge, nextVertexLabel.nextEdge);
outputList.push_back(ContactVertex(intersectionPoint, label));
}
}
}
}
//------------------------------------------------------------ helper functions -----------------------------------------------------------------------//
static void FindClosestPointsToLineSegment(math::vec3 p1, math::vec3 p2,
math::vec3 p3, math::vec3 p4,math::vec3& outp1p2, math::vec3& outp3p4)
{
//------------------find the interpolants of both lines that represent the closest points of the line segments-----------//
//Given the 2 closest points of the given 2 line segments, L1(x) and L2(y), where L1 is the line created from the points
//p1 and p2, and L2 is the line created from the points p3 and p4. and x and y are the interpolants, We know that these
//2 points create a line that is perpendicular to both the lines of L1 and L2. Therefore:
// (L1(x) - L2(y)) . (p2 - p1) = 0
// (L1(x) - L2(y)) . (p4 - p3) = 0
//these 2 function can be simplified into a linear system of 2 variables.
// ax + cy = e
// bx + dy = f
//where a,b,c,d,e,f are equal to the following:
float a, b, c, d, e, f;
// ( [p2] ^ 2 - [p1] ^ 2)
a = math::pow2(p2.x - p1.x) +
math::pow2(p2.y - p1.y) +
math::pow2(p2.z - p1.z);
// ([p4] - [p3])([p2] - [p1])
b = (p4.x - p3.x) * (p2.x - p1.x) +
(p4.y - p3.y) * (p2.y - p1.y) +
(p4.z - p3.z) * (p2.z - p1.z);
// ([p4] - [p3])([p2] - [p1])
c = b;
// ( [p4] ^ 2 - [p3] ^ 2)
d = (math::pow2(p4.x - p3.x) +
math::pow2(p4.y - p3.y) +
math::pow2(p4.z - p3.z));
//([p3] - [p1])([p2] - [p1])
e = (p3.x - p1.x) * (p2.x - p1.x) +
(p3.y - p1.y) * (p2.y - p1.y) +
(p3.z - p1.z) * (p2.z - p1.z);
//([p4] - [p3])([p3] - [p1])
f = (p4.x - p3.x) * (p3.x - p1.x) +
(p4.y - p3.y) * (p3.y - p1.y) +
(p4.z - p3.z) * (p3.z - p1.z);
math::vec2 interpolantVector = LinearSystemCramerSolver2D(a, b, c, d, e, f);
outp1p2 = p1 + (p2 - p1) * math::clamp(interpolantVector.x,0.0f,1.0f);
outp3p4 = p3 + (p4 - p3) * math::clamp(interpolantVector.y,0.0f,1.0f);
}
static math::vec2 LinearSystemCramerSolver2D(float a, float b, float c, float d, float e, float f)
{
//[ a c ] [x] [e]
//[ b d ] [y] [f]
float denom = (a * d) - (c * b);
//[ e c ]
//[ f d ]
float x = ((e * d) - (c * f)) / denom;
//[ a e ]
//[ b f ]
float y = ((a * f) - (e * b)) / denom;
return math::vec2(x,-y);
}
static float PointDistanceToPlane(const math::vec3& planeNormal, const math::vec3& planePosition, const math::vec3& point)
{
return math::dot(point - planePosition, planeNormal);
}
static bool IsPointAbovePlane(const math::vec3& planeNormal, const math::vec3& planePosition, const math::vec3& point)
{
return PointDistanceToPlane(planeNormal, planePosition, point) > 0.0f;
}
static bool FindLineToPlaneIntersectionPoint(const math::vec3& planeNormal, const math::vec3& planePosition,
const math::vec3& startPoint, const math::vec3& endPoint, math::vec3& intersectionPoint)
{
math::vec3 direction = endPoint - startPoint;
if (math::epsilonEqual(math::dot(direction, planeNormal), 0.0f, math::epsilon<float>()))
{
return false;
}
float t = FindLineToPointInterpolant(startPoint, endPoint, planePosition, planeNormal);
intersectionPoint = startPoint + direction * t;
return true;
}
static bool FindLineToPlaneIntersectionPoint(const math::vec3& planeNormal, const math::vec3& planePosition,
const math::vec3& startPoint, const math::vec3& endPoint, math::vec3& intersectionPoint,float& interpolant)
{
math::vec3 direction = endPoint - startPoint;
if (math::epsilonEqual(math::dot(direction, planeNormal), 0.0f, math::epsilon<float>()))
{
return false;
}
interpolant = FindLineToPointInterpolant(startPoint, endPoint, planePosition, planeNormal);
intersectionPoint = startPoint + direction * interpolant;
return true;
}
static float FindLineToPointInterpolant(const math::vec3& startPoint, const math::vec3& endPoint, const math::vec3& planePosition,
const math::vec3& planeNormal)
{
return math::dot(planePosition - startPoint, planeNormal) / math::dot(endPoint - startPoint, planeNormal);
}
static std::vector<std::vector<math::vec4>> GenerateVoronoi(std::vector<math::vec3> points,math::vec2 xRange, math::vec2 yRange,
math::vec2 zRange, math::vec3 containerResolution, bool xPeriodic = false, bool yPeriodic = false, bool zPeriodic = false, int initMem = 8)
{
return GenerateVoronoi(points,xRange.x,xRange.y,yRange.x,yRange.y,zRange.x,zRange.y,containerResolution.x,containerResolution.y,containerResolution.z,xPeriodic,yPeriodic,zPeriodic,initMem);
}
static std::vector<std::vector<math::vec4>> GenerateVoronoi(std::vector<math::vec3> points,const double xMin = -5, const double xMax = 5, const double yMin = -5, const double yMax = 5, const double zMin = -5, const double zMax = 5,const double conResX = 10,const double conResY = 10 , const double conResZ = 10,bool xPeriodic = false,bool yPeriodic = false,bool zPeriodic = false,int initMem = 8)
{
voro::container con(xMin, xMax, yMin, yMax, zMin, zMax, conResX, conResY, conResZ, xPeriodic, yPeriodic, zPeriodic, initMem);
return GenerateVoronoi(con,points);
}
static std::vector<std::vector<math::vec4>> GenerateVoronoi(voro::container& con,std::vector<math::vec3> points)
{
int i = 0;
for (math::vec3 point : points)
{
con.put(i,point.x,point.y,point.z);
i++;
}
return GenerateVoronoi(con);
}
static std::vector<std::vector<math::vec4>> GenerateVoronoi(voro::container& con)
{
con.draw_cells_json("assets/voronoi/output/cells.json");
std::ifstream f("assets/voronoi/output/cells.json");
return serialization::SerializationUtil::JSONDeserialize< std::vector<std::vector<math::vec4>>>(f);
}
static bool CollideAABB(const math::vec3 low0, const math::vec3 high0, const math::vec3 low1, const math::vec3 high1)
{
return low0.x <= high1.x && high0.x >= low1.x &&
low0.y <= high1.y && high0.y >= low1.y
&& low0.z <= high1.z && high0.z >= low1.z;
}
static bool CollideAABB(const std::pair<math::vec3, math::vec3> col0, const std::pair<math::vec3, math::vec3> col1)
{
auto& [low0, high0] = col0;
auto& [low1, high1] = col1;
return CollideAABB(low0, high0, low1, high1);
}
private:
static bool attemptBuildMinkowskiFace(HalfEdgeEdge* edgeA, HalfEdgeEdge* edgeB, const math::mat4& transformA,
const math::mat4& transformB)
{
//TODO the commmented parts are technically more robust and should work but somehow dont, figure out why.
const math::vec3 transformedA1 = transformA *
math::vec4(edgeA->getLocalNormal(), 0);
const math::vec3 transformedA2 = transformA *
math::vec4(edgeA->pairingEdge->getLocalNormal(), 0);
const math::vec3 transformedEdgeDirectionA = math::cross(transformedA1, transformedA2);
//transformA * math::vec4(edgeA->getLocalEdgeDirection(), 0);
const math::vec3 transformedB1 = transformB *
math::vec4(edgeB->getLocalNormal(), 0);
const math::vec3 transformedB2 = transformB *
math::vec4(edgeB->pairingEdge->getLocalNormal(), 0);
const math::vec3 transformedEdgeDirectionB = math::cross(-transformedB1, -transformedB2);
//transformB *math::vec4(edgeB->getLocalEdgeDirection(), 0);
math::vec3 positionA = transformA * math::vec4(edgeA->edgePosition, 1);
return isMinkowskiFace(transformedA1, transformedA2, -transformedB1, -transformedB2
, transformedEdgeDirectionA, transformedEdgeDirectionB);
}
static bool isMinkowskiFace(const math::vec3& transformedA1, const math::vec3& transformedA2,
const math::vec3& transformedB1, const math::vec3& transformedB2
,const math::vec3& planeANormal, const math::vec3& planeBNormal)
{
//------------------------ Check if normals created by arcA seperate normals of B --------------------------------------//
//CBA
float planeADotB1 = math::dot(planeANormal, transformedB1);
//DBA
float planeADotB2 = math::dot(planeANormal, transformedB2);
float dotMultiplyResultA =
planeADotB1 * planeADotB2;
//log::debug("dotMultiplyResultA {}", dotMultiplyResultA);
if (dotMultiplyResultA > 0.0f )
{
return false;
}
//------------------------ Check if normals created by arcB seperate normals of A --------------------------------------//
//ADC
float planeBDotA1 = math::dot(planeBNormal, transformedA1);
//BDC
float planeBDotA2 = math::dot(planeBNormal, transformedA2);
float dotMultiplyResultB = planeBDotA1 * planeBDotA2;
//log::debug("dotMultiplyResultB {}", dotMultiplyResultB);
if (dotMultiplyResultB > 0.0f )
{
return false;
}
//------------------------ Check if arcA and arcB are in the same hemisphere --------------------------------------------//
float dotMultiplyResultAB = planeADotB1 * planeBDotA2;
//log::debug("dotMultiplyResultAB {}", dotMultiplyResultAB);
if (planeADotB1 * planeBDotA2 < 0.0f)
{
return false;
}
return true;
}
};
}