rename function, remove indirection, fix bug

examples/testbed/framework/debug_draw.cpp

@@ -64,7 +64,7 @@ b3Mat44 Camera::BuildProjectionMatrix() const
 b3Transform Camera::BuildWorldTransform() const
 {
 	b3Transform xf;
-	xf.rotation = b3ConvertQuatToRot(m_q);
+	xf.rotation = b3ConvertQuatToMat(m_q);
 	xf.position = (m_zoom * xf.rotation.z) - m_center;
 	return xf;
 }
@@ -78,7 +78,7 @@ b3Mat44 Camera::BuildWorldMatrix() const
 b3Transform Camera::BuildViewTransform() const
 {
 	b3Transform xf;
-	xf.rotation = b3ConvertQuatToRot(m_q);
+	xf.rotation = b3ConvertQuatToMat(m_q);
 	xf.position = (m_zoom * xf.rotation.z) - m_center;
 	return b3Inverse(xf);
 }

examples/testbed/tests/capsule_and_hull_collision_1.h

@@ -25,14 +25,14 @@ public:
 	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_xfA.rotation = b3ConvertQuatToMat(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.55f * B3_PI));
 		
 		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;
 
 		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));
+		m_xfB.rotation = b3ConvertQuatToMat(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.0f * B3_PI));
 
 		b3Transform xf;
 		xf.SetIdentity();

examples/testbed/tests/capsule_and_hull_collision_2.h

@@ -25,14 +25,14 @@ 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_xfA.rotation = b3ConvertQuatToMat(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));
+		m_xfB.rotation = b3ConvertQuatToMat(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.0f * B3_PI));
 
 		b3Transform xf;
 		xf.SetIdentity();

examples/testbed/tests/collide_test.h

@@ -92,7 +92,7 @@ public:
 		if (key == GLFW_KEY_X)
 		{
 			b3Quat qx(b3Vec3(1.0f, 0.0f, 0.0f), 0.05f * B3_PI);
-			b3Mat33 xfx = b3ConvertQuatToRot(qx);
+			b3Mat33 xfx = b3ConvertQuatToMat(qx);
 
 			m_xfB.rotation = m_xfB.rotation * xfx;
 		}
@@ -100,7 +100,7 @@ public:
 		if (key == GLFW_KEY_Y)
 		{
 			b3Quat qy(b3Vec3(0.0f, 1.0f, 0.0f), 0.05f * B3_PI);
-			b3Mat33 xfy = b3ConvertQuatToRot(qy);
+			b3Mat33 xfy = b3ConvertQuatToMat(qy);
 
 			m_xfB.rotation = m_xfB.rotation * xfy;
 		}

examples/testbed/tests/distance_test.h

@@ -103,7 +103,7 @@ public:
 		if (key == GLFW_KEY_X)
 		{
 			b3Quat qx(b3Vec3(1.0f, 0.0f, 0.0f), 0.05f * B3_PI);
-			b3Mat33 xfx = b3ConvertQuatToRot(qx);
+			b3Mat33 xfx = b3ConvertQuatToMat(qx);
 
 			m_xfB.rotation = m_xfB.rotation * xfx;
 		}
@@ -111,7 +111,7 @@ public:
 		if (key == GLFW_KEY_Y)
 		{
 			b3Quat qy(b3Vec3(0.0f, 1.0f, 0.0f), 0.05f * B3_PI);
-			b3Mat33 xfy = b3ConvertQuatToRot(qy);
+			b3Mat33 xfy = b3ConvertQuatToMat(qy);
 
 			m_xfB.rotation = m_xfB.rotation * xfy;
 		}

examples/testbed/tests/shape_stack.h

