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

remove unecessary gjk duplication

Browse Source
This commit is contained in:
Irlan 2017-03-26 13:27:02 -03:00
parent 153abfb2fe
commit a683052e4c
13 changed files with 327 additions and 444 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
examples/testbed/framework/main.cpp
View File

@ -229,11 +229,11 @@ static bool GetTestName(void*, int idx, const char** name)
static void Interface()
{
ImGui::SetNextWindowPos(ImVec2(g_camera.m_width, 0.0f));
ImGui::SetNextWindowPos(ImVec2(g_camera.m_width - 250.0f, 0.0f));
ImGui::SetNextWindowSize(ImVec2(250.0f, g_camera.m_height));
ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 0.0f);
ImGui::Begin("Controls", NULL, ImVec2(0.0f, 0.0f), 0.25f, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoCollapse);
ImGui::Begin("Controls", NULL, ImVec2(0.0f, 0.0f), 0.25f, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize);
ImGui::PushItemWidth(-1.0f);
@ -384,10 +384,10 @@ static void Run()
{
int width, height;
glfwGetWindowSize(g_window, &width, &height);
g_camera.m_width = float32(width) - 250.0f;
g_camera.m_width = float32(width);
g_camera.m_height = float32(height);
glViewport(0, 0, width - 250, height);
glViewport(0, 0, width, height);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
ImGui_ImplGlfwGL3_NewFrame();

1
include/bounce/bounce.h
View File

@ -28,7 +28,6 @@
#include <bounce/common/math/math.h>
#include <bounce/collision/gjk/gjk.h>
#include <bounce/collision/gjk/gjk_cache.h>
#include <bounce/collision/sat/sat.h>
#include <bounce/collision/collision.h>
#include <bounce/collision/broad_phase.h>

43
include/bounce/collision/gjk/gjk.h
View File

@ -19,7 +19,9 @@
#ifndef B3_GJK_H
#define B3_GJK_H
#include <bounce/common/geometry.h>
#include <bounce/common/math/transform.h>
///////////////////////////////////////////////////////////////////////////////////////////////////
class b3GJKProxy;
struct b3SimplexCache;
@ -73,4 +75,43 @@ struct b3GJKOutput
b3GJKOutput b3GJK(const b3Transform& xf1, const b3GJKProxy& proxy1,
const b3Transform& xf2, const b3GJKProxy& proxy2);
///////////////////////////////////////////////////////////////////////////////////////////////////
// A cached simplex is used to improve the performance
// of the GJK when called more than once.
// Make sure to set cache.count to zero before
// passing this structure as an argument to GJK when called
// for the first time.
struct b3SimplexCache
{
float32 metric; // distance or area or volume
u32 iterations; // number of GJK iterations
u16 count; // number of support vertices
u8 index1[4]; // support vertices on proxy 1
u8 index2[4]; // support vertices on proxy 2
};
// A feature pair contains the vertices of the features associated
// with the closest points.
struct b3GJKFeaturePair
{
u32 index1[3]; // vertices on proxy 1
u32 count1; // number of vertices on proxy 1
u32 index2[3]; // vertices on proxy 2
u32 count2; // number of vertices on proxy 2
};
// Identify the vertices of the features that the closest points between two
// GJK proxies are contained on given a cached simplex.
// The GJK must have been called using the pair of proxies and
// cache.count must be < 4, that is, the proxies must not be overlapping.
b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache);
// Find the closest points and distance between two proxies.
// Assumes a simplex is given for increasing the performance of
// the algorithm when called more than once.
b3GJKOutput b3GJK(const b3Transform& xf1, const b3GJKProxy& proxy1,
const b3Transform& xf2, const b3GJKProxy& proxy2,
bool applyRadius, b3SimplexCache* cache);
#endif

61
include/bounce/collision/gjk/gjk_cache.h
View File

