minor changes

include/bounce/common/math/quat.h

@@ -35,12 +35,7 @@ struct b3Quat
 	// of rotation about the axis.
 	b3Quat(const b3Vec3& axis, float32 angle)
 	{
-		float32 theta = 0.5f * angle;
+		Set(axis, angle);
-		float32 s = sin(theta);
-		x = s * axis.x;
-		y = s * axis.y;
-		z = s * axis.z;
-		w = cos(theta);
 	}
 
 	// Add a quaternion to this quaternion.
@@ -77,17 +72,19 @@ struct b3Quat
 		w = _w;
 	}
 	
-	// Normalize this quaternion.
+	// Convert this quaternion to the unit quaternion. Return the length.
-	void Normalize()
+	float32 Normalize()
 	{
-		float32 s = b3Sqrt(x * x + y * y + z * z + w * w);
+		float32 length = b3Sqrt(x * x + y * y + z * z + w * w);
-		if (s != 0.0f)
+		if (length > B3_EPSILON)
 		{
-			x /= s;
+			float32 s = 1.0f / length;
-			y /= s;
+			x *= s;
-			z /= s;
+			y *= s;
-			w /= s;
+			z *= s;
+			w *= s;
 		}
+		return length;
 	}
 
 	// Set this quaternion from an axis and full angle 
@@ -105,37 +102,31 @@ struct b3Quat
 		w = cos(theta);
 	}
 
-	// If this quaternion represents an orientation return 
+	// If this quaternion represents an orientation output 
-	// the axis of rotation.
+	// the axis and angle of rotation about the axis.
-	b3Vec3 GetAxis() const
+	void GetAxisAngle(b3Vec3* axis, float32* angle) const
 	{
 		// sin^2 = 1 - cos^2
 		// sin = sqrt( sin^2 ) = ||v||
 		// axis = v / sin
 		b3Vec3 v(x, y, z);
 		float32 sine = b3Length(v);
+		axis->SetZero();
 		if (sine > B3_EPSILON)
 		{
 			float32 s = 1.0f / sine;
-			return s * v;
+			*axis = s * v;
-		}
-		return v;
 		}
 		
-	// If this quaternion represents an orientation return 
+		*angle = 0.0f;
-	// 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;
+			*angle = 2.0f * theta;
 		}
-
-		return 0.0f;
 	}
 
 	float32 x, y, z, w;
@@ -181,11 +172,11 @@ inline float32 b3Length(const b3Quat& q)
 	return b3Sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
 }
 
-// Normalize a quarternion.
+// Convert a quaternion to the unit quaternion.
 inline b3Quat b3Normalize(const b3Quat& q)
 {
 	float32 s = b3Length(q);
-	if (s != 0.0f)
+	if (s > B3_EPSILON)
 	{
 		s = 1.0f / s;
 		return s * q;
@@ -215,7 +206,7 @@ inline b3Vec3 b3Mul(const b3Quat& q, const b3Vec3& v)
 	return v + qs * t + b3Cross(qv, t);
 }
 
-// Convert a quaternion to a 3-by-3 rotation matrix.
+// Convert an orientation quaternion to a 3-by-3 rotation matrix.
 inline b3Mat33 b3ConvertQuatToRot(const b3Quat& q)
 {
 	float32 x = q.x, y = q.y, z = q.z, w = q.w;

include/bounce/common/math/vec3.h

@@ -67,11 +67,12 @@ struct b3Vec3
 	}
 	
 	// Scale this vector.
-	void operator/=(float32 s)
+	void operator/=(float32 a)
 	{
-		x /= s;
+		float32 s = 1.0f / a;
-		y /= s;
+		x *= s;
-		z /= s;
+		y *= s;
+		z *= s;
 	}
 
 	// Set this vector to the zero vector.
@@ -80,18 +81,6 @@ struct b3Vec3
 		x = y = z = 0.0f;
 	}
 
-	// Normalize this vector.
-	void Normalize()
-	{
-		float32 lenght = b3Sqrt(x * x + y * y + z * z);
-		if (lenght > B3_EPSILON)
-		{
-			x /= lenght;
-			y /= lenght;
-			z /= lenght;
-		}
-	}
-
 	// Set this vector from three coordinates.
 	void Set(float32 _x, float32 _y, float32 _z)
 	{
@@ -100,6 +89,20 @@ struct b3Vec3
 		z = _z;
 	}
 
+	// Convert this vector to the unit vector. Return the length.
+	float32 Normalize()
+	{
+		float32 length = b3Sqrt(x * x + y * y + z * z);
+		if (length > B3_EPSILON)
+		{
+			float32 s = 1.0f / length;
+			x *= s;
+			y *= s;
+			z *= s;
+		}
+		return length;
+	}
+
 	float32 x, y, z;
 };
 

include/bounce/dynamics/body.h

@@ -178,8 +178,14 @@ public:
 	// Get the rotational inertia of the body about the center of mass. Typically in kg/m^3.
 	const b3Mat33& GetInertia() const;
 
+	// Get the linear kinetic energy of the body in Joules (kilogram-meters squared per second squared).
+	float32 GetLinearEnergy() const;
+
+	// Get the angular kinetic energy of the body in Joules (kilogram-meters squared per second squared).
+	float32 GetAngularEnergy() const;
+
 	// Get the total kinetic energy of the body in Joules (kilogram-meters squared per second squared).
-	float32 GetKineticEnergy() const;
+	float32 GetEnergy() const;
 	
 	// Transform a vector to the local space of this body.
 	b3Vec3 GetLocalVector(const b3Vec3& vector) const;
@@ -494,16 +500,24 @@ inline const b3Mat33& b3Body::GetInertia() const
 	return m_I;
 }
 
-inline float32 b3Body::GetKineticEnergy() const
+inline float32 b3Body::GetLinearEnergy() const
 {
 	b3Vec3 P = m_mass * m_linearVelocity;
-	float32 linearEnergy = b3Dot(P, m_linearVelocity);
+	return b3Dot(P, m_linearVelocity);
+}
 
+inline float32 b3Body::GetAngularEnergy() const
+{
 	b3Mat33 I = b3RotateToFrame(m_I, m_xf.rotation);
 	b3Vec3 L = I * m_angularVelocity;
-	float32 angularEnergy = b3Dot(L, m_angularVelocity);
+	return b3Dot(L, m_angularVelocity);
+}
 
-	return 0.5f * (linearEnergy + angularEnergy);
+inline float32 b3Body::GetEnergy() const
+{
+	float32 e1 = GetLinearEnergy();
+	float32 e2 = GetAngularEnergy();
+	return 0.5f * (e1 + e2);
 }
 
 inline void b3Body::ApplyForce(const b3Vec3& force, const b3Vec3& point, bool wake) 

include/testbed/tests/capsule_stack.h

@@ -111,7 +111,10 @@ public:
 			position.y += 0.5f * aabb.Height() + radius;
 
 			// maintain orientation
-			b->SetTransform(position, q.GetAxis(), q.GetAngle());
+			b3Vec3 axis;
+			float32 angle;
+			q.GetAxisAngle(&axis, &angle);
+			b->SetTransform(position, axis, angle);
 		}
 	}