@@ -26,6 +26,7 @@ public:
 	{
 		{
 			b3BodyDef bd;
+			bd.orientation.Set(b3Vec3(0.0f, 1.0f, 0.0f), 0.18f * B3_PI);
 			b3Body* ground = m_world.CreateBody(bd);
 
 			b3MeshShape ms;

include/bounce/common/geometry.h

@@ -100,33 +100,6 @@ inline void b3BarycentricCoordinates(float32 out[3],
 	out[2] = divisor;
 }
 
-// Convert a point Q from euclidean coordinates to barycentric coordinates (u, v, w) 
-// with respect to a triangle ABC.
-// The last output value is the divisor.
-inline void b3BarycentricCoordinates(float32 out[4],
-	const b3Vec3& A, const b3Vec3& B, const b3Vec3& C,
-	const b3Vec3& Q)
-{
-	b3Vec3 AB = B - A;
-	b3Vec3 AC = C - A;
-
-	b3Vec3 QA = A - Q;
-	b3Vec3 QB = B - Q;
-	b3Vec3 QC = C - Q;
-
-	b3Vec3 QB_x_QC = b3Cross(QB, QC);
-	b3Vec3 QC_x_QA = b3Cross(QC, QA);
-	b3Vec3 QA_x_QB = b3Cross(QA, QB);
-
-	b3Vec3 AB_x_AC = b3Cross(AB, AC);
-	float32 divisor = b3Dot(AB_x_AC, AB_x_AC);
-
-	out[0] = b3Dot(QB_x_QC, AB_x_AC);
-	out[1] = b3Dot(QC_x_QA, AB_x_AC);
-	out[2] = b3Dot(QA_x_QB, AB_x_AC);
-	out[3] = divisor;
-}
-
 // Project a point onto a segment AB.
 inline b3Vec3 b3ClosestPointOnSegment(const b3Vec3& P, const b3Vec3& A, const b3Vec3& B)
 {

include/bounce/common/math/quat.h

@@ -205,7 +205,7 @@ inline b3Quat b3Conjugate(const b3Quat& q)
 	return b3Quat(-q.x, -q.y, -q.z, q.w);
 }
 
-// Rotate a vector by an orientation quaternion.
+// Rotate a vector.
 inline b3Vec3 b3Mul(const b3Quat& q, const b3Vec3& v)
 {
 	b3Vec3 qv(q.x, q.y, q.z);
@@ -215,14 +215,14 @@ inline b3Vec3 b3Mul(const b3Quat& q, const b3Vec3& v)
 	return v + qs * t + b3Cross(qv, t);
 }
 
-// Inverse rotate a vector by an orientation quaternion.
+// Inverse rotate a vector.
 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.
+// Convert a 3-by-3 rotation matrix to an rotation quaternion.
-inline b3Quat b3ConvertRotToQuat(const b3Mat33& m)
+inline b3Quat b3ConvertMatToQuat(const b3Mat33& m)
 {
 	// Check the diagonal.
 	float32 trace = m[0][0] + m[1][1] + m[2][2];
@@ -286,8 +286,8 @@ inline b3Quat b3ConvertRotToQuat(const b3Mat33& m)
 	return result;
 }
 
-// Convert an orientation quaternion to a 3-by-3 rotation matrix.
+// Convert an rotation quaternion to a 3-by-3 rotation matrix.
-inline b3Mat33 b3ConvertQuatToRot(const b3Quat& q)
+inline b3Mat33 b3ConvertQuatToMat(const b3Quat& q)
 {
 	float32 x = q.x, y = q.y, z = q.z, w = q.w;
 	float32 x2 = x + x, y2 = y + y, z2 = z + z;
@@ -301,4 +301,4 @@ inline b3Mat33 b3ConvertQuatToRot(const b3Quat& q)
 		b3Vec3(         xz + wy,          yz - wx, 1.0f - (xx + yy)));
 }
 
-#endif
+#endif

include/bounce/common/math/transform.h

@@ -35,7 +35,7 @@ struct b3Transform
 	b3Transform(const b3Vec3& p, const b3Quat& q)
 	{
 		position = p;
-		rotation = b3ConvertQuatToRot(q);
+		rotation = b3ConvertQuatToMat(q);
 	}
 	
 	// Set this transform to the identity.
@@ -75,7 +75,7 @@ inline b3Transform b3Sweep::GetTransform(float32 t) const
 	q.Normalize();
 
 	b3Transform xf;
-	xf.rotation = b3ConvertQuatToRot(q);
+	xf.rotation = b3ConvertQuatToMat(q);
 	xf.position = c - b3Mul(q, localCenter);
 	return xf;
 }

include/bounce/dynamics/body.h

