AnotherFoxGuy/bounce
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

moved SAT code into collision module

Browse Source
This commit is contained in:
Irlan 2018-04-29 22:27:41 -03:00
parent 80ac01f13d
commit c9f0b0cf93
5 changed files with 183 additions and 187 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
include/bounce/collision/sat/sat.h
View File

@ -52,4 +52,57 @@ float32 b3Project(const b3Vec3& P1, const b3Vec3& E1, const b3Vec3& P2, const b3
b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xf1, const b3Hull* hull1,
const b3Transform& xf2, const b3Hull* hull2);
///////////////////////////////////////////////////////////////////////////////////////////////////
enum b3SATCacheType
{
e_separation,
e_overlap,
e_empty,
};
enum b3SATFeatureType
{
e_edge1, // an edge on hull 1 and an edge on hull 2
e_face1, // a face on hull 1 and a vertex/edge/face on hull 2
e_face2, // a face on hull 2 and a vertex/edge/face on hull 1
};
// A cached feature pair is used to improve the performance
// of the SAT when called more than once.
struct b3SATFeaturePair
{
b3SATCacheType state; // sat result
b3SATFeatureType type; // feature pair type
u32 index1; // feature index on hull 1
u32 index2; // feature index on hull 2
};
inline b3SATFeaturePair b3MakeFeaturePair(b3SATCacheType state, b3SATFeatureType type, u32 index1, u32 index2)
{
b3SATFeaturePair pair;
pair.state = state;
pair.type = type;
pair.index1 = index1;
pair.index2 = index2;
return pair;
}
struct b3FeatureCache
{
// Read the current state of the cache.
// Return e_unkown if neither a separation or penetration was detected.
b3SATCacheType ReadState(const b3Transform& xf1, const b3Hull* hull1,
const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius);
b3SATCacheType ReadEdge(const b3Transform& xf1, const b3Hull* hull1,
const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius);
b3SATCacheType ReadFace(const b3Transform& xf1, const b3Hull* hull1,
const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius);
b3SATFeaturePair m_featurePair;
};
#endif

39
include/bounce/dynamics/contacts/collide/collide.h
View File

@ -33,45 +33,6 @@ class b3MeshShape;
struct b3Manifold;
enum b3SATCacheType
{
e_separation,
e_overlap,
e_empty,
};
struct b3SATFeaturePair
{
enum Type
{
e_edge1, // an edge on hull 1 and an edge on hull 2
e_face1, // a face on hull 1 and a vertex/edge/face on hull 2
e_face2, // a face on hull 2 and a vertex/edge/face on hull 1
};
b3SATCacheType state; // sat result
Type type; // feature pair type
u32 index1; // feature index on hull 1
u32 index2; // feature index on hull 2
};
struct b3FeatureCache
{
// Read the current state of the cache.
// Return e_unkown if neither a separation or penetration was detected.
b3SATCacheType ReadState(const b3Transform& xf1, const b3Hull* hull1,
const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius);
b3SATCacheType ReadEdge(const b3Transform& xf1, const b3Hull* hull1,
const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius);
b3SATCacheType ReadFace(const b3Transform& xf1, const b3Hull* hull1,
const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius);
// We could increase the cache size (e.g. a feature pair of the last two frames).
b3SATFeaturePair m_featurePair;
};
// A convex cache contains information used to exploit temporal
// coherence of the contact generation algorithms between two shapes.
struct b3ConvexCache

105
src/bounce/collision/sat/sat.cpp
View File

