fix mouse joint, draw center of mass, cleanup

include/bounce/dynamics/island.h

@@ -29,27 +29,6 @@ struct b3Velocity;
 struct b3Position;
 struct b3Profile;
 
-struct b3IslandBody
-{
-	b3Body* b;
-	u32 id;
-	bool visited;
-};
-
-struct b3IslandJoint
-{
-	b3Joint* j;
-	u32 id;
-	bool visited;
-};
-
-struct b3IslandContact
-{
-	b3Contact* c;
-	u32 id;
-	bool visited;
-};
-
 class b3Island 
 {
 public :

include/bounce/dynamics/joints/mouse_joint.h

@@ -80,6 +80,7 @@ private:
 	u32 m_indexB;
 	float32 m_mB;
 	b3Mat33 m_iB;
+	b3Vec3 m_localCenterB;
 	b3Mat33 m_mass;
 	b3Vec3 m_rB;
 	b3Vec3 m_impulse;

include/testbed/tests/quadrics.h

@@ -103,7 +103,7 @@ public:
 			hull.m_hull = &m_cylinderHull;
 
 			b3ShapeDef sdef;
-			sdef.density = 0.2f;
+			sdef.density = 1.0f;
 			sdef.friction = 0.3f;
 			sdef.shape = &hull;
 

src/bounce/collision/shapes/hull.cpp

@@ -24,6 +24,18 @@ void b3Hull::Validate() const
 	{
 		Validate(faces + i);
 	}
+
+	for (u32 i = 0; i < edgeCount; i += 2)
+	{
+		const b3HalfEdge* edge = edges + i;
+		const b3HalfEdge* twin = edges + i + 1;
+
+		b3Vec3 A = vertices[edge->origin];
+		b3Vec3 B = vertices[twin->origin];
+
+		// Ensure each edge has non-zero length.
+		B3_ASSERT(b3DistanceSquared(A, B) > B3_EPSILON * B3_EPSILON);
+	}
 }
 
 void b3Hull::Validate(const b3Face* face) const 

src/bounce/dynamics/body.cpp

@@ -179,35 +179,44 @@ void b3Body::SynchronizeTransform()
 
 void b3Body::SynchronizeShapes() 
 {
-	b3Transform xf0;
-	xf0.position = m_sweep.worldCenter0;
-	xf0.rotation = b3ConvertQuatToRot(m_sweep.orientation0);
+	b3Transform xf1;
+	xf1.position = m_sweep.worldCenter0;
+	xf1.rotation = b3ConvertQuatToRot(m_sweep.orientation0);
 	
-	b3Transform xf1 = m_xf;
+	b3Transform xf2 = m_xf;
 	
-	b3Vec3 displacement = xf1.position - xf0.position;
+	b3Vec3 displacement = xf2.position - xf1.position;
 
-	// Update the AABBs of all shapes.
+	// Update all shape AABBs.
+	b3BroadPhase* broadPhase = &m_world->m_contactMan.m_broadPhase;
 	for (b3Shape* s = m_shapeList.m_head; s; s = s->m_next)
 	{
-		b3AABB3 aabb;
-		s->ComputeAABB(&aabb, xf1);		
-		m_world->m_contactMan.m_broadPhase.MoveProxy(s->m_broadPhaseID, aabb, displacement);
+		// Compute an AABB that encloses the swept shape AABB.
+		b3AABB3 aabb1, aabb2;
+		s->ComputeAABB(&aabb1, xf1);
+		s->ComputeAABB(&aabb2, xf2);
+		
+		b3AABB3 aabb = b3Combine(aabb1, aabb2);
+
+		broadPhase->MoveProxy(s->m_broadPhaseID, aabb, displacement);
 	}
 }
 
 void b3Body::ResetMass() 
 {
-	// Set mass and inertia tensor to zero.
 	m_mass = 0.0f;
 	m_invMass = 0.0f;
 	m_I.SetZero();
 	m_invI.SetZero();
 	m_worldInvI.SetZero();
+	m_sweep.localCenter.SetZero();
 
 	// Static and kinematic bodies have zero mass.
 	if (m_type == e_staticBody || m_type == e_kinematicBody)
 	{
+		m_sweep.worldCenter0 = m_xf.position;
+		m_sweep.worldCenter = m_xf.position;
+		m_sweep.orientation0 = m_sweep.orientation;
 		return;
 	}
 
@@ -238,7 +247,7 @@ void b3Body::ResetMass()
 	}
 	else 
 	{
-		// Dynamic bodies have positive mass.
+		// Force all dynamic bodies to have positive mass.
 		m_mass = 1.0f;
 		m_invMass = 1.0f;
 	}
@@ -298,15 +307,14 @@ void b3Body::ResetMass()
 		m_worldInvI.y.y = 0.0f;
 	}
 
-	// Update center of mass.
+	// Move center of mass.
+	b3Vec3 oldCenter = m_sweep.worldCenter;
 	m_sweep.localCenter = localCenter;
-	
-	b3Vec3 worldCenter0 = m_sweep.worldCenter;
-	m_sweep.worldCenter = b3Mul(m_xf, localCenter);
+	m_sweep.worldCenter = b3Mul(m_xf, m_sweep.localCenter);
 	m_sweep.worldCenter0 = m_sweep.worldCenter;
 
 	// Update center of mass velocity.
-	m_linearVelocity += b3Cross(m_angularVelocity, m_sweep.worldCenter - worldCenter0);
+	m_linearVelocity += b3Cross(m_angularVelocity, m_sweep.worldCenter - oldCenter);
 }
 
 bool b3Body::ShouldCollide(const b3Body* other) const

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