@ -1,61 +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.
*/
#ifndef B3_GJK_CACHE_H
#define B3_GJK_CACHE_H
#include <bounce/collision/gjk/gjk.h>
// A cached simplex is used to improve the performance
// of the GJK when called more than once.
// Make sure to set cache.count to zero before
// passing this structure as an argument to GJK when called
// for the first time.
struct b3SimplexCache
{
float32 metric; // distance or area or volume
u32 iterations; // number of GJK iterations
u16 count; // number of support vertices
u8 index1[4]; // support vertices on proxy 1
u8 index2[4]; // support vertices on proxy 2
};
// Find the closest points and distance between two proxies.
// Assumes a simplex is given for increasing the performance of
// the algorithm when called more than once.
b3GJKOutput b3GJK(const b3Transform& xf1, const b3GJKProxy& proxy1,
const b3Transform& xf2, const b3GJKProxy& proxy2,
bool applyRadius, b3SimplexCache* cache);
// A feature pair contains the vertices of the features associated
// with the closest points.
struct b3GJKFeaturePair
{
u32 index1[3]; // vertices on proxy 1
u32 count1; // number of vertices on proxy 1
u32 index2[3]; // vertices on proxy 2
u32 count2; // number of vertices on proxy 2
};
// Identify the vertices of the features that the closest points between two
// GJK proxies are contained on given a cached simplex.
// The GJK must have been called using the pair of proxies and
// cache.count must be < 4, that is, the proxies must not be overlapping.
b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache);
#endif

115
include/bounce/common/geometry.h
View File

