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

fix a lot of issues, add gyroscopic force integrator, add contact polygon winding, add some quaternion constraints, add more tests

Browse Source
This commit is contained in:
Irlan
2017-03-24 18:49:41 -03:00
parent dd6ca355e9
commit 8defab9945
103 changed files with 3840 additions and 3355 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
src/bounce/collision/broad_phase.cpp
View File

@ -25,16 +25,16 @@ b3BroadPhase::b3BroadPhase()
memset(m_moveBuffer, 0, m_moveBufferCapacity * sizeof(i32));
m_moveBufferCount = 0;
m_pairBufferCapacity = 16;
m_pairBuffer = (b3Pair*)b3Alloc(m_pairBufferCapacity * sizeof(b3Pair));
memset(m_pairBuffer, 0, m_pairBufferCapacity * sizeof(b3Pair));
m_pairBufferCount = 0;
m_pairCapacity = 16;
m_pairs = (b3Pair*)b3Alloc(m_pairCapacity * sizeof(b3Pair));
memset(m_pairs, 0, m_pairCapacity * sizeof(b3Pair));
m_pairCount = 0;
}
b3BroadPhase::~b3BroadPhase()
{
b3Free(m_moveBuffer);
b3Free(m_pairBuffer);
b3Free(m_pairs);
}
void b3BroadPhase::BufferMove(i32 proxyId)
@ -142,21 +142,21 @@ bool b3BroadPhase::Report(i32 proxyId)
}
// Check capacity.
if (m_pairBufferCount == m_pairBufferCapacity)
if (m_pairCount == m_pairCapacity)
{
// Duplicate capacity.
m_pairBufferCapacity *= 2;
m_pairCapacity *= 2;
b3Pair* oldPairBuffer = m_pairBuffer;
m_pairBuffer = (b3Pair*)b3Alloc(m_pairBufferCapacity * sizeof(b3Pair));
memcpy(m_pairBuffer, oldPairBuffer, m_pairBufferCount * sizeof(b3Pair));
b3Free(oldPairBuffer);
b3Pair* oldPairs = m_pairs;
m_pairs = (b3Pair*)b3Alloc(m_pairCapacity * sizeof(b3Pair));
memcpy(m_pairs, oldPairs, m_pairCount * sizeof(b3Pair));
b3Free(oldPairs);
}
// Add overlapping pair to the pair buffer.
m_pairBuffer[m_pairBufferCount].proxy1 = b3Min(proxyId, m_queryProxyId);
m_pairBuffer[m_pairBufferCount].proxy2 = b3Max(proxyId, m_queryProxyId);
++m_pairBufferCount;
m_pairs[m_pairCount].proxy1 = b3Min(proxyId, m_queryProxyId);
m_pairs[m_pairCount].proxy2 = b3Max(proxyId, m_queryProxyId);
++m_pairCount;
// Keep looking for overlapping pairs.
return true;

260
src/bounce/collision/distance.cpp
View File

