ecker/engine
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

generate proper-ish contacts for triangleAabb/triangleObb, code cleanup

Browse Source
This commit is contained in:
ecker 2026-05-19 15:01:38 -05:00
parent a1f62ef0c3
commit f054874fed
10 changed files with 394 additions and 825 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
bin/data/config.json
View File

@ -337,11 +337,11 @@
}, },
"physics": { "physics": {
"warmup solver": false, "warmup solver": false,
"block solver": true, "block solver": false,
"psg solver": true, "psg solver": false,
"correction percent": 0.0, "correction percent": 0.01,
"gjk": false, "gjk": false,
"debug draw": false, "debug draw": true,
"fixed step": true, "fixed step": true,
"substeps": 4, "substeps": 4,
"solver iterations": 10 "solver iterations": 10

17
engine/inc/uf/utils/math/physics/common.h
View File

@ -12,17 +12,22 @@ namespace impl {
pod::Transform<> getTransform( const pod::PhysicsBody& body ); pod::Transform<> getTransform( const pod::PhysicsBody& body );
pod::Vector3f getPosition( const pod::PhysicsBody& body, bool useTransform = false ); pod::Vector3f getPosition( const pod::PhysicsBody& body, bool useTransform = false );
pod::PhysicsBody physicsBodyHullView( const pod::PhysicsBody& body, int32_t index = -1 ); pod::PhysicsBody physicsBodyHullView( const pod::PhysicsBody& body, int32_t index = -1 );
pod::PhysicsBody physicsBodyTriView( const pod::PhysicsBody& body, const pod::TriangleWithNormal triangle ); pod::PhysicsBody physicsBodyTriView( const pod::TriangleWithNormal triangle, const pod::PhysicsBody& body = {} );
pod::PhysicsBody physicsBodyTriView( const pod::PhysicsBody& body, size_t triID ); pod::PhysicsBody physicsBodyTriView( const pod::PhysicsBody& body, size_t triID );
bool shouldCollide( const pod::Collider& a, const pod::Collider& b ); bool shouldCollide( const pod::Collider& a, const pod::Collider& b );
bool shouldCollide( const pod::PhysicsBody& a, const pod::PhysicsBody& b ); bool shouldCollide( const pod::PhysicsBody& a, const pod::PhysicsBody& b );
pod::Matrix3f computeWorldInverseInertia( const pod::PhysicsBody& b ); pod::Matrix3f computeWorldInverseInertia( const pod::PhysicsBody& b );
pod::Vector3f normalizeDelta( const pod::Vector3f& delta, float dist, const pod::Vector3f& fallback = pod::Vector3f{0,1,0} ); pod::Vector3f normalizeDelta( const pod::Vector3f& delta, float dist, const pod::Vector3f& fallback = pod::Vector3f{0,1,0} );
pod::Vector3f computeTangent( const pod::Vector3f& normal ); pod::Vector3f computeTangent( const pod::Vector3f& normal );
pod::Vector3f closestPointOnSegment( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b ); pod::Vector3f closestPointOnSegment( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b );
std::pair<pod::Vector3f, pod::Vector3f> closestSegmentSegment( const pod::Vector3f& A, const pod::Vector3f& B, const pod::Vector3f& C, const pod::Vector3f& D ); std::pair<pod::Vector3f, pod::Vector3f> closestSegmentSegment( const pod::Vector3f& A, const pod::Vector3f& B, const pod::Vector3f& C, const pod::Vector3f& D );
pod::Vector3f closestPointSegmentAabb( const pod::Vector3f& p1, const pod::Vector3f& p2, const pod::AABB& box ); pod::Vector3f closestPointSegmentAabb( const pod::Vector3f& p1, const pod::Vector3f& p2, const pod::AABB& box );
pod::Vector3f computeBarycentric( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b, const pod::Vector3f& c, bool clamps = false ); pod::Vector3f computeBarycentric( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b, const pod::Vector3f& c, bool clamps = false );
pod::Vector3f computeBarycentric(const pod::Vector3f& p, const pod::Triangle& tri, bool clamps = false ); pod::Vector3f computeBarycentric(const pod::Vector3f& p, const pod::Triangle& tri, bool clamps = false );
pod::Vector3f interpolateWithBarycentric( const pod::Vector3f& bary, const pod::Vector3f a, const pod::Vector3f b, const pod::Vector3f c ); pod::Vector3f interpolateWithBarycentric( const pod::Vector3f& bary, const pod::Vector3f a, const pod::Vector3f b, const pod::Vector3f c );
@ -33,11 +38,13 @@ namespace impl {
pod::Vector3f closestPointOnTriangle( const pod::Vector3f& p, const pod::Triangle& tri ); pod::Vector3f closestPointOnTriangle( const pod::Vector3f& p, const pod::Triangle& tri );
pod::Vector3f orientNormalToAB( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Vector3f n ); pod::Vector3f orientNormalToAB( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Vector3f n );
float segmentTriangleDistanceSq( const pod::Vector3f& p0, const pod::Vector3f& p1, const pod::Triangle& tri, pod::Vector3f& outSeg, pod::Vector3f& outTri ); float segmentTriangleDistanceSq( const pod::Vector3f& p0, const pod::Vector3f& p1, const pod::Triangle& tri, pod::Vector3f& outSeg, pod::Vector3f& outTri );
int clipPolygonAgainstPlane( const pod::Vector3f* inPoly, int inCount, const pod::Vector3f& planeNormal, float planeOffset, pod::Vector3f* outPoly ); bool testSeparatingAxis( const pod::Triangle& triangle, const pod::AABB& box, const pod::Vector3f& axis, const pod::Vector3f axes[3], float& outMinOverlap, pod::Vector3f& outBestAxis );
void clipPolygon( pod::Vector3f* poly, int& polyCount, const pod::Plane& plane );
void clipPolygon( pod::Vector3f* poly, int& polyCount, const pod::AABB& plane );
pod::Vector3f triangleCenter( const pod::Triangle& tri ); pod::Vector3f triangleCenter( const pod::Triangle& tri );
pod::Vector3f triangleNormal( const pod::Triangle& tri ); pod::Vector3f triangleNormal( const pod::Triangle& tri );
pod::Vector3f triangleNormal( const pod::TriangleWithNormal& tri ); pod::Vector3f triangleNormal( const pod::TriangleWithNormal& tri );
bool triangleTriangleIntersect( const pod::Triangle& a, const pod::Triangle& b );
size_t getIndex( const void* pointer, size_t stride, size_t index ); size_t getIndex( const void* pointer, size_t stride, size_t index );
size_t getIndex( const uf::Mesh::View& view, const uf::Mesh::AttributeView& indices, size_t index ); size_t getIndex( const uf::Mesh::View& view, const uf::Mesh::AttributeView& indices, size_t index );
pod::Vector3f getVertex( const uf::Mesh::View& view, const uf::Mesh::AttributeView& positions, size_t index ); pod::Vector3f getVertex( const uf::Mesh::View& view, const uf::Mesh::AttributeView& positions, size_t index );
@ -45,8 +52,6 @@ namespace impl {
/*FORCE_INLINE*/ pod::Triangle fetchTriangle( const uf::Mesh::View& view, size_t triID ); /*FORCE_INLINE*/ pod::Triangle fetchTriangle( const uf::Mesh::View& view, size_t triID );
pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID ); pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID );
pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID, const pod::PhysicsBody& body ); pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID, const pod::PhysicsBody& body );
bool computeTriangleTriangleSegment( const pod::TriangleWithNormal& A, const pod::TriangleWithNormal& B, pod::Vector3f& p0, pod::Vector3f& p1 );
pod::Vector2f projectTriangleOntoAxis( const pod::TriangleWithNormal& tri, const pod::Vector3f& axis );
/*FORCE_INLINE*/ bool aabbOverlap( const pod::AABB& a, const pod::AABB& b ); /*FORCE_INLINE*/ bool aabbOverlap( const pod::AABB& a, const pod::AABB& b );
/*FORCE_INLINE*/ float aabbSurfaceArea( const pod::AABB& aabb ); /*FORCE_INLINE*/ float aabbSurfaceArea( const pod::AABB& aabb );
@ -61,7 +66,5 @@ namespace impl {
pod::AABB transformAabbToWorld( const pod::AABB& localBox, const pod::Transform<>& transform ); pod::AABB transformAabbToWorld( const pod::AABB& localBox, const pod::Transform<>& transform );
std::pair<pod::Vector3f, pod::Vector3f> getCapsuleSegment( const pod::PhysicsBody& body ); std::pair<pod::Vector3f, pod::Vector3f> getCapsuleSegment( const pod::PhysicsBody& body );
pod::AABB computeAABB( const pod::PhysicsBody& body ); pod::AABB computeAABB( const pod::PhysicsBody& body );
float triAabbDistanceSq( const pod::Triangle& tri, const pod::AABB& box );
bool triAabbOverlap( const pod::Triangle& tri, const pod::AABB& box );
pod::AABB transformAabbToLocal( const pod::AABB& box, const pod::Transform<>& transform ); pod::AABB transformAabbToLocal( const pod::AABB& box, const pod::Transform<>& transform );
} }

2
engine/inc/uf/utils/math/vector.h
View File