@ -75,74 +75,6 @@ inline float32 b3Distance(const b3Vec3& P, const b3Plane& plane)
return b3Dot(plane.normal, P) - plane.offset;
}
// Compute barycentric coordinates (u, v) for point Q to segment AB.
// The last output value is the divisor.
inline void b3Barycentric(float32 out[3],
const b3Vec3& A, const b3Vec3& B,
const b3Vec3& Q)
{
b3Vec3 AB = B - A;
b3Vec3 QA = A - Q;
b3Vec3 QB = B - Q;
//float32 divisor = b3Dot(AB, AB);
out[0] = b3Dot(QB, AB);
out[1] = -b3Dot(QA, AB);
out[2] = out[0] + out[1];
}
// Compute barycentric coordinates (u, v, w) for point Q to triangle ABC.
// The last output value is the divisor.
inline void b3Barycentric(float32 out[4],
const b3Vec3& A, const b3Vec3& B, const b3Vec3& C,
const b3Vec3& Q)
{
// RTCD, 140.
b3Vec3 AB = B - A;
b3Vec3 AC = C - A;
b3Vec3 QA = A - Q;
b3Vec3 QB = B - Q;
b3Vec3 QC = C - Q;
b3Vec3 QB_x_QC = b3Cross(QB, QC);
b3Vec3 QC_x_QA = b3Cross(QC, QA);
b3Vec3 QA_x_QB = b3Cross(QA, QB);
b3Vec3 AB_x_AC = b3Cross(AB, AC);
//float32 divisor = b3Dot(AB_x_AC, AB_x_AC);
out[0] = b3Dot(QB_x_QC, AB_x_AC);
out[1] = b3Dot(QC_x_QA, AB_x_AC);
out[2] = b3Dot(QA_x_QB, AB_x_AC);
out[3] = out[0] + out[1] + out[2];
}
// Compute barycentric coordinates (u, v, w, x) for point Q to tetrahedron ABCD.
// The last output value is the (positive) divisor.
inline void b3Barycentric(float32 out[5],
const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, const b3Vec3& D,
const b3Vec3& Q)
{
// RTCD, 48, 49.
b3Vec3 AB = B - A;
b3Vec3 AC = C - A;
b3Vec3 AD = D - A;
b3Vec3 QA = A - Q;
b3Vec3 QB = B - Q;
b3Vec3 QC = C - Q;
b3Vec3 QD = D - Q;
float32 divisor = b3Det(AB, AC, AD);
float32 sign = b3Sign(divisor);
out[0] = sign * b3Det(QB, QC, QD);
out[1] = sign * b3Det(QA, QD, QC);
out[2] = sign * b3Det(QA, QB, QD);
out[3] = sign * b3Det(QA, QC, QB);
out[4] = sign * divisor;
}
// Project a point onto a normal plane.
inline b3Vec3 b3ClosestPointOnPlane(const b3Vec3& P, const b3Plane& plane)
{
@ -150,11 +82,56 @@ inline b3Vec3 b3ClosestPointOnPlane(const b3Vec3& P, const b3Plane& plane)
return P - fraction * plane.normal;
}
// Convert a point Q from euclidean coordinates to barycentric coordinates (u, v)
// with respect to a segment AB.
// The last output value is the divisor.
inline void b3BarycentricCoordinates(float32 out[3],
const b3Vec3& A, const b3Vec3& B,
const b3Vec3& Q)
{
b3Vec3 AB = B - A;
b3Vec3 QA = A - Q;
b3Vec3 QB = B - Q;
float32 divisor = b3Dot(AB, AB);
out[0] = b3Dot(QB, AB);
out[1] = -b3Dot(QA, AB);
out[2] = divisor;
}
// Convert a point Q from euclidean coordinates to barycentric coordinates (u, v, w)
// with respect to a triangle ABC.
// The last output value is the divisor.
inline void b3BarycentricCoordinates(float32 out[4],
const b3Vec3& A, const b3Vec3& B, const b3Vec3& C,
const b3Vec3& Q)
{
b3Vec3 AB = B - A;
b3Vec3 AC = C - A;
b3Vec3 QA = A - Q;
b3Vec3 QB = B - Q;
b3Vec3 QC = C - Q;
b3Vec3 QB_x_QC = b3Cross(QB, QC);
b3Vec3 QC_x_QA = b3Cross(QC, QA);
b3Vec3 QA_x_QB = b3Cross(QA, QB);
b3Vec3 AB_x_AC = b3Cross(AB, AC);
float32 divisor = b3Dot(AB_x_AC, AB_x_AC);
out[0] = b3Dot(QB_x_QC, AB_x_AC);
out[1] = b3Dot(QC_x_QA, AB_x_AC);
out[2] = b3Dot(QA_x_QB, AB_x_AC);
out[3] = divisor;
}
// Project a point onto a segment AB.
inline b3Vec3 b3ClosestPointOnSegment(const b3Vec3& P, const b3Vec3& A, const b3Vec3& B)
{
float32 wAB[3];
b3Barycentric(wAB, A, B, P);
b3BarycentricCoordinates(wAB, A, B, P);
if (wAB[1] <= 0.0f)
{

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

@ -19,8 +19,8 @@
#ifndef B3_COLLIDE_H
#define B3_COLLIDE_H
#include <bounce/collision/gjk/gjk.h>
#include <bounce/collision/gjk/gjk_proxy.h>
#include <bounce/collision/gjk/gjk_cache.h>
#include <bounce/collision/sat/sat.h>
#include <bounce/collision/sat/sat_edge_and_hull.h>
#include <bounce/collision/sat/sat_vertex_and_hull.h>

2
include/bounce/dynamics/world.h
View File

@ -97,7 +97,7 @@ public:
// and the intersection fraction.
void RayCast(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) const;
// Perform a AABB cast with the world.
// Perform a AABB query with the world.
// The query listener will be notified when two shape AABBs are overlapping.
// If the listener returns false then the query is stopped immediately.
// Otherwise, it continues searching for new overlapping shape AABBs.

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

@ -19,11 +19,85 @@
#include <bounce/collision/gjk/gjk.h>
#include <bounce/collision/gjk/gjk_proxy.h>
///////////////////////////////////////////////////////////////////////////////////////////////////
// Implementation of the GJK (Gilbert-Johnson-Keerthi) algorithm
// using Voronoi regions and Barycentric coordinates.
u32 b3_gjkCalls = 0, b3_gjkIters = 0, b3_gjkMaxIters = 0;
// Convert a point Q from euclidean coordinates to barycentric coordinates (u, v)
// with respect to a segment AB.
// The last output value is the divisor.
static B3_FORCE_INLINE void b3Barycentric(float32 out[3],
const b3Vec3& A, const b3Vec3& B,
const b3Vec3& Q)
{
b3Vec3 AB = B - A;
b3Vec3 QA = A - Q;
b3Vec3 QB = B - Q;
//float32 divisor = b3Dot(AB, AB);
out[0] = b3Dot(QB, AB);
out[1] = -b3Dot(QA, AB);
out[2] = out[0] + out[1];
}
// Convert a point Q from euclidean coordinates to barycentric coordinates (u, v, w)
// with respect to a triangle ABC.
// The last output value is the divisor.
static B3_FORCE_INLINE void b3Barycentric(float32 out[4],
const b3Vec3& A, const b3Vec3& B, const b3Vec3& C,
const b3Vec3& Q)
{
b3Vec3 AB = B - A;
b3Vec3 AC = C - A;
b3Vec3 QA = A - Q;
b3Vec3 QB = B - Q;
b3Vec3 QC = C - Q;
b3Vec3 QB_x_QC = b3Cross(QB, QC);
b3Vec3 QC_x_QA = b3Cross(QC, QA);
b3Vec3 QA_x_QB = b3Cross(QA, QB);
b3Vec3 AB_x_AC = b3Cross(AB, AC);
//float32 divisor = b3Dot(AB_x_AC, AB_x_AC);
out[0] = b3Dot(QB_x_QC, AB_x_AC);
out[1] = b3Dot(QC_x_QA, AB_x_AC);
out[2] = b3Dot(QA_x_QB, AB_x_AC);
out[3] = out[0] + out[1] + out[2];
}
// Convert a point Q from euclidean coordinates to barycentric coordinates (u, v, w, x)
// with respect to a tetrahedron ABCD.
// The last output value is the (positive) divisor.
static B3_FORCE_INLINE void b3Barycentric(float32 out[5],
const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, const b3Vec3& D,
const b3Vec3& Q)
{
b3Vec3 AB = B - A;
b3Vec3 AC = C - A;
b3Vec3 AD = D - A;
b3Vec3 QA = A - Q;
b3Vec3 QB = B - Q;
b3Vec3 QC = C - Q;
b3Vec3 QD = D - Q;
float32 divisor = b3Det(AB, AC, AD);
float32 sign = b3Sign(divisor);
out[0] = sign * b3Det(QB, QC, QD);
out[1] = sign * b3Det(QA, QD, QC);
out[2] = sign * b3Det(QA, QB, QD);
out[3] = sign * b3Det(QA, QC, QB);
out[4] = sign * divisor;
}
b3Vec3 b3Simplex::GetSearchDirection(const b3Vec3& Q) const
{
switch (m_count)
@ -508,25 +582,17 @@ void b3Simplex::Solve4(const b3Vec3& Q)
m_vertices[3].weight = s * wABCD[3];
}
b3GJKOutput b3GJK(const b3Transform& xf1, const b3GJKProxy& proxy1, const b3Transform& xf2, const b3GJKProxy& proxy2)
///////////////////////////////////////////////////////////////////////////////////////////////////
b3GJKOutput b3GJK(const b3Transform& xf1, const b3GJKProxy& proxy1,
const b3Transform& xf2, const b3GJKProxy& proxy2,
bool applyRadius, b3SimplexCache* cache)
{
++b3_gjkCalls;
b3Simplex simplex;
// Initialize the simplex.
{
b3SimplexVertex* v = simplex.m_vertices + 0;
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->index1 = 0;
v->index2 = 0;
simplex.m_count = 1;
}
b3Simplex simplex;
simplex.ReadCache(cache, xf1, proxy1, xf2, proxy2);
// Get simplex vertices as an array.
b3SimplexVertex* vertices = simplex.m_vertices;
@ -632,18 +698,162 @@ b3GJKOutput b3GJK(const b3Transform& xf1, const b3GJKProxy& proxy1, const b3Tran
break;
}
// New vertex is needed.
// New vertex is ok and needed.
++simplex.m_count;
}
b3_gjkMaxIters = b3Max(b3_gjkMaxIters, iter);
// Prepare output.
// Prepare result.
b3GJKOutput output;
simplex.GetClosestPoints(&output.point1, &output.point2);
output.distance = b3Distance(output.point1, output.point2);
output.iterations = iter;
// Cache the simplex.
simplex.WriteCache(cache);
// Apply radius if requested.
if (applyRadius)
{
float32 r1 = proxy1.m_radius;
float32 r2 = proxy2.m_radius;
if (output.distance > r1 + r2 && output.distance > B3_EPSILON)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.distance -= r1 + r2;
b3Vec3 d = output.point2 - output.point1;
b3Vec3 normal = b3Normalize(d);
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.point1 + output.point2);
output.point1 = p;
output.point2 = p;
output.distance = 0.0f;
}
}
// Output result.
return output;
}
///////////////////////////////////////////////////////////////////////////////////////////////////
u32 b3_gjkCacheHits = 0;
// Implements b3Simplex routines for a cached simplex.
void b3Simplex::ReadCache(const b3SimplexCache* cache,
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->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;
}
// Compute the new simplex metric
// If it is substantially different than
// old metric then flush the simplex.
if (m_count > 1)
{
float32 metric1 = cache->metric;
float32 metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < B3_EPSILON)
{
// Flush
m_count = 0;
}
else
{
++b3_gjkCacheHits;
}
}
// If cache is empty or flushed choose an arbitrary simplex.
if (m_count == 0)
{
b3SimplexVertex* v = m_vertices + 0;
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->index1 = 0;
v->index2 = 0;
m_count = 1;
}
}
void b3Simplex::WriteCache(b3SimplexCache* cache) const
{
cache->metric = GetMetric();
cache->count = u16(m_count);
for (u32 i = 0; i < m_count; ++i)
{
cache->index1[i] = u8(m_vertices[i].index1);
cache->index2[i] = u8(m_vertices[i].index2);
}
}
float32 b3Simplex::GetMetric() const
{
switch (m_count)
{
case 0:
B3_ASSERT(false);
return 0.0f;
case 1:
return 0.0f;
case 2:
// Magnitude
return b3Distance(m_vertices[0].point, m_vertices[1].point);
case 3:
{
// Area
b3Vec3 E1 = m_vertices[1].point - m_vertices[0].point;
b3Vec3 E2 = m_vertices[2].point - m_vertices[0].point;
return b3Length(b3Cross(E1, E2));
}
case 4:
{
// Volume
b3Vec3 E1 = m_vertices[1].point - m_vertices[0].point;
b3Vec3 E2 = m_vertices[2].point - m_vertices[0].point;
b3Vec3 E3 = m_vertices[3].point - m_vertices[0].point;
float32 det = b3Det(E1, E2, E3);
float32 sign = b3Sign(det);
float32 volume = sign * det;
return volume;
}
default:
B3_ASSERT(false);
return 0.0f;
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////
b3GJKOutput b3GJK(const b3Transform& xf1, const b3GJKProxy& proxy1,
const b3Transform& xf2, const b3GJKProxy& proxy2)
{
b3SimplexCache cache;
cache.count = 0;
return b3GJK(xf1, proxy1, xf2, proxy2, false, &cache);
}

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

@ -1,284 +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/gjk/gjk_cache.h>
#include <bounce/collision/gjk/gjk_proxy.h>
extern u32 b3_gjkCalls, b3_gjkIters, b3_gjkMaxIters;
u32 b3_gjkCacheHits;
// Implements b3Simplex routines for a cached simplex.
void b3Simplex::ReadCache(const b3SimplexCache* cache,
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->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;
}
// Compute the new simplex metric
// If it is substantially different than
// old metric then flush the simplex.
if (m_count > 1)
{
float32 metric1 = cache->metric;
float32 metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < B3_EPSILON)
{
// Flush
m_count = 0;
}
else
{
++b3_gjkCacheHits;
}
}
// If cache is empty or flushed choose an arbitrary simplex.
if (m_count == 0)
{
b3SimplexVertex* v = m_vertices + 0;
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->index1 = 0;
v->index2 = 0;
m_count = 1;
}
}
void b3Simplex::WriteCache(b3SimplexCache* cache) const
{
cache->metric = GetMetric();
cache->count = u16(m_count);
for (u32 i = 0; i < m_count; ++i)
{
cache->index1[i] = u8(m_vertices[i].index1);
cache->index2[i] = u8(m_vertices[i].index2);
}
}
float32 b3Simplex::GetMetric() const
{
switch (m_count)
{
case 0:
B3_ASSERT(false);
return 0.0f;
case 1:
return 0.0f;
case 2:
// Magnitude
return b3Distance(m_vertices[0].point, m_vertices[1].point);
case 3:
{
// Area
b3Vec3 E1 = m_vertices[1].point - m_vertices[0].point;
b3Vec3 E2 = m_vertices[2].point - m_vertices[0].point;
return b3Length(b3Cross(E1, E2));
}
case 4:
{
// Volume
b3Vec3 E1 = m_vertices[1].point - m_vertices[0].point;
b3Vec3 E2 = m_vertices[2].point - m_vertices[0].point;
b3Vec3 E3 = m_vertices[3].point - m_vertices[0].point;
float32 det = b3Det(E1, E2, E3);
float32 sign = b3Sign(det);
float32 volume = sign * det;
return volume;
}
default:
B3_ASSERT(false);
return 0.0f;
}
}
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, 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 save1[4], save2[4];
u32 saveCount = 0;
// Last iteration squared distance for checking if we're getting close
// to the origin and prevent cycling.
float32 distSq1 = B3_MAX_FLOAT;
const b3Vec3 kOrigin(0.0f, 0.0f, 0.0f);
// Limit number of iterations to prevent cycling.
const u32 kMaxIters = 20;
// Main iteration loop.
u32 iter = 0;
while (iter < kMaxIters)
{
// Copy simplex so we can identify duplicates.
saveCount = simplex.m_count;
for (u32 i = 0; i < saveCount; ++i)
{
save1[i] = vertices[i].index1;
save2[i] = vertices[i].index2;
}
// Determine the closest point on the simplex and
// remove unused vertices.
switch (simplex.m_count)
{
case 1:
break;
case 2:
simplex.Solve2(kOrigin);
break;
case 3:
simplex.Solve3(kOrigin);
break;
case 4:
simplex.Solve4(kOrigin);
break;
default:
B3_ASSERT(false);
break;
}
// If we have 4 points, then the origin is in the corresponding tethrahedron.
if (simplex.m_count == 4)
{
break;
}
// Compute the closest point.
b3Vec3 p = simplex.GetClosestPoint();
float32 distSq2 = b3Dot(p, p);
// Ensure we're getting close to the origin.
if (distSq2 >= distSq1)
{
//break;
}
distSq1 = distSq2;
// Get search direction.
b3Vec3 d = simplex.GetSearchDirection(kOrigin);
// Ensure the search direction is non-zero.
if (b3Dot(d, d) < B3_EPSILON * B3_EPSILON)
{
break;
}
// Compute a tentative new simplex vertex using support points.
b3SimplexVertex* vertex = vertices + simplex.m_count;
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;
++b3_gjkIters;
// Check for duplicate support points.
// This is the main termination criteria.
bool duplicate = false;
for (u32 i = 0; i < saveCount; ++i)
{
if (vertex->index1 == save1[i] && vertex->index2 == save2[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.m_count;
}
b3_gjkMaxIters = b3Max(b3_gjkMaxIters, iter);
// Prepare result.
b3GJKOutput output;
simplex.GetClosestPoints(&output.point1, &output.point2);
output.distance = b3Distance(output.point1, output.point2);
output.iterations = iter;
// Cache the simplex.
simplex.WriteCache(cache);
// Apply radius if requested.
if (applyRadius)
{
float32 r1 = proxy1.m_radius;
float32 r2 = proxy2.m_radius;
if (output.distance > r1 + r2 && output.distance > B3_EPSILON)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.distance -= r1 + r2;
b3Vec3 d = output.point2 - output.point1;
b3Vec3 normal = b3Normalize(d);
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.point1 + output.point2);
output.point1 = p;
output.point2 = p;
output.distance = 0.0f;
}
}
// Output result.
return output;
}

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

