diff --git a/examples/testbed/framework/debug_draw.cpp b/examples/testbed/framework/debug_draw.cpp index 5d35051..09f943a 100644 --- a/examples/testbed/framework/debug_draw.cpp +++ b/examples/testbed/framework/debug_draw.cpp @@ -27,7 +27,21 @@ extern Camera g_camera; extern DebugDraw* g_debugDraw; -Mat44 Camera::BuildProjectionMatrix() const +static B3_FORCE_INLINE b3Mat34 Convert34(const b3Transform& T) +{ + return b3Mat34(T.rotation.x, T.rotation.y, T.rotation.z, T.position); +} + +static B3_FORCE_INLINE b3Mat44 Convert44(const b3Transform& T) +{ + return b3Mat44( + b3Vec4(T.rotation.x.x, T.rotation.x.y, T.rotation.x.z, 0.0f), + b3Vec4(T.rotation.y.x, T.rotation.y.y, T.rotation.y.z, 0.0f), + b3Vec4(T.rotation.z.x, T.rotation.z.y, T.rotation.z.z, 0.0f), + b3Vec4(T.position.x, T.position.y, T.position.z, 1.0f)); +} + +b3Mat44 Camera::BuildProjectionMatrix() const { float32 t = tan(0.5f * m_fovy); float32 sy = 1.0f / t; @@ -39,11 +53,11 @@ Mat44 Camera::BuildProjectionMatrix() const float32 sz = invRange * (m_zNear + m_zFar); float32 tz = invRange * m_zNear * m_zFar; - Mat44 m; - m.x.Set(sx, 0.0f, 0.0f, 0.0f); - m.y.Set(0.0f, sy, 0.0f, 0.0f); - m.z.Set(0.0f, 0.0f, sz, -1.0f); - m.w.Set(0.0f, 0.0f, tz, 0.0f); + b3Mat44 m; + m.x = b3Vec4(sx, 0.0f, 0.0f, 0.0f); + m.y = b3Vec4(0.0f, sy, 0.0f, 0.0f); + m.z = b3Vec4(0.0f, 0.0f, sz, -1.0f); + m.w = b3Vec4(0.0f, 0.0f, tz, 0.0f); return m; } @@ -55,10 +69,10 @@ b3Transform Camera::BuildWorldTransform() const return xf; } -Mat44 Camera::BuildWorldMatrix() const +b3Mat44 Camera::BuildWorldMatrix() const { b3Transform xf = BuildWorldTransform(); - return GetMat44(xf); + return Convert44(xf); } b3Transform Camera::BuildViewTransform() const @@ -69,10 +83,10 @@ b3Transform Camera::BuildViewTransform() const return b3Inverse(xf); } -Mat44 Camera::BuildViewMatrix() const +b3Mat44 Camera::BuildViewMatrix() const { b3Transform xf = BuildViewTransform(); - return GetMat44(xf); + return Convert44(xf); } b3Vec2 Camera::ConvertWorldToScreen(const b3Vec3& pw) const @@ -150,11 +164,11 @@ static void PrintLog(GLuint id) static GLuint CreateShader(const char* source, GLenum type) { GLuint shaderId = glCreateShader(type); - + const char* sources[] = { source }; glShaderSource(shaderId, 1, sources, NULL); glCompileShader(shaderId); - + GLint status = GL_FALSE; glGetShaderiv(shaderId, GL_COMPILE_STATUS, &status); if (status == GL_FALSE) @@ -164,7 +178,7 @@ static GLuint CreateShader(const char* source, GLenum type) glDeleteShader(shaderId); return 0; } - + return shaderId; } @@ -225,7 +239,7 @@ struct DrawPoints m_sizeAttribute = 2; glGenBuffers(3, m_vboIds); - + glGenVertexArrays(1, &m_vaoId); glBindVertexArray(m_vaoId); @@ -282,14 +296,14 @@ struct DrawPoints glUseProgram(m_programId); - Mat44 m1 = g_camera.BuildViewMatrix(); - Mat44 m2 = g_camera.BuildProjectionMatrix(); - Mat44 m = m2 * m1; - + b3Mat44 m1 = g_camera.BuildViewMatrix(); + b3Mat44 m2 = g_camera.BuildProjectionMatrix(); + b3Mat44 m = m2 * m1; + glUniformMatrix4fv(m_projectionUniform, 1, GL_FALSE, &m.x.x); glBindVertexArray(m_vaoId); - + glBindBuffer(GL_ARRAY_BUFFER, m_vboIds[0]); glBufferSubData(GL_ARRAY_BUFFER, 0, m_count * sizeof(b3Vec3), m_vertices); @@ -321,13 +335,13 @@ struct DrawPoints b3Color m_colors[e_vertexCapacity]; float32 m_sizes[e_vertexCapacity]; u32 m_count; - + GLuint m_programId; GLuint m_projectionUniform; GLuint m_vertexAttribute; GLuint m_colorAttribute; GLuint m_sizeAttribute; - + GLuint m_vboIds[3]; GLuint m_vaoId; }; @@ -361,8 +375,8 @@ struct DrawLines m_projectionUniform = glGetUniformLocation(m_programId, "projectionMatrix"); m_vertexAttribute = 0; m_colorAttribute = 1; - - glGenVertexArrays(1, &m_vaoId); + + glGenVertexArrays(1, &m_vaoId); glGenBuffers(2, m_vboIds); glBindVertexArray(m_vaoId); @@ -376,7 +390,7 @@ struct DrawLines glBindBuffer(GL_ARRAY_BUFFER, m_vboIds[1]); glVertexAttribPointer(m_colorAttribute, 4, GL_FLOAT, GL_FALSE, 0, BUFFER_OFFSET(0)); glBufferData(GL_ARRAY_BUFFER, e_vertexCapacity * sizeof(b3Color), m_colors, GL_DYNAMIC_DRAW); - + AssertGL(); glBindVertexArray(0); @@ -413,9 +427,9 @@ struct DrawLines glUseProgram(m_programId); - Mat44 m1 = g_camera.BuildViewMatrix(); - Mat44 m2 = g_camera.BuildProjectionMatrix(); - Mat44 m = m2 * m1; + b3Mat44 m1 = g_camera.BuildViewMatrix(); + b3Mat44 m2 = g_camera.BuildProjectionMatrix(); + b3Mat44 m = m2 * m1; glUniformMatrix4fv(m_projectionUniform, 1, GL_FALSE, &m.x.x); glBindVertexArray(m_vaoId); @@ -427,7 +441,7 @@ struct DrawLines glBufferSubData(GL_ARRAY_BUFFER, 0, m_count * sizeof(b3Color), m_colors); glDrawArrays(GL_LINES, 0, m_count); - + AssertGL(); glBindVertexArray(0); @@ -450,7 +464,7 @@ struct DrawLines GLuint m_projectionUniform; GLuint m_vertexAttribute; GLuint m_colorAttribute; - + GLuint m_vboIds[2]; GLuint m_vaoId; }; @@ -470,7 +484,7 @@ struct DrawTriangles "{\n" " vec3 La = vec3(0.5f, 0.5f, 0.5f);\n" " vec3 Ld = vec3(0.5f, 0.5f, 0.5f);\n" - " vec3 L = vec3(0.0f, 0.0f, 1.0f);\n" + " vec3 L = vec3(0.0f, 0.3f, 0.7f);\n" " vec3 Ma = v_color.xyz;\n" " vec3 Md = v_color.xyz;\n" " vec3 a = La * Ma;\n" @@ -498,7 +512,7 @@ struct DrawTriangles glGenBuffers(3, m_vboIds); glBindVertexArray(m_vaoId); - + glBindBuffer(GL_ARRAY_BUFFER, m_vboIds[0]); glBufferData(GL_ARRAY_BUFFER, e_vertexCapacity * sizeof(b3Vec3), m_vertices, GL_DYNAMIC_DRAW); glVertexAttribPointer(m_vertexAttribute, 3, GL_FLOAT, GL_FALSE, 0, BUFFER_OFFSET(0)); @@ -518,7 +532,7 @@ struct DrawTriangles glBindVertexArray(0); glBindBuffer(GL_ARRAY_BUFFER, 0); - + m_count = 0; } @@ -551,9 +565,9 @@ struct DrawTriangles glUseProgram(m_programId); - Mat44 m1 = g_camera.BuildViewMatrix(); - Mat44 m2 = g_camera.BuildProjectionMatrix(); - Mat44 m = m2 * m1; + b3Mat44 m1 = g_camera.BuildViewMatrix(); + b3Mat44 m2 = g_camera.BuildProjectionMatrix(); + b3Mat44 m = m2 * m1; glUniformMatrix4fv(m_projectionUniform, 1, GL_FALSE, &m.x.x); @@ -567,7 +581,7 @@ struct DrawTriangles glBindBuffer(GL_ARRAY_BUFFER, m_vboIds[2]); glBufferSubData(GL_ARRAY_BUFFER, 0, m_count * sizeof(b3Vec3), m_normals); - + glDrawArrays(GL_TRIANGLES, 0, m_count); AssertGL(); @@ -578,7 +592,7 @@ struct DrawTriangles m_count = 0; } - + enum { e_vertexCapacity = 3 * 512 @@ -724,13 +738,13 @@ struct DrawWire { glUseProgram(m_programId); - Mat44 m1 = GetMat44(xf); + b3Mat44 m1 = Convert44(xf); m1.x = radius * m1.x; m1.y = radius * m1.y; m1.z = radius * m1.z; - Mat44 m2 = g_camera.BuildViewMatrix(); - Mat44 m3 = g_camera.BuildProjectionMatrix(); - Mat44 m = m3 * m2 * m1; + b3Mat44 m2 = g_camera.BuildViewMatrix(); + b3Mat44 m3 = g_camera.BuildProjectionMatrix(); + b3Mat44 m = m3 * m2 * m1; glUniformMatrix4fv(m_projectionUniform, 1, GL_FALSE, &m.x.x); @@ -759,8 +773,8 @@ struct DrawSolidSphere { enum { - e_rings = 12, - e_sectors = 12, + e_rings = 18, + e_sectors = 18, e_vertexCount = e_rings * e_sectors, e_indexCount = (e_rings - 1) * (e_sectors - 1) * 6, e_faceCount = e_indexCount / 3 @@ -773,18 +787,31 @@ struct DrawSolidSphere b3Vec3 vs[e_vertexCount]; b3Vec3 ns[e_vertexCount]; - + u32 vc = 0; for (u32 r = 0; r < e_rings; r++) { for (u32 s = 0; s < e_sectors; s++) { - float32 y = sin(-0.5f * B3_PI + B3_PI * r * R); - float32 x = cos(2.0f * B3_PI * s * S) * sin(B3_PI * r * R); - float32 z = sin(2.0f * B3_PI * s * S) * sin(B3_PI * r * R); + float32 a1 = 2.0f * B3_PI * float32(s) * S; + float32 c1 = cos(a1); + float32 s1 = sin(a1); - vs[vc].Set(x, y, z); - ns[vc].Set(x, y, z); + float32 a2 = -0.5f * B3_PI + B3_PI * float32(r) * R; + float32 s2 = sin(a2); + + float32 a3 = B3_PI * float32(r) * R; + float32 s3 = sin(a3); + + float32 x = c1 * s3; + float32 y = s2; + float32 z = s1 * s3; + + b3Vec3 v(x, y, z); + v.Normalize(); + + vs[vc] = v; + ns[vc] = v; ++vc; } } @@ -843,7 +870,7 @@ struct DrawSolidCylinder { enum { - e_segments = 24, + e_segments = 64, e_vertexCount = e_segments * 6, }; @@ -851,12 +878,12 @@ struct DrawSolidCylinder { b3Vec3 vs[e_vertexCount]; b3Vec3 ns[e_vertexCount]; - + u32 vc = 0; for (u32 i = 0; i < e_segments; ++i) { - float32 t0 = 2.0f * B3_PI * (float32)i / (float32)e_segments; - float32 t1 = 2.0f * B3_PI * (float32)(i + 1) / (float32)e_segments; + float32 t0 = 2.0f * B3_PI * float32(i) / float32(e_segments); + float32 t1 = 2.0f * B3_PI * float32(i + 1) / float32(e_segments); float32 c0 = cos(t0); float32 s0 = sin(t0); @@ -868,22 +895,22 @@ struct DrawSolidCylinder v1.x = s0; v1.y = -0.5f; v1.z = c0; - + b3Vec3 v2; v2.x = s1; v2.y = -0.5f; v2.z = c1; - + b3Vec3 v3; v3.x = s1; v3.y = 0.5f; v3.z = c1; - + b3Vec3 v4; v4.x = s0; v4.y = 0.5f; v4.z = c0; - + b3Vec3 n = b3Cross(v2 - v1, v3 - v1); n.Normalize(); @@ -966,7 +993,7 @@ struct DrawSolid " gl_Position = projectionMatrix * vec4(v_position, 1.0f);\n" " vec3 La = vec3(0.5f, 0.5f, 0.5f);\n" " vec3 Ld = vec3(0.5f, 0.5f, 0.5f);\n" - " vec3 L = vec3(0.0f, 0.0f, 1.0f);\n" + " vec3 L = vec3(0.0f, 0.3f, 0.7f);\n" " vec3 Ma = color.xyz;\n" " vec3 Md = color.xyz;\n" " vec3 a = La * Ma;\n" @@ -999,14 +1026,14 @@ struct DrawSolid { glUseProgram(m_programId); - Mat44 m1 = GetMat44(xf); + b3Mat44 m1 = Convert44(xf); m1.x = radius * m1.x; m1.y = height * m1.y; m1.z = radius * m1.z; - Mat44 m2 = g_camera.BuildViewMatrix(); - Mat44 m3 = g_camera.BuildProjectionMatrix(); - Mat44 m = m3 * m2 * m1; + b3Mat44 m2 = g_camera.BuildViewMatrix(); + b3Mat44 m3 = g_camera.BuildProjectionMatrix(); + b3Mat44 m = m3 * m2 * m1; glUniform4fv(m_colorUniform, 1, &c.r); glUniformMatrix4fv(m_modelUniform, 1, GL_FALSE, &m1.x.x); @@ -1033,14 +1060,14 @@ struct DrawSolid { glUseProgram(m_programId); - Mat44 m1 = GetMat44(xf); + b3Mat44 m1 = Convert44(xf); m1.x = radius * m1.x; m1.y = radius * m1.y; m1.z = radius * m1.z; - Mat44 m2 = g_camera.BuildViewMatrix(); - Mat44 m3 = g_camera.BuildProjectionMatrix(); - Mat44 m = m3 * m2 * m1; + b3Mat44 m2 = g_camera.BuildViewMatrix(); + b3Mat44 m3 = g_camera.BuildProjectionMatrix(); + b3Mat44 m = m3 * m2 * m1; glUniform4fv(m_colorUniform, 1, &c.r); glUniformMatrix4fv(m_modelUniform, 1, GL_FALSE, &m1.x.x); @@ -1071,7 +1098,7 @@ struct DrawSolid GLuint m_normalAttribute; DrawSolidSphere m_sphere; - DrawSolidCylinder m_cylinder; + DrawSolidCylinder m_cylinder; }; DebugDraw::DebugDraw() @@ -1115,7 +1142,7 @@ void DebugDraw::DrawSolidTriangle(const b3Vec3& normal, const b3Vec3& p1, const m_triangles->Vertex(p1, color, normal); m_triangles->Vertex(p2, color, normal); m_triangles->Vertex(p3, color, normal); - + b3Color edgeColor(0.0f, 0.0f, 0.0f, 1.0f); DrawTriangle(p2, p3, p3, edgeColor); } @@ -1126,7 +1153,7 @@ void DebugDraw::DrawPolygon(const b3Vec3* vertices, u32 count, const b3Color& co for (u32 i = 0; i < count; ++i) { b3Vec3 p2 = vertices[i]; - + m_lines->Vertex(p1, color); m_lines->Vertex(p2, color); @@ -1143,7 +1170,7 @@ void DebugDraw::DrawSolidPolygon(const b3Vec3& normal, const b3Vec3* vertices, u { b3Vec3 p2 = vertices[i]; b3Vec3 p3 = vertices[i + 1]; - + m_triangles->Vertex(p1, fillColor, normal); m_triangles->Vertex(p2, fillColor, normal); m_triangles->Vertex(p3, fillColor, normal); @@ -1171,7 +1198,7 @@ void DebugDraw::DrawCircle(const b3Vec3& normal, const b3Vec3& center, float32 r } n1.Normalize(); - + // Build a quaternion to rotate the tangent about the normal. u32 kEdgeCount = 20; float32 kAngleInc = 2.0f * B3_PI / float32(kEdgeCount); @@ -1185,7 +1212,7 @@ void DebugDraw::DrawCircle(const b3Vec3& normal, const b3Vec3& center, float32 r m_lines->Vertex(p1, color); m_lines->Vertex(p2, color); - + n1 = n2; p1 = p2; } @@ -1195,7 +1222,7 @@ void DebugDraw::DrawSolidCircle(const b3Vec3& normal, const b3Vec3& center, floa { b3Color fillColor(color.r, color.g, color.b, color.a); b3Color frameColor(0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 1.0f); - + // Build a tangent vector to normal. b3Vec3 u = b3Cross(normal, b3Vec3(1.0f, 0.0f, 0.0f)); b3Vec3 v = b3Cross(normal, b3Vec3(0.0f, 1.0f, 0.0f)); @@ -1223,7 +1250,7 @@ void DebugDraw::DrawSolidCircle(const b3Vec3& normal, const b3Vec3& center, floa { b3Vec3 n2 = b3Mul(q, n1); b3Vec3 p2 = center + radius * n2; - + m_triangles->Vertex(center, fillColor, normal); m_triangles->Vertex(p1, fillColor, normal); m_triangles->Vertex(p2, fillColor, normal); @@ -1281,35 +1308,29 @@ void DebugDraw::DrawAABB(const b3AABB3& aabb, const b3Color& color) b3Vec3 vs[8]; - // Face 1 vs[0] = lower; vs[1] = b3Vec3(lower.x, upper.y, lower.z); vs[2] = b3Vec3(upper.x, upper.y, lower.z); vs[3] = b3Vec3(upper.x, lower.y, lower.z); - // Face 2 vs[4] = upper; vs[5] = b3Vec3(upper.x, lower.y, upper.z); vs[6] = b3Vec3(lower.x, lower.y, upper.z); vs[7] = b3Vec3(lower.x, upper.y, upper.z); - // Face 1 edges DrawSegment(vs[0], vs[1], color); DrawSegment(vs[1], vs[2], color); DrawSegment(vs[2], vs[3], color); DrawSegment(vs[3], vs[0], color); - // Face 2 edges DrawSegment(vs[4], vs[5], color); DrawSegment(vs[5], vs[6], color); DrawSegment(vs[6], vs[7], color); DrawSegment(vs[7], vs[4], color); - // Face 3 edges DrawSegment(vs[2], vs[4], color); DrawSegment(vs[5], vs[3], color); - // Face 4 edges DrawSegment(vs[6], vs[0], color); DrawSegment(vs[1], vs[7], color); } @@ -1349,9 +1370,15 @@ void DebugDraw::DrawCapsule(const b3CapsuleShape* s, const b3Color& c, const b3T if (height > 0.0f) { { + b3Mat33 R; + R.y = (1.0f / height) * (c1 - c2); + R.z = b3Perp(R.y); + R.x = b3Cross(R.y, R.z); + b3Transform xfc; - xfc.rotation = xf.rotation; - xfc.position = xf * ( 0.5f * (c1 + c2) ); + xfc.position = xf * (0.5f * (c1 + c2)); + xfc.rotation = xf.rotation * R; + m_solid->DrawCylinder(radius, height, c, xfc); } @@ -1409,13 +1436,13 @@ void DebugDraw::DrawMesh(const b3MeshShape* s, const b3Color& c, const b3Transfo b3Vec3 n1 = b3Cross(p2 - p1, p3 - p1); n1.Normalize(); - + m_triangles->Vertex(p1, c, n1); m_triangles->Vertex(p2, c, n1); m_triangles->Vertex(p3, c, n1); b3Vec3 n2 = -n1; - + m_triangles->Vertex(p1, c, n2); m_triangles->Vertex(p3, c, n2); m_triangles->Vertex(p2, c, n2); @@ -1481,7 +1508,7 @@ void DebugDraw::Draw(const b3World& world) DrawShape(s, c, xf); } } - + g_debugDraw->Submit(); } diff --git a/examples/testbed/framework/debug_draw.h b/examples/testbed/framework/debug_draw.h index 6e9bede..868b2c0 100644 --- a/examples/testbed/framework/debug_draw.h +++ b/examples/testbed/framework/debug_draw.h @@ -20,7 +20,6 @@ #define DEBUG_DRAW_H #include -#include "mat44.h" struct DrawPoints; struct DrawLines; @@ -28,6 +27,23 @@ struct DrawTriangles; struct DrawWire; struct DrawSolid; +struct Ray3 +{ + b3Vec3 A() const + { + return origin; + } + + b3Vec3 B() const + { + return origin + fraction * direction; + } + + b3Vec3 direction; + b3Vec3 origin; + float32 fraction; +}; + class Camera { public: @@ -38,15 +54,15 @@ public: m_width = 1024.0f; m_height = 768.0f; m_zNear = 1.0f; - m_zFar = 500.0f; + m_zFar = 1000.0f; m_fovy = 0.25f * B3_PI; m_zoom = 10.0f; } - Mat44 BuildProjectionMatrix() const; - Mat44 BuildViewMatrix() const; + b3Mat44 BuildProjectionMatrix() const; + b3Mat44 BuildViewMatrix() const; b3Transform BuildViewTransform() const; - Mat44 BuildWorldMatrix() const; + b3Mat44 BuildWorldMatrix() const; b3Transform BuildWorldTransform() const; b3Vec2 ConvertWorldToScreen(const b3Vec3& pw) const; diff --git a/examples/testbed/framework/main.cpp b/examples/testbed/framework/main.cpp index f7bbc33..4e6c877 100644 --- a/examples/testbed/framework/main.cpp +++ b/examples/testbed/framework/main.cpp @@ -16,13 +16,18 @@ * 3. This notice may not be removed or altered from any source distribution. */ +#if defined(__APPLE_CC__) +#include +#else #include -#include +#endif + #include #include - #include +#include + GLFWwindow* g_window; Settings g_settings; Test* g_test; @@ -35,13 +40,13 @@ bool g_rightDown; bool g_shiftDown; b3Vec2 g_ps0; -void WindowSize(int w, int h) +static void WindowSize(int w, int h) { g_camera.m_width = float32(w); g_camera.m_height = float32(h); } -void MouseMove(GLFWwindow* w, double x, double y) +static void MouseMove(GLFWwindow* w, double x, double y) { b3Vec2 ps; ps.Set(float32(x), float32(y)); @@ -83,7 +88,7 @@ void MouseMove(GLFWwindow* w, double x, double y) } } -void MouseWheel(GLFWwindow* w, double dx, double dy) +static void MouseWheel(GLFWwindow* w, double dx, double dy) { float32 n = b3Clamp(float32(dy), -1.0f, 1.0f); if (g_shiftDown) @@ -92,7 +97,7 @@ void MouseWheel(GLFWwindow* w, double dx, double dy) } } -void MouseButton(GLFWwindow* w, int button, int action, int mods) +static void MouseButton(GLFWwindow* w, int button, int action, int mods) { double x, y; glfwGetCursorPos(w, &x, &y); @@ -147,7 +152,7 @@ void MouseButton(GLFWwindow* w, int button, int action, int mods) } } -void KeyButton(GLFWwindow* w, int button, int scancode, int action, int mods) +static void KeyButton(GLFWwindow* w, int button, int scancode, int action, int mods) { switch (action) { @@ -163,12 +168,12 @@ void KeyButton(GLFWwindow* w, int button, int scancode, int action, int mods) { if (button == GLFW_KEY_DOWN) { - g_camera.m_zoom += 0.05f; + g_camera.m_zoom += 0.2f; } if (button == GLFW_KEY_UP) { - g_camera.m_zoom -= 0.05f; + g_camera.m_zoom -= 0.2f; } } else @@ -199,30 +204,30 @@ void KeyButton(GLFWwindow* w, int button, int scancode, int action, int mods) } } -void Char(GLFWwindow* w, unsigned int codepoint) +static void Char(GLFWwindow* w, unsigned int codepoint) { ImGui_ImplGlfwGL3_CharCallback(w, codepoint); } -void CreateInterface() +static void CreateInterface() { ImGui_ImplGlfwGL3_Init(g_window, false); ImGuiIO& io = ImGui::GetIO(); io.Fonts[0].AddFontDefault(); } -void DestroyInterface() +static void DestroyInterface() { ImGui_ImplGlfwGL3_Shutdown(); } -bool GetTestName(void*, int idx, const char** name) +static bool GetTestName(void*, int idx, const char** name) { *name = g_tests[idx].name; return true; } -void Interface() +static void Interface() { ImGui::SetNextWindowPos(ImVec2(g_camera.m_width, 0.0f)); ImGui::SetNextWindowSize(ImVec2(250.0f, g_camera.m_height)); @@ -255,11 +260,18 @@ void Interface() g_settings.testID = b3Clamp(g_settings.testID + 1, 0, int(g_testCount) - 1); g_settings.lastTestID = -1; } + if (ImGui::Button("Dump", buttonSize)) + { + if (g_test) + { + g_test->Dump(); + } + } if (ImGui::Button("Exit", buttonSize)) { glfwSetWindowShouldClose(g_window, true); } - + ImGui::Separator(); ImGui::Text("Step"); @@ -271,8 +283,8 @@ void Interface() ImGui::Text("Position Iterations"); ImGui::SliderInt("#Position Iterations", &g_settings.positionIterations, 0, 50); ImGui::Checkbox("Sleep", &g_settings.sleep); + ImGui::Checkbox("Convex Cache", &g_settings.convexCache); ImGui::Checkbox("Warm Start", &g_settings.warmStart); - //ImGui::Checkbox("Convex Cache", &g_settings.convexCache); if (ImGui::Button("Play/Pause", buttonSize)) { @@ -283,7 +295,7 @@ void Interface() g_settings.pause = true; g_settings.singleStep = true; } - + ImGui::Separator(); ImGui::Text("View"); @@ -296,6 +308,7 @@ void Interface() ImGui::Checkbox("Contact Points", &g_settings.drawContactPoints); ImGui::Checkbox("Contact Normals", &g_settings.drawContactNormals); ImGui::Checkbox("Contact Tangents", &g_settings.drawContactTangents); + ImGui::Checkbox("Contact Areas", &g_settings.drawContactAreas); ImGui::Checkbox("Statistics", &g_settings.drawStats); ImGui::Checkbox("Profile", &g_settings.drawProfile); @@ -303,10 +316,16 @@ void Interface() ImGui::PopStyleVar(); } -void Step() +static void Step() { if (g_settings.drawGrid) { + b3Color color(0.2f, 0.2f, 0.2f, 1.0f); + + b3Vec3 pn(0.0f, 1.0f, 0.0f); + b3Vec3 p(0.0f, 0.0f, 0.0f); + g_debugDraw->DrawCircle(pn, p, 1.0f, color); + int n = 20; b3Vec3 t; @@ -314,7 +333,6 @@ void Step() t.y = 0.0f; t.z = -0.5f * float32(n); - b3Color color(0.5f, 0.5f, 0.5f, 1.0f); for (int i = 0; i < n; i += 1) { for (int j = 0; j < n; j += 1) @@ -347,16 +365,16 @@ void Step() g_debugDraw->Submit(); } -void Run() +static void Run() { glFrontFace(GL_CCW); glCullFace(GL_BACK); glEnable(GL_CULL_FACE); glEnable(GL_DEPTH_TEST); - glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); - glClearColor(0.2f, 0.2f, 0.2f, 1.0f); + glClearColor(0.3f, 0.3f, 0.3f, 1.0f); glClearDepth(1.0f); double t1 = glfwGetTime(); @@ -489,5 +507,6 @@ int main(int argc, char** args) // Destroy g_window glfwTerminate(); + return 0; } \ No newline at end of file diff --git a/examples/testbed/framework/mat44.h b/examples/testbed/framework/mat44.h deleted file mode 100644 index 01977e2..0000000 --- a/examples/testbed/framework/mat44.h +++ /dev/null @@ -1,131 +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 MAT44_H -#define MAT44_H - -#include - -struct Vec4 -{ - Vec4() { } - Vec4(float32 _x, float32 _y, float32 _z, float32 _w) : x(_x), y(_y), z(_z), w(_w) { } - - void SetZero() - { - x = y = z = w = 0.0f; - } - - void Set(float32 _x, float32 _y, float32 _z, float32 _w) - { - x = _x; - y = _y; - z = _z; - w = _w; - } - - float32 x, y, z, w; -}; - -inline Vec4 operator+(const Vec4& a, const Vec4& b) -{ - return Vec4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w); -} - -inline Vec4 operator*(float32 s, const Vec4& v) -{ - return Vec4(s * v.x, s * v.y, s * v.z, s * v.w); -} - -struct Mat44 -{ - Mat44() { } - Mat44(const Vec4& _x, const Vec4& _y, const Vec4& _z, const Vec4& _w) : x(_x), y(_y), z(_z), w(_w) { } - - void SetIdentity() - { - x.Set(1.0f, 0.0f, 0.0f, 0.0f); - y.Set(0.0f, 1.0f, 0.0f, 0.0f); - z.Set(0.0f, 0.0f, 1.0f, 0.0f); - w.Set(0.0f, 0.0f, 0.0f, 1.0f); - } - - Vec4 x, y, z, w; -}; - -inline Vec4 operator*(const Mat44& A, const Vec4& v) -{ - return v.x * A.x + v.y * A.y + v.z * A.z + v.w * A.w; -} - -inline b3Vec3 operator*(const Mat44& A, const b3Vec3& v) -{ - Vec4 q = v.x * A.x + v.y * A.y + v.z * A.z + A.w; - return b3Vec3(q.x, q.y, q.z); -} - -inline Mat44 operator*(const Mat44& A, const Mat44& B) -{ - return Mat44(A * B.x, A * B.y, A * B.z, A * B.w); -} - -inline Mat44 GetMat44(const b3Transform& T) -{ - return Mat44( - Vec4(T.rotation.x.x, T.rotation.x.y, T.rotation.x.z, 0.0f), - Vec4(T.rotation.y.x, T.rotation.y.y, T.rotation.y.z, 0.0f), - Vec4(T.rotation.z.x, T.rotation.z.y, T.rotation.z.z, 0.0f), - Vec4(T.position.x, T.position.y, T.position.z, 1.0f)); -} - -inline b3Transform GetTransform(const Mat44& T) -{ - b3Transform xf; - xf.rotation.x.Set(T.x.x, T.x.y, T.x.z); - xf.rotation.y.Set(T.y.x, T.y.y, T.y.z); - xf.rotation.z.Set(T.z.x, T.z.y, T.z.z); - xf.position.Set(T.w.x, T.w.y, T.w.z); - return xf; -} - -inline float32 RandomFloat(float32 a, float32 b) -{ - float32 x = float32(rand()) / float32(RAND_MAX); - float32 diff = b - a; - float32 r = x * diff; - return a + r; -} - -struct Ray3 -{ - b3Vec3 Start() const - { - return origin; - } - - b3Vec3 End() const - { - return origin + fraction * direction; - } - - b3Vec3 direction; - b3Vec3 origin; - float32 fraction; -}; - -#endif \ No newline at end of file diff --git a/examples/testbed/framework/test.cpp b/examples/testbed/framework/test.cpp index 8e33a3f..3f097b8 100644 --- a/examples/testbed/framework/test.cpp +++ b/examples/testbed/framework/test.cpp @@ -20,8 +20,8 @@ extern u32 b3_allocCalls, b3_maxAllocCalls; extern u32 b3_gjkCalls, b3_gjkIters, b3_gjkMaxIters; +extern bool b3_convexCache; extern u32 b3_convexCalls, b3_convexCacheHits; -extern bool b3_enableConvexCache; extern b3Draw* b3_debugDraw; extern Settings g_settings; @@ -29,21 +29,86 @@ extern DebugDraw* g_debugDraw; extern Camera g_camera; extern Profiler* g_profiler; +static void BuildGrid(b3Mesh* mesh, u32 w, u32 h, bool randomY = false) +{ + b3Vec3 t; + t.x = -0.5f * float32(w); + t.y = 0.0f; + t.z = -0.5f * float32(h); + + mesh->vertexCount = w * h; + mesh->vertices = (b3Vec3*)b3Alloc(mesh->vertexCount * sizeof(b3Vec3)); + + for (u32 i = 0; i < w; ++i) + { + for (u32 j = 0; j < h; ++j) + { + u32 v1 = i * w + j; + + b3Vec3 v; + v.x = float32(i); + v.y = randomY ? RandomFloat(0.0f, 1.0f) : 0.0f; + v.z = float32(j); + + v += t; + + mesh->vertices[v1] = v; + } + } + + mesh->triangleCount = 2 * (w - 1) * (h - 1); + mesh->triangles = (b3Triangle*)b3Alloc(mesh->triangleCount * sizeof(b3Triangle)); + + u32 triangleCount = 0; + for (u32 i = 0; i < w - 1; ++i) + { + for (u32 j = 0; j < h - 1; ++j) + { + u32 v1 = i * w + j; + u32 v2 = (i + 1) * w + j; + u32 v3 = (i + 1) * w + (j + 1); + u32 v4 = i * w + (j + 1); + + B3_ASSERT(triangleCount < mesh->triangleCount); + b3Triangle* t1 = mesh->triangles + triangleCount; + ++triangleCount; + + t1->v1 = v3; + t1->v2 = v2; + t1->v3 = v1; + + B3_ASSERT(triangleCount < mesh->triangleCount); + b3Triangle* t2 = mesh->triangles + triangleCount; + ++triangleCount; + + t2->v1 = v1; + t2->v2 = v4; + t2->v3 = v3; + } + } + + B3_ASSERT(triangleCount == mesh->triangleCount); + + mesh->BuildTree(); +} + Test::Test() { b3_allocCalls = 0; b3_gjkCalls = 0; b3_gjkIters = 0; b3_gjkMaxIters = 0; + b3_convexCache = g_settings.convexCache; b3_convexCalls = 0; b3_convexCacheHits = 0; - b3_enableConvexCache = g_settings.convexCache; b3_debugDraw = g_debugDraw; m_world.SetContactListener(this); - g_camera.m_q = b3Quat(b3Vec3(0.0f, 1.0f, 0.0f), 0.15f * B3_PI); - g_camera.m_q = g_camera.m_q * b3Quat(b3Vec3(1.0f, 0.0f, 0.0f), -0.15f * B3_PI); + b3Quat q_y(b3Vec3(0.0f, 1.0f, 0.0f), 0.15f * B3_PI); + b3Quat q_x(b3Quat(b3Vec3(1.0f, 0.0f, 0.0f), -0.15f * B3_PI)); + + g_camera.m_q = q_y * q_x; g_camera.m_zoom = 50.0f; g_camera.m_center.SetZero(); @@ -51,255 +116,17 @@ Test::Test() m_mouseJoint = NULL; { - b3Transform xf; - xf.position.SetZero(); - xf.rotation = b3Diagonal(50.0f, 1.0f, 50.0f); - m_groundHull.SetTransform(xf); + b3Transform m; + m.position.SetZero(); + m.rotation = b3Diagonal(50.0f, 1.0f, 50.0f); + m_groundHull.SetTransform(m); } - { - b3Transform xf; - xf.position.SetZero(); - xf.rotation = b3Diagonal(1.0f, 1.0f, 1.0f); - m_boxHull.SetTransform(xf); - } + m_boxHull.SetIdentity(); - { - b3Transform xf; - xf.position.SetZero(); - xf.rotation = b3Diagonal(1.0f, 5.0f, 1.0f); - m_tallHull.SetTransform(xf); - } - - { - b3Transform xf; - xf.position.SetZero(); - xf.rotation = b3Diagonal(2.0f, 4.0f, 0.5f); - m_doorHull.SetTransform(xf); - } - - { - b3Transform xf; - xf.position.SetZero(); - xf.rotation = b3Diagonal(25.0f, 0.5f, 25.0f); - m_rampHull.SetTransform(xf); - } - - { - b3Transform xf; - xf.position.SetZero(); - xf.rotation = b3Diagonal(1.0f, 0.5f, 3.0f); - m_plankHull.SetTransform(xf); - } - - { - b3Transform xf; - xf.position.SetZero(); - xf.rotation = b3Diagonal(4.05f, 2.0f * B3_LINEAR_SLOP, 4.05f); - m_thinHull.SetTransform(xf); - } - - { - const u32 w = 5; - const u32 h = 5; - - b3Vec3 t; - t.x = -0.5f * float32(w); - t.y = 0.0f; - t.z = -0.5f * float32(h); - - b3Mesh* mesh = m_meshes + e_clothMesh; - - mesh->vertexCount = w * h; - mesh->vertices = (b3Vec3*)b3Alloc(mesh->vertexCount * sizeof(b3Vec3)); - - for (u32 i = 0; i < w; ++i) - { - for (u32 j = 0; j < h; ++j) - { - u32 v1 = i * w + j; - - b3Vec3 v; - v.x = float32(i); - v.y = RandomFloat(0.0f, 0.5f); - v.z = float32(j); - - v += t; - - mesh->vertices[v1] = v; - } - } - - mesh->triangleCount = 2 * (w - 1) * (h - 1); - mesh->triangles = (b3Triangle*)b3Alloc(mesh->triangleCount * sizeof(b3Triangle)); - - u32 triangleCount = 0; - for (u32 i = 0; i < w - 1; ++i) - { - for (u32 j = 0; j < h - 1; ++j) - { - u32 v1 = i * w + j; - u32 v2 = (i + 1) * w + j; - u32 v3 = (i + 1) * w + (j + 1); - u32 v4 = i * w + (j + 1); - - B3_ASSERT(triangleCount < mesh->triangleCount); - b3Triangle* t1 = mesh->triangles + triangleCount; - ++triangleCount; - - t1->v1 = v3; - t1->v2 = v2; - t1->v3 = v1; - - B3_ASSERT(triangleCount < mesh->triangleCount); - b3Triangle* t2 = mesh->triangles + triangleCount; - ++triangleCount; - - t2->v1 = v1; - t2->v2 = v4; - t2->v3 = v3; - } - } - - B3_ASSERT(triangleCount == mesh->triangleCount); - - mesh->BuildTree(); - } - - { - const u32 w = 50; - const u32 h = 50; - - b3Vec3 t; - t.x = -0.5f * float32(w); - t.y = 0.0f; - t.z = -0.5f * float32(h); - - b3Mesh* mesh = m_meshes + e_gridMesh; - - mesh->vertexCount = w * h; - mesh->vertices = (b3Vec3*)b3Alloc(mesh->vertexCount * sizeof(b3Vec3)); - - for (u32 i = 0; i < w; ++i) - { - for (u32 j = 0; j < h; ++j) - { - u32 v1 = i * w + j; - - b3Vec3 v; - v.x = float32(i); - v.y = 0.0f; - v.z = float32(j); - - v += t; - - mesh->vertices[v1] = v; - } - } - - // 2 triangles per quad - mesh->triangleCount = 2 * (w - 1) * (h - 1); - mesh->triangles = (b3Triangle*)b3Alloc(mesh->triangleCount * sizeof(b3Triangle)); - - u32 triangleCount = 0; - for (u32 i = 0; i < w - 1; ++i) - { - for (u32 j = 0; j < h - 1; ++j) - { - u32 v1 = i * w + j; - u32 v2 = (i + 1) * w + j; - u32 v3 = (i + 1) * w + (j + 1); - u32 v4 = i * w + (j + 1); - - B3_ASSERT(triangleCount < mesh->triangleCount); - b3Triangle* t1 = mesh->triangles + triangleCount; - ++triangleCount; - - t1->v1 = v3; - t1->v2 = v2; - t1->v3 = v1; - - B3_ASSERT(triangleCount < mesh->triangleCount); - b3Triangle* t2 = mesh->triangles + triangleCount; - ++triangleCount; - - t2->v1 = v1; - t2->v2 = v4; - t2->v3 = v3; - } - } - - B3_ASSERT(triangleCount == mesh->triangleCount); - - mesh->BuildTree(); - } - - { - const u32 w = 50; - const u32 h = 50; - - b3Vec3 t; - t.x = -0.5f * float32(w); - t.y = 0.0f; - t.z = -0.5f * float32(h); - - b3Mesh* mesh = m_meshes + e_terrainMesh; - - mesh->vertexCount = w * h; - mesh->vertices = (b3Vec3*)b3Alloc(mesh->vertexCount * sizeof(b3Vec3)); - - for (u32 i = 0; i < w; ++i) - { - for (u32 j = 0; j < h; ++j) - { - u32 v1 = i * w + j; - - b3Vec3 v; - v.x = 2.0f * float32(i); - v.y = RandomFloat(0.0f, 0.5f); - v.z = 2.0f *float32(j); - - v += t; - - mesh->vertices[v1] = v; - } - } - - mesh->triangleCount = 2 * (w - 1) * (h - 1); - mesh->triangles = (b3Triangle*)b3Alloc(mesh->triangleCount * sizeof(b3Triangle)); - - u32 triangleCount = 0; - for (u32 i = 0; i < w - 1; ++i) - { - for (u32 j = 0; j < h - 1; ++j) - { - u32 v1 = i * w + j; - u32 v2 = (i + 1) * w + j; - u32 v3 = (i + 1) * w + (j + 1); - u32 v4 = i * w + (j + 1); - - B3_ASSERT(triangleCount < mesh->triangleCount); - b3Triangle* t1 = mesh->triangles + triangleCount; - ++triangleCount; - - t1->v1 = v3; - t1->v2 = v2; - t1->v3 = v1; - - B3_ASSERT(triangleCount < mesh->triangleCount); - b3Triangle* t2 = mesh->triangles + triangleCount; - ++triangleCount; - - t2->v1 = v1; - t2->v2 = v4; - t2->v3 = v3; - } - } - - B3_ASSERT(triangleCount == mesh->triangleCount); - - mesh->BuildTree(); - } + BuildGrid(m_meshes + e_gridMesh, 50, 50); + BuildGrid(m_meshes + e_terrainMesh, 50, 50, true); + BuildGrid(m_meshes + e_clothMesh, 10, 10); } Test::~Test() @@ -345,10 +172,10 @@ void Test::Step() b3_gjkCalls = 0; b3_gjkIters = 0; b3_gjkMaxIters = 0; + b3_convexCache = g_settings.convexCache; b3_convexCalls = 0; b3_convexCacheHits = 0; - b3_enableConvexCache = g_settings.convexCache; - + // Step ProfileBegin(); @@ -369,6 +196,7 @@ void Test::Step() drawFlags += g_settings.drawContactPoints * b3Draw::e_contactPointsFlag; drawFlags += g_settings.drawContactNormals * b3Draw::e_contactNormalsFlag; drawFlags += g_settings.drawContactTangents * b3Draw::e_contactTangentsFlag; + drawFlags += g_settings.drawContactAreas * b3Draw::e_contactAreasFlag; g_debugDraw->SetFlags(drawFlags); m_world.DebugDraw(); @@ -448,7 +276,7 @@ void Test::MouseMove(const Ray3& pw) float32 w1 = 1.0f - t; float32 w2 = t; - b3Vec3 target = w1 * pw.Start() + w2 * pw.End(); + b3Vec3 target = w1 * pw.A() + w2 * pw.B(); m_mouseJoint->SetTarget(target); } } @@ -468,8 +296,8 @@ void Test::MouseLeftDown(const Ray3& pw) } // Perform the ray cast - b3Vec3 p1 = pw.Start(); - b3Vec3 p2 = pw.End(); + b3Vec3 p1 = pw.A(); + b3Vec3 p2 = pw.B(); b3RayCastSingleOutput out; if (m_world.RayCastSingle(&out, p1, p2)) diff --git a/examples/testbed/framework/test_entries.cpp b/examples/testbed/framework/test_entries.cpp index 15ff225..e34c580 100644 --- a/examples/testbed/framework/test_entries.cpp +++ b/examples/testbed/framework/test_entries.cpp @@ -20,20 +20,25 @@ #include #include #include -#include #include #include -#include +#include +#include #include +#include #include #include -#include +#include +#include #include +#include +#include #include #include -#include +#include #include #include +#include #include #include #include @@ -51,27 +56,32 @@ #include #include #include +#include TestEntry g_tests[] = { { "Quickhull Test", &QuickhullTest::Create }, { "Cluster Test", &Cluster::Create }, { "Distance Test", &Distance::Create }, - { "Capsule Distance", &CapsuleDistance::Create }, { "Capsule Collision", &CapsuleCollision::Create }, - { "Capsule and Hull Collision", &CapsuleAndHull::Create }, - { "Hull Collision", &HullAndHull::Create }, + { "Capsule and Hull Collision (1)", &CapsuleAndHullCollision1::Create }, + { "Capsule and Hull Collision (2)", &CapsuleAndHullCollision2::Create }, + { "Hull Collision (1)", &HullAndHull::Create }, + { "Hull Collision (2)", &HullAndHull2::Create }, + { "Capsule and Hull Contact (1)", &CapsuleAndHullContact1::Create }, { "Linear Motion", &LinearMotion::Create }, { "Angular Motion", &AngularMotion::Create }, { "Multiple Shapes", &MultipleShapes::Create }, { "Quadric Shapes", &QuadricShapes::Create }, + { "Thin Boxes", &Thin::Create }, + { "Gyroscopic Test", &GyroTest::Create }, { "Springs", &Spring::Create }, { "Weld Test", &WeldTest::Create }, - { "Newton's Cradle", &NewtonCradle::Create }, + { "Cone Test", &ConeTest::Create }, { "Hinge Motor", &HingeMotor::Create }, { "Hinge Chain", &HingeChain::Create }, { "Ragdoll", &Ragdoll::Create }, - { "Thin Boxes", &Thin::Create }, + { "Newton's Cradle", &NewtonCradle::Create }, { "Mesh Contact Test", &MeshContactTest::Create }, { "Sphere Stack", &SphereStack::Create }, { "Capsule Stack", &CapsuleStack::Create }, @@ -87,5 +97,7 @@ TestEntry g_tests[] = { "Varying Friction", &VaryingFriction::Create }, { "Varying Restitution", &VaryingRestitution::Create }, { "Cloth", &Cloth::Create }, + { "Tumbler", &Tumbler::Create }, + { "Initial Overlap", &InitialOverlap::Create }, { NULL, NULL } }; \ No newline at end of file diff --git a/examples/testbed/tests/angular_motion.h b/examples/testbed/tests/angular_motion.h index 2633819..a5674cc 100644 --- a/examples/testbed/tests/angular_motion.h +++ b/examples/testbed/tests/angular_motion.h @@ -24,56 +24,33 @@ class AngularMotion : public Test public: AngularMotion() { - b3BodyDef bdef; - bdef.type = e_dynamicBody; - bdef.position.Set(0.0f, 0.0f, 0.0f); + b3BodyDef bd; + b3Body* ground = m_world.CreateBody(bd); - m_body = m_world.CreateBody(bdef); + bd.type = e_dynamicBody; + bd.angularVelocity.Set(0.0f, B3_PI, 0.0f); + b3Body* body = m_world.CreateBody(bd); b3CapsuleShape shape; - shape.m_centers[0].Set(0.0f, 1.0f, 0.0f); - shape.m_centers[1].Set(0.0f, -1.0f, 0.0f); + shape.m_centers[0].Set(0.0f, 0.0f, -1.0f); + shape.m_centers[1].Set(0.0f, 0.0f, 1.0f); shape.m_radius = 1.0f; b3ShapeDef sdef; sdef.shape = &shape; sdef.density = 1.0f; - m_body->CreateShape(sdef); - - b3MassData data; - m_body->GetMassData(&data); - - m_body->SetMassData(&data); - - b3Vec3 g(0.0f, 0.0f, 0.0f); - m_world.SetGravity(g); - } - - void Step() - { - Test::Step(); - - b3Vec3 v(0.0f, 0.0f, 0.0f); - m_body->SetLinearVelocity(v); - - b3Vec3 p = m_body->GetSweep().worldCenter; - b3Quat quat = m_body->GetSweep().orientation; - - b3Vec3 axis; - float32 angle; - quat.GetAxisAngle(&axis, &angle); + body->CreateShape(sdef); - b3Vec3 q(0.0f, 0.0f, 0.0f); - m_body->SetTransform(q, axis, angle); + b3SphereJointDef jd; + jd.Initialize(ground, body, b3Vec3(0.0f, 0.0f, 0.0f)); + m_world.CreateJoint(jd); } static Test* Create() { return new AngularMotion(); } - - b3Body* m_body; }; #endif \ No newline at end of file diff --git a/examples/testbed/tests/box_stack.h b/examples/testbed/tests/box_stack.h index 3d04383..34f0efa 100644 --- a/examples/testbed/tests/box_stack.h +++ b/examples/testbed/tests/box_stack.h @@ -33,7 +33,7 @@ public: { g_camera.m_center.Set(2.5f, -2.0f, 5.5f); g_camera.m_zoom = 40.0f; - + { b3BodyDef bdef; bdef.type = b3BodyType::e_staticBody; @@ -45,17 +45,22 @@ public: b3ShapeDef sdef; sdef.shape = &hs; - sdef.userData = NULL; sdef.friction = 1.0f; - b3Shape* shape = body->CreateShape(sdef); + body->CreateShape(sdef); } - b3Vec3 stackOrigin; - stackOrigin.Set(0.0f, 4.05f, 0.0f); + b3Vec3 boxScale(1.0f, 1.0f, 1.0f); + + static b3BoxHull boxHull; + + b3Transform m; + m.rotation = b3Diagonal(boxScale.x, boxScale.y, boxScale.z); + m.position.SetZero(); - b3Vec3 boxScale; - boxScale.Set(2.05f, 2.05f, 2.05f); + boxHull.SetTransform(m); + + b3Vec3 stackOrigin(0.0f, 4.05f, 0.0f); for (u32 i = 0; i < e_rowCount; ++i) { @@ -65,24 +70,25 @@ public: { b3BodyDef bdef; bdef.type = b3BodyType::e_dynamicBody; + bdef.orientation.Set(b3Vec3(0.0f, 1.0f, 0.0f), 0.5f * B3_PI); bdef.position.x = float32(i) * boxScale.x; - bdef.position.y = 1.5f * float32(j) * boxScale.y; + bdef.position.y = 2.5f * float32(j) * boxScale.y; bdef.position.z = float32(k) * boxScale.z; + bdef.position += stackOrigin; b3Body* body = m_world.CreateBody(bdef); b3HullShape hs; - hs.m_hull = &m_boxHull; + hs.m_hull = &boxHull; b3ShapeDef sdef; - sdef.density = 0.5f; + sdef.density = 0.1f; sdef.friction = 0.3f; sdef.shape = &hs; - sdef.userData = NULL; - b3Shape* shape = body->CreateShape(sdef); + body->CreateShape(sdef); } } } @@ -94,4 +100,4 @@ public: } }; -#endif +#endif \ No newline at end of file diff --git a/examples/testbed/tests/capsule_and_hull_collision.h b/examples/testbed/tests/capsule_and_hull_collision_1.h similarity index 86% rename from examples/testbed/tests/capsule_and_hull_collision.h rename to examples/testbed/tests/capsule_and_hull_collision_1.h index 8c56921..6c553b1 100644 --- a/examples/testbed/tests/capsule_and_hull_collision.h +++ b/examples/testbed/tests/capsule_and_hull_collision_1.h @@ -16,18 +16,18 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CAPSULE_HULL_H -#define CAPSULE_HULL_H +#ifndef CAPSULE_HULL_COLLISION_1_H +#define CAPSULE_HULL_COLLISION_1_H -class CapsuleAndHull : public Collide +class CapsuleAndHullCollision1 : public Collide { public: - CapsuleAndHull() + CapsuleAndHullCollision1() { m_xfA.position.Set(0.0f, 0.0f, 0.0f); m_xfA.rotation = b3ConvertQuatToRot(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.55f * B3_PI)); - m_sA.m_centers[0].Set(0.0f, -1.0f, 0.0f); + m_sA.m_centers[0].Set(1.0f, -1.0f, 0.0f); m_sA.m_centers[1].Set(0.0f, 1.0f, 0.0f); m_sA.m_radius = 2.0f; @@ -49,7 +49,7 @@ public: static Test* Create() { - return new CapsuleAndHull(); + return new CapsuleAndHullCollision1(); } b3CapsuleShape m_sA; diff --git a/examples/testbed/tests/capsule_and_hull_collision_2.h b/examples/testbed/tests/capsule_and_hull_collision_2.h new file mode 100644 index 0000000..f1b28b9 --- /dev/null +++ b/examples/testbed/tests/capsule_and_hull_collision_2.h @@ -0,0 +1,60 @@ +/* +* 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 CAPSULE_HULL_2_H +#define CAPSULE_HULL_2_H + +class CapsuleAndHullCollision2 : public Collide +{ +public: + CapsuleAndHullCollision2() + { + m_xfA.position.Set(0.0f, 0.0f, 0.0f); + m_xfA.rotation = b3ConvertQuatToRot(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.55f * B3_PI)); + + m_sA.m_centers[0].Set(0.0f, 0.0f, 0.0f); + m_sA.m_centers[1].Set(0.0f, 0.0f, 0.0f); + m_sA.m_radius = 0.05f; + + m_xfB.position.Set(0.f, 0.0f, 0.0f); + m_xfB.rotation = b3ConvertQuatToRot(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.0f * B3_PI)); + + b3Transform xf; + xf.SetIdentity(); + xf.rotation = b3Diagonal(4.0f, 1.0f, 4.0f); + + m_box.SetTransform(xf); + + m_sB.m_hull = &m_box; + + m_shapeA = &m_sA; + m_shapeB = &m_sB; + m_cache.count = 0; + } + + static Test* Create() + { + return new CapsuleAndHullCollision2(); + } + + b3CapsuleShape m_sA; + b3HullShape m_sB; + b3BoxHull m_box; +}; + +#endif diff --git a/examples/testbed/tests/capsule_and_hull_contact_1.h b/examples/testbed/tests/capsule_and_hull_contact_1.h new file mode 100644 index 0000000..7ad5e2f --- /dev/null +++ b/examples/testbed/tests/capsule_and_hull_contact_1.h @@ -0,0 +1,69 @@ +/* +* 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 CAPSULE_HULL_CONTACT_1_H +#define CAPSULE_HULL_CONTACT_1_H + +class CapsuleAndHullContact1 : public Test +{ +public: + CapsuleAndHullContact1() + { + { + b3BodyDef bd; + b3Body* body = m_world.CreateBody(bd); + + b3HullShape hs; + hs.m_hull = &m_groundHull; + + b3ShapeDef sd; + sd.shape = &hs; + + body->CreateShape(sd); + } + + { + b3BodyDef bdef; + bdef.type = e_dynamicBody; + bdef.position.Set(0.0f, 10.0f, 0.0f); + bdef.orientation.Set(b3Vec3(0.0f, 0.0f, -1.0f), 1.5f * B3_PI); + bdef.linearVelocity.Set(0.005f, -10.0f, 0.005f); + bdef.angularVelocity.Set(2000.0f * B3_PI, 2000.0f * B3_PI, 10000.0f * B3_PI); + + b3Body* body = m_world.CreateBody(bdef); + + b3CapsuleShape capsule; + capsule.m_centers[0].Set(0.0f, 4.0f, 0.0f); + capsule.m_centers[1].Set(0.0f, -4.0f, 0.0f); + capsule.m_radius = 0.5f; + + b3ShapeDef sd; + sd.shape = &capsule; + sd.density = 0.1f; + + body->CreateShape(sd); + } + } + + static Test* Create() + { + return new CapsuleAndHullContact1(); + } +}; + +#endif \ No newline at end of file diff --git a/examples/testbed/tests/capsule_distance.h b/examples/testbed/tests/capsule_distance.h deleted file mode 100644 index fc0f708..0000000 --- a/examples/testbed/tests/capsule_distance.h +++ /dev/null @@ -1,128 +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 CAPSULE_DISTANCE_H -#define CAPSULE_DISTANCE_H - -extern DebugDraw* g_debugDraw; -extern Camera g_camera; - -class CapsuleDistance : public Test -{ -public: - CapsuleDistance() - { - g_camera.m_zoom = 25.0f; - - m_xfA.SetIdentity(); - m_xfA.position.Set(-5.0f, 0.0f, 0.0f); - m_xfA.rotation.SetIdentity(); - m_shapeA.m_centers[0].Set(0.0f, -2.0f, 0.0f); - m_shapeA.m_centers[1].Set(0.0f, 2.0f, 0.0f); - m_shapeA.m_radius = 1.0f; - - m_xfB.SetIdentity(); - m_xfB.position.Set(5.0f, 0.0f, 0.0f); - m_xfB.rotation.SetIdentity(); - m_shapeB.m_centers[0].Set(0.0f, -2.0f, 0.0f); - m_shapeB.m_centers[1].Set(0.0f, 2.0f, 0.0f); - m_shapeB.m_radius = 1.0f; - } - - void Step() - { - b3Capsule edgeA; - edgeA.vertices[0] = m_xfA * m_shapeA.m_centers[0]; - edgeA.vertices[1] = m_xfA * m_shapeA.m_centers[1]; - edgeA.radius = m_shapeA.m_radius; - - b3Capsule edgeB; - edgeB.vertices[0] = m_xfB * m_shapeB.m_centers[0]; - edgeB.vertices[1] = m_xfB * m_shapeB.m_centers[1]; - edgeB.radius = m_shapeB.m_radius; - - b3Vec3 pointA, pointB; - b3ClosestPointsOnSegments(&pointA, &pointB, edgeA.vertices[0], edgeA.vertices[1], edgeB.vertices[1], edgeB.vertices[0]); - - if (b3Distance(pointA, pointB) > 0.0f) - { - g_debugDraw->DrawPoint(pointA, 4.0f, b3Color(0.0f, 1.0f, 0.0f)); - g_debugDraw->DrawPoint(pointB, 4.0f, b3Color(0.0f, 1.0f, 0.0f)); - - g_debugDraw->DrawSegment(pointA, pointB, b3Color(1.0f, 1.0f, 1.0f)); - } - - g_debugDraw->DrawTransform(m_xfA); - g_debugDraw->DrawTransform(m_xfB); - - m_world.DrawShape(m_xfA, &m_shapeA); - m_world.DrawShape(m_xfB, &m_shapeB); - } - - void KeyDown(int key) - { - if (key == GLFW_KEY_LEFT) - { - m_xfB.position.x -= 0.05f; - } - - if (key == GLFW_KEY_RIGHT) - { - m_xfB.position.x += 0.05f; - } - - if (key == GLFW_KEY_UP) - { - m_xfB.position.y += 0.05f; - } - - if (key == GLFW_KEY_DOWN) - { - m_xfB.position.y -= 0.05f; - } - - if (key == GLFW_KEY_X) - { - b3Quat qx(b3Vec3(1.0f, 0.0f, 0.0f), 0.05f * B3_PI); - b3Mat33 xfx = b3ConvertQuatToRot(qx); - - m_xfB.rotation = m_xfB.rotation * xfx; - } - - if (key == GLFW_KEY_Y) - { - b3Quat qy(b3Vec3(0.0f, 1.0f, 0.0f), 0.05f * B3_PI); - b3Mat33 xfy = b3ConvertQuatToRot(qy); - - m_xfB.rotation = m_xfB.rotation * xfy; - } - } - - static Test* Create() - { - return new CapsuleDistance(); - } - - b3CapsuleShape m_shapeA; - b3Transform m_xfA; - - b3CapsuleShape m_shapeB; - b3Transform m_xfB; -}; - -#endif diff --git a/examples/testbed/tests/capsule_stack.h b/examples/testbed/tests/capsule_stack.h index 4bd75ff..f0462b8 100644 --- a/examples/testbed/tests/capsule_stack.h +++ b/examples/testbed/tests/capsule_stack.h @@ -24,9 +24,9 @@ class CapsuleStack : public Test public: enum { - e_rowCount = 5, + e_rowCount = 1, e_columnCount = 5, - e_depthCount = 5 + e_depthCount = 1 }; CapsuleStack() @@ -57,8 +57,8 @@ public: b3ShapeDef sdef; sdef.shape = &capsule; - sdef.density = 1.0f; - sdef.friction = 0.3f; + sdef.density = 0.1f; + sdef.friction = 0.4f; const u32 c = e_rowCount * e_columnCount * e_depthCount; b3Body* bs[c]; @@ -78,7 +78,7 @@ public: bdef.type = b3BodyType::e_dynamicBody; bdef.position.x = (2.0f + separation) * float32(i) * (0.5f * height + radius); - bdef.position.y = (2.0f + separation) * float32(j) * radius; + bdef.position.y = 2.0f + (2.0f + separation) * float32(j) * radius; bdef.position.z = (2.0f + separation) * float32(k) * radius; bdef.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.5f * B3_PI); @@ -108,7 +108,7 @@ public: b3Vec3 position = p - center; // move up - position.y += 0.5f * aabb.Height() + radius; + position.y += 5.0f + 0.5f * aabb.Height() + radius; // maintain orientation b3Vec3 axis; diff --git a/examples/testbed/tests/cloth_test.h b/examples/testbed/tests/cloth_test.h index dcd1575..5131a24 100644 --- a/examples/testbed/tests/cloth_test.h +++ b/examples/testbed/tests/cloth_test.h @@ -33,18 +33,24 @@ public: b3ClothDef def; def.mesh = m_meshes + e_clothMesh; def.density = 0.2f; - def.gravity.Set(-10.0f, 1.0f, 0.0f); - def.k1 = 0.5f; - def.k2 = 0.05f; - def.kd = 0.1f; + def.gravity.Set(2.5f, 5.0f, -10.0f); + def.k1 = 0.2f; + def.k2 = 0.1f; + def.kd = 0.005f; def.r = 1.0f; m_cloth.Initialize(def); + m_aabb.m_lower.Set(-5.0f, -1.0f, -6.0f); + m_aabb.m_upper.Set(5.0f, 1.0f, -4.0f); + b3Particle* vs = m_cloth.GetVertices(); - for (u32 i = 0; i < 5; ++i) + for (u32 i = 0; i < m_cloth.GetVertexCount(); ++i) { - vs[i].im = 0.0f; + if (m_aabb.Contains(vs[i].p)) + { + vs[i].im = 0.0f; + } } } @@ -66,6 +72,8 @@ public: m_cloth.Step(dt, g_settings.positionIterations); m_cloth.Draw(g_debugDraw); + + //g_debugDraw->DrawAABB(m_aabb, b3Color_black); } static Test* Create() @@ -74,6 +82,7 @@ public: } b3Cloth m_cloth; + b3AABB3 m_aabb; }; #endif \ No newline at end of file diff --git a/examples/testbed/tests/collide_test.h b/examples/testbed/tests/collide_test.h index 47303a5..7157dab 100644 --- a/examples/testbed/tests/collide_test.h +++ b/examples/testbed/tests/collide_test.h @@ -38,33 +38,33 @@ public: cache.featureCache.m_featurePair.state = b3SATCacheType::e_empty; b3Manifold manifold; - manifold.GuessImpulses(); + manifold.Initialize(); b3CollideShapeAndShape(manifold, m_xfA, m_shapeA, m_xfB, m_shapeB, &cache); - - b3WorldManifold wm; - wm.Initialize(&manifold, m_xfA, m_shapeA->m_radius, m_xfB, m_shapeB->m_radius); - for (u32 i = 0; i < wm.pointCount; ++i) + for (u32 i = 0; i < manifold.pointCount; ++i) { - b3WorldManifoldPoint* wmp = wm.points + i; - b3Vec3 pw = wmp->point; + b3WorldManifold wm; + wm.Initialize(&manifold, m_shapeA->m_radius, m_xfA, m_shapeB->m_radius, m_xfB); + + b3Vec3 pw = wm.points[i].point; b3Vec2 ps = g_camera.ConvertWorldToScreen(pw); g_debugDraw->DrawPoint(pw, 4.0f, b3Color(0.0f, 1.0f, 0.0f)); - g_debugDraw->DrawSegment(pw, pw + wmp->normal, b3Color(1.0f, 1.0f, 1.0f)); + g_debugDraw->DrawSegment(pw, pw + wm.points[i].normal, b3Color(1.0f, 1.0f, 1.0f)); } - if (wm.pointCount > 0) + if (g_settings.drawFaces) { - g_debugDraw->DrawPoint(wm.center, 4.0f, b3Color(1.0f, 1.0f, 0.0f)); - g_debugDraw->DrawSegment(wm.center, wm.center + wm.normal, b3Color(1.0f, 1.0f, 0.0f)); - g_debugDraw->DrawSegment(wm.center, wm.center + wm.tangent1, b3Color(1.0f, 1.0f, 0.0f)); - g_debugDraw->DrawSegment(wm.center, wm.center + wm.tangent2, b3Color(1.0f, 1.0f, 0.0f)); + g_debugDraw->DrawShape(m_shapeA, b3Color(1.0f, 1.0f, 1.0f, 0.5f), m_xfA); + g_debugDraw->DrawShape(m_shapeB, b3Color(1.0f, 1.0f, 1.0f, 0.5f), m_xfB); } - m_world.DrawShape(m_xfA, m_shapeA); - m_world.DrawShape(m_xfB, m_shapeB); + if (g_settings.drawVerticesEdges) + { + m_world.DrawShape(m_xfA, m_shapeA); + m_world.DrawShape(m_xfB, m_shapeB); + } } virtual void KeyDown(int key) diff --git a/examples/testbed/tests/cone_test.h b/examples/testbed/tests/cone_test.h new file mode 100644 index 0000000..f262fc2 --- /dev/null +++ b/examples/testbed/tests/cone_test.h @@ -0,0 +1,84 @@ +/* +* 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 CONE_TEST_H +#define CONE_TEST_H + +class ConeTest : public Test +{ +public: + ConeTest() + { + g_camera.m_zoom = 15.0f; + g_camera.m_q = b3Quat(b3Vec3(0.0f, 1.0f, 0.0f), 0.15f * B3_PI); + g_camera.m_q = g_camera.m_q * b3Quat(b3Vec3(1.0f, 0.0f, 0.0f), -0.2f * B3_PI); + g_camera.m_center.Set(0.0f, 0.0f, 0.0f); + + b3Body* ref; + b3Body* head; + + { + b3BodyDef bd; + //bd.type = e_dynamicBody; + bd.position.Set(0.0f, 0.0f, 0.0f); + ref = m_world.CreateBody(bd); + } + + { + b3BodyDef bd; + bd.type = e_dynamicBody; + bd.position.Set(0.0f, 2.0f, 0.0f); + bd.angularVelocity.Set(0.0f, 0.05f * B3_PI, 0.0f); + head = m_world.CreateBody(bd); + + b3CapsuleShape cs; + cs.m_centers[0].Set(0.0f, 0.15f, 0.0f); + cs.m_centers[1].Set(0.0f, -0.15f, 0.0f); + cs.m_radius = 0.5f; + + b3ShapeDef sd; + sd.shape = &cs; + sd.density = 10.0f; + head->CreateShape(sd); + } + + { + b3Vec3 anchor(0.0f, 0.0f, 0.0f); + b3Vec3 axis(0.0f, 1.0f, 0.0f); + float32 coneAngle = 0.5f * B3_PI; + + b3ConeJointDef cd; + cd.Initialize(ref, head, axis, anchor, coneAngle); + cd.enableLimit = true; + + b3ConeJoint* cj = (b3ConeJoint*)m_world.CreateJoint(cd); + } + + // Invalidate the orientation + b3Vec3 axis(1.0f, 0.0f, 0.0f); + float32 angle = B3_PI; + head->SetTransform(head->GetPosition(), axis, angle); + } + + static Test* Create() + { + return new ConeTest(); + } +}; + +#endif diff --git a/examples/testbed/tests/distance_test.h b/examples/testbed/tests/distance_test.h index 412087d..89f0848 100644 --- a/examples/testbed/tests/distance_test.h +++ b/examples/testbed/tests/distance_test.h @@ -54,22 +54,22 @@ public: { b3GJKFeaturePair featurePair = b3GetFeaturePair(m_cache); - for (u32 i = 0; i < featurePair.countA; ++i) + for (u32 i = 0; i < featurePair.count1; ++i) { - u32 index = featurePair.indexA[i]; + u32 index = featurePair.index1[i]; g_debugDraw->DrawPoint(m_xfA * m_proxyA.GetVertex(index), 4.0f, b3Color(1.0f, 1.0f, 0.0f)); } - for (u32 i = 0; i < featurePair.countB; ++i) + for (u32 i = 0; i < featurePair.count2; ++i) { - u32 index = featurePair.indexB[i]; + u32 index = featurePair.index2[i]; g_debugDraw->DrawPoint(m_xfB * m_proxyB.GetVertex(index), 4.0f, b3Color(1.0f, 1.0f, 0.0f)); } } - g_debugDraw->DrawPoint(out.pointA, 4.0f, b3Color(0.0f, 1.0f, 0.0f)); - g_debugDraw->DrawPoint(out.pointB, 4.0f, b3Color(0.0f, 1.0f, 0.0f)); - g_debugDraw->DrawSegment(out.pointA, out.pointB, b3Color(1.0f, 1.0f, 1.0f)); + g_debugDraw->DrawPoint(out.point1, 4.0f, b3Color(0.0f, 1.0f, 0.0f)); + g_debugDraw->DrawPoint(out.point2, 4.0f, b3Color(0.0f, 1.0f, 0.0f)); + g_debugDraw->DrawSegment(out.point1, out.point2, b3Color(1.0f, 1.0f, 1.0f)); g_debugDraw->DrawTransform(m_xfA); g_debugDraw->DrawTransform(m_xfB); diff --git a/examples/testbed/tests/gyro_test.h b/examples/testbed/tests/gyro_test.h new file mode 100644 index 0000000..38c7f9c --- /dev/null +++ b/examples/testbed/tests/gyro_test.h @@ -0,0 +1,119 @@ +/* +* 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 GYRO_TEST_H +#define GYRO_TEST_H + +#include + +extern DebugDraw* g_debugDraw; +extern Camera g_camera; +extern Settings g_settings; + +class GyroTest : public Test +{ +public: + GyroTest() + { + { + b3StackArray points; + ConstructCylinder(points, 0.95f, 4.0f); + + const u32 size = qhGetMemorySize(points.Count()); + void* p = b3Alloc(size); + + qhHull hull; + hull.Construct(p, points); + m_cylinderHull = ConvertHull(hull); + + b3Free(p); + } + + { + b3BodyDef bd; + b3Body* ground = m_world.CreateBody(bd); + + b3HullShape hs; + hs.m_hull = &m_groundHull; + + b3ShapeDef sd; + sd.shape = &hs; + + ground->CreateShape(sd); + } + + { + b3BodyDef bdef; + bdef.type = e_dynamicBody; + bdef.orientation.Set(b3Vec3(1.0f, 0.0f, 0.0f), 0.5f * B3_PI); + bdef.position.Set(0.0f, 10.0f, 0.0f); + bdef.angularVelocity.Set(0.0f, 0.0f, 4.0f * B3_PI); + + b3Body* body = m_world.CreateBody(bdef); + + { + b3Transform xf; + xf.position.SetZero(); + xf.rotation = b3Diagonal(1.0f, 0.5f, 7.0f); + m_rotorBox.SetTransform(xf); + + b3HullShape hull; + hull.m_hull = &m_rotorBox; + + b3ShapeDef sdef; + sdef.density = 0.1f; + sdef.shape = &hull; + + body->CreateShape(sdef); + } + + { + b3HullShape hull; + hull.m_hull = &m_cylinderHull; + + b3ShapeDef sdef; + sdef.density = 0.2f; + sdef.shape = &hull; + + body->CreateShape(sdef); + } + } + + m_world.SetGravity(b3Vec3(0.0f, 0.0f, 0.0f)); + } + + ~GyroTest() + { + { + b3Free(m_cylinderHull.vertices); + b3Free(m_cylinderHull.edges); + b3Free(m_cylinderHull.faces); + b3Free(m_cylinderHull.planes); + } + } + + static Test* Create() + { + return new GyroTest(); + } + + b3BoxHull m_rotorBox; + b3Hull m_cylinderHull; +}; + +#endif \ No newline at end of file diff --git a/examples/testbed/tests/hinge_chain.h b/examples/testbed/tests/hinge_chain.h index b591ba2..376b7e3 100644 --- a/examples/testbed/tests/hinge_chain.h +++ b/examples/testbed/tests/hinge_chain.h @@ -28,6 +28,14 @@ public: g_camera.m_q = b3Quat(b3Vec3(0.0f, 1.0f, 0.0f), 0.15f * B3_PI); g_camera.m_q = g_camera.m_q * b3Quat(b3Vec3(1.0f, 0.0f, 0.0f), -0.15f * B3_PI); g_camera.m_center.SetZero(); + + static b3BoxHull doorHull; + { + b3Transform xf; + xf.position.SetZero(); + xf.rotation = b3Diagonal(2.0f, 4.0f, 0.5f); + doorHull.SetTransform(xf); + } float32 x = -50.0f; float32 y = 0.0f; @@ -39,7 +47,7 @@ public: lastHinge = m_world.CreateBody(bd); b3HullShape hull; - hull.m_hull = &m_doorHull; + hull.m_hull = &doorHull; b3ShapeDef sdef; sdef.shape = &hull; @@ -57,7 +65,7 @@ public: b3Body* hinge = m_world.CreateBody(bd); b3HullShape hull; - hull.m_hull = &m_doorHull; + hull.m_hull = &doorHull; b3ShapeDef sdef; sdef.shape = &hull; @@ -71,7 +79,7 @@ public: b3RevoluteJointDef jd; jd.Initialize(lastHinge, hinge, hingeAxis, hingeAnchor, 0.0f, 0.5f * B3_PI); - jd.collideLinked = true; + jd.collideLinked = false; b3RevoluteJoint* rj = (b3RevoluteJoint*)m_world.CreateJoint(jd); } diff --git a/examples/testbed/tests/hinge_motor.h b/examples/testbed/tests/hinge_motor.h index fbcb7db..829f42b 100644 --- a/examples/testbed/tests/hinge_motor.h +++ b/examples/testbed/tests/hinge_motor.h @@ -41,50 +41,60 @@ public: { b3BodyDef bd; - bd.position.Set(-2.0f, 5.05f, 0.0f); + bd.type = b3BodyType::e_staticBody; + bd.position.Set(0.0f, 7.0f, 0.0f); hinge = m_world.CreateBody(bd); b3CapsuleShape shape; - shape.m_centers[0].Set(0.0f, -3.5f, 0.0f); - shape.m_centers[1].Set(0.0f, 3.5f, 0.0f); + shape.m_centers[0].Set(0.0f, 0.0f, -4.0f); + shape.m_centers[1].Set(0.0f, 0.0f, 4.0f); shape.m_radius = 0.5f; - + b3ShapeDef sd; sd.shape = &shape; sd.density = 1.0f; hinge->CreateShape(sd); - m_hinge = hinge; + + m_body = hinge; } { b3BodyDef bd; bd.type = b3BodyType::e_dynamicBody; - bd.position.Set(1.0f, 5.05f, 0.0f); + bd.position.Set(2.0f, 7.0f, 0.0f); door = m_world.CreateBody(bd); + m_doorBox.Set(1.0f, 0.5f, 4.0f); + b3HullShape hull; - hull.m_hull = &m_doorHull; + hull.m_hull = &m_doorBox; b3ShapeDef sdef; sdef.shape = &hull; - sdef.density = 2.0f; + sdef.density = 1.0f; door->CreateShape(sdef); } { - b3Vec3 hingeAxis(0.0f, 1.0f, 0.0f); - b3Vec3 hingeAnchor(-2.0f, 5.0f, 0.0f); + b3Vec3 axis(0.0f, 0.0f, 1.0f); + b3Vec3 anchor(0.0f, 7.0f, 0.0f); b3RevoluteJointDef jd; - jd.Initialize(hinge, door, hingeAxis, hingeAnchor, 0.0f, 0.5f * B3_PI); + jd.Initialize(hinge, door, axis, anchor, 0.0f, B3_PI); jd.motorSpeed = B3_PI; - jd.maxMotorTorque = 10000.0f; - + jd.maxMotorTorque = door->GetMass() * 10000.0f; + jd.enableMotor = true; + m_rj = (b3RevoluteJoint*)m_world.CreateJoint(jd); } + + // Invalidate the orientation + b3Vec3 axis(1.0f, 0.0f, 0.0f); + float32 angle = B3_PI; + door->SetTransform(door->GetPosition(), axis, angle); } void KeyDown(int button) @@ -101,17 +111,17 @@ public: if (button == GLFW_KEY_D) { - m_hinge->SetType(e_dynamicBody); + m_body->SetType(e_dynamicBody); } if (button == GLFW_KEY_S) { - m_hinge->SetType(e_staticBody); + m_body->SetType(e_staticBody); } if (button == GLFW_KEY_K) { - m_hinge->SetType(e_kinematicBody); + m_body->SetType(e_kinematicBody); } } @@ -120,7 +130,8 @@ public: return new HingeMotor(); } - b3Body* m_hinge; + b3BoxHull m_doorBox; + b3Body* m_body; b3RevoluteJoint* m_rj; }; diff --git a/examples/testbed/tests/hull_collision.h b/examples/testbed/tests/hull_collision.h index 5af70e1..f6eea52 100644 --- a/examples/testbed/tests/hull_collision.h +++ b/examples/testbed/tests/hull_collision.h @@ -24,21 +24,24 @@ class HullAndHull : public Collide public: HullAndHull() { - b3Transform xf; - xf.rotation = b3Diagonal(1.0f, 2.0f, 1.0f); - xf.position.SetZero(); + b3Transform m; + m.rotation = b3Diagonal(1.0f, 2.0f, 1.0f); + m.position.Set(0.0f, 2.0f, 0.0f); + m_box1.SetTransform(m); - m_box.SetTransform(xf); + m.rotation = b3Diagonal(1.0f, 1.0f, 1.0f); + m.position.Set(0.0f, 0.0f, 0.0f); + m_box2.SetTransform(m); - b3Quat qA(0.0f, 1.0f, 0.0f, 0.025f * B3_PI); m_xfA.SetIdentity(); - m_xfA.position.Set(0.0186814368f, 1.96078217f, 0.0253920462f); - m_xfA.rotation = b3ConvertQuatToRot(qA); - m_sA.m_hull = &m_box; + m_xfA.position.SetZero(); + m_xfA.rotation.SetIdentity(); + m_sA.m_hull = &m_box1; m_xfB.SetIdentity(); - m_xfB.position.Set(0.f, 0.0f, 0.0f); - m_sB.m_hull = &m_box; + m_xfB.position.Set(0.0f, 0.0f, 0.0f); + m_xfB.rotation.SetIdentity(); + m_sB.m_hull = &m_box2; m_cache.count = 0; m_shapeA = &m_sA; @@ -50,7 +53,8 @@ public: return new HullAndHull(); } - b3BoxHull m_box; + b3BoxHull m_box1; + b3BoxHull m_box2; b3HullShape m_sA; b3HullShape m_sB; }; diff --git a/examples/testbed/tests/hull_collision_2.h b/examples/testbed/tests/hull_collision_2.h new file mode 100644 index 0000000..e4b0a1e --- /dev/null +++ b/examples/testbed/tests/hull_collision_2.h @@ -0,0 +1,60 @@ +/* +* 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 HULL_HULL2_H +#define HULL_HULL2_H + +class HullAndHull2 : public Collide +{ +public: + HullAndHull2() + { + b3Transform xf; + xf.position.SetZero(); + xf.rotation = b3Diagonal(1.0f, 2.0f, 1.0f); + m_box.SetTransform(xf); + + m_sA.m_hull = &m_box; + m_sB.m_hull = &m_box; + + m_xfA.position.Set(1.500000, 1.000000, 0.000000); + m_xfA.rotation.x.Set(0.707107, 0.000000, -0.707107); + m_xfA.rotation.y.Set(0.000000, 1.000000, 0.000000); + m_xfA.rotation.z.Set(0.707107, 0.000000, 0.707107); + + m_xfB.position.Set(-1.300000, 0.000000, 0.000000); + m_xfB.rotation.x.Set(0.809017, 0.266849, -0.523721); + m_xfB.rotation.y.Set(0.000000, 0.891007, 0.453991); + m_xfB.rotation.z.Set(0.587785, -0.367286, 0.720840); + + m_cache.count = 0; + m_shapeA = &m_sA; + m_shapeB = &m_sB; + } + + static Test* Create() + { + return new HullAndHull2(); + } + + b3BoxHull m_box; + b3HullShape m_sA; + b3HullShape m_sB; +}; + +#endif diff --git a/examples/testbed/tests/initial_overlap.h b/examples/testbed/tests/initial_overlap.h new file mode 100644 index 0000000..c54576b --- /dev/null +++ b/examples/testbed/tests/initial_overlap.h @@ -0,0 +1,102 @@ +/* +* 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 INITIAL_OVERLAP_H +#define INITIAL_OVERLAP_H + +class InitialOverlap : public Test +{ +public: + InitialOverlap() + { + g_camera.m_center.Set(2.0f, -2.0f, 0.0f); + g_camera.m_zoom = 10.0f; + + { + b3BodyDef bd; + b3Body* body = m_world.CreateBody(bd); + + b3HullShape hs; + hs.m_hull = &m_groundHull; + + b3ShapeDef sd; + sd.shape = &hs; + + body->CreateShape(sd); + } + + b3Vec3 boxScale(1.0f, 0.5f, 2.0f); + + static b3BoxHull boxHull; + + b3Transform m; + m.rotation = b3Diagonal(boxScale.x, boxScale.y, boxScale.z); + m.position.SetZero(); + + boxHull.SetTransform(m); + + { + b3BodyDef bd; + bd.type = e_dynamicBody; + bd.position.Set(0.0f, 1.0f, 0.0f); + + b3Body* body = m_world.CreateBody(bd); + + b3HullShape hs; + hs.m_hull = &boxHull; + + b3ShapeDef sd; + sd.shape = &hs; + sd.density = 0.1f; + sd.friction = 0.3f; + + body->CreateShape(sd); + } + + { + b3BodyDef bd; + bd.type = e_dynamicBody; + bd.position.Set(0.0f, 1.5f, 0.0f); + + b3Quat q_y(b3Vec3(0.0f, 1.0f, 0.0f), 0.4f * B3_PI); + b3Quat q_z(b3Vec3(0.0f, 0.0f, 1.0f), 0.04f * B3_PI); + b3Quat q = q_z * q_y; + + bd.orientation = q; + + b3Body* body = m_world.CreateBody(bd); + + b3HullShape hs; + hs.m_hull = &boxHull; + + b3ShapeDef sd; + sd.shape = &hs; + sd.density = 0.1f; + sd.friction = 0.3f; + + body->CreateShape(sd); + } + } + + static Test* Create() + { + return new InitialOverlap(); + } +}; + +#endif \ No newline at end of file diff --git a/examples/testbed/tests/jenga.h b/examples/testbed/tests/jenga.h index 288ba8a..377cad3 100644 --- a/examples/testbed/tests/jenga.h +++ b/examples/testbed/tests/jenga.h @@ -46,8 +46,15 @@ public: body->CreateShape(sd); } - b3Vec3 boxScale; - boxScale.Set(1.0f, 0.5f, 3.0f); + b3Vec3 boxScale(1.0f, 0.5f, 3.0f); + + static b3BoxHull boxHull; + + b3Transform m; + m.rotation = b3Diagonal(boxScale.x, boxScale.y, boxScale.z); + m.position.SetZero(); + + boxHull.SetTransform(m); float32 y = 2.0f; @@ -65,12 +72,12 @@ public: b3Body* body = m_world.CreateBody(bd); b3HullShape hs; - hs.m_hull = &m_plankHull; + hs.m_hull = &boxHull; b3ShapeDef sd; sd.shape = &hs; sd.density = 0.1f; - sd.friction = 0.1f; + sd.friction = 0.3f; body->CreateShape(sd); } @@ -91,7 +98,7 @@ public: b3Body* body = m_world.CreateBody(bd); b3HullShape hs; - hs.m_hull = &m_plankHull; + hs.m_hull = &boxHull; b3ShapeDef sd; sd.shape = &hs; @@ -111,4 +118,4 @@ public: } }; -#endif +#endif \ No newline at end of file diff --git a/examples/testbed/tests/multiple_shapes.h b/examples/testbed/tests/multiple_shapes.h index 76b1b9f..4d4808f 100644 --- a/examples/testbed/tests/multiple_shapes.h +++ b/examples/testbed/tests/multiple_shapes.h @@ -24,8 +24,6 @@ class MultipleShapes : public Test public: MultipleShapes() { - g_camera.m_center.Set(2.0f, -2.0f, 0.0f); - g_camera.m_zoom = 50.0f; g_settings.drawCenterOfMasses = true; { @@ -79,6 +77,7 @@ public: { b3BodyDef bd; bd.type = e_dynamicBody; + bd.position.Set(0.0f, 0.0f, 0.0f); bd.angularVelocity.Set(0.0f, 200.0f * B3_PI, 0.0f); b3Body* body = m_world.CreateBody(bd); diff --git a/examples/testbed/tests/quadric_shapes.h b/examples/testbed/tests/quadric_shapes.h index f1d49f8..30cb344 100644 --- a/examples/testbed/tests/quadric_shapes.h +++ b/examples/testbed/tests/quadric_shapes.h @@ -34,35 +34,6 @@ public: g_camera.m_zoom = 20.0f; g_settings.drawCenterOfMasses = true; - { - b3StackArray points; - ConstructCone(points); - - u32 size = qhGetMemorySize(points.Count()); - void* p = b3Alloc(size); - - qhHull hull; - hull.Construct(p, points); - m_coneHull = ConvertHull(hull); - - b3Free(p); - } - - { - - b3StackArray points; - ConstructCylinder(points); - - const u32 size = qhGetMemorySize(points.Count()); - void* p = b3Alloc(size); - - qhHull hull; - hull.Construct(p, points); - m_cylinderHull = ConvertHull(hull); - - b3Free(p); - } - { b3BodyDef bd; b3Body* ground = m_world.CreateBody(bd); @@ -79,10 +50,63 @@ public: { b3BodyDef bdef; bdef.type = e_dynamicBody; - bdef.position.Set(2.0f, 5.0f, 0.0f); + bdef.position.Set(-10.0f, 5.0f, 0.0f); b3Body* body = m_world.CreateBody(bdef); + b3SphereShape sphere; + sphere.m_center.SetZero(); + sphere.m_radius = 1.0f; + + b3ShapeDef sdef; + sdef.density = 0.1f; + sdef.friction = 0.3f; + sdef.shape = &sphere; + + body->CreateShape(sdef); + } + + { + b3BodyDef bdef; + bdef.type = e_dynamicBody; + bdef.position.Set(-5.0f, 5.0f, 0.0f); + + b3Body* body = m_world.CreateBody(bdef); + + b3CapsuleShape capsule; + capsule.m_centers[0].Set(0.0f, 0.0f, -1.0f); + capsule.m_centers[1].Set(0.0f, 0.0f, 1.0f); + capsule.m_radius = 1.0f; + + b3ShapeDef sdef; + sdef.density = 0.1f; + sdef.friction = 0.2f; + sdef.shape = &capsule; + + body->CreateShape(sdef); + } + + { + b3BodyDef bdef; + bdef.type = e_dynamicBody; + bdef.position.Set(0.0f, 5.0f, 0.0f); + + b3Body* body = m_world.CreateBody(bdef); + + { + b3StackArray points; + ConstructCone(points); + + u32 size = qhGetMemorySize(points.Count()); + void* p = b3Alloc(size); + + qhHull hull; + hull.Construct(p, points); + m_coneHull = ConvertHull(hull); + + b3Free(p); + } + b3HullShape hull; hull.m_hull = &m_coneHull; @@ -97,16 +121,30 @@ public: { b3BodyDef bdef; bdef.type = e_dynamicBody; - bdef.position.Set(-2.0f, 5.0f, 0.0f); + bdef.position.Set(4.0f, 5.0f, 0.0f); b3Body* body = m_world.CreateBody(bdef); + { + b3StackArray points; + ConstructCylinder(points); + + const u32 size = qhGetMemorySize(points.Count()); + void* p = b3Alloc(size); + + qhHull hull; + hull.Construct(p, points); + m_cylinderHull = ConvertHull(hull); + + b3Free(p); + } + b3HullShape hull; hull.m_hull = &m_cylinderHull; b3ShapeDef sdef; sdef.density = 0.1f; - sdef.friction = 0.3f; + sdef.friction = 0.2f; sdef.shape = &hull; body->CreateShape(sdef); diff --git a/examples/testbed/tests/quickhull_test.h b/examples/testbed/tests/quickhull_test.h index 6fcdadf..33a8f0a 100644 --- a/examples/testbed/tests/quickhull_test.h +++ b/examples/testbed/tests/quickhull_test.h @@ -266,28 +266,26 @@ inline b3Hull ConvertHull(const qhHull& hull) inline void ConstructCylinder(b3Array& points, float32 radius = 1.0f, float32 height = 1.0f) { + u32 kEdgeCount = 20; + float32 kAngleInc = 2.0f * B3_PI / float32(kEdgeCount); + b3Vec3 normal(0.0f, 1.0f, 0.0f); + b3Quat q(normal, kAngleInc); + + points.Resize(4 * kEdgeCount); + + u32 j = 0; + { b3Vec3 center(0.0f, 0.0f, 0.0f); - - b3Vec3 normal; - normal.Set(0.0f, 1.0f, 0.0f); - - u32 kEdgeCount = 20; - float32 kAngleInc = 2.0f * B3_PI / float32(kEdgeCount); - float32 cosInc = cos(kAngleInc); - float32 sinInc = sin(kAngleInc); - float32 tInc = 1.0f - cosInc; - - b3Vec3 n1 = b3Perp(normal); + b3Vec3 n1(1.0f, 0.0f, 0.0f); b3Vec3 v1 = center + radius * n1; for (u32 i = 0; i < kEdgeCount; ++i) { - // Rodrigues' rotation formula - b3Vec3 n2 = cosInc * n1 + sinInc * b3Cross(normal, n1) + tInc * b3Dot(normal, n1) * normal; + b3Vec3 n2 = b3Mul(q, n1); b3Vec3 v2 = center + radius * n2; - points.PushBack(v1); - points.PushBack(v2); + points[j++] = v1; + points[j++] = v2; n1 = n2; v1 = v2; @@ -296,26 +294,15 @@ inline void ConstructCylinder(b3Array& points, float32 radius = 1.0f, fl { b3Vec3 center(0.0f, height, 0.0f); - - b3Vec3 normal; - normal.Set(0.0f, 1.0f, 0.0f); - - u32 kEdgeCount = 20; - float32 kAngleInc = 2.0f * B3_PI / float32(kEdgeCount); - float32 cosInc = cos(kAngleInc); - float32 sinInc = sin(kAngleInc); - float32 tInc = 1.0f - cosInc; - - b3Vec3 n1 = b3Perp(normal); + b3Vec3 n1(1.0f, 0.0f, 0.0f); b3Vec3 v1 = center + radius * n1; for (u32 i = 0; i < kEdgeCount; ++i) { - // Rodrigues' rotation formula - b3Vec3 n2 = cosInc * n1 + sinInc * b3Cross(normal, n1) + tInc * b3Dot(normal, n1) * normal; + b3Vec3 n2 = b3Mul(q, n1); b3Vec3 v2 = center + radius * n2; - points.PushBack(v1); - points.PushBack(v2); + points[j++] = v1; + points[j++] = v2; n1 = n2; v1 = v2; @@ -325,36 +312,34 @@ inline void ConstructCylinder(b3Array& points, float32 radius = 1.0f, fl inline void ConstructCone(b3Array& points, float32 radius = 1.0f, float32 height = 1.0f) { + u32 kEdgeCount = 20; + float32 kAngleInc = 2.0f * B3_PI / float32(kEdgeCount); + b3Vec3 normal(0.0f, 1.0f, 0.0f); + b3Quat q(normal, kAngleInc); + + points.Resize(2 * kEdgeCount + 1); + + u32 j = 0; + { b3Vec3 center(0.0f, 0.0f, 0.0f); - - b3Vec3 normal; - normal.Set(0.0f, 1.0f, 0.0f); - - u32 kEdgeCount = 20; - float32 kAngleInc = 2.0f * B3_PI / float32(kEdgeCount); - float32 cosInc = cos(kAngleInc); - float32 sinInc = sin(kAngleInc); - float32 tInc = 1.0f - cosInc; - - b3Vec3 n1 = b3Perp(normal); + b3Vec3 n1(1.0f, 0.0f, 0.0f); b3Vec3 v1 = center + radius * n1; for (u32 i = 0; i < kEdgeCount; ++i) { - // Rodrigues' rotation formula - b3Vec3 n2 = cosInc * n1 + sinInc * b3Cross(normal, n1) + tInc * b3Dot(normal, n1) * normal; + b3Vec3 n2 = b3Mul(q, n1); b3Vec3 v2 = center + radius * n2; - points.PushBack(v1); - points.PushBack(v2); + points[j++] = v1; + points[j++] = v2; n1 = n2; v1 = v2; } } - + b3Vec3 c(0.0f, height, 0.0f); - points.PushBack(c); + points[j++] = c; } class QuickhullTest : public Test diff --git a/examples/testbed/tests/ragdoll.h b/examples/testbed/tests/ragdoll.h index bd1a616..4942ebb 100644 --- a/examples/testbed/tests/ragdoll.h +++ b/examples/testbed/tests/ragdoll.h @@ -35,7 +35,7 @@ public: sd.shape = &hs; ground->CreateShape(sd); } - + b3Body* head; b3Body* hip; b3Body* lArm; @@ -106,10 +106,10 @@ public: b3ShapeDef sd; sd.shape = &cs; sd.density = 0.25f; - + lArm->CreateShape(sd); } - + // Link left arm to chest { b3ConeJointDef cd; @@ -139,7 +139,7 @@ public: rArm->CreateShape(sd); } - + // Link right arm to chest { b3ConeJointDef cd; @@ -209,7 +209,7 @@ public: b3ConeJoint* cj = (b3ConeJoint*)m_world.CreateJoint(cd); } } - + void KeyDown(int button) { } @@ -220,4 +220,4 @@ public: } }; -#endif +#endif \ No newline at end of file diff --git a/examples/testbed/tests/ray_cast.h b/examples/testbed/tests/ray_cast.h index 541ccc8..524aa96 100644 --- a/examples/testbed/tests/ray_cast.h +++ b/examples/testbed/tests/ray_cast.h @@ -64,9 +64,17 @@ public: bdef.orientation = b3Quat(b3Vec3(0.0f, 1.0f, 0.0f), 0.25f * B3_PI); b3Body* body = m_world.CreateBody(bdef); + + static b3BoxHull boxHull; + { + b3Transform xf; + xf.position.SetZero(); + xf.rotation = b3Diagonal(2.0f, 4.0f, 0.5f); + boxHull.SetTransform(xf); + } b3HullShape hs; - hs.m_hull = &m_tallHull; + hs.m_hull = &boxHull; b3ShapeDef sdef; sdef.shape = &hs; diff --git a/examples/testbed/tests/sensor_test.h b/examples/testbed/tests/sensor_test.h index e47b383..2647db1 100644 --- a/examples/testbed/tests/sensor_test.h +++ b/examples/testbed/tests/sensor_test.h @@ -42,9 +42,17 @@ public: bd.position.Set(0.0f, 6.0f, 0.0f); b3Body* body = m_world.CreateBody(bd); + + static b3BoxHull boxHull; + { + b3Transform xf; + xf.position.SetZero(); + xf.rotation = b3Diagonal(2.0f, 4.0f, 0.5f); + boxHull.SetTransform(xf); + } b3HullShape hs; - hs.m_hull = &m_tallHull; + hs.m_hull = &boxHull; b3ShapeDef sd; sd.shape = &hs; diff --git a/examples/testbed/tests/sphere_stack.h b/examples/testbed/tests/sphere_stack.h index 3b1c644..1800e56 100644 --- a/examples/testbed/tests/sphere_stack.h +++ b/examples/testbed/tests/sphere_stack.h @@ -33,7 +33,7 @@ public: { { b3BodyDef bd; - bd.type = b3BodyType::e_staticBody; + bd.type = e_staticBody; b3Body* ground = m_world.CreateBody(bd); b3HullShape hs; diff --git a/examples/testbed/tests/spring.h b/examples/testbed/tests/spring.h index bdb0934..4d4f8be 100644 --- a/examples/testbed/tests/spring.h +++ b/examples/testbed/tests/spring.h @@ -64,7 +64,7 @@ public: frame = m_world.CreateBody(bdef); b3ShapeDef sdef; - sdef.density = 1.0f; + sdef.density = 0.1f; sdef.friction = 0.3f; sdef.shape = &box; @@ -82,7 +82,7 @@ public: b3ShapeDef sdef; sdef.shape = &sphere; - sdef.density = 1.0f; + sdef.density = 0.1f; sdef.friction = 1.0f; wheelLF->CreateShape(sdef); @@ -108,7 +108,7 @@ public: wheelRF = m_world.CreateBody(bdef); b3ShapeDef sdef; - sdef.density = 1.0f; + sdef.density = 0.1f; sdef.friction = 1.0f; sdef.shape = &sphere; @@ -136,7 +136,7 @@ public: b3ShapeDef sdef; sdef.shape = &sphere; - sdef.density = 1.0f; + sdef.density = 0.1f; sdef.friction = 1.0f; wheelLB->CreateShape(sdef); @@ -162,7 +162,7 @@ public: wheelRB = m_world.CreateBody(bdef); b3ShapeDef sdef; - sdef.density = 1.0f; + sdef.density = 0.1f; sdef.friction = 1.0f; sdef.shape = &sphere; diff --git a/examples/testbed/tests/test.h b/examples/testbed/tests/test.h index 0b49a67..2ae3ed9 100644 --- a/examples/testbed/tests/test.h +++ b/examples/testbed/tests/test.h @@ -27,6 +27,14 @@ #include #include +inline float32 RandomFloat(float32 a, float32 b) +{ + float32 x = float32(rand()) / float32(RAND_MAX); + float32 diff = b - a; + float32 r = x * diff; + return a + r; +} + struct Settings { Settings() @@ -45,8 +53,9 @@ struct Settings drawContactPoints = true; drawContactNormals = false; drawContactTangents = false; - drawStats = true; - drawProfile = true; + drawContactAreas = false; + drawStats = false; + drawProfile = false; drawGrid = true; pause = false; singleStep = false; @@ -56,6 +65,8 @@ struct Settings int lastTestID; int testID; + bool pause; + bool singleStep; float32 hertz; int velocityIterations; @@ -63,6 +74,7 @@ struct Settings bool sleep; bool warmStart; bool convexCache; + bool drawCenterOfMasses; bool drawBounds; bool drawVerticesEdges; @@ -72,11 +84,10 @@ struct Settings bool drawContactPoints; bool drawContactNormals; bool drawContactTangents; + bool drawContactAreas; bool drawStats; bool drawProfile; bool drawGrid; - bool pause; - bool singleStep; }; class Test; @@ -132,22 +143,15 @@ public: virtual void KeyDown(int button) { } virtual void KeyUp(int button) { } + virtual void Dump() { } + b3World m_world; - - b3RayCastSingleOutput m_rayHit; // local space - b3BoxHull m_groundHull; - b3BoxHull m_boxHull; - b3BoxHull m_tallHull; - b3BoxHull m_doorHull; - b3BoxHull m_rampHull; - b3BoxHull m_plankHull; - b3BoxHull m_thinHull; - - b3Hull* m_qhull; - - b3Mesh m_meshes[e_maxMeshes]; - + b3RayCastSingleOutput m_rayHit; // ray hit local space b3MouseJoint* m_mouseJoint; + + b3BoxHull m_groundHull; + b3BoxHull m_boxHull; + b3Mesh m_meshes[e_maxMeshes]; }; #endif diff --git a/examples/testbed/tests/thin.h b/examples/testbed/tests/thin.h index e6afc42..7073591 100644 --- a/examples/testbed/tests/thin.h +++ b/examples/testbed/tests/thin.h @@ -49,6 +49,16 @@ public: b3Shape* shape = body->CreateShape(sdef); } + + static b3BoxHull thinHull; + + { + b3Transform xf; + xf.position.SetZero(); + xf.rotation = b3Diagonal(4.05f, 2.0f * B3_LINEAR_SLOP, 4.05f); + + thinHull.SetTransform(xf); + } b3Vec3 stackOrigin; stackOrigin.Set(0.0f, 4.05f, 0.0f); @@ -73,7 +83,7 @@ public: b3Body* body = m_world.CreateBody(bdef); b3HullShape hs; - hs.m_hull = &m_thinHull; + hs.m_hull = &thinHull; b3ShapeDef sdef; sdef.shape = &hs; diff --git a/examples/testbed/tests/tumbler.h b/examples/testbed/tests/tumbler.h new file mode 100644 index 0000000..c1705d8 --- /dev/null +++ b/examples/testbed/tests/tumbler.h @@ -0,0 +1,337 @@ +/* +* 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 TUMBLER_TEST_H +#define TUMBLER_TEST_H + +#include + +extern DebugDraw* g_debugDraw; +extern Camera g_camera; +extern Settings g_settings; + +class Tumbler : public Test +{ +public: + enum + { + e_count = 100 + }; + + Tumbler() + { + g_camera.m_center.Set(0.0f, 10.0f, 0.0f); + g_camera.m_q.SetIdentity(); + g_camera.m_zoom = 150.0f; + + { + b3BodyDef bd; + b3Body* ground = m_world.CreateBody(bd); + + bd.type = e_dynamicBody; + b3Body* rotor = m_world.CreateBody(bd); + + { + static b3BoxHull box; + + b3Transform m; + m.position.Set(0.0f, -45.0f, 0.0f); + m.rotation = b3Diagonal(50.0f, 1.0f, 200.0f); + + box.SetTransform(m); + + b3HullShape hs; + hs.m_hull = &box; + + b3ShapeDef sd; + sd.density = 5.0f; + sd.shape = &hs; + + rotor->CreateShape(sd); + } + + { + static b3BoxHull box; + + b3Transform m; + m.position.Set(0.0f, 50.0f, 0.0f); + m.rotation = b3Diagonal(50.0f, 1.0f, 200.0f); + + box.SetTransform(m); + + b3HullShape hs; + hs.m_hull = &box; + + b3ShapeDef sd; + sd.density = 5.0f; + sd.shape = &hs; + + rotor->CreateShape(sd); + } + + { + static b3BoxHull box; + + b3Transform m; + m.position.Set(0.0f, 5.0f, -200.0f); + m.rotation = b3Diagonal(50.0f, 50.0f, 1.0f); + + box.SetTransform(m); + + b3HullShape hs; + hs.m_hull = &box; + + b3ShapeDef sd; + sd.density = 5.0f; + sd.shape = &hs; + + rotor->CreateShape(sd); + } + + { + static b3BoxHull box; + + b3Transform m; + m.position.Set(0.0f, 5.0f, 200.0f); + m.rotation = b3Diagonal(50.0f, 50.0f, 1.0f); + + box.SetTransform(m); + + b3HullShape hs; + hs.m_hull = &box; + + b3ShapeDef sd; + sd.density = 5.0f; + sd.shape = &hs; + + rotor->CreateShape(sd); + } + + { + static b3BoxHull box; + + b3Transform m; + m.position.Set(-50.0f, 5.0f, 0.0f); + m.rotation = b3Diagonal(1.0f, 50.0f, 200.0f); + + box.SetTransform(m); + + b3HullShape hs; + hs.m_hull = &box; + + b3ShapeDef sd; + sd.density = 5.0f; + sd.shape = &hs; + + rotor->CreateShape(sd); + } + + { + static b3BoxHull box; + + b3Transform m; + m.position.Set(50.0f, 5.0f, 0.0f); + m.rotation = b3Diagonal(1.0f, 50.0f, 200.0f); + + box.SetTransform(m); + + b3HullShape hs; + hs.m_hull = &box; + + b3ShapeDef sd; + sd.density = 5.0f; + sd.shape = &hs; + + rotor->CreateShape(sd); + } + + { + b3RevoluteJointDef jd; + jd.Initialize(ground, rotor, b3Vec3(0.0f, 0.0f, -1.0f), ground->GetPosition(), -B3_PI, B3_PI); + jd.motorSpeed = 0.05f * B3_PI; + jd.maxMotorTorque = 1000.0f * rotor->GetMass(); + jd.enableMotor = true; + + b3Joint* joint = (b3RevoluteJoint*)m_world.CreateJoint(jd); + } + } + + { + b3StackArray points; + ConstructCone(points); + + u32 size = qhGetMemorySize(points.Count()); + void* p = b3Alloc(size); + + qhHull hull; + hull.Construct(p, points); + m_coneHull = ConvertHull(hull); + + b3Free(p); + } + + { + b3StackArray points; + ConstructCylinder(points); + + const u32 size = qhGetMemorySize(points.Count()); + void* p = b3Alloc(size); + + qhHull hull; + hull.Construct(p, points); + m_cylinderHull = ConvertHull(hull); + + b3Free(p); + } + + m_count = 0; + } + + ~Tumbler() + { + { + b3Free(m_coneHull.vertices); + b3Free(m_coneHull.edges); + b3Free(m_coneHull.faces); + b3Free(m_coneHull.planes); + } + + { + b3Free(m_cylinderHull.vertices); + b3Free(m_cylinderHull.edges); + b3Free(m_cylinderHull.faces); + b3Free(m_cylinderHull.planes); + } + } + + void Step() + { + if(m_count < e_count) + { + ++m_count; + + { + b3BodyDef bdef; + bdef.type = e_dynamicBody; + bdef.position.Set(-10.0f, 5.0f, 0.0f); + + b3Body* body = m_world.CreateBody(bdef); + + b3SphereShape sphere; + sphere.m_center.SetZero(); + sphere.m_radius = 1.0f; + + b3ShapeDef sdef; + sdef.density = 1.0f; + sdef.friction = 0.3f; + sdef.shape = &sphere; + + body->CreateShape(sdef); + } + + { + b3BodyDef bdef; + bdef.type = e_dynamicBody; + bdef.position.Set(-5.0f, 5.0f, 0.0f); + + b3Body* body = m_world.CreateBody(bdef); + + b3CapsuleShape capsule; + capsule.m_centers[0].Set(0.0f, 0.0f, -1.0f); + capsule.m_centers[1].Set(0.0f, 0.0f, 1.0f); + capsule.m_radius = 1.0f; + + b3ShapeDef sdef; + sdef.density = 0.1f; + sdef.friction = 0.2f; + sdef.shape = &capsule; + + body->CreateShape(sdef); + } + + { + b3BodyDef bdef; + bdef.type = e_dynamicBody; + bdef.position.Set(0.0f, 0.0f, 0.0f); + bdef.angularVelocity.Set(0.0f, 0.05f * B3_PI, 0.0f); + b3Body* body = m_world.CreateBody(bdef); + + static b3BoxHull box; + box.SetIdentity(); + + b3HullShape hs; + hs.m_hull = &box; + + b3ShapeDef sd; + sd.density = 0.05f; + sd.shape = &hs; + + body->CreateShape(sd); + } + + { + b3BodyDef bdef; + bdef.type = e_dynamicBody; + bdef.position.Set(0.0f, 5.0f, 0.0f); + + b3Body* body = m_world.CreateBody(bdef); + + b3HullShape hull; + hull.m_hull = &m_coneHull; + + b3ShapeDef sdef; + sdef.density = 1.0f; + sdef.friction = 0.3f; + sdef.shape = &hull; + + body->CreateShape(sdef); + } + + { + b3BodyDef bdef; + bdef.type = e_dynamicBody; + bdef.position.Set(4.0f, 5.0f, 0.0f); + + b3Body* body = m_world.CreateBody(bdef); + + b3HullShape hull; + hull.m_hull = &m_cylinderHull; + + b3ShapeDef sdef; + sdef.density = 1.0f; + sdef.friction = 0.2f; + sdef.shape = &hull; + + body->CreateShape(sdef); + } + } + + Test::Step(); + } + + static Test* Create() + { + return new Tumbler(); + } + + u32 m_count; + b3Hull m_coneHull; + b3Hull m_cylinderHull; +}; + +#endif \ No newline at end of file diff --git a/examples/testbed/tests/varying_friction.h b/examples/testbed/tests/varying_friction.h index 3e8eb07..d9951c0 100644 --- a/examples/testbed/tests/varying_friction.h +++ b/examples/testbed/tests/varying_friction.h @@ -40,15 +40,24 @@ public: ground->CreateShape(sdef); } + static b3BoxHull rampHull; + + { + b3Transform xf; + xf.position.SetZero(); + xf.rotation = b3Diagonal(25.0f, 0.5f, 25.0f); + rampHull.SetTransform(xf); + } + { b3BodyDef bdef; bdef.position.Set(-20.0f, 20.0f, 0.0f); bdef.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), -0.1f * B3_PI); b3Body* ramp = m_world.CreateBody(bdef); - + b3HullShape hs; - hs.m_hull = &m_rampHull; + hs.m_hull = &rampHull; b3ShapeDef sdef; sdef.shape = &hs; @@ -64,7 +73,7 @@ public: b3Body* ramp = m_world.CreateBody(bdef); b3HullShape hs; - hs.m_hull = &m_rampHull; + hs.m_hull = &rampHull; b3ShapeDef sdef; sdef.shape = &hs; @@ -80,7 +89,7 @@ public: b3Body* ramp = m_world.CreateBody(bdef); b3HullShape hs; - hs.m_hull = &m_rampHull; + hs.m_hull = &rampHull; b3ShapeDef sdef; sdef.shape = &hs; @@ -96,7 +105,7 @@ public: b3Body* ramp = m_world.CreateBody(bdef); b3HullShape hs; - hs.m_hull = &m_rampHull; + hs.m_hull = &rampHull; b3ShapeDef sdef; sdef.shape = &hs; diff --git a/examples/testbed/tests/weld_test.h b/examples/testbed/tests/weld_test.h index 8f2498c..b35c825 100644 --- a/examples/testbed/tests/weld_test.h +++ b/examples/testbed/tests/weld_test.h @@ -33,17 +33,17 @@ public: b3ShapeDef sd; sd.shape = &shape; - + ground->CreateShape(sd); } - b3Body* hinge, *door; + b3Body* bA, *bB; { b3BodyDef bd; bd.type = b3BodyType::e_dynamicBody; bd.position.Set(-2.0f, 5.05f, 0.0f); - hinge = m_world.CreateBody(bd); + bA = m_world.CreateBody(bd); b3CapsuleShape shape; shape.m_centers[0].Set(0.0f, -3.5f, 0.0f); @@ -54,34 +54,46 @@ public: sd.shape = &shape; sd.density = 1.0f; - hinge->CreateShape(sd); + bA->CreateShape(sd); } { b3BodyDef bd; bd.type = b3BodyType::e_dynamicBody; - bd.orientation.Set(b3Vec3(1.0f, 0.0f, 0.0f), 0.25f * B3_PI); bd.position.Set(1.0f, 5.05f, 0.0f); - door = m_world.CreateBody(bd); + bB = m_world.CreateBody(bd); + + static b3BoxHull doorHull; + { + b3Transform xf; + xf.position.SetZero(); + xf.rotation = b3Diagonal(2.0f, 4.0f, 0.5f); + doorHull.SetTransform(xf); + } b3HullShape hull; - hull.m_hull = &m_doorHull; + hull.m_hull = &doorHull; b3ShapeDef sdef; sdef.shape = &hull; - sdef.density = 2.0f; + sdef.density = 1.0f; - door->CreateShape(sdef); - } - - { - b3Vec3 hingeAnchor(-2.0f, 5.0f, 0.0f); - - b3WeldJointDef jd; - jd.Initialize(hinge, door, hingeAnchor); + bB->CreateShape(sdef); - b3WeldJoint* wj = (b3WeldJoint*)m_world.CreateJoint(jd); + { + b3Vec3 anchor(-2.0f, 5.0f, 0.0f); + + b3WeldJointDef jd; + jd.Initialize(bA, bB, anchor); + + b3WeldJoint* wj = (b3WeldJoint*)m_world.CreateJoint(jd); + } + + // Invalidate the orientation + b3Vec3 axis(1.0f, 0.0f, 0.0f); + float32 angle = B3_PI; + bB->SetTransform(bB->GetPosition(), axis, angle); } } diff --git a/external/imgui/imgui_impl_glfw_gl3.cpp b/external/imgui/imgui_impl_glfw_gl3.cpp index 2a5cb76..3a8da6a 100644 --- a/external/imgui/imgui_impl_glfw_gl3.cpp +++ b/external/imgui/imgui_impl_glfw_gl3.cpp @@ -6,16 +6,15 @@ // If you are new to ImGui, see examples/README.txt and documentation at the top of imgui.cpp. // https://github.com/ocornut/imgui -#include -#include "imgui_impl_glfw_gl3.h" - #if defined(__APPLE_CC__) #include #else - // GLEW/GL3W/GLAD/... #include #endif +#include +#include "imgui_impl_glfw_gl3.h" + #include #if defined ( _WIN32 ) diff --git a/include/bounce/bounce.h b/include/bounce/bounce.h index 8c7ec24..150dca9 100644 --- a/include/bounce/bounce.h +++ b/include/bounce/bounce.h @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include diff --git a/include/bounce/cloth/cloth.h b/include/bounce/cloth/cloth.h index e3ea09e..9a27afa 100644 --- a/include/bounce/cloth/cloth.h +++ b/include/bounce/cloth/cloth.h @@ -20,7 +20,7 @@ #define B3_CLOTH_H #include -#include +#include struct b3Mesh; class b3Draw; diff --git a/include/bounce/collision/broad_phase.h b/include/bounce/collision/broad_phase.h index 01bbeed..8ac1efe 100644 --- a/include/bounce/collision/broad_phase.h +++ b/include/bounce/collision/broad_phase.h @@ -98,9 +98,9 @@ private : u32 m_moveBufferCapacity; // The buffer holding the unique overlapping AABB pairs. - b3Pair* m_pairBuffer; - u32 m_pairBufferCapacity; - u32 m_pairBufferCount; + b3Pair* m_pairs; + u32 m_pairCapacity; + u32 m_pairCount; }; inline const b3AABB3& b3BroadPhase::GetAABB(i32 proxyId) const @@ -144,7 +144,7 @@ template inline void b3BroadPhase::FindNewPairs(T* callback) { // Reset the overlapping pairs buffer count for the current step. - m_pairBufferCount = 0; + m_pairCount = 0; // Notifying this class with QueryCallback(), gets the (duplicated) overlapping pair buffer. for (u32 i = 0; i < m_moveBufferCount; ++i) @@ -164,22 +164,22 @@ inline void b3BroadPhase::FindNewPairs(T* callback) m_moveBufferCount = 0; // Sort the (duplicated) overlapping pair buffer to prune duplicated pairs. - std::sort(m_pairBuffer, m_pairBuffer + m_pairBufferCount); + std::sort(m_pairs, m_pairs + m_pairCount); // Skip duplicated overlapping pairs. u32 index = 0; - while (index < m_pairBufferCount) + while (index < m_pairCount) { - const b3Pair* primaryPair = m_pairBuffer + index; + const b3Pair* primaryPair = m_pairs + index; // Report an unique overlapping pair to the client. callback->AddPair(m_tree.GetUserData(primaryPair->proxy1), m_tree.GetUserData(primaryPair->proxy2)); // Skip all duplicated pairs until an unique pair is found. ++index; - while (index < m_pairBufferCount) + while (index < m_pairCount) { - const b3Pair* secondaryPair = m_pairBuffer + index; + const b3Pair* secondaryPair = m_pairs + index; if (secondaryPair->proxy1 != primaryPair->proxy1 || secondaryPair->proxy2 != primaryPair->proxy2) { break; diff --git a/include/bounce/collision/distance.h b/include/bounce/collision/collision.h similarity index 51% rename from include/bounce/collision/distance.h rename to include/bounce/collision/collision.h index 49f15f3..1fde28f 100644 --- a/include/bounce/collision/distance.h +++ b/include/bounce/collision/collision.h @@ -16,42 +16,12 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#ifndef B3_DISTANCE_H -#define B3_DISTANCE_H +#ifndef B3_COLLISION_H +#define B3_COLLISION_H #include #include -/////////////////////////////////////////////////////////////////////////////////////////////////// - -// Find the closest point for a point P to a normalized plane. -b3Vec3 b3ClosestPointOnPlane(const b3Vec3& P, const b3Plane& plane); - -// Find the closest point for a point P to a segment AB. -b3Vec3 b3ClosestPointOnSegment(const b3Vec3& P, - const b3Vec3& A, const b3Vec3& B); - -// Find the closest point for a point P to a triangle ABC. -b3Vec3 b3ClosestPointOnTriangle(const b3Vec3& P, - const b3Vec3& A, const b3Vec3& B, const b3Vec3& C); - -// Find the closest points of two lines. -void b3ClosestPointsOnLines(b3Vec3* C1, b3Vec3* C2, - const b3Vec3& P1, const b3Vec3& E1, - const b3Vec3& P2, const b3Vec3& E2); - -// Find the closest points of two normalized lines. -void b3ClosestPointsOnNormalizedLines(b3Vec3* C1, b3Vec3* C2, - const b3Vec3& P1, const b3Vec3& N1, - const b3Vec3& P2, const b3Vec3& N2); - -// Find the closest points of two segments. -void b3ClosestPointsOnSegments(b3Vec3* C1, b3Vec3* C2, - const b3Vec3& P1, const b3Vec3& Q1, - const b3Vec3& P2, const b3Vec3& Q2); - -/////////////////////////////////////////////////////////////////////////////////////////////////// - // Input for a ray cast. struct b3RayCastInput { diff --git a/include/bounce/collision/gjk/gjk.h b/include/bounce/collision/gjk/gjk.h index 652d331..ba4c41c 100644 --- a/include/bounce/collision/gjk/gjk.h +++ b/include/bounce/collision/gjk/gjk.h @@ -26,12 +26,12 @@ struct b3SimplexCache; struct b3SimplexVertex { - b3Vec3 pointA; // support vertex on proxy A - b3Vec3 pointB; // support vertex on proxy B + b3Vec3 point1; // support vertex on proxy 1 + b3Vec3 point2; // support vertex on proxy 2 b3Vec3 point; // minkowski vertex float32 weight; // barycentric coordinate for point - u32 indexA; // support A vertex index - u32 indexB; // support B vertex index + u32 index1; // support 1 vertex index + u32 index2; // support 2 vertex index }; struct b3Simplex @@ -41,7 +41,7 @@ struct b3Simplex b3Vec3 GetSearchDirection(const b3Vec3& Q) const; b3Vec3 GetClosestPoint() const; - void GetClosestPoints(b3Vec3* pA, b3Vec3* pB) const; + void GetClosestPoints(b3Vec3* p1, b3Vec3* p2) const; void Solve2(const b3Vec3& Q); void Solve3(const b3Vec3& Q); @@ -49,9 +49,11 @@ struct b3Simplex // Cache void 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); + void WriteCache(b3SimplexCache* cache) const; + float32 GetMetric() const; }; @@ -61,14 +63,14 @@ struct b3Simplex // If the distance is zero then the proxies are overlapping. struct b3GJKOutput { - b3Vec3 pointA; // closest point on proxy A - b3Vec3 pointB; // closest point on proxy B + b3Vec3 point1; // closest point on proxy 1 + b3Vec3 point2; // closest point on proxy 2 float32 distance; // euclidean distance between the closest points u32 iterations; // number of GJK iterations }; // Find the closest points and distance between two proxies. -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); #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 index a909de2..2b0e8de 100644 --- a/include/bounce/collision/gjk/gjk_cache.h +++ b/include/bounce/collision/gjk/gjk_cache.h @@ -31,25 +31,25 @@ struct b3SimplexCache float32 metric; // distance or area or volume u32 iterations; // number of GJK iterations u16 count; // number of support vertices - u8 indexA[4]; // support vertices on proxy A - u8 indexB[4]; // support vertices on proxy B + 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& 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); // A feature pair contains the vertices of the features associated // with the closest points. struct b3GJKFeaturePair { - u32 indexA[3]; // vertices on proxy A - u32 countA; // number of vertices on proxy A - u32 indexB[3]; // vertices on proxy B - u32 countB; // number of vertices on proxy B + 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 diff --git a/include/bounce/collision/sat/sat.h b/include/bounce/collision/sat/sat.h index afb2431..5066ba7 100644 --- a/include/bounce/collision/sat/sat.h +++ b/include/bounce/collision/sat/sat.h @@ -33,15 +33,15 @@ struct b3FaceQuery float32 b3Project(const b3Hull* hull, const b3Plane& plane); -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); /////////////////////////////////////////////////////////////////////////////////////////////////// struct b3EdgeQuery { - u32 indexA; - u32 indexB; + u32 index1; + u32 index2; float32 separation; }; @@ -49,7 +49,7 @@ 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); -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); #endif diff --git a/include/bounce/collision/sat/sat_edge_and_hull.h b/include/bounce/collision/sat/sat_edge_and_hull.h index 1b796c5..2eabc74 100644 --- a/include/bounce/collision/sat/sat_edge_and_hull.h +++ b/include/bounce/collision/sat/sat_edge_and_hull.h @@ -21,20 +21,20 @@ #include -struct b3Capsule; +struct b3Segment; /////////////////////////////////////////////////////////////////////////////////////////////////// -float32 b3ProjectEdge(const b3Capsule* hull, const b3Plane& plane); +float32 b3ProjectEdge(const b3Segment* hull, const b3Plane& 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); /////////////////////////////////////////////////////////////////////////////////////////////////// float32 b3ProjectEdge(const b3Vec3& P1, const b3Vec3& E1, const b3Vec3& P2, const b3Vec3& E2, const b3Vec3& C2); -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); #endif \ No newline at end of file diff --git a/include/bounce/collision/sat/sat_vertex_and_hull.h b/include/bounce/collision/sat/sat_vertex_and_hull.h index 24c8de6..86ad9d2 100644 --- a/include/bounce/collision/sat/sat_vertex_and_hull.h +++ b/include/bounce/collision/sat/sat_vertex_and_hull.h @@ -21,13 +21,11 @@ #include -struct b3Sphere; - /////////////////////////////////////////////////////////////////////////////////////////////////// -float32 b3ProjectVertex(const b3Sphere* hull, const b3Plane& plane); +float32 b3ProjectVertex(const b3Vec3& hull, const b3Plane& plane); -b3FaceQuery b3QueryFaceSeparation(const b3Transform& xfA, const b3Sphere* hullA, - const b3Transform& xfB, const b3Hull* hullB); +b3FaceQuery b3QueryFaceSeparation(const b3Transform& xf1, const b3Vec3& hull1, + const b3Transform& xf2, const b3Hull* hull2); #endif diff --git a/include/bounce/collision/shapes/box_hull.h b/include/bounce/collision/shapes/box_hull.h index 690c10c..ef20564 100644 --- a/include/bounce/collision/shapes/box_hull.h +++ b/include/bounce/collision/shapes/box_hull.h @@ -90,6 +90,8 @@ struct b3BoxHull : public b3Hull faces = boxFaces; planes = boxPlanes; faceCount = 6; + + Validate(); } // Set this box from three extents and centered at the origin. @@ -102,8 +104,6 @@ struct b3BoxHull : public b3Hull } // Set this box to the unit box and transform it. - // Warning: The transform must not contain non-uniform - // scaling! void SetTransform(const b3Transform& T) { boxVertices[0] = b3Vec3(1.0f, 1.0f, -1.0f); @@ -169,6 +169,8 @@ struct b3BoxHull : public b3Hull faceCount = 6; centroid = T * centroid; + + Validate(); } }; diff --git a/include/bounce/collision/shapes/capsule.h b/include/bounce/collision/shapes/capsule.h index 7ad03b1..012fe66 100644 --- a/include/bounce/collision/shapes/capsule.h +++ b/include/bounce/collision/shapes/capsule.h @@ -21,21 +21,20 @@ #include -struct b3Capsule +struct b3Segment { b3Vec3 vertices[2]; - float32 radius; - + const b3Vec3& GetVertex(u32 index) const; u32 GetSupportVertex(const b3Vec3& direction) const; }; -inline const b3Vec3& b3Capsule::GetVertex(u32 index) const +inline const b3Vec3& b3Segment::GetVertex(u32 index) const { return vertices[index]; } -inline u32 b3Capsule::GetSupportVertex(const b3Vec3& d) const +inline u32 b3Segment::GetSupportVertex(const b3Vec3& d) const { if (b3Dot(d, vertices[0]) > b3Dot(d, vertices[1])) { diff --git a/include/bounce/collision/shapes/hull.h b/include/bounce/collision/shapes/hull.h index c3a43e9..cc27c47 100644 --- a/include/bounce/collision/shapes/hull.h +++ b/include/bounce/collision/shapes/hull.h @@ -51,7 +51,9 @@ struct b3Hull const b3Plane& GetPlane(u32 index) const; u32 GetSupportVertex(const b3Vec3& direction) const; + //u32 GetSupportEdge(const b3Vec3& direction) const; u32 GetSupportFace(const b3Vec3& direction) const; + b3Plane GetEdgeSidePlane(u32 index) const; u32 GetSize() const; diff --git a/include/bounce/collision/trees/dynamic_tree.h b/include/bounce/collision/trees/dynamic_tree.h index ad3b01f..c7496d9 100644 --- a/include/bounce/collision/trees/dynamic_tree.h +++ b/include/bounce/collision/trees/dynamic_tree.h @@ -22,7 +22,7 @@ #include #include #include -#include +#include #define NULL_NODE (-1) diff --git a/include/bounce/collision/trees/static_tree.h b/include/bounce/collision/trees/static_tree.h index 31e1758..70fde14 100644 --- a/include/bounce/collision/trees/static_tree.h +++ b/include/bounce/collision/trees/static_tree.h @@ -22,7 +22,7 @@ #include #include #include -#include +#include #define NULL_NODE_S (0xFFFFFFFF) diff --git a/include/bounce/common/draw.h b/include/bounce/common/draw.h index 5ab1500..80f5750 100644 --- a/include/bounce/common/draw.h +++ b/include/bounce/common/draw.h @@ -54,7 +54,8 @@ public : e_contactPointsFlag = 0x0008, e_contactNormalsFlag = 0x0010, e_contactTangentsFlag = 0x0020, - e_aabbsFlag = 0x0040, + e_contactAreasFlag = 0x0040, + e_aabbsFlag = 0x0080, }; b3Draw() diff --git a/include/bounce/common/geometry.h b/include/bounce/common/geometry.h index c6e18e6..c0baf98 100644 --- a/include/bounce/common/geometry.h +++ b/include/bounce/common/geometry.h @@ -75,13 +75,6 @@ inline float32 b3Distance(const b3Vec3& P, const b3Plane& plane) return b3Dot(plane.normal, P) - plane.offset; } -// Project a point onto a normal plane. -inline b3Vec3 b3Project(const b3Vec3& P, const b3Plane& plane) -{ - float32 fraction = b3Distance(P, plane); - return P - fraction * plane.normal; -} - // Compute barycentric coordinates (u, v) for point Q to segment AB. // The last output value is the divisor. inline void b3Barycentric(float32 out[3], @@ -150,4 +143,33 @@ inline void b3Barycentric(float32 out[5], out[4] = sign * divisor; } +// Project a point onto a normal plane. +inline b3Vec3 b3ClosestPointOnPlane(const b3Vec3& P, const b3Plane& plane) +{ + float32 fraction = b3Distance(P, plane); + return P - fraction * plane.normal; +} + +// 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); + + 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; +} + #endif diff --git a/include/bounce/common/math/mat.h b/include/bounce/common/math/mat.h index e30a0c9..163c573 100644 --- a/include/bounce/common/math/mat.h +++ b/include/bounce/common/math/mat.h @@ -20,69 +20,350 @@ #define B3_MAT_H #include +#include +#include -// A vector stored in column-major order. -template -struct b3Vec +struct b3Mat23 { - b3Vec() { } + b3Mat23() { } - const float32& operator[](u32 i) const + b3Mat23(const b3Vec2& _x, const b3Vec2& _y, const b3Vec2& _z) { - return e[i]; - } - - float32& operator[](u32 i) - { - return e[i]; - } - - void operator+=(const b3Vec& v) - { - for (u32 i = 0; i < n; ++i) - { - e[i] += v[i]; - } + x = _x; + y = _y; + z = _z; } - float32 e[n]; + void SetZero() + { + x.SetZero(); + y.SetZero(); + z.SetZero(); + } + + b3Vec2 x, y, z; }; -template -inline b3Vec operator-(const b3Vec& v) +struct b3Mat24 { - b3Vec result; - for (u32 i = 0; i < n; ++i) + b3Mat24() { } + + b3Mat24(const b3Vec2& _x, const b3Vec2& _y, const b3Vec2& _z, const b3Vec2& _w) { - result[i] = -v[i]; + x = _x; + y = _y; + z = _z; + w = _w; } + + void SetZero() + { + x.SetZero(); + y.SetZero(); + z.SetZero(); + w.SetZero(); + } + + b3Vec2 x, y, z, w; +}; + +struct b3Mat32 +{ + b3Mat32() { } + + b3Mat32(const b3Vec3& _x, const b3Vec3& _y) + { + x = _x; + y = _y; + } + + void SetZero() + { + x.SetZero(); + y.SetZero(); + } + + b3Vec3 x, y; +}; + +struct b3Mat34 +{ + b3Mat34() { } + + b3Mat34(const b3Vec3& _x, const b3Vec3& _y, const b3Vec3& _z, const b3Vec3& _w) + { + x = _x; + y = _y; + z = _z; + w = _w; + } + + void SetZero() + { + x.SetZero(); + y.SetZero(); + z.SetZero(); + w.SetZero(); + } + + b3Vec3 x, y, z, w; +}; + +struct b3Vec4 +{ + b3Vec4() { } + + b3Vec4(float32 _x, float32 _y, float32 _z, float32 _w) + { + x = _x; + y = _y; + z = _z; + w = _w; + } + + void SetZero() + { + x = 0.0f; + y = 0.0f; + z = 0.0f; + w = 0.0f; + } + + float32 x, y, z, w; +}; + +struct b3Mat43 +{ + b3Mat43() { } + + b3Mat43(const b3Vec4& _x, const b3Vec4& _y, const b3Vec4& _z) + { + x = _x; + y = _y; + z = _z; + } + + void SetZero() + { + x.SetZero(); + y.SetZero(); + z.SetZero(); + } + + b3Vec4 x, y, z; +}; + +struct b3Mat44 +{ + b3Mat44() { } + + b3Mat44(const b3Vec4& _x, const b3Vec4& _y, const b3Vec4& _z, const b3Vec4& _w) + { + x = _x; + y = _y; + z = _z; + w = _w; + } + + void SetZero() + { + x.SetZero(); + y.SetZero(); + z.SetZero(); + w.SetZero(); + } + + b3Vec4 x, y, z, w; +}; + +inline b3Vec4 operator+(const b3Vec4& a, const b3Vec4& b) +{ + return b3Vec4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w); +} + +inline b3Vec4 operator-(const b3Vec4& a, const b3Vec4& b) +{ + return b3Vec4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w); +} + +// 1x1 * 1x4 = 1x1 +inline b3Vec4 operator*(float32 s, const b3Vec4& v) +{ + return b3Vec4(s * v.x, s * v.y, s * v.z, s * v.w); +} + +// a * 4x4 = 4x4 +inline b3Mat44 operator*(float32 s, const b3Mat44& A) +{ + return b3Mat44(s * A.x, s * A.y, s * A.z, s * A.w); +} + +// 4x4 * 4x1 = 4x1 +inline b3Vec4 operator*(const b3Mat44& A, const b3Vec4& v) +{ + return v.x * A.x + v.y * A.y + v.z * A.z + v.w * A.w; +} + +// 4x4 * 4x4 = 4x4 +inline b3Mat44 operator*(const b3Mat44& A, const b3Mat44& B) +{ + return b3Mat44(A * B.x, A * B.y, A * B.z, A * B.w); +} + +// a * 3x4 = 3x4 +inline b3Mat34 operator*(float32 s, const b3Mat34& A) +{ + return b3Mat34(s * A.x, s * A.y, s * A.z, s * A.w); +} + +// 4x3 * 3x1 = 4x1 +inline b3Vec4 operator*(const b3Mat43& A, const b3Vec3& v) +{ + return v.x * A.x + v.y * A.y + v.z * A.z; +} + +// 3x4 * 4x1 = 3x1 +inline b3Vec3 operator*(const b3Mat34& A, const b3Vec4& v) +{ + return v.x * A.x + v.y * A.y + v.z * A.z + v.w * A.w; +} + +// 1x4 * 4x1 = 1x1 +inline float32 operator*(const b3Vec4& A, const b3Vec4& B) +{ + return A.x * B.x + A.y * B.y + A.z * B.z + A.w * B.w; +} + +// 3x1 * 1x1 = 3x1 +inline b3Vec3 operator*(const b3Vec3& v, float32 s) +{ + return s * v; +} + +// 2x1 * 1x1 = 2x1 +inline b3Vec2 operator*(const b3Vec2& v, float32 s) +{ + return s * v; +} + +// 1x3 * 3x1 = 1x1 +inline float32 operator*(const b3Vec3& A, const b3Vec3& B) +{ + return A.x * B.x + A.y * B.y + A.z * B.z; +} + +// 1x3 * 3x3 = 1x3 +inline b3Vec3 operator*(const b3Vec3& A, const b3Mat33& B) +{ + return b3Vec3(A * B.x, A * B.y, A * B.z); +} + +// 1x4 * 4x4 = 1x4 +inline b3Vec4 operator*(const b3Vec4& A, const b3Mat44& B) +{ + return b3Vec4(A * B.x, A * B.y, A * B.z, A * B.w); +} + +// 1x4 * 4x3 = 1x3 +inline b3Vec3 operator*(const b3Vec4& A, const b3Mat43& B) +{ + return b3Vec3(A * B.x, A * B.y, A * B.z); +} + +// 3x2 * 2x1 = 3x1 +inline b3Vec3 operator*(const b3Mat32& A, const b3Vec2& B) +{ + return B.x * A.x + B.y * A.y; +} + +// 2x3 * 2x1 = 2x1 +inline b3Vec2 operator*(const b3Mat23& A, const b3Vec3& B) +{ + return B.x * A.x + B.y * A.y + B.z * A.z; +} + +// 2x3 * 2x2 = 2x2 +inline b3Mat22 operator*(const b3Mat23& A, const b3Mat32& B) +{ + return b3Mat22(A * B.x, A * B.y); +} + +// 2x3 * 3x3 = 2x3 +inline b3Mat23 operator*(const b3Mat23& A, const b3Mat33& B) +{ + return b3Mat23(A * B.x, A * B.y, A * B.z); +} + +// 3x4 * 4x3 = 3x3 +inline b3Mat33 operator*(const b3Mat34& A, const b3Mat43& B) +{ + return b3Mat33(A * B.x, A * B.y, A * B.z); +} + +// 3x4 * 4x4 = 3x3 +inline b3Mat34 operator*(const b3Mat34& A, const b3Mat44& B) +{ + return b3Mat34(A * B.x, A * B.y, A * B.z, A * B.w); +} + +// 2x4 * 4x1 = 2x1 +inline b3Vec2 operator*(const b3Mat24& A, const b3Vec4& B) +{ + return B.x * A.x + B.y * A.y + B.z * A.z + B.w * A.w; +} + +// 2x4 * 4x3 = 4x3 +inline b3Mat23 operator*(const b3Mat24& A, const b3Mat43& B) +{ + return b3Mat23(A * B.x, A * B.y, A * B.z); +} + +// 2x4 * 2x4 = 2x4 +inline b3Mat24 operator*(const b3Mat24& A, const b3Mat44& B) +{ + return b3Mat24(A * B.x, A * B.y, A * B.z, A * B.w); +} + +// 4x4 * 4x3 = 4x3 +inline b3Mat43 operator*(const b3Mat44& A, const b3Mat43& B) +{ + return b3Mat43(A * B.x, A * B.y, A * B.z); +} + +inline b3Mat23 b3Transpose(const b3Mat32& A) +{ + b3Mat23 result; + result.x = b3Vec2(A.x.x, A.y.x); + result.y = b3Vec2(A.x.y, A.y.y); + result.z = b3Vec2(A.x.z, A.y.z); return result; } -// A matrix stored in column-major order. -template -struct b3Mat +inline b3Mat32 b3Transpose(const b3Mat23& A) { - b3Mat() { } + b3Mat32 result; + result.x = b3Vec3(A.x.x, A.y.x, A.z.x); + result.y = b3Vec3(A.x.y, A.y.y, A.z.y); + return result; +} - const float32& operator()(u32 i, u32 j) const - { - return e[i + n * j]; - } +inline b3Mat34 b3Transpose(const b3Mat43& A) +{ + b3Mat34 result; + result.x = b3Vec3(A.x.x, A.y.x, A.z.x); + result.y = b3Vec3(A.x.y, A.y.y, A.z.y); + result.z = b3Vec3(A.x.z, A.y.z, A.z.z); + result.w = b3Vec3(A.x.w, A.y.w, A.z.w); + return result; +} - float32& operator()(u32 i, u32 j) - { - return e[i + n * j]; - } - - float32 e[n * m]; -}; - -// Solve Ax = b. -// It doesn't compute the inverse. -// Therefore, is more efficient. -// Returns false if the matrix is singular. -// Warning: Make sure to pass a copy of the original matrix to the function. A will be invalidated. -bool b3Solve(float32* b, float32* A, u32 n); +inline b3Mat43 b3Transpose(const b3Mat34& A) +{ + b3Mat43 result; + result.x = b3Vec4(A.x.x, A.y.x, A.z.x, A.w.x); + result.y = b3Vec4(A.x.y, A.y.y, A.z.y, A.w.y); + result.z = b3Vec4(A.x.z, A.y.z, A.z.z, A.w.z); + return result; +} #endif \ No newline at end of file diff --git a/include/bounce/common/math/mat22.h b/include/bounce/common/math/mat22.h index d4ca2f2..cfdda1c 100644 --- a/include/bounce/common/math/mat22.h +++ b/include/bounce/common/math/mat22.h @@ -30,6 +30,13 @@ struct b3Mat22 // Set this matrix from two vectors. b3Mat22(const b3Vec2& _x, const b3Vec2& _y) : x(_x), y(_y) { } + // Set this matrix to the zero matrix. + void SetZero() + { + x.SetZero(); + y.SetZero(); + } + // Solve Ax = b. // It doesn't compute the inverse. // Therefore, is more efficient. @@ -45,6 +52,12 @@ inline b3Vec2 operator*(const b3Mat22& A, const b3Vec2& v) return v.x * A.x + v.y * A.y; } +// Add two matrices. +inline b3Mat22 operator+(const b3Mat22& A, const b3Mat22& B) +{ + return b3Mat22(A.x + B.x, A.y + B.y); +} + // Multiply a matrix times a vector. inline b3Vec2 b3Mul(const b3Mat22& A, const b3Vec2& v) { @@ -54,6 +67,6 @@ inline b3Vec2 b3Mul(const b3Mat22& A, const b3Vec2& v) // Invert a matrix. // If the matrix determinant is zero this returns // the zero matrix. -inline b3Mat22 b3Inverse(const b3Mat22& A); +b3Mat22 b3Inverse(const b3Mat22& A); #endif diff --git a/include/bounce/common/math/quat.h b/include/bounce/common/math/quat.h index aa012f4..2a3493d 100644 --- a/include/bounce/common/math/quat.h +++ b/include/bounce/common/math/quat.h @@ -37,7 +37,19 @@ struct b3Quat { Set(axis, angle); } + + // Write an indexed value to this quaternion. + float32& operator[](u32 i) + { + return (&x)[i]; + } + // Read an indexed value from this quaternion. + float32 operator[](u32 i) const + { + return (&x)[i]; + } + // Add a quaternion to this quaternion. void operator+=(const b3Quat& q) { @@ -203,6 +215,77 @@ inline b3Vec3 b3Mul(const b3Quat& q, const b3Vec3& v) return v + qs * t + b3Cross(qv, t); } +// Inverse rotate a vector by an orientation quaternion. +inline b3Vec3 b3MulT(const b3Quat& q, const b3Vec3& v) +{ + return b3Mul(b3Conjugate(q), v); +} + +// Convert a 3-by-3 rotation matrix to an orientation quaternion. +inline b3Quat b3ConvertRotToQuat(const b3Mat33& m) +{ + // Check the diagonal. + float32 trace = m[0][0] + m[1][1] + m[2][2]; + + if (trace > 0.0f) + { + b3Quat result; + + float32 s = b3Sqrt(trace + 1.0f); + result.w = 0.5f * s; + + float32 t = 0.5f / s; + result.x = t * (m[1][2] - m[2][1]); + result.y = t * (m[2][0] - m[0][2]); + result.z = t * (m[0][1] - m[1][0]); + return result; + } + + // Diagonal is negative. + const i32 next[3] = { 1, 2, 0 }; + + i32 i = 0; + + if (m[1][1] > m[0][0]) + { + i = 1; + } + + if (m[2][2] > m[i][i]) + { + i = 2; + } + + i32 j = next[i]; + i32 k = next[j]; + + float32 s = sqrt((m[i][i] - (m[j][j] + m[k][k])) + 1.0f); + + float32 q[4]; + q[i] = s * 0.5f; + + float32 t; + if (s != 0.0f) + { + t = 0.5f / s; + } + else + { + t = s; + } + + q[3] = t * (m[j][k] - m[k][j]); + q[j] = t * (m[i][j] + m[j][i]); + q[k] = t * (m[i][k] + m[k][i]); + + b3Quat result; + result.x = q[0]; + result.y = q[1]; + result.z = q[2]; + result.w = q[3]; + return result; +} + // Convert an orientation quaternion to a 3-by-3 rotation matrix. inline b3Mat33 b3ConvertQuatToRot(const b3Quat& q) { @@ -218,43 +301,4 @@ inline b3Mat33 b3ConvertQuatToRot(const b3Quat& q) b3Vec3( xz + wy, yz - wx, 1.0f - (xx + yy))); } -// Perform a linear interpolation between two quaternions. -inline b3Quat b3Lerp(const b3Quat& a, const b3Quat& b, float32 fraction) -{ - B3_ASSERT(fraction >= 0.0f); - B3_ASSERT(fraction <= 1.0f); - float32 w1 = 1.0f - fraction; - float32 w2 = fraction; - return w1 * a + w2 * b; -} - -// Perform a spherical interpolation between two quaternions. -inline b3Quat b3Slerp(const b3Quat& a, const b3Quat& b, float32 fraction) -{ - B3_ASSERT(fraction >= 0.0f); - B3_ASSERT(fraction <= 1.0f); - float32 w1 = 1.0f - fraction; - float32 w2 = fraction; - - float32 cosine = b3Dot(a, b); - b3Quat b2 = b; - if (cosine <= FLT_EPSILON * FLT_EPSILON) - { - b2 = -b; - cosine = -cosine; - } - - if (cosine > 1.0f - FLT_EPSILON) - { - return w1 * a + w2 * b2; - } - - float32 angle = acos(cosine); - float32 sine = sin(angle); - b3Quat q1 = sin(w1 * angle) * a; - b3Quat q2 = sin(w2 * angle) * b2; - float32 invSin = 1.0f / sine; - return invSin * (q1 + q2); -} - #endif diff --git a/include/bounce/common/settings.h b/include/bounce/common/settings.h index b1ba36d..5caab79 100644 --- a/include/bounce/common/settings.h +++ b/include/bounce/common/settings.h @@ -43,10 +43,6 @@ typedef float float32; // Collision -// Maximum number of vertices, edges, and faces a -// polyhedron can have. Don't increase this value. -#define B3_MAX_HULL_FEATURES (256) - // How much an AABB in the broad-phase should be extended by // to disallow unecessary proxy updates. // A larger value increases performance when there are @@ -60,15 +56,11 @@ typedef float float32; #define B3_AABB_MULTIPLIER (2.0f) // Collision and constraint tolerance. -#define B3_LINEAR_SLOP (0.01f) - -// Collision and constraint tolerance. +#define B3_LINEAR_SLOP (0.005f) #define B3_ANGULAR_SLOP (2.0f / 180.0f * B3_PI) // The radius of the hull shape skin. -#define B3_HULL_RADIUS (2.0f * B3_LINEAR_SLOP) - -// Twice the radius of the hull shape skin. +#define B3_HULL_RADIUS (0.0f * B3_LINEAR_SLOP) #define B3_HULL_RADIUS_SUM (2.0f * B3_HULL_RADIUS) // Dynamics @@ -95,26 +87,22 @@ typedef float float32; #define B3_MAX_ROTATION (0.5f * B3_PI) #define B3_MAX_ROTATION_SQUARED (B3_MAX_ROTATION * B3_MAX_ROTATION) -// The maximum linear position correction used when solving constraints. This helps to +// The maximum position correction used when solving constraints. This helps to // prevent overshoot. #define B3_MAX_LINEAR_CORRECTION (0.2f) - -// The maximum angular position correction used when solving constraints. This helps to -// prevent overshoot. #define B3_MAX_ANGULAR_CORRECTION (8.0f / 180.0f * B3_PI) // This controls how faster overlaps should be resolved per step. // This is less than and would be close to 1, so that the all overlap is resolved per step. // However values very close to 1 may lead to overshoot. -#define B3_BAUMGARTE (0.2f) +#define B3_BAUMGARTE (0.1f) // If the relative velocity of a contact point is below // the threshold then restitution is not applied. #define B3_VELOCITY_THRESHOLD (1.0f) // Sleep - -#define B3_TIME_TO_SLEEP (0.2f ) +#define B3_TIME_TO_SLEEP (0.2f) #define B3_SLEEP_LINEAR_TOL (0.05f) #define B3_SLEEP_ANGULAR_TOL (2.0f / 180.0f * B3_PI) @@ -128,6 +116,8 @@ typedef float float32; #define B3_MiB(n) (1024 * B3_KiB(n)) #define B3_GiB(n) (1024 * B3_MiB(n)) +#define B3_FORCE_INLINE __forceinline + #define B3_PROFILE(name) b3ProfileScope scope(name) // You should implement this function to use your own memory allocator. diff --git a/include/bounce/common/template/array.h b/include/bounce/common/template/array.h index da21e5d..55456a0 100644 --- a/include/bounce/common/template/array.h +++ b/include/bounce/common/template/array.h @@ -50,12 +50,12 @@ public: return m_elements + i; } - const T* Elements() const + const T* Begin() const { return m_elements; } - T* Elements() + T* Begin() { return m_elements; } diff --git a/include/bounce/dynamics/contacts/collide/clip.h b/include/bounce/dynamics/contacts/collide/clip.h index ad3f7b2..7bae6c5 100644 --- a/include/bounce/dynamics/contacts/collide/clip.h +++ b/include/bounce/dynamics/contacts/collide/clip.h @@ -27,19 +27,19 @@ // A combination of features used to uniquely identify a vertex on a feature. struct b3FeaturePair { - u8 inEdgeA; // incoming edge on hull A - u8 inEdgeB; // incoming edge on hull B - u8 outEdgeA; // outgoing edge on hull A - u8 outEdgeB; // outgoing edge on hull B + u8 inEdge1; // incoming edge on hull 1 + u8 inEdge2; // incoming edge on hull 2 + u8 outEdge1; // outgoing edge on hull 1 + u8 outEdge2; // outgoing edge on hull 2 }; -inline b3FeaturePair b3MakePair(u32 inEdgeA, u32 inEdgeB, u32 outEdgeA, u32 outEdgeB) +inline b3FeaturePair b3MakePair(u32 inEdge1, u32 inEdge2, u32 outEdge1, u32 outEdge2) { b3FeaturePair out; - out.inEdgeA = u8(inEdgeA); - out.inEdgeB = u8(inEdgeB); - out.outEdgeA = u8(outEdgeA); - out.outEdgeB = u8(outEdgeB); + out.inEdge1 = u8(inEdge1); + out.inEdge2 = u8(inEdge2); + out.outEdge1 = u8(outEdge1); + out.outEdge2 = u8(outEdge2); return out; } @@ -75,11 +75,11 @@ struct b3ClipPlane }; struct b3Hull; -struct b3Capsule; +struct b3Segment; // Build a clip edge for an edge. void b3BuildEdge(b3ClipVertex vOut[2], - const b3Capsule* hull); + const b3Segment* hull); // Build a clip polygon given an index to the polygon face. void b3BuildPolygon(b3ClipPolygon& pOut, @@ -99,15 +99,15 @@ void b3ClipPolygonToPlane(b3ClipPolygon& pOut, // Clip a segment by a hull face (side planes). // Return the number of output points. u32 b3ClipEdgeToFace(b3ClipVertex vOut[2], - const b3ClipVertex vIn[2], const b3Capsule* hull); + const b3ClipVertex vIn[2], const b3Segment* hull); // Clip a segment by a hull face (side planes). // Return the number of output points. 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); // Clip a polygon by a hull 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); #endif diff --git a/include/bounce/dynamics/contacts/collide/collide.h b/include/bounce/dynamics/contacts/collide/collide.h index 062d7ac..901ce63 100644 --- a/include/bounce/dynamics/contacts/collide/collide.h +++ b/include/bounce/dynamics/contacts/collide/collide.h @@ -44,29 +44,29 @@ struct b3SATFeaturePair { enum Type { - e_edgeA, // an edge on hull A and an edge on hull B - e_faceA, // a face on hull A and a vertex/edge/face on hull B - e_faceB, // a face on hull B and a vertex/edge/face on hull A + e_edge1, // an edge on hull 1 and an edge on hull 2 + e_face1, // a face on hull 1 and a vertex/edge/face on hull 2 + e_face2, // a face on hull 2 and a vertex/edge/face on hull 1 }; b3SATCacheType state; // sat result Type type; // feature pair type - u32 indexA; // feature index on hull A - u32 indexB; // feature index on hull B + u32 index1; // feature index on hull 1 + u32 index2; // feature index on hull 2 }; struct b3FeatureCache { // Read the current state of the cache. // Return e_unkown if neither a separation or penetration was detected. - b3SATCacheType ReadState(const b3Transform& xfA, const b3Hull* hullA, - const b3Transform& xfB, const b3Hull* hullB); + b3SATCacheType ReadState(const b3Transform& xf1, float32 r1, const b3Hull* hull1, + const b3Transform& xf2, float32 r2, const b3Hull* hull2); - b3SATCacheType ReadEdge(const b3Transform& xfA, const b3Hull* hullA, - const b3Transform& xfB, const b3Hull* hullB); + b3SATCacheType ReadEdge(const b3Transform& xf1, float32 r1, const b3Hull* hull1, + const b3Transform& xf2, float32 r2, const b3Hull* hull2); - b3SATCacheType ReadFace(const b3Transform& xfA, const b3Hull* hullA, - const b3Transform& xfB, const b3Hull* hullB); + b3SATCacheType ReadFace(const b3Transform& xf1, float32 r1, const b3Hull* hull1, + const b3Transform& xf2, float32 r2, const b3Hull* hull2); // We could increase the cache size (e.g. a feature pair of the last two frames). b3SATFeaturePair m_featurePair; @@ -95,45 +95,45 @@ public: }; // Test if two generic shapes are overlapping. -bool b3TestOverlap(const b3Transform& xfA, u32 indexA, const b3Shape* shapeA, - const b3Transform& xfB, u32 indexB, const b3Shape* shapeB, +bool b3TestOverlap(const b3Transform& xf1, u32 index1, const b3Shape* shape1, + const b3Transform& xf2, u32 index2, const b3Shape* shape2, b3ConvexCache* cache); // Compute a manifold for two generic shapes except when one of them is a mesh. void b3CollideShapeAndShape(b3Manifold& manifold, - const b3Transform& xfA, const b3Shape* shapeA, - const b3Transform& xfB, const b3Shape* shapeB, + const b3Transform& xf1, const b3Shape* shape1, + const b3Transform& xf2, const b3Shape* shape2, b3ConvexCache* cache); // Compute a manifold for two spheres. void b3CollideSphereAndSphere(b3Manifold& manifold, - const b3Transform& xfA, const b3SphereShape* shapeA, - const b3Transform& xfB, const b3SphereShape* shapeB); + const b3Transform& xf1, const b3SphereShape* shape1, + const b3Transform& xf2, const b3SphereShape* shape2); // Compute a manifold for a sphere and a hull. -void b3CollideSphereAndHull(b3Manifold& manifold, - const b3Transform& xfA, const b3SphereShape* shapeA, - const b3Transform& xfB, const b3HullShape* shapeB); +void b3CollideSphereAndHull(b3Manifold& manifold, + const b3Transform& xf1, const b3SphereShape* shape1, + const b3Transform& xf2, const b3HullShape* shape2); // Compute a manifold for a sphere and a capsule. void b3CollideSphereAndCapsule(b3Manifold& manifold, - const b3Transform& xfA, const b3SphereShape* shapeA, - const b3Transform& xfB, const b3CapsuleShape* shapeB); + const b3Transform& xf1, const b3SphereShape* shape1, + const b3Transform& xf2, const b3CapsuleShape* shape2); // Compute a manifold for two capsules. void b3CollideCapsuleAndCapsule(b3Manifold& manifold, - const b3Transform& xfA, const b3CapsuleShape* shapeA, - const b3Transform& xfB, const b3CapsuleShape* shapeB); + const b3Transform& xf1, const b3CapsuleShape* shape1, + const b3Transform& xf2, const b3CapsuleShape* shape2); // Compute a manifold for a capsule and a hull. -void b3CollideCapsuleAndHull(b3Manifold& manifold, - const b3Transform& xfA, const b3CapsuleShape* shapeA, - const b3Transform& xfB, const b3HullShape* shapeB); +void b3CollideCapsuleAndHull(b3Manifold& manifold, + const b3Transform& xf1, const b3CapsuleShape* shape1, + const b3Transform& xf2, const b3HullShape* shape2); // Compute a manifold for two hulls. -void b3CollideHullAndHull(b3Manifold& manifold, - const b3Transform& xfA, const b3HullShape* shapeA, - const b3Transform& xfB, const b3HullShape* shapeB, +void b3CollideHullAndHull(b3Manifold& manifold, + const b3Transform& xf1, const b3HullShape* shape1, + const b3Transform& xf2, const b3HullShape* shape2, b3ConvexCache* cache); -#endif +#endif \ No newline at end of file diff --git a/include/bounce/dynamics/contacts/contact.h b/include/bounce/dynamics/contacts/contact.h index a08ed85..283301f 100644 --- a/include/bounce/dynamics/contacts/contact.h +++ b/include/bounce/dynamics/contacts/contact.h @@ -118,7 +118,7 @@ protected: // Test if the shapes in this contact are overlapping. virtual bool TestOverlap() = 0; - // Build new contact points. + // Initialize contact constraits. virtual void Collide() = 0; b3ContactType m_type; diff --git a/include/bounce/dynamics/contacts/contact_cluster.h b/include/bounce/dynamics/contacts/contact_cluster.h index 6cb09c2..88ddf8d 100644 --- a/include/bounce/dynamics/contacts/contact_cluster.h +++ b/include/bounce/dynamics/contacts/contact_cluster.h @@ -64,6 +64,6 @@ u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32 // Reduce a set of contact points to a quad (approximate convex polygon). // All points must lie in a common plane and an initial point must be given. void b3ReducePolygon(b3ClusterPolygon& pOut, const b3ClusterPolygon& pIn, - u32 startIndex); + u32 startIndex, const b3Vec3& normal); #endif diff --git a/include/bounce/dynamics/contacts/contact_solver.h b/include/bounce/dynamics/contacts/contact_solver.h index 335d49c..24781fa 100644 --- a/include/bounce/dynamics/contacts/contact_solver.h +++ b/include/bounce/dynamics/contacts/contact_solver.h @@ -62,29 +62,25 @@ struct b3VelocityConstraintPoint { b3Vec3 rA; b3Vec3 rB; - + b3Vec3 normal; float32 normalMass; float32 normalImpulse; - - b3Vec3 tangent1; - b3Vec3 tangent2; - b3Mat22 tangentMass; - b3Vec2 tangentImpulse; - float32 velocityBias; }; struct b3VelocityConstraintManifold { - b3Vec3 center; + b3Vec3 rA; + b3Vec3 rB; + b3Vec3 normal; b3Vec3 tangent1; b3Vec3 tangent2; - b3Vec3 rA; - b3Vec3 rB; + //float32 leverArm; + b3Mat22 tangentMass; - b3Vec2 tangentImpulse; + b3Vec2 tangentImpulse; float32 motorImpulse; float32 motorMass; diff --git a/include/bounce/dynamics/contacts/manifold.h b/include/bounce/dynamics/contacts/manifold.h index 906fd2b..0da95d3 100644 --- a/include/bounce/dynamics/contacts/manifold.h +++ b/include/bounce/dynamics/contacts/manifold.h @@ -27,67 +27,54 @@ // A contact manifold point. struct b3ManifoldPoint { + b3Vec3 localNormal1; // local normal on the first shape + b3Vec3 localPoint1; // local point on the first shape without its radius + b3Vec3 localPoint2; // local point on the other shape without its radius + u32 triangleKey; // triangle identifier u32 key; // point identifier - b3Vec3 localNormal; // local normal on the first shape - b3Vec3 localPoint; // local point on the first shape - b3Vec3 localPoint2; // local point on the other shape + float32 normalImpulse; // normal impulse - b3Vec2 tangentImpulse; // tangent impulses - u8 persisting; // indicates that the point is persisting + u32 persisting; // is this point persistent? }; -// A manifold is a group of contact points with similar contact normal. +// A contact manifold is a group of contact points with similar contact normal. struct b3Manifold { - // Choose arbitrary impulses for warm starting. - void GuessImpulses(); + // Clear the manifold. + // Initialize impulses arbitrarily for warm starting. + void Initialize(); - // Initialize impulses for warm starting. - void FindImpulses(const b3Manifold& old); + // Initialize impulses for warm starting from the old manifold. + void Initialize(const b3Manifold& old); b3ManifoldPoint points[B3_MAX_MANIFOLD_POINTS]; // manifold points u32 pointCount; // number of manifold points - b3Vec3 center; - b3Vec3 normal; - b3Vec3 tangent1; - b3Vec3 tangent2; b3Vec2 tangentImpulse; float32 motorImpulse; }; -// A world manifold point. struct b3WorldManifoldPoint { - // Initialize this manifold from a local manifold point and two transforms. - // The radii should come from the shapes that generated the manifold. - void Initialize(const b3ManifoldPoint* point, - const b3Transform& xfA, float32 radiusA, - const b3Transform& xfB, float32 radiusB); - + void Initialize(const b3ManifoldPoint* p, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB); + b3Vec3 point; b3Vec3 normal; - b3Vec2 tangents[2]; float32 separation; }; -// A contact manifold is a group of contact points with similar normal. struct b3WorldManifold { - // Initialize this world manifold from a local manifold and two transforms. - // The radii should come from the shapes that generated the manifold. - void Initialize(const b3Manifold* manifold, - const b3Transform& xfA, float32 radiusA, - const b3Transform& xfB, float32 radiusB); + void Initialize(const b3Manifold* m, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB); + + u32 pointCount; + b3WorldManifoldPoint points[B3_MAX_MANIFOLD_POINTS]; b3Vec3 center; b3Vec3 normal; b3Vec3 tangent1; b3Vec3 tangent2; - - b3WorldManifoldPoint points[B3_MAX_MANIFOLD_POINTS]; // contact points - u32 pointCount; // number of contact points }; -#endif +#endif \ No newline at end of file diff --git a/include/bounce/dynamics/joints/cone_joint.h b/include/bounce/dynamics/joints/cone_joint.h index 932848b..1d2b941 100644 --- a/include/bounce/dynamics/joints/cone_joint.h +++ b/include/bounce/dynamics/joints/cone_joint.h @@ -37,7 +37,7 @@ struct b3ConeJointDef : public b3JointDef // The joint frame relative to body A's frame. b3Transform localFrameA; - + // The joint frame relative to body B's frame. b3Transform localFrameB; @@ -64,7 +64,7 @@ public: // Get the joint frame relative to body A's frame. const b3Transform& GetLocalFrameA() const; - + // Get the joint frame relative to body B's frame. const b3Transform& GetLocalFrameB() const; @@ -109,7 +109,7 @@ private: float32 m_mA; float32 m_mB; b3Mat33 m_iA; - b3Mat33 m_iB; + b3Mat33 m_iB; b3Vec3 m_localCenterA; b3Vec3 m_localCenterB; diff --git a/include/bounce/dynamics/joints/joint.h b/include/bounce/dynamics/joints/joint.h index 59c3e20..e552ebb 100644 --- a/include/bounce/dynamics/joints/joint.h +++ b/include/bounce/dynamics/joints/joint.h @@ -20,6 +20,7 @@ #define B3_JOINT_H #include +#include #include #include diff --git a/include/bounce/dynamics/joints/revolute_joint.h b/include/bounce/dynamics/joints/revolute_joint.h index 14e4f8b..f17f98d 100644 --- a/include/bounce/dynamics/joints/revolute_joint.h +++ b/include/bounce/dynamics/joints/revolute_joint.h @@ -20,15 +20,17 @@ #define B3_REVOLUTE_JOINT_H #include -#include struct b3RevoluteJointDef : public b3JointDef { b3RevoluteJointDef() { type = e_revoluteJoint; - localFrameA.SetIdentity(); - localFrameB.SetIdentity(); + localAnchorA.SetZero(); + localRotationA.SetIdentity(); + localAnchorB.SetZero(); + localRotationB.SetIdentity(); + referenceRotation.SetIdentity(); enableLimit = false; lowerAngle = 0.0f; upperAngle = 0.0f; @@ -40,19 +42,28 @@ struct b3RevoluteJointDef : public b3JointDef // Initialize this definition from hinge axis, anchor point, and the lower and upper angle limits in radians. void Initialize(b3Body* bodyA, b3Body* bodyB, const b3Vec3& axis, const b3Vec3& anchor, float32 lowerAngle, float32 upperAngle); - // The joint frame relative body A's frame. - b3Transform localFrameA; - - // The joint frame relative body B's frame. - b3Transform localFrameB; + // The joint anchor relative body A's origin. + b3Vec3 localAnchorA; + + // The joint orientation relative body A's orientation. + b3Quat localRotationA; + + // The joint anchor relative body B's origin. + b3Vec3 localAnchorB; + + // The joint orientation relative body B's orientation. + b3Quat localRotationB; + + // The initial relative rotation from body A to body B. + b3Quat referenceRotation; // Enable the joint limit. bool enableLimit; - // The lower angle limit in radians. + // The hinge lower angle limit in radians. float32 lowerAngle; - // The upper angle limit in radians. + // The hinge upper angle limit in radians. float32 upperAngle; // Enable the joint motor. @@ -67,7 +78,7 @@ struct b3RevoluteJointDef : public b3JointDef // A revolute joint constrains two bodies to share a point and an axis while // they are free to rotate about the point and the axis. -// The relative rotation about the shared axis is the joint angle. +// The relative rotation about the shared axis is the joint rotation. // You can limit the relative rotation with a lower and upper angle limit. // You can use a motor to drive the relative rotation about the shared axis. // A maximum motor torque is provided so that infinite forces are not generated. @@ -82,10 +93,10 @@ public: b3Transform GetFrameB() const; // Get the joint frame relative body A's frame. - const b3Transform& GetLocalFrameA() const; + b3Transform GetLocalFrameA() const; // Get the joint frame relative body B's frame. - const b3Transform& GetLocalFrameB() const; + b3Transform GetLocalFrameB() const; // Is the joint limit enabled? bool IsLimitEnabled() const; @@ -135,8 +146,13 @@ private: virtual bool SolvePositionConstraints(const b3SolverData* data); // Solver shared - b3Transform m_localFrameA; - b3Transform m_localFrameB; + b3Quat m_referenceRotation; + + b3Vec3 m_localAnchorA; + b3Quat m_localRotationA; + + b3Vec3 m_localAnchorB; + b3Quat m_localRotationB; bool m_enableMotor; float32 m_motorSpeed; @@ -155,24 +171,31 @@ private: b3Mat33 m_iB; b3Vec3 m_localCenterA; b3Vec3 m_localCenterB; - - // Motor - // The limit axis is the same as the motor axis + + // Hinge motor + b3Vec3 m_motor_J1; // 1x3 (row) + b3Vec3 m_motor_J2; // 1x3 (row) float32 m_motorMass; float32 m_motorImpulse; - // Limit - b3Vec3 m_limitAxis; // axis of rotation for limit contraint - float32 m_limitImpulse; + // Hinge limit + // The limit axis and constraint space mass are the same as the motor's b3LimitState m_limitState; // constraint state + float32 m_limitImpulse; - // Point-to-point + axes-to-axes + // Spherical b3Vec3 m_rA; b3Vec3 m_rB; - b3Vec3 m_nA; - b3Vec3 m_nB; - b3Mat<5, 5> m_mass; // block solver - b3Vec<5> m_impulse; // block solver + b3Mat33 m_mass; + b3Vec3 m_impulse; + + // Hinge + b3Mat23 m_J1; + b3Mat23 m_J2; + b3Mat32 m_J1T; + b3Mat32 m_J2T; + b3Mat22 m_K; + b3Vec2 m_axisImpulse; }; #endif \ No newline at end of file diff --git a/include/bounce/dynamics/joints/weld_joint.h b/include/bounce/dynamics/joints/weld_joint.h index 959334e..be60426 100644 --- a/include/bounce/dynamics/joints/weld_joint.h +++ b/include/bounce/dynamics/joints/weld_joint.h @@ -28,7 +28,7 @@ struct b3WeldJointDef : public b3JointDef type = e_weldJoint; localAnchorA.SetZero(); localAnchorB.SetZero(); - relativeRotation.SetIdentity(); + referenceRotation.SetIdentity(); } // Initialize this definition from bodies and world anchor point. @@ -41,7 +41,7 @@ struct b3WeldJointDef : public b3JointDef b3Vec3 localAnchorB; // The initial relative rotation from body A to body B. - b3Quat relativeRotation; + b3Quat referenceRotation; }; // A weld joint removes the relative movement between two bodies. @@ -73,8 +73,7 @@ private: // Solver shared b3Vec3 m_localAnchorA; b3Vec3 m_localAnchorB; - - b3Quat m_dq0; + b3Quat m_referenceRotation; // Solver temp u32 m_indexA; @@ -92,8 +91,13 @@ private: b3Vec3 m_impulse; b3Mat33 m_mass; + // Weld constraint + b3Mat33 m_J1; + b3Mat33 m_J2; + b3Mat33 m_J1T; + b3Mat33 m_J2T; b3Vec3 m_axisImpulse; - //b3Vec3 m_velocityBias; + b3Mat33 m_K; }; #endif \ No newline at end of file diff --git a/include/bounce/dynamics/shapes/capsule_shape.h b/include/bounce/dynamics/shapes/capsule_shape.h index 398b679..eb0f500 100644 --- a/include/bounce/dynamics/shapes/capsule_shape.h +++ b/include/bounce/dynamics/shapes/capsule_shape.h @@ -21,7 +21,6 @@ #include -// A capsule defined along the up-axis. class b3CapsuleShape : public b3Shape { public: diff --git a/include/bounce/dynamics/shapes/shape.h b/include/bounce/dynamics/shapes/shape.h index ca1e750..9179556 100644 --- a/include/bounce/dynamics/shapes/shape.h +++ b/include/bounce/dynamics/shapes/shape.h @@ -21,7 +21,7 @@ #include #include -#include +#include struct b3ContactEdge; diff --git a/src/bounce/collision/broad_phase.cpp b/src/bounce/collision/broad_phase.cpp index 4d7be99..f381ec9 100644 --- a/src/bounce/collision/broad_phase.cpp +++ b/src/bounce/collision/broad_phase.cpp @@ -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; diff --git a/src/bounce/collision/distance.cpp b/src/bounce/collision/distance.cpp deleted file mode 100644 index 4fc2766..0000000 --- a/src/bounce/collision/distance.cpp +++ /dev/null @@ -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 -#include -#include -#include - -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); -} diff --git a/src/bounce/collision/gjk/gjk.cpp b/src/bounce/collision/gjk/gjk.cpp index 4c46081..445e756 100644 --- a/src/bounce/collision/gjk/gjk.cpp +++ b/src/bounce/collision/gjk/gjk.cpp @@ -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. diff --git a/src/bounce/collision/gjk/gjk_cache.cpp b/src/bounce/collision/gjk/gjk_cache.cpp index 31678a3..1d058b7 100644 --- a/src/bounce/collision/gjk/gjk_cache.cpp +++ b/src/bounce/collision/gjk/gjk_cache.cpp @@ -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; } } diff --git a/src/bounce/collision/gjk/gjk_feature_pair.cpp b/src/bounce/collision/gjk/gjk_feature_pair.cpp index 39cf294..efeb58b 100644 --- a/src/bounce/collision/gjk/gjk_feature_pair.cpp +++ b/src/bounce/collision/gjk/gjk_feature_pair.cpp @@ -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; } diff --git a/src/bounce/collision/sat/sat.cpp b/src/bounce/collision/sat/sat.cpp index 3eb2284..0985189 100644 --- a/src/bounce/collision/sat/sat.cpp +++ b/src/bounce/collision/sat/sat.cpp @@ -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; } diff --git a/src/bounce/collision/sat/sat_edge_and_hull.cpp b/src/bounce/collision/sat/sat_edge_and_hull.cpp index aa1fd35..ca6ca9a 100644 --- a/src/bounce/collision/sat/sat_edge_and_hull.cpp +++ b/src/bounce/collision/sat/sat_edge_and_hull.cpp @@ -20,26 +20,26 @@ #include #include -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; } diff --git a/src/bounce/collision/sat/sat_vertex_and_hull.cpp b/src/bounce/collision/sat/sat_vertex_and_hull.cpp index e7f8da8..b3ea83c 100644 --- a/src/bounce/collision/sat/sat_vertex_and_hull.cpp +++ b/src/bounce/collision/sat/sat_vertex_and_hull.cpp @@ -20,24 +20,24 @@ #include #include -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) diff --git a/src/bounce/collision/shapes/hull.cpp b/src/bounce/collision/shapes/hull.cpp index 2b73245..c9ac864 100644 --- a/src/bounce/collision/shapes/hull.cpp +++ b/src/bounce/collision/shapes/hull.cpp @@ -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); } } diff --git a/src/bounce/common/math/mat.cpp b/src/bounce/common/math/mat.cpp index db118f0..af561c8 100644 --- a/src/bounce/common/math/mat.cpp +++ b/src/bounce/common/math/mat.cpp @@ -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; -} diff --git a/src/bounce/dynamics/contacts/collide/clip.cpp b/src/bounce/dynamics/contacts/collide/clip.cpp index 650e7b1..48e7b38 100644 --- a/src/bounce/dynamics/contacts/collide/clip.cpp +++ b/src/bounce/dynamics/contacts/collide/clip.cpp @@ -21,7 +21,7 @@ #include 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; diff --git a/src/bounce/dynamics/contacts/collide/collide.cpp b/src/bounce/dynamics/contacts/collide/collide.cpp index 02b56b5..0630b79 100644 --- a/src/bounce/dynamics/contacts/collide/collide.cpp +++ b/src/bounce/dynamics/contacts/collide/collide.cpp @@ -25,7 +25,7 @@ #include #include #include -#include +#include 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] = diff --git a/src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp b/src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp index db2d1b1..2013c01 100644 --- a/src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp @@ -19,159 +19,167 @@ #include #include #include +#include #include #include +#include #include #include -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); } -} +} \ No newline at end of file diff --git a/src/bounce/dynamics/contacts/collide/collide_capsules.cpp b/src/bounce/dynamics/contacts/collide/collide_capsules.cpp index 87d2720..54ce95c 100644 --- a/src/bounce/dynamics/contacts/collide/collide_capsules.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_capsules.cpp @@ -22,16 +22,128 @@ #include #include -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; } } \ No newline at end of file diff --git a/src/bounce/dynamics/contacts/collide/collide_hulls.cpp b/src/bounce/dynamics/contacts/collide/collide_hulls.cpp index 655be8d..0ea2f51 100644 --- a/src/bounce/dynamics/contacts/collide/collide_hulls.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_hulls.cpp @@ -23,236 +23,245 @@ #include #include -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 polygonB; - u32 indexB = hullB->GetSupportFace(-normalA); - b3BuildPolygon(polygonB, xfB, indexB, hullB); + // Find the support face polygon in the *negated* direction. + b3StackArray 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 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 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 polygonA; + b3StackArray 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 reducedPolygonA; - b3ReducePolygon(reducedPolygonA, polygonA, minIndex); - B3_ASSERT(!reducedPolygonA.IsEmpty()); + b3Vec3 normal = plane1.normal; + b3Vec3 s_normal = flipNormal ? -normal : normal; + + b3StackArray 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); + } +} \ No newline at end of file diff --git a/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp b/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp index 5b8d9b5..01dbfdd 100644 --- a/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp @@ -23,95 +23,114 @@ #include #include -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); +} \ No newline at end of file diff --git a/src/bounce/dynamics/contacts/collide/collide_sphere_capsule.cpp b/src/bounce/dynamics/contacts/collide/collide_sphere_capsule.cpp index 1418de7..88ce3fe 100644 --- a/src/bounce/dynamics/contacts/collide/collide_sphere_capsule.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_sphere_capsule.cpp @@ -20,24 +20,22 @@ #include #include #include -#include -#include -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); } diff --git a/src/bounce/dynamics/contacts/collide/collide_sphere_hull.cpp b/src/bounce/dynamics/contacts/collide/collide_sphere_hull.cpp index ebdafe7..52d225c 100644 --- a/src/bounce/dynamics/contacts/collide/collide_sphere_hull.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_sphere_hull.cpp @@ -23,69 +23,63 @@ #include #include -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); } \ No newline at end of file diff --git a/src/bounce/dynamics/contacts/collide/collide_sphere_mesh.cpp b/src/bounce/dynamics/contacts/collide/collide_sphere_mesh.cpp deleted file mode 100644 index 7c29324..0000000 --- a/src/bounce/dynamics/contacts/collide/collide_sphere_mesh.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include - -// 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); -} \ No newline at end of file diff --git a/src/bounce/dynamics/contacts/collide/collide_spheres.cpp b/src/bounce/dynamics/contacts/collide/collide_spheres.cpp index 79b527f..5944b15 100644 --- a/src/bounce/dynamics/contacts/collide/collide_spheres.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_spheres.cpp @@ -21,43 +21,35 @@ #include #include -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); } \ No newline at end of file diff --git a/src/bounce/dynamics/contacts/contact.cpp b/src/bounce/dynamics/contacts/contact.cpp index 5270f8f..5968963 100644 --- a/src/bounce/dynamics/contacts/contact.cpp +++ b/src/bounce/dynamics/contacts/contact.cpp @@ -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) { diff --git a/src/bounce/dynamics/contacts/contact_cluster.cpp b/src/bounce/dynamics/contacts/contact_cluster.cpp index 0c3c672..bd9474a 100644 --- a/src/bounce/dynamics/contacts/contact_cluster.cpp +++ b/src/bounce/dynamics/contacts/contact_cluster.cpp @@ -17,9 +17,9 @@ */ #include -#include +#include -inline void AddCluster(b3Array& clusters, const b3Vec3& centroid) +static void AddCluster(b3Array& clusters, const b3Vec3& centroid) { const float32 kTol = 0.05f; for (u32 i = 0; i < clusters.Count(); ++i) @@ -165,7 +165,7 @@ void b3InitializeClusters(b3Array& outClusters, const b3Array& observations, u32 fromCluster, u32 toCluster) +static void b3MoveObsToCluster(b3Array& observations, u32 fromCluster, u32 toCluster) { for (u32 i = 0; i < observations.Count(); ++i) { @@ -177,7 +177,7 @@ inline void b3MoveObsToCluster(b3Array& observations, u32 fromClu } } -inline u32 b3BestCluster(const b3Array& clusters, const b3Vec3& point) +static u32 b3BestCluster(const b3Array& clusters, const b3Vec3& point) { u32 bestIndex = 0; float32 bestValue = B3_MAX_FLOAT; @@ -276,21 +276,49 @@ void b3Clusterize(b3Array& outClusters, b3Array& 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 reducedB; - b3ReducePolygon(reducedB, polygonB, minIndex); + b3ReducePolygon(reducedB, polygonB, minIndex, normal); for (u32 j = 0; j < reducedB.Count(); ++j) { b3ClusterVertex v = reducedB[j]; diff --git a/src/bounce/dynamics/contacts/contact_solver.cpp b/src/bounce/dynamics/contacts/contact_solver.cpp index 73030af..e80fb47 100644 --- a/src/bounce/dynamics/contacts/contact_solver.cpp +++ b/src/bounce/dynamics/contacts/contact_solver.cpp @@ -21,9 +21,7 @@ #include #include #include - -// Turn on or off central friction. This is an important optimization for old hardwares. -#define B3_CENTRAL_FRICTION 0 +#include // 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; } diff --git a/src/bounce/dynamics/contacts/manifold.cpp b/src/bounce/dynamics/contacts/manifold.cpp index 14b934d..e5f23c0 100644 --- a/src/bounce/dynamics/contacts/manifold.cpp +++ b/src/bounce/dynamics/contacts/manifold.cpp @@ -18,7 +18,7 @@ #include -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); + } +} \ No newline at end of file diff --git a/src/bounce/dynamics/contacts/mesh_contact.cpp b/src/bounce/dynamics/contacts/mesh_contact.cpp index 40e783a..9b24dea 100644 --- a/src/bounce/dynamics/contacts/mesh_contact.cpp +++ b/src/bounce/dynamics/contacts/mesh_contact.cpp @@ -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); } \ No newline at end of file diff --git a/src/bounce/dynamics/draw_world.cpp b/src/bounce/dynamics/draw_world.cpp index 105e71b..bf7846d 100644 --- a/src/bounce/dynamics/draw_world.cpp +++ b/src/bounce/dynamics/draw_world.cpp @@ -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); + } } } } diff --git a/src/bounce/dynamics/island.cpp b/src/bounce/dynamics/island.cpp index 2647d2a..c77a786 100644 --- a/src/bounce/dynamics/island.cpp +++ b/src/bounce/dynamics/island.cpp @@ -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; diff --git a/src/bounce/dynamics/joints/cone_joint.cpp b/src/bounce/dynamics/joints/cone_joint.cpp index 673fc7f..a5217a1 100644 --- a/src/bounce/dynamics/joints/cone_joint.cpp +++ b/src/bounce/dynamics/joints/cone_joint.cpp @@ -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); diff --git a/src/bounce/dynamics/joints/revolute_joint.cpp b/src/bounce/dynamics/joints/revolute_joint.cpp index 29b74bd..2c75321 100644 --- a/src/bounce/dynamics/joints/revolute_joint.cpp +++ b/src/bounce/dynamics/joints/revolute_joint.cpp @@ -20,71 +20,216 @@ #include #include -// 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 diff --git a/src/bounce/dynamics/joints/weld_joint.cpp b/src/bounce/dynamics/joints/weld_joint.cpp index 6e40395..1df83c3 100644 --- a/src/bounce/dynamics/joints/weld_joint.cpp +++ b/src/bounce/dynamics/joints/weld_joint.cpp @@ -19,7 +19,76 @@ #include #include #include -#include + +/* +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; diff --git a/src/bounce/dynamics/shapes/capsule_shape.cpp b/src/bounce/dynamics/shapes/capsule_shape.cpp index a28aa6d..5373b11 100644 --- a/src/bounce/dynamics/shapes/capsule_shape.cpp +++ b/src/bounce/dynamics/shapes/capsule_shape.cpp @@ -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 diff --git a/src/bounce/quickhull/qh_hull.cpp b/src/bounce/quickhull/qh_hull.cpp index c39474f..42179b2 100644 --- a/src/bounce/quickhull/qh_hull.cpp +++ b/src/bounce/quickhull/qh_hull.cpp @@ -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)