@ -131,6 +131,8 @@ namespace uf {
template<typename T> /*FORCE_INLINE*/ T /*UF_API*/ abs( const T& vector ); // gets the absolute value of a vector template<typename T> /*FORCE_INLINE*/ T /*UF_API*/ abs( const T& vector ); // gets the absolute value of a vector
template<typename T> /*FORCE_INLINE*/ T /*UF_API*/ min( const T& left, const T& right ); // returns the minimum of each component between two vectors template<typename T> /*FORCE_INLINE*/ T /*UF_API*/ min( const T& left, const T& right ); // returns the minimum of each component between two vectors
template<typename T> /*FORCE_INLINE*/ T /*UF_API*/ max( const T& left, const T& right ); // returns the maximum of each component between two vectors template<typename T> /*FORCE_INLINE*/ T /*UF_API*/ max( const T& left, const T& right ); // returns the maximum of each component between two vectors
template<typename T> /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ min( const T& v ); // returns the minimum component of a vector
template<typename T> /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ max( const T& v ); // returns the maximum component of a vector
template<typename T> /*FORCE_INLINE*/ T /*UF_API*/ clamp( const T& vector, const T& min, const T& max ); // clamps a vector between two bounds template<typename T> /*FORCE_INLINE*/ T /*UF_API*/ clamp( const T& vector, const T& min, const T& max ); // clamps a vector between two bounds
template<typename T> /*FORCE_INLINE*/ T /*UF_API*/ ceil( const T& vector ); // rounds each component of the the vector up template<typename T> /*FORCE_INLINE*/ T /*UF_API*/ ceil( const T& vector ); // rounds each component of the the vector up
template<typename T> /*FORCE_INLINE*/ T /*UF_API*/ floor( const T& vector ); // rounds each component of the vector down template<typename T> /*FORCE_INLINE*/ T /*UF_API*/ floor( const T& vector ); // rounds each component of the vector down

16
engine/inc/uf/utils/math/vector/pod.inl
View File

@ -486,6 +486,14 @@ T& uf::vector::normalize_( T& vector ) {
return ( norm < 1.0e-6 ) ? T{} : ( vector = uf::vector::divide((const T&) vector, norm) ); return ( norm < 1.0e-6 ) ? T{} : ( vector = uf::vector::divide((const T&) vector, norm) );
} }
template<typename T> template<typename T>
typename T::type_t uf::vector::min( const T& v ) {
typename T::type_t res = v[0];
FOR_EACH(T::size - 1, {
res = std::min( res, v[i + 1] );
});
return res;
}
template<typename T>
T uf::vector::min( const T& left, const T& right ) { T uf::vector::min( const T& left, const T& right ) {
#if UF_USE_SIMD #if UF_USE_SIMD
if constexpr ( simd_able_v<typename T::type_t> ) { if constexpr ( simd_able_v<typename T::type_t> ) {
@ -499,6 +507,14 @@ T uf::vector::min( const T& left, const T& right ) {
return res; return res;
} }
template<typename T> template<typename T>
typename T::type_t uf::vector::max( const T& v ) {
typename T::type_t res = v[0];
FOR_EACH(T::size - 1, {
res = std::max( res, v[i+1] );
});
return res;
}
template<typename T>
T uf::vector::max( const T& left, const T& right ) { T uf::vector::max( const T& left, const T& right ) {
#if UF_USE_SIMD #if UF_USE_SIMD
if constexpr ( simd_able_v<typename T::type_t> ) { if constexpr ( simd_able_v<typename T::type_t> ) {

468
engine/inc/uf/utils/mesh/clip.h
View File

@ -6,401 +6,121 @@
namespace uf { namespace uf {
namespace shapes { namespace shapes {
inline float planePointSide( const pod::Plane& plane, const pod::Vector3f& point ) { template <typename T>
return uf::vector::dot( point, plane.normal ) - plane.offset; void clipPolygon( T* poly, int& polyCount, const pod::Plane& plane ) {
} if ( polyCount == 0 ) return;
inline float planeEdgeIntersection( const pod::Plane& plane, const pod::Vector3f& p1, const pod::Vector3f& p2 ) { int outCount = 0;
return (plane.offset - uf::vector::dot(p1, plane.normal)) / uf::vector::dot(uf::vector::subtract(p2, p1), plane.normal); T out[16];
}
template<typename T, typename U = uint32_t> for ( auto i = 0; i < polyCount; i++ ) {
uf::stl::vector<pod::Polygon<T>> verticesPolygonate( const uf::stl::vector<T>& vertices, const uf::stl::vector<U>& indices ) { const T& curr = poly[i];
uf::stl::vector<pod::Polygon<T>> polygons; const T& prev = poly[(i + polyCount - 1) % polyCount];
polygons.reserve( indices.size() / 3 );
for ( auto i = 0; i < indices.size(); i += 3 ) { float dCurr = uf::vector::dot(plane.normal, curr.position) - plane.offset;
polygons.emplace_back( pod::Polygon<T>{ { vertices[indices[i+0]], vertices[indices[i+1]], vertices[indices[i+2]] } }); float dPrev = uf::vector::dot(plane.normal, prev.position) - plane.offset;
}
return polygons; if ( dCurr >= 0.0f ) {
} if ( dPrev < 0.0f ) {
float t = dPrev / (dPrev - dCurr);
template<typename T> out[outCount++] = T::interpolate( prev, curr, t );
uf::stl::vector<pod::Polygon<T>> verticesPolygonate( const uf::stl::vector<T>& vertices ) {
uf::stl::vector<pod::Polygon<T>> polygons;
polygons.reserve( vertices.size() / 3 );
for ( auto i = 0; i < vertices.size(); i += 3 ) {
polygons.emplace_back( pod::Polygon<T>{ { vertices[i+0], vertices[i+1], vertices[i+2] } });
}
return polygons;
}
template<typename T>
uf::stl::vector<T> polygonsTriangulate( const uf::stl::vector<pod::Polygon<T>>& polygons ) {
uf::stl::vector<T> vertices;
vertices.reserve( polygons.size() * 3 );
for ( const auto& polygon : polygons ) {
for (size_t i = 1; i < polygon.points.size() - 1; ++i) {
vertices.emplace_back(polygon.points[0]);
vertices.emplace_back(polygon.points[i]);
vertices.emplace_back(polygon.points[i + 1]);
}
}
// rewind
for ( auto i = 0; i < vertices.size(); i += 3 ) {
for ( const auto& polygon : polygons ) {
const auto& p0 = vertices[i+0];
const auto& p1 = vertices[i+1];
const auto& p2 = vertices[i+2];
auto edge1 = p1.position - p0.position;
auto edge2 = p2.position - p0.position;
auto polygonNormal = uf::vector::cross(edge1, edge2);
if ( uf::vector::dot( polygonNormal, p0.normal ) < 0 ) {
std::swap( vertices[i+1], vertices[i+2] );
}
}
}
return vertices;
}
template<typename T, typename U = uint32_t>
uf::stl::vector<U> verticesIndex( const uf::stl::vector<T>& vertices ) {
uf::stl::vector<U> indices;
indices.reserve( vertices.size() );
for ( auto i = 0; i < vertices.size(); ++i ) {
indices.emplace_back( i );
}
return indices;
}
template<typename T, typename U = uint32_t>
uf::stl::vector<T> indexedVertices( const uf::stl::vector<T>& vertices, const uf::stl::vector<U>& indices ) {
uf::stl::vector<T> newVertices;
for ( auto idx : indices ) {
newVertices.emplace_back( vertices[idx] );
}
return newVertices;
}
// to-do: fix why these causes problems
// the polygon approach works fine enough
template<typename T>
uf::stl::vector<T> verticesPlaneClip( const uf::stl::vector<T>& vertices, const pod::Plane& plane ) {
uf::stl::vector<T> newVertices;
for ( auto i = 0; i < vertices.size(); i += 3 ) {
const auto& v0 = vertices[i+0];
const auto& v1 = vertices[i+1];
const auto& v2 = vertices[i+2];
/*
float s0 = uf::shapes::planePointSide( plane, v0.position ) > 0;
float s1 = uf::shapes::planePointSide( plane, v1.position ) > 0;
float s2 = uf::shapes::planePointSide( plane, v2.position ) > 0;
if ( s0 == s1 && s1 == s2 ) {
newVertices.emplace_back( v0 );
newVertices.emplace_back( v1 );
newVertices.emplace_back( v2 );
continue;
}
if ( s0 != s1 ) {
float t = planeEdgeIntersection( plane, v0.position, v1.position );
T intersection = T::interpolate( v0, v1, t );
if ( s0 ) {
newVertices.emplace_back( v0 );
newVertices.emplace_back( intersection );
newVertices.emplace_back( v1 );
} else {
newVertices.emplace_back( v1 );
newVertices.emplace_back( intersection );
newVertices.emplace_back( v0 );
}
}
if ( s1 != s2 ) {
float t = planeEdgeIntersection( plane, v1.position, v2.position );
T intersection = T::interpolate( v1, v2, t );
if ( s1 ) {
newVertices.emplace_back( v1 );
newVertices.emplace_back( intersection );
newVertices.emplace_back( v2 );
} else {
newVertices.emplace_back( v2 );
newVertices.emplace_back( intersection );
newVertices.emplace_back( v1 );
}
}
if ( s2 != s0 ) {
float t = planeEdgeIntersection( plane, v2.position, v0.position );
T intersection = T::interpolate( v2, v0, t );
if ( s2 ) {
newVertices.emplace_back( v2 );
newVertices.emplace_back( intersection );
newVertices.emplace_back( v0 );
} else {
newVertices.emplace_back( v0 );
newVertices.emplace_back( intersection );
newVertices.emplace_back( v2 );
}
}
*/
float d0 = uf::shapes::planePointSide( plane, v0.position );
float d1 = uf::shapes::planePointSide( plane, v1.position );
float d2 = uf::shapes::planePointSide( plane, v2.position );
if (d0 < 0 && d1 < 0 && d2 < 0) {
continue;
}
if (d0 >= 0 && d1 >= 0 && d2 >= 0) {
newVertices.emplace_back( v0 );
newVertices.emplace_back( v1 );
newVertices.emplace_back( v2 );
continue;
}
{
// Some vertices are in front and some are behind
bool edge0Crosses = (d0 >= 0) != (d1 >= 0);
bool edge1Crosses = (d1 >= 0) != (d2 >= 0);
bool edge2Crosses = (d2 >= 0) != (d0 >= 0);
T intersection01;
T intersection12;
T intersection20;
if ( edge0Crosses ) {
auto t = planeEdgeIntersection( plane, v0.position, v1.position );
intersection01 = T::interpolate( v0, v1, t );
}
if ( edge1Crosses ) {
auto t = planeEdgeIntersection( plane, v1.position, v2.position );
intersection12 = T::interpolate( v1, v2, t );
}
if ( edge2Crosses ) {
auto t = planeEdgeIntersection( plane, v2.position, v0.position );
intersection20 = T::interpolate( v2, v0, t );
}
if (edge0Crosses && edge1Crosses && edge2Crosses) {
// All edges cross the plane: split into three triangles
newVertices.emplace_back( intersection01 );
newVertices.emplace_back( intersection12 );
newVertices.emplace_back( intersection20 );
} else if (edge0Crosses && edge1Crosses) {
// Edges 0 and 1 cross: create a quad
newVertices.emplace_back( intersection01 );
newVertices.emplace_back( intersection12 );
newVertices.emplace_back( v1 );
newVertices.emplace_back( intersection01 );
newVertices.emplace_back( v1 );
newVertices.emplace_back( v0 );
} else if (edge1Crosses && edge2Crosses) {
// Edges 1 and 2 cross: create a quad
newVertices.emplace_back( intersection12 );
newVertices.emplace_back( intersection20 );
newVertices.emplace_back( v2 );
newVertices.emplace_back( intersection12 );
newVertices.emplace_back( v2 );
newVertices.emplace_back( v1 );
} else if (edge2Crosses && edge0Crosses) {
// Edges 2 and 0 cross: create a quad
newVertices.emplace_back( intersection20 );
newVertices.emplace_back( intersection01 );
newVertices.emplace_back( v0 );
newVertices.emplace_back( intersection20 );
newVertices.emplace_back( v0 );
newVertices.emplace_back( v2 );
} else if (edge0Crosses) {
// Only edge 0 crosses: create a triangle
newVertices.emplace_back( intersection01 );
newVertices.emplace_back( v1 );
newVertices.emplace_back( v0 );
} else if (edge1Crosses) {
// Only edge 1 crosses: create a triangle
newVertices.emplace_back( intersection12 );
newVertices.emplace_back( v2 );
newVertices.emplace_back( v1 );
} else if (edge2Crosses) {
// Only edge 2 crosses: create a triangle
newVertices.emplace_back( intersection20 );
newVertices.emplace_back( v0 );
newVertices.emplace_back( v2 );
} }
out[outCount++] = curr;
} else if ( dPrev >= 0.0f ) {
float t = dPrev / (dPrev - dCurr);
out[outCount++] = T::interpolate( prev, curr, t );
} }
} }
return newVertices; polyCount = outCount;
for ( auto i = 0; i < outCount; i++ ) poly[i] = out[i];
} }
template<typename T> template <typename T>
pod::Polygon<T> polygonPlaneClip( const pod::Polygon<T>& polygon, const pod::Plane& plane ) { void clipPolygon( T* poly, int& polyCount, const pod::AABB& bounds ) {
pod::Polygon<T> clipped; pod::Plane planes[6] = {
{ pod::Vector3f{-1, 0, 0}, -bounds.max.x },
auto count = polygon.points.size(); { pod::Vector3f{ 1, 0, 0}, bounds.min.x },
bool modified = false; { pod::Vector3f{ 0,-1, 0}, -bounds.max.y },
for (auto i = 0; i < count; ++i) { { pod::Vector3f{ 0, 1, 0}, bounds.min.y },
const T& current = polygon.points[i]; { pod::Vector3f{ 0, 0,-1}, -bounds.max.z },
const T& previous = polygon.points[(i - 1 + count) % count]; { pod::Vector3f{ 0, 0, 1}, bounds.min.z }
float currentSide = uf::shapes::planePointSide( plane, current.position );
float previousSide = uf::shapes::planePointSide( plane, previous.position );
if (currentSide >= 0) {
if (previousSide < 0) {
float t = uf::shapes::planeEdgeIntersection( plane, previous.position, current.position );
T interpolated = T::interpolate( previous, current, t );
clipped.points.emplace_back(interpolated);
modified = true;
}
clipped.points.emplace_back(current);
} else if (previousSide >= 0) {
float t = uf::shapes::planeEdgeIntersection( plane, previous.position, current.position );
T interpolated = T::interpolate( previous, current, t );
clipped.points.emplace_back(interpolated);
modified = true;
}
}
if ( modified ) {
std::reverse( clipped.points.begin(), clipped.points.end() );
}
return clipped;
}
template<typename T>
uf::stl::vector<pod::Polygon<T>>& clip(
uf::stl::vector<pod::Polygon<T>>& polygons,
const pod::Vector3f& min,
const pod::Vector3f& max
) {
// inverted because its needed
uf::stl::vector<pod::Plane> planes = {
// Right plane: x = max.x
{ pod::Vector3f{-1, 0, 0}, -max.x },
// Left plane: x = min.x
{ pod::Vector3f{1, 0, 0}, min.x },
// Top plane: y = max.y
{ pod::Vector3f{0, -1, 0}, -max.y },
// Bottom plane: y = min.y
{ pod::Vector3f{0, 1, 0}, min.y },
// Front plane: z = max.z
{ pod::Vector3f{0, 0, -1}, -max.z },
// Back plane: z = min.z
{ pod::Vector3f{0, 0, 1}, min.z }
}; };
// clip against planes for ( int i = 0; i < 6; i++ ) {
for ( const auto& plane : planes ) { clipPolygon<T>( poly, polyCount, planes[i] );
uf::stl::vector<pod::Polygon<T>> clippedPolygons; if ( polyCount < 3 ) {
for ( const auto& polygon : polygons ) { polyCount = 0;
auto clipped = uf::shapes::polygonPlaneClip<T>( polygon, plane ); break;
if ( clipped.points.size() >= 3 ) clippedPolygons.emplace_back( clipped );
} }
polygons = std::move(clippedPolygons);
} }
return polygons;
}
template<typename T, typename U = uint32_t>
uf::stl::vector<T>& clip(
uf::stl::vector<T>& vertices,
const pod::Vector3f& min,
const pod::Vector3f& max,
bool polygons = true
) {
if ( polygons ) {
// convert to polys
auto polygons = uf::shapes::verticesPolygonate<T>( vertices );
// clip against planes
uf::shapes::clip<T>( polygons, min, max );
// triangulate
vertices = uf::shapes::polygonsTriangulate( polygons );
return vertices;
}
// inverted because its needed
uf::stl::vector<pod::Plane> planes = {
// Right plane: x = max.x
{ pod::Vector3f{-1, 0, 0}, -max.x },
// Left plane: x = min.x
{ pod::Vector3f{1, 0, 0}, min.x },
// Top plane: y = max.y
{ pod::Vector3f{0, -1, 0}, -max.y },
// Bottom plane: y = min.y
{ pod::Vector3f{0, 1, 0}, min.y },
// Front plane: z = max.z
{ pod::Vector3f{0, 0, -1}, -max.z },
// Back plane: z = min.z
{ pod::Vector3f{0, 0, 1}, min.z }
};
// clip against planes
for ( const auto& plane : planes ) {
vertices = uf::shapes::verticesPlaneClip( vertices, plane );
}
return vertices;
} }
template<typename T, typename U = uint32_t> template<typename T, typename U = uint32_t>
void clip( void clip( uf::stl::vector<T>& vertices, uf::stl::vector<U>& indices, const pod::AABB& bounds ) {
uf::stl::vector<T>& vertices, if ( indices.empty() || vertices.empty() ) return;
uf::stl::vector<U>& indices,
const pod::Vector3f& min, uf::stl::vector<T> outVertices;
const pod::Vector3f& max, uf::stl::vector<U> outIndices;
bool polygons = true outVertices.reserve( vertices.size() );
) { outIndices.reserve( indices.size() );
if ( polygons ) {
// convert to polys for ( size_t i = 0; i < indices.size(); i += 3 ) {
auto polygons = uf::shapes::verticesPolygonate<T, U>( vertices, indices ); T poly[16];
// clip against planes poly[0] = vertices[indices[i + 0]];
uf::shapes::clip<T>( polygons, min, max ); poly[1] = vertices[indices[i + 1]];
// triangulate poly[2] = vertices[indices[i + 2]];
vertices = uf::shapes::polygonsTriangulate( polygons ); int polyCount = 3;
indices = uf::shapes::verticesIndex( vertices );
return; clipPolygon<T>( poly, polyCount, bounds );
if ( polyCount < 3 ) continue;
U baseIndex = static_cast<U>(outVertices.size());
for ( int j = 0; j < polyCount; ++j ) {
outVertices.emplace_back( poly[j] );
}
for ( int j = 1; j < polyCount - 1; ++j ) {
outIndices.emplace_back( baseIndex );
outIndices.emplace_back( baseIndex + j );
outIndices.emplace_back( baseIndex + j + 1 );
}
} }
// deindex vertices vertices = std::move( outVertices );
vertices = uf::shapes::indexedVertices( vertices, indices ); indices = std::move( outIndices );
// clip vertices
uf::shapes::clip( vertices, min, max );
// reindex
indices = uf::shapes::verticesIndex( vertices );
} }
template<typename T>
void clip( uf::stl::vector<T>& vertices, const pod::AABB& bounds ) {
if ( vertices.empty() ) return;
uf::stl::vector<T> outVertices;
outVertices.reserve( vertices.size() );
for ( size_t i = 0; i < vertices.size(); i += 3 ) {
T poly[16];
poly[0] = vertices[i + 0];
poly[1] = vertices[i + 1];
poly[2] = vertices[i + 2];
int polyCount = 3;
clipPolygon<T>( poly, polyCount, bounds );
if ( polyCount < 3 ) continue;
for ( int j = 1; j < polyCount - 1; ++j ) {
outVertices.emplace_back( poly[0] );
outVertices.emplace_back( poly[j] );
outVertices.emplace_back( poly[j + 1] );
}
}
vertices = std::move( outVertices );
}
} }
} }

10
engine/inc/uf/utils/mesh/grid.h
View File

@ -85,6 +85,7 @@ namespace uf {
uf::meshgrid::calculate( grid, eps ); uf::meshgrid::calculate( grid, eps );
// it's better to naively clip the mesh multiple times rather than calculate the triangles needed to clip // it's better to naively clip the mesh multiple times rather than calculate the triangles needed to clip
#if 0
if ( clip ) { if ( clip ) {
for ( auto& pair : grid.nodes ) { for ( auto& pair : grid.nodes ) {
++atlasID; ++atlasID;
@ -94,7 +95,7 @@ namespace uf {
uf::stl::vector<T> vertices = meshlet.vertices; uf::stl::vector<T> vertices = meshlet.vertices;
uf::stl::vector<U> indices = meshlet.indices; uf::stl::vector<U> indices = meshlet.indices;
uf::shapes::clip<T,U>( vertices, indices, node.extents.min, node.extents.max ); uf::shapes::clip<T,U>( vertices, indices, pod::AABB{ node.extents.min, node.extents.max } );
if ( vertices.empty() || indices.empty() ) continue; if ( vertices.empty() || indices.empty() ) continue;
@ -130,6 +131,7 @@ namespace uf {
return partitioned; return partitioned;
} }
#endif
for ( auto& meshlet : meshlets ) uf::meshgrid::partition<T,U>( grid, meshlet.vertices, meshlet.indices, meshlet.primitive ); for ( auto& meshlet : meshlets ) uf::meshgrid::partition<T,U>( grid, meshlet.vertices, meshlet.indices, meshlet.primitive );
if ( cleanup ) uf::meshgrid::cleanup( grid ); if ( cleanup ) uf::meshgrid::cleanup( grid );
@ -154,13 +156,13 @@ namespace uf {
vertex.id.y = meshlet.primitive.instance.meshID; vertex.id.y = meshlet.primitive.instance.meshID;
} }
/* #if 1
if ( clip ) { if ( clip ) {
node.effectiveExtents.min = node.extents.min; node.effectiveExtents.min = node.extents.min;
node.effectiveExtents.max = node.extents.max; node.effectiveExtents.max = node.extents.max;
uf::shapes::clip<T,U>( slice.vertices, slice.indices, node.extents.min, node.extents.max ); uf::shapes::clip<T,U>( slice.vertices, slice.indices, pod::AABB{ node.extents.min, node.extents.max } );
} }
*/ #endif
slice.primitive.instance = meshlet.primitive.instance; slice.primitive.instance = meshlet.primitive.instance;
slice.primitive.instance.materialID = meshlet.primitive.instance.materialID; slice.primitive.instance.materialID = meshlet.primitive.instance.materialID;

386
engine/src/utils/math/physics/common.cpp
View File

@ -1,14 +1,5 @@
#include <uf/utils/math/physics/common.h> #include <uf/utils/math/physics/common.h>
namespace impl {
void updateStaticBody( pod::PhysicsBody& body ) {
if ( !body.isStatic ) return;
body.bounds = impl::computeAABB( body );
if ( body.world ) body.world->staticBvh.dirty = true;
}
}
// create ID from pointers // create ID from pointers
uint64_t impl::makePairKey( const pod::PhysicsBody& a, const pod::PhysicsBody& b ) { uint64_t impl::makePairKey( const pod::PhysicsBody& a, const pod::PhysicsBody& b ) {
uint64_t lhs = reinterpret_cast<uint64_t>(&a); uint64_t lhs = reinterpret_cast<uint64_t>(&a);
@ -28,8 +19,13 @@ void impl::wakeBody( pod::PhysicsBody& body ) {
} }
body.activity.awake = true; body.activity.awake = true;
if ( body.isStatic ) impl::updateStaticBody( body );
if ( body.isStatic ) {
body.bounds = impl::computeAABB( body );
if ( body.world ) body.world->staticBvh.dirty = true;
}
} }
// marks body as awake
void impl::sleepBody( pod::PhysicsBody& body ) { void impl::sleepBody( pod::PhysicsBody& body ) {
bool wasAsleep = !body.activity.awake; bool wasAsleep = !body.activity.awake;
@ -37,6 +33,7 @@ void impl::sleepBody( pod::PhysicsBody& body ) {
body.velocity = pod::Vector3f{}; body.velocity = pod::Vector3f{};
body.angularVelocity = pod::Vector3f{}; body.angularVelocity = pod::Vector3f{};
} }
// update body's grounded / sleep states
void impl::updateActivity( pod::PhysicsBody& body, float dt ) { void impl::updateActivity( pod::PhysicsBody& body, float dt ) {
// reset grounded state // reset grounded state
bool wasGrounded = body.activity.grounded; bool wasGrounded = body.activity.grounded;
@ -72,21 +69,19 @@ pod::Transform<> impl::getTransform( const pod::PhysicsBody& body ) {
t.reference = body.transform; t.reference = body.transform;
return uf::transform::flatten( t ); return uf::transform::flatten( t );
} }
// get position of a body, uses bounds center or transform's position
pod::Vector3f impl::getPosition( const pod::PhysicsBody& body, bool useTransform ) { pod::Vector3f impl::getPosition( const pod::PhysicsBody& body, bool useTransform ) {
useTransform = true; // guh
if ( !useTransform ) return impl::aabbCenter( body.bounds ); if ( !useTransform ) return impl::aabbCenter( body.bounds );
return impl::getTransform( body ).position; return impl::getTransform( body ).position;
} }
// creates a view of a hull body
pod::PhysicsBody impl::physicsBodyHullView( const pod::PhysicsBody& body, int32_t index ) { pod::PhysicsBody impl::physicsBodyHullView( const pod::PhysicsBody& body, int32_t index ) {
pod::PhysicsBody view = body; pod::PhysicsBody view = body;
view.viewIndex = index; view.viewIndex = index;
return view; return view;
} }
// creates a view of a triangle
pod::PhysicsBody impl::physicsBodyTriView( const pod::PhysicsBody& body, const pod::TriangleWithNormal triangle ) { pod::PhysicsBody impl::physicsBodyTriView( const pod::TriangleWithNormal triangle, const pod::PhysicsBody& body ) {
pod::PhysicsBody view = body; pod::PhysicsBody view = body;
view.collider.type = pod::ShapeType::TRIANGLE; view.collider.type = pod::ShapeType::TRIANGLE;
view.collider.triangle = triangle; view.collider.triangle = triangle;
@ -95,11 +90,12 @@ pod::PhysicsBody impl::physicsBodyTriView( const pod::PhysicsBody& body, const p
view.transform = NULL; view.transform = NULL;
return view; return view;
} }
// creates a view of a mesh's triangle by triID
pod::PhysicsBody impl::physicsBodyTriView( const pod::PhysicsBody& body, size_t triID ) { pod::PhysicsBody impl::physicsBodyTriView( const pod::PhysicsBody& body, size_t triID ) {
auto tri = impl::fetchTriangle( *body.collider.mesh.mesh, triID, body ); auto tri = impl::fetchTriangle( *body.collider.mesh.mesh, triID, body );
return impl::physicsBodyTriView( body, tri ); return impl::physicsBodyTriView( tri, body );
} }
// checks whether or not two bodies would collide by mask
bool impl::shouldCollide( const pod::Collider& a, const pod::Collider& b ) { bool impl::shouldCollide( const pod::Collider& a, const pod::Collider& b ) {
return ( a.category & b.mask ) && ( b.category & a.mask ); return ( a.category & b.mask ) && ( b.category & a.mask );
} }
@ -108,6 +104,7 @@ bool impl::shouldCollide( const pod::PhysicsBody& a, const pod::PhysicsBody& b )
return impl::shouldCollide( a.collider, b.collider ); return impl::shouldCollide( a.collider, b.collider );
} }
// returns an inverse inertia matrix from an inertia tensor
pod::Matrix3f impl::computeWorldInverseInertia( const pod::PhysicsBody& b ) { pod::Matrix3f impl::computeWorldInverseInertia( const pod::PhysicsBody& b ) {
if ( b.isStatic || b.inverseMass == 0.0f ) return pod::Matrix3f{}; if ( b.isStatic || b.inverseMass == 0.0f ) return pod::Matrix3f{};
@ -127,18 +124,20 @@ pod::Vector3f impl::normalizeDelta( const pod::Vector3f& delta, float dist, cons
return ( dist > EPS ) ? delta / dist : fallback; return ( dist > EPS ) ? delta / dist : fallback;
} }
// computes the tangent of a normal
pod::Vector3f impl::computeTangent( const pod::Vector3f& normal ) { pod::Vector3f impl::computeTangent( const pod::Vector3f& normal ) {
pod::Vector3f up = ( std::fabs(normal.y) < 0.999f ) ? pod::Vector3f{0,1,0} : pod::Vector3f{1,0,0}; // pick a vector not parallel to normal pod::Vector3f up = ( std::fabs(normal.y) < 0.999f ) ? pod::Vector3f{0,1,0} : pod::Vector3f{1,0,0}; // pick a vector not parallel to normal
pod::Vector3f tangent = uf::vector::normalize( uf::vector::cross( up, normal ) ); pod::Vector3f tangent = uf::vector::normalize( uf::vector::cross( up, normal ) );
return tangent; return tangent;
} }
// returns the closest point on an A->B segment
pod::Vector3f impl::closestPointOnSegment( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b ) { pod::Vector3f impl::closestPointOnSegment( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b ) {
pod::Vector3f ab = b - a; pod::Vector3f ab = b - a;
float t = uf::vector::dot(p - a, ab) / uf::vector::dot(ab, ab); float t = uf::vector::dot(p - a, ab) / uf::vector::dot(ab, ab);
t = std::clamp( t, 0.0f, 1.0f ); t = std::clamp( t, 0.0f, 1.0f );
return a + ab * t; return a + ab * t;
} }
//
std::pair<pod::Vector3f, pod::Vector3f> impl::closestSegmentSegment( const pod::Vector3f& A, const pod::Vector3f& B, const pod::Vector3f& C, const pod::Vector3f& D ) { std::pair<pod::Vector3f, pod::Vector3f> impl::closestSegmentSegment( const pod::Vector3f& A, const pod::Vector3f& B, const pod::Vector3f& C, const pod::Vector3f& D ) {
auto u = B - A; auto u = B - A;
auto v = D - C; auto v = D - C;
@ -184,11 +183,11 @@ std::pair<pod::Vector3f, pod::Vector3f> impl::closestSegmentSegment( const pod::
return { A + u * sc, C + v * tc }; return { A + u * sc, C + v * tc };
} }
//
pod::Vector3f impl::closestPointSegmentAabb( const pod::Vector3f& p1, const pod::Vector3f& p2, const pod::AABB& box ) { pod::Vector3f impl::closestPointSegmentAabb( const pod::Vector3f& p1, const pod::Vector3f& p2, const pod::AABB& box ) {
// AABB center and half extents // AABB center and half extents
auto c = ( box.min + box.max ) * 0.5f; auto c = impl::aabbCenter( box );
auto e = ( box.max - box.min ) * 0.5f; auto e = impl::aabbExtent( box );
// direction of line segment // direction of line segment
auto d = p2 - p1; auto d = p2 - p1;
@ -207,7 +206,7 @@ pod::Vector3f impl::closestPointSegmentAabb( const pod::Vector3f& p1, const pod:
// clamp this point into AABB // clamp this point into AABB
return uf::vector::clamp( segClosest, box.min, box.max ); return uf::vector::clamp( segClosest, box.min, box.max );
} }
// returns the barycentric coordinates of a point on a triangle
pod::Vector3f impl::computeBarycentric( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b, const pod::Vector3f& c, bool clamps ) { pod::Vector3f impl::computeBarycentric( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b, const pod::Vector3f& c, bool clamps ) {
// edges // edges
auto ab = b - a; auto ab = b - a;
@ -270,7 +269,7 @@ pod::Vector3f impl::interpolateWithBarycentric( const pod::Vector3f& bary, const
pod::Vector3f impl::interpolateWithBarycentric( const pod::Vector3f& bary, const pod::Vector3f points[3] ) { pod::Vector3f impl::interpolateWithBarycentric( const pod::Vector3f& bary, const pod::Vector3f points[3] ) {
return impl::interpolateWithBarycentric( bary, points[0], points[1], points[2] ); return impl::interpolateWithBarycentric( bary, points[0], points[1], points[2] );
} }
// returns if a point is inside a triangle
bool impl::pointInTriangle( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b, const pod::Vector3f& c ) { bool impl::pointInTriangle( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b, const pod::Vector3f& c ) {
auto bary = impl::computeBarycentric( p, a, b, c, false ); auto bary = impl::computeBarycentric( p, a, b, c, false );
return ( bary.x >= -EPS && bary.y >= -EPS && bary.z >= -EPS ); return ( bary.x >= -EPS && bary.y >= -EPS && bary.z >= -EPS );
@ -279,7 +278,7 @@ bool impl::pointInTriangle( const pod::Vector3f& p, const pod::Triangle& tri ) {
auto bary = impl::computeBarycentric( p, tri, false ); auto bary = impl::computeBarycentric( p, tri, false );
return ( bary.x >= -EPS && bary.y >= -EPS && bary.z >= -EPS ); return ( bary.x >= -EPS && bary.y >= -EPS && bary.z >= -EPS );
} }
// returns the closest point on a triangle (possible duplicate of computeBarycentric)
pod::Vector3f impl::closestPointOnTriangle( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b, const pod::Vector3f& c ) { pod::Vector3f impl::closestPointOnTriangle( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b, const pod::Vector3f& c ) {
// check if P in vertex region outside A // check if P in vertex region outside A
pod::Vector3f ab = b - a; pod::Vector3f ab = b - a;
@ -331,11 +330,11 @@ pod::Vector3f impl::closestPointOnTriangle( const pod::Vector3f& p, const pod::V
pod::Vector3f impl::closestPointOnTriangle( const pod::Vector3f& p, const pod::Triangle& tri ) { pod::Vector3f impl::closestPointOnTriangle( const pod::Vector3f& p, const pod::Triangle& tri ) {
return impl::closestPointOnTriangle( p, tri.points[0], tri.points[1], tri.points[2] ); return impl::closestPointOnTriangle( p, tri.points[0], tri.points[1], tri.points[2] );
} }
// reorients a normal from body A to body B
pod::Vector3f impl::orientNormalToAB( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Vector3f n ) { pod::Vector3f impl::orientNormalToAB( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Vector3f n ) {
return uf::vector::normalize( uf::vector::dot( n, impl::getPosition( b ) - impl::getPosition( a ) ) < 0.0f ? -n : n ); return uf::vector::normalize( uf::vector::dot( n, impl::getPosition( b ) - impl::getPosition( a ) ) < 0.0f ? -n : n );
} }
//
float impl::segmentTriangleDistanceSq( const pod::Vector3f& p0, const pod::Vector3f& p1, const pod::Triangle& tri, pod::Vector3f& outSeg, pod::Vector3f& outTri ) { float impl::segmentTriangleDistanceSq( const pod::Vector3f& p0, const pod::Vector3f& p1, const pod::Triangle& tri, pod::Vector3f& outSeg, pod::Vector3f& outTri ) {
float best = std::numeric_limits<float>::max(); float best = std::numeric_limits<float>::max();
@ -382,64 +381,103 @@ float impl::segmentTriangleDistanceSq( const pod::Vector3f& p0, const pod::Vecto
return best; return best;
} }
int impl::clipPolygonAgainstPlane( const pod::Vector3f* inPoly, int inCount, const pod::Vector3f& planeNormal, float planeOffset, pod::Vector3f* outPoly ) { // Separating Axis Theorem test
if (inCount == 0) return 0; bool impl::testSeparatingAxis( const pod::Triangle& triangle, const pod::AABB& box, const pod::Vector3f& axis, const pod::Vector3f axes[3], float& outMinOverlap, pod::Vector3f& outBestAxis ) {
float mag = uf::vector::magnitude(axis);
if ( mag < EPS2 ) return true;
pod::Vector3f n = axis / mag;
int outCount = 0; // project triangle
pod::Vector3f prevPoint = inPoly[inCount - 1]; float p0 = uf::vector::dot( triangle.points[0], n );
float prevDistance = uf::vector::dot(prevPoint, planeNormal) - planeOffset; float p1 = uf::vector::dot( triangle.points[1], n );
float p2 = uf::vector::dot( triangle.points[2], n );
float minT = std::min( { p0, p1, p2 } );
float maxT = std::max( { p0, p1, p2 } );
for (int i = 0; i < inCount; ++i) { // project box
pod::Vector3f currPoint = inPoly[i]; pod::Vector3f cB = impl::aabbCenter( box );
float currDistance = uf::vector::dot(currPoint, planeNormal) - planeOffset; pod::Vector3f eB = impl::aabbExtent( box );
float pB = uf::vector::dot(cB, n);
float rB = eB.x * std::fabs(uf::vector::dot(axes[0], n)) +
eB.y * std::fabs(uf::vector::dot(axes[1], n)) +
eB.z * std::fabs(uf::vector::dot(axes[2], n));
// If they cross the plane, compute the intersection point float minB = pB - rB;
if ((prevDistance * currDistance) < 0.0f) { float maxB = pB + rB;
float t = prevDistance / (prevDistance - currDistance);
outPoly[outCount++] = prevPoint + (currPoint - prevPoint) * t;
}
// If the current point is 'inside' or on the plane (distance <= 0), keep it // check for separation
if (currDistance <= 0.0f) { if ( minT > maxB || maxT < minB ) return false;
outPoly[outCount++] = currPoint;
}
prevPoint = currPoint; // calculate overlap depth
prevDistance = currDistance; float overlap = std::min(maxT, maxB) - std::max(minT, minB);
if ( overlap < outMinOverlap ) {
outMinOverlap = overlap;
outBestAxis = n;
} }
return true;
return outCount;
} }
// Sutherland-Hodgman polygon clipping
void impl::clipPolygon( pod::Vector3f* poly, int& polyCount, const pod::Plane& plane ) {
if ( polyCount == 0 ) return;
int outCount = 0;
pod::Vector3f out[8];
for ( auto i = 0; i < polyCount; i++ ) {
auto curr = poly[i];
auto prev = poly[(i + polyCount - 1) % polyCount];
float dCurr = uf::vector::dot(plane.normal, curr) - plane.offset;
float dPrev = uf::vector::dot(plane.normal, prev) - plane.offset;
if ( dCurr <= 0.0f ) {
if ( dPrev > 0.0f ) {
float t = dPrev / (dPrev - dCurr);
out[outCount++] = prev + (curr - prev) * t;
}
out[outCount++] = curr;
}
else if ( dPrev <= 0.0f ) {
float t = dPrev / (dPrev - dCurr);
out[outCount++] = prev + (curr - prev) * t;
}
}
polyCount = outCount;
for ( auto i = 0; i < outCount; i++ ) poly[i] = out[i];
}
void impl::clipPolygon( pod::Vector3f* poly, int& polyCount, const pod::AABB& aabb ) {
pod::Plane planes[6] = {
{ pod::Vector3f{-1, 0, 0}, -aabb.max.x },
{ pod::Vector3f{ 1, 0, 0}, aabb.min.x },
{ pod::Vector3f{ 0,-1, 0}, -aabb.max.y },
{ pod::Vector3f{ 0, 1, 0}, aabb.min.y },
{ pod::Vector3f{ 0, 0,-1}, -aabb.max.z },
{ pod::Vector3f{ 0, 0, 1}, aabb.min.z }
};
for ( auto i = 0; i < 6; i++ ) {
impl::clipPolygon( poly, polyCount, planes[i] );
// degenerated
if ( polyCount < 3 ) {
polyCount = 0;
break;
}
}
}
// returns the center of a triangle
pod::Vector3f impl::triangleCenter( const pod::Triangle& tri ) { pod::Vector3f impl::triangleCenter( const pod::Triangle& tri ) {
return ( tri.points[0] + tri.points[1] + tri.points[2] ) / 3.0f; return ( tri.points[0] + tri.points[1] + tri.points[2] ) / 3.0f;
} }
// returns the normal of a triangle
pod::Vector3f impl::triangleNormal( const pod::Triangle& tri ) { pod::Vector3f impl::triangleNormal( const pod::Triangle& tri ) {
return uf::vector::normalize(uf::vector::cross(tri.points[1] - tri.points[0], tri.points[2] - tri.points[0])); return uf::vector::normalize(uf::vector::cross(tri.points[1] - tri.points[0], tri.points[2] - tri.points[0]));
} }
pod::Vector3f impl::triangleNormal( const pod::TriangleWithNormal& tri ) { pod::Vector3f impl::triangleNormal( const pod::TriangleWithNormal& tri ) {
return tri.normal; return tri.normal;
//return uf::vector::normalize( tri.normals[0] + tri.normals[1] + tri.normals[2] );
} }
// mesh accessing
bool impl::triangleTriangleIntersect( const pod::Triangle& a, const pod::Triangle& b ) {
auto boxA = impl::computeTriangleAABB( a );
auto boxB = impl::computeTriangleAABB( b );
if ( !impl::aabbOverlap( boxA, boxB ) ) return false;
// check vertices of a inside b or vice versa
for ( auto i = 0; i < 3; ++i ) {
auto q = impl::closestPointOnTriangle( a.points[i], b );
if ( uf::vector::magnitude( q - a.points[i] ) < EPS2 ) return true;
};
for ( auto i = 0; i < 3; ++i ) {
auto q = impl::closestPointOnTriangle( b.points[i], a );
if ( uf::vector::magnitude( q - b.points[i] ) < EPS2 ) return true;
};
return false;
}
size_t impl::getIndex( const void* pointer, size_t stride, size_t index ) { size_t impl::getIndex( const void* pointer, size_t stride, size_t index ) {
#define CAST_INDEX(T) case sizeof(T): return ((T*) pointer)[index]; #define CAST_INDEX(T) case sizeof(T): return ((T*) pointer)[index];
switch ( stride ) { switch ( stride ) {
@ -551,91 +589,7 @@ pod::TriangleWithNormal impl::fetchTriangle( const uf::Mesh& mesh, size_t triID,
return tri; return tri;
} }
bool impl::computeTriangleTriangleSegment( const pod::TriangleWithNormal& A, const pod::TriangleWithNormal& B, pod::Vector3f& p0, pod::Vector3f& p1 ) { // returns whether or not two AABBs are overlapping (with SIMD speedup)
int intersections = 0;
pod::Vector3f intersectionBuffers[6] = {};
auto checkAndPush = [&]( const pod::Vector3f& pt ) {
// avoid duplicates
for ( auto& v : intersectionBuffers ) {
if ( uf::vector::distanceSquared( v, pt ) < EPS*EPS ) return;
}
intersectionBuffers[intersections++] = pt;
};
// segment-plane intersection
auto intersectSegmentPlane = [&](const pod::Vector3f& a, const pod::Vector3f& b, const pod::Vector3f& n, float d, pod::Vector3f& out)->bool {
pod::Vector3f ab = b - a;
float denom = uf::vector::dot( n, ab );
if (fabs(denom) < EPS) return false; // parallel
float t = (d - uf::vector::dot( n, a )) / denom;
if ( t < -EPS || t > 1.0f + EPS ) return false;
out = a + ab * t;
return true;
};
// planes
auto nA = impl::triangleNormal( A );
auto nB = impl::triangleNormal( B );
float dA = uf::vector::dot( nA, A.points[0] );
float dB = uf::vector::dot( nB, B.points[0] );
// clip edges of A against plane of B
const pod::Vector3f At[3] = { A.points[0], A.points[1], A.points[2] };
FOR_EACH(3, {
auto j = ( i + 1 ) % 3;
pod::Vector3f p;
if ( intersectSegmentPlane( At[i], At[j], nB, dB, p ) ) {
// check if intersection lies inside triangle B
if ( impl::pointInTriangle( p, B ) ) checkAndPush(p);
}
});
// clip edges of B against plane of A
const pod::Vector3f Bt[3] = { B.points[0], B.points[1], B.points[2] };
FOR_EACH(3, {
auto j = ( i + 1 ) % 3;
pod::Vector3f p;
if ( intersectSegmentPlane( Bt[i], Bt[j], nA, dA, p ) ) {
if ( impl::pointInTriangle( p, A ) ) checkAndPush(p);
}
});
if ( intersections == 0 ) return false;
// degenerate intersection
if ( intersections == 1 ) {
p0 = p1 = intersectionBuffers[0];
return true;
}
// find two furthest apart points for intersection segment
float maxDist2 = -1.0f;
for ( auto i = 0 ; i < intersections; i++ ) {
for ( auto j = i + 1; j < intersections; j++ ) {
float d2 = uf::vector::distanceSquared( intersectionBuffers[i], intersectionBuffers[j] );
if ( d2 > maxDist2 ) {
maxDist2 = d2;
p0 = intersectionBuffers[i];
p1 = intersectionBuffers[j];
}
}
}
return maxDist2 >= 0.0f;
}
pod::Vector2f impl::projectTriangleOntoAxis( const pod::TriangleWithNormal& tri, const pod::Vector3f& axis ) {
pod::Vector3f normal = uf::vector::normalize( axis );
float p0 = uf::vector::dot( tri.points[0], normal );
float p1 = uf::vector::dot( tri.points[1], normal );
float p2 = uf::vector::dot( tri.points[2], normal );
return { std::min({ p0, p1, p2 }), std::max({ p0, p1, p2 }) };
}
bool impl::aabbOverlap( const pod::AABB& a, const pod::AABB& b ) { bool impl::aabbOverlap( const pod::AABB& a, const pod::AABB& b ) {
#if UF_USE_SIMD #if UF_USE_SIMD
return uf::simd::all( uf::simd::lessEquals( a.min, b.max ) ) && uf::simd::all( uf::simd::greaterEquals( a.max, b.min ) ); return uf::simd::all( uf::simd::lessEquals( a.min, b.max ) ) && uf::simd::all( uf::simd::greaterEquals( a.max, b.min ) );
@ -643,30 +597,30 @@ bool impl::aabbOverlap( const pod::AABB& a, const pod::AABB& b ) {
return ( a.min - EPS ) <= ( b.max + EPS ) && ( a.max + EPS ) >= ( b.min - EPS ); return ( a.min - EPS ) <= ( b.max + EPS ) && ( a.max + EPS ) >= ( b.min - EPS );
#endif #endif
} }
// returns the surface area an AABB covers
float impl::aabbSurfaceArea(const pod::AABB& aabb) { float impl::aabbSurfaceArea(const pod::AABB& aabb) {
auto d = uf::vector::max( ( aabb.max - aabb.min ), pod::Vector3f{} ); auto d = uf::vector::max( ( aabb.max - aabb.min ), pod::Vector3f{} );
return 2.0f * (d.x * d.y + d.y * d.z + d.z * d.x); return 2.0f * (d.x * d.y + d.y * d.z + d.z * d.x);
} }
// returns the bounds a line segment covers with a radius (for capsules)
pod::AABB impl::computeSegmentAABB( const pod::Vector3f& p1, const pod::Vector3f p2, float r ) { pod::AABB impl::computeSegmentAABB( const pod::Vector3f& p1, const pod::Vector3f p2, float r ) {
return { return {
uf::vector::min( p1, p2 ) - r, uf::vector::min( p1, p2 ) - r,
uf::vector::max( p1, p2 ) + r, uf::vector::max( p1, p2 ) + r,
}; };
} }
// returns the closest point on an AABB
pod::Vector3f impl::closestPointOnAABB(const pod::Vector3f& p, const pod::AABB& box) { pod::Vector3f impl::closestPointOnAABB(const pod::Vector3f& p, const pod::AABB& box) {
return uf::vector::clamp( p, box.min, box.max ); return uf::vector::clamp( p, box.min, box.max );
} }
// returns the AABB of a triangle
pod::AABB impl::computeTriangleAABB( const pod::Triangle& tri ) { pod::AABB impl::computeTriangleAABB( const pod::Triangle& tri ) {
return { return {
uf::vector::min( uf::vector::min( tri.points[0], tri.points[1] ), tri.points[2] ), uf::vector::min( uf::vector::min( tri.points[0], tri.points[1] ), tri.points[2] ),
uf::vector::max( uf::vector::max( tri.points[0], tri.points[1] ), tri.points[2] ), uf::vector::max( uf::vector::max( tri.points[0], tri.points[1] ), tri.points[2] ),
}; };
} }
// returns the AABB of a hull
pod::AABB impl::computeConvexHullAABB( const uf::Mesh::View& view, const uf::Mesh::AttributeView& positions, pod::AABB bounds ) { pod::AABB impl::computeConvexHullAABB( const uf::Mesh::View& view, const uf::Mesh::AttributeView& positions, pod::AABB bounds ) {
for ( size_t i = 0; i < view.vertex.count; ++i ) { for ( size_t i = 0; i < view.vertex.count; ++i ) {
pod::Vector3f v = impl::getVertex( view, positions, i ); pod::Vector3f v = impl::getVertex( view, positions, i );
@ -679,46 +633,46 @@ pod::AABB impl::computeConvexHullAABB( const uf::Mesh::View& view, const uf::Mes
pod::AABB impl::computeConvexHullAABB( const uf::Mesh::View& view, pod::AABB bounds ) { pod::AABB impl::computeConvexHullAABB( const uf::Mesh::View& view, pod::AABB bounds ) {
return impl::computeConvexHullAABB( view, view["position"], bounds ); return impl::computeConvexHullAABB( view, view["position"], bounds );
} }
// combines two AABBs
pod::AABB impl::mergeAabb( const pod::AABB& a, const pod::AABB& b ) { pod::AABB impl::mergeAabb( const pod::AABB& a, const pod::AABB& b ) {
return { return {
uf::vector::min( a.min, b.min ), uf::vector::min( a.min, b.min ),
uf::vector::max( a.max, b.max ), uf::vector::max( a.max, b.max ),
}; };
} }
// returns the center of an AABB
pod::Vector3f impl::aabbCenter( const pod::AABB& aabb ) { pod::Vector3f impl::aabbCenter( const pod::AABB& aabb ) {
return ( aabb.max + aabb.min ) * 0.5f; return ( aabb.max + aabb.min ) * 0.5f;
} }
// returns the half extents of an AABB
pod::Vector3f impl::aabbExtent( const pod::AABB& aabb ) { pod::Vector3f impl::aabbExtent( const pod::AABB& aabb ) {
return ( aabb.max - aabb.min ) * 0.5f; return ( aabb.max - aabb.min ) * 0.5f;
} }
// transforms an AABB into world-space
pod::AABB impl::transformAabbToWorld( const pod::AABB& localBox, const pod::Transform<>& transform ) { pod::AABB impl::transformAabbToWorld( const pod::AABB& aabb, const pod::Transform<>& transform ) {
// to-do: flatten, since transform might not be flattened (even though getTransform does that)
const auto& q = transform.orientation; const auto& q = transform.orientation;
const auto& p = transform.position; const auto& p = transform.position;
pod::Vector3f center = (localBox.min + localBox.max) * 0.5f; pod::Vector3f cB = impl::aabbCenter( aabb );
pod::Vector3f extents = (localBox.max - localBox.min) * 0.5f; pod::Vector3f eB = impl::aabbExtent( aabb );
pod::Vector3f axes[] = {
pod::Vector3f axisX = uf::quaternion::rotate(q, pod::Vector3f{1,0,0}); uf::quaternion::rotate(q, pod::Vector3f{1,0,0}),
pod::Vector3f axisY = uf::quaternion::rotate(q, pod::Vector3f{0,1,0}); uf::quaternion::rotate(q, pod::Vector3f{0,1,0}),
pod::Vector3f axisZ = uf::quaternion::rotate(q, pod::Vector3f{0,0,1}); uf::quaternion::rotate(q, pod::Vector3f{0,0,1}),
pod::Vector3f worldCenter = uf::quaternion::rotate(q, center) + p;
pod::Vector3f worldExtents = {
fabs(axisX.x) * extents.x + fabs(axisY.x) * extents.y + fabs(axisZ.x) * extents.z,
fabs(axisX.y) * extents.x + fabs(axisY.y) * extents.y + fabs(axisZ.y) * extents.z,
fabs(axisX.z) * extents.x + fabs(axisY.z) * extents.y + fabs(axisZ.z) * extents.z
}; };
return { pod::Vector3f cW = uf::quaternion::rotate(q, cB) + p;
worldCenter - worldExtents, // to-do: a cleaner way of doing this with uf::vector::abs
worldCenter + worldExtents pod::Vector3f cE = {
fabs(axes[0].x) * eB.x + fabs(axes[1].x) * eB.y + fabs(axes[2].x) * eB.z,
fabs(axes[0].y) * eB.x + fabs(axes[1].y) * eB.y + fabs(axes[2].y) * eB.z,
fabs(axes[0].z) * eB.x + fabs(axes[1].z) * eB.y + fabs(axes[2].z) * eB.z
}; };
return { cW - cE, cW + cE };
} }
// returns the line segment of a capsule
std::pair<pod::Vector3f, pod::Vector3f> impl::getCapsuleSegment( const pod::PhysicsBody& body ) { std::pair<pod::Vector3f, pod::Vector3f> impl::getCapsuleSegment( const pod::PhysicsBody& body ) {
const auto transform = impl::getTransform( body ); const auto transform = impl::getTransform( body );
const auto& capsule = body.collider.capsule; const auto& capsule = body.collider.capsule;
@ -729,7 +683,7 @@ std::pair<pod::Vector3f, pod::Vector3f> impl::getCapsuleSegment( const pod::Phys
auto p2 = transform.position - up * capsule.halfHeight; auto p2 = transform.position - up * capsule.halfHeight;
return { p1, p2 }; return { p1, p2 };
} }
// computes the AABB for a given body
pod::AABB impl::computeAABB( const pod::PhysicsBody& body ) { pod::AABB impl::computeAABB( const pod::PhysicsBody& body ) {
const auto transform = impl::getTransform( body ); const auto transform = impl::getTransform( body );
switch ( body.collider.type ) { switch ( body.collider.type ) {
@ -768,77 +722,7 @@ pod::AABB impl::computeAABB( const pod::PhysicsBody& body ) {
return {}; return {};
} }
// transforms an AABB into local space
float impl::triAabbDistanceSq( const pod::Triangle& tri, const pod::AABB& box ) {
float minDistSq = FLT_MAX;
FOR_EACH(3, {
auto cp = impl::closestPointOnAABB( tri.points[i], box );
auto d = tri.points[i] - cp;
minDistSq = std::min( minDistSq, uf::vector::dot( d, d ) );
});
return minDistSq;
}
bool impl::triAabbOverlap( const pod::Triangle& tri, const pod::AABB& box ) {
// compute box center and half extents
auto c = ( box.min + box.max ) * 0.5f;
auto e = ( box.max - box.min ) * 0.5f;
// move triangle into box's local space
auto v0 = tri.points[0] - c;
auto v1 = tri.points[1] - c;
auto v2 = tri.points[2] - c;
// triangle edges
auto f0 = v1 - v0;
auto f1 = v2 - v1;
auto f2 = v0 - v2;
// SAT: test the 9 edge cross axes
auto axisTest = [&]( const pod::Vector3f& axis ) {
float norm = uf::vector::norm( axis );
if ( norm < EPS ) return true;
auto a = axis / norm;
float p0 = uf::vector::dot( v0, a );
float p1 = uf::vector::dot( v1, a );
float p2 = uf::vector::dot( v2, a );
float r = e.x * fabs(a.x) + e.y * fabs(a.y) + e.z * fabs(a.z);
float minP = std::min({p0, p1, p2});
float maxP = std::max({p0, p1, p2});
return !(minP > r || maxP < -r);
};
if ( !axisTest( {0, -f0.z, f0.y} ) ) return false;
if ( !axisTest( {0, -f1.z, f1.y} ) ) return false;
if ( !axisTest( {0, -f2.z, f2.y} ) ) return false;
if ( !axisTest( {f0.z, 0, -f0.x} ) ) return false;
if ( !axisTest( {f1.z, 0, -f1.x} ) ) return false;
if ( !axisTest( {f2.z, 0, -f2.x} ) ) return false;
if ( !axisTest( {-f0.y, f0.x, 0} ) ) return false;
if ( !axisTest( {-f1.y, f1.x, 0} ) ) return false;
if ( !axisTest( {-f2.y, f2.x, 0} ) ) return false;
// test AABB face axes
for ( auto i = 0; i < 3; ++i ) {
float minVal = std::min({v0[i], v1[i], v2[i]});
float maxVal = std::max({v0[i], v1[i], v2[i]});
if ( minVal > e[i] || maxVal < -e[i] ) return false;
};
// test triangle normal axis
auto n = uf::vector::cross( f0, f1 );
float d0 = uf::vector::dot( v0, n );
float r = e.x * fabs(n.x) + e.y * fabs(n.y) + e.z * fabs(n.z);
if ( fabs(d0) > r ) return false;
return true;
}
pod::AABB impl::transformAabbToLocal( const pod::AABB& box, const pod::Transform<>& transform ) { pod::AABB impl::transformAabbToLocal( const pod::AABB& box, const pod::Transform<>& transform ) {
auto inv = uf::transform::inverse( transform ); auto inv = uf::transform::inverse( transform );

2
engine/src/utils/math/physics/draw.cpp
View File

@ -83,7 +83,7 @@ void impl::draw( const pod::World& world, float dt ) {
::lines.clear(); ::lines.clear();
for ( auto* body : world.bodies ) impl::drawBody( *body ); // for ( auto* body : world.bodies ) impl::drawBody( *body );
for ( auto it = ::transientLines.begin(); it != ::transientLines.end(); ) { for ( auto it = ::transientLines.begin(); it != ::transientLines.end(); ) {
auto& line = it->second; auto& line = it->second;

25
engine/src/utils/math/physics/narrowphase/epa.cpp
View File

@ -124,7 +124,7 @@ void impl::getSupportFace( const pod::PhysicsBody& body, const pod::Vector3f& di
bool impl::generateClippingManifold( const pod::PhysicsBody& a, const pod::PhysicsBody& b, const pod::Contact& contact, pod::Manifold& manifold ) { bool impl::generateClippingManifold( const pod::PhysicsBody& a, const pod::PhysicsBody& b, const pod::Contact& contact, pod::Manifold& manifold ) {
if ( !uf::vector::isValid(contact.point) ) return false; if ( !uf::vector::isValid(contact.point) ) return false;
auto& normal = contact.normal; auto& normal = contact.normal;
pod::Vector3f polyA[4]; pod::Vector3f polyA[4];
@ -145,36 +145,33 @@ bool impl::generateClippingManifold( const pod::PhysicsBody& a, const pod::Physi
int refCount = countA; int refCount = countA;
int incCount = countB; int incCount = countB;
pod::Vector3f clipBuffer1[8]; pod::Vector3f clipBuffer[8];
pod::Vector3f clipBuffer2[8];
int clipCount = incCount; int clipCount = incCount;
for ( auto i = 0; i < incCount; ++i ) clipBuffer1[i] = incPoly[i]; for ( auto i = 0; i < incCount; ++i ) clipBuffer[i] = incPoly[i];
pod::Vector3f refNormal = uf::vector::normalize(uf::vector::cross(refPoly[1] - refPoly[0], refPoly[2] - refPoly[0])); pod::Vector3f refNormal = uf::vector::normalize(uf::vector::cross(refPoly[1] - refPoly[0], refPoly[2] - refPoly[0]));
for ( auto i = 0; i < refCount; ++i ) { for ( auto i = 0; i < refCount; ++i ) {
pod::Vector3f edgeStart = refPoly[i]; pod::Vector3f edgeStart = refPoly[i];
pod::Vector3f edgeEnd = refPoly[(i + 1) % refCount]; pod::Vector3f edgeEnd = refPoly[(i + 1) % refCount];
pod::Vector3f edgeVector = edgeEnd - edgeStart; pod::Vector3f edgeVector = edgeEnd - edgeStart;
pod::Vector3f edgePlaneNormal = uf::vector::normalize(uf::vector::cross(edgeVector, refNormal)); pod::Plane edgePlane;
float edgePlaneOffset = uf::vector::dot(edgePlaneNormal, edgeStart); edgePlane.normal = uf::vector::normalize(uf::vector::cross(edgeVector, refNormal));
edgePlane.offset = uf::vector::dot(edgePlane.normal, edgeStart);
if (i % 2 == 0) { impl::clipPolygon( clipBuffer, clipCount, edgePlane );
clipCount = impl::clipPolygonAgainstPlane(clipBuffer1, clipCount, edgePlaneNormal, edgePlaneOffset, clipBuffer2); if ( clipCount == 0 ) break;
} else {
clipCount = impl::clipPolygonAgainstPlane(clipBuffer2, clipCount, edgePlaneNormal, edgePlaneOffset, clipBuffer1);
}
} }
pod::Vector3f* finalPoly = (refCount % 2 == 0) ? clipBuffer1 : clipBuffer2;
float refOffset = uf::vector::dot(refNormal, refPoly[0]); float refOffset = uf::vector::dot(refNormal, refPoly[0]);
for (int i = 0; i < clipCount; ++i) { for (int i = 0; i < clipCount; ++i) {
float distance = uf::vector::dot(finalPoly[i], refNormal) - refOffset; float distance = uf::vector::dot(clipBuffer[i], refNormal) - refOffset;
// point is penetrating or touching // point is penetrating or touching
if ( distance <= EPS ) { if ( distance <= EPS ) {
pod::Contact c; pod::Contact c;
c.point = finalPoly[i] - refNormal * (distance * 0.5f); c.point = clipBuffer[i] - refNormal * (distance * 0.5f);
c.normal = normal; c.normal = normal;
c.penetration = -distance; c.penetration = -distance;
manifold.points.emplace_back(c); manifold.points.emplace_back(c);

285
engine/src/utils/math/physics/narrowphase/triangle.cpp
View File

@ -14,9 +14,7 @@ namespace impl {
} }
bool triangleGeneric( const pod::TriangleWithNormal& a, const pod::PhysicsBody& b, pod::Manifold& manifold ) { bool triangleGeneric( const pod::TriangleWithNormal& a, const pod::PhysicsBody& b, pod::Manifold& manifold ) {
pod::PhysicsBody tri = {}; auto tri = impl::physicsBodyTriView( a );
tri.collider.type = pod::ShapeType::TRIANGLE;
tri.collider.triangle = a;
const auto& body = b; const auto& body = b;
return triangleGeneric( tri, body, manifold ); return triangleGeneric( tri, body, manifold );
@ -25,28 +23,23 @@ namespace impl {
bool impl::triangleTriangle( const pod::TriangleWithNormal& a, const pod::TriangleWithNormal& b, pod::Manifold& manifold ) { bool impl::triangleTriangle( const pod::TriangleWithNormal& a, const pod::TriangleWithNormal& b, pod::Manifold& manifold ) {
if ( uf::physics::settings.useGjk ) { if ( uf::physics::settings.useGjk ) {
pod::PhysicsBody A = {}; auto A = impl::physicsBodyTriView( a );
A.collider.type = pod::ShapeType::TRIANGLE; auto B = impl::physicsBodyTriView( b );
A.collider.triangle = a;
pod::PhysicsBody B = {};
B.collider.type = pod::ShapeType::TRIANGLE;
B.collider.triangle = b;
return impl::triangleGeneric( A, B, manifold ); return impl::triangleGeneric( A, B, manifold );
} }
size_t axes = 0; size_t axesCount = 0;
pod::Vector3f axesBuffer[12]; pod::Vector3f axes[14]; // 2 normals + 9 edge cross product
axesBuffer[axes++] = impl::triangleNormal(a); axes[axesCount++] = impl::triangleNormal(a);
axesBuffer[axes++] = impl::triangleNormal(b); axes[axesCount++] = impl::triangleNormal(b);
for (int i = 0; i < 3; i++) { for ( auto i = 0; i < 3; i++ ) {
auto ea = a.points[(i+1)%3] - a.points[i]; auto ea = a.points[(i+1)%3] - a.points[i];
for (int j = 0; j < 3; j++) { for ( auto j = 0; j < 3; j++ ) {
auto eb = b.points[(j+1)%3] - b.points[j]; auto eb = b.points[(j+1)%3] - b.points[j];
auto axis = uf::vector::cross(ea, eb); auto axis = uf::vector::cross(ea, eb);
if ( uf::vector::magnitude( axis ) > EPS2 ) axesBuffer[axes++] = uf::vector::normalize(axis); if ( uf::vector::magnitude( axis ) > EPS2 ) axes[axesCount++] = uf::vector::normalize(axis);
} }
} }
@ -54,67 +47,54 @@ bool impl::triangleTriangle( const pod::TriangleWithNormal& a, const pod::Triang
float minOverlap = FLT_MAX; float minOverlap = FLT_MAX;
pod::Vector3f bestAxis; pod::Vector3f bestAxis;
for ( auto& axis : axesBuffer ) { for ( auto i = 0; i < axesCount; i++ ) {
auto projA = impl::projectTriangleOntoAxis(a, axis); const auto& axis = axes[i];
auto projB = impl::projectTriangleOntoAxis(b, axis); auto nA = uf::vector::normalize( axis );
auto pA = pod::Vector3f{
uf::vector::dot( a.points[0], nA ),
uf::vector::dot( a.points[1], nA ),
uf::vector::dot( a.points[2], nA )
};
auto pB = pod::Vector3f{
uf::vector::dot( b.points[0], nA ),
uf::vector::dot( b.points[1], nA ),
uf::vector::dot( b.points[2], nA )
};
auto projA = pod::Vector2f{ uf::vector::min( pA ), uf::vector::max( pA ) };
auto projB = pod::Vector2f{ uf::vector::min( pB ), uf::vector::max( pB ) };
float overlap = std::min(projA.y, projB.y) - std::max(projA.x, projB.x); float overlap = std::min(projA.y, projB.y) - std::max(projA.x, projB.x);
if (overlap < 0) return false; // separating axis if ( overlap < 0 ) return false;
if (overlap < minOverlap) { if ( overlap < minOverlap ) {
minOverlap = overlap; minOverlap = overlap;
bestAxis = axis; bestAxis = axis;
} }
} }
// clip polygons
int polyCount = 0;
pod::Vector3f poly[8];
poly[polyCount++] = b.points[0];
poly[polyCount++] = b.points[1];
poly[polyCount++] = b.points[2];
auto clipAgainstPlane = [&](const pod::Vector3f& n, const pod::Vector3f& p) {
int outCount = 0;
pod::Vector3f out[8];
for ( auto i = 0; i < polyCount; i++ ) {
auto curr = poly[i];
auto prev = poly[(i+polyCount-1)%polyCount];
float dCurr = uf::vector::dot(n, curr - p);
float dPrev = uf::vector::dot(n, prev - p);
if ( dCurr >= 0 ) {
if ( dPrev < 0 ) {
float t = dPrev / (dPrev - dCurr);
out[outCount++] = prev + (curr - prev) * t;
}
out[outCount++] = curr;
} else if ( dPrev >= 0 ) {
float t = dPrev / (dPrev - dCurr);
out[outCount++] = prev + (curr - prev) * t;
}
}
// copy back
polyCount = outCount;
for ( auto i = 0; i < outCount; i++ ) poly[i] = out[i];
};
if ( uf::vector::dot(bestAxis, impl::triangleCenter(b) - impl::triangleCenter(a)) < 0.0f ) bestAxis = -bestAxis; if ( uf::vector::dot(bestAxis, impl::triangleCenter(b) - impl::triangleCenter(a)) < 0.0f ) bestAxis = -bestAxis;
int polyCount = 3;
pod::Vector3f poly[8];
poly[0] = b.points[0];
poly[1] = b.points[1];
poly[2] = b.points[2];
for ( auto i = 0; i < 3; i++ ) { for ( auto i = 0; i < 3; i++ ) {
auto p0 = a.points[i]; auto p0 = a.points[i];
auto p1 = a.points[(i+1)%3]; auto p1 = a.points[(i+1)%3];
auto edge = p1 - p0; auto edge = p1 - p0;
auto edgeNormal = uf::vector::normalize(uf::vector::cross(bestAxis, edge)); auto edgeNormal = uf::vector::normalize(uf::vector::cross(bestAxis, edge));
clipAgainstPlane(edgeNormal, p0);
impl::clipPolygon( poly, polyCount, pod::Plane{ edgeNormal, uf::vector::dot(edgeNormal, p0) } );
if ( polyCount == 0 ) return false; if ( polyCount == 0 ) return false;
} }
// build manifold // build manifold
float penetration = std::max( minOverlap, 0.05f ); // slop float penetration = std::max( minOverlap, 0.05f ); // slop
for (int i = 0; i < polyCount; i++) { for ( auto i = 0; i < polyCount; i++ ) {
manifold.points.emplace_back(pod::Contact{ poly[i], bestAxis, penetration }); manifold.points.emplace_back(pod::Contact{ poly[i], bestAxis, penetration });
} }
@ -124,142 +104,110 @@ bool impl::triangleTriangle( const pod::TriangleWithNormal& a, const pod::Triang
bool impl::triangleAabb( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold ) { bool impl::triangleAabb( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold ) {
if ( uf::physics::settings.useGjk ) return impl::triangleGeneric( tri, body, manifold ); if ( uf::physics::settings.useGjk ) return impl::triangleGeneric( tri, body, manifold );
const auto& aabb = body.bounds; const auto& box = body.bounds;
pod::Vector3f axes[3] = { {1,0,0}, {0,1,0}, {0,0,1} };
// box center and half extents
pod::Vector3f boxCenter = impl::aabbCenter( aabb );
pod::Vector3f boxHalf = impl::aabbExtent( aabb );
// move triangle into box-local space
pod::Vector3f v0 = tri.points[0] - boxCenter;
pod::Vector3f v1 = tri.points[1] - boxCenter;
pod::Vector3f v2 = tri.points[2] - boxCenter;
// triangle edges
pod::Vector3f e0 = v1 - v0;
pod::Vector3f e1 = v2 - v1;
pod::Vector3f e2 = v0 - v2;
float minOverlap = FLT_MAX; float minOverlap = FLT_MAX;
pod::Vector3f bestAxis; pod::Vector3f bestAxis;
if ( !impl::testSeparatingAxis( tri, box, tri.normal, axes, minOverlap, bestAxis ) ) return false;
if ( !impl::testSeparatingAxis( tri, box, axes[0], axes, minOverlap, bestAxis ) ) return false;
if ( !impl::testSeparatingAxis( tri, box, axes[1], axes, minOverlap, bestAxis ) ) return false;
if ( !impl::testSeparatingAxis( tri, box, axes[2], axes, minOverlap, bestAxis ) ) return false;
auto testAxis = [&](const pod::Vector3f& axis) -> bool { // test edges (3 triangle edges crossed with 3 AABB axes)
if ( uf::vector::magnitude( axis ) < EPS2 ) return true; // skip degenerate pod::Vector3f edges[3] = {
tri.points[1] - tri.points[0],
pod::Vector3f n = uf::vector::normalize(axis); tri.points[2] - tri.points[1],
tri.points[0] - tri.points[2]
// project triangle
float t0 = uf::vector::dot(v0, n);
float t1 = uf::vector::dot(v1, n);
float t2 = uf::vector::dot(v2, n);
float triMin = std::min({t0, t1, t2});
float triMax = std::max({t0, t1, t2});
// project box (radius along axis)
float r = boxHalf.x * fabs(n.x) + boxHalf.y * fabs(n.y) + boxHalf.z * fabs(n.z); // to-do: use boxHalf + uf::vector::abs( n ) or something
// overlap test
if ( triMin > r || triMax < -r ) return false; // separating axis
// compute overlap depth
float overlap = std::min(triMax + r, r - triMin);
if ( overlap < minOverlap ) {
minOverlap = overlap;
bestAxis = n;
}
return true;
}; };
for ( int i = 0; i < 3; i++ ) {
if ( !testAxis({1,0,0}) ) return false; for ( int j = 0; j < 3; j++ ) {
if ( !testAxis({0,1,0}) ) return false; auto axis = uf::vector::cross(edges[i], axes[j]);
if ( !testAxis({0,0,1}) ) return false; if ( !impl::testSeparatingAxis( tri, box, axis, axes, minOverlap, bestAxis ) ) return false;
if ( !testAxis(uf::vector::cross(e0, e1)) ) return false; }
pod::Vector3f boxAxes[3] = { {1,0,0}, {0,1,0}, {0,0,1} };
pod::Vector3f edges[3] = { e0, e1, e2 };
for ( auto& edge : edges ) {
for ( auto& axis : boxAxes ) if ( !testAxis(uf::vector::cross(edge, axis)) ) return false;
} }
pod::Vector3f triNormal = uf::vector::normalize(uf::vector::cross(e0, e1)); auto cB = impl::aabbCenter(box);
float planeDist = uf::vector::dot(triNormal, v0); auto eB = impl::aabbExtent(box);
if ( uf::vector::dot(bestAxis, triNormal) < 0.0f ) bestAxis = -bestAxis; auto cT = impl::triangleCenter(tri);
pod::Vector3f contact = boxCenter - bestAxis * (boxHalf.x * fabs(bestAxis.x) + boxHalf.y * fabs(bestAxis.y) + boxHalf.z * fabs(bestAxis.z)); if ( uf::vector::dot( bestAxis, cB - cT ) < 0.0f ) bestAxis = -bestAxis;
int polyCount = 3;
pod::Vector3f poly[8];
poly[0] = tri.points[0];
poly[1] = tri.points[1];
poly[2] = tri.points[2];
impl::clipPolygon( poly, polyCount, pod::Plane{ { 1, 0, 0}, eB.x + cB.x} );
impl::clipPolygon( poly, polyCount, pod::Plane{ {-1, 0, 0}, eB.x - cB.x} );
impl::clipPolygon( poly, polyCount, pod::Plane{ { 0, 1, 0}, eB.y + cB.y} );
impl::clipPolygon( poly, polyCount, pod::Plane{ { 0, -1, 0}, eB.y - cB.y} );
impl::clipPolygon( poly, polyCount, pod::Plane{ { 0, 0, 1}, eB.z + cB.z} );
impl::clipPolygon( poly, polyCount, pod::Plane{ { 0, 0, -1}, eB.z - cB.z} );
if ( polyCount == 0 ) return false;
for ( auto i = 0; i < polyCount; i++ ) {
manifold.points.emplace_back( pod::Contact{ poly[i], bestAxis, minOverlap } );
}
manifold.points.emplace_back( pod::Contact{ contact, bestAxis, minOverlap } );
return true; return true;
} }
bool impl::triangleObb( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold ) { bool impl::triangleObb( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold ) {
if ( uf::physics::settings.useGjk ) return impl::triangleGeneric( tri, body, manifold ); if ( uf::physics::settings.useGjk ) return impl::triangleGeneric( tri, body, manifold );
auto tB = impl::getTransform( body ); const auto& box = body.bounds;
auto tB = impl::getTransform( body );
pod::Vector3f cB = uf::transform::apply( tB, (body.collider.obb.max + body.collider.obb.min) * 0.5f ); pod::Vector3f axes[3] = {
pod::Vector3f eB = (body.collider.obb.max - body.collider.obb.min) * 0.5f;
pod::Vector3f axesB[3] = {
uf::quaternion::rotate(tB.orientation, pod::Vector3f{1,0,0}), uf::quaternion::rotate(tB.orientation, pod::Vector3f{1,0,0}),
uf::quaternion::rotate(tB.orientation, pod::Vector3f{0,1,0}), uf::quaternion::rotate(tB.orientation, pod::Vector3f{0,1,0}),
uf::quaternion::rotate(tB.orientation, pod::Vector3f{0,0,1}) uf::quaternion::rotate(tB.orientation, pod::Vector3f{0,0,1})
}; };
pod::Vector3f v0 = tri.points[0];
pod::Vector3f v1 = tri.points[1];
pod::Vector3f v2 = tri.points[2];
pod::Vector3f e0 = v1 - v0;
pod::Vector3f e1 = v2 - v1;
pod::Vector3f e2 = v0 - v2;
pod::Vector3f edges[3] = { e0, e1, e2 };
pod::Vector3f triNormal = tri.normal;
float minOverlap = FLT_MAX; float minOverlap = FLT_MAX;
pod::Vector3f bestAxis; pod::Vector3f bestAxis;
auto testAxis = [&](const pod::Vector3f& axis) -> bool { if ( !impl::testSeparatingAxis( tri, box, tri.normal, axes, minOverlap, bestAxis ) ) return false;
float mag = uf::vector::magnitude(axis); if ( !impl::testSeparatingAxis( tri, box, axes[0], axes, minOverlap, bestAxis ) ) return false;
if (mag < EPS) return true; // degenerate if ( !impl::testSeparatingAxis( tri, box, axes[1], axes, minOverlap, bestAxis ) ) return false;
pod::Vector3f n = axis / mag; if ( !impl::testSeparatingAxis( tri, box, axes[2], axes, minOverlap, bestAxis ) ) return false;
float p0 = uf::vector::dot(v0, n); pod::Vector3f edges[3] = {
float p1 = uf::vector::dot(v1, n); tri.points[1] - tri.points[0],
float p2 = uf::vector::dot(v2, n); tri.points[2] - tri.points[1],
float minT = std::min({p0, p1, p2}); tri.points[0] - tri.points[2]
float maxT = std::max({p0, p1, p2}); };
for ( auto i = 0; i < 3; i++ ) {
float pB = uf::vector::dot(cB, n); for ( auto j = 0; j < 3; j++ ) {
float rB = eB.x * std::fabs(uf::vector::dot(axesB[0], n)) + pod::Vector3f axis = uf::vector::cross( edges[i], axes[j] );
eB.y * std::fabs(uf::vector::dot(axesB[1], n)) + if ( !impl::testSeparatingAxis( tri, box, axis, axes, minOverlap, bestAxis ) ) return false;
eB.z * std::fabs(uf::vector::dot(axesB[2], n));
float minB = pB - rB;
float maxB = pB + rB;
if ( minT > maxB || maxT < minB ) return false;
float overlap = std::min(maxT, maxB) - std::max(minT, minB);
if ( overlap < minOverlap ) {
minOverlap = overlap;
bestAxis = n;
} }
return true; }
};
if ( !testAxis(triNormal) ) return false; auto cB = impl::aabbCenter(box);
for ( auto i = 0; i < 3; ++i ) if ( !testAxis(axesB[i]) ) return false;; auto eB = impl::aabbExtent(box);
for ( auto i = 0; i < 3; ++i ) { auto cT = impl::triangleCenter(tri);
for ( auto j = 0; j < 3; j++ ) if ( !testAxis(uf::vector::cross(edges[i], axesB[j])) ) return false; if ( uf::vector::dot( bestAxis, cB - cT ) < 0.0f ) bestAxis = -bestAxis;
};
pod::Vector3f triCenter = (v0 + v1 + v2) / 3.0f; int polyCount = 3;
if ( uf::vector::dot(bestAxis, cB - triCenter) < 0.0f ) bestAxis = -bestAxis; pod::Vector3f poly[8];
poly[0] = tri.points[0];
poly[1] = tri.points[1];
poly[2] = tri.points[2];
float rB = eB.x * std::fabs(uf::vector::dot(axesB[0], bestAxis)) + impl::clipPolygon( poly, polyCount, pod::Plane{ axes[0], eB.x + uf::vector::dot(axes[0], cB) });
eB.y * std::fabs(uf::vector::dot(axesB[1], bestAxis)) + impl::clipPolygon( poly, polyCount, pod::Plane{ -axes[0], eB.x - uf::vector::dot(axes[0], cB) });
eB.z * std::fabs(uf::vector::dot(axesB[2], bestAxis)); impl::clipPolygon( poly, polyCount, pod::Plane{ axes[1], eB.y + uf::vector::dot(axes[1], cB) });
impl::clipPolygon( poly, polyCount, pod::Plane{ -axes[1], eB.y - uf::vector::dot(axes[1], cB) });
impl::clipPolygon( poly, polyCount, pod::Plane{ axes[2], eB.z + uf::vector::dot(axes[2], cB) });
impl::clipPolygon( poly, polyCount, pod::Plane{ -axes[2], eB.z - uf::vector::dot(axes[2], cB) });
pod::Vector3f contact = cB - bestAxis * rB; if ( polyCount == 0 ) return false;
for ( auto i = 0; i < polyCount; i++ ) {
manifold.points.emplace_back( pod::Contact{ poly[i], bestAxis, minOverlap } );
}
manifold.points.emplace_back( pod::Contact{ contact, bestAxis, minOverlap } );
return true; return true;
} }
bool impl::triangleSphere( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold ) { bool impl::triangleSphere( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold ) {
@ -292,7 +240,6 @@ bool impl::triangleSphere( const pod::TriangleWithNormal& tri, const pod::Physic
manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); manifold.points.emplace_back(pod::Contact{ contact, normal, penetration });
return true; return true;
} }
// to-do: implement
bool impl::trianglePlane( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold ) { bool impl::trianglePlane( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold ) {
if ( uf::physics::settings.useGjk ) return impl::triangleGeneric( tri, body, manifold ); if ( uf::physics::settings.useGjk ) return impl::triangleGeneric( tri, body, manifold );
@ -374,9 +321,7 @@ bool impl::triangleCapsule( const pod::TriangleWithNormal& tri, const pod::Physi
return true; return true;
} }
bool impl::triangleHull( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold ) { bool impl::triangleHull( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold ) {
pod::PhysicsBody triView = {}; auto triView = impl::physicsBodyTriView( tri );
triView.collider.type = pod::ShapeType::TRIANGLE;
triView.collider.triangle = tri;
return impl::triangleHull( triView, body, manifold ); return impl::triangleHull( triView, body, manifold );
} }