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

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

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

197
examples/testbed/framework/debug_draw.cpp
View File

@ -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();
}

26
examples/testbed/framework/debug_draw.h
View File

@ -20,7 +20,6 @@
#define DEBUG_DRAW_H
#include <bounce/bounce.h>
#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;

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

@ -16,13 +16,18 @@
* 3. This notice may not be removed or altered from any source distribution.
*/
#if defined(__APPLE_CC__)
#include <OpenGL/gl3.h>
#else
#include <glad/glad.h>
#include <glfw/glfw3.h>
#endif
#include <imgui/imgui.h>
#include <imgui/imgui_impl_glfw_gl3.h>
#include <testbed/tests/test.h>
#include <glfw/glfw3.h>
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;
}

131
examples/testbed/framework/mat44.h
View File

@ -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 <bounce/bounce.h>
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

338
examples/testbed/framework/test.cpp
View File

@ -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))

30
examples/testbed/framework/test_entries.cpp
View File

@ -20,20 +20,25 @@
#include <testbed/tests/quickhull_test.h>
#include <testbed/tests/cluster_test.h>
#include <testbed/tests/distance_test.h>
#include <testbed/tests/capsule_distance.h>
#include <testbed/tests/collide_test.h>
#include <testbed/tests/capsule_collision.h>
#include <testbed/tests/capsule_and_hull_collision.h>
#include <testbed/tests/capsule_and_hull_collision_1.h>
#include <testbed/tests/capsule_and_hull_collision_2.h>
#include <testbed/tests/hull_collision.h>
#include <testbed/tests/hull_collision_2.h>
#include <testbed/tests/linear_motion.h>
#include <testbed/tests/angular_motion.h>
#include <testbed/tests/multiple_shapes.h>
#include <testbed/tests/initial_overlap.h>
#include <testbed/tests/capsule_and_hull_contact_1.h>
#include <testbed/tests/quadric_shapes.h>
#include <testbed/tests/multiple_shapes.h>
#include <testbed/tests/gyro_test.h>
#include <testbed/tests/spring.h>
#include <testbed/tests/weld_test.h>
#include <testbed/tests/newton_cradle.h>
#include <testbed/tests/cone_test.h>
#include <testbed/tests/hinge_motor.h>
#include <testbed/tests/hinge_chain.h>
#include <testbed/tests/newton_cradle.h>
#include <testbed/tests/ragdoll.h>
#include <testbed/tests/mesh_contact_test.h>
#include <testbed/tests/sphere_stack.h>
@ -51,27 +56,32 @@
#include <testbed/tests/varying_friction.h>
#include <testbed/tests/varying_restitution.h>
#include <testbed/tests/cloth_test.h>
#include <testbed/tests/tumbler.h>
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 }
};

45
examples/testbed/tests/angular_motion.h
View File

@ -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

32
examples/testbed/tests/box_stack.h
View File

@ -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

12
examples/testbed/tests/capsule_and_hull_collision.h → examples/testbed/tests/capsule_and_hull_collision_1.h
View File

@ -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;

60
examples/testbed/tests/capsule_and_hull_collision_2.h Normal file
View File

@ -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

69
examples/testbed/tests/capsule_and_hull_contact_1.h Normal file
View File

@ -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

128
examples/testbed/tests/capsule_distance.h
View File

@ -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

12
examples/testbed/tests/capsule_stack.h
View File

@ -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;

21
examples/testbed/tests/cloth_test.h
View File

@ -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

30
examples/testbed/tests/collide_test.h
View File

@ -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)

84
examples/testbed/tests/cone_test.h Normal file
View File

@ -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

14
examples/testbed/tests/distance_test.h
View File

@ -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);

119
examples/testbed/tests/gyro_test.h Normal file
View File

@ -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 <testbed/tests/quickhull_test.h>
extern DebugDraw* g_debugDraw;
extern Camera g_camera;
extern Settings g_settings;
class GyroTest : public Test
{
public:
GyroTest()
{
{
b3StackArray<b3Vec3, 32> 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

14
examples/testbed/tests/hinge_chain.h
View File

@ -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);
}

45
examples/testbed/tests/hinge_motor.h
View File

@ -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;
};