@ -1,260 +0,0 @@
/*
* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#include <bounce/collision/distance.h>
#include <bounce/common/math/vec2.h>
#include <bounce/common/math/vec3.h>
#include <bounce/common/math/mat22.h>
b3Vec3 b3ClosestPointOnPlane(const b3Vec3& P, const b3Plane& plane)
{
return b3Project(P, plane);
}
b3Vec3 b3ClosestPointOnSegment(const b3Vec3& P,
const b3Vec3& A, const b3Vec3& B)
{
float32 wAB[3];
b3Barycentric(wAB, A, B, P);
if (wAB[1] <= 0.0f)
{
return A;
}
if (wAB[0] <= 0.0f)
{
return B;
}
float32 s = 1.0f / wAB[2];
float32 wA = s * wAB[0];
float32 wB = s * wAB[1];
return wA * A + wB * B;
}
b3Vec3 b3ClosestPointOnTriangle(const b3Vec3& P,
const b3Vec3& A, const b3Vec3& B, const b3Vec3& C)
{
// Test vertex regions
float32 wAB[3], wBC[3], wCA[3];
b3Barycentric(wAB, A, B, P);
b3Barycentric(wBC, B, C, P);
b3Barycentric(wCA, C, A, P);
// R A
if (wAB[1] <= 0.0f && wCA[0] <= 0.0f)
{
return A;
}
// R B
if (wAB[0] <= 0.0f && wBC[2] <= 0.0f)
{
return B;
}
// R C
if (wBC[0] <= 0.0f && wCA[1] <= 0.0f)
{
return C;
}
// Test edge regions
float32 wABC[4];
b3Barycentric(wABC, A, B, C, P);
// R AB
if (wAB[0] > 0.0f && wAB[1] > 0.0f && wABC[2] <= 0.0f)
{
float32 s = 1.0f / wAB[2];
float32 wA = s * wAB[0];
float32 wB = s * wAB[1];
return wA * A + wB * B;
}
// R BC
if (wBC[0] > 0.0f && wBC[1] > 0.0f && wABC[0] <= 0.0f)
{
float32 s = 1.0f / wBC[2];
float32 wB = s * wBC[0];
float32 wC = s * wBC[1];
return wB * B + wC * C;
}
// R CA
if (wCA[0] > 0.0f && wCA[1] > 0.0f && wABC[1] <= 0.0f)
{
float32 s = 1.0f / wCA[2];
float32 wC = s * wCA[0];
float32 wA = s * wCA[1];
return wC * C + wA * A;
}
if (wABC[3] <= 0.0f)
{
// ABC degenerates into an edge or vertex.
// Give up.
return A;
}
// R ABC/ACB
float32 divisor = wABC[3];
float32 s = 1.0f / divisor;
float32 wA = s * wABC[0];
float32 wB = s * wABC[1];
float32 wC = s * wABC[2];
return wA * A + wB * B + wC * C;
}
void b3ClosestPointsOnLines(b3Vec3* C1, b3Vec3* C2,
const b3Vec3& P1, const b3Vec3& E1,
const b3Vec3& P2, const b3Vec3& E2)
{
// E1 = Q1 - P1
// E2 = Q2 - P2
// S1 = P1 + x1 * E1
// S2 = P2 + x2 * E2
// [dot(E1, E1) -dot(E1, E2)][x1] = [-dot(E1, P1 - P2)]
// [dot(E2, E1) -dot(E2, E2)][x2] = [-dot(E2, P1 - P2)]
// Ax = b
b3Vec3 E3 = P1 - P2;
b3Mat22 A;
A.x.x = b3Dot(E1, E1);
A.x.y = b3Dot(E2, E1);
A.y.x = -b3Dot(E1, E2);
A.y.y = -b3Dot(E2, E2);
b3Vec2 b;
b.x = -b3Dot(E1, E3);
b.y = -b3Dot(E2, E3);
// If the lines are parallel then choose P1 and P2 as
// the closest points.
b3Vec2 x = A.Solve(b);
*C1 = P1 + x.x * E1;
*C2 = P2 + x.y * E2;
}
void b3ClosestPointsOnNormalizedLines(b3Vec3* C1, b3Vec3* C2,
const b3Vec3& P1, const b3Vec3& N1,
const b3Vec3& P2, const b3Vec3& N2)
{
const float32 kTol = 0.001f;
float32 b = b3Dot(N1, N2);
float32 den = 1.0f - b * b;
if (den < kTol * kTol)
{
*C1 = P1;
*C2 = P2;
return;
}
float32 inv_den = 1.0f / den;
b3Vec3 E3 = P1 - P2;
float32 d = b3Dot(N1, E3);
float32 e = b3Dot(N2, E3);
float32 s = inv_den * (b * e - d);
float32 t = inv_den * (e - b * d);
*C1 = P1 + s * N1;
*C2 = P2 + t * N2;
}
void b3ClosestPointsOnSegments(b3Vec3* C1, b3Vec3* C2,
const b3Vec3& P1, const b3Vec3& Q1,
const b3Vec3& P2, const b3Vec3& Q2)
{
b3Vec3 E1 = Q1 - P1;
float32 L1 = b3Length(E1);
b3Vec3 E2 = Q2 - P2;
float32 L2 = b3Length(E2);
if (L1 < B3_LINEAR_SLOP && L2 < B3_LINEAR_SLOP)
{
*C1 = P1;
*C2 = P2;
return;
}
if (L1 < B3_LINEAR_SLOP)
{
*C1 = P1;
*C2 = b3ClosestPointOnSegment(P1, P2, Q2);
return;
}
if (L2 < B3_LINEAR_SLOP)
{
*C1 = b3ClosestPointOnSegment(P2, P1, Q1);
*C2 = P2;
return;
}
// Here and in 3D we need to start "GJK" with the closest points between the two edges
// since the cross product between their direction is a possible separating axis.
b3Vec3 N1 = (1.0f / L1) * E1;
b3Vec3 N2 = (1.0f / L2) * E2;
// sin = norm( cross(n1, n2) )
// or
// sin^2 = 1 - cos^2
const float32 kTol = 0.005f;
float32 b = b3Dot(N1, N2);
float32 den = 1.0f - b * b;
if (den < kTol * kTol)
{
*C1 = P1;
*C2 = P2;
}
else
{
// b = dot(n1, n2)
// d = dot(n1, e3)
// e = dot(n2, e3)
// s - b * t = -d
// b * s - t = -e
// s = ( b * e - d ) / den
// t = ( e - b * d ) / den
b3Vec3 E3 = P1 - P2;
float32 d = b3Dot(N1, E3);
float32 e = b3Dot(N2, E3);
float32 inv_den = 1.0f / den;
float32 s = inv_den * (b * e - d);
float32 t = inv_den * (e - b * d);
*C1 = P1 + s * N1;
*C2 = P2 + t * N2;
}
*C1 = b3ClosestPointOnSegment(*C1, P1, Q1);
*C2 = b3ClosestPointOnSegment(*C1, P2, Q2);
*C1 = b3ClosestPointOnSegment(*C2, P1, Q1);
}

56
src/bounce/collision/gjk/gjk.cpp
View File

@ -92,7 +92,7 @@ b3Vec3 b3Simplex::GetClosestPoint() const
}
}
void b3Simplex::GetClosestPoints(b3Vec3* pA, b3Vec3* pB) const
void b3Simplex::GetClosestPoints(b3Vec3* p1, b3Vec3* p2) const
{
switch (m_count)
{
@ -100,22 +100,22 @@ void b3Simplex::GetClosestPoints(b3Vec3* pA, b3Vec3* pB) const
B3_ASSERT(false);
break;
case 1:
*pA = m_vertices[0].pointA;
*pB = m_vertices[0].pointB;
*p1 = m_vertices[0].point1;
*p2 = m_vertices[0].point2;
break;
case 2:
*pA = m_vertices[0].weight * m_vertices[0].pointA + m_vertices[1].weight * m_vertices[1].pointA;
*pB = m_vertices[0].weight * m_vertices[0].pointB + m_vertices[1].weight * m_vertices[1].pointB;
*p1 = m_vertices[0].weight * m_vertices[0].point1 + m_vertices[1].weight * m_vertices[1].point1;
*p2 = m_vertices[0].weight * m_vertices[0].point2 + m_vertices[1].weight * m_vertices[1].point2;
break;
case 3:
*pA = m_vertices[0].weight * m_vertices[0].pointA + m_vertices[1].weight * m_vertices[1].pointA + m_vertices[2].weight * m_vertices[2].pointA;
*pB = m_vertices[0].weight * m_vertices[0].pointB + m_vertices[1].weight * m_vertices[1].pointB + m_vertices[2].weight * m_vertices[2].pointB;
*p1 = m_vertices[0].weight * m_vertices[0].point1 + m_vertices[1].weight * m_vertices[1].point1 + m_vertices[2].weight * m_vertices[2].point1;
*p2 = m_vertices[0].weight * m_vertices[0].point2 + m_vertices[1].weight * m_vertices[1].point2 + m_vertices[2].weight * m_vertices[2].point2;
break;
case 4:
*pA = m_vertices[0].weight * m_vertices[0].pointA + m_vertices[1].weight * m_vertices[1].pointA + m_vertices[2].weight * m_vertices[2].pointA + m_vertices[3].weight * m_vertices[3].pointA;
*pB = *pA;
*p1 = m_vertices[0].weight * m_vertices[0].point1 + m_vertices[1].weight * m_vertices[1].point1 + m_vertices[2].weight * m_vertices[2].point1 + m_vertices[3].weight * m_vertices[3].point1;
*p2 = *p1;
break;
default:
B3_ASSERT(false);
@ -508,7 +508,7 @@ void b3Simplex::Solve4(const b3Vec3& Q)
m_vertices[3].weight = s * wABCD[3];
}
b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA, const b3Transform& xfB, const b3GJKProxy& proxyB)
b3GJKOutput b3GJK(const b3Transform& xf1, const b3GJKProxy& proxy1, const b3Transform& xf2, const b3GJKProxy& proxy2)
{
++b3_gjkCalls;
@ -517,14 +517,14 @@ b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA, const b3Tran
// Initialize the simplex.
{
b3SimplexVertex* v = simplex.m_vertices + 0;
b3Vec3 wALocal = proxyA.GetVertex(0);
b3Vec3 wBLocal = proxyB.GetVertex(0);
v->pointA = b3Mul(xfA, wALocal);
v->pointB = b3Mul(xfB, wBLocal);
v->point = v->pointB - v->pointA;
b3Vec3 w1Local = proxy1.GetVertex(0);
b3Vec3 w2Local = proxy2.GetVertex(0);
v->point1 = b3Mul(xf1, w1Local);
v->point2 = b3Mul(xf2, w2Local);
v->point = v->point2 - v->point1;
v->weight = 1.0f;
v->indexA = 0;
v->indexB = 0;
v->index1 = 0;
v->index2 = 0;
simplex.m_count = 1;
}
@ -533,7 +533,7 @@ b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA, const b3Tran
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
u32 saveA[4], saveB[4];
u32 save1[4], save2[4];
u32 saveCount = 0;
// Last iteration squared distance for checking if we're getting close
@ -553,8 +553,8 @@ b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA, const b3Tran
saveCount = simplex.m_count;
for (u32 i = 0; i < saveCount; ++i)
{
saveA[i] = vertices[i].indexA;
saveB[i] = vertices[i].indexB;
save1[i] = vertices[i].index1;
save2[i] = vertices[i].index2;
}
// Determine the closest point on the simplex and
@ -604,11 +604,11 @@ b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA, const b3Tran
// Compute a tentative new simplex vertex using support points.
b3SimplexVertex* vertex = vertices + simplex.m_count;
vertex->indexA = proxyA.GetSupportIndex(b3MulT(xfA.rotation, -d));
vertex->pointA = b3Mul(xfA, proxyA.GetVertex(vertex->indexA));
vertex->indexB = proxyB.GetSupportIndex(b3MulT(xfB.rotation, d));
vertex->pointB = b3Mul(xfB, proxyB.GetVertex(vertex->indexB));
vertex->point = vertex->pointB - vertex->pointA;
vertex->index1 = proxy1.GetSupportIndex(b3MulT(xf1.rotation, -d));
vertex->point1 = b3Mul(xf1, proxy1.GetVertex(vertex->index1));
vertex->index2 = proxy2.GetSupportIndex(b3MulT(xf2.rotation, d));
vertex->point2 = b3Mul(xf2, proxy2.GetVertex(vertex->index2));
vertex->point = vertex->point2 - vertex->point1;
// Iteration count is equated to the number of support point calls.
++iter;
@ -619,7 +619,7 @@ b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA, const b3Tran
bool duplicate = false;
for (u32 i = 0; i < saveCount; ++i)
{
if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])
if (vertex->index1 == save1[i] && vertex->index2 == save2[i])
{
duplicate = true;
break;
@ -640,8 +640,8 @@ b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA, const b3Tran
// Prepare output.
b3GJKOutput output;
simplex.GetClosestPoints(&output.pointA, &output.pointB);
output.distance = b3Distance(output.pointA, output.pointB);
simplex.GetClosestPoints(&output.point1, &output.point2);
output.distance = b3Distance(output.point1, output.point2);
output.iterations = iter;
// Output result.

84
src/bounce/collision/gjk/gjk_cache.cpp
View File

@ -24,21 +24,21 @@ u32 b3_gjkCacheHits;
// Implements b3Simplex routines for a cached simplex.
void b3Simplex::ReadCache(const b3SimplexCache* cache,
const b3Transform& xfA, const b3GJKProxy& proxyA,
const b3Transform& xfB, const b3GJKProxy& proxyB)
const b3Transform& xf1, const b3GJKProxy& proxy1,
const b3Transform& xf2, const b3GJKProxy& proxy2)
{
B3_ASSERT(cache->count <= 4);
m_count = (u8)cache->count;
for (u32 i = 0; i < m_count; ++i)
{
b3SimplexVertex* v = m_vertices + i;
v->indexA = cache->indexA[i];
v->indexB = cache->indexB[i];
b3Vec3 wALocal = proxyA.GetVertex(v->indexA);
b3Vec3 wBLocal = proxyB.GetVertex(v->indexB);
v->pointA = xfA * wALocal;
v->pointB = xfB * wBLocal;
v->point = v->pointB - v->pointA;
v->index1 = cache->index1[i];
v->index2 = cache->index2[i];
b3Vec3 wALocal = proxy1.GetVertex(v->index1);
b3Vec3 wBLocal = proxy2.GetVertex(v->index2);
v->point1 = xf1 * wALocal;
v->point2 = xf2 * wBLocal;
v->point = v->point2 - v->point1;
v->weight = 0.0f;
}
@ -64,14 +64,14 @@ void b3Simplex::ReadCache(const b3SimplexCache* cache,
if (m_count == 0)
{
b3SimplexVertex* v = m_vertices + 0;
b3Vec3 wALocal = proxyA.GetVertex(0);
b3Vec3 wBLocal = proxyB.GetVertex(0);
v->pointA = b3Mul(xfA, wALocal);
v->pointB = b3Mul(xfB, wBLocal);
v->point = v->pointB - v->pointA;
b3Vec3 w1Local = proxy1.GetVertex(0);
b3Vec3 w2Local = proxy2.GetVertex(0);
v->point1 = b3Mul(xf1, w1Local);
v->point2 = b3Mul(xf2, w2Local);
v->point = v->point2 - v->point1;
v->weight = 1.0f;
v->indexA = 0;
v->indexB = 0;
v->index1 = 0;
v->index2 = 0;
m_count = 1;
}
}
@ -82,8 +82,8 @@ void b3Simplex::WriteCache(b3SimplexCache* cache) const
cache->count = u16(m_count);
for (u32 i = 0; i < m_count; ++i)
{
cache->indexA[i] = u8(m_vertices[i].indexA);
cache->indexB[i] = u8(m_vertices[i].indexB);
cache->index1[i] = u8(m_vertices[i].index1);
cache->index2[i] = u8(m_vertices[i].index2);
}
}
@ -123,22 +123,22 @@ float32 b3Simplex::GetMetric() const
}
}
b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA,
const b3Transform& xfB, const b3GJKProxy& proxyB,
b3GJKOutput b3GJK(const b3Transform& xf1, const b3GJKProxy& proxy1,
const b3Transform& xf2, const b3GJKProxy& proxy2,
bool applyRadius, b3SimplexCache* cache)
{
++b3_gjkCalls;
// Initialize the simplex.
b3Simplex simplex;
simplex.ReadCache(cache, xfA, proxyA, xfB, proxyB);
simplex.ReadCache(cache, xf1, proxy1, xf2, proxy2);
// Get simplex vertices as an array.
b3SimplexVertex* vertices = simplex.m_vertices;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
u32 saveA[4], saveB[4];
u32 save1[4], save2[4];
u32 saveCount = 0;
// Last iteration squared distance for checking if we're getting close
@ -158,8 +158,8 @@ b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA,
saveCount = simplex.m_count;
for (u32 i = 0; i < saveCount; ++i)
{
saveA[i] = vertices[i].indexA;
saveB[i] = vertices[i].indexB;
save1[i] = vertices[i].index1;
save2[i] = vertices[i].index2;
}
// Determine the closest point on the simplex and
@ -209,11 +209,11 @@ b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA,
// Compute a tentative new simplex vertex using support points.
b3SimplexVertex* vertex = vertices + simplex.m_count;
vertex->indexA = proxyA.GetSupportIndex(b3MulT(xfA.rotation, -d));
vertex->pointA = b3Mul(xfA, proxyA.GetVertex(vertex->indexA));
vertex->indexB = proxyB.GetSupportIndex(b3MulT(xfB.rotation, d));
vertex->pointB = b3Mul(xfB, proxyB.GetVertex(vertex->indexB));
vertex->point = vertex->pointB - vertex->pointA;
vertex->index1 = proxy1.GetSupportIndex(b3MulT(xf1.rotation, -d));
vertex->point1 = b3Mul(xf1, proxy1.GetVertex(vertex->index1));
vertex->index2 = proxy2.GetSupportIndex(b3MulT(xf2.rotation, d));
vertex->point2 = b3Mul(xf2, proxy2.GetVertex(vertex->index2));
vertex->point = vertex->point2 - vertex->point1;
// Iteration count is equated to the number of support point calls.
++iter;
@ -224,7 +224,7 @@ b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA,
bool duplicate = false;
for (u32 i = 0; i < saveCount; ++i)
{
if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])
if (vertex->index1 == save1[i] && vertex->index2 == save2[i])
{
duplicate = true;
break;
@ -245,8 +245,8 @@ b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA,
// Prepare result.
b3GJKOutput output;
simplex.GetClosestPoints(&output.pointA, &output.pointB);
output.distance = b3Distance(output.pointA, output.pointB);
simplex.GetClosestPoints(&output.point1, &output.point2);
output.distance = b3Distance(output.point1, output.point2);
output.iterations = iter;
// Cache the simplex.
@ -255,26 +255,26 @@ b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA,
// Apply radius if requested.
if (applyRadius)
{
float32 rA = proxyA.m_radius;
float32 rB = proxyB.m_radius;
float32 r1 = proxy1.m_radius;
float32 r2 = proxy2.m_radius;
if (output.distance > rA + rB && output.distance > B3_EPSILON)
if (output.distance > r1 + r2 && output.distance > B3_EPSILON)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.distance -= rA + rB;
b3Vec3 d = output.pointB - output.pointA;
output.distance -= r1 + r2;
b3Vec3 d = output.point2 - output.point1;
b3Vec3 normal = b3Normalize(d);
output.pointA += rA * normal;
output.pointB -= rB * normal;
output.point1 += r1 * normal;
output.point2 -= r2 * normal;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
b3Vec3 p = 0.5f * (output.pointA + output.pointB);
output.pointA = p;
output.pointB = p;
b3Vec3 p = 0.5f * (output.point1 + output.point2);
output.point1 = p;
output.point2 = p;
output.distance = 0.0f;
}
}

168
src/bounce/collision/gjk/gjk_feature_pair.cpp
View File

@ -23,17 +23,17 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
B3_ASSERT(0 < cache.count && cache.count < 4);
u32 vertexCount = cache.count;
u32 uniqueCount1 = b3UniqueCount(cache.indexA, vertexCount);
u32 uniqueCount2 = b3UniqueCount(cache.indexB, vertexCount);
u32 uniqueCount1 = b3UniqueCount(cache.index1, vertexCount);
u32 uniqueCount2 = b3UniqueCount(cache.index2, vertexCount);
if (vertexCount == 1)
{
// VV
b3GJKFeaturePair pair;
pair.countA = 1;
pair.countB = 1;
pair.indexA[0] = cache.indexA[0];
pair.indexB[0] = cache.indexB[0];
pair.count1 = 1;
pair.count2 = 1;
pair.index1[0] = cache.index1[0];
pair.index2[0] = cache.index2[0];
return pair;
}
@ -43,14 +43,14 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
{
// EE
b3GJKFeaturePair pair;
pair.countA = 2;
pair.countB = 2;
pair.indexA[0] = cache.indexA[0];
pair.indexA[1] = cache.indexA[1];
pair.indexB[0] = cache.indexB[0];
pair.indexB[1] = cache.indexB[1];
B3_ASSERT(pair.indexA[0] != pair.indexA[1]);
B3_ASSERT(pair.indexB[0] != pair.indexB[1]);
pair.count1 = 2;
pair.count2 = 2;
pair.index1[0] = cache.index1[0];
pair.index1[1] = cache.index1[1];
pair.index2[0] = cache.index2[0];
pair.index2[1] = cache.index2[1];
B3_ASSERT(pair.index1[0] != pair.index1[1]);
B3_ASSERT(pair.index2[0] != pair.index2[1]);
return pair;
}
@ -58,12 +58,12 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
{
// VE
b3GJKFeaturePair pair;
pair.countA = 1;
pair.countB = 2;
pair.indexA[0] = cache.indexA[0];
pair.indexB[0] = cache.indexB[0];
pair.indexB[1] = cache.indexB[1];
B3_ASSERT(pair.indexB[0] != pair.indexB[1]);
pair.count1 = 1;
pair.count2 = 2;
pair.index1[0] = cache.index1[0];
pair.index2[0] = cache.index2[0];
pair.index2[1] = cache.index2[1];
B3_ASSERT(pair.index2[0] != pair.index2[1]);
return pair;
}
@ -71,12 +71,12 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
{
// EV
b3GJKFeaturePair pair;
pair.countA = 2;
pair.countB = 1;
pair.indexA[0] = cache.indexA[0];
pair.indexA[1] = cache.indexA[1];
pair.indexB[0] = cache.indexB[0];
B3_ASSERT(pair.indexA[0] != pair.indexA[1]);
pair.count1 = 2;
pair.count2 = 1;
pair.index1[0] = cache.index1[0];
pair.index1[1] = cache.index1[1];
pair.index2[0] = cache.index2[0];
B3_ASSERT(pair.index1[0] != pair.index1[1]);
return pair;
}
B3_ASSERT(false);
@ -88,15 +88,15 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
{
// VF
b3GJKFeaturePair pair;
pair.countA = 1;
pair.countB = 3;
pair.indexA[0] = cache.indexA[0];
pair.indexB[0] = cache.indexB[0];
pair.indexB[1] = cache.indexB[1];
pair.indexB[2] = cache.indexB[2];
B3_ASSERT(pair.indexB[0] != pair.indexB[1]);
B3_ASSERT(pair.indexB[1] != pair.indexB[2]);
B3_ASSERT(pair.indexB[2] != pair.indexB[0]);
pair.count1 = 1;
pair.count2 = 3;
pair.index1[0] = cache.index1[0];
pair.index2[0] = cache.index2[0];
pair.index2[1] = cache.index2[1];
pair.index2[2] = cache.index2[2];
B3_ASSERT(pair.index2[0] != pair.index2[1]);
B3_ASSERT(pair.index2[1] != pair.index2[2]);
B3_ASSERT(pair.index2[2] != pair.index2[0]);
return pair;
}
@ -104,15 +104,15 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
{
// FV
b3GJKFeaturePair pair;
pair.countA = 3;
pair.countB = 1;
pair.indexA[0] = cache.indexA[0];
pair.indexA[1] = cache.indexA[1];
pair.indexA[2] = cache.indexA[2];
pair.indexB[0] = cache.indexB[0];
B3_ASSERT(pair.indexA[0] != pair.indexA[1]);
B3_ASSERT(pair.indexA[1] != pair.indexA[2]);
B3_ASSERT(pair.indexA[2] != pair.indexA[0]);
pair.count1 = 3;
pair.count2 = 1;
pair.index1[0] = cache.index1[0];
pair.index1[1] = cache.index1[1];
pair.index1[2] = cache.index1[2];
pair.index2[0] = cache.index2[0];
B3_ASSERT(pair.index1[0] != pair.index1[1]);
B3_ASSERT(pair.index1[1] != pair.index1[2]);
B3_ASSERT(pair.index1[2] != pair.index1[0]);
return pair;
}
@ -120,30 +120,30 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
{
// EE
b3GJKFeaturePair pair;
pair.countA = 2;
pair.countB = 2;
pair.indexA[0] = cache.indexA[0];
pair.indexB[0] = cache.indexB[0];
pair.count1 = 2;
pair.count2 = 2;
pair.index1[0] = cache.index1[0];
pair.index2[0] = cache.index2[0];
if (cache.indexA[0] == cache.indexA[1])
if (cache.index1[0] == cache.index1[1])
{
pair.indexA[1] = cache.indexA[2];
pair.index1[1] = cache.index1[2];
}
else
{
pair.indexA[1] = cache.indexA[1];
pair.index1[1] = cache.index1[1];
}
if (cache.indexB[0] == cache.indexB[1])
if (cache.index2[0] == cache.index2[1])
{
pair.indexB[1] = cache.indexB[2];
pair.index2[1] = cache.index2[2];
}
else
{
pair.indexB[1] = cache.indexB[1];
pair.index2[1] = cache.index2[1];
}
B3_ASSERT(pair.indexA[0] != pair.indexA[1]);
B3_ASSERT(pair.indexB[0] != pair.indexB[1]);
B3_ASSERT(pair.index1[0] != pair.index1[1]);
B3_ASSERT(pair.index2[0] != pair.index2[1]);
return pair;
}
@ -151,27 +151,27 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
{
// EF
b3GJKFeaturePair pair;
pair.countA = 2;
pair.countB = 3;
pair.count1 = 2;
pair.count2 = 3;
pair.indexA[0] = cache.indexA[0];
if (cache.indexA[0] == cache.indexA[1])
pair.index1[0] = cache.index1[0];
if (cache.index1[0] == cache.index1[1])
{
pair.indexA[1] = cache.indexA[2];
pair.index1[1] = cache.index1[2];
}
else
{
pair.indexA[1] = cache.indexA[1];
pair.index1[1] = cache.index1[1];
}
pair.indexB[0] = cache.indexB[0];
pair.indexB[1] = cache.indexB[1];
pair.indexB[2] = cache.indexB[2];
pair.index2[0] = cache.index2[0];
pair.index2[1] = cache.index2[1];
pair.index2[2] = cache.index2[2];
B3_ASSERT(pair.indexA[0] != pair.indexA[1]);
B3_ASSERT(pair.indexB[0] != pair.indexB[1]);
B3_ASSERT(pair.indexB[1] != pair.indexB[2]);
B3_ASSERT(pair.indexB[2] != pair.indexB[0]);
B3_ASSERT(pair.index1[0] != pair.index1[1]);
B3_ASSERT(pair.index2[0] != pair.index2[1]);
B3_ASSERT(pair.index2[1] != pair.index2[2]);
B3_ASSERT(pair.index2[2] != pair.index2[0]);
return pair;
}
@ -179,26 +179,26 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
{
// FE
b3GJKFeaturePair pair;
pair.countA = 3;
pair.countB = 2;
pair.indexA[0] = cache.indexA[0];
pair.indexA[1] = cache.indexA[1];
pair.indexA[2] = cache.indexA[2];
pair.count1 = 3;
pair.count2 = 2;
pair.index1[0] = cache.index1[0];
pair.index1[1] = cache.index1[1];
pair.index1[2] = cache.index1[2];
pair.indexB[0] = cache.indexB[0];
if (cache.indexB[0] == cache.indexB[1])
pair.index2[0] = cache.index2[0];
if (cache.index2[0] == cache.index2[1])
{
pair.indexB[1] = cache.indexB[2];
pair.index2[1] = cache.index2[2];
}
else
{
pair.indexB[1] = cache.indexB[1];
pair.index2[1] = cache.index2[1];
}
B3_ASSERT(pair.indexA[0] != pair.indexA[1]);
B3_ASSERT(pair.indexA[1] != pair.indexA[2]);
B3_ASSERT(pair.indexA[2] != pair.indexA[0]);
B3_ASSERT(pair.indexB[0] != pair.indexB[1]);
B3_ASSERT(pair.index1[0] != pair.index1[1]);
B3_ASSERT(pair.index1[1] != pair.index1[2]);
B3_ASSERT(pair.index1[2] != pair.index1[0]);
B3_ASSERT(pair.index2[0] != pair.index2[1]);
return pair;
}
@ -208,7 +208,7 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
B3_ASSERT(false);
b3GJKFeaturePair pair;
pair.countA = 0;
pair.countB = 0;
pair.count1 = 0;
pair.count2 = 0;
return pair;
}

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

@ -30,20 +30,20 @@ float32 b3Project(const b3Hull* hull, const b3Plane& plane)
}
// Query minimum separation distance and axis of the first hull planes.
b3FaceQuery b3QueryFaceSeparation(const b3Transform& xfA, const b3Hull* hullA,
const b3Transform& xfB, const b3Hull* hullB)
b3FaceQuery b3QueryFaceSeparation(const b3Transform& xf1, const b3Hull* hull1,
const b3Transform& xf2, const b3Hull* hull2)
{
// Perform computations in the local space of the second hull.
b3Transform xf = b3MulT(xfB, xfA);
b3Transform xf = b3MulT(xf2, xf1);
// Here greater means less than since is a signed distance.
u32 maxIndex = 0;
float32 maxSeparation = -B3_MAX_FLOAT;
for (u32 i = 0; i < hullA->faceCount; ++i)
for (u32 i = 0; i < hull1->faceCount; ++i)
{
b3Plane plane = xf * hullA->GetPlane(i);
float32 separation = b3Project(hullB, plane);
b3Plane plane = xf * hull1->GetPlane(i);
float32 separation = b3Project(hull2, plane);
if (separation > maxSeparation)
{
maxIndex = i;
@ -72,13 +72,26 @@ bool b3IsMinkowskiFace(const b3Vec3& A, const b3Vec3& B, const b3Vec3& B_x_A, co
float32 b3Project(const b3Vec3& P1, const b3Vec3& E1, const b3Vec3& P2, const b3Vec3& E2, const b3Vec3& C1)
{
float32 L1 = b3Length(E1);
B3_ASSERT(L1 > B3_LINEAR_SLOP);
if (L1 < B3_LINEAR_SLOP)
{
return -B3_MAX_FLOAT;
}
float32 L2 = b3Length(E2);
B3_ASSERT(L2 > B3_LINEAR_SLOP);
if (L2 < B3_LINEAR_SLOP)
{
return -B3_MAX_FLOAT;
}
// Skip over almost parallel edges.
const float32 kTol = 0.005f;
b3Vec3 E1_x_E2 = b3Cross(E1, E2);
float32 L = b3Length(E1_x_E2);
const float32 kTol = 0.005f;
if (L < kTol * b3Sqrt(b3LengthSquared(E1) * b3LengthSquared(E2)))
if (L < kTol * L1 * L2)
{
return -B3_MAX_FLOAT;
}
@ -94,49 +107,49 @@ float32 b3Project(const b3Vec3& P1, const b3Vec3& E1, const b3Vec3& P2, const b3
return b3Dot(N, P2 - P1);
}
b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xfA, const b3Hull* hullA,
const b3Transform& xfB, const b3Hull* hullB)
b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xf1, const b3Hull* hull1,
const b3Transform& xf2, const b3Hull* hull2)
{
// Query minimum separation distance and axis of the first hull planes.
// Perform computations in the local space of the second hull.
b3Transform xf = b3MulT(xfB, xfA);
b3Vec3 C1 = xf * hullA->centroid;
b3Transform xf = b3MulT(xf2, xf1);
b3Vec3 C1 = xf * hull1->centroid;
u32 maxIndex1 = 0;
u32 maxIndex2 = 0;
float32 maxSeparation = -B3_MAX_FLOAT;
// Loop through the first hull's unique edges.
for (u32 i = 0; i < hullA->edgeCount; i += 2)
for (u32 i = 0; i < hull1->edgeCount; i += 2)
{
const b3HalfEdge* edge1 = hullA->GetEdge(i);
const b3HalfEdge* twin1 = hullA->GetEdge(i + 1);
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 * hullA->GetVertex(edge1->origin);
b3Vec3 Q1 = xf * hullA->GetVertex(twin1->origin);
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 * hullA->GetPlane(edge1->face).normal;
b3Vec3 V1 = xf.rotation * hullA->GetPlane(twin1->face).normal;
b3Vec3 U1 = xf.rotation * hull1->GetPlane(edge1->face).normal;
b3Vec3 V1 = xf.rotation * hull1->GetPlane(twin1->face).normal;
// Loop through the second hull's unique edges.
for (u32 j = 0; j < hullB->edgeCount; j += 2)
for (u32 j = 0; j < hull2->edgeCount; j += 2)
{
const b3HalfEdge* edge2 = hullB->GetEdge(j);
const b3HalfEdge* twin2 = hullB->GetEdge(j + 1);
const b3HalfEdge* edge2 = hull2->GetEdge(j);
const b3HalfEdge* twin2 = hull2->GetEdge(j + 1);
B3_ASSERT(edge2->twin == j + 1 && twin2->twin == j);
b3Vec3 P2 = hullB->GetVertex(edge2->origin);
b3Vec3 Q2 = hullB->GetVertex(twin2->origin);
b3Vec3 P2 = hull2->GetVertex(edge2->origin);
b3Vec3 Q2 = hull2->GetVertex(twin2->origin);
b3Vec3 E2 = Q2 - P2;
// The Gauss Map of edge 2.
b3Vec3 U2 = hullB->GetPlane(edge2->face).normal;
b3Vec3 V2 = hullB->GetPlane(twin2->face).normal;
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))
@ -153,8 +166,8 @@ b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xfA, const b3Hull* hullA,
}
b3EdgeQuery out;
out.indexA = maxIndex1;
out.indexB = maxIndex2;
out.index1 = maxIndex1;
out.index2 = maxIndex2;
out.separation = maxSeparation;
return out;
}

61
src/bounce/collision/sat/sat_edge_and_hull.cpp
View File

@ -20,26 +20,26 @@
#include <bounce/collision/shapes/capsule.h>
#include <bounce/collision/shapes/hull.h>
float32 b3ProjectEdge(const b3Capsule* hull, const b3Plane& plane)
float32 b3ProjectEdge(const b3Segment* hull, const b3Plane& plane)
{
b3Vec3 support = hull->GetVertex(hull->GetSupportVertex(-plane.normal));
return b3Distance(support, plane);
}
b3FaceQuery b3QueryFaceSeparation(const b3Transform& xfA, const b3Capsule* hullA,
const b3Transform& xfB, const b3Hull* hullB)
b3FaceQuery b3QueryFaceSeparation(const b3Transform& xf1, const b3Segment* hull1,
const b3Transform& xf2, const b3Hull* hull2)
{
// Perform computations in the local space of the first hull.
b3Transform xf = b3MulT(xfA, xfB);
b3Transform xf = b3MulT(xf1, xf2);
// Here greater means less than since is a signed distance.
u32 maxIndex = 0;
float32 maxSeparation = -B3_MAX_FLOAT;
for (u32 i = 0; i < hullB->faceCount; ++i)
for (u32 i = 0; i < hull2->faceCount; ++i)
{
b3Plane plane = b3Mul(xf, hullB->GetPlane(i));
float32 separation = b3ProjectEdge(hullA, plane);
b3Plane plane = b3Mul(xf, hull2->GetPlane(i));
float32 separation = b3ProjectEdge(hull1, plane);
if (separation > maxSeparation)
{
maxIndex = i;
@ -62,13 +62,24 @@ bool b3IsMinkowskiFaceEdge(const b3Vec3& N, const b3Vec3& C, const b3Vec3& D)
float32 b3ProjectEdge(const b3Vec3& P1, const b3Vec3& E1,
const b3Vec3& P2, const b3Vec3& E2, const b3Vec3& C2)
{
float32 L1 = b3Length(E1);
if (L1 < B3_LINEAR_SLOP)
{
return -B3_MAX_FLOAT;
}
float32 L2 = b3Length(E2);
if (L2 < B3_LINEAR_SLOP)
{
return -B3_MAX_FLOAT;
}
// Skip over almost parallel edges.
b3Vec3 E1_x_E2 = b3Cross(E1, E2);
float32 L = b3Length(E1_x_E2);
const float32 kTol = 0.005f;
if (L < kTol * b3Sqrt(b3LengthSquared(E1) * b3LengthSquared(E2)))
b3Vec3 E1_x_E2 = b3Cross(E1, E2);
float32 L = b3Length(E1_x_E2);
if (L < kTol * L1 * L2)
{
return -B3_MAX_FLOAT;
}
@ -84,34 +95,34 @@ float32 b3ProjectEdge(const b3Vec3& P1, const b3Vec3& E1,
return b3Dot(N, P2 - P1);
}
b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xfA, const b3Capsule* hullA, const b3Transform& xfB, const b3Hull* hullB)
b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xf1, const b3Segment* hull1, const b3Transform& xf2, const b3Hull* hull2)
{
// Query minimum edge separation.
u32 maxIndex = 0;
float32 maxSeparation = -B3_MAX_FLOAT;
// Perform computations in the local space of the second hull.
b3Transform xf = b3MulT(xfB, xfA);
b3Transform xf = b3MulT(xf2, xf1);
b3Vec3 P1 = b3Mul(xf, hullA->vertices[0]);
b3Vec3 Q1 = b3Mul(xf, hullA->vertices[1]);
b3Vec3 P1 = b3Mul(xf, hull1->vertices[0]);
b3Vec3 Q1 = b3Mul(xf, hull1->vertices[1]);
b3Vec3 E1 = Q1 - P1;
b3Vec3 C2 = hullB->centroid;
b3Vec3 C2 = hull2->centroid;
for (u32 i = 0; i < hullB->edgeCount; i += 2)
for (u32 i = 0; i < hull2->edgeCount; i += 2)
{
const b3HalfEdge* edge2 = hullB->GetEdge(i);
const b3HalfEdge* twin2 = hullB->GetEdge(i + 1);
const b3HalfEdge* edge2 = hull2->GetEdge(i);
const b3HalfEdge* twin2 = hull2->GetEdge(i + 1);
B3_ASSERT(edge2->twin == i + 1 && twin2->twin == i);
b3Vec3 P2 = hullB->GetVertex(edge2->origin);
b3Vec3 Q2 = hullB->GetVertex(twin2->origin);
b3Vec3 P2 = hull2->GetVertex(edge2->origin);
b3Vec3 Q2 = hull2->GetVertex(twin2->origin);
b3Vec3 E2 = Q2 - P2;
b3Vec3 U2 = hullB->GetPlane(edge2->face).normal;
b3Vec3 V2 = hullB->GetPlane(twin2->face).normal;
b3Vec3 U2 = hull2->GetPlane(edge2->face).normal;
b3Vec3 V2 = hull2->GetPlane(twin2->face).normal;
if (b3IsMinkowskiFaceEdge(E1, U2, V2))
{
@ -125,8 +136,8 @@ b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xfA, const b3Capsule* hullA
}
b3EdgeQuery out;
out.indexA = 0;
out.indexB = maxIndex;
out.index1 = 0;
out.index2 = maxIndex;
out.separation = maxSeparation;
return out;
}

14
src/bounce/collision/sat/sat_vertex_and_hull.cpp
View File

@ -20,24 +20,24 @@
#include <bounce/collision/shapes/sphere.h>
#include <bounce/collision/shapes/hull.h>
float32 b3ProjectVertex(const b3Sphere* hull, const b3Plane& plane)
float32 b3ProjectVertex(const b3Vec3& hull, const b3Plane& plane)
{
b3Vec3 support = hull->GetVertex(hull->GetSupportVertex(-plane.normal));
b3Vec3 support = hull;
return b3Distance(support, plane);
}
b3FaceQuery b3QueryFaceSeparation(const b3Transform& xfA, const b3Sphere* hullA,
const b3Transform& xfB, const b3Hull* hullB)
b3FaceQuery b3QueryFaceSeparation(const b3Transform& xf1, const b3Vec3& hull,
const b3Transform& xf2, const b3Hull* hull2)
{
// Perform computations in the local space of the second hull.
b3Vec3 support = b3MulT(xfB, b3Mul(xfA, hullA->vertex));
b3Vec3 support = b3MulT(xf2, b3Mul(xf1, hull));
u32 maxIndex = 0;
float32 maxSeparation = -B3_MAX_FLOAT;
for (u32 i = 0; i < hullB->faceCount; ++i)
for (u32 i = 0; i < hull2->faceCount; ++i)
{
b3Plane plane = hullB->GetPlane(i);
b3Plane plane = hull2->GetPlane(i);
float32 separation = b3Distance(support, plane);
if (separation > maxSeparation)

2
src/bounce/collision/shapes/hull.cpp
View File

@ -34,7 +34,7 @@ void b3Hull::Validate() const
b3Vec3 B = vertices[twin->origin];
// Ensure each edge has non-zero length.
B3_ASSERT(b3DistanceSquared(A, B) > B3_EPSILON * B3_EPSILON);
B3_ASSERT(b3DistanceSquared(A, B) > B3_LINEAR_SLOP * B3_LINEAR_SLOP);
}
}

87
src/bounce/common/math/mat.cpp
View File

@ -72,7 +72,7 @@ b3Vec3 b3Mat33::Solve(const b3Vec3& b) const
return xn;
}
b3Mat33 b3Adjucate(const b3Mat33& A)
static B3_FORCE_INLINE b3Mat33 b3Adjucate(const b3Mat33& A)
{
b3Vec3 c1 = b3Cross(A.y, A.z);
b3Vec3 c2 = b3Cross(A.z, A.x);
@ -95,88 +95,3 @@ b3Mat33 b3Inverse(const b3Mat33& A)
}
return det * b3Adjucate(A);
}
bool b3Solve(float32* b, float32* A, u32 n)
{
// Gaussian Elimination.
// Loop through the diagonal elements.
for (u32 pivot = 0; pivot < n; ++pivot)
{
// Find the largest element in the current column.
u32 maxRow = pivot;
float32 maxElem = b3Abs(A[maxRow + n * pivot]);
for (u32 i = pivot + 1; i < n; ++i)
{
float32 e = b3Abs(A[i + n * pivot]);
if (e > maxElem)
{
maxElem = e;
maxRow = i;
}
}
// Singularity check.
if (b3Abs(maxElem) <= B3_EPSILON)
{
return false;
}
// Swap rowns if not in the current row.
if (maxRow != pivot)
{
// Swap the row.
for (u32 j = 0; j < n; ++j)
{
float32 a0 = A[maxRow + n * j];
A[maxRow + n * j] = A[pivot + n * j];
A[pivot + n * j] = a0;
}
// Swap b elements.
float32 b0 = b[maxRow];
b[maxRow] = b[pivot];
b[pivot] = b0;
}
// Divide current row by pivot.
float32 invPivot = 1.0f / A[n * pivot + pivot];
for (u32 j = 0; j < n; ++j)
{
A[pivot + n * j] *= invPivot;
}
b[pivot] *= invPivot;
// Ensure pivot is 1.
A[pivot + n * pivot] = 1.0f;
// Zero pivot column in other rows.
for (u32 i = pivot + 1; i < n; ++i)
{
// Subtract multiple of pivot row from current row,
// such that pivot column element becomes 0.
float32 factor = A[i + n * pivot];
// Subtract multiple of row.
for (u32 j = 0; j < n; ++j)
{
A[i + n * j] -= factor * A[pivot + n * j];
}
b[i] -= factor * b[pivot];
}
}
// Backwards substitution.
u32 p = n - 1;
do
{
--p;
for (u32 j = p + 1; j < n; ++j)
{
b[p] -= A[p + n*j] * b[j];
}
} while (p > 0);
return true;
}

20
src/bounce/dynamics/contacts/collide/clip.cpp
View File

@ -21,7 +21,7 @@
#include <bounce/collision/shapes/hull.h>
void b3BuildEdge(b3ClipVertex vOut[2],
const b3Capsule* hull)
const b3Segment* hull)
{
vOut[0].position = hull->vertices[0];
vOut[0].pair = b3MakePair(0, B3_NULL_EDGE, 0, B3_NULL_EDGE);
@ -81,14 +81,14 @@ u32 b3ClipEdgeToPlane(b3ClipVertex vOut[2],
{
float32 fraction = distance1 / (distance1 - distance2);
vOut[numOut].position = vIn[0].position + fraction * (vIn[1].position - vIn[0].position);
vOut[numOut].pair = b3MakePair(vIn[0].pair.inEdgeA, B3_NULL_EDGE, B3_NULL_EDGE, plane.id);
vOut[numOut].pair = b3MakePair(vIn[0].pair.inEdge1, B3_NULL_EDGE, B3_NULL_EDGE, plane.id);
++numOut;
}
else if (distance1 > 0.0f && distance2 <= 0.0f)
{
float32 fraction = distance1 / (distance1 - distance2);
vOut[numOut].position = vIn[0].position + fraction * (vIn[1].position - vIn[0].position);
vOut[numOut].pair = b3MakePair(vIn[1].pair.inEdgeA, plane.id, B3_NULL_EDGE, B3_NULL_EDGE);
vOut[numOut].pair = b3MakePair(vIn[1].pair.inEdge1, plane.id, B3_NULL_EDGE, B3_NULL_EDGE);
++numOut;
}
@ -124,7 +124,7 @@ void b3ClipPolygonToPlane(b3ClipPolygon& pOut,
b3ClipVertex vertex;
vertex.position = v1.position + fraction * (v2.position - v1.position);
vertex.pair = b3MakePair(v1.pair.inEdgeA, B3_NULL_EDGE, B3_NULL_EDGE, plane.id);
vertex.pair = b3MakePair(v1.pair.inEdge1, B3_NULL_EDGE, B3_NULL_EDGE, plane.id);
pOut.PushBack(vertex);
}
@ -136,7 +136,7 @@ void b3ClipPolygonToPlane(b3ClipPolygon& pOut,
b3ClipVertex vertex;
vertex.position = v1.position + fraction * (v2.position - v1.position);
vertex.pair = b3MakePair(v1.pair.inEdgeA, plane.id, B3_NULL_EDGE, B3_NULL_EDGE);
vertex.pair = b3MakePair(v1.pair.inEdge1, plane.id, B3_NULL_EDGE, B3_NULL_EDGE);
pOut.PushBack(vertex);
pOut.PushBack(v2);
@ -150,7 +150,7 @@ void b3ClipPolygonToPlane(b3ClipPolygon& pOut,
// Clip a segment to edge side planes.
u32 b3ClipEdgeToFace(b3ClipVertex vOut[2],
const b3ClipVertex vIn[2], const b3Capsule* hull)
const b3ClipVertex vIn[2], const b3Segment* hull)
{
// Start from somewhere.
vOut[0] = vIn[0];
@ -199,7 +199,7 @@ u32 b3ClipEdgeToFace(b3ClipVertex vOut[2],
// Clip a segment to face side planes.
u32 b3ClipEdgeToFace(b3ClipVertex vOut[2],
const b3ClipVertex vIn[2], const b3Transform& xf, u32 index, const b3Hull* hull)
const b3ClipVertex vIn[2], const b3Transform& xf, float32 r, u32 index, const b3Hull* hull)
{
// Start from somewhere.
vOut[0] = vIn[0];
@ -215,7 +215,7 @@ u32 b3ClipEdgeToFace(b3ClipVertex vOut[2],
u32 edgeId = u32(twin->twin);
b3Plane plane = hull->GetEdgeSidePlane(edgeId);
plane.offset += B3_HULL_RADIUS_SUM;
plane.offset += r;
b3ClipPlane clipPlane;
clipPlane.id = edgeId;
@ -241,7 +241,7 @@ u32 b3ClipEdgeToFace(b3ClipVertex vOut[2],
// Clip a polygon to face side planes.
void b3ClipPolygonToFace(b3ClipPolygon& pOut,
const b3ClipPolygon& pIn, const b3Transform& xf, u32 index, const b3Hull* hull)
const b3ClipPolygon& pIn, const b3Transform& xf, float32 r, u32 index, const b3Hull* hull)
{
B3_ASSERT(pIn.Count() > 0);
B3_ASSERT(pOut.Count() == 0);
@ -258,7 +258,7 @@ void b3ClipPolygonToFace(b3ClipPolygon& pOut,
u32 edgeId = u32(twin->twin);
b3Plane plane = hull->GetEdgeSidePlane(edgeId);
plane.offset += B3_HULL_RADIUS_SUM;
plane.offset += r;
b3ClipPlane clipPlane;
clipPlane.id = edgeId;

14
src/bounce/dynamics/contacts/collide/collide.cpp
View File

@ -25,7 +25,7 @@
#include <bounce/collision/shapes/capsule.h>
#include <bounce/collision/shapes/hull.h>
#include <bounce/collision/shapes/mesh.h>
#include <bounce/collision/distance.h>
#include <bounce/collision/collision.h>
void b3ShapeGJKProxy::Set(const b3Shape* shape, u32 index)
{
@ -90,7 +90,7 @@ bool b3TestOverlap(const b3Transform& xfA, u32 indexA, const b3Shape* shapeA,
b3GJKOutput distance = b3GJK(xfA, proxyA, xfB, proxyB, true, &cache->simplexCache);
const float32 kTol = 2.0f * B3_EPSILON;
const float32 kTol = 10.0f * B3_EPSILON;
return distance.distance <= kTol;
}
@ -127,7 +127,7 @@ void b3CollideSphereAndCapsuleShapes(b3Manifold& manifold,
b3CollideSphereAndCapsule(manifold, xfA, hullA, xfB, hullB);
}
void b3CollideCapsuleAndCapsuleShapes(b3Manifold& manifold,
void b3CollideCapsuleAndCapsuleShapes(b3Manifold& manifold,
const b3Transform& xfA, const b3Shape* shapeA,
const b3Transform& xfB, const b3Shape* shapeB,
b3ConvexCache* cache)
@ -138,7 +138,7 @@ void b3CollideCapsuleAndCapsuleShapes(b3Manifold& manifold,
b3CollideCapsuleAndCapsule(manifold, xfA, hullA, xfB, hullB);
}
void b3CollideCapsuleAndHullShapes(b3Manifold& manifold,
void b3CollideCapsuleAndHullShapes(b3Manifold& manifold,
const b3Transform& xfA, const b3Shape* shapeA,
const b3Transform& xfB, const b3Shape* shapeB,
b3ConvexCache* cache)
@ -148,7 +148,7 @@ void b3CollideCapsuleAndHullShapes(b3Manifold& manifold,
b3CollideCapsuleAndHull(manifold, xfA, hullA, xfB, hullB);
}
void b3CollideHullAndHullShapes(b3Manifold& manifold,
void b3CollideHullAndHullShapes(b3Manifold& manifold,
const b3Transform& xfA, const b3Shape* shapeA,
const b3Transform& xfB, const b3Shape* shapeB,
b3ConvexCache* cache)
@ -164,8 +164,8 @@ void b3CollideShapeAndShape(b3Manifold& manifold,
b3ConvexCache* cache)
{
typedef void(*b3CollideFunction)(b3Manifold&,
const b3Transform&, const class b3Shape*,
const b3Transform&, const class b3Shape*,
const b3Transform&, const b3Shape*,
const b3Transform&, const b3Shape*,
b3ConvexCache*);
static const b3CollideFunction s_CollideMatrix[e_maxShapes][e_maxShapes] =

205
src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp
View File

@ -19,159 +19,167 @@
#include <bounce/dynamics/contacts/collide/collide.h>
#include <bounce/dynamics/contacts/collide/clip.h>
#include <bounce/dynamics/contacts/manifold.h>
#include <bounce/dynamics/shapes/sphere_shape.h>
#include <bounce/dynamics/shapes/capsule_shape.h>
#include <bounce/dynamics/shapes/hull_shape.h>
#include <bounce/collision/shapes/sphere.h>
#include <bounce/collision/shapes/capsule.h>
#include <bounce/collision/shapes/hull.h>
void b3BuildEdgeContact(b3Manifold& manifold,
const b3Transform& xf1, const b3Capsule* hull1,
static void b3BuildEdgeContact(b3Manifold& manifold,
const b3Transform& xf1, const b3Segment* hull1,
const b3Transform& xf2, u32 index2, const b3Hull* hull2)
{
b3Vec3 P1 = b3Mul(xf1, hull1->GetVertex(0));
b3Vec3 Q1 = b3Mul(xf1, hull1->GetVertex(1));
b3Vec3 P1 = xf1 * hull1->GetVertex(0);
b3Vec3 Q1 = xf1 * hull1->GetVertex(1);
b3Vec3 E1 = Q1 - P1;
b3Vec3 N1 = b3Normalize(E1);
b3Vec3 N1 = E1;
float32 L1 = N1.Normalize();
B3_ASSERT(L1 > 0.0f);
const b3HalfEdge* edge2 = hull2->GetEdge(index2);
const b3HalfEdge* twin2 = hull2->GetEdge(index2 + 1);
b3Vec3 C2 = b3Mul(xf2, hull2->centroid);
b3Vec3 P2 = b3Mul(xf2, hull2->GetVertex(edge2->origin));
b3Vec3 Q2 = b3Mul(xf2, hull2->GetVertex(twin2->origin));
b3Vec3 C2 = xf2 * hull2->centroid;
b3Vec3 P2 = xf2 * hull2->GetVertex(edge2->origin);
b3Vec3 Q2 = xf2 * hull2->GetVertex(twin2->origin);
b3Vec3 E2 = Q2 - P2;
b3Vec3 N2 = b3Normalize(E2);
b3Vec3 N2 = E2;
float32 L2 = N2.Normalize();
B3_ASSERT(L2 > 0.0f);
// Compute the closest points on the two lines.
float32 b = b3Dot(N1, N2);
float32 den = 1.0f - b * b;
if (den <= 0.0f)
{
return;
}
float32 inv_den = 1.0f / den;
b3Vec3 E3 = P1 - P2;
float32 d = b3Dot(N1, E3);
float32 e = b3Dot(N2, E3);
float32 s = inv_den * (b * e - d);
float32 t = inv_den * (e - b * d);
b3Vec3 c1 = P1 + s * N1;
b3Vec3 c2 = P2 + t * N2;
b3Vec3 N = b3Cross(E1, E2);
N.Normalize();
float32 LN = N.Normalize();
B3_ASSERT(LN > 0.0f);
if (b3Dot(N, P2 - C2) > 0.0f)
{
N = -N;
}
b3Vec3 PA, PB;
b3ClosestPointsOnNormalizedLines(&PA, &PB, P1, N1, P2, N2);
b3FeaturePair pair = b3MakePair(0, 1, index2, index2 + 1);
manifold.pointCount = 1;
manifold.points[0].localNormal1 = b3MulT(xf1.rotation, N);
manifold.points[0].localPoint1 = b3MulT(xf1, c1);
manifold.points[0].localPoint2 = b3MulT(xf2, c2);
manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
manifold.points[0].key = b3MakeKey(pair);
manifold.points[0].localNormal = b3MulT(xf1.rotation, N);
manifold.points[0].localPoint = b3MulT(xf1, PA);
manifold.points[0].localPoint2 = b3MulT(xf2, PB);
manifold.center = 0.5f * (PA + hull1->radius * N + PB - B3_HULL_RADIUS * N);
manifold.normal = N;
manifold.tangent1 = b3Perp(N);
manifold.tangent2 = b3Cross(manifold.tangent1, N);
}
void b3BuildFaceContact(b3Manifold& manifold,
const b3Transform& xf1, const b3Capsule* hull1,
const b3Transform& xf2, u32 index2, const b3Hull* hull2)
static void b3BuildFaceContact(b3Manifold& manifold,
const b3Transform& xf1, float32 r1, const b3Segment* hull1,
const b3Transform& xf2, float32 r2, u32 index2, const b3Hull* hull2)
{
// Clip edge 1 against the side planes of the face 2.
b3Capsule tempHull1;
tempHull1.vertices[0] = b3Mul(xf1, hull1->vertices[0]);
tempHull1.vertices[1] = b3Mul(xf1, hull1->vertices[1]);
tempHull1.radius = hull1->radius;
b3Segment tempEdge1;
tempEdge1.vertices[0] = xf1 * hull1->vertices[0];
tempEdge1.vertices[1] = xf1 * hull1->vertices[1];
b3ClipVertex edge1[2];
b3BuildEdge(edge1, &tempHull1);
b3BuildEdge(edge1, &tempEdge1);
b3ClipVertex clipEdge1[2];
u32 clipCount = b3ClipEdgeToFace(clipEdge1, edge1, xf2, index2, hull2);
u32 clipCount = b3ClipEdgeToFace(clipEdge1, edge1, xf2, r2, index2, hull2);
// Project clipped edge 1 onto face plane 2.
float32 r1 = hull1->radius;
float32 r2 = B3_HULL_RADIUS;
float32 totalRadius = r1 + r2;
b3Plane localPlane2 = hull2->GetPlane(index2);
b3Plane plane2 = b3Mul(xf2, localPlane2);
b3Plane plane2 = xf2 * localPlane2;
const b3Face* face2 = hull2->GetFace(index2);
const b3HalfEdge* edge2 = hull2->GetEdge(face2->edge);
b3Vec3 localPoint2 = hull2->GetVertex(edge2->origin);
b3Vec3 normal = -plane2.normal;
b3Vec3 center;
center.SetZero();
b3Vec3 n1 = -plane2.normal;
float32 totalRadius = r1 + r2;
u32 pointCount = 0;
for (u32 i = 0; i < clipCount; ++i)
{
float32 s = b3Distance(clipEdge1[i].position, plane2);
b3Vec3 c1 = clipEdge1[i].position;
float32 s = b3Distance(c1, plane2);
if (s <= totalRadius)
{
b3Vec3 cp = b3ClosestPointOnPlane(clipEdge1[i].position, plane2);
b3Vec3 p = 0.5f * (clipEdge1[i].position + r1 * normal + cp - r2 * normal);
b3Vec3 c2 = b3ClosestPointOnPlane(c1, plane2);
b3ManifoldPoint* mp = manifold.points + pointCount;
mp->localNormal1 = b3MulT(xf1.rotation, n1);
mp->localPoint1 = b3MulT(xf1, c1);
mp->localPoint2 = b3MulT(xf2, c2);
mp->triangleKey = B3_NULL_TRIANGLE;
mp->key = b3MakeKey(clipEdge1[i].pair);
mp->localNormal = b3MulT(xf1.rotation, normal);
mp->localPoint = b3MulT(xf1, clipEdge1[i].position);
mp->localPoint2 = b3MulT(xf2, cp);
++pointCount;
center += p;
}
}
if (pointCount > 0)
{
center /= pointCount;
manifold.center = center;
manifold.normal = normal;
manifold.tangent1 = b3Perp(normal);
manifold.tangent2 = b3Cross(manifold.tangent1, normal);
manifold.pointCount = pointCount;
}
manifold.pointCount = pointCount;
}
void b3CollideCapsuleAndHull(b3Manifold& manifold,
const b3Transform& xfA, const b3CapsuleShape* sA,
const b3Transform& xfB, const b3HullShape* sB)
void b3CollideCapsuleAndHull(b3Manifold& manifold,
const b3Transform& xf1, const b3CapsuleShape* s1,
const b3Transform& xf2, const b3HullShape* s2)
{
b3Capsule hullA;
hullA.vertices[0] = sA->m_centers[0];
hullA.vertices[1] = sA->m_centers[1];
hullA.radius = sA->m_radius;
const b3Hull* hullB = sB->m_hull;
b3ShapeGJKProxy proxy1(s1, 0);
b3ShapeGJKProxy proxy2(s2, 0);
b3GJKOutput gjk = b3GJK(xf1, proxy1, xf2, proxy2);
float32 r1 = s1->m_radius, r2 = s2->m_radius;
b3ShapeGJKProxy proxyA(sA, 0);
b3ShapeGJKProxy proxyB(sB, 0);
b3GJKOutput distance = b3GJK(xfA, proxyA, xfB, proxyB);
float32 totalRadius = hullA.radius + B3_HULL_RADIUS;
if (distance.distance > totalRadius)
float32 totalRadius = r1 + r2;
if (gjk.distance > totalRadius)
{
return;
}
if (distance.distance > 0.0f)
b3Segment hull1;
hull1.vertices[0] = s1->m_centers[0];
hull1.vertices[1] = s1->m_centers[1];
const b3Hull* hull2 = s2->m_hull;
if (gjk.distance > B3_EPSILON)
{
// Define incident face.
b3Vec3 NA = (distance.pointB - distance.pointA) / distance.distance;
b3Vec3 localNA = b3MulT(xfB.rotation, NA);
b3Vec3 N1 = (gjk.point2 - gjk.point1) / gjk.distance;
b3Vec3 localN1 = b3MulT(xf2.rotation, N1);
// Search reference face.
u32 indexB = hullB->GetSupportFace(-localNA);
b3Vec3 localNB = hullB->GetPlane(indexB).normal;
b3Vec3 NB = b3Mul(xfB.rotation, localNB);
u32 index2 = hull2->GetSupportFace(-localN1);
b3Vec3 localN2 = hull2->GetPlane(index2).normal;
b3Vec3 N2 = xf2.rotation * localN2;
// Paralell vectors |v1xv2| = sin(theta)
const float32 kTol = 0.005f;
b3Vec3 axis = b3Cross(NA, NB);
float32 L = b3Dot(axis, axis);
b3Vec3 N = b3Cross(N1, N2);
float32 L = b3Dot(N, N);
if (L < kTol * kTol)
{
// Reference face found.
// Try to build a face contact.
b3BuildFaceContact(manifold, xfA, &hullA, xfB, indexB, hullB);
b3BuildFaceContact(manifold, xf1, r1, &hull1, xf2, r2, index2, hull2);
if (manifold.pointCount == 2)
{
return;
@ -179,41 +187,34 @@ void b3CollideCapsuleAndHull(b3Manifold& manifold,
}
manifold.pointCount = 1;
manifold.points[0].localNormal1 = b3MulT(xf1.rotation, N1);
manifold.points[0].localPoint1 = b3MulT(xf1, gjk.point1);
manifold.points[0].localPoint2 = b3MulT(xf2, gjk.point2);
manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
manifold.points[0].key = 0;
manifold.points[0].localNormal = b3MulT(xfA.rotation, NA);
manifold.points[0].localPoint = b3MulT(xfA, distance.pointA);
manifold.points[0].localPoint2 = b3MulT(xfB, distance.pointB);
manifold.center = 0.5f * (distance.pointA + sA->m_radius * NA + distance.pointB - B3_HULL_RADIUS * NA);
manifold.normal = NA;
manifold.tangent1 = b3Perp(NA);
manifold.tangent2 = b3Cross(manifold.tangent1, NA);
return;
}
b3FaceQuery faceQueryB = b3QueryFaceSeparation(xfA, &hullA, xfB, hullB);
if (faceQueryB.separation > totalRadius)
b3FaceQuery faceQuery2 = b3QueryFaceSeparation(xf1, &hull1, xf2, hull2);
if (faceQuery2.separation > totalRadius)
{
return;
}
b3EdgeQuery edgeQuery = b3QueryEdgeSeparation(xfA, &hullA, xfB, hullB);
b3EdgeQuery edgeQuery = b3QueryEdgeSeparation(xf1, &hull1, xf2, hull2);
if (edgeQuery.separation > totalRadius)
{
return;
}
// Now the hulls are overlapping.
const float32 kRelEdgeTol = 0.95f;
const float32 kAbsTol = 0.05f;
if (edgeQuery.separation > kRelEdgeTol * faceQueryB.separation + kAbsTol)
const float32 kTol = 0.1f * B3_LINEAR_SLOP;
if (edgeQuery.separation > faceQuery2.separation + kTol)
{
b3BuildEdgeContact(manifold, xfA, &hullA, xfB, edgeQuery.indexB, hullB);
b3BuildEdgeContact(manifold, xf1, &hull1, xf2, edgeQuery.index2, hull2);
}
else
{
b3BuildFaceContact(manifold, xfA, &hullA, xfB, faceQueryB.index, hullB);
b3BuildFaceContact(manifold, xf1, r1, &hull1, xf2, r2, faceQuery2.index, hull2);
}
}
}

264
src/bounce/dynamics/contacts/collide/collide_capsules.cpp
View File

@ -22,16 +22,128 @@
#include <bounce/dynamics/shapes/capsule_shape.h>
#include <bounce/collision/shapes/capsule.h>
static bool b3AreParalell(const b3Capsule& hullA, const b3Capsule& hullB)
// Compute the closest point on a segment to a point.
static b3Vec3 b3ClosestPoint(const b3Vec3& Q, const b3Segment& hull)
{
b3Vec3 E1 = hullA.vertices[1] - hullA.vertices[0];
b3Vec3 A = hull.vertices[0];
b3Vec3 B = hull.vertices[1];
b3Vec3 AB = B - A;
// Barycentric coordinates for Q
float32 u = b3Dot(B - Q, AB);
float32 v = b3Dot(Q - A, AB);
if (v <= 0.0f)
{
return A;
}
if (u <= 0.0f)
{
return B;
}
float32 w = b3Dot(AB, AB);
if (w <= B3_LINEAR_SLOP * B3_LINEAR_SLOP)
{
return A;
}
float32 den = 1.0f / w;
b3Vec3 P = den * (u * A + v * B);
return P;
}
// Compute the closest points between two line segments.
static void b3ClosestPoints(b3Vec3& C1, b3Vec3& C2,
const b3Segment& hull1, const b3Segment& hull2)
{
b3Vec3 P1 = hull1.vertices[0];
b3Vec3 Q1 = hull1.vertices[1];
b3Vec3 P2 = hull2.vertices[0];
b3Vec3 Q2 = hull2.vertices[1];
b3Vec3 E1 = Q1 - P1;
float32 L1 = b3Length(E1);
b3Vec3 E2 = Q2 - P2;
float32 L2 = b3Length(E2);
if (L1 < B3_LINEAR_SLOP && L2 < B3_LINEAR_SLOP)
{
C1 = P1;
C2 = P2;
return;
}
if (L1 < B3_LINEAR_SLOP)
{
C1 = P1;
C2 = b3ClosestPoint(P1, hull2);
return;
}
if (L2 < B3_LINEAR_SLOP)
{
C1 = b3ClosestPoint(P2, hull1);
C2 = P2;
return;
}
// Here and in 3D we need to start "GJK" with the closest points between the two edges
// since the cross product between their direction is a possible separating axis.
b3Vec3 N1 = E1;
float32 LN1 = N1.Normalize();
b3Vec3 N2 = E2;
float32 LN2 = N2.Normalize();
float32 b = b3Dot(N1, N2);
float32 den = 1.0f - b * b;
const float32 kTol = 0.005f;
if (den < kTol * kTol)
{
C1 = P1;
C2 = P2;
}
else
{
// s - b * t = -d
// b * s - t = -e
// s = ( b * e - d ) / den
// t = ( e - b * d ) / den
b3Vec3 E3 = P1 - P2;
float32 d = b3Dot(N1, E3);
float32 e = b3Dot(N2, E3);
float32 inv_den = 1.0f / den;
float32 s = inv_den * (b * e - d);
float32 t = inv_den * (e - b * d);
C1 = P1 + s * N1;
C2 = P2 + t * N2;
}
C1 = b3ClosestPoint(C1, hull1);
C2 = b3ClosestPoint(C1, hull2);
C1 = b3ClosestPoint(C2, hull1);
}
static bool b3AreParalell(const b3Segment& hull1, const b3Segment& hull2)
{
b3Vec3 E1 = hull1.vertices[1] - hull1.vertices[0];
float32 L1 = b3Length(E1);
if (L1 < B3_LINEAR_SLOP)
{
return false;
}
b3Vec3 E2 = hullB.vertices[1] - hullB.vertices[0];
b3Vec3 E2 = hull2.vertices[1] - hull2.vertices[0];
float32 L2 = b3Length(E2);
if (L2 < B3_LINEAR_SLOP)
{
@ -44,77 +156,26 @@ static bool b3AreParalell(const b3Capsule& hullA, const b3Capsule& hullB)
return b3Length(N) < kTol * L1 * L2;
}
void b3CollideCapsuleAndCapsule(b3Manifold& manifold,
const b3Transform& xfA, const b3CapsuleShape* sA,
const b3Transform& xfB, const b3CapsuleShape* sB)
void b3CollideCapsuleAndCapsule(b3Manifold& manifold,
const b3Transform& xf1, const b3CapsuleShape* s1,
const b3Transform& xf2, const b3CapsuleShape* s2)
{
b3Capsule hullA;
hullA.vertices[0] = b3Mul(xfA, sA->m_centers[0]);
hullA.vertices[1] = b3Mul(xfA, sA->m_centers[1]);
hullA.radius = sA->m_radius;
b3Capsule hullB;
hullB.vertices[0] = b3Mul(xfB, sB->m_centers[0]);
hullB.vertices[1] = b3Mul(xfB, sB->m_centers[1]);
hullB.radius = sB->m_radius;
float32 totalRadius = hullA.radius + hullB.radius;
b3Segment hull1;
hull1.vertices[0] = xf1 * s1->m_centers[0];
hull1.vertices[1] = xf1 * s1->m_centers[1];
if (b3AreParalell(hullA, hullB))
{
// Clip edge A against the side planes of edge B.
b3ClipVertex edgeA[2];
b3BuildEdge(edgeA, &hullA);
b3Segment hull2;
hull2.vertices[0] = xf2 * s2->m_centers[0];
hull2.vertices[1] = xf2 * s2->m_centers[1];
b3Vec3 point1, point2;
b3ClosestPoints(point1, point2, hull1, hull2);
float32 distance = b3Distance(point1, point2);
b3ClipVertex clipEdgeA[2];
u32 clipCount = b3ClipEdgeToFace(clipEdgeA, edgeA, &hullB);
if (clipCount == 2)
{
b3Vec3 cp1 = b3ClosestPointOnSegment(clipEdgeA[0].position, hullB.vertices[0], hullB.vertices[1]);
b3Vec3 cp2 = b3ClosestPointOnSegment(clipEdgeA[1].position, hullB.vertices[0], hullB.vertices[1]);
float32 d1 = b3Distance(clipEdgeA[0].position, cp1);
float32 d2 = b3Distance(clipEdgeA[1].position, cp2);
if (d1 > B3_EPSILON && d1 <= totalRadius && d2 > B3_EPSILON && d2 <= totalRadius)
{
b3Vec3 n1 = (cp1 - clipEdgeA[0].position) / d1;
b3Vec3 n2 = (cp2 - clipEdgeA[1].position) / d2;
b3Vec3 p1 = 0.5f * (clipEdgeA[0].position + hullA.radius * n1 + cp1 - hullB.radius * n1);
b3Vec3 p2 = 0.5f * (clipEdgeA[1].position + hullA.radius * n2 + cp2 - hullB.radius * n2);
b3Vec3 center = 0.5f * (p1 + p2);
b3Vec3 normal = b3Normalize(n1 + n2);
manifold.pointCount = 2;
manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
manifold.points[0].key = b3MakeKey(clipEdgeA[0].pair);
manifold.points[0].localNormal = b3MulT(xfA.rotation, n1);
manifold.points[0].localPoint = b3MulT(xfA, clipEdgeA[0].position);
manifold.points[0].localPoint2 = b3MulT(xfB, cp1);
manifold.points[1].triangleKey = B3_NULL_TRIANGLE;
manifold.points[1].key = b3MakeKey(clipEdgeA[1].pair);
manifold.points[1].localNormal = b3MulT(xfA.rotation, n2);
manifold.points[1].localPoint = b3MulT(xfA, clipEdgeA[1].position);
manifold.points[1].localPoint2 = b3MulT(xfB, cp2);
manifold.center = center;
manifold.normal = normal;
manifold.tangent1 = b3Perp(normal);
manifold.tangent2 = b3Cross(manifold.tangent1, normal);
return;
}
}
}
b3Vec3 pointA, pointB;
b3ClosestPointsOnSegments(&pointA, &pointB, hullA.vertices[0], hullA.vertices[1], hullB.vertices[0], hullB.vertices[1]);
float32 distance = b3Distance(pointA, pointB);
float32 r1 = s1->m_radius;
float32 r2 = s2->m_radius;
float32 totalRadius = r1 + r2;
if (distance > totalRadius)
{
return;
@ -122,19 +183,62 @@ void b3CollideCapsuleAndCapsule(b3Manifold& manifold,
if (distance > B3_EPSILON)
{
b3Vec3 normal = (pointB - pointA) / distance;
b3Vec3 center = 0.5f * (pointA + hullA.radius * normal + pointB - hullB.radius * normal);
if (b3AreParalell(hull1, hull2))
{
// Clip edge 1 against the side planes of edge 2.
b3ClipVertex edge1[2];
b3BuildEdge(edge1, &hull1);
b3ClipVertex clipEdge1[2];
u32 clipCount = b3ClipEdgeToFace(clipEdge1, edge1, &hull2);
if (clipCount == 2)
{
b3Vec3 cp1 = b3ClosestPointOnSegment(clipEdge1[0].position, hull2.vertices[0], hull2.vertices[1]);
b3Vec3 cp2 = b3ClosestPointOnSegment(clipEdge1[1].position, hull2.vertices[0], hull2.vertices[1]);
float32 d1 = b3Distance(clipEdge1[0].position, cp1);
float32 d2 = b3Distance(clipEdge1[1].position, cp2);
if (d1 > B3_EPSILON && d1 <= totalRadius && d2 > B3_EPSILON && d2 <= totalRadius)
{
b3Vec3 n1 = (cp1 - clipEdge1[0].position) / d1;
b3Vec3 n2 = (cp2 - clipEdge1[1].position) / d2;
b3Vec3 p1 = 0.5f * (clipEdge1[0].position + r1 * n1 + cp1 - r2 * n1);
b3Vec3 p2 = 0.5f * (clipEdge1[1].position + r1 * n2 + cp2 - r2 * n2);
b3Vec3 center = 0.5f * (p1 + p2);
b3Vec3 normal = b3Normalize(n1 + n2);
manifold.pointCount = 2;
manifold.points[0].localNormal1 = b3MulT(xf1.rotation, n1);
manifold.points[0].localPoint1 = b3MulT(xf1, clipEdge1[0].position);
manifold.points[0].localPoint2 = b3MulT(xf2, cp1);
manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
manifold.points[0].key = b3MakeKey(clipEdge1[0].pair);
manifold.points[1].localNormal1 = b3MulT(xf1.rotation, n2);
manifold.points[1].localPoint1 = b3MulT(xf1, clipEdge1[1].position);
manifold.points[1].localPoint2 = b3MulT(xf2, cp2);
manifold.points[1].triangleKey = B3_NULL_TRIANGLE;
manifold.points[1].key = b3MakeKey(clipEdge1[1].pair);
return;
}
}
}
b3Vec3 normal = (point2 - point1) / distance;
manifold.pointCount = 1;
manifold.points[0].localNormal1 = b3MulT(xf1.rotation, normal);
manifold.points[0].localPoint1 = b3MulT(xf1, point1);
manifold.points[0].localPoint2 = b3MulT(xf2, point2);
manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
manifold.points[0].key = 0;
manifold.points[0].localNormal = b3MulT(xfA.rotation, normal);
manifold.points[0].localPoint = b3MulT(xfA, pointA);
manifold.points[0].localPoint2 = b3MulT(xfB, pointB);
manifold.center = center;
manifold.normal = normal;
manifold.tangent1 = b3Perp(normal);
manifold.tangent2 = b3Cross(manifold.tangent1, normal);
return;
}
}

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

@ -23,236 +23,245 @@
#include <bounce/dynamics/shapes/hull_shape.h>
#include <bounce/collision/shapes/hull.h>
void b3BuildEdgeContact(b3Manifold& manifold,
const b3Transform& xfA, const b3Hull* hullA,
const b3Transform& xfB, const b3Hull* hullB,
const b3EdgeQuery& query)
void b3BuildEdgeContact(b3Manifold& manifold,
const b3Transform& xf1, u32 index1, const b3Hull* hull1,
const b3Transform& xf2, u32 index2, const b3Hull* hull2)
{
u32 indexA = query.indexA;
const b3HalfEdge* edge1 = hull1->GetEdge(index1);
const b3HalfEdge* twin1 = hull1->GetEdge(index1 + 1);
const b3HalfEdge* edge1 = hullA->GetEdge(indexA);
const b3HalfEdge* twin1 = hullA->GetEdge(indexA + 1);
b3Vec3 C1 = b3Mul(xfA, hullA->centroid);
b3Vec3 P1 = b3Mul(xfA, hullA->GetVertex(edge1->origin));
b3Vec3 Q1 = b3Mul(xfA, hullA->GetVertex(twin1->origin));
b3Vec3 C1 = xf1 * hull1->centroid;
b3Vec3 P1 = xf1 * hull1->GetVertex(edge1->origin);
b3Vec3 Q1 = xf1 * hull1->GetVertex(twin1->origin);
b3Vec3 E1 = Q1 - P1;
b3Vec3 N1 = E1;
float32 L1 = N1.Normalize();
B3_ASSERT(L1 > B3_LINEAR_SLOP);
u32 indexB = query.indexB;
const b3HalfEdge* edge2 = hullB->GetEdge(indexB);
const b3HalfEdge* twin2 = hullB->GetEdge(indexB + 1);
const b3HalfEdge* edge2 = hull2->GetEdge(index2);
const b3HalfEdge* twin2 = hull2->GetEdge(index2 + 1);
b3Vec3 C2 = b3Mul(xfB, hullB->centroid);
b3Vec3 P2 = b3Mul(xfB, hullB->GetVertex(edge2->origin));
b3Vec3 Q2 = b3Mul(xfB, hullB->GetVertex(twin2->origin));
b3Vec3 C2 = xf2 * hull2->centroid;
b3Vec3 P2 = xf2 * hull2->GetVertex(edge2->origin);
b3Vec3 Q2 = xf2 * hull2->GetVertex(twin2->origin);
b3Vec3 E2 = Q2 - P2;
b3Vec3 N2 = E2;
float32 L2 = N2.Normalize();
B3_ASSERT(L2 > B3_LINEAR_SLOP);
// Compute the closest points on the two lines.
float32 b = b3Dot(N1, N2);
float32 den = 1.0f - b * b;
if (den <= 0.0f)
{
return;
}
float32 inv_den = 1.0f / den;
b3Vec3 E3 = P1 - P2;
float32 d = b3Dot(N1, E3);
float32 e = b3Dot(N2, E3);
float32 s = inv_den * (b * e - d);
float32 t = inv_den * (e - b * d);
b3Vec3 c1 = P1 + s * N1;
b3Vec3 c2 = P2 + t * N2;
b3Vec3 N = b3Cross(E1, E2);
float32 LN = N.Normalize();
B3_ASSERT(LN > 0.0f);
if (b3Dot(N, P1 - C1) < 0.0f)
{
N = -N;
}
N.Normalize();
b3Vec3 PA, PB;
b3ClosestPointsOnLines(&PA, &PB, P1, E1, P2, E2);
b3FeaturePair pair = b3MakePair(indexA, indexA + 1, indexB, indexB + 1);
b3FeaturePair pair = b3MakePair(index1, index1 + 1, index2, index2 + 1);
manifold.pointCount = 1;
manifold.points[0].localNormal1 = b3MulT(xf1.rotation, N);
manifold.points[0].localPoint1 = b3MulT(xf1, c1);
manifold.points[0].localPoint2 = b3MulT(xf2, c2);
manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
manifold.points[0].key = b3MakeKey(pair);
manifold.points[0].localNormal = b3MulT(xfA.rotation, N);
manifold.points[0].localPoint = b3MulT(xfA, PA);
manifold.points[0].localPoint2 = b3MulT(xfB, PB);
manifold.center = 0.5f * (PA + B3_HULL_RADIUS * N + PB - B3_HULL_RADIUS * N);
manifold.normal = N;
manifold.tangent1 = b3Perp(N);
manifold.tangent2 = b3Cross(manifold.tangent1, N);
}
void b3BuildFaceContact(b3Manifold& manifold,
const b3Transform& xfA, const b3Hull* hullA,
const b3Transform& xfB, const b3Hull* hullB,
const b3FaceQuery& query, bool flipNormal)
const b3Transform& xf1, float32 r1, u32 index1, const b3Hull* hull1,
const b3Transform& xf2, float32 r2, const b3Hull* hull2,
bool flipNormal)
{
// 1. Define the reference face plane (A).
u32 indexA = query.index;
const b3Face* faceA = hullA->GetFace(indexA);
const b3HalfEdge* edgeA = hullA->GetEdge(faceA->edge);
b3Plane localPlaneA = hullA->GetPlane(indexA);
b3Vec3 localNormalA = localPlaneA.normal;
b3Vec3 localPointA = hullA->GetVertex(edgeA->origin);
b3Plane planeA = b3Mul(xfA, localPlaneA);
// 1. Define the reference face plane (1).
const b3Face* face1 = hull1->GetFace(index1);
const b3HalfEdge* edge1 = hull1->GetEdge(face1->edge);
b3Plane localPlane1 = hull1->GetPlane(index1);
b3Vec3 localNormal1 = localPlane1.normal;
b3Vec3 localPoint1 = hull1->GetVertex(edge1->origin);
b3Plane plane1 = b3Mul(xf1, localPlane1);
// 2. Find the incident face polygon (B).
// 2. Find the incident face polygon (2).
// Put the reference plane normal in the frame of the incident hull (B).
b3Vec3 normalA = b3MulT(xfB.rotation, planeA.normal);
// Put the reference plane normal in the frame of the incident hull (2).
b3Vec3 normal1 = b3MulT(xf2.rotation, plane1.normal);
// Find the support polygon in the *negated* direction.
b3StackArray<b3ClipVertex, 32> polygonB;
u32 indexB = hullB->GetSupportFace(-normalA);
b3BuildPolygon(polygonB, xfB, indexB, hullB);
// Find the support face polygon in the *negated* direction.
b3StackArray<b3ClipVertex, 32> polygon2;
u32 index2 = hull2->GetSupportFace(-normal1);
b3BuildPolygon(polygon2, xf2, index2, hull2);
// 3. Clip incident face polygon (B) against the reference face (A) side planes.
b3StackArray<b3ClipVertex, 32> clipPolygonB;
b3ClipPolygonToFace(clipPolygonB, polygonB, xfA, indexA, hullA);
if (clipPolygonB.IsEmpty())
// 3. Clip incident face polygon (2) against the reference face (1) side planes.
float32 totalRadius = r1 + r2;
b3StackArray<b3ClipVertex, 32> clipPolygon2;
b3ClipPolygonToFace(clipPolygon2, polygon2, xf1, totalRadius, index1, hull1);
if (clipPolygon2.IsEmpty())
{
return;
}
// 4. Project the clipped polygon on the reference plane for reduction.
// Ensure the deepest point is contained in the reduced polygon.
b3StackArray<b3ClusterVertex, 32> polygonA;
b3StackArray<b3ClusterVertex, 32> polygon1;
u32 minIndex = 0;
float32 minSeparation = B3_MAX_FLOAT;
for (u32 i = 0; i < clipPolygonB.Count(); ++i)
for (u32 i = 0; i < clipPolygon2.Count(); ++i)
{
b3ClipVertex vB = clipPolygonB[i];
float32 separation = b3Distance(vB.position, planeA);
b3ClipVertex v2 = clipPolygon2[i];
float32 separation = b3Distance(v2.position, plane1);
if (separation <= B3_HULL_RADIUS_SUM)
if (separation <= totalRadius)
{
if (separation < minSeparation)
{
minIndex = polygonA.Count();
minIndex = polygon1.Count();
minSeparation = separation;
}
b3ClusterVertex vA;
vA.position = b3Project(vB.position, planeA);
vA.clipIndex = i;
polygonA.PushBack(vA);
b3ClusterVertex v1;
v1.position = b3ClosestPointOnPlane(v2.position, plane1);
v1.clipIndex = i;
polygon1.PushBack(v1);
}
}
if (polygonA.IsEmpty())
if (polygon1.IsEmpty())
{
return;
}
// 5. Reduce.
b3StackArray<b3ClusterVertex, 32> reducedPolygonA;
b3ReducePolygon(reducedPolygonA, polygonA, minIndex);
B3_ASSERT(!reducedPolygonA.IsEmpty());
b3Vec3 normal = plane1.normal;
b3Vec3 s_normal = flipNormal ? -normal : normal;
b3StackArray<b3ClusterVertex, 32> reducedPolygon1;
b3ReducePolygon(reducedPolygon1, polygon1, minIndex, s_normal);
B3_ASSERT(!reducedPolygon1.IsEmpty());
// 6. Build face contact.
b3Vec3 normal = planeA.normal;
manifold.center.SetZero();
u32 pointCount = reducedPolygonA.Count();
u32 pointCount = reducedPolygon1.Count();
for (u32 i = 0; i < pointCount; ++i)
{
u32 clipIndex = reducedPolygonA[i].clipIndex;
b3ClipVertex vB = clipPolygonB[clipIndex];
b3Vec3 vA = b3ClosestPointOnPlane(vB.position, planeA);
u32 clipIndex = reducedPolygon1[i].clipIndex;
b3ClipVertex v2 = clipPolygon2[clipIndex];
b3Vec3 v1 = b3ClosestPointOnPlane(v2.position, plane1);
b3ManifoldPoint* cp = manifold.points + i;
b3ManifoldPoint* mp = manifold.points + i;
if (flipNormal)
{
b3FeaturePair pair;
pair.inEdgeA = vB.pair.inEdgeB;
pair.outEdgeA = vB.pair.outEdgeB;
pair.inEdgeB = vB.pair.inEdgeA;
pair.outEdgeB = vB.pair.outEdgeA;
cp->triangleKey = B3_NULL_TRIANGLE;
cp->key = b3MakeKey(pair);
cp->localNormal = b3MulT(xfB.rotation, -normal);
cp->localPoint = b3MulT(xfB, vB.position);
cp->localPoint2 = b3MulT(xfA, vA);
b3FeaturePair pair = b3MakePair(v2.pair.inEdge2, v2.pair.inEdge1, v2.pair.outEdge2, v2.pair.outEdge1);
manifold.center += 0.5f * (vA + B3_HULL_RADIUS * normal + vB.position - B3_HULL_RADIUS * normal);
mp->localNormal1 = b3MulT(xf2.rotation, s_normal);
mp->localPoint1 = b3MulT(xf2, v2.position);
mp->localPoint2 = b3MulT(xf1, v1);
mp->triangleKey = B3_NULL_TRIANGLE;
mp->key = b3MakeKey(pair);
}
else
{
cp->triangleKey = B3_NULL_TRIANGLE;
cp->key = b3MakeKey(vB.pair);
cp->localNormal = b3MulT(xfA.rotation, normal);
cp->localPoint = b3MulT(xfA, vA);
cp->localPoint2 = b3MulT(xfB, vB.position);
manifold.center += 0.5f * (vA + B3_HULL_RADIUS * normal + vB.position - B3_HULL_RADIUS * normal);
mp->localNormal1 = b3MulT(xf1.rotation, normal);
mp->localPoint1 = b3MulT(xf1, v1);
mp->localPoint2 = b3MulT(xf2, v2.position);
mp->triangleKey = B3_NULL_TRIANGLE;
mp->key = b3MakeKey(v2.pair);
}
}
if (flipNormal)
{
localNormalA = -localNormalA;
normal = -normal;
}
manifold.center /= float32(pointCount);
manifold.normal = normal;
manifold.tangent1 = b3Perp(normal);
manifold.tangent2 = b3Cross(manifold.tangent1, normal);
manifold.pointCount = pointCount;
}
void b3CollideHulls(b3Manifold& manifold,
const b3Transform& xfA, const b3Hull* hullA,
const b3Transform& xfB, const b3Hull* hullB)
void b3CollideHulls(b3Manifold& manifold,
const b3Transform& xf1, const b3HullShape* s1,
const b3Transform& xf2, const b3HullShape* s2)
{
b3FaceQuery faceQueryA = b3QueryFaceSeparation(xfA, hullA, xfB, hullB);
if (faceQueryA.separation > B3_HULL_RADIUS_SUM)
const b3Hull* hull1 = s1->m_hull;
const b3Hull* hull2 = s2->m_hull;
float32 r1 = s1->m_radius;
float32 r2 = s2->m_radius;
float32 totalRadius = r1 + r2;
b3FaceQuery faceQuery1 = b3QueryFaceSeparation(xf1, hull1, xf2, hull2);
if (faceQuery1.separation > totalRadius)
{
return;
}
b3FaceQuery faceQueryB = b3QueryFaceSeparation(xfB, hullB, xfA, hullA);
if (faceQueryB.separation > B3_HULL_RADIUS_SUM)
b3FaceQuery faceQuery2 = b3QueryFaceSeparation(xf2, hull2, xf1, hull1);
if (faceQuery2.separation > totalRadius)
{
return;
}
b3EdgeQuery edgeQuery = b3QueryEdgeSeparation(xfA, hullA, xfB, hullB);
if (edgeQuery.separation > B3_HULL_RADIUS_SUM)
b3EdgeQuery edgeQuery = b3QueryEdgeSeparation(xf1, hull1, xf2, hull2);
if (edgeQuery.separation > totalRadius)
{
return;
}
const float32 kRelEdgeTol = 0.90f;
const float32 kRelFaceTol = 0.98f;
const float32 kAbsTol = 0.05f;
if (edgeQuery.separation > kRelEdgeTol * b3Max(faceQueryA.separation, faceQueryB.separation) + kAbsTol)
const float32 kTol = 0.1f * B3_LINEAR_SLOP;
if (edgeQuery.separation > b3Max(faceQuery1.separation, faceQuery2.separation) + kTol)
{
b3BuildEdgeContact(manifold, xfA, hullA, xfB, hullB, edgeQuery);
b3BuildEdgeContact(manifold, xf1, edgeQuery.index1, hull1, xf2, edgeQuery.index2, hull2);
}
else
{
if (faceQueryA.separation > kRelFaceTol * faceQueryB.separation + kAbsTol)
if (faceQuery1.separation + kTol > faceQuery2.separation)
{
b3BuildFaceContact(manifold, xfA, hullA, xfB, hullB, faceQueryA, false);
b3BuildFaceContact(manifold, xf1, r1, faceQuery1.index, hull1, xf2, r2, hull2, false);
}
else
{
b3BuildFaceContact(manifold, xfB, hullB, xfA, hullA, faceQueryB, true);
b3BuildFaceContact(manifold, xf2, r2, faceQuery2.index, hull2, xf1, r1, hull1, true);
}
}
}
//
bool b3_enableConvexCache = true;
bool b3_convexCache = true;
u32 b3_convexCalls = 0, b3_convexCacheHits = 0;
void b3CollideHullAndHull(b3Manifold& manifold,
const b3Transform& xfA, const b3HullShape* sA,
const b3Transform& xfB, const b3HullShape* sB,
void b3CollideHulls(b3Manifold& manifold,
const b3Transform& xf1, const b3HullShape* s1,
const b3Transform& xf2, const b3HullShape* s2,
b3FeatureCache* cache);
void b3CollideHullAndHull(b3Manifold& manifold,
const b3Transform& xfA, const b3HullShape* sA,
const b3Transform& xfB, const b3HullShape* sB,
void b3CollideHullAndHull(b3Manifold& manifold,
const b3Transform& xf1, const b3HullShape* s1,
const b3Transform& xf2, const b3HullShape* s2,
b3ConvexCache* cache)
{
++b3_convexCalls;
b3CollideHullAndHull(manifold, xfA, sA, xfB, sB, &cache->featureCache);
}
if (b3_convexCache)
{
b3CollideHulls(manifold, xf1, s1, xf2, s2, &cache->featureCache);
}
else
{
b3CollideHulls(manifold, xf1, s1, xf2, s2);
}
}

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

@ -23,95 +23,114 @@
#include <bounce/dynamics/body.h>
#include <bounce/collision/shapes/hull.h>
extern u32 b3_convexCacheHits;
void b3BuildEdgeContact(b3Manifold& manifold,
const b3Transform& xfA, const b3Hull* hullA,
const b3Transform& xfB, const b3Hull* hullB,
const b3EdgeQuery& query);
const b3Transform& xf1, u32 index1, const b3Hull* hull1,
const b3Transform& xf2, u32 index2, const b3Hull* hull2);
void b3BuildFaceContact(b3Manifold& manifold,
const b3Transform& xfA, const b3Hull* hullA,
const b3Transform& xfB, const b3Hull* hullB,
const b3FaceQuery& query, bool flipNormal);
const b3Transform& xf1, float32 r1, u32 index1, const b3Hull* hull1,
const b3Transform& xf2, float32 r2, const b3Hull* hull2,
bool flipNormal);
void b3RebuildEdgeContact(b3Manifold& manifold,
const b3Transform& xfA, u32 indexA, const b3HullShape* sA,
const b3Transform& xfB, u32 indexB, const b3HullShape* sB)
static void b3RebuildEdgeContact(b3Manifold& manifold,
const b3Transform& xf1, u32 index1, const b3HullShape* s1,
const b3Transform& xf2, u32 index2, const b3HullShape* s2)
{
const b3Hull* hullA = sA->m_hull;
const b3Hull* hullB = sB->m_hull;
const b3Hull* hull1 = s1->m_hull;
const b3Hull* hull2 = s2->m_hull;
const b3HalfEdge* edge1 = hull1->GetEdge(index1);
const b3HalfEdge* twin1 = hull1->GetEdge(index1 + 1);
const b3HalfEdge* edge1 = hullA->GetEdge(indexA);
const b3HalfEdge* twin1 = hullA->GetEdge(indexA + 1);
b3Vec3 C1 = b3Mul(xfA, hullA->centroid);
b3Vec3 P1 = b3Mul(xfA, hullA->GetVertex(edge1->origin));
b3Vec3 Q1 = b3Mul(xfA, hullA->GetVertex(twin1->origin));
b3Vec3 C1 = xf1 * hull1->centroid;
b3Vec3 P1 = xf1 * hull1->GetVertex(edge1->origin);
b3Vec3 Q1 = xf1 * hull1->GetVertex(twin1->origin);
b3Vec3 E1 = Q1 - P1;
b3Vec3 N1 = E1;
float32 L1 = N1.Normalize();
B3_ASSERT(L1 > 0.0f);
const b3HalfEdge* edge2 = hullB->GetEdge(indexB);
const b3HalfEdge* twin2 = hullB->GetEdge(indexB + 1);
const b3HalfEdge* edge2 = hull2->GetEdge(index2);
const b3HalfEdge* twin2 = hull2->GetEdge(index2 + 1);
b3Vec3 C2 = b3Mul(xfB, hullB->centroid);
b3Vec3 P2 = b3Mul(xfB, hullB->GetVertex(edge2->origin));
b3Vec3 Q2 = b3Mul(xfB, hullB->GetVertex(twin2->origin));
b3Vec3 C2 = xf2 * hull2->centroid;
b3Vec3 P2 = xf2 * hull2->GetVertex(edge2->origin);
b3Vec3 Q2 = xf2 * hull2->GetVertex(twin2->origin);
b3Vec3 E2 = Q2 - P2;
b3Vec3 N2 = E2;
float32 L2 = N2.Normalize();
B3_ASSERT(L2 > 0.0f);
b3Vec3 PA, PB;
b3ClosestPointsOnLines(&PA, &PB, P1, E1, P2, E2);
// Compute the closest points on the two lines.
float32 b = b3Dot(N1, N2);
float32 den = 1.0f - b * b;
if (den <= 0.0f)
{
return;
}
float32 inv_den = 1.0f / den;
b3Vec3 E3 = P1 - P2;
float32 d = b3Dot(N1, E3);
float32 e = b3Dot(N2, E3);
float32 s = inv_den * (b * e - d);
float32 t = inv_den * (e - b * d);
b3Vec3 c1 = P1 + s * N1;
b3Vec3 c2 = P2 + t * N2;
// Check if the closest points are still lying on the opposite segments
// using Barycentric coordinates.
float32 wB[3];
b3Barycentric(wB, P1, Q1, PB);
float32 w2[3];
b3Barycentric(w2, P1, Q1, c2);
float32 wA[3];
b3Barycentric(wA, P2, Q2, PA);
float32 w1[3];
b3Barycentric(w1, P2, Q2, c1);
if (wB[1] > 0.0f && wB[1] <= wB[2] &&
wA[1] > 0.0f && wA[1] <= wA[2])
if (w2[1] > 0.0f && w2[1] <= w2[2] &&
w1[1] > 0.0f && w1[1] <= w1[2])
{
b3Vec3 N = b3Cross(E1, E2);
float32 LN = N.Normalize();
B3_ASSERT(LN > 0.0f);
if (b3Dot(N, P1 - C1) < 0.0f)
{
N = -N;
}
N.Normalize();
b3FeaturePair pair = b3MakePair(indexA, indexA + 1, indexB, indexB + 1);
b3FeaturePair pair = b3MakePair(index1, index1 + 1, index2, index2 + 1);
manifold.pointCount = 1;
manifold.points[0].localNormal1 = b3MulT(xf1.rotation, N);
manifold.points[0].localPoint1 = b3MulT(xf1, c1);
manifold.points[0].localPoint2 = b3MulT(xf2, c2);
manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
manifold.points[0].key = b3MakeKey(pair);
manifold.points[0].localNormal = b3MulT(xfA.rotation, N);
manifold.points[0].localPoint = b3MulT(xfA, PA);
manifold.points[0].localPoint2 = b3MulT(xfB, PB);
manifold.center = 0.5f * (PA + B3_HULL_RADIUS * N + PB - B3_HULL_RADIUS * N);
manifold.normal = N;
manifold.tangent1 = b3Perp(N);
manifold.tangent2 = b3Cross(manifold.tangent1, N);
}
}
void b3RebuildFaceContact(b3Manifold& manifold,
const b3Transform& xfA, u32 indexA, const b3HullShape* sA,
const b3Transform& xfB, const b3HullShape* sB, bool flipNormal)
static void b3RebuildFaceContact(b3Manifold& manifold,
const b3Transform& xf1, u32 index1, const b3HullShape* s1,
const b3Transform& xf2, const b3HullShape* s2, bool flipNormal)
{
const b3Hull* hullA = sA->m_hull;
const b3Hull* hullB = sB->m_hull;
const b3Hull* hull1 = s1->m_hull;
float32 r1 = s1->m_radius;
const b3Body* body1 = s1->GetBody();
const b3Body* bodyA = sA->GetBody();
const b3Body* bodyB = sB->GetBody();
const b3Hull* hull2 = s2->m_hull;
float32 r2 = s2->m_radius;
const b3Body* body2 = s2->GetBody();
const b3Sweep& sweepA = bodyA->GetSweep();
b3Quat q10 = sweepA.orientation0;
b3Quat q1 = sweepA.orientation;
const b3Sweep& sweep1 = body1->GetSweep();
b3Quat q10 = sweep1.orientation0;
b3Quat q1 = sweep1.orientation;
const b3Sweep& sweepB = bodyB->GetSweep();
b3Quat q20 = sweepB.orientation0;
b3Quat q2 = sweepB.orientation;
const b3Sweep& sweep2 = body2->GetSweep();
b3Quat q20 = sweep2.orientation0;
b3Quat q2 = sweep2.orientation;
// Check if the relative orientation has changed.
// Here the second orientation seen by the first orientation.
@ -136,126 +155,126 @@ void b3RebuildFaceContact(b3Manifold& manifold,
const float32 kTol = 0.995f;
if (b3Abs(q.w) > kTol)
{
b3FaceQuery query;
query.index = indexA;
b3BuildFaceContact(manifold, xfA, hullA, xfB, hullB, query, flipNormal);
b3BuildFaceContact(manifold, xf1, r1, index1, hull1, xf2, r2, hull2, flipNormal);
}
}
void b3CollideCache(b3Manifold& manifold,
const b3Transform& xfA, const b3HullShape* sA,
const b3Transform& xfB, const b3HullShape* sB,
const b3Transform& xf1, const b3HullShape* s1,
const b3Transform& xf2, const b3HullShape* s2,
b3FeatureCache* cache)
{
B3_ASSERT(cache->m_featurePair.state == b3SATCacheType::e_empty);
const b3Hull* hullA = sA->m_hull;
const b3Hull* hullB = sB->m_hull;
const b3Hull* hull1 = s1->m_hull;
const b3Hull* hull2 = s2->m_hull;
b3FaceQuery faceQueryA = b3QueryFaceSeparation(xfA, hullA, xfB, hullB);
if (faceQueryA.separation > B3_HULL_RADIUS_SUM)
float32 r1 = s1->m_radius;
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_faceA;
cache->m_featurePair.indexA = faceQueryA.index;
cache->m_featurePair.indexB = faceQueryA.index;
cache->m_featurePair.type = b3SATFeaturePair::e_face1;
cache->m_featurePair.index1 = faceQuery1.index;
cache->m_featurePair.index2 = faceQuery1.index;
return;
}
b3FaceQuery faceQueryB = b3QueryFaceSeparation(xfB, hullB, xfA, hullA);
if (faceQueryB.separation > B3_HULL_RADIUS_SUM)
b3FaceQuery faceQuery2 = b3QueryFaceSeparation(xf2, hull2, xf1, hull1);
if (faceQuery2.separation > totalRadius)
{
// Write a separation cache.
cache->m_featurePair.state = b3SATCacheType::e_separation;
cache->m_featurePair.type = b3SATFeaturePair::e_faceB;
cache->m_featurePair.indexA = faceQueryB.index;
cache->m_featurePair.indexB = faceQueryB.index;
cache->m_featurePair.type = b3SATFeaturePair::e_face2;
cache->m_featurePair.index1 = faceQuery2.index;
cache->m_featurePair.index2 = faceQuery2.index;
return;
}
b3EdgeQuery edgeQuery = b3QueryEdgeSeparation(xfA, hullA, xfB, hullB);
if (edgeQuery.separation > B3_HULL_RADIUS_SUM)
b3EdgeQuery edgeQuery = b3QueryEdgeSeparation(xf1, hull1, xf2, hull2);
if (edgeQuery.separation > totalRadius)
{
// Write a separation cache.
cache->m_featurePair.state = b3SATCacheType::e_separation;
cache->m_featurePair.type = b3SATFeaturePair::e_edgeA;
cache->m_featurePair.indexA = edgeQuery.indexA;
cache->m_featurePair.indexB = edgeQuery.indexB;
cache->m_featurePair.type = b3SATFeaturePair::e_edge1;
cache->m_featurePair.index1 = edgeQuery.index1;
cache->m_featurePair.index2 = edgeQuery.index2;
return;
}
const float32 kRelEdgeTol = 0.90f;
const float32 kRelFaceTol = 0.98f;
const float32 kAbsTol = 0.05f;
const float32 kTol = 0.1f * B3_LINEAR_SLOP;
if (edgeQuery.separation > kRelEdgeTol * b3Max(faceQueryA.separation, faceQueryB.separation) + kAbsTol)
if (edgeQuery.separation > b3Max(faceQuery1.separation, faceQuery2.separation) + kTol)
{
b3BuildEdgeContact(manifold, xfA, hullA, xfB, hullB, edgeQuery);
b3BuildEdgeContact(manifold, xf1, edgeQuery.index1, hull1, xf2, edgeQuery.index2, hull2);
if(manifold.pointCount > 0)
{
// Write an overlap cache.
cache->m_featurePair.state = b3SATCacheType::e_overlap;
cache->m_featurePair.type = b3SATFeaturePair::e_edgeA;
cache->m_featurePair.indexA = edgeQuery.indexA;
cache->m_featurePair.indexB = edgeQuery.indexB;
cache->m_featurePair.type = b3SATFeaturePair::e_edge1;
cache->m_featurePair.index1 = edgeQuery.index1;
cache->m_featurePair.index2 = edgeQuery.index2;
}
}
else
{
if (faceQueryA.separation > kRelFaceTol * faceQueryB.separation + kAbsTol)
if (faceQuery1.separation + kTol > faceQuery2.separation)
{
b3BuildFaceContact(manifold, xfA, hullA, xfB, hullB, faceQueryA, false);
b3BuildFaceContact(manifold, xf1, r1, faceQuery1.index, hull1, xf2, r2, hull2, false);
if(manifold.pointCount > 0)
{
// Write an overlap cache.
cache->m_featurePair.state = b3SATCacheType::e_overlap;
cache->m_featurePair.type = b3SATFeaturePair::e_faceA;
cache->m_featurePair.indexA = faceQueryA.index;
cache->m_featurePair.indexB = faceQueryA.index;
cache->m_featurePair.type = b3SATFeaturePair::e_face1;
cache->m_featurePair.index1 = faceQuery1.index;
cache->m_featurePair.index2 = faceQuery1.index;
}
}
else
{
b3BuildFaceContact(manifold, xfB, hullB, xfA, hullA, faceQueryB, true);
b3BuildFaceContact(manifold, xf2, r2, faceQuery2.index, hull2, xf1, r1, hull1, true);
if(manifold.pointCount > 0)
{
// Write an overlap cache.
cache->m_featurePair.state = b3SATCacheType::e_overlap;
cache->m_featurePair.type = b3SATFeaturePair::e_faceB;
cache->m_featurePair.indexA = faceQueryB.index;
cache->m_featurePair.indexB = faceQueryB.index;
cache->m_featurePair.type = b3SATFeaturePair::e_face2;
cache->m_featurePair.index1 = faceQuery2.index;
cache->m_featurePair.index2 = faceQuery2.index;
}
}
}
}
b3SATCacheType b3FeatureCache::ReadState(
const b3Transform& xfA, const b3Hull* hullA,
const b3Transform& xfB, const b3Hull* hullB)
const b3Transform& xf1, float32 r1, const b3Hull* hull1,
const b3Transform& xf2, float32 r2, const b3Hull* hull2)
{
// 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_faceA;
m_featurePair.indexA = 0;
m_featurePair.indexB = 0;
m_featurePair.type = b3SATFeaturePair::e_face1;
m_featurePair.index1 = 0;
m_featurePair.index2 = 0;
}
switch (m_featurePair.type)
{
case b3SATFeaturePair::e_edgeA:
case b3SATFeaturePair::e_edge1:
{
return ReadEdge(xfA, hullA, xfB, hullB);
return ReadEdge(xf1, r1, hull1, xf2, r2, hull2);
}
case b3SATFeaturePair::e_faceA:
case b3SATFeaturePair::e_face1:
{
return ReadFace(xfA, hullA, xfB, hullB);
return ReadFace(xf1, r1, hull1, xf2, r2, hull2);
}
case b3SATFeaturePair::e_faceB:
case b3SATFeaturePair::e_face2:
{
return ReadFace(xfB, hullB, xfA, hullA);
return ReadFace(xf2, r2, hull2, xf1, r1, hull1);
}
default:
{
@ -265,14 +284,14 @@ b3SATCacheType b3FeatureCache::ReadState(
}
b3SATCacheType b3FeatureCache::ReadFace(
const b3Transform& xfA, const b3Hull* hullA,
const b3Transform& xfB, const b3Hull* hullB)
const b3Transform& xf1, float32 r1, const b3Hull* hull1,
const b3Transform& xf2, float32 r2, const b3Hull* hull2)
{
// Perform computations in the local space of the second hull.
b3Transform xf = b3MulT(xfB, xfA);
b3Plane plane = xf * hullA->GetPlane(m_featurePair.indexA);
float32 separation = b3Project(hullB, plane);
if (separation > B3_HULL_RADIUS_SUM)
b3Transform xf = b3MulT(xf2, xf1);
b3Plane plane = xf * hull1->GetPlane(m_featurePair.index1);
float32 separation = b3Project(hull2, plane);
if (separation > r1 + r2)
{
return e_separation;
}
@ -280,48 +299,48 @@ b3SATCacheType b3FeatureCache::ReadFace(
}
b3SATCacheType b3FeatureCache::ReadEdge(
const b3Transform& xfA, const b3Hull* hullA,
const b3Transform& xfB, const b3Hull* hullB)
const b3Transform& xf1, float32 r1, const b3Hull* hull1,
const b3Transform& xf2, float32 r2, const b3Hull* hull2)
{
u32 i = m_featurePair.indexA;
u32 j = m_featurePair.indexB;
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(xfB, xfA);
b3Vec3 C1 = xf * hullA->centroid;
b3Transform xf = b3MulT(xf2, xf1);
b3Vec3 C1 = xf * hull1->centroid;
const b3HalfEdge* edge1 = hullA->GetEdge(i);
const b3HalfEdge* twin1 = hullA->GetEdge(i + 1);
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 * hullA->GetVertex(edge1->origin);
b3Vec3 Q1 = xf * hullA->GetVertex(twin1->origin);
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 * hullA->GetPlane(edge1->face).normal;
b3Vec3 V1 = xf.rotation * hullA->GetPlane(twin1->face).normal;
b3Vec3 U1 = xf.rotation * hull1->GetPlane(edge1->face).normal;
b3Vec3 V1 = xf.rotation * hull1->GetPlane(twin1->face).normal;
const b3HalfEdge* edge2 = hullB->GetEdge(j);
const b3HalfEdge* twin2 = hullB->GetEdge(j + 1);
const b3HalfEdge* edge2 = hull2->GetEdge(j);
const b3HalfEdge* twin2 = hull2->GetEdge(j + 1);
B3_ASSERT(edge2->twin == j + 1 && twin2->twin == j);
b3Vec3 P2 = hullB->GetVertex(edge2->origin);
b3Vec3 Q2 = hullB->GetVertex(twin2->origin);
b3Vec3 P2 = hull2->GetVertex(edge2->origin);
b3Vec3 Q2 = hull2->GetVertex(twin2->origin);
b3Vec3 E2 = Q2 - P2;
// The Gauss Map of edge 2.
b3Vec3 U2 = hullB->GetPlane(edge2->face).normal;
b3Vec3 V2 = hullB->GetPlane(twin2->face).normal;
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 > B3_HULL_RADIUS_SUM)
if (separation > r1 + r2)
{
return b3SATCacheType::e_separation;
}
@ -336,17 +355,22 @@ b3SATCacheType b3FeatureCache::ReadEdge(
return b3SATCacheType::e_empty;
}
void b3CollideHullAndHull(b3Manifold& manifold,
const b3Transform& xfA, const b3HullShape* sA,
const b3Transform& xfB, const b3HullShape* sB,
extern u32 b3_convexCacheHits;
void b3CollideHulls(b3Manifold& manifold,
const b3Transform& xf1, const b3HullShape* s1,
const b3Transform& xf2, const b3HullShape* s2,
b3FeatureCache* cache)
{
const b3Hull* hullA = sA->m_hull;
const b3Hull* hullB = sB->m_hull;
const b3Hull* hull1 = s1->m_hull;
float32 r1 = s1->m_radius;
const b3Hull* hull2 = s2->m_hull;
float32 r2 = s2->m_radius;
// Read cache
b3SATCacheType state0 = cache->m_featurePair.state;
b3SATCacheType state1 = cache->ReadState(xfA, hullA, xfB, hullB);
b3SATCacheType state1 = cache->ReadState(xf1, r1, hull1, xf2, r2, hull2);
if (state0 == b3SATCacheType::e_separation &&
state1 == b3SATCacheType::e_separation)
@ -361,19 +385,19 @@ void b3CollideHullAndHull(b3Manifold& manifold,
// Try to rebuild or reclip the features.
switch (cache->m_featurePair.type)
{
case b3SATFeaturePair::e_edgeA:
case b3SATFeaturePair::e_edge1:
{
b3RebuildEdgeContact(manifold, xfA, cache->m_featurePair.indexA, sA, xfB, cache->m_featurePair.indexB, sB);
b3RebuildEdgeContact(manifold, xf1, cache->m_featurePair.index1, s1, xf2, cache->m_featurePair.index2, s2);
break;
}
case b3SATFeaturePair::e_faceA:
case b3SATFeaturePair::e_face1:
{
b3RebuildFaceContact(manifold, xfA, cache->m_featurePair.indexA, sA, xfB, sB, false);
b3RebuildFaceContact(manifold, xf1, cache->m_featurePair.index1, s1, xf2, s2, false);
break;
}
case b3SATFeaturePair::e_faceB:
case b3SATFeaturePair::e_face2:
{
b3RebuildFaceContact(manifold, xfB, cache->m_featurePair.indexA, sB, xfA, sA, true);
b3RebuildFaceContact(manifold, xf2, cache->m_featurePair.index1, s2, xf1, s1, true);
break;
}
default:
@ -394,5 +418,5 @@ void b3CollideHullAndHull(b3Manifold& manifold,
// Overlap cache miss.
// Flush the cache.
cache->m_featurePair.state = b3SATCacheType::e_empty;
b3CollideCache(manifold, xfA, sA, xfB, sB, cache);
}
b3CollideCache(manifold, xf1, s1, xf2, s2, cache);
}

48
src/bounce/dynamics/contacts/collide/collide_sphere_capsule.cpp
View File

@ -20,24 +20,22 @@
#include <bounce/dynamics/contacts/manifold.h>
#include <bounce/dynamics/shapes/sphere_shape.h>
#include <bounce/dynamics/shapes/capsule_shape.h>
#include <bounce/collision/shapes/sphere.h>
#include <bounce/collision/shapes/capsule.h>
void b3CollideSphereAndCapsule(b3Manifold& manifold,
const b3Transform& xfA, const b3SphereShape* sA,
const b3Transform& xfB, const b3CapsuleShape* sB)
void b3CollideSphereAndCapsule(b3Manifold& manifold,
const b3Transform& xf1, const b3SphereShape* s1,
const b3Transform& xf2, const b3CapsuleShape* s2)
{
b3Vec3 Q = b3Mul(xfA, sA->m_center);
b3Vec3 Q = b3Mul(xf1, s1->m_center);
b3Vec3 A = b3Mul(xfB, sB->m_centers[0]);
b3Vec3 B = b3Mul(xfB, sB->m_centers[1]);
b3Vec3 A = b3Mul(xf2, s2->m_centers[0]);
b3Vec3 B = b3Mul(xf2, s2->m_centers[1]);
b3Vec3 AB = B - A;
// Barycentric coordinates for Q
float32 u = b3Dot(B - Q, AB);
float32 v = b3Dot(Q - A, AB);
float32 radius = sA->m_radius + sB->m_radius;
float32 radius = s1->m_radius + s2->m_radius;
if (v <= 0.0f)
{
@ -58,16 +56,12 @@ void b3CollideSphereAndCapsule(b3Manifold& manifold,
}
manifold.pointCount = 1;
manifold.points[0].localNormal1 = b3MulT(xf1.rotation, n);
manifold.points[0].localPoint1 = s1->m_center;
manifold.points[0].localPoint2 = s2->m_centers[0];
manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
manifold.points[0].key = 0;
manifold.points[0].localNormal = b3MulT(xfA.rotation, n);
manifold.points[0].localPoint = sA->m_center;
manifold.points[0].localPoint2 = sB->m_centers[0];
manifold.center = 0.5f * (P + sA->m_radius * n + Q - sB->m_radius * n);
manifold.normal = n;
manifold.tangent1 = b3Perp(n);
manifold.tangent2 = b3Cross(manifold.tangent1, n);
return;
}
@ -90,21 +84,16 @@ void b3CollideSphereAndCapsule(b3Manifold& manifold,
}
manifold.pointCount = 1;
manifold.points[0].localNormal1 = b3MulT(xf1.rotation, n);
manifold.points[0].localPoint1 = s1->m_center;
manifold.points[0].localPoint2 = s2->m_centers[1];
manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
manifold.points[0].key = 0;
manifold.points[0].localNormal = b3MulT(xfA.rotation, n);
manifold.points[0].localPoint = sA->m_center;
manifold.points[0].localPoint2 = sB->m_centers[1];
manifold.center = 0.5f * (P + sA->m_radius * n + Q - sB->m_radius * n);
manifold.normal = n;
manifold.tangent1 = b3Perp(n);
manifold.tangent2 = b3Cross(manifold.tangent1, n);
return;
}
// AB
//float32 s = u + v;
float32 s = b3Dot(AB, AB);
//B3_ASSERT(s > 0.0f);
b3Vec3 P;
@ -134,14 +123,9 @@ void b3CollideSphereAndCapsule(b3Manifold& manifold,
n.Normalize();
manifold.pointCount = 1;
manifold.points[0].localNormal1 = b3MulT(xf1.rotation, n);
manifold.points[0].localPoint1 = s1->m_center;
manifold.points[0].localPoint2 = b3MulT(xf2, P);
manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
manifold.points[0].key = 2;
manifold.points[0].localNormal = b3MulT(xfA.rotation, n);
manifold.points[0].localPoint = sA->m_center;
manifold.points[0].localPoint2 = b3MulT(xfB, P);
manifold.center = 0.5f * (P + sA->m_radius * n + Q - sB->m_radius * n);
manifold.normal = n;
manifold.tangent1 = b3Perp(n);
manifold.tangent2 = b3Cross(manifold.tangent1, n);
}

64
src/bounce/dynamics/contacts/collide/collide_sphere_hull.cpp
View File

@ -23,69 +23,63 @@
#include <bounce/collision/shapes/sphere.h>
#include <bounce/collision/shapes/hull.h>
void b3CollideSphereAndHull(b3Manifold& manifold,
void b3CollideSphereAndHull(b3Manifold& manifold,
const b3Transform& xf1, const b3SphereShape* s1,
const b3Transform& xf2, const b3HullShape* s2)
{
b3ShapeGJKProxy proxy1(s1, 0);
b3ShapeGJKProxy proxy2(s2, 0);
b3GJKOutput distance = b3GJK(xf1, proxy1, xf2, proxy2);
float32 totalRadius = s1->m_radius + s2->m_radius;
if (distance.distance > totalRadius)
b3GJKOutput gjk = b3GJK(xf1, proxy1, xf2, proxy2);
float32 r1 = s1->m_radius;
float32 r2 = s2->m_radius;
float32 totalRadius = r1 + r2;
if (gjk.distance > totalRadius)
{
return;
}
if (distance.distance > 0.0f)
if (gjk.distance > 0.0f)
{
b3Vec3 p1 = distance.pointA;
b3Vec3 p2 = distance.pointB;
b3Vec3 normal = (p2 - p1) / distance.distance;
b3Vec3 c1 = gjk.point1;
b3Vec3 c2 = gjk.point2;
b3Vec3 normal = (c2 - c1) / gjk.distance;
manifold.pointCount = 1;
manifold.points[0].localNormal1 = b3MulT(xf1.rotation, normal);
manifold.points[0].localPoint1 = s1->m_center;
manifold.points[0].localPoint2 = b3MulT(xf2, c2);
manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
manifold.points[0].key = 0;
manifold.points[0].localNormal = b3MulT(xf1.rotation, normal);
manifold.points[0].localPoint = s1->m_center;
manifold.points[0].localPoint2 = b3MulT(xf2, p2);
manifold.center = 0.5f * (p1 + s1->m_radius * normal + p2 - s2->m_radius * normal);
manifold.normal = normal;
manifold.tangent1 = b3Perp(normal);
manifold.tangent2 = b3Cross(manifold.tangent1, normal);
return;
}
b3Sphere hull1;
hull1.vertex = s1->m_center;
hull1.radius = s1->m_radius;
const b3Vec3& hull1 = s1->m_center;
const b3Hull* hull2 = s2->m_hull;
b3FaceQuery faceQuery = b3QueryFaceSeparation(xf1, &hull1, xf2, hull2);
b3FaceQuery faceQuery = b3QueryFaceSeparation(xf1, hull1, xf2, hull2);
if (faceQuery.separation > totalRadius)
{
return;
}
b3Plane localPlane = hull2->GetPlane(faceQuery.index);
b3Plane plane = b3Mul(xf2, localPlane);
b3Vec3 cp1 = b3Mul(xf1, hull1.vertex);
b3Vec3 cp2 = b3ClosestPointOnPlane(cp1, plane);
b3Plane localPlane2 = hull2->planes[faceQuery.index];
b3Plane plane2 = xf2 * localPlane2;
// Ensure normal orientation to shape B
b3Vec3 normal = -plane.normal;
b3Vec3 c1 = xf1 * hull1;
b3Vec3 c2 = b3ClosestPointOnPlane(c1, plane2);
// Ensure normal orientation to shape 2
b3Vec3 n1 = -plane2.normal;
manifold.pointCount = 1;
manifold.points[0].localNormal1 = b3MulT(xf1.rotation, n1);
manifold.points[0].localPoint1 = s1->m_center;
manifold.points[0].localPoint2 = b3MulT(xf2, c2);
manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
manifold.points[0].key = 1;
manifold.points[0].localNormal = b3MulT(xf1.rotation, normal);
manifold.points[0].localPoint = s1->m_center;
manifold.points[0].localPoint2 = b3MulT(xf2, cp2);
manifold.center = 0.5f * (cp1 + s1->m_radius * normal + cp2 - B3_HULL_RADIUS * normal);
manifold.normal = normal;
manifold.tangent1 = b3Perp(normal);
manifold.tangent2 = b3Cross(manifold.tangent1, normal);
}

391
src/bounce/dynamics/contacts/collide/collide_sphere_mesh.cpp
View File

@ -1,391 +0,0 @@
/*
* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be RemoveTriangled or altered from any source distribution.
*/
#include <bounce/dynamics/contacts/mesh_contact.h>
#include <bounce/dynamics/shapes/sphere_shape.h>
#include <bounce/dynamics/shapes/mesh_shape.h>
#include <bounce/dynamics/contacts/contact_cluster.h>
#include <bounce/dynamics/world.h>
#include <bounce/dynamics/body.h>
#include <bounce/collision/shapes/mesh.h>
#include <bounce/collision/gjk/gjk_cache.h>
#include <bounce/common/memory/stack_allocator.h>
// Implementation based on Peter Tchernev's relevant quote
// in the Bullet forum.
// The mesh must contain unique vertices for this collision
// filtering algorithm to work correctly.
// Otherwise we'll need to switch to comparisons
// using the binary representation of each vertex coordinate,
// which will be slight slower.
// Therefore it is recommended for your resource compiler
// to enforce this restriction on each physics mesh.
struct b3FeaturePoint
{
u32 index;
b3GJKOutput output;
b3GJKFeaturePair pair;
};
// This is used for fast sorting.
struct b3SortKey
{
bool operator<(const b3SortKey& b) const
{
return distance < b.distance;
}
u32 index; // original index to contact point
float32 distance; // distance between closest points
};
struct b3SMCollider
{
b3SMCollider(b3StackAllocator* allocator,
b3TriangleCache* triangles, u32 triangleCount,
const b3Transform& xfA, const b3SphereShape* sA,
const b3Transform& xfB, const b3MeshShape* sB);
~b3SMCollider();
u32 Collide(b3Manifold manifolds[3]);
void CollideTriangle(u32 i);
void Filter();
bool ShouldCollide(u32 v) const;
bool ShouldCollide(u32 v1, u32 v2) const;
bool ShouldCollide(const b3FeaturePoint* p) const;
void AddPoint(u32 triangle, const b3GJKOutput& output, const b3GJKFeaturePair& pair);
void AddDelayedPoint(u32 triangle, const b3GJKOutput& output, const b3GJKFeaturePair& pair);
void AddValidPoint(u32 triangle, const b3GJKOutput& output, const b3GJKFeaturePair& pair);
void RemoveTriangle(const b3Triangle* triangle);
b3StackAllocator* m_alloc;
b3ShapeGJKProxy m_proxyA;
b3Vec3 m_localVertexA;
b3Vec3 m_vertexA;
float32 m_rA;
b3Transform m_xfA;
b3ShapeGJKProxy m_proxyB;
const b3MeshShape* m_meshB;
float32 m_rB;
b3Transform m_xfB;
float32 m_totalRadius;
b3TriangleCache* m_triangles;
u32 m_triangleCount;
b3Manifold* m_manifolds;
u32 m_manifoldCount;
// Triangles that cannot collide anymore.
b3Triangle* m_removeds;
u32 m_removedCount;
// Vertex or edge contact points that might collide.
b3FeaturePoint* m_delayeds;
b3SortKey* m_keys;
u32 m_delayedCount;
};
b3SMCollider::b3SMCollider(b3StackAllocator* allocator,
b3TriangleCache* triangles, u32 triangleCount,
const b3Transform& xfA, const b3SphereShape* sA,
const b3Transform& xfB, const b3MeshShape* sB)
{
m_alloc = allocator;
m_triangles = triangles;
m_triangleCount = triangleCount;
m_xfA = xfA;
m_xfB = xfB;
m_proxyA.Set(sA, 0);
m_localVertexA = sA->m_center;
m_vertexA = b3Mul(m_xfA, m_localVertexA);
m_rA = sA->m_radius;
m_meshB = sB;
m_rB = sB->m_radius;
m_totalRadius = m_rA + m_rB;
m_delayeds = (b3FeaturePoint*)m_alloc->Allocate(m_triangleCount * sizeof(b3FeaturePoint));
m_keys = (b3SortKey*)m_alloc->Allocate(m_triangleCount * sizeof(b3SortKey));
m_delayedCount = 0;
m_removeds = (b3Triangle*)m_alloc->Allocate(m_triangleCount * sizeof(b3Triangle));
m_removedCount = 0;
m_manifolds = (b3Manifold*)m_alloc->Allocate(m_triangleCount * sizeof(b3Manifold));
m_manifoldCount = 0;
}
b3SMCollider::~b3SMCollider()
{
m_alloc->Free(m_manifolds);
m_alloc->Free(m_removeds);
m_alloc->Free(m_keys);
m_alloc->Free(m_delayeds);
}
u32 b3SMCollider::Collide(b3Manifold manifolds[3])
{
for (u32 i = 0; i < m_triangleCount; ++i)
{
CollideTriangle(i);
}
Filter();
u32 numOut = b3Clusterize(manifolds, m_manifolds, m_manifoldCount, m_xfA, m_rA, m_xfB, m_rB);
return numOut;
}
void b3SMCollider::CollideTriangle(u32 i)
{
b3TriangleCache* t = m_triangles + i;
b3ShapeGJKProxy proxyB(m_meshB, t->index);
b3GJKOutput output = b3GJK(m_xfA, m_proxyA, m_xfB, proxyB, false, &t->cache.simplexCache);
if (output.distance > m_totalRadius)
{
return;
}
if (output.distance > B3_EPSILON)
{
b3GJKFeaturePair pair = b3GetFeaturePair(t->cache.simplexCache);
AddPoint(i, output, pair);
return;
}
b3Triangle* triangle = m_meshB->m_mesh->triangles + t->index;
b3Vec3 A = b3Mul(m_xfB, m_meshB->m_mesh->vertices[triangle->v1]);
b3Vec3 B = b3Mul(m_xfB, m_meshB->m_mesh->vertices[triangle->v2]);
b3Vec3 C = b3Mul(m_xfB, m_meshB->m_mesh->vertices[triangle->v3]);
b3Plane plane(A, B, C);
float32 separation = b3Distance(m_vertexA, plane);
float32 sign = b3Sign(separation);
if (sign * separation > m_totalRadius)
{
return;
}
b3Vec3 normal = sign * plane.normal;
B3_ASSERT(m_manifoldCount < m_triangleCount);
b3Manifold* m = m_manifolds + m_manifoldCount;
m->GuessImpulses();
++m_manifoldCount;
b3Vec3 p1 = m_vertexA;
b3Vec3 p2 = b3ClosestPointOnPlane(p1, plane);
// Ensure normal orientation to shape B.
normal = -normal;
m->pointCount = 1;
m->points[0].triangleKey = t->index;
m->points[0].key = 0;
m->points[0].localNormal = b3MulT(m_xfA.rotation, normal);
m->points[0].localPoint = m_localVertexA;
m->points[0].localPoint2 = p2;
m->center = 0.5f * (p1 + m_rA * normal + p2 - m_rB * normal);
m->normal = normal;
m->tangent1 = b3Perp(normal);
m->tangent2 = b3Cross(m->tangent1, normal);
RemoveTriangle(triangle);
}
void b3SMCollider::Filter()
{
// Sort contact points according to distance to triangles.
std::sort(m_keys, m_keys + m_delayedCount);
for (u32 i = 0; i < m_delayedCount; ++i)
{
const b3SortKey* key = m_keys + i;
const b3FeaturePoint* p = m_delayeds + key->index;
const b3TriangleCache* tc = m_triangles + p->index;
const b3Triangle* t = m_meshB->m_mesh->triangles + tc->index;
bool ok = ShouldCollide(p);
if (ok)
{
AddValidPoint(p->index, p->output, p->pair);
}
// Now this triangle cannot collide anymore.
RemoveTriangle(t);
}
}
void b3SMCollider::RemoveTriangle(const b3Triangle* triangle)
{
B3_ASSERT(m_removedCount < m_triangleCount);
m_removeds[m_removedCount] = *triangle;
++m_removedCount;
}
bool b3SMCollider::ShouldCollide(u32 v) const
{
for (u32 i = 0; i < m_removedCount; ++i)
{
if (m_removeds[i].TestVertex(v))
{
return false;
}
}
return true;
}
bool b3SMCollider::ShouldCollide(u32 v1, u32 v2) const
{
for (u32 i = 0; i < m_removedCount; ++i)
{
if (m_removeds[i].TestEdge(v1, v2))
{
return false;
}
}
return true;
}
bool b3SMCollider::ShouldCollide(const b3FeaturePoint* p) const
{
b3TriangleCache* tc = m_triangles + p->index;
b3Triangle* t = m_meshB->m_mesh->triangles + tc->index;
u32 is[3] = { t->v1, t->v2, t->v3 };
switch (p->pair.countB)
{
case 1:
{
u32 v1 = is[p->pair.indexB[0]];
return ShouldCollide(v1);
}
case 2:
{
u32 v1 = is[p->pair.indexB[0]];
u32 v2 = is[p->pair.indexB[1]];
return ShouldCollide(v1, v2);
}
case 3:
{
return true;
}
default:
{
B3_ASSERT(false);
return true;
}
}
return false;
}
void b3SMCollider::AddPoint(u32 i, const b3GJKOutput& output, const b3GJKFeaturePair& pair)
{
if (pair.countB == 3)
{
b3TriangleCache* tc = m_triangles + i;
b3Triangle* t = m_meshB->m_mesh->triangles + tc->index;
AddValidPoint(i, output, pair);
RemoveTriangle(t);
}
else
{
AddDelayedPoint(i, output, pair);
}
}
void b3SMCollider::AddDelayedPoint(u32 i, const b3GJKOutput& output, const b3GJKFeaturePair& pair)
{
b3TriangleCache* tc = m_triangles + i;
b3Triangle* t = m_meshB->m_mesh->triangles + tc->index;
B3_ASSERT(m_delayedCount < m_triangleCount);
b3FeaturePoint* p = m_delayeds + m_delayedCount;
b3SortKey* k = m_keys + m_delayedCount;
p->index = i;
p->output = output;
p->pair = pair;
k->index = m_delayedCount;
k->distance = output.distance;
++m_delayedCount;
}
void b3SMCollider::AddValidPoint(u32 i, const b3GJKOutput& output, const b3GJKFeaturePair& pair)
{
b3TriangleCache* tc = m_triangles + i;
b3Triangle* t = m_meshB->m_mesh->triangles + tc->index;
B3_ASSERT(m_manifoldCount < m_triangleCount);
b3Manifold* m = m_manifolds + m_manifoldCount;
++m_manifoldCount;
m->GuessImpulses();
b3Vec3 p1 = output.pointA;
b3Vec3 p2 = output.pointB;
b3Vec3 n = (p2 - p1) / output.distance;
m->pointCount = 1;
m->points[0].triangleKey = tc->index;
m->points[0].key = 0;
m->points[0].localNormal = b3MulT(m_xfA.rotation, n);
m->points[0].localPoint = m_localVertexA;
m->points[0].localPoint2 = b3MulT(m_xfB, p2);
m->center = 0.5f * (p1 + m_rA * n + p2 - m_rB * n);
m->normal = n;
m->tangent1 = b3Perp(n);
m->tangent2 = b3Cross(m->tangent1, n);
}
void b3MeshContact::CollideSphere()
{
b3SphereShape* sA = (b3SphereShape*)GetShapeA();
b3Body* bodyA = sA->GetBody();
b3Transform xfA = bodyA->GetTransform();
b3MeshShape* sB = (b3MeshShape*)GetShapeB();
b3Body* bodyB = sB->GetBody();
b3Transform xfB = bodyB->GetTransform();
b3World* world = bodyA->GetWorld();
b3StackAllocator* allocator = &world->m_stackAllocator;
b3SMCollider collider(allocator, m_triangles, m_triangleCount, xfA, sA, xfB, sB);
m_manifoldCount = collider.Collide(m_stackManifolds);
}