@ -171,3 +171,108 @@ b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xf1, const b3Hull* hull1,
out.separation = maxSeparation;
return out;
}
///////////////////////////////////////////////////////////////////////////////////////////////////
b3SATCacheType b3FeatureCache::ReadState(
const b3Transform& xf1, const b3Hull* hull1,
const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius)
{
// If the cache was empty or flushed choose an arbitrary feature pair.
if (m_featurePair.state == b3SATCacheType::e_empty)
{
m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_separation, b3SATFeatureType::e_face1, 0, 0);
}
switch (m_featurePair.type)
{
case b3SATFeatureType::e_edge1:
{
return ReadEdge(xf1, hull1, xf2, hull2, totalRadius);
}
case b3SATFeatureType::e_face1:
{
return ReadFace(xf1, hull1, xf2, hull2, totalRadius);
}
case b3SATFeatureType::e_face2:
{
return ReadFace(xf2, hull2, xf1, hull1, totalRadius);
}
default:
{
return b3SATCacheType::e_empty;
}
}
}
b3SATCacheType b3FeatureCache::ReadFace(
const b3Transform& xf1, const b3Hull* hull1,
const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius)
{
// Perform computations in the local space of the second hull.
b3Transform xf = b3MulT(xf2, xf1);
b3Plane plane = xf * hull1->GetPlane(m_featurePair.index1);
float32 separation = b3Project(hull2, plane);
if (separation > totalRadius)
{
return e_separation;
}
return e_overlap;
}
b3SATCacheType b3FeatureCache::ReadEdge(
const b3Transform& xf1, const b3Hull* hull1,
const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius)
{
u32 i = m_featurePair.index1;
u32 j = m_featurePair.index2;
// Query minimum separation distance and axis of the first hull planes.
// Perform computations in the local space of the second hull.
b3Transform xf = b3MulT(xf2, xf1);
b3Vec3 C1 = xf * hull1->centroid;
const b3HalfEdge* edge1 = hull1->GetEdge(i);
const b3HalfEdge* twin1 = hull1->GetEdge(i + 1);
B3_ASSERT(edge1->twin == i + 1 && twin1->twin == i);
b3Vec3 P1 = xf * hull1->GetVertex(edge1->origin);
b3Vec3 Q1 = xf * hull1->GetVertex(twin1->origin);
b3Vec3 E1 = Q1 - P1;
// The Gauss Map of edge 1.
b3Vec3 U1 = xf.rotation * hull1->GetPlane(edge1->face).normal;
b3Vec3 V1 = xf.rotation * hull1->GetPlane(twin1->face).normal;
const b3HalfEdge* edge2 = hull2->GetEdge(j);
const b3HalfEdge* twin2 = hull2->GetEdge(j + 1);
B3_ASSERT(edge2->twin == j + 1 && twin2->twin == j);
b3Vec3 P2 = hull2->GetVertex(edge2->origin);
b3Vec3 Q2 = hull2->GetVertex(twin2->origin);
b3Vec3 E2 = Q2 - P2;
// The Gauss Map of edge 2.
b3Vec3 U2 = hull2->GetPlane(edge2->face).normal;
b3Vec3 V2 = hull2->GetPlane(twin2->face).normal;
// Negate the Gauss Map 2 for account for the MD.
if (b3IsMinkowskiFace(U1, V1, -E1, -U2, -V2, -E2))
{
float32 separation = b3Project(P1, E1, P2, E2, C1);
if (separation > totalRadius)
{
return b3SATCacheType::e_separation;
}
else
{
return b3SATCacheType::e_overlap;
}
}
// We can't determine the cache type
// therefore must run SAT.
return b3SATCacheType::e_empty;
}

2
src/bounce/dynamics/contacts/collide/collide_hulls.cpp
View File

