physics dev (shape)

geometry/geometry.go

@@ -383,7 +383,7 @@ func (g *Geometry) Volume() float32 {
 // RotationalInertia returns the rotational inertia tensor, also known as the moment of inertia.
 // This assumes constant density of 1 (kg/m^2).
 // To adjust for a different constant density simply scale the returning matrix by the density.
-func (g *Geometry) RotationalInertia() math32.Matrix3 {
+func (g *Geometry) RotationalInertia(mass float32) math32.Matrix3 {
 
 	// If valid, return its value
 	if g.rotInertiaValid {
@@ -397,7 +397,7 @@ func (g *Geometry) RotationalInertia() math32.Matrix3 {
 	b := math32.NewVec3()
 	box := g.BoundingBox()
 	box.Size(b)
-	multiplier := g.Volume() / 12.0
+	multiplier := mass / 12.0
 
 	x := (b.Y*b.Y + b.Z*b.Z) * multiplier
 	y := (b.X*b.X + b.Z*b.Z) * multiplier

physics/collision/broadphase.go

@@ -3,20 +3,21 @@
 // license that can be found in the LICENSE file.
 
 // Package collision implements collision related algorithms and data structures.
-package collision
+package physics
 
 import (
 	"github.com/g3n/engine/physics/object"
 )
 
-// Broadphase is the base class for broadphase implementations.
-type Broadphase struct {}
-
-type Pair struct {
+// CollisionPair is a pair of bodies that may be colliding.
+type CollisionPair struct {
 	BodyA *object.Body
 	BodyB *object.Body
 }
 
+// Broadphase is the base class for broadphase implementations.
+type Broadphase struct {}
+
 // NewBroadphase creates and returns a pointer to a new Broadphase.
 func NewBroadphase() *Broadphase {
 
@@ -25,9 +26,9 @@ func NewBroadphase() *Broadphase {
 }
 
 // FindCollisionPairs (naive implementation)
-func (b *Broadphase) FindCollisionPairs(objects []*object.Body) []Pair {
+func (b *Broadphase) FindCollisionPairs(objects []*object.Body) []CollisionPair {
 
-	pairs := make([]Pair,0)
+	pairs := make([]CollisionPair,0)
 
 	for iA, bodyA := range objects {
 		for _, bodyB := range objects[iA+1:] {
@@ -35,7 +36,7 @@ func (b *Broadphase) FindCollisionPairs(objects []*object.Body) []Pair {
 				BBa := bodyA.BoundingBox()
 				BBb := bodyB.BoundingBox()
 				if BBa.IsIntersectionBox(&BBb) {
-					pairs = append(pairs, Pair{bodyA, bodyB})
+					pairs = append(pairs, CollisionPair{bodyA, bodyB})
 				}
 			}
 		}

physics/collision.go

@@ -1,21 +0,0 @@
-// Copyright 2016 The G3N Authors. All rights reserved.
-// Use of this source code is governed by a BSD-style
-// license that can be found in the LICENSE file.
-
-package physics
-
-import (
-	"github.com/g3n/engine/core"
-)
-
-// Particle represents a physics-driven particle.
-type Collision struct {
-	// TODO
-}
-
-// NewCollision creates and returns a pointer to a new Collision.
-func  NewCollision(inode core.INode) *Collision {
-
-	c := new(Collision)
-	return c
-}

physics/collision/contact.go

@@ -0,0 +1,15 @@
+// Copyright 2016 The G3N Authors. All rights reserved.
+// Use of this source code is governed by a BSD-style
+// license that can be found in the LICENSE file.
+
+// Package collision implements collision related algorithms and data structures.
+package collision
+
+import "github.com/g3n/engine/math32"
+
+// Contact describes a contact point, normal, and depth.
+type Contact struct {
+	Point  math32.Vector3
+	Normal math32.Vector3
+	Depth  float32
+}

physics/debug.go

@@ -12,6 +12,7 @@ import (
 	"github.com/g3n/engine/gls"
 	"github.com/g3n/engine/graphic"
 	"github.com/g3n/engine/material"
+	"github.com/g3n/engine/physics/collision"
 )
 
 // This file contains helpful infrastructure for debugging physics
@@ -69,7 +70,7 @@ func ShowPenAxis(scene *core.Node, axis *math32.Vector3) {//}, min, max float32)
 	scene.Add(faceGraphic)
 }
 
-func ShowContact(scene *core.Node, contact *Contact) {
+func ShowContact(scene *core.Node, contact *collision.Contact) {
 
 	vertices := math32.NewArrayF32(0, 16)
 

physics/equation/contact.go

@@ -24,7 +24,7 @@ func NewContact(bodyA, bodyB IBody, minForce, maxForce float32) *Contact {
 
 	// minForce default should be 0.
 
-	ce.restitution = 0.3
+	ce.restitution = 0.5
 	ce.rA = &math32.Vector3{0,0,0}
 	ce.rB = &math32.Vector3{0,0,0}
 	ce.nA = &math32.Vector3{0,0,0}

physics/equation/rotationalmotor.go

@@ -11,7 +11,7 @@ import (
 // RotationalMotor is a rotational motor constraint equation.
 // Tries to keep the relative angular velocity of the bodies to a given value.
 type RotationalMotor struct {
-	Equation
+	Equation // TODO maybe this should embed Rotational instead ?
 	axisA       *math32.Vector3 // World oriented rotational axis
 	axisB       *math32.Vector3 // World oriented rotational axis
 	targetSpeed float32         // Target speed

physics/material/material.go

@@ -3,7 +3,7 @@
 // license that can be found in the LICENSE file.
 
 // Package physics implements a basic physics engine.
-package material
+package physics
 
 type Material struct {
 	name        string

physics/object/body.go

@@ -8,6 +8,7 @@ import (
 	"github.com/g3n/engine/graphic"
 	"github.com/g3n/engine/math32"
 	"github.com/g3n/engine/material"
+	"github.com/g3n/engine/physics/shape"
 )
 
 // Body represents a physics-driven body.
@@ -86,6 +87,7 @@ type Body struct {
 	uniqueEdges      []math32.Vector3
 	worldUniqueEdges []math32.Vector3
 
+	shape shape.IShape
 
 	// TODO future (for now a body is a single graphic with a single geometry)
 	// shapes          []*Shape
@@ -151,7 +153,6 @@ func NewBody(igraphic graphic.IGraphic) *Body {
 
 	b := new(Body)
 	b.Graphic = igraphic.GetGraphic()
-	b.SetMass(1)
 	b.bodyType = Dynamic
 
 	// Rotational inertia and related properties
@@ -206,116 +207,54 @@ func NewBody(igraphic graphic.IGraphic) *Body {
 
 	b.wakeUpAfterNarrowphase = false
 
-	// Perform single-time computations
-	b.computeFaceNormalsAndUniqueEdges()
+	b.SetShape(shape.NewConvexHull(b.GetGeometry()))
 
+	b.SetMass(1)
 	b.UpdateMassProperties()
 	b.UpdateEffectiveMassProperties()
 
 	return b
 }
 
-// Compute and store face normals and unique edges
-func (b *Body) computeFaceNormalsAndUniqueEdges() {
-
-	b.GetGeometry().ReadFaces(func(vA, vB, vC math32.Vector3) bool {
-
-		// Store face vertices
-		var face [3]math32.Vector3
-		face[0] = vA
-		face[1] = vB
-		face[2] = vC
-		b.faces = append(b.faces, face)
-
-		// Compute edges
-		edge1 := math32.NewVec3().SubVectors(&vB, &vA)
-		edge2 := math32.NewVec3().SubVectors(&vC, &vB)
-		edge3 := math32.NewVec3().SubVectors(&vA, &vC)
-
-		// Compute and store face normal in b.faceNormals
-		faceNormal := math32.NewVec3().CrossVectors(edge2, edge1)
-		if faceNormal.Length() > 0 {
-			faceNormal.Normalize().Negate()
-		}
-		b.faceNormals = append(b.faceNormals, *faceNormal)
-
-		// Compare unique edges recorded so far with the three new face edges and store the unique ones
-		tol := float32(1e-6)
-		for p := 0; p < len(b.uniqueEdges); p++ {
-			ue := b.uniqueEdges[p]
-			if !ue.AlmostEquals(edge1, tol) {
-				b.uniqueEdges = append(b.uniqueEdges, *edge1)
-			}
-			if !ue.AlmostEquals(edge2, tol) {
-				b.uniqueEdges = append(b.uniqueEdges, *edge1)
-			}
-			if !ue.AlmostEquals(edge3, tol) {
-				b.uniqueEdges = append(b.uniqueEdges, *edge1)
-			}
-		}
-
-		return false
-	})
-
-	// Allocate space for worldFaceNormals and worldUniqueEdges
-	b.worldFaceNormals = make([]math32.Vector3, len(b.faceNormals))
-	b.worldUniqueEdges = make([]math32.Vector3, len(b.uniqueEdges))
-}
-
-// ComputeWorldFaceNormalsAndUniqueEdges
-func (b *Body) ComputeWorldFaceNormalsAndUniqueEdges() {
-
-	// Re-compute world face normals from local face normals
-	for i := 0; i < len(b.faceNormals); i++ {
-		b.worldFaceNormals[i] = b.faceNormals[i]
-		b.worldFaceNormals[i].ApplyQuaternion(b.quaternion)
-	}
-	// Re-compute world unique edges from local unique edges
-	for i := 0; i < len(b.uniqueEdges); i++ {
-		b.worldUniqueEdges[i] = b.uniqueEdges[i]
-		b.worldUniqueEdges[i].ApplyQuaternion(b.quaternion)
-	}
-}
-
-func (b *Body) Faces() [][3]math32.Vector3 {
-
-	return b.faces
-}
-
-func (b *Body) FaceNormals() []math32.Vector3 {
-
-	return b.faceNormals
-}
-
-func (b *Body) WorldFaceNormals() []math32.Vector3 {
-
-	return b.worldFaceNormals
-}
-
-func (b *Body) UniqueEdges() []math32.Vector3 {
+// TODO future: modify this to be "AddShape" and keep track of list of shapes, their positions and orientations
+// For now each body can only be a single shape or a single geometry
+func (b *Body) SetShape(shape shape.IShape) {
 
-	return b.uniqueEdges
+	b.shape = shape
 }
 
-func (b *Body) WorldUniqueEdges() []math32.Vector3 {
+func (b *Body) Shape() shape.IShape {
 
-	return b.worldUniqueEdges
+	return b.shape
 }
 
 func (b *Body) BoundingBox() math32.Box3 {
 
-	return b.GetGeometry().BoundingBox()
+	// TODO future allow multiple shapes
+	mat4 := math32.NewMatrix4().Compose(b.position, b.quaternion, math32.NewVector3(1,1,1))
+	localBB := b.shape.BoundingBox()
+	worldBB := localBB.ApplyMatrix4(mat4)
+	return *worldBB
 }
 
 func (b *Body) SetMass(mass float32) {
 
+	// Do nothing if current mass is already the specified mass
+	if mass == b.mass {
+		return
+	}
+
+	// Set mass and update inverse mass
 	b.mass = mass
 	if b.mass > 0 {
 		b.invMass = 1.0 / b.mass
 	} else {
+		// Body mass is zero - this means that the body is static
 		b.invMass = 0
 		b.bodyType = Static
 	}
+
+	b.UpdateMassProperties()
 }
 
 func (b *Body) SetIndex(i int) {
@@ -363,13 +302,27 @@ func (b *Body) SleepState() BodySleepState {
 	return b.sleepState
 }
 
-func (b *Body) SetBodyType(bt BodyType) {
+// SetBodyType sets the body type.
+func (b *Body) SetBodyType(bodyType BodyType) {
+
+	// Do nothing if body is already of the specified bodyType
+	if b.bodyType == bodyType {
+		return
+	}
 
-	if bt == Static {
+	// If we want the body to be static we need to zero its mass
+	if bodyType == Static {
 		b.mass = 0
-		b.invMass = 0
 	}
-	b.bodyType = bt
+
+	// Temporarily save original body type and update current body type
+	origBodyType := b.bodyType
+	b.bodyType = bodyType
+
+	// If changed body type to or from Static then we need to update mass properties
+	if origBodyType == Static || b.bodyType == Static {
+		b.UpdateMassProperties()
+	}
 }
 
 func (b *Body) BodyType() BodyType {
@@ -387,12 +340,14 @@ func (b *Body) WakeUpAfterNarrowphase() bool {
 	return b.wakeUpAfterNarrowphase
 }
 
+// ApplyVelocityDeltas adds the specified deltas to the body's linear and angular velocities.
 func (b *Body) ApplyVelocityDeltas(linearD, angularD *math32.Vector3) {
 
 	b.velocity.Add(linearD.Multiply(b.linearFactor))
 	b.angularVelocity.Add(angularD.Multiply(b.angularFactor))
 }
 
+// ClearForces clears all forces on the body.
 func (b *Body) ClearForces() {
 
 	b.force.Zero()
@@ -495,7 +450,18 @@ func (b *Body) AngularFactor() math32.Vector3 {
 	return *b.angularFactor
 }
 
+// SetFixedRotation specifies whether the body should rotate.
+func (b *Body) SetFixedRotation(state bool) {
 
+	// Do nothing if the fixedRotation flag already has the specified value
+	if b.fixedRotation == state {
+		return
+	}
+
+	// Set the fixedRotation flag and update mass properties
+	b.fixedRotation = state
+	b.UpdateMassProperties()
+}
 
 // WakeUp wakes the body up.
 func (b *Body) WakeUp() {
@@ -604,19 +570,18 @@ func (b *Body) UpdateEffectiveMassProperties() {
 // 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
 	} else {
 		b.invMass = 0
 	}
 
-	if b.fixedRotation {
+	if b.fixedRotation || b.bodyType == Static {
 		b.rotInertia.Zero()
 		b.invRotInertia.Zero()
 	} else {
-		*b.rotInertia = b.GetGeometry().RotationalInertia()
-		b.rotInertia.MultiplyScalar(1000) // multiply by high density // TODO remove this ?
+		*b.rotInertia = b.GetGeometry().RotationalInertia(b.mass)
+		b.rotInertia.MultiplyScalar(10) // multiply by high density // TODO remove this ?
 		b.invRotInertia.GetInverse(b.rotInertia) // Note: rotInertia is always positive definite and thus always invertible
 	}
 

physics/shape/convexhull.go

@@ -0,0 +1,465 @@
+// Copyright 2016 The G3N Authors. All rights reserved.
+// Use of this source code is governed by a BSD-style
+// license that can be found in the LICENSE file.
+
+package shape
+
+import (
+	"github.com/g3n/engine/geometry"
+	"github.com/g3n/engine/math32"
+	"github.com/g3n/engine/physics/collision"
+)
+
+// ConvexHull is a convex triangle-based geometry used for collision detection and contact resolution.
+type ConvexHull struct {
+	geometry.Geometry
+
+	// Cached geometry properties
+	faces            [][3]math32.Vector3
+	faceNormals      []math32.Vector3
+	worldFaceNormals []math32.Vector3
+	uniqueEdges      []math32.Vector3
+	worldUniqueEdges []math32.Vector3
+
+}
+
+func NewConvexHull(geom *geometry.Geometry) *ConvexHull {
+
+	ch := new(ConvexHull)
+	// // TODO check if geometry is convex, panic if not
+	//if !geom.IsConvex() {
+	//	panic("geometry needs to be convex")
+	//}
+	// // TODO future: create function to break up geometry into convex shapes and add all shapes to body
+
+	ch.Geometry = *geom
+
+	// Perform single-time computations
+	ch.computeFaceNormalsAndUniqueEdges()
+
+	return ch
+}
+
+// Compute and store face normals and unique edges
+func (ch *ConvexHull) computeFaceNormalsAndUniqueEdges() {
+
+	ch.GetGeometry().ReadFaces(func(vA, vB, vC math32.Vector3) bool {
+
+		// Store face vertices
+		var face [3]math32.Vector3
+		face[0] = vA
+		face[1] = vB
+		face[2] = vC
+		ch.faces = append(ch.faces, face)
+
+		// Compute edges
+		edge1 := math32.NewVec3().SubVectors(&vB, &vA)
+		edge2 := math32.NewVec3().SubVectors(&vC, &vB)
+		edge3 := math32.NewVec3().SubVectors(&vA, &vC)
+
+		// Compute and store face normal in b.faceNormals
+		faceNormal := math32.NewVec3().CrossVectors(edge2, edge1)
+		if faceNormal.Length() > 0 {
+			faceNormal.Normalize().Negate()
+		}
+		ch.faceNormals = append(ch.faceNormals, *faceNormal)
+
+		// Compare unique edges recorded so far with the three new face edges and store the unique ones
+		tol := float32(1e-6)
+		for p := 0; p < len(ch.uniqueEdges); p++ {
+			ue := ch.uniqueEdges[p]
+			if !ue.AlmostEquals(edge1, tol) {
+				ch.uniqueEdges = append(ch.uniqueEdges, *edge1)
+			}
+			if !ue.AlmostEquals(edge2, tol) {
+				ch.uniqueEdges = append(ch.uniqueEdges, *edge1)
+			}
+			if !ue.AlmostEquals(edge3, tol) {
+				ch.uniqueEdges = append(ch.uniqueEdges, *edge1)
+			}
+		}
+
+		return false
+	})
+
+	// Allocate space for worldFaceNormals and worldUniqueEdges
+	ch.worldFaceNormals = make([]math32.Vector3, len(ch.faceNormals))
+	ch.worldUniqueEdges = make([]math32.Vector3, len(ch.uniqueEdges))
+}
+
+// ComputeWorldFaceNormalsAndUniqueEdges
+func (ch *ConvexHull) ComputeWorldFaceNormalsAndUniqueEdges(quat *math32.Quaternion) {
+
+	// Re-compute world face normals from local face normals
+	for i := 0; i < len(ch.faceNormals); i++ {
+		ch.worldFaceNormals[i] = ch.faceNormals[i]
+		ch.worldFaceNormals[i].ApplyQuaternion(quat)
+	}
+	// Re-compute world unique edges from local unique edges
+	for i := 0; i < len(ch.uniqueEdges); i++ {
+		ch.worldUniqueEdges[i] = ch.uniqueEdges[i]
+		ch.worldUniqueEdges[i].ApplyQuaternion(quat)
+	}
+}
+
+func (ch *ConvexHull) Faces() [][3]math32.Vector3 {
+
+	return ch.faces
+}
+
+func (ch *ConvexHull) FaceNormals() []math32.Vector3 {
+
+	return ch.faceNormals
+}
+
+func (ch *ConvexHull) WorldFaceNormals() []math32.Vector3 {
+
+	return ch.worldFaceNormals
+}
+
+func (ch *ConvexHull) UniqueEdges() []math32.Vector3 {
+
+	return ch.uniqueEdges
+}
+
+func (ch *ConvexHull) WorldUniqueEdges() []math32.Vector3 {
+
+	return ch.worldUniqueEdges
+}
+
+// FindPenetrationAxis finds the penetration axis between two convex bodies.
+// The normal points from bodyA to bodyB.
+// Returns false if there is no penetration. If there is a penetration - returns true and the penetration axis.
+func (ch *ConvexHull) FindPenetrationAxis(chB *ConvexHull, posA, posB *math32.Vector3, quatA, quatB *math32.Quaternion) (bool, math32.Vector3) {
+
+	// Keep track of the smaller depth found so far
+	// Note that the penetration axis is the one that causes
+	// the smallest penetration depth when the two hulls are squished onto that axis!
+	// (may seem a bit counter-intuitive)
+	depthMin := math32.Inf(1)
+
+	var penetrationAxis math32.Vector3
+	var depth float32
+
+	// Assume the geometries are penetrating.
+	// As soon as (and if) we figure out that they are not, then return false.
+	penetrating := true
+
+	worldFaceNormalsA := ch.WorldFaceNormals()
+	worldFaceNormalsB := ch.WorldFaceNormals()
+
+	// Check world normals of body A
+	for _, worldFaceNormal := range worldFaceNormalsA {
+		// Check whether the face is colliding with geomB
+		penetrating, depth = ch.TestPenetrationAxis(chB, &worldFaceNormal, posA, posB, quatA, quatB)
+		if !penetrating {
+			return false, penetrationAxis // penetrationAxis doesn't matter since not penetrating
+		}
+		if depth < depthMin {
+			depthMin = depth
+			penetrationAxis.Copy(&worldFaceNormal)
+		}
+	}
+
+	// Check world normals of body B
+	for _, worldFaceNormal := range worldFaceNormalsB {
+		// Check whether the face is colliding with geomB
+		penetrating, depth = ch.TestPenetrationAxis(chB, &worldFaceNormal, posA, posB, quatA, quatB)
+		if !penetrating {
+			return false, penetrationAxis // penetrationAxis doesn't matter since not penetrating
+		}
+		if depth < depthMin {
+			depthMin = depth
+			penetrationAxis.Copy(&worldFaceNormal)
+		}
+	}
+
+	worldUniqueEdgesA := ch.WorldUniqueEdges()
+	worldUniqueEdgesB := ch.WorldUniqueEdges()
+
+	// Check all combinations of unique world edges
+	for _, worldUniqueEdgeA := range worldUniqueEdgesA {
+		for _, worldUniqueEdgeB := range worldUniqueEdgesB {
+			// Cross edges
+			edgeCross := math32.NewVec3().CrossVectors(&worldUniqueEdgeA, &worldUniqueEdgeB)
+			// If the edges are not aligned
+			tol := float32(1e-6)
+			if edgeCross.Length() > tol { // Cross product is not close to zero
+				edgeCross.Normalize()
+				penetrating, depth = ch.TestPenetrationAxis(chB, edgeCross, posA, posB, quatA, quatB)
+				if !penetrating {
+					return false, penetrationAxis
+				}
+				if depth < depthMin {
+					depthMin = depth
+					penetrationAxis.Copy(edgeCross)
+				}
+			}
+		}
+	}
+
+	deltaC := math32.NewVec3().SubVectors(posA, posB)
+	if deltaC.Dot(&penetrationAxis) > 0.0 {
+		penetrationAxis.Negate()
+	}
+
+	return true, penetrationAxis
+}
+
+// Both hulls are projected onto the axis and the overlap size (penetration depth) is returned if there is one.
+// return {number} The overlap depth, or FALSE if no penetration.
+func (ch *ConvexHull) TestPenetrationAxis(chB *ConvexHull, worldAxis, posA, posB *math32.Vector3, quatA, quatB *math32.Quaternion) (bool, float32) {
+
+	maxA, minA := ch.ProjectOntoWorldAxis(worldAxis, posA, quatA)
+	maxB, minB := chB.ProjectOntoWorldAxis(worldAxis, posB, quatB)
+
+	if maxA < minB || maxB < minA {
+		return false, 0 // Separated
+	}
+
+	d0 := maxA - minB
+	d1 := maxB - minA
+
+	if d0 < d1 {
+		return true, d0
+	} else {
+		return true, d1
+	}
+}
+
+// ProjectOntoWorldAxis projects the geometry onto the specified world axis.
+func (ch *ConvexHull) ProjectOntoWorldAxis(worldAxis, pos *math32.Vector3, quat *math32.Quaternion) (float32, float32) {
+
+	// Transform the world axis to local
+	quatConj := quat.Conjugate()
+	localAxis := worldAxis.Clone().ApplyQuaternion(quatConj)
+
+	// Project onto the local axis
+	max, min := ch.GetGeometry().ProjectOntoAxis(localAxis)
+
+	// Offset to obtain values relative to world origin
+	localOrigin := math32.NewVec3().Sub(pos).ApplyQuaternion(quatConj)
+	add := localOrigin.Dot(localAxis)
+	min -= add
+	max -= add
+
+	return max, min
+}
+
+// =====================================================================
+
+//{array} result The an array of contact point objects, see clipFaceAgainstHull
+func (ch *ConvexHull) ClipAgainstHull(chB *ConvexHull, posA, posB *math32.Vector3, quatA, quatB *math32.Quaternion, penAxis *math32.Vector3, minDist, maxDist float32) []collision.Contact {
+
+	var contacts []collision.Contact
+
+	// Invert penetration axis so it points from b to a
+	invPenAxis := penAxis.Clone().Negate()
+
+	// Find face of B that is closest (i.e. that is most aligned with the penetration axis)
+	closestFaceBidx := -1
+	dmax := math32.Inf(-1)
+	worldFaceNormalsB := chB.WorldFaceNormals()
+	for i, worldFaceNormal := range worldFaceNormalsB {
+		// Note - normals must be pointing out of the body so that they align with the penetration axis in the line below
+		d := worldFaceNormal.Dot(invPenAxis)
+		if d > dmax {
+			dmax = d
+			closestFaceBidx = i
+		}
+	}
+
+	// If found a closest face (sometimes we don't find one)
+	if closestFaceBidx >= 0 {
+
+		// Copy and transform face vertices to world coordinates
+		faces := chB.Faces()
+		worldClosestFaceB := ch.WorldFace(faces[closestFaceBidx], posB, quatB)
+
+		// Clip the closest world face of B to only the portion that is inside the hull of A
+		contacts = ch.clipFaceAgainstHull(posA, penAxis, quatA, worldClosestFaceB, minDist, maxDist)
+	}
+
+	return contacts
+}
+
+func (ch *ConvexHull) WorldFace(face [3]math32.Vector3, pos *math32.Vector3, quat *math32.Quaternion) [3]math32.Vector3 {
+
+	var result [3]math32.Vector3
+	result[0] = face[0]
+	result[1] = face[1]
+	result[2] = face[2]
+	result[0].ApplyQuaternion(quat).Add(pos)
+	result[1].ApplyQuaternion(quat).Add(pos)
+	result[2].ApplyQuaternion(quat).Add(pos)
+	return result
+}
+
+// Clip a face against a hull.
+//@param {Array} worldVertsB1 An array of Vec3 with vertices in the world frame.
+//@param Array result Array to store resulting contact points in. Will be objects with properties: point, depth, normal. These are represented in world coordinates.
+func (ch *ConvexHull) clipFaceAgainstHull(posA, penAxis *math32.Vector3, quatA *math32.Quaternion, worldClosestFaceB [3]math32.Vector3, minDist, maxDist float32) []collision.Contact {
+
+	contacts := make([]collision.Contact, 0)
+
+	// Find the face of A with normal closest to the separating axis (i.e. that is most aligned with the penetration axis)
+	closestFaceAidx := -1
+	dmax := math32.Inf(-1)
+	worldFaceNormalsA := ch.WorldFaceNormals()
+	for i, worldFaceNormal := range worldFaceNormalsA {
+		// Note - normals must be pointing out of the body so that they align with the penetration axis in the line below
+		d := worldFaceNormal.Dot(penAxis)
+		if d > dmax {
+			dmax = d
+			closestFaceAidx = i
+		}
+	}
+
+	if closestFaceAidx < 0 {
+		// Did not find any closest face...
+		return contacts
+	}
+
+	//console.log("closest A: ",worldClosestFaceA);
+
+	// Get the face and construct connected faces
+	facesA := ch.Faces()
+	//worldClosestFaceA := n.WorldFace(facesA[closestFaceAidx], bodyA)
+	closestFaceA := facesA[closestFaceAidx]
+	connectedFaces := make([]int, 0) // indexes of the connected faces
+	for faceIdx := 0; faceIdx < len(facesA); faceIdx++ {
+		// Skip worldClosestFaceA
+		if faceIdx == closestFaceAidx {
+			continue
+		}
+		// Test that face has not already been added
+		for _, cfidx := range connectedFaces {
+			if cfidx == faceIdx {
+				continue
+			}
+		}
+		face := facesA[faceIdx]
+		// Loop through face vertices and see if any of them are also present in the closest face
+		// If a vertex is shared and this connected face hasn't been recorded yet - record and break inner loop
+		for pConnFaceVidx := 0; pConnFaceVidx < len(face); pConnFaceVidx++ {
+			var goToNextFace bool
+			// Test if face shares a vertex with closetFaceA - add it to connectedFaces if so and break out of both loops
+			for closFaceVidx := 0; closFaceVidx < len(closestFaceA); closFaceVidx++ {
+				if closestFaceA[closFaceVidx].Equals(&face[pConnFaceVidx]) {
+					connectedFaces = append(connectedFaces, faceIdx)
+					goToNextFace = true
+					break
+				}
+			}
+			if goToNextFace {
+				break
+			}
+		}
+	}
+
+
+	worldClosestFaceA := ch.WorldFace(closestFaceA, posA, quatA)
+	// DEBUGGING
+	//if n.debugging {
+	//	//log.Error("CONN-FACES: %v", len(connectedFaces))
+	//	for _, fidx := range connectedFaces {
+	//		wFace := n.WorldFace(facesA[fidx], bodyA)
+	//		ShowWorldFace(n.simulation.Scene(), wFace[:], &math32.Color{0.8, 0.8, 0.8})
+	//	}
+	//	//log.Error("worldClosestFaceA: %v", worldClosestFaceA)
+	//	//log.Error("worldClosestFaceB: %v", worldClosestFaceB)
+	//	ShowWorldFace(n.simulation.Scene(), worldClosestFaceA[:], &math32.Color{2, 0, 0})
+	//	ShowWorldFace(n.simulation.Scene(), worldClosestFaceB[:], &math32.Color{0, 2, 0})
+	//}
+
+	clippedFace := make([]math32.Vector3, len(worldClosestFaceB))
+	for i, v := range worldClosestFaceB {
+		clippedFace[i] = v
+	}
+
+	// TODO port simplified loop to cannon.js once done and verified
+	// https://github.com/schteppe/cannon.js/issues/378
+	// https://github.com/TheRohans/cannon.js/commit/62a1ce47a851b7045e68f7b120b9e4ecb0d91aab#r29106924
+	// Iterate over connected faces and clip the planes associated with their normals
+	for _, cfidx := range connectedFaces {
+		connFace := facesA[cfidx]
+		connFaceNormal := worldFaceNormalsA[cfidx]
+		// Choose a vertex in the connected face and use it to find the plane constant
+		worldFirstVertex := connFace[0].Clone().ApplyQuaternion(quatA).Add(posA)
+		planeDelta := - worldFirstVertex.Dot(&connFaceNormal)
+		clippedFace = ch.clipFaceAgainstPlane(clippedFace, connFaceNormal.Clone(), planeDelta)
+	}
+
+	// Plot clipped face
+	//if n.debugging {
+	//	log.Error("worldClosestFaceBClipped: %v", clippedFace)
+	//	ShowWorldFace(n.simulation.Scene(), clippedFace, &math32.Color{0, 0, 2})
+	//}
+
+	closestFaceAnormal := worldFaceNormalsA[closestFaceAidx]
+	worldFirstVertex := worldClosestFaceA[0].Clone()//.ApplyQuaternion(quatA).Add(&posA)
+	planeDelta := -worldFirstVertex.Dot(&closestFaceAnormal)
+
+	// For each vertex in the clipped face resolve its depth (relative to the closestFaceA) and create a contact
+	for _, vertex := range clippedFace {
+		depth := closestFaceAnormal.Dot(&vertex) + planeDelta
+		// Cap distance
+		if depth <= minDist {
+			depth = minDist
+		}
+		if depth <= maxDist {
+			if depth <= 0 {
+				contacts = append(contacts, collision.Contact{
+					Point: vertex,
+					Normal: closestFaceAnormal,
+					Depth: depth,
+				})
+			}
+		}
+
+	}
+
+	return contacts
+}
+
+
+// clipFaceAgainstPlane clips the specified face against the back of the specified plane.
+// This is used multiple times when finding the portion of a face of one convex hull that is inside another convex hull.
+// planeNormal and planeConstant satisfy the plane equation n*x = p where n is the planeNormal and p is the planeConstant (and x is a point on the plane).
+func (ch *ConvexHull) clipFaceAgainstPlane(face []math32.Vector3, planeNormal *math32.Vector3, planeConstant float32) []math32.Vector3 {
+
+	// inVertices are the verts making up the face of hullB
+
+	clippedFace := make([]math32.Vector3, 0)
+
+	// If not a face (if an edge or a vertex) - don't clip it
+	if len(face) < 2 {
+		return face
+	}
+
+	firstVertex := face[len(face)-1]
+	dotFirst := planeNormal.Dot(&firstVertex) + planeConstant
+
+	for vi := 0; vi < len(face); vi++ {
+		lastVertex := face[vi]
+		dotLast := planeNormal.Dot(&lastVertex) + planeConstant
+		if dotFirst < 0 { // Inside hull
+			if dotLast < 0 { // Start < 0, end < 0, so output lastVertex
+				clippedFace = append(clippedFace, lastVertex)
+			} else { // Start < 0, end >= 0, so output intersection
+				newv := firstVertex.Clone().Lerp(&lastVertex, dotFirst / (dotFirst - dotLast))
+				clippedFace = append(clippedFace, *newv)
+			}
+		} else { // Outside hull
+			if dotLast < 0 { // Start >= 0, end < 0 so output intersection and end
+				newv := firstVertex.Clone().Lerp(&lastVertex, dotFirst / (dotFirst - dotLast))
+				clippedFace = append(clippedFace, *newv)
+				clippedFace = append(clippedFace, lastVertex)
+			}
+		}
+		firstVertex = lastVertex
+		dotFirst = dotLast
+	}
+
+	return clippedFace
+}

physics/shape/plane.go

@@ -0,0 +1,76 @@
+// Copyright 2016 The G3N Authors. All rights reserved.
+// Use of this source code is governed by a BSD-style
+// license that can be found in the LICENSE file.
+
+package shape
+
+import "github.com/g3n/engine/math32"
+
+// Plane is an analytical collision Plane.
+// A plane, facing in the +Z direction. The plane has its surface at z=0 and everything below z=0 is assumed to be solid.
+type Plane struct {
+	normal math32.Vector3
+}
+
+// NewPlane creates and returns a pointer to a new analytical collision plane.
+func NewPlane() *Plane {
+
+	p := new(Plane)
+	p.normal = *math32.NewVector3(0,0,1) //*normal
+	return p
+}
+
+// SetRadius sets the radius of the analytical collision sphere.
+//func (p *Plane) SetNormal(normal *math32.Vector3) {
+//
+//	p.normal = *normal
+//}
+
+// Normal returns the normal of the analytical collision plane.
+func (p *Plane) Normal() math32.Vector3 {
+
+	return p.normal
+}
+
+// IShape =============================================================
+
+// BoundingBox computes and returns the bounding box of the analytical collision plane.
+func (p *Plane) BoundingBox() math32.Box3 {
+
+	//return math32.Box3{math32.Vector3{math32.Inf(-1), math32.Inf(-1), math32.Inf(-1)}, math32.Vector3{math32.Inf(1), 0, math32.Inf(1)}}
+	return math32.Box3{math32.Vector3{-1000, -1000, -1000}, math32.Vector3{1000, 1000, 0}}
+}
+
+// BoundingSphere computes and returns the bounding sphere of the analytical collision plane.
+func (p *Plane) BoundingSphere() math32.Sphere {
+
+	return *math32.NewSphere(math32.NewVec3(), math32.Inf(1))
+}
+
+// Area returns the surface area of the analytical collision plane.
+func (p *Plane) Area() float32 {
+
+	return math32.Inf(1)
+}
+
+// Volume returns the volume of the analytical collision sphere.
+func (p *Plane) Volume() float32 {
+
+	return math32.Inf(1)
+}
+
+// RotationalInertia computes and returns the rotational inertia of the analytical collision plane.
+func (p *Plane) RotationalInertia(mass float32) math32.Matrix3 {
+
+	return *math32.NewMatrix3().Zero()
+}
+
+// ProjectOntoAxis returns the minimum and maximum distances of the analytical collision plane projected onto the specified local axis.
+func (p *Plane) ProjectOntoAxis(localAxis *math32.Vector3) (float32, float32) {
+
+	if localAxis.Equals(&p.normal) {
+		return math32.Inf(-1), 0
+	} else {
+		return math32.Inf(-1), math32.Inf(1)
+	}
+}

physics/shape/shape.go

@@ -0,0 +1,37 @@
+// Copyright 2016 The G3N Authors. All rights reserved.
+// Use of this source code is governed by a BSD-style
+// license that can be found in the LICENSE file.
+
+package shape
+
+import "github.com/g3n/engine/math32"
+
+// IShape is the interface for all collision shapes.
+// Shapes in this package satisfy this interface and also geometry.Geometry.
+type IShape interface {
+	BoundingBox() math32.Box3
+	BoundingSphere() math32.Sphere
+	Area() float32
+	Volume() float32
+	RotationalInertia(mass float32) math32.Matrix3
+	ProjectOntoAxis(localAxis *math32.Vector3) (float32, float32)
+}
+
+// Shape is a collision shape.
+// It can be an analytical geometry such as a sphere, plane, etc.. or it can be defined by a polygonal Geometry.
+type Shape struct {
+
+	// TODO
+	//material
+
+	// Collision filtering
+	colFilterGroup int
+	colFilterMask  int
+}
+
+func (s *Shape) initialize() {
+
+	// Collision filtering
+	s.colFilterGroup = 1
+	s.colFilterMask = -1
+}

physics/shape/sphere.go

@@ -0,0 +1,75 @@
+// Copyright 2016 The G3N Authors. All rights reserved.
+// Use of this source code is governed by a BSD-style
+// license that can be found in the LICENSE file.
+
+package shape
+
+import "github.com/g3n/engine/math32"
+
+// Sphere is an analytical collision sphere.
+type Sphere struct {
+	radius float32
+}
+
+// NewSphere creates and returns a pointer to a new analytical collision sphere.
+func NewSphere(radius float32) *Sphere {
+
+	s := new(Sphere)
+	s.radius = radius
+	return s
+}
+
+// SetRadius sets the radius of the analytical collision sphere.
+func (s *Sphere) SetRadius(radius float32) {
+
+	s.radius = radius
+}
+
+// Radius returns the radius of the analytical collision sphere.
+func (s *Sphere) Radius() float32 {
+
+	return s.radius
+}
+
+// IShape =============================================================
+
+// BoundingBox computes and returns the bounding box of the analytical collision sphere.
+func (s *Sphere) BoundingBox() math32.Box3 {
+
+	return math32.Box3{math32.Vector3{-s.radius, -s.radius, -s.radius}, math32.Vector3{s.radius, s.radius, s.radius}}
+}
+
+// BoundingSphere computes and returns the bounding sphere of the analytical collision sphere.
+func (s *Sphere) BoundingSphere() math32.Sphere {
+
+	return *math32.NewSphere(math32.NewVec3(), s.radius)
+}
+
+// Area computes and returns the surface area of the analytical collision sphere.
+func (s *Sphere) Area() float32 {
+
+	return 4 * math32.Pi * s.radius * s.radius
+}
+
+// Volume computes and returns the volume of the analytical collision sphere.
+func (s *Sphere) Volume() float32 {
+
+	return (4/3) * math32.Pi * s.radius * s.radius * s.radius
+}
+
+// RotationalInertia computes and returns the rotational inertia of the analytical collision sphere.
+func (s *Sphere) RotationalInertia(mass float32) math32.Matrix3 {
+
+	v := (2/5) * mass * s.radius * s.radius
+	return *math32.NewMatrix3().Set(
+		v, 0, 0,
+		0, v, 0,
+		0, 0, v,
+	)
+}
+
+// ProjectOntoAxis computes and returns the minimum and maximum distances of the analytical collision sphere projected onto the specified local axis.
+func (s *Sphere) ProjectOntoAxis(localAxis *math32.Vector3) (float32, float32) {
+
+	return -s.radius, s.radius
+}

physics/simulation.go

@@ -11,38 +11,29 @@ import (
 	"github.com/g3n/engine/physics/collision"
 	"github.com/g3n/engine/math32"
 	"github.com/g3n/engine/physics/object"
-	"github.com/g3n/engine/physics/material"
 	"github.com/g3n/engine/core"
+	"github.com/g3n/engine/physics/shape"
 )
 
-type ICollidable interface {
-	Collided() bool
-	Static() bool
-	//GetCollisions() []*Collision
-}
-
-type CollisionGroup struct {
-	// TODO future
-}
-
 // Simulation represents a physics simulation.
 type Simulation struct {
+	scene       *core.Node
 	forceFields []ForceField
 
-	bodies      []*object.Body
-	nilBodies   []int   // Array keeps track of which indices of the 'bodies' array are nil
+	// Bodies under simulation
+	bodies      []*object.Body  // Slice of bodies. May contain nil values.
+	nilBodies   []int           // Array keeps track of which indices of the 'bodies' array are nil
 
+	// Collision tracking
 	collisionMatrix     collision.Matrix // Boolean triangular matrix indicating which pairs of bodies are colliding
 	prevCollisionMatrix collision.Matrix // CollisionMatrix from the previous step.
 
 	allowSleep  bool                // Makes bodies go to sleep when they've been inactive
-	contactEqs  []*equation.Contact // All the current contacts (instances of ContactEquation) in the world.
-	frictionEqs []*equation.Friction
 	paused bool
 
-	quatNormalizeSkip int // How often to normalize quaternions. Set to 0 for every step, 1 for every second etc..
-	                      // A larger value increases performance.
-	                      // If bodies tend to explode, set to a smaller value (zero to be sure nothing can go wrong).
+	quatNormalizeSkip int  // How often to normalize quaternions. Set to 0 for every step, 1 for every second etc..
+	                       // A larger value increases performance.
+	                       // If bodies tend to explode, set to a smaller value (zero to be sure nothing can go wrong).
 	quatNormalizeFast bool // Set to true to use fast quaternion normalization. It is often enough accurate to use. If bodies tend to explode, set to false.
 
 	time float32      // The wall-clock time since simulation start
@@ -50,27 +41,22 @@ type Simulation struct {
 	default_dt float32 // Default and last timestep sizes
 	dt float32      // Currently / last used timestep. Is set to -1 if not available. This value is updated before each internal step, which means that it is "fresh" inside event callbacks.
 
-
 	accumulator float32 // Time accumulator for interpolation. See http://gafferongames.com/game-physics/fix-your-timestep/
 
-	broadphase *collision.Broadphase // The broadphase algorithm to use. Default is NaiveBroadphase
-	narrowphase *Narrowphase // The narrowphase algorithm to use
-	solver solver.ISolver    // The solver algorithm to use. Default is GSSolver
+	broadphase  *Broadphase    // The broadphase algorithm to use, default is NaiveBroadphase
+	narrowphase *Narrowphase   // The narrowphase algorithm to use
+	solver      solver.ISolver // The solver algorithm to use, default is Gauss-Seidel
 
 	constraints       []constraint.IConstraint  // All constraints
 
-	materials         []*material.Material               // All added materials
-	cMaterials        []*material.ContactMaterial
-
-
+	materials         []*Material               // All added materials
+	cMaterials        []*ContactMaterial
 
 	//contactMaterialTable map[intPair]*ContactMaterial // Used to look up a ContactMaterial given two instances of Material.
 	//defaultMaterial *Material
-	defaultContactMaterial *material.ContactMaterial
-
+	defaultContactMaterial *ContactMaterial
 
 	doProfiling      bool
-	scene *core.Node
 }
 
 // NewSimulation creates and returns a pointer to a new physics simulation.
@@ -83,7 +69,7 @@ func NewSimulation(scene *core.Node) *Simulation {
 	s.scene = scene
 
 	// Set up broadphase, narrowphase, and solver
-	s.broadphase = collision.NewBroadphase()
+	s.broadphase = NewBroadphase()
 	s.narrowphase = NewNarrowphase(s)
 	s.solver = solver.NewGaussSeidel()
 
@@ -92,7 +78,7 @@ func NewSimulation(scene *core.Node) *Simulation {
 
 	//s.contactMaterialTable = make(map[intPair]*ContactMaterial)
 	//s.defaultMaterial = NewMaterial
-	s.defaultContactMaterial = material.NewContactMaterial()
+	s.defaultContactMaterial = NewContactMaterial()
 
 	return s
 }
@@ -126,13 +112,15 @@ func (s *Simulation) RemoveForceField(ff ForceField) bool {
 // AddBody adds a body to the simulation.
 func (s *Simulation) AddBody(body *object.Body, name string) {
 
-	// Do nothing if body already present
+	// Do nothing if body is already present
 	for _, existingBody := range s.bodies {
 		if existingBody == body {
 			return // Do nothing
 		}
 	}
 
+	// If there are any open/nil spots in the body slice - add the body to one of them
+	// Else, just append to the end of the slice. Either way compute the body's index in the slice.
 	var idx int
 	nilLen := len(s.nilBodies)
 	if nilLen > 0 {
@@ -141,15 +129,15 @@ func (s *Simulation) AddBody(body *object.Body, name string) {
 	} else {
 		idx = len(s.bodies)
 		s.bodies = append(s.bodies, body)
+
+		// Initialize collision matrix values up to the current index (and set the colliding flag to false)
+		s.collisionMatrix.Set(idx, idx, false)
+		s.prevCollisionMatrix.Set(idx, idx, false)
 	}
 
 	body.SetIndex(idx)
 	body.SetName(name)
 
-	// Initialize values up to the current index (and set the colliding flag to false)
-	s.collisionMatrix.Set(idx, idx, false)
-	s.prevCollisionMatrix.Set(idx, idx, false)
-
 	// TODO dispatch add-body event
 	//s.Dispatch(AddBodyEvent, BodyEvent{body})
 }
@@ -161,14 +149,11 @@ func (s *Simulation) RemoveBody(body *object.Body) bool {
 	for idx, current := range s.bodies {
 		if current == body {
 			s.bodies[idx] = nil
-
 			// TODO dispatch remove-body event
 			//s.Dispatch(AddBodyEvent, BodyEvent{body})
-
 			return true
 		}
 	}
-
 	return false
 }
 
@@ -242,12 +227,13 @@ func (s *Simulation) StepPlus(frameDelta float32, timeSinceLastCalled float32, m
 
 }
 
+// SetPaused sets the paused state of the simulation.
 func (s *Simulation) SetPaused(state bool) {
 
 	s.paused = state
 }
 
-
+// Paused returns the paused state of the simulation.
 func (s *Simulation) Paused() bool {
 
 	return s.paused
@@ -261,7 +247,7 @@ func (s *Simulation) ClearForces() {
 	}
 }
 
-// Add a constraint to the simulation.
+// AddConstraint adds a constraint to the simulation.
 func (s *Simulation) AddConstraint(c constraint.IConstraint) {
 
 	s.constraints = append(s.constraints, c)
@@ -272,18 +258,18 @@ func (s *Simulation) RemoveConstraint(c constraint.IConstraint) {
 	// TODO
 }
 
-func (s *Simulation) AddMaterial(mat *material.Material) {
+func (s *Simulation) AddMaterial(mat *Material) {
 
 	s.materials = append(s.materials, mat)
 }
 
-func (s *Simulation) RemoveMaterial(mat *material.Material) {
+func (s *Simulation) RemoveMaterial(mat *Material) {
 
 	// TODO
 }
 
 // Adds a contact material to the simulation
-func (s *Simulation) AddContactMaterial(cmat *material.ContactMaterial) {
+func (s *Simulation) AddContactMaterial(cmat *ContactMaterial) {
 
 	s.cMaterials = append(s.cMaterials, cmat)
 
@@ -292,9 +278,9 @@ func (s *Simulation) AddContactMaterial(cmat *material.ContactMaterial) {
 }
 
 // GetContactMaterial returns the contact material between the specified bodies.
-func (s *Simulation) GetContactMaterial(bodyA, bodyB *object.Body) *material.ContactMaterial {
+func (s *Simulation) GetContactMaterial(bodyA, bodyB *object.Body) *ContactMaterial {
 
-	var cm *material.ContactMaterial
+	var cm *ContactMaterial
 	// TODO
 	//if bodyA.material != nil && bodyB.material != nil {
 	//	cm = s.contactMaterialTable.get(bodyA.material.id, bodyB.material.id)
@@ -330,7 +316,9 @@ const (
 // ===========================
 
 
-// Note - this method alters the solution arrays
+// ApplySolution applies the specified solution to the bodies under simulation.
+// The solution is a set of linear and angular velocity deltas for each body.
+// This method alters the solution arrays.
 func (s *Simulation) ApplySolution(sol *solver.Solution) {
 
 	// Add results to velocity and angular velocity of bodies
@@ -358,15 +346,44 @@ func (s *Simulation) collisionMatrixTick() {
 	//s.shapeOverlapKeeper.tick()
 }
 
+func (s *Simulation) uniqueBodiesFromPairs(pairs []CollisionPair) []*object.Body {
+
+	bodiesUndergoingNarrowphase := make([]*object.Body, 0) // array of indices of s.bodies
+	for _, pair := range pairs {
+		alreadyAddedA := false
+		alreadyAddedB := false
+		for _, body := range bodiesUndergoingNarrowphase {
+			if pair.BodyA == body {
+				alreadyAddedA = true
+				if alreadyAddedB {
+					break
+				}
+			}
+			if pair.BodyB == body {
+				alreadyAddedB = true
+				if alreadyAddedA {
+					break
+				}
+			}
+		}
+		if !alreadyAddedA {
+			bodiesUndergoingNarrowphase = append(bodiesUndergoingNarrowphase, pair.BodyA)
+		}
+		if !alreadyAddedB {
+			bodiesUndergoingNarrowphase = append(bodiesUndergoingNarrowphase, pair.BodyB)
+		}
+	}
+
+	return bodiesUndergoingNarrowphase
+}
+
 // TODO read https://gafferongames.com/post/fix_your_timestep/
 func (s *Simulation) internalStep(dt float32) {
 
 	s.dt = dt
 
-	// Apply force fields and compute world normals/edges
+	// Apply force fields (only to dynamic bodies
 	for _, b := range s.bodies {
-
-		// Only apply to dynamic bodies
 		if b.BodyType() == object.Dynamic {
 			for _, ff := range s.forceFields {
 				pos := b.Position()
@@ -374,80 +391,94 @@ func (s *Simulation) internalStep(dt float32) {
 				b.ApplyForceField(&force)
 			}
 		}
-
-		// Compute for the new rotation
-		// TODO optimization: only need to compute the below for objects that will go through narrowphase
-		b.ComputeWorldFaceNormalsAndUniqueEdges()
 	}
 
-    // TODO update subsystems ?
-
-    // Find pairs of bodies that are potentially colliding
+    // Find pairs of bodies that are potentially colliding (broadphase)
 	pairs := s.broadphase.FindCollisionPairs(s.bodies)
 
-	// Remove some pairs before proceeding to narrophase based on constraints' colConn property
-	// which specified if constrained bodies should collide with one another
-    s.prunePairs(pairs) // TODO review
+	// Remove some pairs before proceeding to narrowphase based on constraints' colConn property
+	// which specifies if constrained bodies should collide with one another
+    s.prunePairs(pairs) // TODO review/implement
 
-	//
+	// Precompute world normals/edges only for convex bodies that will undergo narrowphase
+	for _, body := range s.uniqueBodiesFromPairs(pairs) {
+		if ch, ok := body.Shape().(*shape.ConvexHull); ok{
+			ch.ComputeWorldFaceNormalsAndUniqueEdges(body.Quaternion())
+		}
+	}
+
+	// Switch collision matrices (to keep track of which collisions started/ended)
     s.collisionMatrixTick()
 
-    // Generate contacts
-	s.generateContacts(pairs)
+    // Resolve collisions and generate contact and friction equations
+	contactEqs, frictionEqs := s.narrowphase.GenerateEquations(pairs)
 
-    s.emitContactEvents()
+	// Add all friction equations to solver
+	for i := 0; i < len(frictionEqs); i++ {
+		s.solver.AddEquation(frictionEqs[i])
+	}
 
-    // Wake up bodies
-    // TODO why not wake bodies up inside s.generateContacs when setting the WakeUpAfterNarrowphase flag?
-    // Maybe there we are only looking at bodies that belong to current contact equations...
-    // and need to wake up all marked bodies
-    for i := 0; i < len(s.bodies); i++ {
-        bi := s.bodies[i]
-        if bi != nil && bi.WakeUpAfterNarrowphase() {
-            bi.WakeUp()
-            bi.SetWakeUpAfterNarrowphase(false)
-        }
-    }
+	// Add all contact equations to solver (and update some things)
+	for i := 0; i < len(contactEqs); i++ {
+		s.solver.AddEquation(contactEqs[i])
+		s.updateSleepAndCollisionMatrix(contactEqs[i])
+	}
 
-    // Add user-added constraints
+    // Add all equations from user-added constraints to the solver
 	userAddedEquations := 0
     for i := 0; i < len(s.constraints); i++ {
-        c := s.constraints[i]
-        c.Update()
-        eqs := c.Equations()
+		s.constraints[i].Update()
+        eqs := s.constraints[i].Equations()
         for j := 0; j < len(eqs); j++ {
 			userAddedEquations++
             s.solver.AddEquation(eqs[j])
         }
     }
 
-	// Update solve mass for all bodies
-	// NOTE: This was originally inside the beginning of solver.Solve()
-	if len(s.frictionEqs) + len(s.contactEqs) + userAddedEquations > 0 {
+	// Emit events TODO implement
+	s.emitContactEvents()
+	// Wake up bodies
+	// TODO why not wake bodies up inside s.updateSleepAndCollisionMatrix when setting the WakeUpAfterNarrowphase flag?
+	// Maybe there we are only looking at bodies that belong to current contact equations...
+	// and need to wake up all marked bodies
+	for i := 0; i < len(s.bodies); i++ {
+		bi := s.bodies[i]
+		if bi != nil && bi.WakeUpAfterNarrowphase() {
+			bi.WakeUp()
+			bi.SetWakeUpAfterNarrowphase(false)
+		}
+	}
+
+	// If we have any equations to solve
+	if len(frictionEqs) + len(contactEqs) + userAddedEquations > 0 {
+		// Update effective mass for all bodies
 		for i := 0; i < len(s.bodies); i++ {
 			s.bodies[i].UpdateEffectiveMassProperties()
 		}
+		// Solve the constrained system
+		solution := s.solver.Solve(dt, len(s.bodies))
+		// Apply linear and angular velocity deltas to bodies
+		s.ApplySolution(solution)
+		// Clear all equations added to the solver
+		s.solver.ClearEquations()
 	}
 
-    // Solve the constrained system
-    solution := s.solver.Solve(dt, len(s.bodies))
-	s.ApplySolution(solution) // Applies velocity deltas
-	s.solver.ClearEquations()
-
-    // Apply damping, see http://code.google.com/p/bullet/issues/detail?id=74 for details
-    for i := 0; i < len(s.bodies); i++ {
-        bi := s.bodies[i]
-        if bi != nil && bi.BodyType() == object.Dynamic { // Only apply damping to dynamic bodies
-			bi.ApplyDamping(dt)
+    // Apply damping (only to dynamic bodies)
+    // See http://code.google.com/p/bullet/issues/detail?id=74 for details
+    for _, body := range s.bodies {
+        if body != nil && body.BodyType() == object.Dynamic {
+			body.ApplyDamping(dt)
         }
     }
 
     // TODO s.Dispatch(World_step_preStepEvent)
 
-	// Integrate the forces into velocities into position deltas
-    quatNormalize := true// s.stepnumber % (s.quatNormalizeSkip + 1) == 0
-    for i := 0; i < len(s.bodies); i++ {
-		s.bodies[i].Integrate(dt, quatNormalize, s.quatNormalizeFast)
+	// Integrate the forces into velocities and the velocities into position deltas for all bodies
+    // TODO future: quatNormalize := s.stepnumber % (s.quatNormalizeSkip + 1) == 0
+    for _, body := range s.bodies {
+		if body != nil {
+			body.Integrate(dt, true, s.quatNormalizeFast)
+		}
     }
     s.ClearForces()
 
@@ -469,7 +500,7 @@ func (s *Simulation) internalStep(dt float32) {
 }
 
 // TODO - REVIEW THIS
-func (s *Simulation) prunePairs(pairs []collision.Pair) []collision.Pair {
+func (s *Simulation) prunePairs(pairs []CollisionPair) []CollisionPair {
 
 	// TODO There is probably a bug here when the same body can have multiple constraints and appear in multiple pairs
 
@@ -491,7 +522,7 @@ func (s *Simulation) prunePairs(pairs []collision.Pair) []collision.Pair {
 	//}
 	//
 	//// Remove pairs
-	////var prunedPairs []Pair
+	////var prunedPairs []CollisionPair
 	//for i := range pairs {
 	//	for _, idx := range pairIdxsToRemove {
 	//		copy(pairs[i:], pairs[i+1:])
@@ -504,63 +535,47 @@ func (s *Simulation) prunePairs(pairs []collision.Pair) []collision.Pair {
 }
 
 // generateContacts
-func (s *Simulation) generateContacts(pairs []collision.Pair) {
+func (s *Simulation) updateSleepAndCollisionMatrix(contactEq *equation.Contact) {
 
-	// Find all contacts and generate contact and friction equations (narrowphase)
-	s.contactEqs, s.frictionEqs = s.narrowphase.GetContacts(pairs)
+	// Get current collision indices
+	bodyA := s.bodies[contactEq.BodyA().Index()]
+	bodyB := s.bodies[contactEq.BodyB().Index()]
 
-	// Add all friction equations to solver
-	for i := 0; i < len(s.frictionEqs); i++ {
-		s.solver.AddEquation(s.frictionEqs[i])
-	}
-
-	for k := 0; k < len(s.contactEqs); k++ {
-
-		// Current contact
-		contactEq := s.contactEqs[k]
-
-		// Get current collision indices
-		bodyA := s.bodies[contactEq.BodyA().Index()]
-		bodyB := s.bodies[contactEq.BodyB().Index()]
+	// TODO future: update equations with physical material properties
 
-		// TODO future: update equations with physical material properties
-
-		s.solver.AddEquation(contactEq)
-
-		if bodyA.AllowSleep() && bodyA.BodyType() == object.Dynamic && bodyA.SleepState() == object.Sleeping && bodyB.SleepState() == object.Awake && bodyB.BodyType() != object.Static {
-			velocityB := bodyB.Velocity()
-			angularVelocityB := bodyB.AngularVelocity()
-			speedSquaredB := velocityB.LengthSq() + angularVelocityB.LengthSq()
-			speedLimitSquaredB := math32.Pow(bodyB.SleepSpeedLimit(), 2)
-			if speedSquaredB >= speedLimitSquaredB*2 {
-				bodyA.SetWakeUpAfterNarrowphase(true)
-			}
+	if bodyA.AllowSleep() && bodyA.BodyType() == object.Dynamic && bodyA.SleepState() == object.Sleeping && bodyB.SleepState() == object.Awake && bodyB.BodyType() != object.Static {
+		velocityB := bodyB.Velocity()
+		angularVelocityB := bodyB.AngularVelocity()
+		speedSquaredB := velocityB.LengthSq() + angularVelocityB.LengthSq()
+		speedLimitSquaredB := math32.Pow(bodyB.SleepSpeedLimit(), 2)
+		if speedSquaredB >= speedLimitSquaredB*2 {
+			bodyA.SetWakeUpAfterNarrowphase(true)
 		}
+	}
 
-		if bodyB.AllowSleep() && bodyB.BodyType() == object.Dynamic && bodyB.SleepState() == object.Sleeping && bodyA.SleepState() == object.Awake && bodyA.BodyType() != object.Static {
-			velocityA := bodyA.Velocity()
-			angularVelocityA := bodyA.AngularVelocity()
-			speedSquaredA := velocityA.LengthSq() + angularVelocityA.LengthSq()
-			speedLimitSquaredA := math32.Pow(bodyA.SleepSpeedLimit(), 2)
-			if speedSquaredA >= speedLimitSquaredA*2 {
-				bodyB.SetWakeUpAfterNarrowphase(true)
-			}
+	if bodyB.AllowSleep() && bodyB.BodyType() == object.Dynamic && bodyB.SleepState() == object.Sleeping && bodyA.SleepState() == object.Awake && bodyA.BodyType() != object.Static {
+		velocityA := bodyA.Velocity()
+		angularVelocityA := bodyA.AngularVelocity()
+		speedSquaredA := velocityA.LengthSq() + angularVelocityA.LengthSq()
+		speedLimitSquaredA := math32.Pow(bodyA.SleepSpeedLimit(), 2)
+		if speedSquaredA >= speedLimitSquaredA*2 {
+			bodyB.SetWakeUpAfterNarrowphase(true)
 		}
+	}
 
-		// Now we know that i and j are in contact. Set collision matrix state
-		s.collisionMatrix.Set(bodyA.Index(), bodyB.Index(), true)
-
-		if s.prevCollisionMatrix.Get(bodyA.Index(), bodyB.Index()) == false {
-			// First contact!
-			bodyA.Dispatch(CollisionEv, &CollideEvent{bodyB, contactEq})
-			bodyB.Dispatch(CollisionEv, &CollideEvent{bodyA, contactEq})
-		}
+	// Now we know that i and j are in contact. Set collision matrix state
+	s.collisionMatrix.Set(bodyA.Index(), bodyB.Index(), true)
 
-		// TODO this is only for events
-		//s.bodyOverlapKeeper.set(bodyA.id, bodyB.id)
-		//s.shapeOverlapKeeper.set(si.id, sj.id)
+	if s.prevCollisionMatrix.Get(bodyA.Index(), bodyB.Index()) == false {
+		// First contact!
+		bodyA.Dispatch(CollisionEv, &CollideEvent{bodyB, contactEq})
+		bodyB.Dispatch(CollisionEv, &CollideEvent{bodyA, contactEq})
 	}
 
+	// TODO this is only for events
+	//s.bodyOverlapKeeper.set(bodyA.id, bodyB.id)
+	//s.shapeOverlapKeeper.set(si.id, sj.id)
+
 }
 
 func (s *Simulation) emitContactEvents() {