fix capsule drawing, add quaternion convenience functions

include/bounce/common/math/quat.h

@@ -77,18 +77,6 @@ struct b3Quat
 		w = _w;
 	}
 	
-	// Set this quaternion from an axis and an angle 
-	// of rotation about the axis.
-	void Set(const b3Vec3& axis, float32 angle)
-	{
-		float32 theta = 0.5f * angle;
-		float32 s = sin(theta);
-		x = s * axis.x;
-		y = s * axis.y;
-		z = s * axis.z;
-		w = cos(theta);
-	}
-
 	// Normalize this quaternion.
 	void Normalize()
 	{
@@ -102,6 +90,54 @@ struct b3Quat
 		}
 	}
 
+	// Set this quaternion from an axis and full angle 
+	// of rotation about the axis.
+	void Set(const b3Vec3& axis, float32 angle)
+	{
+		// half angle
+		float32 theta = 0.5f * angle;
+		
+		float32 sine = sin(theta);
+		x = sine * axis.x;
+		y = sine * axis.y;
+		z = sine * axis.z;
+
+		w = cos(theta);
+	}
+
+	// If this quaternion represents an orientation return 
+	// the axis of rotation.
+	b3Vec3 GetAxis() const
+	{
+		// sin^2 = 1 - cos^2
+		// sin = sqrt( sin^2 ) = ||v||
+		// axis = v / sin
+		b3Vec3 v(x, y, z);
+		float32 sine = b3Length(v);
+		if (sine > B3_EPSILON)
+		{
+			float32 s = 1.0f / sine;
+			return s * v;
+		}
+		return v;
+	}
+
+	// If this quaternion represents an orientation return 
+	// the full angle of rotation.
+	float32 GetAngle() const
+	{
+		// cosine check
+		if (w >= -1.0f && w <= 1.0f)
+		{
+			// half angle
+			float32 theta = acos(w);
+			// full angle
+			return 2.0f * theta;
+		}
+
+		return 0.0f;
+	}
+
 	float32 x, y, z, w;
 };
 

include/testbed/tests/capsule_stack.h

@@ -22,8 +22,15 @@
 class CapsuleStack : public Test
 {
 public:
+	enum
+	{
+		e_rowCount = 5,
+		e_columnCount = 5,
+		e_depthCount = 5
+	};
+
 	CapsuleStack()
-	{		
+	{
 		{
 			b3BodyDef bd;
 			bd.type = b3BodyType::e_staticBody;
@@ -35,34 +42,76 @@ public:
 
 			b3ShapeDef sd;
 			sd.shape = &hs;
-			sd.density = 0.0f;
-			sd.friction = 1.0f;
-			sd.restitution = 0.0f;
 			
-			b3Shape* shape = body->CreateShape(sd);
+			body->CreateShape(sd);
 		}
-				
-		for (float32 y = 2.5f; y < 20.0f; y += 2.5f)
+		
+		float32 height = 3.0f;
+		float32 radius = 1.0f;
+		float32 separation = 0.0f;
+
+		b3CapsuleShape capsule;
+		capsule.m_centers[0].Set(0.0f, -0.5f * height, 0.0f);
+		capsule.m_centers[1].Set(0.0f, 0.5f * height, 0.0f);
+		capsule.m_radius = radius;
+
+		b3ShapeDef sdef;
+		sdef.shape = &capsule;
+		sdef.density = 1.0f;
+		sdef.friction = 0.3f;
+
+		const u32 c = e_rowCount * e_columnCount * e_depthCount;
+		b3Body* bs[c];
+		u32 n = 0;
+
+		b3AABB3 aabb;
+		aabb.m_lower.Set(0.0f, 0.0f, 0.0f);
+		aabb.m_upper.Set(0.0f, 0.0f, 0.0f);
+
+		for (u32 i = 0; i < e_rowCount; ++i)
 		{
-			b3BodyDef bdef;
-			bdef.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.5f * B3_PI);
-			bdef.position.Set(0.0f, y, 0.0f);
-			bdef.linearVelocity.Set(0.0f, -1.0f, 0.0f);
-			bdef.type = b3BodyType::e_dynamicBody;
+			for (u32 j = 0; j < e_columnCount; ++j)
+			{
+				for (u32 k = 0; k < e_depthCount; ++k)
+				{
+					b3BodyDef bdef;
+					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.z = (2.0f + separation) * float32(k) * radius;
+					
+					bdef.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.5f * B3_PI);
+					
+					b3Body* body = m_world.CreateBody(bdef);
+					bs[n++] = body;
 
-			b3Body* body = m_world.CreateBody(bdef);
+					b3Shape* shape = body->CreateShape(sdef);
 
-			b3CapsuleShape capsule;
-			capsule.m_centers[0].Set(0.0f, -1.0f, 0.0f);
-			capsule.m_centers[1].Set(0.0f, 1.0f, 0.0f);
-			capsule.m_radius = 1.0f;
+					b3AABB3 aabb2;
+					shape->ComputeAABB(&aabb2, body->GetTransform());
 
-			b3ShapeDef sdef;
-			sdef.shape = &capsule;
-			sdef.density = 1.0f;
-			sdef.friction = 0.3f;
+					aabb = b3Combine(aabb, aabb2);
+				}
+			}
+		}
 
-			b3Shape* shape = body->CreateShape(sdef);
+		b3Vec3 center = aabb.Centroid();
+		
+		for (u32 i = 0; i < n; ++i)
+		{
+			b3Body* b = bs[i];
+			const b3Vec3& p = b->GetSweep().worldCenter;
+			const b3Quat& q = b->GetSweep().orientation;
+
+			// centralize
+			b3Vec3 position = p - center;
+			
+			// move up
+			position.y += 0.5f * aabb.Height() + radius;
+
+			// maintain orientation
+			b->SetTransform(position, q.GetAxis(), q.GetAngle());
 		}
 	}