32
src/bounce/dynamics/contacts/collide/collide_spheres.cpp
View File

@ -21,43 +21,35 @@
#include <bounce/dynamics/shapes/sphere_shape.h>
#include <bounce/collision/shapes/sphere.h>
void b3CollideSphereAndSphere(b3Manifold& manifold,
void b3CollideSphereAndSphere(b3Manifold& manifold,
const b3Transform& xf1, const b3SphereShape* s1,
const b3Transform& xf2, const b3SphereShape* s2)
{
b3Vec3 c1 = b3Mul(xf1, s1->m_center);
b3Vec3 c2 = b3Mul(xf2, s2->m_center);
b3Vec3 c1 = xf1 * s1->m_center;
float32 r1 = s1->m_radius;
b3Vec3 c2 = xf2 * s2->m_center;
float32 r2 = s2->m_radius;
b3Vec3 d = c2 - c1;
float32 dd = b3Dot(d, d);
float32 totalRadius = s1->m_radius + s2->m_radius;
float32 totalRadius = r1 + r2;
if (dd > totalRadius * totalRadius)
{
return;
}
float32 distance = b3Length(d);
b3Vec3 normal;
b3Vec3 normal(0.0f, 1.0f, 0.0f);
if (distance > B3_EPSILON)
{
normal = d / distance;
}
else
{
normal.Set(0.0f, 1.0f, 0.0f);
}
b3Vec3 point = 0.5f * (c1 + s1->m_radius * normal + c2 - s2->m_radius * normal);
manifold.pointCount = 1;
manifold.points[0].localNormal1 = b3MulT(xf1.rotation, normal);
manifold.points[0].localPoint1 = s1->m_center;
manifold.points[0].localPoint2 = s2->m_center;
manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
manifold.points[0].key = 0;
manifold.points[0].localNormal = b3MulT(xf1.rotation, normal);
manifold.points[0].localPoint = s1->m_center;
manifold.points[0].localPoint2 = s2->m_center;
manifold.center = point;
manifold.normal = normal;
manifold.tangent1 = b3Perp(normal);
manifold.tangent2 = b3Cross(manifold.tangent1, normal);
}

28
src/bounce/dynamics/contacts/contact.cpp
View File

@ -39,17 +39,19 @@ void b3Contact::GetWorldManifold(b3WorldManifold* out, u32 index) const
{
B3_ASSERT(index < m_manifoldCount);
b3Manifold* m = m_manifolds + index;
const b3Shape* shapeA = GetShapeA();
b3Transform xfA = shapeA->GetBody()->GetTransform();
const b3Body* bodyA = shapeA->GetBody();
b3Transform xfA = bodyA->GetTransform();
const b3Shape* shapeB = GetShapeB();
b3Transform xfB = shapeB->GetBody()->GetTransform();
const b3Body* bodyB = shapeB->GetBody();
b3Transform xfB = bodyB->GetTransform();
out->Initialize(m, xfA, shapeA->m_radius, xfB, shapeB->m_radius);
out->Initialize(m, shapeA->m_radius, xfA, shapeB->m_radius, xfB);
}
void b3Contact::Update(b3ContactListener* listener)
void b3Contact::Update(b3ContactListener* listener)
{
b3Shape* shapeA = GetShapeA();
b3Body* bodyA = shapeA->GetBody();
@ -66,29 +68,29 @@ void b3Contact::Update(b3ContactListener* listener)
bool wasOverlapping = IsOverlapping();
bool isOverlapping = false;
bool isSensorContact = shapeA->IsSensor() || shapeB->IsSensor();
if (isSensorContact == true)
{
isOverlapping = TestOverlap();
m_manifoldCount = 0;
}
else
else
{
// Copy the old contact points.
b3Manifold oldManifolds[B3_MAX_MANIFOLDS];
u32 oldManifoldCount = m_manifoldCount;
memcpy(oldManifolds, m_manifolds, oldManifoldCount * sizeof(b3Manifold));
// Clear all contact points.
m_manifoldCount = 0;
for (u32 i = 0; i < m_manifoldCapacity; ++i)
{
m_manifolds[i].GuessImpulses();
m_manifolds[i].Initialize();
}
// Generate new contact points for the solver.
Collide();
// Initialize the new built contact points for warm starting the solver.
if (world->m_warmStarting == true)
{
@ -98,7 +100,7 @@ void b3Contact::Update(b3ContactListener* listener)
for (u32 j = 0; j < oldManifoldCount; ++j)
{
const b3Manifold* m1 = oldManifolds + j;
m2->FindImpulses(*m1);
m2->Initialize(*m1);
}
}
}
@ -114,7 +116,7 @@ void b3Contact::Update(b3ContactListener* listener)
}
}
}
// Update the contact state.
if (isOverlapping == true)
@ -134,7 +136,7 @@ void b3Contact::Update(b3ContactListener* listener)
}
// Notify the contact listener the new contact state.
if (listener != NULL)
if (listener != NULL)
{
if (wasOverlapping == false && isOverlapping == true)
{

123
src/bounce/dynamics/contacts/contact_cluster.cpp
View File

@ -17,9 +17,9 @@
*/
#include <bounce/dynamics/contacts/contact_cluster.h>
#include <bounce/collision/distance.h>
#include <bounce/collision/collision.h>
inline void AddCluster(b3Array<b3Cluster>& clusters, const b3Vec3& centroid)
static void AddCluster(b3Array<b3Cluster>& clusters, const b3Vec3& centroid)
{
const float32 kTol = 0.05f;
for (u32 i = 0; i < clusters.Count(); ++i)
@ -165,7 +165,7 @@ void b3InitializeClusters(b3Array<b3Cluster>& outClusters, const b3Array<b3Obser
}
}
inline void b3MoveObsToCluster(b3Array<b3Observation>& observations, u32 fromCluster, u32 toCluster)
static void b3MoveObsToCluster(b3Array<b3Observation>& observations, u32 fromCluster, u32 toCluster)
{
for (u32 i = 0; i < observations.Count(); ++i)
{
@ -177,7 +177,7 @@ inline void b3MoveObsToCluster(b3Array<b3Observation>& observations, u32 fromClu
}
}
inline u32 b3BestCluster(const b3Array<b3Cluster>& clusters, const b3Vec3& point)
static u32 b3BestCluster(const b3Array<b3Cluster>& clusters, const b3Vec3& point)
{
u32 bestIndex = 0;
float32 bestValue = B3_MAX_FLOAT;
@ -276,21 +276,49 @@ void b3Clusterize(b3Array<b3Cluster>& outClusters, b3Array<b3Observation>& outOb
}
}
void b3ReducePolygon(b3ClusterPolygon& pOut,
const b3ClusterPolygon& pIn, u32 startIndex)
static B3_FORCE_INLINE bool b3IsCCW(const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, const b3Vec3& N)
{
B3_ASSERT(startIndex < pIn.Count());
b3Vec3 n = b3Cross(B - A, C - A);
return b3Dot(n, N) > 0.0f;
}
static B3_FORCE_INLINE bool b3IsCCW(const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, const b3Vec3& D, const b3Vec3& N)
{
return b3IsCCW(A, B, C, N) && b3IsCCW(C, D, A, N);
}
static void b3Weld(b3ClusterPolygon& pOut, const b3Vec3& N)
{
B3_ASSERT(pOut.Count() > 0);
b3Vec3 A = pOut[0].position;
for (u32 i = 1; i < pOut.Count(); ++i)
{
b3ClusterVertex& vi = pOut[i];
b3Vec3 B = vi.position;
for (u32 j = i + 1; j < pOut.Count(); ++j)
{
b3ClusterVertex& vj = pOut[j];
b3Vec3 C = vj.position;
if (b3IsCCW(A, B, C, N) == false)
{
b3Swap(vi, vj);
}
}
}
}
void b3ReducePolygon(b3ClusterPolygon& pOut,
const b3ClusterPolygon& pIn, u32 startIndex, const b3Vec3& normal)
{
B3_ASSERT(pIn.Count() > 0);
B3_ASSERT(pOut.Count() == 0);
B3_ASSERT(startIndex < pIn.Count());
pOut.Reserve(pIn.Count());
if (pIn.Count() <= B3_MAX_MANIFOLD_POINTS)
{
for (u32 i = 0; i < pIn.Count(); ++i)
{
pOut.PushBack(pIn[i]);
}
pOut = pIn;
b3Weld(pOut, normal);
return;
}
@ -328,6 +356,12 @@ void b3ReducePolygon(b3ClusterPolygon& pOut,
}
}
// Coincidence check.
if (max < B3_EPSILON * B3_EPSILON)
{
return;
}
pOut.PushBack(pIn[index]);
chosens[index] = true;
}
@ -343,16 +377,32 @@ void b3ReducePolygon(b3ClusterPolygon& pOut,
if (chosens[i]) { continue; }
b3Vec3 C = pIn[i].position;
b3Vec3 Q = b3ClosestPointOnSegment(C, A, B);
b3Vec3 d = Q - C;
float32 dd = b3Dot(d, d);
if (dd > max)
b3Vec3 N = b3Cross(B - A, C - A);
float32 sa2 = b3Dot(N, normal);
float32 a2 = b3Abs(sa2);
if (a2 > max)
{
max = dd;
max = a2;
index = i;
}
}
// Colinearity check.
// Use wanky tolerance for reasonable performance.
const float32 kAreaTol = 0.01f;
if (max < 2.0f * kAreaTol)
{
// Return the largest segment AB.
return;
}
// Ensure CCW order of triangle ABC.
b3Vec3 C = pIn[index].position;
if (b3IsCCW(A, B, C, normal) == false)
{
b3Swap(pOut[0], pOut[1]);
}
pOut.PushBack(pIn[index]);
chosens[index] = true;
}
@ -362,31 +412,40 @@ void b3ReducePolygon(b3ClusterPolygon& pOut,
b3Vec3 B = pOut[1].position;
b3Vec3 C = pOut[2].position;
B3_ASSERT(b3IsCCW(A, B, C, normal));
u32 index = 0;
float32 max = -B3_MAX_FLOAT;
float32 min = B3_MAX_FLOAT;
for (u32 i = 0; i < pIn.Count(); ++i)
{
if (chosens[i]) { continue; }
b3Vec3 D = pIn[i].position;
b3Vec3 Q = b3ClosestPointOnTriangle(D, A, B, C);
b3Vec3 d = Q - D;
float32 dd = b3Dot(d, d);
if (dd > max)
b3Vec3 N = b3Cross(B - A, D - A);
float32 sa2 = b3Dot(N, normal);
if (sa2 < min)
{
max = dd;
min = sa2;
index = i;
}
}
// Colinearity check.
const float32 kAreaTol = 0.01f;
if (b3Abs(min) < 2.0f * kAreaTol)
{
// Return the face ABC.
return;
}
pOut.PushBack(pIn[index]);
chosens[index] = true;
}
B3_ASSERT(pOut.Count() <= B3_MAX_MANIFOLD_POINTS);
b3Weld(pOut, normal);
}
u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32 numIn,
const b3Transform& xfA, float32 radiusA, const b3Transform& xfB, float32 radiusB)
{
@ -397,7 +456,7 @@ u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32
for (u32 i = 0; i < numIn; ++i)
{
b3WorldManifold wm;
wm.Initialize(inManifolds + i, xfA, radiusA, xfB, radiusB);
wm.Initialize(inManifolds + i, radiusA, xfA, radiusB, xfB);
for (u32 j = 0; j < wm.pointCount; ++j)
{
@ -444,7 +503,7 @@ u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32
const b3ManifoldPoint* mp = m->points + o.manifoldPoint;
b3WorldManifoldPoint wmp;
wmp.Initialize(mp, xfA, radiusA, xfB, radiusB);
wmp.Initialize(mp, radiusA, xfA, radiusB, xfB);
b3ClusterVertex cv;
cv.position = wmp.point;
@ -461,18 +520,12 @@ u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32
}
center /= float32(polygonB.Count());
//normal /= float32(polygonB.Count());
normal.Normalize();
B3_ASSERT(numOut < B3_MAX_MANIFOLDS);
b3Manifold* manifold = outManifolds + numOut;
++numOut;
manifold->center = center;
manifold->normal = normal;
manifold->tangent1 = b3Perp(normal);
manifold->tangent2 = b3Cross(manifold->tangent1, normal);
// Reduce.
// Ensure deepest point is contained.
u32 minIndex = 0;
@ -484,7 +537,7 @@ u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32
const b3ManifoldPoint* inPoint = inManifold->points + o->manifoldPoint;
b3WorldManifoldPoint wmp;
wmp.Initialize(inPoint, xfA, radiusA, xfB, radiusB);
wmp.Initialize(inPoint, radiusA, xfA, radiusB, xfB);
float32 separation = wmp.separation;
if (separation < minSeparation)
@ -497,7 +550,7 @@ u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32
}
b3StackArray<b3ClusterVertex, 32> reducedB;
b3ReducePolygon(reducedB, polygonB, minIndex);
b3ReducePolygon(reducedB, polygonB, minIndex, normal);
for (u32 j = 0; j < reducedB.Count(); ++j)
{
b3ClusterVertex v = reducedB[j];

195
src/bounce/dynamics/contacts/contact_solver.cpp
View File

@ -21,9 +21,7 @@
#include <bounce/dynamics/shapes/shape.h>
#include <bounce/dynamics/body.h>
#include <bounce/common/memory/stack_allocator.h>
// Turn on or off central friction. This is an important optimization for old hardwares.
#define B3_CENTRAL_FRICTION 0
#include <bounce/common/math/mat.h>
// This solver implements PGS for solving velocity constraints and
// NGS for solving position constraints.
@ -136,12 +134,11 @@ void b3ContactSolver::InitializeConstraints()
b3PositionConstraintPoint* pcp = pcm->points + k;
b3VelocityConstraintPoint* vcp = vcm->points + k;
pcp->localNormalA = cp->localNormal;
pcp->localPointA = cp->localPoint;
pcp->localNormalA = cp->localNormal1;
pcp->localPointA = cp->localPoint1;
pcp->localPointB = cp->localPoint2;
vcp->normalImpulse = cp->normalImpulse;
vcp->tangentImpulse = cp->tangentImpulse;
}
}
}
@ -151,12 +148,15 @@ void b3ContactSolver::InitializeConstraints()
b3Contact* c = m_contacts[i];
u32 manifoldCount = c->m_manifoldCount;
b3ContactVelocityConstraint* vc = m_velocityConstraints + i;
b3ContactPositionConstraint* pc = m_positionConstraints + i;
b3ContactVelocityConstraint* vc = m_velocityConstraints + i;
float32 radiusA = pc->radiusA;
float32 radiusB = pc->radiusB;
b3Vec3 localCenterA = pc->localCenterA;
b3Vec3 localCenterB = pc->localCenterB;
u32 indexA = vc->indexA;
float32 mA = vc->invMassA;
b3Mat33 iA = vc->invIA;
@ -175,8 +175,13 @@ void b3ContactSolver::InitializeConstraints()
b3Vec3 vB = m_velocities[indexB].v;
b3Vec3 wB = m_velocities[indexB].w;
b3Transform xfA(xA, qA);
b3Transform xfB(xB, qB);
b3Transform xfA;
xfA.rotation = b3ConvertQuatToRot(qA);
xfA.position = xA - b3Mul(xfA.rotation, localCenterA);
b3Transform xfB;
xfB.rotation = b3ConvertQuatToRot(qB);
xfB.position = xB - b3Mul(xfB.rotation, localCenterB);
for (u32 j = 0; j < manifoldCount; ++j)
{
@ -185,9 +190,8 @@ void b3ContactSolver::InitializeConstraints()
b3VelocityConstraintManifold* vcm = vc->manifolds + j;
b3WorldManifold wm;
wm.Initialize(m, xfA, radiusA, xfB, radiusB);
wm.Initialize(m, radiusA, xfA, radiusB, xfB);
vcm->center = wm.center;
vcm->normal = wm.normal;
vcm->tangent1 = wm.tangent1;
vcm->tangent2 = wm.tangent2;
@ -196,14 +200,14 @@ void b3ContactSolver::InitializeConstraints()
for (u32 k = 0; k < pointCount; ++k)
{
b3ManifoldPoint* mp = m->points + k;
b3WorldManifoldPoint* wmp = wm.points + k;
b3WorldManifoldPoint* mp = wm.points + k;
b3PositionConstraintPoint* pcp = pcm->points + k;
b3VelocityConstraintPoint* vcp = vcm->points + k;
b3Vec3 normal = wmp->normal;
b3Vec3 point = wmp->point;
b3Vec3 normal = mp->normal;
b3Vec3 point = mp->point;
b3Vec3 rA = point - xA;
b3Vec3 rB = point - xB;
@ -229,87 +233,47 @@ void b3ContactSolver::InitializeConstraints()
vcp->velocityBias = -vc->restitution * vn;
}
}
#if B3_CENTRAL_FRICTION == 0
// Add friction constraints.
{
vcp->tangent1 = vcm->tangent1;
vcp->tangent2 = vcm->tangent2;
b3Vec3 t1 = vcp->tangent1;
b3Vec3 t2 = vcp->tangent2;
// Compute effective mass.
// Identities used:
// I = I^T because I is symmetric. Its inverse is also symmetric.
// dot(a * u, b * v) = a * b * dot(u, v)
// dot(t1, t2) = 0
b3Vec3 rt1A = b3Cross(rA, t1);
b3Vec3 rt1B = b3Cross(rB, t1);
b3Vec3 rt2A = b3Cross(rA, t2);
b3Vec3 rt2B = b3Cross(rB, t2);
float32 kTan11 = mA + mB + b3Dot(iA * rt1A, rt1A) + b3Dot(iB * rt2B, rt2B);
float32 kTan12 = b3Dot(iA * rt1A, rt2A) + b3Dot(iB * rt1B, rt2B);
float32 kTan21 = kTan12;
float32 kTan22 = mA + mB + b3Dot(iA * rt2A, rt2A) + b3Dot(iB * rt2B, rt2B);
b3Mat22 K;
K.x.x = kTan11;
K.x.y = kTan12;
K.y.x = kTan21;
K.y.y = kTan22;
vcp->tangentMass = K;
}
#endif
}
#if B3_CENTRAL_FRICTION == 1
if (pointCount > 0)
B3_ASSERT(pointCount > 0);
// Add friction constraints.
if(pointCount > 0)
{
b3Vec3 rA = vcm->center - xA;
b3Vec3 rB = vcm->center - xB;
b3Vec3 rA = wm.center - xA;
b3Vec3 rB = wm.center - xB;
vcm->rA = rA;
vcm->rB = rB;
// Add friction constraints.
{
b3Vec3 t1 = vcm->tangent1;
b3Vec3 t2 = vcm->tangent2;
b3Vec3 rn1A = b3Cross(rA, t1);
b3Vec3 rn1B = b3Cross(rB, t1);
b3Vec3 rn2A = b3Cross(rA, t2);
b3Vec3 rn2B = b3Cross(rB, t2);
// Compute effective mass.
// Identities used:
// I = I^T because I is symmetric. Its inverse is also symmetric.
// dot(a * u, b * v) = a * b * dot(u, v)
// dot(t1, t2) = 0
b3Vec3 rt1A = b3Cross(rA, t1);
b3Vec3 rt1B = b3Cross(rB, t1);
b3Vec3 rt2A = b3Cross(rA, t2);
b3Vec3 rt2B = b3Cross(rB, t2);
float32 kTan11 = mA + mB + b3Dot(iA * rt1A, rt1A) + b3Dot(iB * rt2B, rt2B);
float32 kTan12 = b3Dot(iA * rt1A, rt2A) + b3Dot(iB * rt1B, rt2B);
float32 kTan21 = kTan12;
float32 kTan22 = mA + mB + b3Dot(iA * rt2A, rt2A) + b3Dot(iB * rt2B, rt2B);
// J1_l1 * M1 * J2_l1 = J1_l2 * M2 * J2_l2 = 0
float32 k11 = mA + mB + b3Dot(iA * rn1A, rn1A) + b3Dot(iB * rn1B, rn1B);
float32 k12 = b3Dot(iA * rn1A, rn2A) + b3Dot(iB * rn1B, rn2B);
float32 k22 = mA + mB + b3Dot(iA * rn2A, rn2A) + b3Dot(iB * rn2B, rn2B);
b3Mat22 K;
K.x.x = kTan11;
K.x.y = kTan12;
K.y.x = kTan21;
K.y.y = kTan22;
K.x.Set(k11, k12);
K.y.Set(k12, k22);
vcm->tangentMass = K;
vcm->tangentMass = b3Inverse(K);
}
// Add motor constraint.
// Add twist constraint.
{
float32 mass = b3Dot(vcm->normal, (iA + iB) * vcm->normal);
vcm->motorMass = mass > 0.0f ? 1.0f / mass : 0.0f;
}
}
#endif
}
}
}
@ -344,11 +308,8 @@ void b3ContactSolver::WarmStart()
{
b3VelocityConstraintPoint* vcp = vcm->points + k;
b3Vec3 P1 = vcp->normalImpulse * vcp->normal;
b3Vec3 P2 = vcp->tangentImpulse.x * vcp->tangent1;
b3Vec3 P3 = vcp->tangentImpulse.y * vcp->tangent2;
b3Vec3 P = P1 + P2 + P3;
b3Vec3 P = vcp->normalImpulse * vcp->normal;
vA -= mA * P;
wA -= iA * b3Cross(vcp->rA, P);
@ -356,7 +317,6 @@ void b3ContactSolver::WarmStart()
wB += iB * b3Cross(vcp->rB, P);
}
#if B3_CENTRAL_FRICTION == 1
if (pointCount > 0)
{
b3Vec3 P1 = vcm->tangentImpulse.x * vcm->tangent1;
@ -369,7 +329,6 @@ void b3ContactSolver::WarmStart()
vB += mB * (P1 + P2);
wB += iB * (b3Cross(vcm->rB, P1 + P2) + P3);
}
#endif
}
m_velocities[indexA].v = vA;
@ -405,13 +364,12 @@ void b3ContactSolver::SolveVelocityConstraints()
u32 pointCount = vcm->pointCount;
float32 normalImpulse = 0.0f;
for (u32 k = 0; k < pointCount; ++k)
{
b3VelocityConstraintPoint* vcp = vcm->points + k;
B3_ASSERT(vcp->normalImpulse >= 0.0f);
// Solve normal constraint.
// Solve normal constraints.
{
b3Vec3 dv = vB + b3Cross(wB, vcp->rB) - vA - b3Cross(wA, vcp->rA);
float32 Cdot = b3Dot(vcp->normal, dv);
@ -432,43 +390,8 @@ void b3ContactSolver::SolveVelocityConstraints()
normalImpulse += vcp->normalImpulse;
}
#if B3_CENTRAL_FRICTION == 0
// Solve tangent constraints.
{
b3Vec3 dv = vB + b3Cross(wB, vcp->rB) - vA - b3Cross(wA, vcp->rA);
b3Vec2 Cdot;
Cdot.x = b3Dot(dv, vcp->tangent1);
Cdot.y = b3Dot(dv, vcp->tangent2);
b3Vec2 impulse = vcp->tangentMass.Solve(-Cdot);
b3Vec2 oldImpulse = vcp->tangentImpulse;
vcp->tangentImpulse += impulse;
float32 maxImpulse = vc->friction * vcp->normalImpulse;
if (b3Dot(vcp->tangentImpulse, vcp->tangentImpulse) > maxImpulse * maxImpulse)
{
vcp->tangentImpulse.Normalize();
vcp->tangentImpulse *= maxImpulse;
}
impulse = vcp->tangentImpulse - oldImpulse;
b3Vec3 P1 = impulse.x * vcp->tangent1;
b3Vec3 P2 = impulse.y * vcp->tangent2;
b3Vec3 P = P1 + P2;
vA -= mA * P;
wA -= iA * b3Cross(vcp->rA, P);
vB += mB * P;
wB += iB * b3Cross(vcp->rB, P);
}
#endif
}
#if B3_CENTRAL_FRICTION == 1
if (pointCount > 0)
{
// Solve tangent constraints.
@ -479,7 +402,7 @@ void b3ContactSolver::SolveVelocityConstraints()
Cdot.x = b3Dot(dv, vcm->tangent1);
Cdot.y = b3Dot(dv, vcm->tangent2);
b3Vec2 impulse = vcm->tangentMass.Solve(-Cdot);
b3Vec2 impulse = vcm->tangentMass * -Cdot;
b3Vec2 oldImpulse = vcm->tangentImpulse;
vcm->tangentImpulse += impulse;
@ -518,7 +441,6 @@ void b3ContactSolver::SolveVelocityConstraints()
wB += iB * P;
}
}
#endif
}
m_velocities[indexA].v = vA;
@ -552,7 +474,6 @@ void b3ContactSolver::StoreImpulses()
b3ManifoldPoint* cp = m->points + k;
b3VelocityConstraintPoint* vcp = vcm->points + k;
cp->normalImpulse = vcp->normalImpulse;
cp->tangentImpulse = vcp->tangentImpulse;
}
}
}
@ -563,10 +484,10 @@ struct b3ContactPositionSolverPoint
void Initialize(const b3ContactPositionConstraint* pc, const b3PositionConstraintPoint* pcp, const b3Transform& xfA, const b3Transform& xfB)
{
normal = b3Mul(xfA.rotation, pcp->localNormalA);
b3Vec3 p1 = b3Mul(xfA, pcp->localPointA);
b3Vec3 p2 = b3Mul(xfB, pcp->localPointB);
point = 0.5f * (p1 + pc->radiusA * normal + p2 - pc->radiusB * normal);
separation = b3Dot(p2 - p1, normal) - pc->radiusA - pc->radiusB;
b3Vec3 c1 = b3Mul(xfA, pcp->localPointA);
b3Vec3 c2 = b3Mul(xfB, pcp->localPointB);
point = c2;
separation = b3Dot(c2 - c1, normal) - pc->radiusA - pc->radiusB;
}
b3Vec3 normal;
@ -592,10 +513,10 @@ bool b3ContactSolver::SolvePositionConstraints()
b3Mat33 iB = pc->invIB;
b3Vec3 localCenterB = pc->localCenterB;
b3Vec3 xA = m_positions[indexA].x;
b3Vec3 cA = m_positions[indexA].x;
b3Quat qA = m_positions[indexA].q;
b3Vec3 xB = m_positions[indexB].x;
b3Vec3 cB = m_positions[indexB].x;
b3Quat qB = m_positions[indexB].q;
u32 manifoldCount = pc->manifoldCount;
@ -612,11 +533,11 @@ bool b3ContactSolver::SolvePositionConstraints()
b3Transform xfA;
xfA.rotation = b3ConvertQuatToRot(qA);
xfA.position = xA - b3Mul(xfA.rotation, localCenterA);
xfA.position = cA - b3Mul(xfA.rotation, localCenterA);
b3Transform xfB;
xfB.rotation = b3ConvertQuatToRot(qB);
xfB.position = xB - b3Mul(xfB.rotation, localCenterB);
xfB.position = cB - b3Mul(xfB.rotation, localCenterB);
b3ContactPositionSolverPoint cpcp;
cpcp.Initialize(pc, pcp, xfA, xfB);
@ -625,16 +546,16 @@ bool b3ContactSolver::SolvePositionConstraints()
b3Vec3 point = cpcp.point;
float32 separation = cpcp.separation;
b3Vec3 rA = point - xA;
b3Vec3 rB = point - xB;
// Update max constraint error.
minSeparation = b3Min(minSeparation, separation);
// Prevent large corrections and allow some slop.
// Allow some slop and prevent large corrections.
float32 C = b3Clamp(B3_BAUMGARTE * (separation + B3_LINEAR_SLOP), -B3_MAX_LINEAR_CORRECTION, 0.0f);
// Compute effective mass.
b3Vec3 rA = point - cA;
b3Vec3 rB = point - cB;
b3Vec3 rnA = b3Cross(rA, normal);
b3Vec3 rnB = b3Cross(rB, normal);
float32 K = mA + mB + b3Dot(rnA, iA * rnA) + b3Dot(rnB, iB * rnB);
@ -643,20 +564,20 @@ bool b3ContactSolver::SolvePositionConstraints()
float32 impulse = K > 0.0f ? -C / K : 0.0f;
b3Vec3 P = impulse * normal;
xA -= mA * P;
cA -= mA * P;
qA -= b3Derivative(qA, iA * b3Cross(rA, P));
qA.Normalize();
xB += mB * P;
cB += mB * P;
qB += b3Derivative(qB, iB * b3Cross(rB, P));
qB.Normalize();
}
}
m_positions[indexA].x = xA;
m_positions[indexA].x = cA;
m_positions[indexA].q = qA;
m_positions[indexB].x = xB;
m_positions[indexB].x = cB;
m_positions[indexB].q = qB;
}