@ -16,7 +16,7 @@
* 3. This notice may not be removed or altered from any source distribution.
*/
#include <bounce/collision/gjk/gjk_cache.h>
#include <bounce/collision/gjk/gjk.h>
b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
{

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

@ -84,10 +84,10 @@ static void b3RebuildEdgeContact(b3Manifold& manifold,
// Check if the closest points are still lying on the opposite segments
// using Barycentric coordinates.
float32 w2[3];
b3Barycentric(w2, P1, Q1, c2);
b3BarycentricCoordinates(w2, P1, Q1, c2);
float32 w1[3];
b3Barycentric(w1, P2, Q2, c1);
b3BarycentricCoordinates(w1, P2, Q2, c1);
if (w2[1] > 0.0f && w2[1] <= w2[2] &&
w1[1] > 0.0f && w1[1] <= w1[2])

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

@ -155,7 +155,8 @@ void b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations,
// 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
// Toss out I2 from f using local I2 (constant) and local w1
// to remove its time dependency.
b3Vec3 w2 = b3SolveGyroscopic(q, b->m_I, w, h);
b3Vec3 dw2 = w2 - w;

2
src/bounce/dynamics/shapes/mesh_shape.cpp
View File

@ -106,7 +106,7 @@ bool b3MeshShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input,
b3Vec3 q = p1 + t * d;
float32 w[4];
b3Barycentric(w, v1, v2, v3, q);
b3BarycentricCoordinates(w, v1, v2, v3, q);
if (w[0] > 0.0f && w[1] > 0.0f && w[2] > 0.0f)
{