@@ -104,6 +104,7 @@ void b3RebuildFaceContact(b3Manifold& manifold,
 	b3FaceQuery query;
 	query.index = indexA;
 	
+	// @todo Use heuristic (relative orientation) to increase performance.
 	b3BuildFaceContact(manifold, xfA, hullA, xfB, hullB, query, flipNormal);
 }
 

src/bounce/dynamics/draw_world.cpp

@@ -58,7 +58,8 @@ void b3World::DebugDraw() const
 	{
 		for (b3Body* b = m_bodyList.m_head; b; b = b->m_next)
 		{
-			const b3Transform& xf = b->GetTransform();
+			b3Transform xf = b->m_xf;
+			xf.position = b->m_sweep.worldCenter;
 			m_debugDraw->DrawTransform(xf);
 		}
 	}

src/bounce/dynamics/joints/mouse_joint.cpp

@@ -36,19 +36,19 @@ void b3MouseJoint::InitializeConstraints(const b3SolverData* data)
 	m_indexB = m_bodyB->m_islandID;
 	m_mB = m_bodyB->m_invMass;
 	m_iB = m_bodyB->m_worldInvI;
+	m_localCenterB = m_bodyB->m_sweep.localCenter;
 
 	b3Vec3 xB = data->positions[m_indexB].x;
 	b3Quat qB = data->positions[m_indexB].q;
 
-	b3Vec3 worldAnchorB = b3Mul(qB, m_localAnchorB) + xB;
-	
-	m_C = worldAnchorB - m_worldTargetA;	
-	m_rB = worldAnchorB - xB;
-
+	// Compute the effective mass matrix.
+	m_rB = b3Mul(qB, m_localAnchorB - m_localCenterB);
 	b3Mat33 M = b3Diagonal(m_mB);
 	b3Mat33 RB = b3Skew(m_rB);
 	b3Mat33 RBT = b3Transpose(RB);
 	m_mass = M + RB * m_iB * RBT;
+	
+	m_C = xB + m_rB - m_worldTargetA;
 }
 
 void b3MouseJoint::WarmStart(const b3SolverData* data) 
@@ -64,19 +64,14 @@ void b3MouseJoint::SolveVelocityConstraints(const b3SolverData* data)
 
 	b3Vec3 Cdot = vB + b3Cross(wB, m_rB);
 
-	b3Vec3 impulse = m_mass.Solve(-(Cdot + data->invdt * B3_BAUMGARTE * m_C));
+	b3Vec3 impulse = m_mass.Solve(-(Cdot + B3_BAUMGARTE * data->invdt * m_C));
 	b3Vec3 oldImpulse = m_impulse;
 	m_impulse += impulse;
-
-	// Prevent large reaction impulses.
 	float32 maxImpulse = data->dt * m_maxForce;
-	float32 sqrImpulse = b3Dot(m_impulse, m_impulse);
-	if (sqrImpulse > maxImpulse * maxImpulse)
+	if (b3Dot(m_impulse, m_impulse) > maxImpulse * maxImpulse)
 	{
-		float32 ratio = maxImpulse / b3Sqrt(sqrImpulse);
-		m_impulse *= ratio;
+		m_impulse *= maxImpulse / b3Length(m_impulse);
 	}	
-	
 	impulse = m_impulse - oldImpulse;
 
 	vB += m_mB * impulse;

src/bounce/dynamics/shapes/hull_shape.cpp

@@ -135,10 +135,6 @@ void b3HullShape::ComputeMass(b3MassData* massData, float32 density) const
 	{
 		invVolume = 1.0f / volume;
 	}
-	else
-	{
-		invVolume = 0.0f;
-	}
 	
 	diag = invVolume * diag;
 	offDiag = invVolume * offDiag;

src/bounce/dynamics/world.cpp

@@ -319,11 +319,11 @@ void b3World::Solve(float32 dt, u32 velocityIterations, u32 positionIterations)
 			b->SynchronizeShapes();
 		}
 
-		// Notify the contacts the shapes may have moved.
+		// Notify the contacts the shapes may have been moved.
 		m_contactMan.SynchronizeShapes();
 
-		time.Update();
 		m_contactMan.FindNewContacts();
+		
 		time.Update();
 		m_profile.collide.broadphase = time.GetElapsedMilis();
 	}

src/testbed/framework/main.cpp

@@ -207,12 +207,6 @@ void CreateInterface()
 	ImGui_ImplGlfwGL3_Init(g_window, false);
 	ImGuiIO& io = ImGui::GetIO();
 	io.Fonts[0].AddFontDefault();
-
-	ImGuiStyle& style = ImGui::GetStyle();
-	style.FrameRounding = style.GrabRounding = style.ScrollbarRounding = 2.0f;
-	style.FramePadding = ImVec2(4, 2);
-	style.DisplayWindowPadding = ImVec2(0, 0);
-	style.DisplaySafeAreaPadding = ImVec2(0, 0);
 }
 
 void DestroyInterface()

src/testbed/framework/test.cpp

@@ -358,9 +358,9 @@ void Test::MouseMove(const Ray3& pw)
 {
 	if (m_mouseJoint)
 	{
-		float32 hitFraction = m_rayHit.fraction;
-		float32 w1 = 1.0f - hitFraction;
-		float32 w2 = hitFraction;
+		float32 t = m_rayHit.fraction;
+		float32 w1 = 1.0f - t;
+		float32 w2 = t;
 
 		b3Vec3 target = w1 * pw.Start() + w2 * pw.End();
 		m_mouseJoint->SetTarget(target);