62
src/bounce/dynamics/contacts/manifold.cpp
View File

@ -18,7 +18,7 @@
#include <bounce/dynamics/contacts/manifold.h>
void b3Manifold::GuessImpulses()
void b3Manifold::Initialize()
{
pointCount = 0;
tangentImpulse.SetZero();
@ -27,12 +27,11 @@ void b3Manifold::GuessImpulses()
{
b3ManifoldPoint* p = points + i;
p->normalImpulse = 0.0f;
p->tangentImpulse.SetZero();
p->persisting = 0;
}
}
void b3Manifold::FindImpulses(const b3Manifold& oldManifold)
void b3Manifold::Initialize(const b3Manifold& oldManifold)
{
tangentImpulse = oldManifold.tangentImpulse;
motorImpulse = oldManifold.motorImpulse;
@ -46,7 +45,6 @@ void b3Manifold::FindImpulses(const b3Manifold& oldManifold)
if (p2->triangleKey == p1->triangleKey && p2->key == p1->key)
{
p2->normalImpulse = p1->normalImpulse;
p2->tangentImpulse = p1->tangentImpulse;
p2->persisting = 1;
break;
}
@ -54,36 +52,42 @@ void b3Manifold::FindImpulses(const b3Manifold& oldManifold)
}
}
void b3WorldManifoldPoint::Initialize(const b3ManifoldPoint* mp,
const b3Transform& xfA, float32 radiusA,
const b3Transform& xfB, float32 radiusB)
void b3WorldManifoldPoint::Initialize(const b3ManifoldPoint* p, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB)
{
normal = b3Mul(xfA.rotation, mp->localNormal);
b3Vec3 p1 = b3Mul(xfA, mp->localPoint);
b3Vec3 p2 = b3Mul(xfB, mp->localPoint2);
point = 0.5f * (p1 + radiusA * normal + p2 - radiusB * normal);
separation = b3Dot(p2 - p1, normal) - radiusA - radiusB;
b3Vec3 nA = xfA.rotation * p->localNormal1;
b3Vec3 cA = xfA * p->localPoint1;
b3Vec3 cB = xfB * p->localPoint2;
b3Vec3 pA = cA + rA * nA;
b3Vec3 pB = cB - rB * nA;
point = 0.5f * (pA + pB);
normal = nA;
separation = b3Dot(cB - cA, nA) - rA - rB;
}
void b3WorldManifold::Initialize(const b3Manifold* manifold,
const b3Transform& xfA, float32 radiusA,
const b3Transform& xfB, float32 radiusB)
void b3WorldManifold::Initialize(const b3Manifold* m, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB)
{
if (manifold->pointCount > 0)
{
center = manifold->center;
normal = manifold->normal;
tangent1 = manifold->tangent1;
tangent2 = manifold->tangent2;
}
pointCount = manifold->pointCount;
center.SetZero();
normal.SetZero();
pointCount = m->pointCount;
for (u32 i = 0; i < pointCount; ++i)
{
const b3ManifoldPoint* mp = manifold->points + i;
b3WorldManifoldPoint* wmp = points + i;
const b3ManifoldPoint* p = m->points + i;
b3WorldManifoldPoint* wp = points + i;
wmp->Initialize(mp, xfA, radiusA, xfB, radiusB);
wp->Initialize(p, rA, xfA, rB, xfB);
center += wp->point;
normal += wp->normal;
}
}
if (pointCount > 0)
{
center /= float32(pointCount);
normal.Normalize();
tangent1 = b3Perp(normal);
tangent2 = b3Cross(tangent1, normal);
}
}