@@ -385,7 +385,7 @@ inline void b3Body::SetTransform(const b3Vec3& position, const b3Vec3& axis, flo
 	b3Quat q = b3Quat(axis, angle);
 	
 	m_xf.position = position;
-	m_xf.rotation = b3ConvertQuatToRot(q);
+	m_xf.rotation = b3ConvertQuatToMat(q);
 
 	m_sweep.worldCenter = b3Mul(m_xf, m_sweep.localCenter);
 	m_sweep.orientation = q;

src/bounce/dynamics/body.cpp

@@ -77,7 +77,7 @@ b3Body::b3Body(const b3BodyDef& def, b3World* world)
 	m_sweep.t0 = 0.0f;
 
 	m_xf.position = m_sweep.worldCenter;
-	m_xf.rotation = b3ConvertQuatToRot(m_sweep.orientation);
+	m_xf.rotation = b3ConvertQuatToMat(m_sweep.orientation);
 	
 	m_gravityScale = def.gravityScale;
 	m_userData = def.userData;

src/bounce/dynamics/contacts/collide/collide.cpp

@@ -184,4 +184,4 @@ void b3CollideShapeAndShape(b3Manifold& manifold,
 	
 	B3_ASSERT(CollideFunc);
 	CollideFunc(manifold, xfA, shapeA, xfB, shapeB, cache);
-}
+}

src/bounce/dynamics/contacts/contact_solver.cpp

@@ -176,11 +176,11 @@ void b3ContactSolver::InitializeConstraints()
 		b3Vec3 wB = m_velocities[indexB].w;
 
 		b3Transform xfA;
-		xfA.rotation = b3ConvertQuatToRot(qA);
+		xfA.rotation = b3ConvertQuatToMat(qA);
 		xfA.position = xA - b3Mul(xfA.rotation, localCenterA);
 		
 		b3Transform xfB;
-		xfB.rotation = b3ConvertQuatToRot(qB);
+		xfB.rotation = b3ConvertQuatToMat(qB);
 		xfB.position = xB - b3Mul(xfB.rotation, localCenterB);
 
 		for (u32 j = 0; j < manifoldCount; ++j)
@@ -532,11 +532,11 @@ bool b3ContactSolver::SolvePositionConstraints()
 				b3PositionConstraintPoint* pcp = pcm->points + k;
 
 				b3Transform xfA;
-				xfA.rotation = b3ConvertQuatToRot(qA);
+				xfA.rotation = b3ConvertQuatToMat(qA);
 				xfA.position = cA - b3Mul(xfA.rotation, localCenterA);
 
 				b3Transform xfB;
-				xfB.rotation = b3ConvertQuatToRot(qB);
+				xfB.rotation = b3ConvertQuatToMat(qB);
 				xfB.position = cB - b3Mul(xfB.rotation, localCenterB);
 
 				b3ContactPositionSolverPoint cpcp;

src/bounce/dynamics/contacts/mesh_contact.cpp

@@ -72,7 +72,7 @@ void b3MeshContact::SynchronizeShapes()
 	b3Sweep* sweepA = &bodyA->m_sweep;
 	b3Transform xfA0;
 	xfA0.position = sweepA->worldCenter0;
-	xfA0.rotation = b3ConvertQuatToRot(sweepA->orientation0);
+	xfA0.rotation = b3ConvertQuatToMat(sweepA->orientation0);
 		
 	// Calculate the displacement of body A using its position at the last 
 	// time step and the current position.

src/bounce/dynamics/joints/revolute_joint.cpp

