diff --git a/examples/testbed/framework/main.cpp b/examples/testbed/framework/main.cpp index 4e6c877..c613636 100644 --- a/examples/testbed/framework/main.cpp +++ b/examples/testbed/framework/main.cpp @@ -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(); diff --git a/include/bounce/bounce.h b/include/bounce/bounce.h index 150dca9..55d82b0 100644 --- a/include/bounce/bounce.h +++ b/include/bounce/bounce.h @@ -28,7 +28,6 @@ #include #include -#include #include #include #include diff --git a/include/bounce/collision/gjk/gjk.h b/include/bounce/collision/gjk/gjk.h index ba4c41c..d1f06b9 100644 --- a/include/bounce/collision/gjk/gjk.h +++ b/include/bounce/collision/gjk/gjk.h @@ -19,7 +19,9 @@ #ifndef B3_GJK_H #define B3_GJK_H -#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////////////// 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 \ No newline at end of file diff --git a/include/bounce/collision/gjk/gjk_cache.h b/include/bounce/collision/gjk/gjk_cache.h deleted file mode 100644 index 2b0e8de..0000000 --- a/include/bounce/collision/gjk/gjk_cache.h +++ /dev/null @@ -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 - -// 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 \ No newline at end of file diff --git a/include/bounce/common/geometry.h b/include/bounce/common/geometry.h index c0baf98..3a8c45c 100644 --- a/include/bounce/common/geometry.h +++ b/include/bounce/common/geometry.h @@ -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) { diff --git a/include/bounce/dynamics/contacts/collide/collide.h b/include/bounce/dynamics/contacts/collide/collide.h index 2026b05..2664c69 100644 --- a/include/bounce/dynamics/contacts/collide/collide.h +++ b/include/bounce/dynamics/contacts/collide/collide.h @@ -19,8 +19,8 @@ #ifndef B3_COLLIDE_H #define B3_COLLIDE_H +#include #include -#include #include #include #include diff --git a/include/bounce/dynamics/world.h b/include/bounce/dynamics/world.h index 7e24339..ca71d91 100644 --- a/include/bounce/dynamics/world.h +++ b/include/bounce/dynamics/world.h @@ -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. diff --git a/src/bounce/collision/gjk/gjk.cpp b/src/bounce/collision/gjk/gjk.cpp index 445e756..d31dd27 100644 --- a/src/bounce/collision/gjk/gjk.cpp +++ b/src/bounce/collision/gjk/gjk.cpp @@ -19,11 +19,85 @@ #include #include +/////////////////////////////////////////////////////////////////////////////////////////////////// + // 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); +} \ No newline at end of file diff --git a/src/bounce/collision/gjk/gjk_cache.cpp b/src/bounce/collision/gjk/gjk_cache.cpp deleted file mode 100644 index 1d058b7..0000000 --- a/src/bounce/collision/gjk/gjk_cache.cpp +++ /dev/null @@ -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 -#include - -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; -} diff --git a/src/bounce/collision/gjk/gjk_feature_pair.cpp b/src/bounce/collision/gjk/gjk_feature_pair.cpp index efeb58b..34ada7d 100644 --- a/src/bounce/collision/gjk/gjk_feature_pair.cpp +++ b/src/bounce/collision/gjk/gjk_feature_pair.cpp @@ -16,7 +16,7 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#include +#include b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache) { diff --git a/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp b/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp index 19f5402..563cd7e 100644 --- a/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp @@ -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]) diff --git a/src/bounce/dynamics/island.cpp b/src/bounce/dynamics/island.cpp index c77a786..6ec8f8b 100644 --- a/src/bounce/dynamics/island.cpp +++ b/src/bounce/dynamics/island.cpp @@ -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; diff --git a/src/bounce/dynamics/shapes/mesh_shape.cpp b/src/bounce/dynamics/shapes/mesh_shape.cpp index d8730db..ca76573 100644 --- a/src/bounce/dynamics/shapes/mesh_shape.cpp +++ b/src/bounce/dynamics/shapes/mesh_shape.cpp @@ -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) {