17
src/bounce/dynamics/contacts/mesh_contact.cpp
View File

@ -231,17 +231,6 @@ void b3MeshContact::Collide()
b3MeshShape* meshShapeB = (b3MeshShape*)shapeB;
b3Transform xfB = bodyB->GetTransform();
// Remove this conditional inclusion if collisions
// between spheres and the internal features of meshes
// should be filtered.
#if 0
if (shapeA->GetType() == e_sphereShape)
{
CollideSphere();
return;
}
#endif
b3World* world = bodyA->GetWorld();
b3StackAllocator* allocator = &world->m_stackAllocator;
@ -266,9 +255,10 @@ void b3MeshContact::Collide()
shapeB.m_body = bodyB;
shapeB.m_hull = &hullB;
shapeB.m_radius = B3_HULL_RADIUS;
b3Manifold* manifold = tempManifolds + tempCount;
manifold->GuessImpulses();
manifold->Initialize();
b3CollideShapeAndShape(*manifold, xfA, shapeA, xfB, &shapeB, &triangleCache->cache);
for (u32 j = 0; j < manifold->pointCount; ++j)
@ -282,5 +272,6 @@ void b3MeshContact::Collide()
// Send contact manifolds for clustering. This is an important optimization.
B3_ASSERT(m_manifoldCount == 0);
m_manifoldCount = b3Clusterize(m_stackManifolds, tempManifolds, tempCount, xfA, shapeA->m_radius, xfB, B3_HULL_RADIUS);
allocator->Free(tempManifolds);
}