@@ -180,7 +180,7 @@ void b3RevoluteJointDef::Initialize(b3Body* bA, b3Body* bB,
 	rotation.y = b3Perp(axis);
 	rotation.x = b3Cross(rotation.y, axis);
 
-	b3Quat q = b3ConvertRotToQuat(rotation);
+	b3Quat q = b3ConvertMatToQuat(rotation);
 	float32 len = q.Normalize();
 	B3_ASSERT(len > B3_EPSILON);
 

src/bounce/dynamics/shapes/mesh_shape.cpp

@@ -62,7 +62,15 @@ void b3MeshShape::ComputeAABB(b3AABB3* output, const b3Transform& xf) const
 
 void b3MeshShape::ComputeAABB(b3AABB3* output, const b3Transform& xf, u32 index) const
 {
-	*output = m_mesh->GetTriangleAABB(index);
+	B3_ASSERT(index < m_mesh->triangleCount);
+	const b3Triangle* triangle = m_mesh->triangles + index;
+	b3Vec3 v1 = b3Mul(xf, m_mesh->vertices[triangle->v1]);
+	b3Vec3 v2 = b3Mul(xf, m_mesh->vertices[triangle->v2]);
+	b3Vec3 v3 = b3Mul(xf, m_mesh->vertices[triangle->v3]);
+
+	output->m_lower = b3Min(b3Min(v1, v2), v3);
+	output->m_upper = b3Max(b3Max(v1, v2), v3);
+	output->Extend(m_radius);
 }
 
 bool b3MeshShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const
@@ -98,7 +106,7 @@ bool b3MeshShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input,
 
 	float32 t = numerator / denominator;
 		
-	// Is the intersection point on the segment?
+	// Is the intersection not on the segment?
 	if (t < 0.0f || input.maxFraction < t)
 	{
 		return false;
@@ -106,11 +114,32 @@ bool b3MeshShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input,
 
 	b3Vec3 q = p1 + t * d;
 
-	float32 w[4];
+	// Barycentric coordinates for q
-	b3BarycentricCoordinates(w, v1, v2, v3, q);
+	b3Vec3 Q = q;
+	b3Vec3 A = v1;
+	b3Vec3 B = v2;
+	b3Vec3 C = v3;
 
-	// Is the intersection point on the triangle?
+	b3Vec3 AB = B - A;
-	if (w[0] > 0.0f && w[1] > 0.0f && w[2] > 0.0f)
+	b3Vec3 AC = C - A;
+	
+	b3Vec3 QA = A - Q;
+	b3Vec3 QB = B - Q;
+	b3Vec3 QC = C - Q;
+
+	b3Vec3 QB_x_QC = b3Cross(QB, QC);
+	b3Vec3 QC_x_QA = b3Cross(QC, QA);
+	b3Vec3 QA_x_QB = b3Cross(QA, QB);
+
+	b3Vec3 AB_x_AC = b3Cross(AB, AC);
+	float32 den = b3Dot(AB_x_AC, AB_x_AC);
+
+	float32 u = b3Dot(QB_x_QC, AB_x_AC);
+	float32 v = b3Dot(QC_x_QA, AB_x_AC);
+	float32 w = b3Dot(QA_x_QB, AB_x_AC);
+
+	// Is the intersection on the triangle?
+	if (u > 0.0f && v > 0.0f && w > 0.0f)
 	{
 		output->fraction = t;
 		
@@ -132,8 +161,10 @@ bool b3MeshShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input,
 
 struct b3MeshRayCastCallback
 {
-	float32 Report(const b3RayCastInput& input, u32 proxyId)
+	float32 Report(const b3RayCastInput& subInput, u32 proxyId)
 	{
+		B3_NOT_USED(subInput);
+
 		u32 childIndex = mesh->m_mesh->tree.GetUserData(proxyId);
 		
 		b3RayCastOutput childOutput;
@@ -150,6 +181,7 @@ struct b3MeshRayCastCallback
 		return 1.0f;
 	}
 
+	b3RayCastInput input;
 	const b3MeshShape* mesh;
 	b3Transform xf;
 	
@@ -160,12 +192,17 @@ struct b3MeshRayCastCallback
 bool b3MeshShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const 
 {
 	b3MeshRayCastCallback callback;
+	callback.input = input;
 	callback.mesh = this;
 	callback.xf = xf;
 	callback.hit = false;
 	callback.output.fraction = B3_MAX_FLOAT;
 	
-	m_mesh->tree.RayCast(&callback, input);
+	b3RayCastInput subInput;
+	subInput.p1 = b3MulT(xf, input.p1);
+	subInput.p2 = b3MulT(xf, input.p2);
+	subInput.maxFraction = input.maxFraction;
+	m_mesh->tree.RayCast(&callback, subInput);
 
 	output->fraction = callback.output.fraction;
 	output->normal = callback.output.normal;