physics dev (body, math32)

math32/matrix3.go

@@ -35,6 +35,17 @@ func (m *Matrix3) Set(n11, n12, n13, n21, n22, n23, n31, n32, n33 float32) *Matr
 	return m
 }
 
+// SetFromMatrix4 sets the matrix elements based on a Matrix4.
+func (m *Matrix3) SetFromMatrix4(src *Matrix4) *Matrix3 {
+
+	m.Set(
+		src[0], src[4], src[8],
+		src[1], src[5], src[9],
+		src[2], src[6], src[10],
+	)
+	return m
+}
+
 // Identity sets this matrix as the identity matrix.
 // Returns the pointer to this updated matrix.
 func (m *Matrix3) Identity() *Matrix3 {
@@ -216,26 +227,42 @@ func (m *Matrix3) Determinant() float32 {
 // GetInverse sets this matrix to the inverse of the src matrix.
 // If the src matrix cannot be inverted returns error and
 // sets this matrix to the identity matrix.
-func (m *Matrix3) GetInverse(src *Matrix4) error {
+func (m *Matrix3) GetInverse(src *Matrix3) error {
+
+	n11 := src[0]
+	n21 := src[1]
+	n31 := src[2]
+	n12 := src[3]
+	n22 := src[4]
+	n32 := src[5]
+	n13 := src[6]
+	n23 := src[7]
+	n33 := src[8]
 
-	m[0] = src[10]*src[5] - src[6]*src[9]
-	m[1] = -src[10]*src[1] + src[2]*src[9]
-	m[2] = src[6]*src[1] - src[2]*src[5]
-	m[3] = -src[10]*src[4] + src[6]*src[8]
-	m[4] = src[10]*src[0] - src[2]*src[8]
-	m[5] = -src[6]*src[0] + src[2]*src[4]
-	m[6] = src[9]*src[4] - src[5]*src[8]
-	m[7] = -src[9]*src[0] + src[1]*src[8]
-	m[8] = src[5]*src[0] - src[1]*src[4]
+	t11 := n33*n22 - n32*n23
+	t12 := n32*n13 - n33*n12
+	t13 := n23*n12 - n22*n13
 
-	det := src[0]*m[0] + src[1]*m[3] + src[2]*m[6]
+	det := n11*t11 + n21*t12 + n31*t13
 
 	// no inverse
 	if det == 0 {
 		m.Identity()
-		return errors.New("Cannot inverse matrix")
+		return errors.New("cannot invert matrix")
 	}
-	m.MultiplyScalar(1.0 / det)
+
+	detInv := 1 / det
+
+	m[0] = t11 * detInv
+	m[1] = ( n31 * n23 - n33 * n21 ) * detInv
+	m[2] = ( n32 * n21 - n31 * n22 ) * detInv
+	m[3] = t12 * detInv
+	m[4] = ( n33 * n11 - n31 * n13 ) * detInv
+	m[5] = ( n31 * n12 - n32 * n11 ) * detInv
+	m[6] = t13 * detInv
+	m[7] = ( n21 * n13 - n23 * n11 ) * detInv
+	m[8] = ( n22 * n11 - n21 * n12 ) * detInv
+
 	return nil
 }
 
@@ -261,7 +288,8 @@ func (m *Matrix3) Transpose() *Matrix3 {
 // If the src matrix cannot be inverted returns error.
 func (m *Matrix3) GetNormalMatrix(src *Matrix4) error {
 
-	err := m.GetInverse(src)
+	m.SetFromMatrix4(src)
+	err := m.GetInverse(m)
 	m.Transpose()
 	return err
 }

math32/matrix4.go

@@ -450,30 +450,40 @@ func (m *Matrix4) GetInverse(src *Matrix4) error {
 	n43 := src[11]
 	n44 := src[15]
 
-	m[0] = n23*n34*n42 - n24*n33*n42 + n24*n32*n43 - n22*n34*n43 - n23*n32*n44 + n22*n33*n44
-	m[4] = n14*n33*n42 - n13*n34*n42 - n14*n32*n43 + n12*n34*n43 + n13*n32*n44 - n12*n33*n44
-	m[8] = n13*n24*n42 - n14*n23*n42 + n14*n22*n43 - n12*n24*n43 - n13*n22*n44 + n12*n23*n44
-	m[12] = n14*n23*n32 - n13*n24*n32 - n14*n22*n33 + n12*n24*n33 + n13*n22*n34 - n12*n23*n34
-	m[1] = n24*n33*n41 - n23*n34*n41 - n24*n31*n43 + n21*n34*n43 + n23*n31*n44 - n21*n33*n44
-	m[5] = n13*n34*n41 - n14*n33*n41 + n14*n31*n43 - n11*n34*n43 - n13*n31*n44 + n11*n33*n44
-	m[9] = n14*n23*n41 - n13*n24*n41 - n14*n21*n43 + n11*n24*n43 + n13*n21*n44 - n11*n23*n44
-	m[13] = n13*n24*n31 - n14*n23*n31 + n14*n21*n33 - n11*n24*n33 - n13*n21*n34 + n11*n23*n34
-	m[2] = n22*n34*n41 - n24*n32*n41 + n24*n31*n42 - n21*n34*n42 - n22*n31*n44 + n21*n32*n44
-	m[6] = n14*n32*n41 - n12*n34*n41 - n14*n31*n42 + n11*n34*n42 + n12*n31*n44 - n11*n32*n44
-	m[10] = n12*n24*n41 - n14*n22*n41 + n14*n21*n42 - n11*n24*n42 - n12*n21*n44 + n11*n22*n44
-	m[14] = n14*n22*n31 - n12*n24*n31 - n14*n21*n32 + n11*n24*n32 + n12*n21*n34 - n11*n22*n34
-	m[3] = n23*n32*n41 - n22*n33*n41 - n23*n31*n42 + n21*n33*n42 + n22*n31*n43 - n21*n32*n43
-	m[7] = n12*n33*n41 - n13*n32*n41 + n13*n31*n42 - n11*n33*n42 - n12*n31*n43 + n11*n32*n43
-	m[11] = n13*n22*n41 - n12*n23*n41 - n13*n21*n42 + n11*n23*n42 + n12*n21*n43 - n11*n22*n43
-	m[15] = n12*n23*n31 - n13*n22*n31 + n13*n21*n32 - n11*n23*n32 - n12*n21*n33 + n11*n22*n33
-
-	det := n11*m[0] + n21*m[4] + n31*m[8] + n41*m[12]
+	t11 := n23*n34*n42 - n24*n33*n42 + n24*n32*n43 - n22*n34*n43 - n23*n32*n44 + n22*n33*n44
+	t12 := n14*n33*n42 - n13*n34*n42 - n14*n32*n43 + n12*n34*n43 + n13*n32*n44 - n12*n33*n44
+	t13 := n13*n24*n42 - n14*n23*n42 + n14*n22*n43 - n12*n24*n43 - n13*n22*n44 + n12*n23*n44
+	t14 := n14*n23*n32 - n13*n24*n32 - n14*n22*n33 + n12*n24*n33 + n13*n22*n34 - n12*n23*n34
+
+	det := n11 * t11 + n21 * t12 + n31 * t13 + n41 * t14
 
 	if det == 0 {
 		m.Identity()
 		return errors.New("Cannot inverse matrix")
 	}
-	m.MultiplyScalar(1.0 / det)
+
+	detInv := 1 / det
+
+	m[0] = t11 * detInv
+	m[1] = ( n24*n33*n41 - n23*n34*n41 - n24*n31*n43 + n21*n34*n43 + n23*n31*n44 - n21*n33*n44 ) * detInv
+	m[2] = ( n22*n34*n41 - n24*n32*n41 + n24*n31*n42 - n21*n34*n42 - n22*n31*n44 + n21*n32*n44 ) * detInv
+	m[3] = ( n23*n32*n41 - n22*n33*n41 - n23*n31*n42 + n21*n33*n42 + n22*n31*n43 - n21*n32*n43 ) * detInv
+
+	m[4] = t12 * detInv
+	m[5] = ( n13*n34*n41 - n14*n33*n41 + n14*n31*n43 - n11*n34*n43 - n13*n31*n44 + n11*n33*n44 ) * detInv
+	m[6] = ( n14*n32*n41 - n12*n34*n41 - n14*n31*n42 + n11*n34*n42 + n12*n31*n44 - n11*n32*n44 ) * detInv
+	m[7] = ( n12*n33*n41 - n13*n32*n41 + n13*n31*n42 - n11*n33*n42 - n12*n31*n43 + n11*n32*n43 ) * detInv
+
+	m[8] = t13 * detInv
+	m[9] = ( n14*n23*n41 - n13*n24*n41 - n14*n21*n43 + n11*n24*n43 + n13*n21*n44 - n11*n23*n44 ) * detInv
+	m[10] = ( n12*n24*n41 - n14*n22*n41 + n14*n21*n42 - n11*n24*n42 - n12*n21*n44 + n11*n22*n44 ) * detInv
+	m[11] = ( n13*n22*n41 - n12*n23*n41 - n13*n21*n42 + n11*n23*n42 + n12*n21*n43 - n11*n22*n43 ) * detInv
+
+	m[12] = t14 * detInv
+	m[13] = ( n13*n24*n31 - n14*n23*n31 + n14*n21*n33 - n11*n24*n33 - n13*n21*n34 + n11*n23*n34 ) * detInv
+	m[14] = ( n14*n22*n31 - n12*n24*n31 - n14*n21*n32 + n11*n24*n32 + n12*n21*n34 - n11*n22*n34 ) * detInv
+	m[15] = ( n12*n23*n31 - n13*n22*n31 + n13*n21*n32 - n11*n23*n32 - n12*n21*n33 + n11*n22*n33 ) * detInv
+
 	return nil
 }
 

math32/quaternion.go

@@ -405,3 +405,9 @@ func (q *Quaternion) ToArray(array []float32, offset int) []float32 {
 
 	return array
 }
+
+// Clone returns a copy of this quaternion
+func (q *Quaternion) Clone() *Quaternion {
+
+	return NewQuaternion(q.X, q.Y, q.Z, q.W)
+}

physics/body.go

@@ -5,82 +5,80 @@
 package physics
 
 import (
-	"github.com/g3n/engine/math32"
 	"github.com/g3n/engine/graphic"
+	"github.com/g3n/engine/math32"
 )
 
 // Body represents a physics-driven body.
 type Body struct {
-	//core.INode // TODO instead of embedding INode - embed Node and have a method SetNode ?
-
-	*graphic.Graphic
-
-	mass            float32        // Total mass
-	invMass         float32
-	invMassSolve    float32
-
-	velocity        *math32.Vector3 // Linear velocity (World space velocity of the body.)
-	initVelocity    *math32.Vector3 // Initial linear velocity (World space velocity of the body.)
-	vLambda         *math32.Vector3
-
-	angularMass     *math32.Matrix3 // Angular mass i.e. moment of inertia
-
-	inertia              *math32.Vector3
-	invInertia           *math32.Vector3
-	invInertiaSolve      *math32.Vector3
-	invInertiaWorld      *math32.Matrix3
-	invInertiaWorldSolve *math32.Matrix3
+	*graphic.Graphic // TODO future - embed core.Node instead and calculate properties recursively
+	simulation           *Simulation // Reference to the simulation the body is living in\
+	material             *Material   // Physics material specifying friction and restitution
+	index int
 
-	fixedRotation    bool  // Set to true if you don't want the body to rotate. Make sure to run .updateMassProperties() after changing this.
+	// Mass properties
+	mass       float32 // Total mass
+	invMass    float32
+	invMassEff float32 // Effective inverse mass
+
+	// Rotational inertia and related properties
+	rotInertia            *math32.Matrix3 // Angular mass i.e. moment of inertia in local coordinates
+	invRotInertia         *math32.Matrix3 // Inverse rotational inertia in local coordinates
+	invRotInertiaEff      *math32.Matrix3 // Effective inverse rotational inertia in local coordinates
+	invRotInertiaWorld    *math32.Matrix3 // Inverse rotational inertia in world coordinates
+	invRotInertiaWorldEff *math32.Matrix3 // Effective rotational inertia in world coordinates
+	fixedRotation         bool            // Set to true if you don't want the body to rotate. Make sure to run .updateMassProperties() after changing this.
+
+	// Position
+	position       *math32.Vector3 // World position of the center of gravity (World space position of the body.)
+	initPosition   *math32.Vector3 // Initial position of the body.
+	prevPosition   *math32.Vector3 // Previous position
+	interpPosition *math32.Vector3 // Interpolated position of the body.
+
+	// Rotation
+	quaternion       *math32.Quaternion // World space orientation of the body.
+	initQuaternion   *math32.Quaternion
+	prevQuaternion   *math32.Quaternion
+	interpQuaternion *math32.Quaternion // Interpolated orientation of the body.
 
+	// Linear and angular velocities
+	velocity            *math32.Vector3 // Linear velocity (World space velocity of the body.)
+	initVelocity        *math32.Vector3 // Initial linear velocity (World space velocity of the body.)
 	angularVelocity     *math32.Vector3 // Angular velocity of the body, in world space. Think of the angular velocity as a vector, which the body rotates around. The length of this vector determines how fast (in radians per second) the body rotates.
 	initAngularVelocity *math32.Vector3
-	wLambda             *math32.Vector3
-
 
-	force           *math32.Vector3 // Linear force on the body in world space.
-	torque          *math32.Vector3 // World space rotational force on the body, around center of mass.
+	// Force and torque
+	force  *math32.Vector3 // Linear force on the body in world space.
+	torque *math32.Vector3 // World space rotational force on the body, around center of mass.
 
-	position        *math32.Vector3 // World position of the center of gravity (World space position of the body.)
-	prevPosition    *math32.Vector3 // Previous position
-	interpPosition  *math32.Vector3 // Interpolated position of the body.
-	initPosition    *math32.Vector3 // Initial position of the body.
-
-	quaternion       *math32.Quaternion // World space orientation of the body.
-	initQuaternion   *math32.Quaternion
-	prevQuaternion   *math32.Quaternion
-	interpQuaternion *math32.Quaternion // Interpolated orientation of the body.
+	// Damping and factors
+	linearDamping  float32
+	angularDamping float32
+	linearFactor   *math32.Vector3 // Use this property to limit the motion along any world axis. (1,1,1) will allow motion along all axes while (0,0,0) allows none.
+	angularFactor  *math32.Vector3 // Use this property to limit the rotational motion along any world axis. (1,1,1) will allow rotation along all axes while (0,0,0) allows none.
 
+	// Body type and sleep settings
 	bodyType        BodyType
 	sleepState      BodySleepState // Current sleep state.
 	allowSleep      bool           // If true, the body will automatically fall to sleep.
 	sleepSpeedLimit float32        // If the speed (the norm of the velocity) is smaller than this value, the body is considered sleepy.
 	sleepTimeLimit  float32        // If the body has been sleepy for this sleepTimeLimit seconds, it is considered sleeping.
 	timeLastSleepy  float32
-
-	simulation             *Simulation // Reference to the simulation the body is living in\
-	collisionFilterGroup   int
-	collisionFilterMask    int
-	collisionResponse      bool // Whether to produce contact forces when in contact with other bodies. Note that contacts will be generated, but they will be disabled.
-
 	wakeUpAfterNarrowphase bool
-	material               *Material
 
-	linearDamping          float32
-	angularDamping         float32
+	// Collision settings
+	collisionFilterGroup int
+	collisionFilterMask  int
+	collisionResponse    bool // Whether to produce contact forces when in contact with other bodies. Note that contacts will be generated, but they will be disabled.
 
-	linearFactor           *math32.Vector3 // Use this property to limit the motion along any world axis. (1,1,1) will allow motion along all axes while (0,0,0) allows none.
-	angularFactor          *math32.Vector3 // Use this property to limit the rotational motion along any world axis. (1,1,1) will allow rotation along all axes while (0,0,0) allows none.
-
-	//aabb            *AABB   // World space bounding box of the body and its shapes.
-	aabbNeedsUpdate   bool    // Indicates if the AABB needs to be updated before use.
-	boundingRadius    float32 // Total bounding radius of the Body including its shapes, relative to body.position.
+	aabb            *math32.Box3 // World space bounding box of the body and its shapes.
+	aabbNeedsUpdate bool         // Indicates if the AABB needs to be updated before use.
+	boundingRadius  float32      // Total bounding radius of the body (TODO including its shapes, relative to body.position.)
 
+	// TODO future (for now a body is a single graphic with a single geometry)
 	// shapes          []*Shape
 	// shapeOffsets    []float32 // Position of each Shape in the body, given in local Body space.
 	// shapeOrientations [] ?
-
-	index int
 }
 
 // BodyType specifies how the body is affected during the simulation.
@@ -91,7 +89,7 @@ const (
 	// Static bodies can be moved manually by setting the position of the body.
 	// The velocity of a static body is always zero.
 	// Static bodies do not collide with other static or kinematic bodies.
-	Static       = BodyType(iota)
+	Static = BodyType(iota)
 
 	// A kinematic body moves under simulation according to its velocity.
 	// They do not respond to forces.
@@ -124,7 +122,6 @@ const (
 	CollideEvent = "physics.CollideEvent" // Dispatched after two bodies collide. This event is dispatched on each of the two bodies involved in the collision.
 )
 
-
 // NewBody creates and returns a pointer to a new RigidBody.
 func NewBody(igraphic graphic.IGraphic) *Body {
 
@@ -134,41 +131,48 @@ func NewBody(igraphic graphic.IGraphic) *Body {
 	// TODO mass setter/getter
 	b.mass = 1 // cannon.js default is 0
 	if b.mass > 0 {
-		b.invMass = 1.0/b.mass
+		b.invMass = 1.0 / b.mass
 	} else {
 		b.invMass = 0
 	}
 	b.bodyType = Dynamic // TODO auto set to Static if mass == 0
 
-	b.collisionFilterGroup = 1
-	b.collisionFilterMask = -1
-
-	pos := igraphic.GetNode().Position()
-	b.position 			= math32.NewVector3(0,0,0).Copy(&pos)
-	b.prevPosition 		= math32.NewVector3(0,0,0).Copy(&pos)
-	b.interpPosition 	= math32.NewVector3(0,0,0).Copy(&pos)
-	b.initPosition 		= math32.NewVector3(0,0,0).Copy(&pos)
-
-	quat := igraphic.GetNode().Quaternion()
-	b.quaternion 		= math32.NewQuaternion(0,0,0,1).Copy(&quat)
-	b.prevQuaternion 	= math32.NewQuaternion(0,0,0,1).Copy(&quat)
-	b.interpQuaternion 	= math32.NewQuaternion(0,0,0,1).Copy(&quat)
-	b.initQuaternion 	= math32.NewQuaternion(0,0,0,1).Copy(&quat)
-
-	b.velocity = math32.NewVector3(0,0,0) // TODO copy options.velocity
-	b.initVelocity = math32.NewVector3(0,0,0) // don't copy
-
-	b.angularVelocity = math32.NewVector3(0,0,0)
-	b.initAngularVelocity = math32.NewVector3(0,0,0)
-
-	b.vLambda = math32.NewVector3(0,0,0)
-	b.wLambda = math32.NewVector3(0,0,0)
-
+	// Rotational inertia and related properties
+	b.rotInertia = math32.NewMatrix3()
+	b.invRotInertia = math32.NewMatrix3()
+	b.invRotInertiaEff = math32.NewMatrix3()
+	b.invRotInertiaWorld = math32.NewMatrix3()
+	b.invRotInertiaWorldEff = math32.NewMatrix3()
+
+	// Position
+	pos := b.GetNode().Position()
+	b.position = pos.Clone()
+	b.prevPosition = pos.Clone()
+	b.interpPosition = pos.Clone()
+	b.initPosition = pos.Clone()
+
+	// Rotation
+	quat := b.GetNode().Quaternion()
+	b.quaternion = quat.Clone()
+	b.prevQuaternion = quat.Clone()
+	b.interpQuaternion = quat.Clone()
+	b.initQuaternion = quat.Clone()
+
+	// Linear and angular velocities
+	b.velocity = math32.NewVec3()
+	b.initVelocity = math32.NewVec3()
+	b.angularVelocity = math32.NewVec3()
+	b.initAngularVelocity = math32.NewVec3()
+
+	// Force and torque
+	b.force = math32.NewVec3()
+	b.torque = math32.NewVec3()
+
+	// Damping and factors
 	b.linearDamping = 0.01
 	b.angularDamping = 0.01
-
-	b.linearFactor = math32.NewVector3(1,1,1)
-	b.angularFactor = math32.NewVector3(1,1,1)
+	b.linearFactor = math32.NewVector3(1, 1, 1)
+	b.angularFactor = math32.NewVector3(1, 1, 1)
 
 	b.allowSleep = true
 	b.sleepState = Awake
@@ -176,8 +180,8 @@ func NewBody(igraphic graphic.IGraphic) *Body {
 	b.sleepTimeLimit = 1
 	b.timeLastSleepy = 0
 
-	b.force = math32.NewVector3(0,0,0)
-	b.torque = math32.NewVector3(0,0,0)
+	b.collisionFilterGroup = 1
+	b.collisionFilterMask = -1
 
 	b.wakeUpAfterNarrowphase = false
 
@@ -186,19 +190,29 @@ func NewBody(igraphic graphic.IGraphic) *Body {
 	return b
 }
 
+func (b *Body) InvMassEff() float32 {
+
+	return b.invMassEff
+}
+
+func (b *Body) InvRotInertiaWorldEff() *math32.Matrix3 {
+
+	return b.invRotInertiaWorldEff
+}
+
 func (b *Body) Position() math32.Vector3 {
 
 	return *b.position
 }
 
-func (b *Body) SetVelocity(vel *math32.Vector3) {
+func (b *Body) Quaternion() *math32.Quaternion {
 
-	b.velocity = vel
+	return b.quaternion
 }
 
-func (b *Body) AddToVelocity(delta *math32.Vector3) {
+func (b *Body) SetVelocity(vel *math32.Vector3) {
 
-	b.velocity.Add(delta)
+	b.velocity = vel
 }
 
 func (b *Body) Velocity() math32.Vector3 {
@@ -211,11 +225,6 @@ func (b *Body) SetAngularVelocity(vel *math32.Vector3) {
 	b.angularVelocity = vel
 }
 
-func (b *Body) AddToAngularVelocity(delta *math32.Vector3) {
-
-	b.angularVelocity.Add(delta)
-}
-
 func (b *Body) AngularVelocity() math32.Vector3 {
 
 	return *b.angularVelocity
@@ -231,49 +240,14 @@ func (b *Body) Torque() math32.Vector3 {
 	return *b.torque
 }
 
-func (b *Body) SetVlambda(vLambda *math32.Vector3) {
-
-	b.vLambda = vLambda
-}
-
-func (b *Body) AddToVlambda(delta *math32.Vector3) {
-
-	b.vLambda.Add(delta)
-}
-
-func (b *Body) Vlambda() math32.Vector3 {
-
-	return *b.vLambda
-}
-
-func (b *Body) SetWlambda(wLambda *math32.Vector3) {
-
-	b.wLambda = wLambda
-}
-
-func (b *Body) AddToWlambda(delta *math32.Vector3) {
-
-	b.wLambda.Add(delta)
-}
-
-func (b *Body) Wlambda() math32.Vector3 {
-
-	return *b.wLambda
-}
-
-func (b *Body) InvMassSolve() float32 {
+func (b *Body) LinearDamping() float32 {
 
-	return b.invMassSolve
+	return b.linearDamping
 }
 
-func (b *Body) InvInertiaWorldSolve() *math32.Matrix3 {
+func (b *Body) AngularDamping() float32 {
 
-	return b.invInertiaWorldSolve
-}
-
-func (b *Body) Quaternion() *math32.Quaternion {
-
-	return b.quaternion
+	return b.angularDamping
 }
 
 func (b *Body) LinearFactor() *math32.Vector3 {
@@ -301,8 +275,8 @@ func (b *Body) WakeUp() {
 func (b *Body) Sleep() {
 
 	b.sleepState = Sleeping
-	b.velocity.Set(0,0,0)
-	b.angularVelocity.Set(0,0,0)
+	b.velocity.Set(0, 0, 0)
+	b.angularVelocity.Set(0, 0, 0)
 	b.wakeUpAfterNarrowphase = false
 }
 
@@ -312,151 +286,98 @@ func (b *Body) SleepTick(time float32) {
 
 	if b.allowSleep {
 		speedSquared := b.velocity.LengthSq() + b.angularVelocity.LengthSq()
-		speedLimitSquared := math32.Pow(b.sleepSpeedLimit,2)
+		speedLimitSquared := math32.Pow(b.sleepSpeedLimit, 2)
 		if b.sleepState == Awake && speedSquared < speedLimitSquared {
 			b.sleepState = Sleepy
 			b.timeLastSleepy = time
 			b.Dispatch(SleepyEvent, nil)
 		} else if b.sleepState == Sleepy && speedSquared > speedLimitSquared {
 			b.WakeUp() // Wake up
-		} else if b.sleepState == Sleepy && (time - b.timeLastSleepy ) > b.sleepTimeLimit {
+		} else if b.sleepState == Sleepy && (time-b.timeLastSleepy) > b.sleepTimeLimit {
 			b.Sleep() // Sleeping
 			b.Dispatch(SleepEvent, nil)
 		}
 	}
 }
 
-// TODO maybe return vector instead of pointer in below methods
-
 // PointToLocal converts a world point to local body frame. TODO maybe move to Node
 func (b *Body) PointToLocal(worldPoint *math32.Vector3) math32.Vector3 {
 
-	result := math32.NewVector3(0,0,0).SubVectors(worldPoint, b.position)
-	conj := b.quaternion.Conjugate()
-	result.ApplyQuaternion(conj)
-
-	return *result
+	return *worldPoint.Clone().Sub(b.position).ApplyQuaternion(b.quaternion.Conjugate())
 }
 
 // VectorToLocal converts a world vector to local body frame. TODO maybe move to Node
 func (b *Body) VectorToLocal(worldVector *math32.Vector3) math32.Vector3 {
 
-	result := math32.NewVector3(0,0,0).Copy(worldVector)
-	conj := b.quaternion.Conjugate()
-	result.ApplyQuaternion(conj)
-
-	return *result
+	return *worldVector.Clone().ApplyQuaternion(b.quaternion.Conjugate())
 }
 
 // PointToWorld converts a local point to world frame. TODO maybe move to Node
 func (b *Body) PointToWorld(localPoint *math32.Vector3) math32.Vector3 {
 
-	result := math32.NewVector3(0,0,0).Copy(localPoint)
-	result.ApplyQuaternion(b.quaternion)
-	result.Add(b.position)
-
-	return *result
+	return *localPoint.Clone().ApplyQuaternion(b.quaternion).Add(b.position)
 }
 
 // VectorToWorld converts a local vector to world frame. TODO maybe move to Node
 func (b *Body) VectorToWorld(localVector *math32.Vector3) math32.Vector3 {
 
-	result := math32.NewVector3(0,0,0).Copy(localVector)
-	result.ApplyQuaternion(b.quaternion)
-
-	return *result
+	return *localVector.Clone().ApplyQuaternion(b.quaternion)
 }
 
-
-
-func (b *Body) ComputeAABB() {
-	// TODO
-}
-
-
-// UpdateSolveMassProperties
-// If the body is sleeping, it should be immovable / have infinite mass during solve. We solve it by having a separate "solve mass".
-func (b *Body) UpdateSolveMassProperties() {
+// UpdateEffectiveMassProperties
+// If the body is sleeping, it should be immovable and thus have infinite mass during solve.
+// This is solved by having a separate "effective mass" and other "effective" properties
+func (b *Body) UpdateEffectiveMassProperties() {
 
 	if b.sleepState == Sleeping || b.bodyType == Kinematic {
-		b.invMassSolve = 0
-		b.invInertiaSolve.Zero()
-		b.invInertiaWorldSolve.Zero()
+		b.invMassEff = 0
+		b.invRotInertiaEff.Zero()
+		b.invRotInertiaWorldEff.Zero()
 	} else {
-		b.invMassSolve = b.invMass
-		b.invInertiaSolve.Copy(b.invInertia)
-		b.invInertiaWorldSolve.Copy(b.invInertiaWorld)
+		b.invMassEff = b.invMass
+		b.invRotInertiaEff.Copy(b.invRotInertia)
+		b.invRotInertiaWorldEff.Copy(b.invRotInertiaWorld)
 	}
 }
 
-// UpdateMassProperties // TODO
+// UpdateMassProperties
 // Should be called whenever you change the body shape or mass.
 func (b *Body) UpdateMassProperties() {
 
 	// TODO getter of invMass ?
 	if b.mass > 0 {
-		b.invMass = 1.0/b.mass
+		b.invMass = 1.0 / b.mass
 	} else {
 		b.invMass = 0
 	}
 
-	// Approximate with AABB box
-	b.ComputeAABB()
-	//halfExtents := math32.NewVector3(0,0,0).Set( // TODO
-	//	(b.aabb.upperBound.x-b.aabb.lowerBound.x) / 2,
-	//	(b.aabb.upperBound.y-b.aabb.lowerBound.y) / 2,
-	//	(b.aabb.upperBound.z-b.aabb.lowerBound.z) / 2,
-	//)
-	//Box.CalculateInertia(halfExtents, b.mass, b.inertia) // TODO
-
 	if b.fixedRotation {
-		b.invInertia.Zero()
+		b.rotInertia.Zero()
+		b.invRotInertia.Zero()
 	} else {
-		if b.inertia.X > 0 {
-			b.invInertia.SetX(1/b.inertia.X)
-		} else {
-			b.invInertia.SetX(0)
-		}
-		if b.inertia.Y > 0 {
-			b.invInertia.SetY(1/b.inertia.Y)
-		} else {
-			b.invInertia.SetY(0)
-		}
-		if b.inertia.Z > 0 {
-			b.invInertia.SetZ(1/b.inertia.Z)
-		} else {
-			b.invInertia.SetZ(0)
-		}
-	}
+		*b.rotInertia = b.GetGeometry().RotationalInertia()
+		b.invRotInertia.GetInverse(b.rotInertia)
+		// Note: rotInertia is always positive definite and thus always invertible
+}
 
 	b.UpdateInertiaWorld(true)
 }
 
-// Update .inertiaWorld and .invInertiaWorld
+// Update .inertiaWorld and .invRotInertiaWorld
 func (b *Body) UpdateInertiaWorld(force bool) {
 
-    I := b.invInertia
-	// If angular mass M = s*I, where I is identity and s a scalar, then
+	iRI := b.invRotInertia
+	// If rotational inertia M = s*I, where I is identity and s a scalar, then
 	//    R*M*R' = R*(s*I)*R' = s*R*I*R' = s*R*R' = s*I = M
 	// where R is the rotation matrix.
 	// In other words, we don't have to do the transformation if all diagonal entries are equal.
-    if I.X != I.Y || I.Y != I.Z || force {
-    	//
-    	// AngularMassWorld^(-1) = Rotation * AngularMassBody^(-1) * Rotation^(T)
-    	//          3x3              3x3            3x3                  3x3
-    	//
-    	// Since AngularMassBodyTensor^(-1) is diagonal, then Rotation*AngularMassBodyTensor^(-1) is
-    	// just scaling the columns of AngularMassBodyTensor by the diagonal components.
-    	//
-        m1 := math32.NewMatrix3()
-        m2 := math32.NewMatrix3()
-
-        m1.MakeRotationFromQuaternion(b.quaternion)
-		m2.Copy(m1).Transpose()
-        m1.ScaleColumns(I)
-
-		b.invInertiaWorld.MultiplyMatrices(m1, m2)
-    }
+	if iRI[0] != iRI[4] || iRI[4] != iRI[8] || force {
+		// iRIW = R * iRI * R'
+		m1 := math32.NewMatrix3().MakeRotationFromQuaternion(b.quaternion)
+		m2 := m1.Clone().Transpose()
+		m2.Multiply(iRI)
+		b.invRotInertiaWorld.MultiplyMatrices(m2, m1)
+	}
 }
 
 // Apply force to a world point.
@@ -469,21 +390,17 @@ func (b *Body) ApplyForce(force, relativePoint *math32.Vector3) {
 		return
 	}
 
-	// Compute produced rotational force
-	rotForce := math32.NewVector3(0,0,0)
-	rotForce.CrossVectors(relativePoint, force)
-
 	// Add linear force
 	b.force.Add(force) // TODO shouldn't rotational momentum be subtracted from linear momentum?
 
 	// Add rotational force
-	b.torque.Add(rotForce)
+	b.torque.Add(math32.NewVec3().CrossVectors(relativePoint, force))
 }
 
 // Apply force to a local point in the body.
 // force: The force vector to apply, defined locally in the body frame.
 // localPoint: A local point in the body to apply the force on.
-func (b *Body) ApplyLocalForce(localForce, localPoint *math32.Vector3)  {
+func (b *Body) ApplyLocalForce(localForce, localPoint *math32.Vector3) {
 
 	if b.bodyType != Dynamic {
 		return
@@ -508,22 +425,21 @@ func (b *Body) ApplyImpulse(impulse, relativePoint *math32.Vector3) {
 		return
 	}
 
-    // Compute point position relative to the body center
-    r := relativePoint
+	// Compute point position relative to the body center
+	r := relativePoint
 
-    // Compute produced central impulse velocity
-    velo := math32.NewVector3(0,0,0).Copy(impulse)
-    velo.MultiplyScalar(b.invMass)
+	// Compute produced central impulse velocity
+	velo := impulse.Clone().MultiplyScalar(b.invMass)
 
-    // Add linear impulse
-    b.velocity.Add(velo)
+	// Add linear impulse
+	b.velocity.Add(velo)
 
-    // Compute produced rotational impulse velocity
-	rotVelo := math32.NewVector3(0,0,0).CrossVectors(r, impulse)
-	rotVelo.ApplyMatrix3(b.invInertiaWorld)
+	// Compute produced rotational impulse velocity
+	rotVelo := math32.NewVec3().CrossVectors(r, impulse)
+	rotVelo.ApplyMatrix3(b.invRotInertiaWorld)
 
-    // Add rotational Impulse
-    b.angularVelocity.Add(rotVelo)
+	// Add rotational Impulse
+	b.angularVelocity.Add(rotVelo)
 }
 
 // Apply locally-defined impulse to a local point in the body.
@@ -545,8 +461,7 @@ func (b *Body) ApplyLocalImpulse(localImpulse, localPoint *math32.Vector3) {
 // Get world velocity of a point in the body.
 func (b *Body) GetVelocityAtWorldPoint(worldPoint *math32.Vector3) *math32.Vector3 {
 
-	r := math32.NewVector3(0,0,0)
-	r.SubVectors(worldPoint, b.position)
+	r := math32.NewVec3().SubVectors(worldPoint, b.position)
 	r.CrossVectors(b.angularVelocity, r)
 	r.Add(b.velocity)
 
@@ -559,35 +474,34 @@ func (b *Body) GetVelocityAtWorldPoint(worldPoint *math32.Vector3) *math32.Vecto
 // quatNormalizeFast: If the quaternion should be normalized using "fast" quaternion normalization
 func (b *Body) Integrate(dt float32, quatNormalize, quatNormalizeFast bool) {
 
+	// Save previous position and rotation
+	b.prevPosition.Copy(b.position)
+	b.prevQuaternion.Copy(b.quaternion)
 
-    // Save previous position and rotation
-    b.prevPosition.Copy(b.position)
-    b.prevQuaternion.Copy(b.quaternion)
-
-    // If static or sleeping - skip
-    if !(b.bodyType == Dynamic || b.bodyType == Kinematic) || b.sleepState == Sleeping {
-        return
-    }
+	// If static or sleeping - skip
+	if !(b.bodyType == Dynamic || b.bodyType == Kinematic) || b.sleepState == Sleeping {
+		return
+	}
 
-    // Integrate force over mass (acceleration) to obtain estimate for instantaneous velocities
-    iMdt := b.invMass * dt
-    b.velocity.X += b.force.X * iMdt * b.linearFactor.X
-    b.velocity.Y += b.force.Y * iMdt * b.linearFactor.Y
-    b.velocity.Z += b.force.Z * iMdt * b.linearFactor.Z
+	// Integrate force over mass (acceleration) to obtain estimate for instantaneous velocities
+	iMdt := b.invMass * dt
+	b.velocity.X += b.force.X * iMdt * b.linearFactor.X
+	b.velocity.Y += b.force.Y * iMdt * b.linearFactor.Y
+	b.velocity.Z += b.force.Z * iMdt * b.linearFactor.Z
 
 	// Integrate inverse angular mass times torque to obtain estimate for instantaneous angular velocities
-    e := b.invInertiaWorld
-    tx := b.torque.X * b.angularFactor.X
-    ty := b.torque.Y * b.angularFactor.Y
-    tz := b.torque.Z * b.angularFactor.Z
-    b.angularVelocity.X += dt * (e[0]*tx + e[3]*ty + e[6]*tz)
-    b.angularVelocity.Y += dt * (e[1]*tx + e[4]*ty + e[7]*tz)
-    b.angularVelocity.Z += dt * (e[2]*tx + e[5]*ty + e[8]*tz)
+	e := b.invRotInertiaWorld
+	tx := b.torque.X * b.angularFactor.X
+	ty := b.torque.Y * b.angularFactor.Y
+	tz := b.torque.Z * b.angularFactor.Z
+	b.angularVelocity.X += dt * (e[0]*tx + e[3]*ty + e[6]*tz)
+	b.angularVelocity.Y += dt * (e[1]*tx + e[4]*ty + e[7]*tz)
+	b.angularVelocity.Z += dt * (e[2]*tx + e[5]*ty + e[8]*tz)
 
 	// Integrate velocity to obtain estimate for position
-    b.position.X += b.velocity.X * dt
-    b.position.Y += b.velocity.Y * dt
-    b.position.Z += b.velocity.Z * dt
+	b.position.X += b.velocity.X * dt
+	b.position.Y += b.velocity.Y * dt
+	b.position.Z += b.velocity.Z * dt
 
 	// Integrate angular velocity to obtain estimate for rotation
 	ax := b.angularVelocity.X * b.angularFactor.X
@@ -598,22 +512,22 @@ func (b *Body) Integrate(dt float32, quatNormalize, quatNormalizeFast bool) {
 	bz := b.quaternion.Z
 	bw := b.quaternion.W
 	halfDt := dt * 0.5
-	b.quaternion.X += halfDt * (ax * bw + ay * bz - az * by)
-	b.quaternion.Y += halfDt * (ay * bw + az * bx - ax * bz)
-	b.quaternion.X += halfDt * (az * bw + ax * by - ay * bx)
-	b.quaternion.W += halfDt * (- ax * bx - ay * by - az * bz)
+	b.quaternion.X += halfDt * (ax*bw + ay*bz - az*by)
+	b.quaternion.Y += halfDt * (ay*bw + az*bx - ax*bz)
+	b.quaternion.X += halfDt * (az*bw + ax*by - ay*bx)
+	b.quaternion.W += halfDt * (-ax*bx - ay*by - az*bz)
 
 	// Normalize quaternion
-    if quatNormalize {
-       if quatNormalizeFast {
+	if quatNormalize {
+		if quatNormalizeFast {
 			b.quaternion.NormalizeFast()
-       } else {
+		} else {
 			b.quaternion.Normalize()
-       }
-    }
+		}
+	}
 
-    b.aabbNeedsUpdate = true  // TODO
+	b.aabbNeedsUpdate = true
 
-    // Update world inertia
-    b.UpdateInertiaWorld(false)
+	// Update world inertia
+	b.UpdateInertiaWorld(false)
 }

physics/simulation.go

@@ -338,11 +338,7 @@ func (s *Simulation) ApplySolution(solution *solver.Solution) {
 	// Add results to velocity and angular velocity of bodies
 	for i := 0; i < len(s.bodies); i++ {
 		b := s.bodies[i]
-
-		vDelta := solution.VelocityDeltas[i].Multiply(b.LinearFactor())
-		b.AddToVelocity(vDelta)
-
-		wDelta := solution.AngularVelocityDeltas[i].Multiply(b.AngularFactor())
-		b.AddToAngularVelocity(wDelta)
+		b.velocity.Add(solution.VelocityDeltas[i].Multiply(b.LinearFactor()))
+		b.angularVelocity.Add(solution.AngularVelocityDeltas[i].Multiply(b.AngularFactor()))
 	}
 }