62
src/bounce/dynamics/draw_world.cpp
View File

@ -80,55 +80,29 @@ void b3World::DebugDraw() const
{
const b3Shape* shapeA = c->GetShapeA();
const b3Shape* shapeB = c->GetShapeB();
u32 manifoldCount = c->m_manifoldCount;
const b3Manifold* manifolds = c->m_manifolds;
for (u32 i = 0; i < manifoldCount; ++i)
{
const b3Manifold* m = manifolds + i;
b3WorldManifold wm;
wm.Initialize(m, shapeA->GetBody()->GetTransform(), shapeA->m_radius, shapeB->GetBody()->GetTransform(), shapeB->m_radius);
c->GetWorldManifold(&wm, i);
if (wm.pointCount > 0)
{
b3Vec3 n = wm.normal;
b3Vec3 t1 = wm.tangent1;
b3Vec3 t2 = wm.tangent2;
b3Vec3 p = wm.center;
float32 Pt1 = m->tangentImpulse.x;
float32 Pt2 = m->tangentImpulse.y;
if (flags & b3Draw::e_contactPointsFlag)
{
b3_debugDraw->DrawPoint(p, 4.0f, b3Color_yellow);
}
b3Vec3 t1 = wm.tangent1;
b3Vec3 t2 = wm.tangent2;
if (flags & b3Draw::e_contactNormalsFlag)
{
b3_debugDraw->DrawSegment(p, p + n, b3Color_yellow);
}
if (flags & b3Draw::e_contactTangentsFlag)
{
b3_debugDraw->DrawSegment(p, p + t1, b3Color_yellow);
b3_debugDraw->DrawSegment(p, p + t2, b3Color_yellow);
}
}
for (u32 j = 0; j < wm.pointCount; ++j)
b3Vec3 points[B3_MAX_MANIFOLD_POINTS];
for (u32 j = 0; j < m->pointCount; ++j)
{
const b3ManifoldPoint* mp = m->points + j;
const b3WorldManifoldPoint* wmp = wm.points + j;
b3Vec3 n = wmp->normal;
b3Vec3 t1 = wm.tangent1;
b3Vec3 t2 = wm.tangent2;
b3Vec3 p = wmp->point;
float32 Pn = mp->normalImpulse;
float32 Pt1 = mp->tangentImpulse.x;
float32 Pt2 = mp->tangentImpulse.y;
points[j] = p;
if (flags & b3Draw::e_contactPointsFlag)
{
@ -139,12 +113,30 @@ void b3World::DebugDraw() const
{
b3_debugDraw->DrawSegment(p, p + n, b3Color_white);
}
}
if (m->pointCount > 0)
{
b3Vec3 p = wm.center;
b3Vec3 n = wm.normal;
b3Vec3 t1 = wm.tangent1;
b3Vec3 t2 = wm.tangent2;
if (flags & b3Draw::e_contactNormalsFlag)
{
b3_debugDraw->DrawSegment(p, p + n, b3Color_yellow);
}
if (flags & b3Draw::e_contactTangentsFlag)
{
b3_debugDraw->DrawSegment(p, p + t1, b3Color_yellow);
b3_debugDraw->DrawSegment(p, p + t2, b3Color_yellow);
}
if (flags & b3Draw::e_contactAreasFlag)
{
b3_debugDraw->DrawSolidPolygon(wm.normal, points, m->pointCount, b3Color_pink);
}
}
}
}

54
src/bounce/dynamics/island.cpp
View File

@ -82,6 +82,28 @@ void b3Island::Add(b3Joint* j)
++m_jointCount;
}
// Box2D
static B3_FORCE_INLINE b3Vec3 b3SolveGyroscopic(const b3Quat& q, const b3Mat33& Ib, const b3Vec3& w1, float32 h)
{
// Convert angular velocity to body coordinates
b3Vec3 w1b = b3MulT(q, w1);
// Jacobian of f
b3Mat33 J = Ib + h * (b3Skew(w1b) * Ib - b3Skew(Ib * w1b));
// One iteration of Newton-Raphson
// Residual vector
b3Vec3 f;
{
f = h * b3Cross(w1b, Ib * w1b);
w1b -= J.Solve(f);
}
// Convert angular velocity back to world coordinates
b3Vec3 w2 = b3Mul(q, w1b);
return w2;
}
void b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations, u32 positionIterations, u32 flags)
{
float32 h = dt;
@ -91,7 +113,7 @@ void b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations,
// Solution: v(t) = v0 * exp(-c * t)
// Step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
const float32 k_d = 0.1f;
const float32 k_d = 0.005f;
float32 d = exp(-k_d * h);
// 1. Integrate velocities
@ -110,16 +132,35 @@ void b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations,
if (b->m_type == e_dynamicBody)
{
b3Vec3 force = b->m_force + b->m_gravityScale * gravity;
// Integrate forces
b3Vec3 force = b->m_force + b->m_gravityScale * gravity;
v += h * b->m_invMass * force;
// Clear forces
b->m_force.SetZero();
// Integrate torques
// @todo add gyroscopic term
w += h * b->m_worldInvI * b->m_torque;
// Superposition Principle
// w2 - w1 = dw1 + dw2
// w2 - w1 = h * I^1 * bt + h * I^1 * -gt
// w2 = w1 + dw1 + dw2
// Explicit Euler on current inertia and applied torque
// w2 = w1 + h * I1^1 * bt1
b3Vec3 dw1 = h * b->m_worldInvI * b->m_torque;
// Implicit Euler on next inertia and angular velocity
// w2 = w1 - h * I2^1 * cross(w2, I2 * w2)
// w2 - w1 = -I2^1 * h * cross(w2, I2 * w2)
// I2 * (w2 - w1) = -h * cross(w2, I2 * w2)
// I2 * (w2 - w1) + h * cross(w2, I2 * w2) = 0
// Toss out I2 from f using local I2 (constant) and local w1
b3Vec3 w2 = b3SolveGyroscopic(q, b->m_I, w, h);
b3Vec3 dw2 = w2 - w;
w += dw1 + dw2;
// Clear torques
b->m_torque.SetZero();
@ -209,9 +250,8 @@ void b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations,
w *= ratio;
}
// Integrate linear velocity
// Integrate
x += h * v;
// Integrate angular velocity
q = b3Integrate(q, w, h);
m_positions[i].x = x;

22
src/bounce/dynamics/joints/cone_joint.cpp
View File

@ -36,7 +36,7 @@ void b3ConeJointDef::Initialize(b3Body* bA, b3Body* bB,
{
bodyA = bA;
bodyB = bB;
b3Transform xf;
xf.rotation.y = axis;
xf.rotation.z = b3Perp(axis);
@ -80,7 +80,7 @@ void b3ConeJoint::InitializeConstraints(const b3SolverData* data)
b3Vec3 xA = data->positions[m_indexA].x;
b3Quat qA = data->positions[m_indexA].q;
b3Vec3 xB = data->positions[m_indexB].x;
b3Quat qB = data->positions[m_indexB].q;
@ -108,10 +108,10 @@ void b3ConeJoint::InitializeConstraints(const b3SolverData* data)
b3Vec3 u2 = xfB.rotation.y;
m_limitAxis = b3Cross(u2, u1);
float32 mass = b3Dot((m_iA + m_iB) * m_limitAxis, m_limitAxis);
m_limitMass = mass > 0.0f ? 1.0f / mass : 0.0f;
// C = cone / 2 - angle >= 0
float32 cosine = b3Dot(u2, u1);
float32 sine = b3Length(m_limitAxis);
@ -142,7 +142,7 @@ void b3ConeJoint::WarmStart(const b3SolverData* data)
b3Vec3 wA = data->velocities[m_indexA].w;
b3Vec3 vB = data->velocities[m_indexB].v;
b3Vec3 wB = data->velocities[m_indexB].w;
{
vA -= m_mA * m_impulse;
wA -= m_iA * b3Cross(m_rA, m_impulse);
@ -175,7 +175,7 @@ void b3ConeJoint::SolveVelocityConstraints(const b3SolverData* data)
{
b3Vec3 Cdot = vB + b3Cross(wB, m_rB) - vA - b3Cross(wA, m_rA);
b3Vec3 P = m_mass.Solve(-Cdot);
m_impulse += P;
vA -= m_mA * P;
@ -234,7 +234,7 @@ bool b3ConeJoint::SolvePositionConstraints(const b3SolverData* data)
b3Mat33 RB = b3Skew(rB);
b3Mat33 RBT = b3Transpose(RB);
b3Mat33 mass = M + RA * iA * RAT + RB * iB * RBT;
b3Vec3 P = mass.Solve(-C);
xA -= mA * P;
@ -254,7 +254,7 @@ bool b3ConeJoint::SolvePositionConstraints(const b3SolverData* data)
b3Vec3 u1 = b3Mul(qA, m_localFrameA.rotation.y);
b3Vec3 u2 = b3Mul(qB, m_localFrameB.rotation.y);
b3Vec3 limitAxis = b3Cross(u2, u1);
// Compute fresh effective mass.
float32 mass = b3Dot((iA + iB) * limitAxis, limitAxis);
float32 limitMass = mass > 0.0f ? 1.0f / mass : 0.0f;
@ -263,19 +263,19 @@ bool b3ConeJoint::SolvePositionConstraints(const b3SolverData* data)
float32 cosine = b3Dot(u2, u1);
float32 sine = b3Length(limitAxis);
float32 angle = atan2(sine, cosine);
float32 limitImpulse = 0.0f;
if (0.5f * m_coneAngle < angle)
{
float32 C = 0.5f * m_coneAngle - angle;
limitError = -C;
// Allow some slop and prevent large corrections
C = b3Clamp(C + B3_ANGULAR_SLOP, -B3_MAX_ANGULAR_CORRECTION, 0.0f);
limitImpulse = -C * limitMass;
}
b3Vec3 P = limitImpulse * limitAxis;
qA -= b3Derivative(qA, iA * P);

640
src/bounce/dynamics/joints/revolute_joint.cpp
View File

@ -20,71 +20,216 @@
#include <bounce/dynamics/body.h>
#include <bounce/common/draw.h>
// C1 = p2 - p1
// C2 = dot(u2, w1)
// C3 = dot(v2, w1)
// Cdot = dot(u2, omega1 x w1) + dot(w1, omega2 x u2)
// Cyclic identity:
// Cdot = dot(w1 x u2, omega1) + dot(u2 x w1, omega2) =
// dot(-u2 x w1, omega1) + dot(u2 x w1, omega2)
/*
// n1 = cross(u2, w1)^T
// n2 = cross(v2, w1)^T
Algebra:
// J = [-I skew(r1) I -skew(r2)]
// [0 -n1 0 n1]
// [0 -n2 0 n2]
Q(p) * P(q) = P(q) * Q(p)
q' = 0.5 * w * q
// W = [i1 0 0 0]
// [0 m1 0 0]
// [0 0 i2 0]
// [0 0 0 m2]
P = [0 1 0 0]
[0 0 1 0]
[0 0 0 1]
Hinge projection matrix:
P_hin = [x^T] * P = [0 1 0 0]
[y^T] [0 0 1 0]
Constraint:
q = conj(q1) * q2
C = P_hin * q
Chain rule:
q' =
conj(q1)' * q2 + conj(q1) * q2' =
conj(q1') * q2 + conj(q1) * q2'
1st term:
conj(q1') * q2 =
0.5 * conj(w1 * q1) * w2 =
0.5 * conj(q1) * conj(w1) * q2 =
0.5 * conj(q1) * -w1 * q2 =
-0.5 * conj(q1) * w1 * q2 =
-0.5 * Q(conj(q1)) * P(q2) * Q(w1)
J1 = -0.5 * Q(conj(qA)) * P(qB)
2nd term:
conj(q1) * q2' =
0.5 * conj(q1) * w2 * q2 =
0.5 * Q(conj(q1)) * Q(w2) * Q(q2) =
0.5 * Q(conj(q1)) * P(q2) * Q(w2)
J2 = 0.5 * Q(conj(q1)) * P(q2)
C' = P_hin * q' =
P_hin * (J1 * P^T * w1 + J2 * P^T * w2) =
P_hin * J1 * P^T * w1 + P_hin * J2 * P^T * w2
New Jacobians:
J1 = P_hin * J1 * P^T
J2 = P_hin * J2 * P^T
Limit constraint:
q = conj(q1) * q2
C = 2 * atan(q.z / q.w)
Chain rule:
f( g( q(t) ) ) = 2 * atan( g( q(t) ) )
g( q(t) ) = q.z / q.w
df / dt = del_f / del_g * del_g / del_q * dq / dt
del_f / del_g =
1 / (g^2 + 1) =
1 / ((q.z / q.w)^2 + 1) =
q.w^2 / (q.w^2 + q.z^2) ~
q.w^2
del_g / del_q =
[del_g / del_w | del_g / del_x | del_g / del_y | del_g / del_z] =
[-q.z/q.w^2 0 0 q.w/q.w^2] =
[-q.z/q.w^2 0 0 1/q.w] =
1 / q.w^2 * [-q.z 0 0 q.w]
df / dt =
q.w^2 * 1 / q.w^2 * [-q.z 0 0 q.w] * dq / dt =
[-q.z 0 0 q.w] * dq / dt
P_lim = [-q.z 0 0 q.w]
C' = P_lim * (P_hinge * q') - target_speed
*/
static B3_FORCE_INLINE b3Mat44 iQ_mat(const b3Quat& q)
{
b3Mat44 Q;
Q.x = b3Vec4(q.w, q.x, q.y, q.z);
Q.y = b3Vec4(-q.x, q.w, q.z, -q.y);
Q.z = b3Vec4(-q.y, -q.z, q.w, q.x);
Q.w = b3Vec4(-q.z, q.y, -q.x, q.w);
return Q;
}
static B3_FORCE_INLINE b3Mat44 iP_mat(const b3Quat& q)
{
b3Mat44 P;
P.x = b3Vec4(q.w, q.x, q.y, q.z);
P.y = b3Vec4(-q.x, q.w, -q.z, q.y);
P.z = b3Vec4(-q.y, q.z, q.w, -q.x);
P.w = b3Vec4(-q.z, -q.y, q.x, q.w);
return P;
}
static B3_FORCE_INLINE b3Mat34 P_mat()
{
b3Mat34 P;
P.x = b3Vec3(0.0f, 0.0f, 0.0f);
P.y = b3Vec3(1.0f, 0.0f, 0.0f);
P.z = b3Vec3(0.0f, 1.0f, 0.0f);
P.w = b3Vec3(0.0f, 0.0f, 1.0f);
return P;
}
static B3_FORCE_INLINE b3Mat24 P_hinge_mat()
{
b3Mat24 P;
P.x = b3Vec2(0.0f, 0.0f);
P.y = b3Vec2(1.0f, 0.0f);
P.z = b3Vec2(0.0f, 1.0f);
P.w = b3Vec2(0.0f, 0.0f);
return P;
}
// 1x4
static B3_FORCE_INLINE b3Vec4 P_hinge_limit_mat(const b3Quat& q)
{
return b3Vec4(-q.z, 0.0f, 0.0f, q.w);
}
// 4x1
static B3_FORCE_INLINE b3Vec4 q_to_v(const b3Quat& q)
{
return b3Vec4(q.w, q.x, q.y, q.z);
}
static b3Mat34 P = P_mat();
static b3Mat43 PT = b3Transpose(P);
static b3Mat24 P_hinge = P_hinge_mat();
void b3RevoluteJointDef::Initialize(b3Body* bA, b3Body* bB,
const b3Vec3& axis, const b3Vec3& anchor,
float32 lower, float32 upper)
{
b3Transform xf;
xf.rotation.z = axis;
xf.rotation.y = b3Perp(axis);
xf.rotation.x = b3Cross(xf.rotation.y, axis);
xf.position = anchor;
B3_ASSERT(b3Length(axis) > B3_EPSILON);
B3_ASSERT(lowerAngle <= upperAngle);
b3Mat33 rotation;
rotation.z = axis;
rotation.y = b3Perp(axis);
rotation.x = b3Cross(rotation.y, axis);
b3Quat q = b3ConvertRotToQuat(rotation);
float32 len = q.Normalize();
B3_ASSERT(len > B3_EPSILON);
B3_ASSERT(b3Length(q) > B3_EPSILON);
b3Quat qA = bA->GetOrientation();
b3Quat qB = bB->GetOrientation();
bodyA = bA;
bodyB = bB;
localFrameA = bodyA->GetLocalFrame(xf);
localFrameB = bodyB->GetLocalFrame(xf);
localAnchorA = bA->GetLocalPoint(anchor);
localRotationA = b3Conjugate(qA) * q;
localAnchorB = bB->GetLocalPoint(anchor);
localRotationB = b3Conjugate(qB) * q;
referenceRotation = b3Conjugate(qA * localRotationA) * qB * localRotationB;
lowerAngle = lower;
upperAngle = upper;
B3_ASSERT(lowerAngle <= upperAngle);
}
b3RevoluteJoint::b3RevoluteJoint(const b3RevoluteJointDef* def)
{
m_type = e_revoluteJoint;
m_localFrameA = def->localFrameA;
m_localFrameB = def->localFrameB;
m_enableLimit = def->enableLimit;
m_lowerAngle = def->lowerAngle;
m_upperAngle = def->upperAngle;
B3_ASSERT(m_lowerAngle <= m_upperAngle);
m_referenceRotation = def->referenceRotation;
m_localAnchorA = def->localAnchorA;
m_localRotationA = def->localRotationA;
m_localAnchorB = def->localAnchorB;
m_localRotationB = def->localRotationB;
m_enableMotor = def->enableMotor;
m_motorSpeed = def->motorSpeed;
m_maxMotorTorque = def->maxMotorTorque;
m_impulse[0] = 0.0f;
m_impulse[1] = 0.0f;
m_impulse[2] = 0.0f;
m_impulse[3] = 0.0f;
m_impulse[4] = 0.0f;
m_nA.SetZero();
m_nB.SetZero();
m_enableLimit = def->enableLimit;
m_lowerAngle = def->lowerAngle;
m_upperAngle = def->upperAngle;
B3_ASSERT(m_lowerAngle <= m_upperAngle);
m_motorImpulse = 0.0f;
m_limitState = e_inactiveLimit;
m_limitImpulse = 0.0f;
m_limitAxis.SetZero();
m_impulse.SetZero();
m_axisImpulse.SetZero();
}
void b3RevoluteJoint::InitializeConstraints(const b3SolverData* data)
@ -109,31 +254,38 @@ void b3RevoluteJoint::InitializeConstraints(const b3SolverData* data)
float32 mB = m_mB;
b3Mat33 iB = m_iB;
b3Transform xfA = m_bodyA->GetWorldFrame(m_localFrameA);
b3Vec3 u1 = xfA.rotation.x;
b3Vec3 v1 = xfA.rotation.y;
b3Vec3 w1 = xfA.rotation.z;
b3Transform xfB = m_bodyB->GetWorldFrame(m_localFrameB);
b3Vec3 u2 = xfB.rotation.x;
b3Vec3 v2 = xfB.rotation.y;
b3Vec3 w2 = xfB.rotation.z;
// Joint rotation
b3Quat fA = qA * m_localRotationA;
b3Quat fB = qB * m_localRotationB;
b3Quat q = b3Conjugate(m_referenceRotation) * b3Conjugate(fA) * fB;
// Add motor constraint.
if (m_enableMotor || m_enableLimit)
{
m_limitAxis = w1;
float32 mass = b3Dot(m_limitAxis, (iA + iB) * m_limitAxis);
m_motorMass = mass > 0.0f ? 1.0f / mass : 0.0f;
b3Vec4 P_hinge_limit = P_hinge_limit_mat(q);
b3Mat44 G1 = -0.5f * iQ_mat(b3Conjugate(fA)) * iP_mat(fB);
b3Mat44 G2 = 0.5f * iQ_mat(b3Conjugate(fA)) * iP_mat(fB);
b3Vec3 J1 = P_hinge_limit * G1 * PT;
b3Vec3 J2 = P_hinge_limit * G2 * PT;
b3Vec3 J1T = J1;
b3Vec3 J2T = J2;
m_motor_J1 = J1;
m_motor_J2 = J2;
float32 K = J1 * iA * J1T + J2 * iB * J2T;
m_motorMass = K > 0.0f ? 1.0f / K : 0.0f;
}
// Add limit constraint.
if (m_enableLimit)
{
float32 cosine = b3Dot(u2, u1);
float32 sine = b3Dot(u2, v1);
float32 angle = atan2(sine, cosine);
// Compute joint angle
float32 angle = 2.0f * atan2(q.z, q.w);
if (b3Abs(m_upperAngle - m_lowerAngle) < 2.0f * B3_ANGULAR_SLOP)
{
if (m_limitState != e_equalLimits)
@ -169,77 +321,39 @@ void b3RevoluteJoint::InitializeConstraints(const b3SolverData* data)
m_limitState = e_inactiveLimit;
}
// Add point-to-point and axes-to-axes constraints.
// Add point-to-point constraints.
{
m_rA = b3Mul(qA, m_localFrameA.position - m_localCenterA);
m_rB = b3Mul(qB, m_localFrameB.position - m_localCenterB);
m_rA = b3Mul(qA, m_localAnchorA - m_localCenterA);
m_rB = b3Mul(qB, m_localAnchorB - m_localCenterB);
m_nA = b3Cross(u2, w1);
m_nB = b3Cross(v2, w1);
// Compute effective mass matrix.
b3Vec3 rA = m_rA, rB = m_rB;
b3Vec3 nA = m_nA, nB = m_nB;
b3Mat33 M = b3Diagonal(mA + mB);
b3Mat33 I = iA + iB;
b3Mat33 RA = b3Skew(rA);
b3Mat33 RA = b3Skew(m_rA);
b3Mat33 RAT = b3Transpose(RA);
b3Mat33 RB = b3Skew(rB);
b3Mat33 RB = b3Skew(m_rB);
b3Mat33 RBT = b3Transpose(RB);
b3Mat33 M = b3Diagonal(mA + mB);
// Identities and properties used:
// 1x3 * 3x3 = transpose(3x3)*1x3
// 1x3 * 3x1 = dot(1x3, 3x1)
// K = transpose(K) (SPD)
b3Mat33 e11 = M + RA * iA * RAT + RB * iB * RBT;
b3Vec3 e21 = b3Cross(rA, iA * -nA) + b3Cross(-rB, iB * nA);
b3Vec3 e31 = b3Cross(rA, iA * -nB) + b3Cross(-rB, iB * nB);
m_mass = M + RA * iA * RAT + RB * iB * RBT;
}
// Add hinge constraints.
{
b3Mat44 G1 = -0.5f * iQ_mat(b3Conjugate(fA)) * iP_mat(fB);
b3Mat44 G2 = 0.5f * iQ_mat(b3Conjugate(fA)) * iP_mat(fB);
b3Mat23 J1 = P_hinge * G1 * PT;
b3Mat23 J2 = P_hinge * G2 * PT;
b3Vec3 e12 = e21;
float32 e22 = b3Dot(nA, I * nA);
float32 e32 = b3Dot(nA, I * nB);
b3Mat32 J1T = b3Transpose(J1);
b3Mat32 J2T = b3Transpose(J2);
b3Vec3 e13 = e31;
float32 e23 = e32;
float32 e33 = b3Dot(nB, I * nB);
m_J1 = J1;
m_J2 = J2;
b3Mat<5, 5> K;
m_J1T = J1T;
m_J2T = J2T;
K(0, 0) = e11(0, 0);
K(1, 0) = e11(1, 0);
K(2, 0) = e11(2, 0);
K(0, 1) = e11(0, 1);
K(1, 1) = e11(1, 1);
K(2, 1) = e11(2, 1);
K(0, 2) = e11(0, 2);
K(1, 2) = e11(1, 2);
K(2, 2) = e11(2, 2);
K(3, 0) = e21.x;
K(3, 1) = e21.y;
K(3, 2) = e21.z;
K(4, 0) = e31.x;
K(4, 1) = e31.y;
K(4, 2) = e31.z;
K(0, 3) = e12.x;
K(1, 3) = e12.y;
K(2, 3) = e12.z;
K(0, 4) = e13.x;
K(1, 4) = e13.y;
K(2, 4) = e13.z;
K(3, 3) = e22;
K(3, 4) = e23;
K(4, 3) = e32;
K(4, 4) = e33;
m_mass = K;
b3Mat22 K = J1 * iA * J1T + J2 * iB * J2T;
m_K = b3Inverse(K);
}
}
@ -252,27 +366,36 @@ void b3RevoluteJoint::WarmStart(const b3SolverData* data)
if (m_enableMotor && m_limitState != e_equalLimits)
{
b3Vec3 P = m_motorImpulse * m_limitAxis;
wA -= m_iA * P;
wB += m_iB * P;
b3Vec3 P1 = m_motor_J1 * m_motorImpulse;
b3Vec3 P2 = m_motor_J2 * m_motorImpulse;
wA += m_iA * P1;
wB += m_iB * P2;
}
if (m_enableLimit && m_limitState != e_inactiveLimit)
{
b3Vec3 P = m_limitImpulse * m_limitAxis;
wA -= m_iA * P;
wB += m_iB * P;
b3Vec3 P1 = m_motor_J1 * m_limitImpulse;
b3Vec3 P2 = m_motor_J2 * m_limitImpulse;
wA += m_iA * P1;
wB += m_iB * P2;
}
{
b3Vec3 P1(m_impulse[0], m_impulse[1], m_impulse[2]);
b3Vec3 P2 = m_impulse[3] * m_nA + m_impulse[4] * m_nB;
vA -= m_mA * m_impulse;
wA -= m_iA * b3Cross(m_rA, m_impulse);
vA -= m_mA * P1;
wA -= m_iA * (b3Cross(m_rA, P1) + P2);
vB += m_mB * m_impulse;
wB += m_iB * b3Cross(m_rB, m_impulse);
}
vB += m_mB * P1;
wB += m_iB * (b3Cross(m_rB, P1) + P2);
{
b3Vec3 P1 = m_J1T * m_axisImpulse;
b3Vec3 P2 = m_J2T * m_axisImpulse;
wA += m_iA * P1;
wB += m_iB * P2;
}
data->velocities[m_indexA].v = vA;
@ -296,23 +419,25 @@ void b3RevoluteJoint::SolveVelocityConstraints(const b3SolverData* data)
// Solve motor constraint.
if (m_enableMotor && m_limitState != e_equalLimits)
{
float32 Cdot = b3Dot(m_limitAxis, wB - wA) - m_motorSpeed;
float32 dw = m_motor_J1 * wA + m_motor_J2 * wB;
float32 Cdot = dw - m_motorSpeed;
float32 impulse = -m_motorMass * Cdot;
float32 oldImpulse = m_motorImpulse;
float32 maxImpulse = data->dt * m_maxMotorTorque;
m_motorImpulse = b3Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
b3Vec3 P = impulse * m_limitAxis;
b3Vec3 P1 = m_motor_J1 * impulse;
b3Vec3 P2 = m_motor_J2 * impulse;
wA -= iA * P;
wB += iB * P;
wA += iA * P1;
wB += iB * P2;
}
// Solve limit constraint.
if (m_enableLimit && m_limitState != e_inactiveLimit)
{
float32 Cdot = b3Dot(m_limitAxis, wB - wA);
float32 Cdot = m_motor_J1 * wA + m_motor_J2 * wB;
float32 impulse = -m_motorMass * Cdot;
if (m_limitState == e_equalLimits)
@ -332,49 +457,39 @@ void b3RevoluteJoint::SolveVelocityConstraints(const b3SolverData* data)
impulse = m_limitImpulse - oldImpulse;
}
b3Vec3 P = impulse * m_limitAxis;
b3Vec3 P1 = m_motor_J1 * impulse;
b3Vec3 P2 = m_motor_J2 * impulse;
wA -= iA * P;
wB += iB * P;
wA += iA * P1;
wB += iB * P2;
}
// Solve point-to-point and axes-to-axes constraint.
// Solve point-to-point constraints.
{
b3Vec3 rA = m_rA;
b3Vec3 rB = m_rB;
b3Vec3 Cdot1 = vB + b3Cross(wB, rB) - vA - b3Cross(wA, rA);
b3Vec3 Cdot = vB + b3Cross(wB, m_rB) - vA - b3Cross(wA, m_rA);
b3Vec3 impulse = m_mass.Solve(-Cdot);
b3Vec3 dw = wB - wA;
b3Vec3 nA = m_nA;
float32 Cdot2 = b3Dot(nA, dw);
m_impulse += impulse;
b3Vec3 nB = m_nB;
float32 Cdot3 = b3Dot(nB, dw);
vA -= m_mA * impulse;
wA -= m_iA * b3Cross(m_rA, impulse);
b3Vec<5> Cdot;
Cdot[0] = Cdot1.x;
Cdot[1] = Cdot1.y;
Cdot[2] = Cdot1.z;
Cdot[3] = Cdot2;
Cdot[4] = Cdot3;
vB += m_mB * impulse;
wB += m_iB * b3Cross(m_rB, impulse);
}
// Copy the matrix so it can be destroyed in the linear solver.
b3Mat<5, 5> mass = m_mass;
b3Vec<5> impulse = -Cdot;
if (b3Solve(impulse.e, mass.e, 5))
{
m_impulse += impulse;
// Solve axes-to-axes constraint.
{
b3Vec2 Cdot = m_J1 * wA + m_J2 * wB;
b3Vec2 impulse = m_K * -Cdot;
b3Vec3 P1(impulse[0], impulse[1], impulse[2]);
b3Vec3 P2 = impulse[3] * nA + impulse[4] * nB;
m_axisImpulse += impulse;
vA -= mA * P1;
wA -= iA * (b3Cross(rA, P1) + P2);
b3Vec3 P1 = m_J1T * impulse;
b3Vec3 P2 = m_J2T * impulse;
vB += mB * P1;
wB += iB * (b3Cross(rB, P1) + P2);
}
wA += m_iA * P1;
wB += m_iB * P2;
}
data->velocities[m_indexA].v = vA;
@ -397,25 +512,31 @@ bool b3RevoluteJoint::SolvePositionConstraints(const b3SolverData* data)
// Solve limit constraint.
float32 limitError = 0.0f;
if (m_enableLimit)
{
b3Vec3 limitAxis = b3Mul(qA, m_localFrameA.rotation.z);
// Compute fresh effective mass.
float32 mass = b3Dot((iA + iB) * limitAxis, limitAxis);
float32 limitMass = mass > 0.0f ? 1.0f / mass : 0.0f;
b3Quat fA = qA * m_localRotationA;
b3Quat fB = qB * m_localRotationB;
b3Quat q = b3Conjugate(m_referenceRotation) * b3Conjugate(fA) * fB;
// Compute joint angle.
b3Vec3 u1 = b3Mul(qA, m_localFrameA.rotation.x);
b3Vec3 v1 = b3Mul(qA, m_localFrameA.rotation.y);
b3Vec3 u2 = b3Mul(qB, m_localFrameB.rotation.x);
float32 cosine = b3Dot(u2, u1);
float32 sine = b3Dot(u2, v1);
float32 angle = atan2(sine, cosine);
b3Vec4 P_hinge = P_hinge_limit_mat(q);
b3Mat44 G1 = -0.5f * iQ_mat(b3Conjugate(fA)) * iP_mat(fB);
b3Mat44 G2 = 0.5f * iQ_mat(b3Conjugate(fA)) * iP_mat(fB);
b3Vec3 J1 = P_hinge * G1 * PT;
b3Vec3 J2 = P_hinge * G2 * PT;
b3Vec3 J1T = J1;
b3Vec3 J2T = J2;
float32 K = J1 * iA * J1T + J2 * iB * J2T;
float32 limitMass = K > 0.0f ? 1.0f / K : 0.0f;
float32 limitImpulse = 0.0f;
float32 angle = 2.0f * atan2(q.z, q.w);
if (b3Abs(m_upperAngle - m_lowerAngle) < 2.0f * B3_ANGULAR_SLOP)
{
float32 C = angle - m_lowerAngle;
@ -444,114 +565,80 @@ bool b3RevoluteJoint::SolvePositionConstraints(const b3SolverData* data)
limitImpulse = -C * limitMass;
}
b3Vec3 P = limitImpulse * limitAxis;
b3Vec3 P1 = J1T * limitImpulse;
b3Vec3 P2 = J2T * limitImpulse;
qA -= b3Derivative(qA, iA * P);
qA += b3Derivative(qA, iA * P1);
qA.Normalize();
qB += b3Derivative(qB, iB * P);
qB += b3Derivative(qB, iB * P2);
qB.Normalize();
}
// Solve point-to-point and axes-to-axes constraints.
// Solve point-to-point constraints.
float32 linearError = 0.0f;
float32 angularError1 = 0.0f;
float32 angularError2 = 0.0f;
{
b3Vec3 rA = b3Mul(qA, m_localFrameA.position - m_localCenterA);
b3Vec3 rB = b3Mul(qB, m_localFrameB.position - m_localCenterB);
b3Vec3 C1 = xB + rB - xA - rA;
linearError = b3Length(C1);
b3Vec3 rA = b3Mul(qA, m_localAnchorA - m_localCenterA);
b3Vec3 rB = b3Mul(qB, m_localAnchorB - m_localCenterB);
b3Vec3 w1 = b3Mul(qA, m_localFrameA.rotation.z);
b3Vec3 C = xB + rB - xA - rA;
b3Vec3 u2 = b3Mul(qB, m_localFrameB.rotation.x);
float32 C2 = b3Dot(u2, w1);
angularError1 = b3Abs(C2);
b3Vec3 v2 = b3Mul(qB, m_localFrameB.rotation.y);
float32 C3 = b3Dot(v2, w1);
angularError2 = b3Abs(C3);
b3Vec<5> C;
C[0] = C1.x;
C[1] = C1.y;
C[2] = C1.z;
C[3] = C2;
C[4] = C3;
// Compute effective mass matrix.
b3Vec3 nA = b3Cross(u2, w1);
b3Vec3 nB = b3Cross(v2, w1);
linearError += b3Length(C);
// Compute effective mass
b3Mat33 M = b3Diagonal(mA + mB);
b3Mat33 RA = b3Skew(rA);
b3Mat33 RAT = b3Transpose(RA);
b3Mat33 RB = b3Skew(rB);
b3Mat33 RBT = b3Transpose(RB);
b3Mat33 M = b3Diagonal(mA + mB);
b3Mat33 I = iA + iB;
b3Mat33 e11 = M + RA * iA * RAT + RB * iB * RBT;
b3Vec3 e21 = b3Cross(rA, iA * -nA) + b3Cross(-rB, iB * nA);
b3Vec3 e31 = b3Cross(rA, iA * -nB) + b3Cross(-rB, iB * nB);
b3Mat33 mass = M + RA * iA * RAT + RB * iB * RBT;
b3Vec3 impulse = mass.Solve(-C);
xA -= mA * impulse;
qA -= b3Derivative(qA, iA * b3Cross(rA, impulse));
qA.Normalize();
xB += mB * impulse;
qB += b3Derivative(qB, iB * b3Cross(rB, impulse));
qB.Normalize();
}
// Solve hinge constraints.
float32 angularError = 0.0f;
{
b3Quat fA = m_referenceRotation * qA * m_localRotationA;
b3Quat fB = m_referenceRotation * qB * m_localRotationB;
b3Quat q = b3Conjugate(fA) * fB;
b3Vec2 C = P_hinge * q_to_v(q);
b3Vec3 e12 = e21;
float32 e22 = b3Dot(nA, I * nA);
float32 e32 = b3Dot(nA, I * nB);
angularError += b3Length(C);
// Compute effective mass
b3Mat44 G1 = -0.5f * iQ_mat(b3Conjugate(fA)) * iP_mat(fB);
b3Mat44 G2 = 0.5f * iQ_mat(b3Conjugate(fA)) * iP_mat(fB);
b3Vec3 e13 = e31;
float32 e23 = e32;
float32 e33 = b3Dot(nB, I * nB);
b3Mat23 J1 = P_hinge * G1 * PT;
b3Mat23 J2 = P_hinge * G2 * PT;
b3Mat32 J1T = b3Transpose(J1);
b3Mat32 J2T = b3Transpose(J2);
b3Mat22 mass = J1 * iA * J1T + J2 * iB * J2T;
b3Vec2 impulse = mass.Solve(-C);
b3Mat<5, 5> K;
b3Vec3 P1 = J1T * impulse;
b3Vec3 P2 = J2T * impulse;
K(0, 0) = e11(0, 0);
K(1, 0) = e11(1, 0);
K(2, 0) = e11(2, 0);
qA += b3Derivative(qA, iA * P1);
qA.Normalize();
K(0, 1) = e11(0, 1);
K(1, 1) = e11(1, 1);
K(2, 1) = e11(2, 1);
K(0, 2) = e11(0, 2);
K(1, 2) = e11(1, 2);
K(2, 2) = e11(2, 2);
K(3, 0) = e21.x;
K(3, 1) = e21.y;
K(3, 2) = e21.z;
K(4, 0) = e31.x;
K(4, 1) = e31.y;
K(4, 2) = e31.z;
K(0, 3) = e12.x;
K(1, 3) = e12.y;
K(2, 3) = e12.z;
K(0, 4) = e13.x;
K(1, 4) = e13.y;
K(2, 4) = e13.z;
K(3, 3) = e22;
K(3, 4) = e23;
K(4, 3) = e32;
K(4, 4) = e33;
b3Vec<5> impulse = -C;
if (b3Solve(impulse.e, K.e, 5))
{
b3Vec3 P1(impulse[0], impulse[1], impulse[2]);
b3Vec3 P2 = impulse[3] * nA + impulse[4] * nB;
xA -= mA * P1;
qA -= b3Derivative(qA, iA * (b3Cross(rA, P1) + P2));
qA.Normalize();
xB += mB * P1;
qB += b3Derivative(qB, iB * (b3Cross(rB, P1) + P2));
qB.Normalize();
}
qB += b3Derivative(qB, iB * P2);
qB.Normalize();
}
data->positions[m_indexA].x = xA;
@ -559,30 +646,31 @@ bool b3RevoluteJoint::SolvePositionConstraints(const b3SolverData* data)
data->positions[m_indexB].x = xB;
data->positions[m_indexB].q = qB;
return linearError <= B3_LINEAR_SLOP &&
angularError1 <= B3_ANGULAR_SLOP &&
angularError2 <= B3_ANGULAR_SLOP &&
return linearError <= B3_LINEAR_SLOP &&
angularError <= B3_ANGULAR_SLOP &&
limitError <= B3_ANGULAR_SLOP;
}
b3Transform b3RevoluteJoint::GetFrameA() const
{
return GetBodyA()->GetWorldFrame(m_localFrameA);
b3Transform xf(m_localAnchorA, m_localRotationA);
return GetBodyA()->GetWorldFrame(xf);
}
b3Transform b3RevoluteJoint::GetFrameB() const
{
return GetBodyB()->GetWorldFrame(m_localFrameB);
b3Transform xf(m_localAnchorB, m_localRotationB);
return GetBodyB()->GetWorldFrame(xf);
}
const b3Transform& b3RevoluteJoint::GetLocalFrameA() const
b3Transform b3RevoluteJoint::GetLocalFrameA() const
{
return m_localFrameA;
return b3Transform(m_localAnchorA, m_localRotationA);
}
const b3Transform& b3RevoluteJoint::GetLocalFrameB() const
b3Transform b3RevoluteJoint::GetLocalFrameB() const
{
return m_localFrameB;
return b3Transform(m_localAnchorB, m_localRotationB);
}
bool b3RevoluteJoint::IsLimitEnabled() const

161
src/bounce/dynamics/joints/weld_joint.cpp
View File

@ -19,7 +19,76 @@
#include <bounce/dynamics/joints/weld_joint.h>
#include <bounce/dynamics/body.h>
#include <bounce/common/draw.h>
#include <bounce/common/math/mat.h>
/*
P = [0 1 0 0]
[0 0 1 0]
[0 0 0 1]
q = conj(q1) * q2
C = P * q
C' = P * q'
q' =
conj(q1)' * q2 + conj(q1) * q2' =
conj(q2') * q2 + conj(q1) * q2'
J1 = -0.5 * Q(conj(q1)) * P(q2)
J2 = 0.5 * Q(conj(q1)) * P(q2)
J1 = P * J1 * P^T
J2 = P * J2 * P^T
*/
static b3Mat44 iQ_mat(const b3Quat& q)
{
b3Mat44 Q;
Q.x = b3Vec4(q.w, q.x, q.y, q.z);
Q.y = b3Vec4(-q.x, q.w, q.z, -q.y);
Q.z = b3Vec4(-q.y, -q.z, q.w, q.x);
Q.w = b3Vec4(-q.z, q.y, -q.x, q.w);
return Q;
}
static b3Mat44 iP_mat(const b3Quat& q)
{
b3Mat44 P;
P.x = b3Vec4(q.w, q.x, q.y, q.z);
P.y = b3Vec4(-q.x, q.w, -q.z, q.y);
P.z = b3Vec4(-q.y, q.z, q.w, -q.x);
P.w = b3Vec4(-q.z, -q.y, q.x, q.w);
return P;
}
static b3Mat34 P_mat()
{
b3Mat34 P;
P.x = b3Vec3(0.0f, 0.0f, 0.0f);
P.y = b3Vec3(1.0f, 0.0f, 0.0f);
P.z = b3Vec3(0.0f, 1.0f, 0.0f);
P.w = b3Vec3(0.0f, 0.0f, 1.0f);
return P;
}
static b3Mat34 P_lock_mat()
{
b3Mat34 P;
P.x = b3Vec3(0.0f, 0.0f, 0.0f);
P.y = b3Vec3(1.0f, 0.0f, 0.0f);
P.z = b3Vec3(0.0f, 1.0f, 0.0f);
P.w = b3Vec3(0.0f, 0.0f, 1.0f);
return P;
}
static b3Vec4 q_to_v(const b3Quat& q)
{
return b3Vec4(q.w, q.x, q.y, q.z);
}
static b3Mat34 P = P_mat();
static b3Mat43 PT = b3Transpose(P);
static b3Mat34 P_lock = P_lock_mat();
void b3WeldJointDef::Initialize(b3Body* bA, b3Body* bB, const b3Vec3& anchor)
{
@ -31,7 +100,7 @@ void b3WeldJointDef::Initialize(b3Body* bA, b3Body* bB, const b3Vec3& anchor)
b3Quat qA = bodyA->GetOrientation();
b3Quat qB = bodyB->GetOrientation();
relativeRotation = b3Conjugate(qA) * qB;
referenceRotation = b3Conjugate(qA) * qB;
}
b3WeldJoint::b3WeldJoint(const b3WeldJointDef* def)
@ -39,7 +108,7 @@ b3WeldJoint::b3WeldJoint(const b3WeldJointDef* def)
m_type = e_weldJoint;
m_localAnchorA = def->localAnchorA;
m_localAnchorB = def->localAnchorB;
m_dq0 = def->relativeRotation;
m_referenceRotation = def->referenceRotation;
m_impulse.SetZero();
m_axisImpulse.SetZero();
}
@ -74,6 +143,18 @@ void b3WeldJoint::InitializeConstraints(const b3SolverData* data)
m_mass = M + RA * m_iA * RAT + RB * m_iB * RBT;
}
{
b3Quat dq = b3Conjugate(m_referenceRotation) * b3Conjugate(qA) * qB;
m_J1 = -0.5f * P_lock * iQ_mat(b3Conjugate(qA)) * iP_mat(qB) * PT;
m_J2 = 0.5f * P_lock * iQ_mat(b3Conjugate(qA)) * iP_mat(qB) * PT;
m_J1T = b3Transpose(m_J1);
m_J2T = b3Transpose(m_J2);
m_K = m_J1 * m_iA * m_J1T + m_J2 * m_iB * m_J2T;
}
}
void b3WeldJoint::WarmStart(const b3SolverData* data)
@ -83,11 +164,21 @@ void b3WeldJoint::WarmStart(const b3SolverData* data)
b3Vec3 vB = data->velocities[m_indexB].v;
b3Vec3 wB = data->velocities[m_indexB].w;
vA -= m_mA * m_impulse;
wA -= m_iA * (b3Cross(m_rA, m_impulse) + m_axisImpulse);
{
vA -= m_mA * m_impulse;
wA -= m_iA * b3Cross(m_rA, m_impulse);
vB += m_mB * m_impulse;
wB += m_iB * (b3Cross(m_rB, m_impulse) + m_axisImpulse);
vB += m_mB * m_impulse;
wB += m_iB * b3Cross(m_rB, m_impulse);
}
{
b3Vec3 P1 = m_J1T * m_axisImpulse;
b3Vec3 P2 = m_J2T * m_axisImpulse;
wA += m_iA * P1;
wB += m_iB * P2;
}
data->velocities[m_indexA].v = vA;
data->velocities[m_indexA].w = wA;
@ -119,13 +210,16 @@ void b3WeldJoint::SolveVelocityConstraints(const b3SolverData* data)
}
{
b3Vec3 Cdot = wB - wA;
b3Vec3 impulse = -Cdot;
b3Vec3 Cdot = m_J1 * wA + m_J2 * wB;
b3Vec3 impulse = m_K.Solve(-Cdot);
m_axisImpulse += impulse;
wA -= m_iA * impulse;
wB += m_iB * impulse;
b3Vec3 P1 = m_J1T * impulse;
b3Vec3 P2 = m_J2T * impulse;
wA += m_iA * P1;
wB += m_iB * P2;
}
data->velocities[m_indexA].v = vA;
@ -172,34 +266,33 @@ bool b3WeldJoint::SolvePositionConstraints(const b3SolverData* data)
float32 angularError = 0.0f;
{
// qC = inv(dq0) * dq = q_identity
// qB - qA - q_rel0
// Small angle approximation
// C = 2 * sin(theta / 2) = theta
b3Quat dq = b3Conjugate(m_dq0) * b3Conjugate(qA) * qB;
if (dq.w < 0.0f)
{
dq.x = -dq.x;
dq.y = -dq.y;
dq.z = -dq.z;
}
b3Vec3 axis(dq.x, dq.y, dq.z);
axis = b3Mul(qA, axis);
{
b3Quat dq = b3Conjugate(m_referenceRotation) * b3Conjugate(qA) * qB;
b3Vec4 dq_v = q_to_v(dq);
b3Vec3 C = 2.0f * axis;
b3Vec3 C = P * dq_v;
b3Vec3 P = -C;
qA -= b3Derivative(qA, m_iA * P);
angularError += b3Length(C);
b3Mat33 J1 = -0.5f * P_lock * iQ_mat(b3Conjugate(qA)) * iP_mat(qB) * PT;
b3Mat33 J2 = 0.5f * P_lock * iQ_mat(b3Conjugate(qA)) * iP_mat(qB) * PT;
b3Mat33 J1T = b3Transpose(J1);
b3Mat33 J2T = b3Transpose(J2);
b3Mat33 mass = J1 * m_iA * J1T + J2 * m_iB * J2T;
b3Vec3 impulse = mass.Solve(-C);
b3Vec3 P1 = J1T * impulse;
b3Vec3 P2 = J2T * impulse;
qA += b3Derivative(qA, m_iA * P1);
qA.Normalize();
qB += b3Derivative(qB, m_iB * P);
qB += b3Derivative(qB, m_iB * P2);
qB.Normalize();
angularError += b3Length(C);
}
data->positions[m_indexA].x = xA;
data->positions[m_indexA].q = qA;
data->positions[m_indexB].x = xB;

13
src/bounce/dynamics/shapes/capsule_shape.cpp
View File

@ -45,7 +45,6 @@ void b3CapsuleShape::ComputeMass(b3MassData* massData, float32 density) const
b3Vec3 d = B - A;
float32 h = b3Length(d);
B3_ASSERT(h > B3_LINEAR_SLOP);
float32 h2 = h * h;
float32 r = m_radius;
@ -116,10 +115,14 @@ void b3CapsuleShape::ComputeMass(b3MassData* massData, float32 density) const
// Center of mass doesn't change
B3_ASSERT(h > B3_LINEAR_SLOP);
b3Mat33 R;
R.y = (1.0f / h) * d;
R.x = b3Perp(R.y);
R.z = b3Cross(R.y, R.x);
R.SetIdentity();
if (h > B3_LINEAR_SLOP)
{
R.y = (1.0f / h) * d;
R.x = b3Perp(R.y);
R.z = b3Cross(R.y, R.x);
}
b3Mat33 Ic = b3RotateToFrame(Ic_Capsule.I, R);
// Inertia about the center of mass

2
src/bounce/quickhull/qh_hull.cpp
View File

@ -757,7 +757,7 @@ void qhHull::Draw(b3Draw* draw) const
edge = edge->next;
} while (edge != begin);
draw->DrawSolidPolygon(n, vs.Elements(), vs.Count(), b3Color(1.0f, 1.0f, 1.0f, 0.5f));
draw->DrawSolidPolygon(n, vs.Begin(), vs.Count(), b3Color(1.0f, 1.0f, 1.0f, 0.5f));
qhVertex* v = face->conflictList.head;
while (v)