@ -236,7 +236,7 @@ void b3CollideHulls(b3Manifold& manifold,
return;
}
const float32 kTol = 0.1f * B3_LINEAR_SLOP;
const float32 kTol = 0.05f * B3_LINEAR_SLOP;
if (edgeQuery.separation > b3Max(faceQuery1.separation, faceQuery2.separation) + kTol)
{
b3BuildEdgeContact(manifold, xf1, edgeQuery.index1, s1, xf2, edgeQuery.index2, s2);

171
src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp
View File

@ -139,7 +139,7 @@ static void b3RebuildFaceContact(b3Manifold& manifold,
// The new relative rotation.
// "q(2) - q(1)"
b3Quat dq = b3Conjugate(q1) * q2;
// Relative rotation between the new relative rotation and the old relative rotation.
// "dq(2) - dq0(1)"
b3Quat q = b3Conjugate(dq0) * dq;
@ -162,20 +162,17 @@ void b3CollideCache(b3Manifold& manifold,
const b3Hull* hull1 = s1->m_hull;
float32 r1 = s1->m_radius;
const b3Hull* hull2 = s2->m_hull;
float32 r2 = s2->m_radius;
float32 totalRadius = r1 + r2;
b3FaceQuery faceQuery1 = b3QueryFaceSeparation(xf1, hull1, xf2, hull2);
if (faceQuery1.separation > totalRadius)
{
// Write a separation cache.
cache->m_featurePair.state = b3SATCacheType::e_separation;
cache->m_featurePair.type = b3SATFeaturePair::e_face1;
cache->m_featurePair.index1 = faceQuery1.index;
cache->m_featurePair.index2 = faceQuery1.index;
cache->m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_separation, b3SATFeatureType::e_face1, faceQuery1.index, faceQuery1.index);
return;
}
@ -183,10 +180,7 @@ void b3CollideCache(b3Manifold& manifold,
if (faceQuery2.separation > totalRadius)
{
// Write a separation cache.
cache->m_featurePair.state = b3SATCacheType::e_separation;
cache->m_featurePair.type = b3SATFeaturePair::e_face2;
cache->m_featurePair.index1 = faceQuery2.index;
cache->m_featurePair.index2 = faceQuery2.index;
cache->m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_separation, b3SATFeatureType::e_face2, faceQuery2.index, faceQuery2.index);
return;
}
@ -194,25 +188,19 @@ void b3CollideCache(b3Manifold& manifold,
if (edgeQuery.separation > totalRadius)
{
// Write a separation cache.
cache->m_featurePair.state = b3SATCacheType::e_separation;
cache->m_featurePair.type = b3SATFeaturePair::e_edge1;
cache->m_featurePair.index1 = edgeQuery.index1;
cache->m_featurePair.index2 = edgeQuery.index2;
cache->m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_separation, b3SATFeatureType::e_edge1, edgeQuery.index1, edgeQuery.index2);
return;
}
const float32 kTol = 0.1f * B3_LINEAR_SLOP;
const float32 kTol = 0.05f * B3_LINEAR_SLOP;
if (edgeQuery.separation > b3Max(faceQuery1.separation, faceQuery2.separation) + kTol)
{
b3BuildEdgeContact(manifold, xf1, edgeQuery.index1, s1, xf2, edgeQuery.index2, s2);
if(manifold.pointCount > 0)
{
// Write an overlap cache.
cache->m_featurePair.state = b3SATCacheType::e_overlap;
cache->m_featurePair.type = b3SATFeaturePair::e_edge1;
cache->m_featurePair.index1 = edgeQuery.index1;
cache->m_featurePair.index2 = edgeQuery.index2;
if (manifold.pointCount > 0)
{
// Write an overlap cache.
cache->m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_overlap, b3SATFeatureType::e_edge1, edgeQuery.index1, edgeQuery.index2);
}
}
else
@ -220,136 +208,24 @@ void b3CollideCache(b3Manifold& manifold,
if (faceQuery1.separation + kTol > faceQuery2.separation)
{
b3BuildFaceContact(manifold, xf1, faceQuery1.index, s1, xf2, s2, false);
if(manifold.pointCount > 0)
if (manifold.pointCount > 0)
{
// Write an overlap cache.
cache->m_featurePair.state = b3SATCacheType::e_overlap;
cache->m_featurePair.type = b3SATFeaturePair::e_face1;
cache->m_featurePair.index1 = faceQuery1.index;
cache->m_featurePair.index2 = faceQuery1.index;
cache->m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_overlap, b3SATFeatureType::e_face1, faceQuery1.index, faceQuery1.index);
}
}
else
{
b3BuildFaceContact(manifold, xf2, faceQuery2.index, s2, xf1, s1, true);
if(manifold.pointCount > 0)
if (manifold.pointCount > 0)
{
// Write an overlap cache.
cache->m_featurePair.state = b3SATCacheType::e_overlap;
cache->m_featurePair.type = b3SATFeaturePair::e_face2;
cache->m_featurePair.index1 = faceQuery2.index;
cache->m_featurePair.index2 = faceQuery2.index;
cache->m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_overlap, b3SATFeatureType::e_face2, faceQuery2.index, faceQuery2.index);
}
}
}
}
b3SATCacheType b3FeatureCache::ReadState(
const b3Transform& xf1, const b3Hull* hull1,
const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius)
{
// If the cache was empty or flushed choose an arbitrary feature pair.
if (m_featurePair.state == b3SATCacheType::e_empty)
{
m_featurePair.state = b3SATCacheType::e_separation;
m_featurePair.type = b3SATFeaturePair::e_face1;
m_featurePair.index1 = 0;
m_featurePair.index2 = 0;
}
switch (m_featurePair.type)
{
case b3SATFeaturePair::e_edge1:
{
return ReadEdge(xf1, hull1, xf2, hull2, totalRadius);
}
case b3SATFeaturePair::e_face1:
{
return ReadFace(xf1, hull1, xf2, hull2, totalRadius);
}
case b3SATFeaturePair::e_face2:
{
return ReadFace(xf2, hull2, xf1, hull1, totalRadius);
}
default:
{
return b3SATCacheType::e_empty;
}
}
}
b3SATCacheType b3FeatureCache::ReadFace(
const b3Transform& xf1, const b3Hull* hull1,
const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius)
{
// Perform computations in the local space of the second hull.
b3Transform xf = b3MulT(xf2, xf1);
b3Plane plane = xf * hull1->GetPlane(m_featurePair.index1);
float32 separation = b3Project(hull2, plane);
if (separation > totalRadius)
{
return e_separation;
}
return e_overlap;
}
b3SATCacheType b3FeatureCache::ReadEdge(
const b3Transform& xf1, const b3Hull* hull1,
const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius)
{
u32 i = m_featurePair.index1;
u32 j = m_featurePair.index2;
// Query minimum separation distance and axis of the first hull planes.
// Perform computations in the local space of the second hull.
b3Transform xf = b3MulT(xf2, xf1);
b3Vec3 C1 = xf * hull1->centroid;
const b3HalfEdge* edge1 = hull1->GetEdge(i);
const b3HalfEdge* twin1 = hull1->GetEdge(i + 1);
B3_ASSERT(edge1->twin == i + 1 && twin1->twin == i);
b3Vec3 P1 = xf * hull1->GetVertex(edge1->origin);
b3Vec3 Q1 = xf * hull1->GetVertex(twin1->origin);
b3Vec3 E1 = Q1 - P1;
// The Gauss Map of edge 1.
b3Vec3 U1 = xf.rotation * hull1->GetPlane(edge1->face).normal;
b3Vec3 V1 = xf.rotation * hull1->GetPlane(twin1->face).normal;
const b3HalfEdge* edge2 = hull2->GetEdge(j);
const b3HalfEdge* twin2 = hull2->GetEdge(j + 1);
B3_ASSERT(edge2->twin == j + 1 && twin2->twin == j);
b3Vec3 P2 = hull2->GetVertex(edge2->origin);
b3Vec3 Q2 = hull2->GetVertex(twin2->origin);
b3Vec3 E2 = Q2 - P2;
// The Gauss Map of edge 2.
b3Vec3 U2 = hull2->GetPlane(edge2->face).normal;
b3Vec3 V2 = hull2->GetPlane(twin2->face).normal;
// Negate the Gauss Map 2 for account for the MD.
if (b3IsMinkowskiFace(U1, V1, -E1, -U2, -V2, -E2))
{
float32 separation = b3Project(P1, E1, P2, E2, C1);
if (separation > totalRadius)
{
return b3SATCacheType::e_separation;
}
else
{
return b3SATCacheType::e_overlap;
}
}
// We can't determine the cache type
// therefore must run SAT.
return b3SATCacheType::e_empty;
}
extern u32 b3_convexCacheHits;
void b3CollideHulls(b3Manifold& manifold,
@ -359,16 +235,16 @@ void b3CollideHulls(b3Manifold& manifold,
{
const b3Hull* hull1 = s1->m_hull;
float32 r1 = s1->m_radius;
const b3Hull* hull2 = s2->m_hull;
float32 r2 = s2->m_radius;
float32 totalRadius = r1 + r2;
// Read cache
b3SATCacheType state0 = cache->m_featurePair.state;
b3SATCacheType state1 = cache->ReadState(xf1, hull1, xf2, hull2, totalRadius);
if (state0 == b3SATCacheType::e_separation &&
state1 == b3SATCacheType::e_separation)
{
@ -376,23 +252,24 @@ void b3CollideHulls(b3Manifold& manifold,
++b3_convexCacheHits;
return;
}
else if (state0 == b3SATCacheType::e_overlap &&
state1 == b3SATCacheType::e_overlap)
if (state0 == b3SATCacheType::e_overlap &&
state1 == b3SATCacheType::e_overlap)
{
// Try to rebuild or reclip the features.
switch (cache->m_featurePair.type)
{
case b3SATFeaturePair::e_edge1:
case b3SATFeatureType::e_edge1:
{
b3RebuildEdgeContact(manifold, xf1, cache->m_featurePair.index1, s1, xf2, cache->m_featurePair.index2, s2);
break;
}
case b3SATFeaturePair::e_face1:
case b3SATFeatureType::e_face1:
{
b3RebuildFaceContact(manifold, xf1, cache->m_featurePair.index1, s1, xf2, s2, false);
break;
}
case b3SATFeaturePair::e_face2:
case b3SATFeatureType::e_face2:
{
b3RebuildFaceContact(manifold, xf2, cache->m_featurePair.index1, s2, xf1, s1, true);
break;