generate proper-ish contacts for triangleAabb/triangleObb, code cleanup
This commit is contained in:
parent
a1f62ef0c3
commit
f054874fed
@ -337,11 +337,11 @@
|
||||
},
|
||||
"physics": {
|
||||
"warmup solver": false,
|
||||
"block solver": true,
|
||||
"psg solver": true,
|
||||
"correction percent": 0.0,
|
||||
"block solver": false,
|
||||
"psg solver": false,
|
||||
"correction percent": 0.01,
|
||||
"gjk": false,
|
||||
"debug draw": false,
|
||||
"debug draw": true,
|
||||
"fixed step": true,
|
||||
"substeps": 4,
|
||||
"solver iterations": 10
|
||||
|
||||
@ -12,17 +12,22 @@ namespace impl {
|
||||
|
||||
pod::Transform<> getTransform( const pod::PhysicsBody& body );
|
||||
pod::Vector3f getPosition( const pod::PhysicsBody& body, bool useTransform = false );
|
||||
|
||||
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 );
|
||||
|
||||
bool shouldCollide( const pod::Collider& a, const pod::Collider& b );
|
||||
bool shouldCollide( const pod::PhysicsBody& a, 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 computeTangent( const pod::Vector3f& normal );
|
||||
|
||||
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 );
|
||||
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::Triangle& tri, bool clamps = false );
|
||||
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 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 );
|
||||
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 triangleNormal( const pod::Triangle& 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 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 );
|
||||
@ -45,8 +52,6 @@ namespace impl {
|
||||
/*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, 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*/ float aabbSurfaceArea( const pod::AABB& aabb );
|
||||
@ -61,7 +66,5 @@ namespace impl {
|
||||
pod::AABB transformAabbToWorld( const pod::AABB& localBox, const pod::Transform<>& transform );
|
||||
std::pair<pod::Vector3f, pod::Vector3f> getCapsuleSegment( 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 );
|
||||
}
|
||||
@ -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*/ 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*/ 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*/ 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
|
||||
|
||||
@ -486,6 +486,14 @@ T& uf::vector::normalize_( T& vector ) {
|
||||
return ( norm < 1.0e-6 ) ? T{} : ( vector = uf::vector::divide((const T&) vector, norm) );
|
||||
}
|
||||
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 ) {
|
||||
#if UF_USE_SIMD
|
||||
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;
|
||||
}
|
||||
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 ) {
|
||||
#if UF_USE_SIMD
|
||||
if constexpr ( simd_able_v<typename T::type_t> ) {
|
||||
|
||||
@ -6,401 +6,121 @@
|
||||
|
||||
namespace uf {
|
||||
namespace shapes {
|
||||
inline float planePointSide( const pod::Plane& plane, const pod::Vector3f& point ) {
|
||||
return uf::vector::dot( point, plane.normal ) - plane.offset;
|
||||
}
|
||||
template <typename T>
|
||||
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 ) {
|
||||
return (plane.offset - uf::vector::dot(p1, plane.normal)) / uf::vector::dot(uf::vector::subtract(p2, p1), plane.normal);
|
||||
}
|
||||
int outCount = 0;
|
||||
T out[16];
|
||||
|
||||
template<typename T, typename U = uint32_t>
|
||||
uf::stl::vector<pod::Polygon<T>> verticesPolygonate( const uf::stl::vector<T>& vertices, const uf::stl::vector<U>& indices ) {
|
||||
uf::stl::vector<pod::Polygon<T>> polygons;
|
||||
polygons.reserve( indices.size() / 3 );
|
||||
for ( auto i = 0; i < polyCount; i++ ) {
|
||||
const T& curr = poly[i];
|
||||
const T& prev = poly[(i + polyCount - 1) % polyCount];
|
||||
|
||||
for ( auto i = 0; i < indices.size(); i += 3 ) {
|
||||
polygons.emplace_back( pod::Polygon<T>{ { vertices[indices[i+0]], vertices[indices[i+1]], vertices[indices[i+2]] } });
|
||||
}
|
||||
float dCurr = uf::vector::dot(plane.normal, curr.position) - plane.offset;
|
||||
float dPrev = uf::vector::dot(plane.normal, prev.position) - plane.offset;
|
||||
|
||||
return polygons;
|
||||
}
|
||||
|
||||
template<typename 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 );
|
||||
if ( dCurr >= 0.0f ) {
|
||||
if ( dPrev < 0.0f ) {
|
||||
float t = dPrev / (dPrev - dCurr);
|
||||
out[outCount++] = T::interpolate( prev, curr, t );
|
||||
}
|
||||
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>
|
||||
pod::Polygon<T> polygonPlaneClip( const pod::Polygon<T>& polygon, const pod::Plane& plane ) {
|
||||
pod::Polygon<T> clipped;
|
||||
|
||||
auto count = polygon.points.size();
|
||||
bool modified = false;
|
||||
for (auto i = 0; i < count; ++i) {
|
||||
const T& current = polygon.points[i];
|
||||
const T& previous = polygon.points[(i - 1 + count) % count];
|
||||
|
||||
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 }
|
||||
template <typename T>
|
||||
void clipPolygon( T* poly, int& polyCount, const pod::AABB& bounds ) {
|
||||
pod::Plane planes[6] = {
|
||||
{ pod::Vector3f{-1, 0, 0}, -bounds.max.x },
|
||||
{ pod::Vector3f{ 1, 0, 0}, bounds.min.x },
|
||||
{ pod::Vector3f{ 0,-1, 0}, -bounds.max.y },
|
||||
{ pod::Vector3f{ 0, 1, 0}, bounds.min.y },
|
||||
{ pod::Vector3f{ 0, 0,-1}, -bounds.max.z },
|
||||
{ pod::Vector3f{ 0, 0, 1}, bounds.min.z }
|
||||
};
|
||||
|
||||
// clip against planes
|
||||
for ( const auto& plane : planes ) {
|
||||
uf::stl::vector<pod::Polygon<T>> clippedPolygons;
|
||||
for ( const auto& polygon : polygons ) {
|
||||
auto clipped = uf::shapes::polygonPlaneClip<T>( polygon, plane );
|
||||
if ( clipped.points.size() >= 3 ) clippedPolygons.emplace_back( clipped );
|
||||
for ( int i = 0; i < 6; i++ ) {
|
||||
clipPolygon<T>( poly, polyCount, planes[i] );
|
||||
if ( polyCount < 3 ) {
|
||||
polyCount = 0;
|
||||
break;
|
||||
}
|
||||
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>
|
||||
void clip(
|
||||
uf::stl::vector<T>& vertices,
|
||||
uf::stl::vector<U>& indices,
|
||||
void clip( uf::stl::vector<T>& vertices, uf::stl::vector<U>& indices, const pod::AABB& bounds ) {
|
||||
if ( indices.empty() || vertices.empty() ) return;
|
||||
|
||||
const pod::Vector3f& min,
|
||||
const pod::Vector3f& max,
|
||||
uf::stl::vector<T> outVertices;
|
||||
uf::stl::vector<U> outIndices;
|
||||
|
||||
bool polygons = true
|
||||
) {
|
||||
if ( polygons ) {
|
||||
// convert to polys
|
||||
auto polygons = uf::shapes::verticesPolygonate<T, U>( vertices, indices );
|
||||
// clip against planes
|
||||
uf::shapes::clip<T>( polygons, min, max );
|
||||
// triangulate
|
||||
vertices = uf::shapes::polygonsTriangulate( polygons );
|
||||
indices = uf::shapes::verticesIndex( vertices );
|
||||
return;
|
||||
outVertices.reserve( vertices.size() );
|
||||
outIndices.reserve( indices.size() );
|
||||
|
||||
for ( size_t i = 0; i < indices.size(); i += 3 ) {
|
||||
T poly[16];
|
||||
poly[0] = vertices[indices[i + 0]];
|
||||
poly[1] = vertices[indices[i + 1]];
|
||||
poly[2] = vertices[indices[i + 2]];
|
||||
int polyCount = 3;
|
||||
|
||||
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 = uf::shapes::indexedVertices( vertices, indices );
|
||||
// clip vertices
|
||||
uf::shapes::clip( vertices, min, max );
|
||||
// reindex
|
||||
indices = uf::shapes::verticesIndex( vertices );
|
||||
vertices = std::move( outVertices );
|
||||
indices = std::move( outIndices );
|
||||
}
|
||||
|
||||
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 );
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@ -85,6 +85,7 @@ namespace uf {
|
||||
uf::meshgrid::calculate( grid, eps );
|
||||
|
||||
// it's better to naively clip the mesh multiple times rather than calculate the triangles needed to clip
|
||||
#if 0
|
||||
if ( clip ) {
|
||||
for ( auto& pair : grid.nodes ) {
|
||||
++atlasID;
|
||||
@ -94,7 +95,7 @@ namespace uf {
|
||||
uf::stl::vector<T> vertices = meshlet.vertices;
|
||||
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;
|
||||
|
||||
@ -130,6 +131,7 @@ namespace uf {
|
||||
|
||||
return partitioned;
|
||||
}
|
||||
#endif
|
||||
|
||||
for ( auto& meshlet : meshlets ) uf::meshgrid::partition<T,U>( grid, meshlet.vertices, meshlet.indices, meshlet.primitive );
|
||||
if ( cleanup ) uf::meshgrid::cleanup( grid );
|
||||
@ -154,13 +156,13 @@ namespace uf {
|
||||
vertex.id.y = meshlet.primitive.instance.meshID;
|
||||
}
|
||||
|
||||
/*
|
||||
#if 1
|
||||
if ( clip ) {
|
||||
node.effectiveExtents.min = node.extents.min;
|
||||
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.materialID = meshlet.primitive.instance.materialID;
|
||||
|
||||
@ -1,14 +1,5 @@
|
||||
#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
|
||||
uint64_t impl::makePairKey( const pod::PhysicsBody& a, const pod::PhysicsBody& b ) {
|
||||
uint64_t lhs = reinterpret_cast<uint64_t>(&a);
|
||||
@ -28,8 +19,13 @@ void impl::wakeBody( pod::PhysicsBody& body ) {
|
||||
}
|
||||
|
||||
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 ) {
|
||||
bool wasAsleep = !body.activity.awake;
|
||||
|
||||
@ -37,6 +33,7 @@ void impl::sleepBody( pod::PhysicsBody& body ) {
|
||||
body.velocity = pod::Vector3f{};
|
||||
body.angularVelocity = pod::Vector3f{};
|
||||
}
|
||||
// update body's grounded / sleep states
|
||||
void impl::updateActivity( pod::PhysicsBody& body, float dt ) {
|
||||
// reset grounded state
|
||||
bool wasGrounded = body.activity.grounded;
|
||||
@ -72,21 +69,19 @@ pod::Transform<> impl::getTransform( const pod::PhysicsBody& body ) {
|
||||
t.reference = body.transform;
|
||||
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 ) {
|
||||
useTransform = true; // guh
|
||||
|
||||
if ( !useTransform ) return impl::aabbCenter( body.bounds );
|
||||
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 view = body;
|
||||
view.viewIndex = index;
|
||||
return view;
|
||||
}
|
||||
|
||||
pod::PhysicsBody impl::physicsBodyTriView( const pod::PhysicsBody& body, const pod::TriangleWithNormal triangle ) {
|
||||
// creates a view of a triangle
|
||||
pod::PhysicsBody impl::physicsBodyTriView( const pod::TriangleWithNormal triangle, const pod::PhysicsBody& body ) {
|
||||
pod::PhysicsBody view = body;
|
||||
view.collider.type = pod::ShapeType::TRIANGLE;
|
||||
view.collider.triangle = triangle;
|
||||
@ -95,11 +90,12 @@ pod::PhysicsBody impl::physicsBodyTriView( const pod::PhysicsBody& body, const p
|
||||
view.transform = NULL;
|
||||
return view;
|
||||
}
|
||||
// creates a view of a mesh's triangle by triID
|
||||
pod::PhysicsBody impl::physicsBodyTriView( const pod::PhysicsBody& body, size_t triID ) {
|
||||
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 ) {
|
||||
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 );
|
||||
}
|
||||
|
||||
// returns an inverse inertia matrix from an inertia tensor
|
||||
pod::Matrix3f impl::computeWorldInverseInertia( const pod::PhysicsBody& b ) {
|
||||
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;
|
||||
}
|
||||
|
||||
// computes the tangent of a 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 tangent = uf::vector::normalize( uf::vector::cross( up, normal ) );
|
||||
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 ab = b - a;
|
||||
float t = uf::vector::dot(p - a, ab) / uf::vector::dot(ab, ab);
|
||||
t = std::clamp( t, 0.0f, 1.0f );
|
||||
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 ) {
|
||||
auto u = B - A;
|
||||
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 };
|
||||
}
|
||||
|
||||
//
|
||||
pod::Vector3f impl::closestPointSegmentAabb( const pod::Vector3f& p1, const pod::Vector3f& p2, const pod::AABB& box ) {
|
||||
// AABB center and half extents
|
||||
auto c = ( box.min + box.max ) * 0.5f;
|
||||
auto e = ( box.max - box.min ) * 0.5f;
|
||||
auto c = impl::aabbCenter( box );
|
||||
auto e = impl::aabbExtent( box );
|
||||
|
||||
// direction of line segment
|
||||
auto d = p2 - p1;
|
||||
@ -207,7 +206,7 @@ pod::Vector3f impl::closestPointSegmentAabb( const pod::Vector3f& p1, const pod:
|
||||
// clamp this point into AABB
|
||||
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 ) {
|
||||
// edges
|
||||
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] ) {
|
||||
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 ) {
|
||||
auto bary = impl::computeBarycentric( p, a, b, c, false );
|
||||
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 );
|
||||
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 ) {
|
||||
// check if P in vertex region outside 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 ) {
|
||||
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 ) {
|
||||
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 best = std::numeric_limits<float>::max();
|
||||
|
||||
@ -382,64 +381,103 @@ float impl::segmentTriangleDistanceSq( const pod::Vector3f& p0, const pod::Vecto
|
||||
return best;
|
||||
}
|
||||
|
||||
int impl::clipPolygonAgainstPlane( const pod::Vector3f* inPoly, int inCount, const pod::Vector3f& planeNormal, float planeOffset, pod::Vector3f* outPoly ) {
|
||||
if (inCount == 0) return 0;
|
||||
// Separating Axis Theorem test
|
||||
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;
|
||||
pod::Vector3f prevPoint = inPoly[inCount - 1];
|
||||
float prevDistance = uf::vector::dot(prevPoint, planeNormal) - planeOffset;
|
||||
// project triangle
|
||||
float p0 = uf::vector::dot( triangle.points[0], n );
|
||||
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) {
|
||||
pod::Vector3f currPoint = inPoly[i];
|
||||
float currDistance = uf::vector::dot(currPoint, planeNormal) - planeOffset;
|
||||
// project box
|
||||
pod::Vector3f cB = impl::aabbCenter( box );
|
||||
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
|
||||
if ((prevDistance * currDistance) < 0.0f) {
|
||||
float t = prevDistance / (prevDistance - currDistance);
|
||||
outPoly[outCount++] = prevPoint + (currPoint - prevPoint) * t;
|
||||
}
|
||||
float minB = pB - rB;
|
||||
float maxB = pB + rB;
|
||||
|
||||
// If the current point is 'inside' or on the plane (distance <= 0), keep it
|
||||
if (currDistance <= 0.0f) {
|
||||
outPoly[outCount++] = currPoint;
|
||||
}
|
||||
// check for separation
|
||||
if ( minT > maxB || maxT < minB ) return false;
|
||||
|
||||
prevPoint = currPoint;
|
||||
prevDistance = currDistance;
|
||||
// calculate overlap depth
|
||||
float overlap = std::min(maxT, maxB) - std::max(minT, minB);
|
||||
if ( overlap < outMinOverlap ) {
|
||||
outMinOverlap = overlap;
|
||||
outBestAxis = n;
|
||||
}
|
||||
|
||||
return outCount;
|
||||
return true;
|
||||
}
|
||||
|
||||
// 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 ) {
|
||||
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 ) {
|
||||
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 ) {
|
||||
return tri.normal;
|
||||
//return uf::vector::normalize( tri.normals[0] + tri.normals[1] + tri.normals[2] );
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
// mesh accessing
|
||||
size_t impl::getIndex( const void* pointer, size_t stride, size_t index ) {
|
||||
#define CAST_INDEX(T) case sizeof(T): return ((T*) pointer)[index];
|
||||
switch ( stride ) {
|
||||
@ -551,91 +589,7 @@ pod::TriangleWithNormal impl::fetchTriangle( const uf::Mesh& mesh, size_t triID,
|
||||
return tri;
|
||||
}
|
||||
|
||||
bool impl::computeTriangleTriangleSegment( const pod::TriangleWithNormal& A, const pod::TriangleWithNormal& B, pod::Vector3f& p0, pod::Vector3f& p1 ) {
|
||||
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 }) };
|
||||
}
|
||||
|
||||
// returns whether or not two AABBs are overlapping (with SIMD speedup)
|
||||
bool impl::aabbOverlap( const pod::AABB& a, const pod::AABB& b ) {
|
||||
#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 ) );
|
||||
@ -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 );
|
||||
#endif
|
||||
}
|
||||
|
||||
// returns the surface area an AABB covers
|
||||
float impl::aabbSurfaceArea(const pod::AABB& aabb) {
|
||||
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);
|
||||
}
|
||||
|
||||
// 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 ) {
|
||||
return {
|
||||
uf::vector::min( 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) {
|
||||
return uf::vector::clamp( p, box.min, box.max );
|
||||
}
|
||||
|
||||
// returns the AABB of a triangle
|
||||
pod::AABB impl::computeTriangleAABB( const pod::Triangle& tri ) {
|
||||
return {
|
||||
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] ),
|
||||
};
|
||||
}
|
||||
|
||||
// returns the AABB of a hull
|
||||
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 ) {
|
||||
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 ) {
|
||||
return impl::computeConvexHullAABB( view, view["position"], bounds );
|
||||
}
|
||||
|
||||
// combines two AABBs
|
||||
pod::AABB impl::mergeAabb( const pod::AABB& a, const pod::AABB& b ) {
|
||||
return {
|
||||
uf::vector::min( a.min, b.min ),
|
||||
uf::vector::max( a.max, b.max ),
|
||||
};
|
||||
}
|
||||
|
||||
// returns the center of an AABB
|
||||
pod::Vector3f impl::aabbCenter( const pod::AABB& aabb ) {
|
||||
return ( aabb.max + aabb.min ) * 0.5f;
|
||||
}
|
||||
// returns the half extents of an AABB
|
||||
pod::Vector3f impl::aabbExtent( const pod::AABB& aabb ) {
|
||||
return ( aabb.max - aabb.min ) * 0.5f;
|
||||
}
|
||||
|
||||
pod::AABB impl::transformAabbToWorld( const pod::AABB& localBox, const pod::Transform<>& transform ) {
|
||||
// transforms an AABB into world-space
|
||||
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& p = transform.position;
|
||||
|
||||
pod::Vector3f center = (localBox.min + localBox.max) * 0.5f;
|
||||
pod::Vector3f extents = (localBox.max - localBox.min) * 0.5f;
|
||||
|
||||
pod::Vector3f axisX = uf::quaternion::rotate(q, pod::Vector3f{1,0,0});
|
||||
pod::Vector3f axisY = uf::quaternion::rotate(q, pod::Vector3f{0,1,0});
|
||||
pod::Vector3f axisZ = 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
|
||||
pod::Vector3f cB = impl::aabbCenter( aabb );
|
||||
pod::Vector3f eB = impl::aabbExtent( aabb );
|
||||
pod::Vector3f axes[] = {
|
||||
uf::quaternion::rotate(q, pod::Vector3f{1,0,0}),
|
||||
uf::quaternion::rotate(q, pod::Vector3f{0,1,0}),
|
||||
uf::quaternion::rotate(q, pod::Vector3f{0,0,1}),
|
||||
};
|
||||
|
||||
return {
|
||||
worldCenter - worldExtents,
|
||||
worldCenter + worldExtents
|
||||
pod::Vector3f cW = uf::quaternion::rotate(q, cB) + p;
|
||||
// to-do: a cleaner way of doing this with uf::vector::abs
|
||||
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 ) {
|
||||
const auto transform = impl::getTransform( body );
|
||||
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;
|
||||
return { p1, p2 };
|
||||
}
|
||||
|
||||
// computes the AABB for a given body
|
||||
pod::AABB impl::computeAABB( const pod::PhysicsBody& body ) {
|
||||
const auto transform = impl::getTransform( body );
|
||||
switch ( body.collider.type ) {
|
||||
@ -768,77 +722,7 @@ pod::AABB impl::computeAABB( const pod::PhysicsBody& body ) {
|
||||
|
||||
return {};
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
// transforms an AABB into local space
|
||||
pod::AABB impl::transformAabbToLocal( const pod::AABB& box, const pod::Transform<>& transform ) {
|
||||
auto inv = uf::transform::inverse( transform );
|
||||
|
||||
|
||||
@ -83,7 +83,7 @@ void impl::draw( const pod::World& world, float dt ) {
|
||||
|
||||
::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(); ) {
|
||||
auto& line = it->second;
|
||||
|
||||
|
||||
@ -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 ) {
|
||||
if ( !uf::vector::isValid(contact.point) ) return false;
|
||||
|
||||
|
||||
auto& normal = contact.normal;
|
||||
|
||||
pod::Vector3f polyA[4];
|
||||
@ -145,36 +145,33 @@ bool impl::generateClippingManifold( const pod::PhysicsBody& a, const pod::Physi
|
||||
int refCount = countA;
|
||||
int incCount = countB;
|
||||
|
||||
pod::Vector3f clipBuffer1[8];
|
||||
pod::Vector3f clipBuffer2[8];
|
||||
pod::Vector3f clipBuffer[8];
|
||||
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]));
|
||||
|
||||
for ( auto i = 0; i < refCount; ++i ) {
|
||||
pod::Vector3f edgeStart = refPoly[i];
|
||||
pod::Vector3f edgeEnd = refPoly[(i + 1) % refCount];
|
||||
pod::Vector3f edgeVector = edgeEnd - edgeStart;
|
||||
|
||||
pod::Vector3f edgePlaneNormal = uf::vector::normalize(uf::vector::cross(edgeVector, refNormal));
|
||||
float edgePlaneOffset = uf::vector::dot(edgePlaneNormal, edgeStart);
|
||||
pod::Plane edgePlane;
|
||||
edgePlane.normal = uf::vector::normalize(uf::vector::cross(edgeVector, refNormal));
|
||||
edgePlane.offset = uf::vector::dot(edgePlane.normal, edgeStart);
|
||||
|
||||
if (i % 2 == 0) {
|
||||
clipCount = impl::clipPolygonAgainstPlane(clipBuffer1, clipCount, edgePlaneNormal, edgePlaneOffset, clipBuffer2);
|
||||
} else {
|
||||
clipCount = impl::clipPolygonAgainstPlane(clipBuffer2, clipCount, edgePlaneNormal, edgePlaneOffset, clipBuffer1);
|
||||
}
|
||||
impl::clipPolygon( clipBuffer, clipCount, edgePlane );
|
||||
if ( clipCount == 0 ) break;
|
||||
}
|
||||
|
||||
pod::Vector3f* finalPoly = (refCount % 2 == 0) ? clipBuffer1 : clipBuffer2;
|
||||
float refOffset = uf::vector::dot(refNormal, refPoly[0]);
|
||||
|
||||
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
|
||||
if ( distance <= EPS ) {
|
||||
pod::Contact c;
|
||||
c.point = finalPoly[i] - refNormal * (distance * 0.5f);
|
||||
c.point = clipBuffer[i] - refNormal * (distance * 0.5f);
|
||||
c.normal = normal;
|
||||
c.penetration = -distance;
|
||||
manifold.points.emplace_back(c);
|
||||
|
||||
@ -14,9 +14,7 @@ namespace impl {
|
||||
}
|
||||
|
||||
bool triangleGeneric( const pod::TriangleWithNormal& a, const pod::PhysicsBody& b, pod::Manifold& manifold ) {
|
||||
pod::PhysicsBody tri = {};
|
||||
tri.collider.type = pod::ShapeType::TRIANGLE;
|
||||
tri.collider.triangle = a;
|
||||
auto tri = impl::physicsBodyTriView( a );
|
||||
const auto& body = b;
|
||||
|
||||
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 ) {
|
||||
if ( uf::physics::settings.useGjk ) {
|
||||
pod::PhysicsBody A = {};
|
||||
A.collider.type = pod::ShapeType::TRIANGLE;
|
||||
A.collider.triangle = a;
|
||||
|
||||
pod::PhysicsBody B = {};
|
||||
B.collider.type = pod::ShapeType::TRIANGLE;
|
||||
B.collider.triangle = b;
|
||||
auto A = impl::physicsBodyTriView( a );
|
||||
auto B = impl::physicsBodyTriView( b );
|
||||
|
||||
return impl::triangleGeneric( A, B, manifold );
|
||||
}
|
||||
|
||||
size_t axes = 0;
|
||||
pod::Vector3f axesBuffer[12];
|
||||
axesBuffer[axes++] = impl::triangleNormal(a);
|
||||
axesBuffer[axes++] = impl::triangleNormal(b);
|
||||
size_t axesCount = 0;
|
||||
pod::Vector3f axes[14]; // 2 normals + 9 edge cross product
|
||||
axes[axesCount++] = impl::triangleNormal(a);
|
||||
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];
|
||||
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 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;
|
||||
pod::Vector3f bestAxis;
|
||||
|
||||
for ( auto& axis : axesBuffer ) {
|
||||
auto projA = impl::projectTriangleOntoAxis(a, axis);
|
||||
auto projB = impl::projectTriangleOntoAxis(b, axis);
|
||||
for ( auto i = 0; i < axesCount; i++ ) {
|
||||
const auto& axis = axes[i];
|
||||
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);
|
||||
if (overlap < 0) return false; // separating axis
|
||||
if ( overlap < 0 ) return false;
|
||||
|
||||
if (overlap < minOverlap) {
|
||||
if ( overlap < minOverlap ) {
|
||||
minOverlap = overlap;
|
||||
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;
|
||||
|
||||
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++ ) {
|
||||
auto p0 = a.points[i];
|
||||
auto p1 = a.points[(i+1)%3];
|
||||
auto edge = p1 - p0;
|
||||
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;
|
||||
}
|
||||
|
||||
// build manifold
|
||||
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 });
|
||||
}
|
||||
|
||||
@ -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 ) {
|
||||
if ( uf::physics::settings.useGjk ) return impl::triangleGeneric( tri, body, manifold );
|
||||
|
||||
const auto& aabb = body.bounds;
|
||||
|
||||
// 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;
|
||||
const auto& box = body.bounds;
|
||||
pod::Vector3f axes[3] = { {1,0,0}, {0,1,0}, {0,0,1} };
|
||||
|
||||
float minOverlap = FLT_MAX;
|
||||
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 {
|
||||
if ( uf::vector::magnitude( axis ) < EPS2 ) return true; // skip degenerate
|
||||
|
||||
pod::Vector3f n = uf::vector::normalize(axis);
|
||||
|
||||
// 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;
|
||||
// test edges (3 triangle edges crossed with 3 AABB axes)
|
||||
pod::Vector3f edges[3] = {
|
||||
tri.points[1] - tri.points[0],
|
||||
tri.points[2] - tri.points[1],
|
||||
tri.points[0] - tri.points[2]
|
||||
};
|
||||
|
||||
if ( !testAxis({1,0,0}) ) return false;
|
||||
if ( !testAxis({0,1,0}) ) return false;
|
||||
if ( !testAxis({0,0,1}) ) 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;
|
||||
for ( int i = 0; i < 3; i++ ) {
|
||||
for ( int j = 0; j < 3; j++ ) {
|
||||
auto axis = uf::vector::cross(edges[i], axes[j]);
|
||||
if ( !impl::testSeparatingAxis( tri, box, axis, axes, minOverlap, bestAxis ) ) return false;
|
||||
}
|
||||
}
|
||||
|
||||
pod::Vector3f triNormal = uf::vector::normalize(uf::vector::cross(e0, e1));
|
||||
float planeDist = uf::vector::dot(triNormal, v0);
|
||||
if ( uf::vector::dot(bestAxis, triNormal) < 0.0f ) bestAxis = -bestAxis;
|
||||
pod::Vector3f contact = boxCenter - bestAxis * (boxHalf.x * fabs(bestAxis.x) + boxHalf.y * fabs(bestAxis.y) + boxHalf.z * fabs(bestAxis.z));
|
||||
auto cB = impl::aabbCenter(box);
|
||||
auto eB = impl::aabbExtent(box);
|
||||
auto cT = impl::triangleCenter(tri);
|
||||
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;
|
||||
}
|
||||
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 );
|
||||
|
||||
auto tB = impl::getTransform( body );
|
||||
|
||||
pod::Vector3f cB = uf::transform::apply( tB, (body.collider.obb.max + body.collider.obb.min) * 0.5f );
|
||||
pod::Vector3f eB = (body.collider.obb.max - body.collider.obb.min) * 0.5f;
|
||||
pod::Vector3f axesB[3] = {
|
||||
const auto& box = body.bounds;
|
||||
auto tB = impl::getTransform( body );
|
||||
pod::Vector3f axes[3] = {
|
||||
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,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;
|
||||
pod::Vector3f bestAxis;
|
||||
|
||||
auto testAxis = [&](const pod::Vector3f& axis) -> bool {
|
||||
float mag = uf::vector::magnitude(axis);
|
||||
if (mag < EPS) return true; // degenerate
|
||||
pod::Vector3f n = axis / mag;
|
||||
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;
|
||||
|
||||
float p0 = uf::vector::dot(v0, n);
|
||||
float p1 = uf::vector::dot(v1, n);
|
||||
float p2 = uf::vector::dot(v2, n);
|
||||
float minT = std::min({p0, p1, p2});
|
||||
float maxT = std::max({p0, p1, p2});
|
||||
|
||||
float pB = uf::vector::dot(cB, n);
|
||||
float rB = eB.x * std::fabs(uf::vector::dot(axesB[0], n)) +
|
||||
eB.y * std::fabs(uf::vector::dot(axesB[1], n)) +
|
||||
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;
|
||||
pod::Vector3f edges[3] = {
|
||||
tri.points[1] - tri.points[0],
|
||||
tri.points[2] - tri.points[1],
|
||||
tri.points[0] - tri.points[2]
|
||||
};
|
||||
for ( auto i = 0; i < 3; i++ ) {
|
||||
for ( auto j = 0; j < 3; j++ ) {
|
||||
pod::Vector3f axis = uf::vector::cross( edges[i], axes[j] );
|
||||
if ( !impl::testSeparatingAxis( tri, box, axis, axes, minOverlap, bestAxis ) ) return false;
|
||||
}
|
||||
return true;
|
||||
};
|
||||
}
|
||||
|
||||
if ( !testAxis(triNormal) ) return false;
|
||||
for ( auto i = 0; i < 3; ++i ) if ( !testAxis(axesB[i]) ) return false;;
|
||||
for ( auto i = 0; i < 3; ++i ) {
|
||||
for ( auto j = 0; j < 3; j++ ) if ( !testAxis(uf::vector::cross(edges[i], axesB[j])) ) return false;
|
||||
};
|
||||
auto cB = impl::aabbCenter(box);
|
||||
auto eB = impl::aabbExtent(box);
|
||||
auto cT = impl::triangleCenter(tri);
|
||||
if ( uf::vector::dot( bestAxis, cB - cT ) < 0.0f ) bestAxis = -bestAxis;
|
||||
|
||||
pod::Vector3f triCenter = (v0 + v1 + v2) / 3.0f;
|
||||
if ( uf::vector::dot(bestAxis, cB - triCenter) < 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];
|
||||
|
||||
float rB = eB.x * std::fabs(uf::vector::dot(axesB[0], bestAxis)) +
|
||||
eB.y * std::fabs(uf::vector::dot(axesB[1], bestAxis)) +
|
||||
eB.z * std::fabs(uf::vector::dot(axesB[2], bestAxis));
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ axes[0], eB.x + uf::vector::dot(axes[0], cB) });
|
||||
impl::clipPolygon( poly, polyCount, pod::Plane{ -axes[0], eB.x - uf::vector::dot(axes[0], cB) });
|
||||
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;
|
||||
}
|
||||
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 });
|
||||
return true;
|
||||
}
|
||||
// to-do: implement
|
||||
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 );
|
||||
|
||||
@ -374,9 +321,7 @@ bool impl::triangleCapsule( const pod::TriangleWithNormal& tri, const pod::Physi
|
||||
return true;
|
||||
}
|
||||
bool impl::triangleHull( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold ) {
|
||||
pod::PhysicsBody triView = {};
|
||||
triView.collider.type = pod::ShapeType::TRIANGLE;
|
||||
triView.collider.triangle = tri;
|
||||
auto triView = impl::physicsBodyTriView( tri );
|
||||
|
||||
return impl::triangleHull( triView, body, manifold );
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user