26
examples/testbed/tests/hull_collision.h
View File

@ -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;
};

60
examples/testbed/tests/hull_collision_2.h Normal file
View File

@ -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

102
examples/testbed/tests/initial_overlap.h Normal file
View File

@ -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

19
examples/testbed/tests/jenga.h
View File

@ -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

3
examples/testbed/tests/multiple_shapes.h
View File

@ -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);

102
examples/testbed/tests/quadric_shapes.h
View File

@ -34,35 +34,6 @@ public:
g_camera.m_zoom = 20.0f;
g_settings.drawCenterOfMasses = true;
{
b3StackArray<b3Vec3, 32> 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<b3Vec3, 32> 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<b3Vec3, 32> 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<b3Vec3, 32> 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);

79
examples/testbed/tests/quickhull_test.h
View File

@ -266,28 +266,26 @@ inline b3Hull ConvertHull(const qhHull& hull)
inline void ConstructCylinder(b3Array<b3Vec3>& 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<b3Vec3>& 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<b3Vec3>& points, float32 radius = 1.0f, fl
inline void ConstructCone(b3Array<b3Vec3>& 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

12
examples/testbed/tests/ragdoll.h
View File

@ -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

10
examples/testbed/tests/ray_cast.h
View File

@ -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;

10
examples/testbed/tests/sensor_test.h
View File

@ -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;

2
examples/testbed/tests/sphere_stack.h
View File

@ -33,7 +33,7 @@ public:
{
{
b3BodyDef bd;
bd.type = b3BodyType::e_staticBody;
bd.type = e_staticBody;
b3Body* ground = m_world.CreateBody(bd);
b3HullShape hs;

10
examples/testbed/tests/spring.h
View File

@ -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;

40
examples/testbed/tests/test.h
View File

@ -27,6 +27,14 @@
#include <testbed/framework/debug_draw.h>
#include <testbed/framework/profiler.h>
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

12
examples/testbed/tests/thin.h
View File

@ -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;

337
examples/testbed/tests/tumbler.h Normal file
View File

@ -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 <testbed/tests/quickhull_test.h>
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<b3Vec3, 32> 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<b3Vec3, 32> 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

19
examples/testbed/tests/varying_friction.h
View File

@ -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;

46
examples/testbed/tests/weld_test.h
View File

@ -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);
}
}

7
external/imgui/imgui_impl_glfw_gl3.cpp vendored
View File

@ -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 <imgui/imgui.h>
#include "imgui_impl_glfw_gl3.h"
#if defined(__APPLE_CC__)
#include <OpenGL/gl3.h>
#else
// GLEW/GL3W/GLAD/...
#include <glad/glad.h>
#endif
#include <imgui/imgui.h>
#include "imgui_impl_glfw_gl3.h"
#include <glfw/glfw3.h>
#if defined ( _WIN32 )

2
include/bounce/bounce.h
View File

@ -30,7 +30,7 @@
#include <bounce/collision/gjk/gjk.h>
#include <bounce/collision/gjk/gjk_cache.h>
#include <bounce/collision/sat/sat.h>
#include <bounce/collision/distance.h>
#include <bounce/collision/collision.h>
#include <bounce/collision/broad_phase.h>
#include <bounce/collision/shapes/sphere.h>

2
include/bounce/cloth/cloth.h
View File

@ -20,7 +20,7 @@
#define B3_CLOTH_H
#include <bounce/common/math/vec3.h>
#include <bounce/collision/distance.h>
#include <bounce/collision/collision.h>
struct b3Mesh;
class b3Draw;

18
include/bounce/collision/broad_phase.h
View File

@ -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<class T>
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;

34
include/bounce/collision/distance.h → include/bounce/collision/collision.h
View File

@ -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 <bounce/common/geometry.h>
#include <bounce/collision/shapes/aabb3.h>
///////////////////////////////////////////////////////////////////////////////////////////////////
// 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
{

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

@ -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

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

@ -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

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

@ -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

12
include/bounce/collision/sat/sat_edge_and_hull.h
View File

@ -21,20 +21,20 @@
#include <bounce/collision/sat/sat.h>
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

8
include/bounce/collision/sat/sat_vertex_and_hull.h
View File

@ -21,13 +21,11 @@
#include <bounce/collision/sat/sat.h>
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

6
include/bounce/collision/shapes/box_hull.h
View File

@ -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();
}
};

9
include/bounce/collision/shapes/capsule.h
View File

@ -21,21 +21,20 @@
#include <bounce/common/math/vec3.h>
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]))
{

2
include/bounce/collision/shapes/hull.h
View File

@ -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;

2
include/bounce/collision/trees/dynamic_tree.h
View File

@ -22,7 +22,7 @@
#include <bounce/common/draw.h>
#include <bounce/common/template/stack.h>
#include <bounce/collision/shapes/aabb3.h>
#include <bounce/collision/distance.h>
#include <bounce/collision/collision.h>
#define NULL_NODE (-1)

2
include/bounce/collision/trees/static_tree.h
View File

@ -22,7 +22,7 @@
#include <bounce/common/draw.h>
#include <bounce/common/template/stack.h>
#include <bounce/collision/shapes/aabb3.h>
#include <bounce/collision/distance.h>
#include <bounce/collision/collision.h>
#define NULL_NODE_S (0xFFFFFFFF)

3
include/bounce/common/draw.h
View File

@ -54,7 +54,8 @@ public :
e_contactPointsFlag = 0x0008,
e_contactNormalsFlag = 0x0010,
e_contactTangentsFlag = 0x0020,
e_aabbsFlag = 0x0040,
e_contactAreasFlag = 0x0040,
e_aabbsFlag = 0x0080,
};
b3Draw()

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

@ -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

375
include/bounce/common/math/mat.h
View File

@ -20,69 +20,350 @@
#define B3_MAT_H
#include <bounce/common/math/math.h>
#include <bounce/common/math/mat22.h>
#include <bounce/common/math/mat33.h>
// A vector stored in column-major order.
template<u32 n>
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<n>& 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<u32 n>
inline b3Vec<n> operator-(const b3Vec<n>& v)
struct b3Mat24
{
b3Vec<n> 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<u32 n, u32 m>
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

15
include/bounce/common/math/mat22.h
View File

@ -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

122
include/bounce/common/math/quat.h
View File

@ -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

24
include/bounce/common/settings.h
View File

@ -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.

4
include/bounce/common/template/array.h
View File

@ -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;
}

28
include/bounce/dynamics/contacts/collide/clip.h
View File

@ -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

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

@ -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

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

@ -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;

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

@ -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

18
include/bounce/dynamics/contacts/contact_solver.h
View File

@ -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;

51
include/bounce/dynamics/contacts/manifold.h
View File

@ -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

6
include/bounce/dynamics/joints/cone_joint.h
View File

@ -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;

1
include/bounce/dynamics/joints/joint.h
View File

@ -20,6 +20,7 @@
#define B3_JOINT_H
#include <bounce/common/math/transform.h>
#include <bounce/common/math/mat.h>
#include <bounce/common/template/list.h>
#include <bounce/dynamics/time_step.h>

75
include/bounce/dynamics/joints/revolute_joint.h
View File

@ -20,15 +20,17 @@
#define B3_REVOLUTE_JOINT_H
#include <bounce/dynamics/joints/joint.h>
#include <bounce/common/math/mat.h>
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

14
include/bounce/dynamics/joints/weld_joint.h
View File

@ -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

1
include/bounce/dynamics/shapes/capsule_shape.h
View File

@ -21,7 +21,6 @@
#include <bounce/dynamics/shapes/shape.h>
// A capsule defined along the up-axis.
class b3CapsuleShape : public b3Shape
{
public:

2
include/bounce/dynamics/shapes/shape.h
View File

@ -21,7 +21,7 @@
#include <bounce/common/math/transform.h>
#include <bounce/common/template/list.h>
#include <bounce/collision/distance.h>
#include <bounce/collision/collision.h>
struct b3ContactEdge;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Some files were not shown because too many files have changed in this diff Show More