physics dev (narrowphase, broadphase, simulation, body, ...)

geometry/geometry.go

@@ -430,6 +430,12 @@ func (g *Geometry) ProjectOntoAxis(localAxis *math32.Vector3) (float32, float32)
 	return max, min
 }
 
+// TODO:
+// https://stackoverflow.com/questions/21640545/how-to-check-for-convexity-of-a-3d-mesh
+// func (g *Geometry) IsConvex() bool {
+//
+// {
+
 // ApplyMatrix multiplies each of the geometry position vertices
 // by the specified matrix and apply the correspondent normal
 // transform matrix to the geometry normal vectors.

math32/vector3.go

@@ -666,3 +666,15 @@ func (v *Vector3) RandomTangents() (*Vector3, *Vector3) {
 
 	return t1, t2
 }
+
+// TODO: implement similar methods for Vector2 and Vector4
+// AlmostEquals returns whether the vector is almost equal to another vector within the specified tolerance.
+func (v *Vector3) AlmostEquals(other *Vector3, tolerance float32) bool {
+
+	if (v.X - other.X < tolerance) &&
+		(v.Y - other.Y < tolerance) &&
+		(v.Z - other.Z < tolerance) {
+			return true
+	}
+	return false
+}

physics/collision/broadphase.go

@@ -0,0 +1,54 @@
+// 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/physics/object"
+)
+
+// Broadphase is the base class for broadphase implementations.
+type Broadphase struct {}
+
+type Pair struct {
+	BodyA *object.Body
+	BodyB *object.Body
+}
+
+// NewBroadphase creates and returns a pointer to a new Broadphase.
+func NewBroadphase() *Broadphase {
+
+	b := new(Broadphase)
+	return b
+}
+
+// FindCollisionPairs (naive implementation)
+func (b *Broadphase) FindCollisionPairs(objects []*object.Body) []Pair {
+
+	pairs := make([]Pair,0)
+
+	for _, bodyA := range objects {
+		for _, bodyB := range objects {
+			if b.NeedTest(bodyA, bodyB) {
+				BBa := bodyA.BoundingBox()
+				BBb := bodyB.BoundingBox()
+				if BBa.IsIntersectionBox(&BBb) {
+					pairs = append(pairs, Pair{bodyA, bodyB})
+				}
+			}
+		}
+	}
+
+	return pairs
+}
+
+func (b *Broadphase) NeedTest(bodyA, bodyB *object.Body) bool {
+
+	if !bodyA.CollidableWith(bodyB) || (bodyA.Sleeping() && bodyB.Sleeping()) {
+		return false
+	}
+
+	return true
+}

physics/collision/matrix.go

@@ -6,16 +6,12 @@
 package collision
 
 // Matrix is a triangular collision matrix indicating which pairs of bodies are colliding.
-type Matrix struct {
-	col [][]bool
-}
+type Matrix [][]bool
 
 // NewMatrix creates and returns a pointer to a new collision Matrix.
-func NewMatrix() *Matrix {
+func NewMatrix() Matrix {
 
-	m := new(Matrix)
-	m.col = make([][]bool, 0)
-	return m
+	return make([][]bool, 0)
 }
 
 // Set sets whether i and j are colliding.
@@ -29,21 +25,21 @@ func (m *Matrix) Set(i, j int, val bool) {
 		s = j
 		l = i
 	}
-	diff := s + 1 - len(m.col)
+	diff := s + 1 - len(*m)
 	if diff > 0 {
 		for i := 0; i < diff; i++ {
-			m.col = append(m.col, make([]bool,0))
+			*m = append(*m, make([]bool, 0))
 		}
 	}
-	for idx := range m.col {
-		diff = l + 1 - len(m.col[idx]) - idx
+	for idx := range *m {
+		diff = l + 1 - len((*m)[idx]) - idx
 		if diff > 0 {
 			for i := 0; i < diff; i++ {
-				m.col[idx] = append(m.col[idx], false)
+				(*m)[idx] = append((*m)[idx], false)
 			}
 		}
 	}
-	m.col[s][l-s] = val
+	(*m)[s][l-s] = val
 }
 
 // Get returns whether i and j are colliding.
@@ -57,5 +53,11 @@ func (m *Matrix) Get(i, j int) bool {
 		s = j
 		l = i
 	}
-	return m.col[s][l-s]
+	return (*m)[s][l-s]
+}
+
+// Reset clears all values.
+func (m *Matrix) Reset() {
+
+	*m = make([][]bool, 0)
 }

physics/constraint/conetwist.go

@@ -51,8 +51,8 @@ func NewConeTwist(bodyA, bodyB IBody, pivotA, pivotB, axisA, axisB *math32.Vecto
 	ctc.twistEq.SetMaxForce(0)
 	ctc.twistEq.SetMinForce(-maxForce)
 
-	ctc.AddEquation(&ctc.coneEq.Equation)
-	ctc.AddEquation(&ctc.twistEq.Equation)
+	ctc.AddEquation(ctc.coneEq)
+	ctc.AddEquation(ctc.twistEq)
 
 	return ctc
 }

physics/constraint/constraint.go

@@ -21,30 +21,33 @@ type IBody interface {
 
 type IConstraint interface {
 	Update() // Update all the equations with data.
+	Equations() []equation.IEquation
+	CollideConnected() bool
+	BodyA() IBody
+	BodyB() IBody
 }
 
 // Constraint base struct.
 type Constraint struct {
-	equations []*equation.Equation // Equations to be solved in this constraint
+	equations []equation.IEquation // Equations to be solved in this constraint
 	bodyA     IBody
 	bodyB     IBody
-	//id
-	collideConnected bool // Set to true if you want the bodies to collide when they are connected.
+	colConn   bool // Set to true if you want the bodies to collide when they are connected.
 }
 
 // NewConstraint creates and returns a pointer to a new Constraint object.
-func NewConstraint(bodyA, bodyB IBody, collideConnected, wakeUpBodies bool) *Constraint {
+//func NewConstraint(bodyA, bodyB IBody, colConn, wakeUpBodies bool) *Constraint {
+//
+//	c := new(Constraint)
+//	c.initialize(bodyA, bodyB, colConn, wakeUpBodies)
+//	return c
+//}
 
-	c := new(Constraint)
-	c.initialize(bodyA, bodyB, collideConnected, wakeUpBodies)
-	return c
-}
-
-func (c *Constraint) initialize(bodyA, bodyB IBody, collideConnected, wakeUpBodies bool) {
+func (c *Constraint) initialize(bodyA, bodyB IBody, colConn, wakeUpBodies bool) {
 
 	c.bodyA = bodyA
 	c.bodyB = bodyB
-	c.collideConnected = collideConnected // true
+	c.colConn = colConn // true
 
 	if wakeUpBodies { // true
 		if bodyA != nil {
@@ -57,11 +60,32 @@ func (c *Constraint) initialize(bodyA, bodyB IBody, collideConnected, wakeUpBodi
 }
 
 // AddEquation adds an equation to the constraint.
-func (c *Constraint) AddEquation(eq *equation.Equation) {
+func (c *Constraint) AddEquation(eq equation.IEquation) {
 
 	c.equations = append(c.equations, eq)
 }
 
+// Equations returns the constraint's equations.
+func (c *Constraint) Equations() []equation.IEquation {
+
+	return c.equations
+}
+
+func (c *Constraint) CollideConnected() bool {
+
+	return c.colConn
+}
+
+func (c *Constraint) BodyA() IBody {
+
+	return c.bodyA
+}
+
+func (c *Constraint) BodyB() IBody {
+
+	return c.bodyB
+}
+
 // SetEnable sets the enabled flag of the constraint equations.
 func (c *Constraint) SetEnabled(state bool) {
 

physics/constraint/distance.go

@@ -28,8 +28,7 @@ func NewDistance(bodyA, bodyB IBody, distance, maxForce float32) *Distance {
 	dc.distance = distance
 
 	dc.equation = equation.NewContact(bodyA, bodyB, -maxForce, maxForce) // Make it bidirectional
-	dc.equations = append(dc.equations, &dc.equation.Equation)
-
+	dc.AddEquation(dc.equation)
 
 	return dc
 }

physics/constraint/hinge.go

@@ -19,7 +19,6 @@ type Hinge struct {
 	rotEq1  *equation.Rotational
 	rotEq2  *equation.Rotational
 	motorEq *equation.RotationalMotor
-
 }
 
 // NewHinge creates and returns a pointer to a new Hinge constraint object.
@@ -39,9 +38,9 @@ func NewHinge(bodyA, bodyB IBody, pivotA, pivotB, axisA, axisB *math32.Vector3,
 	hc.motorEq = equation.NewRotationalMotor(bodyA, bodyB, maxForce)
 	hc.motorEq.SetEnabled(false) // Not enabled by default
 
-	hc.equations = append(hc.equations, &hc.rotEq1.Equation)
-	hc.equations = append(hc.equations, &hc.rotEq2.Equation)
-	hc.equations = append(hc.equations, &hc.motorEq.Equation)
+	hc.AddEquation(hc.rotEq1)
+	hc.AddEquation(hc.rotEq2)
+	hc.AddEquation(hc.motorEq)
 
 	return hc
 }

physics/constraint/lock.go

@@ -61,9 +61,9 @@ func NewLock(bodyA, bodyB IBody, maxForce float32) *Lock {
 	lc.rotEq2 = equation.NewRotational(bodyA, bodyB, maxForce)
 	lc.rotEq3 = equation.NewRotational(bodyA, bodyB, maxForce)
 
-	lc.equations = append(lc.equations, &lc.rotEq1.Equation)
-	lc.equations = append(lc.equations, &lc.rotEq2.Equation)
-	lc.equations = append(lc.equations, &lc.rotEq3.Equation)
+	lc.AddEquation(lc.rotEq1)
+	lc.AddEquation(lc.rotEq2)
+	lc.AddEquation(lc.rotEq3)
 
 	return lc
 }

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
+	ce.restitution = 0.3
 	ce.rA = &math32.Vector3{0,0,0}
 	ce.rB = &math32.Vector3{0,0,0}
 	ce.nA = &math32.Vector3{0,0,0}
@@ -34,6 +34,16 @@ func NewContact(bodyA, bodyB IBody, minForce, maxForce float32) *Contact {
 	return ce
 }
 
+func (ce *Contact) SetRestitution(r float32) {
+
+	ce.restitution = r
+}
+
+func (ce *Contact) Restitution() float32 {
+
+	return ce.restitution
+}
+
 func (ce *Contact) SetNormal(newNormal *math32.Vector3)  {
 
 	ce.nA = newNormal
@@ -44,9 +54,9 @@ func (ce *Contact) Normal() math32.Vector3 {
 	return *ce.nA
 }
 
-func (ce *Contact) SetRA(newRi *math32.Vector3)  {
+func (ce *Contact) SetRA(newRa *math32.Vector3)  {
 
-	ce.rA = newRi
+	ce.rA = newRa
 }
 
 func (ce *Contact) RA() math32.Vector3 {
@@ -54,9 +64,9 @@ func (ce *Contact) RA() math32.Vector3 {
 	return *ce.rA
 }
 
-func (ce *Contact) SetRB(newRj *math32.Vector3)  {
+func (ce *Contact) SetRB(newRb *math32.Vector3)  {
 
-	ce.rB = newRj
+	ce.rB = newRb
 }
 
 func (ce *Contact) RB() math32.Vector3 {

physics/equation/equation.go

@@ -20,8 +20,26 @@ type IBody interface {
 	AngularVelocity() math32.Vector3
 	Force() math32.Vector3
 	Torque() math32.Vector3
-	InvMassSolve() float32
-	InvInertiaWorldSolve() *math32.Matrix3
+	InvMassEff() float32
+	InvRotInertiaWorldEff() *math32.Matrix3
+}
+
+// IEquation is the interface type for all equations types.
+type IEquation interface {
+	SetBodyA(IBody)
+	BodyA() IBody
+	SetBodyB(IBody)
+	BodyB() IBody
+	JeA() JacobianElement
+	JeB() JacobianElement
+	SetEnabled(state bool)
+	Enabled() bool
+	MinForce() float32
+	MaxForce() float32
+	Eps() float32
+	SetMultiplier(multiplier float32)
+	ComputeB(h float32) float32
+	ComputeC() float32
 }
 
 // Equation is a SPOOK constraint equation.
@@ -63,7 +81,12 @@ func (e *Equation) initialize(bi, bj IBody, minForce, maxForce float32) {
 	e.multiplier = 0
 
 	// Set typical spook params
-	e.SetSpookParams(1e7, 4, 1/60)
+	e.SetSpookParams(1e7, 3, 1/60)
+}
+
+func (e *Equation) SetBodyA(ibody IBody) {
+
+	e.bA = ibody
 }
 
 func (e *Equation) BodyA() IBody {
@@ -71,6 +94,11 @@ func (e *Equation) BodyA() IBody {
 	return e.bA
 }
 
+func (e *Equation) SetBodyB(ibody IBody) {
+
+	e.bB = ibody
+}
+
 func (e *Equation) BodyB() IBody {
 
 	return e.bB
@@ -110,7 +138,7 @@ func (e *Equation) MaxForce() float32 {
 	return e.maxForce
 }
 
-// TODO
+// Returns epsilon - the regularization constant which is multiplied by the identity matrix.
 func (e *Equation) Eps() float32 {
 
 	return e.eps
@@ -183,14 +211,14 @@ func (e *Equation) ComputeGiMf() float32 {
 	forceA := e.bA.Force()
 	forceB := e.bB.Force()
 
-	iMfA := forceA.MultiplyScalar(e.bA.InvMassSolve())
-	iMfB := forceB.MultiplyScalar(e.bB.InvMassSolve())
+	iMfA := forceA.MultiplyScalar(e.bA.InvMassEff())
+	iMfB := forceB.MultiplyScalar(e.bB.InvMassEff())
 
 	torqueA := e.bA.Torque()
 	torqueB := e.bB.Torque()
 
-	invIiTaui := torqueA.ApplyMatrix3(e.bA.InvInertiaWorldSolve())
-	invIjTauj := torqueB.ApplyMatrix3(e.bB.InvInertiaWorldSolve())
+	invIiTaui := torqueA.ApplyMatrix3(e.bA.InvRotInertiaWorldEff())
+	invIjTauj := torqueB.ApplyMatrix3(e.bB.InvRotInertiaWorldEff())
 
 	return e.jeA.MultiplyVectors(iMfA, invIiTaui) + e.jeB.MultiplyVectors(iMfB, invIjTauj)
 }
@@ -203,9 +231,9 @@ func (e *Equation) ComputeGiMGt() float32 {
 	rotAcopy := e.jeA.Rotational()
 	rotBcopy := e.jeB.Rotational()
 
-	result := e.bA.InvMassSolve() + e.bB.InvMassSolve()
-	result += rotA.ApplyMatrix3(e.bA.InvInertiaWorldSolve()).Dot(&rotAcopy)
-	result += rotB.ApplyMatrix3(e.bB.InvInertiaWorldSolve()).Dot(&rotBcopy)
+	result := e.bA.InvMassEff() + e.bB.InvMassEff()
+	result += rotA.ApplyMatrix3(e.bA.InvRotInertiaWorldEff()).Dot(&rotAcopy)
+	result += rotB.ApplyMatrix3(e.bB.InvRotInertiaWorldEff()).Dot(&rotBcopy)
 
 	return result
 }

physics/equation/friction.go

@@ -31,6 +31,36 @@ func NewFriction(bodyA, bodyB IBody, slipForce float32) *Friction {
 	return fe
 }
 
+func (fe *Friction) SetTangent(newTangent *math32.Vector3)  {
+
+	fe.t = newTangent
+}
+
+func (fe *Friction) Tangent() math32.Vector3 {
+
+	return *fe.t
+}
+
+func (fe *Friction) SetRA(newRa *math32.Vector3)  {
+
+	fe.rA = newRa
+}
+
+func (fe *Friction) RA() math32.Vector3 {
+
+	return *fe.rA
+}
+
+func (fe *Friction) SetRB(newRb *math32.Vector3)  {
+
+	fe.rB = newRb
+}
+
+func (fe *Friction) RB() math32.Vector3 {
+
+	return *fe.rB
+}
+
 // ComputeB
 func (fe *Friction) ComputeB(h float32) float32 {
 

physics/material.go

@@ -3,7 +3,7 @@
 // license that can be found in the LICENSE file.
 
 // Package physics implements a basic physics engine.
-package physics
+package material
 
 type Material struct {
 	name        string
@@ -21,3 +21,23 @@ type ContactMaterial struct {
 	frictionEquationStiffness  float32
 	frictionEquationRelaxation float32
 }
+
+func NewContactMaterial() *ContactMaterial {
+
+	cm := new(ContactMaterial)
+	cm.friction = 0.3
+	cm.restitution = 0.3
+	cm.contactEquationStiffness = 1e7
+	cm.contactEquationRelaxation = 3
+	cm.frictionEquationStiffness = 1e7
+	cm.frictionEquationRelaxation = 3
+	return cm
+}
+
+
+//type intPair struct {
+//	i int
+//	j int
+//}
+
+//type ContactMaterialTable map[intPair]*ContactMaterial

physics/narrowphase.go

@@ -0,0 +1,667 @@
+// 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/physics/object"
+	"github.com/g3n/engine/physics/collision"
+	"github.com/g3n/engine/physics/equation"
+	"github.com/g3n/engine/math32"
+	"github.com/g3n/engine/physics/material"
+)
+
+// Narrowphase
+type Narrowphase struct {
+	simulation *Simulation
+	currentContactMaterial *material.ContactMaterial
+
+	enableFrictionReduction bool // If true friction is computed as average
+}
+
+type Pair struct {
+	bodyA *object.Body
+	bodyB *object.Body
+}
+
+// NewNarrowphase creates and returns a pointer to a new Narrowphase.
+func NewNarrowphase(simulation *Simulation) *Narrowphase {
+
+	n := new(Narrowphase)
+	n.simulation = simulation
+	n.enableFrictionReduction = true
+	return n
+}
+
+
+func (n *Narrowphase) GetContacts(pairs []collision.Pair) ([]*equation.Contact, []*equation.Friction) {
+
+	allContactEqs := make([]*equation.Contact, 0)
+	allFrictionEqs := make([]*equation.Friction, 0)
+
+	for k := 0; k < len(pairs); k++ {
+
+		// Get current collision bodies
+		bodyA := pairs[k].BodyA
+		bodyB := pairs[k].BodyB
+
+		bodyTypeA := bodyA.BodyType()
+		bodyTypeB := bodyB.BodyType()
+
+		// For now these collisions are ignored
+		// TODO future: just want to check for collision (in order to dispatch events) and not create equations
+		justTest := (bodyTypeA == object.Kinematic) && (bodyTypeB == object.Static) ||
+				    (bodyTypeA == object.Static)    && (bodyTypeB == object.Kinematic) ||
+				    (bodyTypeA == object.Kinematic) && (bodyTypeB == object.Kinematic)
+
+		// Get contacts
+		if !justTest {
+			_, contactEqs, frictionEqs := n.Resolve(bodyA, bodyB)
+			allContactEqs = append(allContactEqs, contactEqs...)
+			allFrictionEqs = append(allFrictionEqs, frictionEqs...)
+		}
+   }
+
+   return allContactEqs, allFrictionEqs
+}
+
+// Convex - Convex collision detection
+//func (n *Narrowphase) Resolve(si,sj,xi,xj,qi,qj,bi,bj,rsi,rsj,justTest) {
+func (n *Narrowphase) Resolve(bodyA, bodyB *object.Body) (bool, []*equation.Contact, []*equation.Friction) {
+
+	contactEqs := make([]*equation.Contact, 0)
+	frictionEqs := make([]*equation.Friction, 0)
+
+	// Check if colliding and find penetration axis
+	penetrating, penAxis := n.FindPenetrationAxis(bodyA, bodyB)
+
+	if penetrating {
+		// Colliding! Find contacts.
+		contacts := n.ClipAgainstHull(bodyA, bodyB, &penAxis, -100, 100)
+
+		posA := bodyA.Position()
+		posB := bodyB.Position()
+
+		for j := 0; j < len(contacts); j++ {
+
+			contact := contacts[j]
+
+			// Create contact equation and append it
+			contactEq := equation.NewContact(bodyA, bodyB, 0, 1e6)
+			contactEq.SetEnabled(bodyA.CollisionResponse() && bodyB.CollisionResponse())
+			contactEq.SetNormal(penAxis.Negate())
+			contactEq.SetRA(contact.Normal.Negate().MultiplyScalar(contact.Depth).Add(&contact.Point).Sub(&posA))
+			contactEq.SetRB(contact.Point.Clone().Sub(&posB))
+			contactEqs = append(contactEqs, contactEq)
+
+			// If enableFrictionReduction is true then skip creating friction equations for individual contacts
+			// We will create average friction equations later based on all contacts
+			if !n.enableFrictionReduction {
+				fEq1, fEq2 := n.createFrictionEquationsFromContact(contactEq)
+				frictionEqs = append(frictionEqs, fEq1, fEq2)
+			}
+		}
+
+		// If enableFrictionReduction is true then we skipped creating friction equations for individual contacts
+		// We now want to create average friction equations based on all contact points.
+		// If we only have one contact however, then friction is small and we don't need to create the friction equations at all.
+		if n.enableFrictionReduction && len(contactEqs) > 1 {
+			fEq1, fEq2 := n.createFrictionFromAverage(contactEqs)
+			frictionEqs = append(frictionEqs, fEq1, fEq2)
+		}
+	}
+
+	return false, contactEqs, frictionEqs
+}
+
+func (n *Narrowphase) createFrictionEquationsFromContact(contactEquation *equation.Contact) (*equation.Friction, *equation.Friction) { //}, outArray) bool {
+
+	bodyA := n.simulation.bodies[contactEquation.BodyA().Index()]
+	bodyB := n.simulation.bodies[contactEquation.BodyB().Index()]
+
+	// TODO
+	// friction = n.currentContactMaterial.friction
+	// if materials are defined then override: friction = matA.friction * matB.friction
+	//var mug = friction * world.gravity.length()
+	//var reducedMass = bodyA.InvMass() + bodyB.InvMass()
+	//if reducedMass > 0 {
+	//	reducedMass = 1/reducedMass
+	//}
+	slipForce := float32(0.5) //mug*reducedMass
+
+	fricEq1 := equation.NewFriction(bodyA, bodyB, slipForce)
+	fricEq2 := equation.NewFriction(bodyA, bodyB, slipForce)
+
+	// Copy over the relative vectors
+	cRA := contactEquation.RA()
+	cRB := contactEquation.RB()
+	fricEq1.SetRA(&cRA)
+	fricEq1.SetRB(&cRB)
+	fricEq2.SetRA(&cRA)
+	fricEq2.SetRB(&cRB)
+
+	// Construct tangents
+	cNormal := contactEquation.Normal()
+	t1, t2 := cNormal.RandomTangents()
+	fricEq1.SetTangent(t1)
+	fricEq2.SetTangent(t2)
+
+	// Copy enabled state
+	cEnabled := contactEquation.Enabled()
+	fricEq1.SetEnabled(cEnabled)
+	fricEq2.SetEnabled(cEnabled)
+
+	return fricEq1, fricEq2
+}
+
+func (n *Narrowphase) createFrictionFromAverage(contactEqs []*equation.Contact) (*equation.Friction, *equation.Friction) {
+
+	// The last contactEquation
+	lastContactEq := contactEqs[len(contactEqs)-1]
+
+	// Create a friction equation based on the last contact (we will modify it to take into account all contacts)
+	fEq1, fEq2 := n.createFrictionEquationsFromContact(lastContactEq)
+	if (fEq1 == nil && fEq2 == nil) || len(contactEqs) == 1 {
+		return fEq1, fEq2
+	}
+
+	averageNormal := math32.NewVec3()
+	averageContactPointA := math32.NewVec3()
+	averageContactPointB := math32.NewVec3()
+
+	bodyA := lastContactEq.BodyA()
+	//bodyB := lastContactEq.BodyB()
+	normal := lastContactEq.Normal()
+	rA := lastContactEq.RA()
+	rB := lastContactEq.RB()
+
+	for _, cEq := range contactEqs {
+		if cEq.BodyA() != bodyA {
+			averageNormal.Add(&normal)
+			averageContactPointA.Add(&rA)
+			averageContactPointB.Add(&rB)
+		} else {
+			averageNormal.Sub(&normal)
+			averageContactPointA.Add(&rB)
+			averageContactPointB.Add(&rA)
+		}
+	}
+
+	invNumContacts := float32(1) / float32(len(contactEqs))
+
+	averageContactPointA.MultiplyScalar(invNumContacts)
+	averageContactPointB.MultiplyScalar(invNumContacts)
+
+	// Should be the same for both friction equations
+	fEq1.SetRA(averageContactPointA)
+	fEq1.SetRB(averageContactPointB)
+	fEq2.SetRA(averageContactPointA)
+	fEq2.SetRB(averageContactPointB)
+
+	// Set tangents
+	averageNormal.Normalize()
+	t1, t2 := averageNormal.RandomTangents()
+	fEq1.SetTangent(t1)
+	fEq2.SetTangent(t2)
+
+	return fEq1, fEq2
+}
+
+
+//
+// Penetration Axis =============================================
+//
+
+// FindSeparatingAxis between two convex hulls
+// Returns false if a separation is found, else true
+//@param {Vec3} target The target vector to save the axis in
+func (n *Narrowphase) FindPenetrationAxis(bodyA, bodyB *object.Body) (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 := bodyA.WorldFaceNormals()
+	worldFaceNormalsB := bodyB.WorldFaceNormals()
+
+	// Check world normals of body A
+	for _, worldFaceNormal := range worldFaceNormalsA {
+		// Check whether the face is colliding with geomB
+		penetrating, depth = n.TestPenetrationAxis(&worldFaceNormal, bodyA, bodyB)
+		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 = n.TestPenetrationAxis(&worldFaceNormal, bodyA, bodyB)
+		if !penetrating {
+			return false, penetrationAxis // penetrationAxis doesn't matter since not penetrating
+		}
+		if depth < depthMin {
+			depthMin = depth
+			penetrationAxis.Copy(&worldFaceNormal)
+		}
+	}
+
+	worldUniqueEdgesA := bodyA.WorldUniqueEdges()
+	worldUniqueEdgesB := bodyB.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 = n.TestPenetrationAxis(edgeCross, bodyA, bodyB)
+				if !penetrating {
+					return false, penetrationAxis
+				}
+				if depth < depthMin {
+					depthMin = depth
+					penetrationAxis.Copy(edgeCross)
+				}
+			}
+		}
+	}
+
+	posA := bodyA.Position()
+	posB := bodyB.Position()
+
+	deltaC := math32.NewVec3().SubVectors(&posB, &posA)
+   	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 (n *Narrowphase) TestPenetrationAxis(worldAxis *math32.Vector3, bodyA, bodyB *object.Body) (bool, float32) {
+
+	maxA, minA := n.ProjectOntoWorldAxis(bodyA, worldAxis)
+	maxB, minB := n.ProjectOntoWorldAxis(bodyB, worldAxis)
+
+	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 (n *Narrowphase) ProjectOntoWorldAxis(body *object.Body, axis *math32.Vector3) (float32, float32) {
+
+	// Transform the axis to local
+	quatConj := body.Quaternion().Conjugate()
+	localAxis := axis.Clone().ApplyQuaternion(quatConj)
+	max, min := body.GetGeometry().ProjectOntoAxis(localAxis)
+
+	// Offset to obtain values relative to world origin
+	bodyPos := body.Position()
+	localOrigin := math32.NewVec3().Sub(&bodyPos).ApplyQuaternion(quatConj)
+	add := localOrigin.Dot(localAxis)
+	min -= add
+	max -= add
+
+	return max, min
+}
+
+//
+// Contact Finding =============================================
+//
+
+// Contact describes a contact point.
+type Contact struct {
+	Point  math32.Vector3
+	Normal math32.Vector3
+	Depth  float32
+}
+
+//{array} result The an array of contact point objects, see clipFaceAgainstHull
+func (n *Narrowphase) ClipAgainstHull(bodyA, bodyB *object.Body, penAxis *math32.Vector3, minDist, maxDist float32) []Contact {
+
+
+	// Find face of B that is closest (i.e. that is most aligned with the penetration axis)
+	closestFaceBidx := -1
+	dmax := math32.Inf(-1)
+	worldFaceNormalsB := bodyB.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(penAxis)
+		if d > dmax {
+			dmax = d
+			closestFaceBidx = i
+		}
+	}
+
+	// If found a closest face (TODO is this check necessary?)
+	//if closestFaceBidx >= 0 {
+
+	// Copy and transform face vertices to world coordinates
+	faces := bodyB.Faces()
+	worldClosestFaceB := n.WorldFace(faces[closestFaceBidx], bodyB)
+
+	return n.ClipFaceAgainstHull(penAxis, bodyA, worldClosestFaceB, minDist, maxDist)
+	//}
+}
+
+func (n *Narrowphase) WorldFace(face [3]math32.Vector3, body *object.Body) [3]math32.Vector3 {
+
+	var result [3]math32.Vector3
+	result[0] = face[0]
+	result[1] = face[1]
+	result[2] = face[2]
+
+	pos := body.Position()
+	result[0].ApplyQuaternion(body.Quaternion()).Add(&pos)
+	result[1].ApplyQuaternion(body.Quaternion()).Add(&pos)
+	result[2].ApplyQuaternion(body.Quaternion()).Add(&pos)
+	return result
+}
+
+func (n *Narrowphase) WorldFaceNormal(normal *math32.Vector3, body *object.Body) math32.Vector3 {
+
+	pos := body.Position()
+	result := normal.Clone().ApplyQuaternion(body.Quaternion()).Add(&pos)
+	return *result
+}
+
+// TODO move to geometry ?
+// 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 (n *Narrowphase) ClipFaceAgainstHull(penAxis *math32.Vector3, bodyA *object.Body, worldClosestFaceB [3]math32.Vector3, minDist, maxDist float32) []Contact {
+
+	//faceANormalWS := cfah_faceANormalWS
+	//edge0 := cfah_edge0
+	//WorldEdge0 := cfah_WorldEdge0
+	//worldPlaneAnormal1 := cfah_worldPlaneAnormal1
+	//planeNormalWS1 := cfah_planeNormalWS1
+	//worldA1 := cfah_worldA1
+	//localPlaneNormal := cfah_localPlaneNormal
+	//planeNormalWS := cfah_planeNormalWS
+	//
+	//worldVertsB2 := []
+	//pVtxIn := worldVertsB1
+	//pVtxOut := worldVertsB2
+
+	contacts := make([]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 := bodyA.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: ",closestFaceA);
+
+	// Get the face and construct connected faces
+	facesA := bodyA.Faces()
+	closestFaceA := n.WorldFace(facesA[closestFaceAidx], bodyA)
+	connectedFaces := make([]int, 0) // indexes of the connected faces
+	for faceIdx := 0; faceIdx < len(facesA); faceIdx++ {
+		// Skip closestFaceA
+		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
+			}
+	    }
+	}
+
+	// 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
+	posA := bodyA.Position()
+	quatA := bodyA.Quaternion()
+	// 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)
+		n.ClipFaceAgainstPlane(worldClosestFaceB, &connFaceNormal, planeDelta)
+	}
+
+	//console.log("Resulting points after clip:",pVtxIn);
+
+	closestFaceAnormal := worldFaceNormalsA[closestFaceAidx]
+	worldFirstVertex := closestFaceA[0].Clone().ApplyQuaternion(quatA).Add(&posA)
+	planeDelta := - worldFirstVertex.Dot(&closestFaceAnormal)
+	planeEqWS := planeDelta - closestFaceAnormal.Dot(&posA)
+
+	for _, vertex := range worldClosestFaceB {
+		depth := closestFaceAnormal.Dot(&vertex) + planeEqWS // TODO verify MATH! Depth should be negative when penetrating (according to cannon.js)
+		// Cap distance
+		if depth <= minDist {
+			depth = minDist
+		}
+		if depth <= maxDist {
+			if depth <= 0 {
+				contacts = append(contacts, Contact{
+					Point: vertex,
+					Normal: closestFaceAnormal,
+					Depth: depth,
+				})
+			}
+		}
+
+	}
+
+	return contacts
+}
+
+
+// Clip a face in a hull against the back of a plane.
+// @param {Number} planeConstant The constant in the mathematical plane equation
+func (n *Narrowphase) ClipFaceAgainstPlane(face [3]math32.Vector3, planeNormal *math32.Vector3, planeConstant float32) []math32.Vector3 {
+
+	// inVertices are the verts making up the face of hullB
+	var outVertices []math32.Vector3
+
+	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 {
+	    	var newv *math32.Vector3
+	        if dotLast < 0 { // Start < 0, end < 0, so output lastVertex
+	            newv = lastVertex.Clone()
+	        } else { // Start < 0, end >= 0, so output intersection
+	            newv = firstVertex.Clone().Lerp(&lastVertex, dotFirst / (dotFirst - dotLast))
+	        }
+			outVertices = append(outVertices, *newv)
+	    } else {
+	        if dotLast < 0 { // Start >= 0, end < 0 so output intersection and end
+	            newv := firstVertex.Clone().Lerp(&lastVertex, dotFirst / (dotFirst - dotLast))
+				outVertices = append(outVertices, *newv, lastVertex)
+	        }
+	    }
+	    firstVertex = lastVertex
+		dotFirst = dotLast
+	}
+
+	return outVertices
+}
+
+//// TODO ?
+//func (n *Narrowphase) GetAveragePointLocal(target) {
+//
+//	target = target || new Vec3()
+//   n := this.vertices.length
+//   verts := this.vertices
+//   for i := 0; i < n; i++ {
+//       target.vadd(verts[i],target)
+//   }
+//   target.mult(1/n,target)
+//   return target
+//}
+//
+//
+//// Checks whether p is inside the polyhedra. Must be in local coords.
+//// The point lies outside of the convex hull of the other points if and only if
+//// the direction of all the vectors from it to those other points are on less than one half of a sphere around it.
+//// p is A point given in local coordinates
+//func (n *Narrowphase) PointIsInside(p) {
+//
+//	verts := this.vertices
+//	faces := this.faces
+//	normals := this.faceNormals
+//   positiveResult := null
+//   N := this.faces.length
+//   pointInside := ConvexPolyhedron_pointIsInside
+//   this.getAveragePointLocal(pointInside)
+//   for i := 0; i < N; i++ {
+//       numVertices := this.faces[i].length
+//       n := normals[i]
+//       v := verts[faces[i][0]] // We only need one point in the face
+//
+//       // This dot product determines which side of the edge the point is
+//       vToP := ConvexPolyhedron_vToP
+//       p.vsub(v,vToP)
+//       r1 := n.dot(vToP)
+//
+//       vToPointInside := ConvexPolyhedron_vToPointInside
+//       pointInside.vsub(v,vToPointInside)
+//       r2 := n.dot(vToPointInside)
+//
+//       if (r1<0 && r2>0) || (r1>0 && r2<0) {
+//           return false // Encountered some other sign. Exit.
+//       }
+//   }
+//
+//   // If we got here, all dot products were of the same sign.
+//   return positiveResult ? 1 : -1
+//}
+
+// TODO
+//func (n *Narrowphase) planevConvex(
+//	planeShape,
+//	convexShape,
+//	planePosition,
+//	convexPosition,
+//	planeQuat,
+//	convexQuat,
+//	planeBody,
+//	convexBody,
+//	si,
+//	sj,
+//	justTest) {
+//
+//	// Simply return the points behind the plane.
+//    worldVertex := planeConvex_v
+//    worldNormal := planeConvex_normal
+//    worldNormal.set(0,0,1)
+//    planeQuat.vmult(worldNormal,worldNormal) // Turn normal according to plane orientation
+//
+//    var numContacts = 0
+//    var relpos = planeConvex_relpos
+//    for i := 0; i < len(convexShape.vertices); i++ {
+//
+//        // Get world convex vertex
+//        worldVertex.copy(convexShape.vertices[i])
+//        convexQuat.vmult(worldVertex, worldVertex)
+//        convexPosition.vadd(worldVertex, worldVertex)
+//        worldVertex.vsub(planePosition, relpos)
+//
+//        var dot = worldNormal.dot(relpos)
+//        if dot <= 0.0 {
+//            if justTest {
+//                return true
+//            }
+//
+//            var r = this.createContactEquation(planeBody, convexBody, planeShape, convexShape, si, sj)
+//
+//            // Get vertex position projected on plane
+//            var projected = planeConvex_projected
+//            worldNormal.mult(worldNormal.dot(relpos),projected)
+//            worldVertex.vsub(projected, projected)
+//            projected.vsub(planePosition, r.ri) // From plane to vertex projected on plane
+//
+//            r.ni.copy(worldNormal) // Contact normal is the plane normal out from plane
+//
+//            // rj is now just the vector from the convex center to the vertex
+//            worldVertex.vsub(convexPosition, r.rj)
+//
+//            // Make it relative to the body
+//            r.ri.vadd(planePosition, r.ri)
+//            r.ri.vsub(planeBody.position, r.ri)
+//            r.rj.vadd(convexPosition, r.rj)
+//            r.rj.vsub(convexBody.position, r.rj)
+//
+//            this.result.push(r)
+//            numContacts++
+//            if !this.enableFrictionReduction {
+//                this.createFrictionEquationsFromContact(r, this.frictionResult)
+//            }
+//        }
+//    }
+//
+//    if this.enableFrictionReduction && numContacts {
+//        this.createFrictionFromAverage(numContacts)
+//    }
+//}

physics/body.go

@@ -2,18 +2,19 @@
 // Use of this source code is governed by a BSD-style
 // license that can be found in the LICENSE file.
 
-package physics
+package object
 
 import (
 	"github.com/g3n/engine/graphic"
 	"github.com/g3n/engine/math32"
+	"github.com/g3n/engine/material"
 )
 
 // Body represents a physics-driven body.
 type Body struct {
 	*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
+
+	material             *material.Material   // Physics material specifying friction and restitution
 	index int
 
 	// Mass properties
@@ -67,14 +68,24 @@ type Body struct {
 	wakeUpAfterNarrowphase bool
 
 	// 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.
+	colFilterGroup int  // Collision filter group
+	colFilterMask  int  // Collision filter mask
+	colResponse    bool // Whether to produce contact forces when in contact with other bodies. Note that contacts will be generated, but they will be disabled.
 
 	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.)
 
+	// Cached geometry properties
+	faces            [][3]math32.Vector3
+
+	faceNormals      []math32.Vector3
+	worldFaceNormals []math32.Vector3
+
+	uniqueEdges      []math32.Vector3
+	worldUniqueEdges []math32.Vector3
+
+
 	// 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.
@@ -105,6 +116,8 @@ const (
 	Dynamic
 )
 
+// TODO Update simulation checks for BodyType to use bitwise operators ?
+
 // BodyStatus specifies
 type BodySleepState int
 
@@ -123,6 +136,7 @@ const (
 )
 
 // NewBody creates and returns a pointer to a new RigidBody.
+// The igraphic's geometry *must* be convex.
 func NewBody(igraphic graphic.IGraphic) *Body {
 
 	b := new(Body)
@@ -180,16 +194,179 @@ func NewBody(igraphic graphic.IGraphic) *Body {
 	b.sleepTimeLimit = 1
 	b.timeLastSleepy = 0
 
-	b.collisionFilterGroup = 1
-	b.collisionFilterMask = -1
+	b.colFilterGroup = 1
+	b.colFilterMask = -1
 
 	b.wakeUpAfterNarrowphase = false
 
+	// Perform single-time computations
+	b.computeFaceNormalsAndUniqueEdges()
+
 	b.UpdateMassProperties()
 
 	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()
+		}
+		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 {
+
+	return b.uniqueEdges
+}
+
+func (b *Body) WorldUniqueEdges() []math32.Vector3 {
+
+	return b.worldUniqueEdges
+}
+
+func (b *Body) BoundingBox() math32.Box3 {
+
+	return b.GetGeometry().BoundingBox()
+}
+
+func (b *Body) SetIndex(i int) {
+
+	b.index = i
+}
+
+func (b *Body) Index() int {
+
+	return b.index
+}
+
+func (b *Body) Material() *material.Material {
+
+	return b.material
+}
+
+func (b *Body) SetAllowSleep(state bool) {
+
+	b.allowSleep = state
+}
+
+func (b *Body) AllowSleep() bool {
+
+	return b.allowSleep
+}
+
+func (b *Body) SleepSpeedLimit() float32 {
+
+	return b.sleepSpeedLimit
+}
+
+func (b *Body) SleepState() BodySleepState {
+
+	return b.sleepState
+}
+
+func (b *Body) BodyType() BodyType {
+
+	return b. bodyType
+}
+
+func (b *Body) SetWakeUpAfterNarrowphase(state bool) {
+
+	b.wakeUpAfterNarrowphase = state
+}
+
+func (b *Body) WakeUpAfterNarrowphase() bool {
+
+	return b.wakeUpAfterNarrowphase
+}
+
+func (b *Body) ApplyDamping(dt float32) {
+
+	b.velocity.MultiplyScalar(math32.Pow(1.0 - b.linearDamping, dt))
+	b.angularVelocity.MultiplyScalar(math32.Pow(1.0 - b.angularDamping, dt))
+}
+
+func (b *Body) ApplyVelocityDeltas(linearD, angularD *math32.Vector3) {
+
+	b.velocity.Add(linearD.Multiply(b.LinearFactor()))
+	b.angularVelocity.Add(angularD.Multiply(b.AngularFactor()))
+}
+
+func (b *Body) ClearForces() {
+
+	b.force.Zero()
+	b.torque.Zero()
+}
+
 func (b *Body) InvMassEff() float32 {
 
 	return b.invMassEff
@@ -300,6 +477,29 @@ func (b *Body) SleepTick(time float32) {
 	}
 }
 
+// If checkSleeping is true then returns false if both bodies are currently sleeping.
+func (b *Body) Sleeping() bool {
+
+	return b.sleepState == Sleeping
+}
+
+// CollidableWith returns whether the body can collide with the specified body.
+func (b *Body) CollidableWith(other *Body) bool {
+
+	if (b.colFilterGroup & other.colFilterMask == 0) ||
+		(other.colFilterGroup & b.colFilterMask == 0) ||
+		(b.bodyType == Static) && (other.bodyType == Static) {
+		return false
+	}
+
+	return true
+}
+
+func (b *Body) CollisionResponse() bool {
+
+	return b.colResponse
+}
+
 // PointToLocal converts a world point to local body frame. TODO maybe move to Node
 func (b *Body) PointToLocal(worldPoint *math32.Vector3) math32.Vector3 {
 
@@ -356,8 +556,7 @@ func (b *Body) UpdateMassProperties() {
 		b.invRotInertia.Zero()
 	} else {
 		*b.rotInertia = b.GetGeometry().RotationalInertia()
-		b.invRotInertia.GetInverse(b.rotInertia)
-		// Note: rotInertia is always positive definite and thus always invertible
+		b.invRotInertia.GetInverse(b.rotInertia) // Note: rotInertia is always positive definite and thus always invertible
 }
 
 	b.UpdateInertiaWorld(true)
@@ -380,6 +579,12 @@ func (b *Body) UpdateInertiaWorld(force bool) {
 	}
 }
 
+// Forces from a force field need to be multiplied by mass.
+func (b *Body) ApplyForceField(force *math32.Vector3) {
+
+	b.force.Add(force.MultiplyScalar(b.mass))
+}
+
 // Apply force to a world point.
 // This could for example be a point on the Body surface.
 // Applying force this way will add to Body.force and Body.torque.
@@ -517,6 +722,11 @@ func (b *Body) Integrate(dt float32, quatNormalize, quatNormalizeFast bool) {
 	b.quaternion.X += halfDt * (az*bw + ax*by - ay*bx)
 	b.quaternion.W += halfDt * (-ax*bx - ay*by - az*bz)
 
+	// Update position and rotation of Node (containing visual representation of the body)
+	b.GetNode().SetPositionVec(b.position)
+	vRot := math32.NewVec3().SetFromQuaternion(b.quaternion)
+	b.GetNode().SetRotationVec(vRot)
+
 	// Normalize quaternion
 	if quatNormalize {
 		if quatNormalizeFast {

physics/particle.go

@@ -4,28 +4,28 @@
 
 package physics
 
-import (
-	"github.com/g3n/engine/math32"
-	"github.com/g3n/engine/graphic"
-)
-
-// Particle represents a physics-driven particle.
-type Particle struct {
-	Body
-	mass      float32
-	radius    float32
-	position  math32.Vector3
-	velocity  math32.Vector3
-	//netForce  math32.Vector3
-	colliding bool
-}
-
-// NewParticle creates and returns a pointer to a new Particle.
-func  NewParticle(igraphic graphic.IGraphic) *Particle {
-
-	p := new(Particle)
-	p.Graphic = igraphic.GetGraphic()
-	p.mass = 1
-	p.radius = 1
-	return p
-}
+//import (
+//	"github.com/g3n/engine/math32"
+//	"github.com/g3n/engine/graphic"
+//)
+//
+//// Particle represents a physics-driven particle.
+//type Particle struct {
+//	Body
+//	mass      float32
+//	radius    float32
+//	position  math32.Vector3
+//	velocity  math32.Vector3
+//	//netForce  math32.Vector3
+//	colliding bool
+//}
+//
+//// NewParticle creates and returns a pointer to a new Particle.
+//func  NewParticle(igraphic graphic.IGraphic) *Particle {
+//
+//	p := new(Particle)
+//	p.Graphic = igraphic.GetGraphic()
+//	p.mass = 1
+//	p.radius = 1
+//	return p
+//}

physics/simulation.go

@@ -9,6 +9,10 @@ import (
 	"github.com/g3n/engine/physics/equation"
 	"github.com/g3n/engine/physics/solver"
 	"github.com/g3n/engine/physics/constraint"
+	"github.com/g3n/engine/physics/collision"
+	"github.com/g3n/engine/math32"
+	"github.com/g3n/engine/physics/object"
+	"github.com/g3n/engine/physics/material"
 )
 
 type ICollidable interface {
@@ -24,16 +28,16 @@ type CollisionGroup struct {
 // Simulation represents a physics simulation.
 type Simulation struct {
 	forceFields []ForceField
-	bodies      []*Body
-	particles   []*Particle
 
-	//dynamical   []int // non collidable but still affected by force fields
-	//collidables []ICollidable
+	bodies      []*object.Body
+	nilBodies   []int   // Array keeps track of which indices of the 'bodies' array are nil
 
+	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
+	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
 
 	quatNormalizeSkip int // How often to normalize quaternions. Set to 0 for every step, 1 for every second etc..
 	                      // A larger value increases performance.
@@ -43,20 +47,25 @@ type Simulation struct {
 	time float32      // The wall-clock time since simulation start
 	stepnumber int    // Number of timesteps taken since start
 	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 IBroadphase // The broadphase algorithm to use. Default is NaiveBroadphase
-	//narrowphase INarrowphase // The narrowphase algorithm to use
+	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
 
-	constraints       []*constraint.Constraint  // All constraints
-	materials         []*Material               // All added materials
-	cMaterials        []*ContactMaterial
+	constraints       []constraint.IConstraint  // All constraints
 
+	materials         []*material.Material               // All added materials
+	cMaterials        []*material.ContactMaterial
 
 
 
+	//contactMaterialTable map[intPair]*ContactMaterial // Used to look up a ContactMaterial given two instances of Material.
+	//defaultMaterial *Material
+	defaultContactMaterial *material.ContactMaterial
 
 
 	doProfiling      bool
@@ -67,12 +76,18 @@ func NewSimulation() *Simulation {
 
 	s := new(Simulation)
 	s.time = 0
+	s.dt = -1
 	s.default_dt = 1/60
 
-	//s.broadphase = NewNaiveBroadphase()
-	//s.narrowphase = NewNarrowphase()
+	// Set up broadphase, narrowphase, and solver
+	s.broadphase = collision.NewBroadphase()
+	s.narrowphase = NewNarrowphase(s)
 	s.solver = solver.NewGaussSeidel()
 
+	//s.contactMaterialTable = make(map[intPair]*ContactMaterial)
+	//s.defaultMaterial = NewMaterial
+	s.defaultContactMaterial = material.NewContactMaterial()
+
 	return s
 }
 
@@ -98,148 +113,73 @@ func (s *Simulation) RemoveForceField(ff ForceField) bool {
 }
 
 // AddBody adds a body to the simulation.
-// @todo If the simulation has not yet started, why recrete and copy arrays for each body? Accumulate in dynamic arrays in this case.
-// @todo Adding an array of bodies should be possible. This would save some loops too
-func (s *Simulation) AddBody(body *Body) {
-
-	// TODO only add if not already present
-	s.bodies = append(s.bodies, body)
-
-	//body.index = this.bodies.length
-	//s.bodies.push(body)
-	//body.simulation = s // TODO
-	//body.initPosition.Copy(body.position)
-	//body.initVelocity.Copy(body.velocity)
-	//body.timeLastSleepy = s.time
-
-	//if body instanceof Body { // TODO
-	//	body.initAngularVelocity.Copy(body.angularVelocity)
-	//	body.initQuaternion.Copy(body.quaternion)
-	//}
-	//
-	//// TODO
-	//s.collisionMatrix.setNumObjects(len(s.bodies))
-	//s.addBodyEvent.body = body
-	//s.idToBodyMap[body.id] = body
-	//s.dispatchEvent(s.addBodyEvent)
-}
-
-// RemoveBody removes the specified body from the simulation.
-// Returns true if found, false otherwise.
-func (s *Simulation) RemoveBody(body *Body) bool {
-
-	for pos, current := range s.bodies {
-		if current == body {
-			copy(s.bodies[pos:], s.bodies[pos+1:])
-			s.bodies[len(s.bodies)-1] = nil
-			s.bodies = s.bodies[:len(s.bodies)-1]
-
-			body.simulation = nil
-
-			// Recompute body indices (each body has a .index int property)
-			for i:=0; i<len(s.bodies); i++ {
-				s.bodies[i].index = i
-			}
-
-			// TODO
-			//s.collisionMatrix.setNumObjects(len(s.bodies) - 1)
-			//s.removeBodyEvent.body = body
-			//delete s.idToBodyMap[body.id]
-			//s.dispatchEvent(s.removeBodyEvent)
+func (s *Simulation) AddBody(body *object.Body) {
 
-			return true
+	// Do nothing if body already present
+	for _, existingBody := range s.bodies {
+		if existingBody == body {
+			return // Do nothing
 		}
 	}
 
-	return false
-}
-
-// Bodies returns the bodies under simulation.
-func (s *Simulation) Bodies() []*Body{
-
-	return s.bodies
-}
+	var idx int
+	nilLen := len(s.nilBodies)
+	if nilLen > 0 {
+		idx = s.nilBodies[nilLen]
+		s.nilBodies = s.nilBodies[0:nilLen-1]
+	} else {
+		s.bodies = append(s.bodies, body)
+	}
 
-// AddParticle adds a particle to the simulation.
-func (s *Simulation) AddParticle(rb *Particle) {
+	body.SetIndex(idx)
 
-	s.particles = append(s.particles, rb)
+	// TODO dispatch add-body event
+	//s.Dispatch(AddBodyEvent, BodyEvent{body})
 }
 
-// RemoveParticle removes the specified particle from the simulation.
+// RemoveBody removes the specified body from the simulation.
 // Returns true if found, false otherwise.
-func (s *Simulation) RemoveParticle(rb *Particle) bool {
+func (s *Simulation) RemoveBody(body *object.Body) bool {
 
-	for pos, current := range s.particles {
-		if current == rb {
-			copy(s.particles[pos:], s.particles[pos+1:])
-			s.particles[len(s.particles)-1] = nil
-			s.particles = s.particles[:len(s.particles)-1]
-			return true
-		}
-	}
-	return false
-}
+	for idx, current := range s.bodies {
+		if current == body {
+			s.bodies[idx] = nil
 
-func (s *Simulation) applyForceFields(frameDelta time.Duration) {
-
-	for _, ff := range s.forceFields {
-		for _, rb := range s.bodies {
-			pos := rb.GetNode().Position()
-			force := ff.ForceAt(&pos)
-			//log.Error("force: %v", force)
-			//log.Error("mass: %v", rb.mass)
-			//log.Error("frameDelta: %v", frameDelta.Seconds())
-			vdiff := force.MultiplyScalar(float32(frameDelta.Seconds())/rb.mass)
-			//log.Error("vdiff: %v", vdiff)
-			rb.velocity.Add(vdiff)
-		}
-	}
+			// TODO dispatch remove-body event
+			//s.Dispatch(AddBodyEvent, BodyEvent{body})
 
-	for _, ff := range s.forceFields {
-		for _, rb := range s.particles {
-			pos := rb.GetNode().Position()
-			force := ff.ForceAt(&pos)
-			//log.Error("force: %v", force)
-			//log.Error("mass: %v", rb.mass)
-			//log.Error("frameDelta: %v", frameDelta.Seconds())
-			vdiff := force.MultiplyScalar(float32(frameDelta.Seconds())/rb.mass)
-			//log.Error("vdiff: %v", vdiff)
-			rb.velocity.Add(vdiff)
+			return true
 		}
 	}
-}
-
-// updatePositions integrates the velocity of the objects to obtain the position in the next frame.
-func (s *Simulation) updatePositions(frameDelta time.Duration) {
-
-	for _, rb := range s.bodies {
-		pos := rb.GetNode().Position()
-		posDelta := rb.velocity.Clone()
-		posDelta.MultiplyScalar(float32(frameDelta.Seconds()))
-		pos.Add(posDelta)
-		rb.GetNode().SetPositionVec(&pos)
-	}
 
-	for _, rb := range s.particles {
-		pos := rb.GetNode().Position()
-		posDelta := rb.velocity.Clone()
-		posDelta.MultiplyScalar(float32(frameDelta.Seconds()))
-		pos.Add(posDelta)
-		rb.GetNode().SetPositionVec(&pos)
-	}
+	return false
 }
 
-//func (s *Simulation) CheckCollisions() []*Collision{//collidables []ICollidable) {
+// Clean removes nil bodies from the bodies array, recalculates the body indices and updates the collision matrix.
+//func (s *Simulation) Clean() {
+//
+//	// TODO Remove nil bodies from array
+//	//copy(s.bodies[pos:], s.bodies[pos+1:])
+//	//s.bodies[len(s.bodies)-1] = nil
+//	//s.bodies = s.bodies[:len(s.bodies)-1]
+//
+//	// Recompute body indices (each body has a .index int property)
+//	for i:=0; i<len(s.bodies); i++ {
+//		s.bodies[i].SetIndex(i)
+//	}
+//
+//	// TODO Update collision matrix
 //
-//	return
 //}
 
+// Bodies returns the slice of bodies under simulation.
+// The slice may contain nil values!
+func (s *Simulation) Bodies() []*object.Body{
 
-func (s *Simulation) Backtrack() {
-
+	return s.bodies
 }
 
+
 // Step steps the simulation.
 func (s *Simulation) Step(frameDelta time.Duration) {
 
@@ -251,14 +191,14 @@ func (s *Simulation) Step(frameDelta time.Duration) {
 	//}
 
 	// Apply static forces/inertia/impulses (only to objects that did not collide)
-	s.applyForceFields(frameDelta)
+	//s.applyForceFields()
 	// s.applyDrag(frameDelta) // TODO
 
 	// Apply impact forces/inertia/impulses to objects that collided
 	//s.applyImpactForces(frameDelta)
 
 	// Update object positions based on calculated final speeds
-	s.updatePositions(frameDelta)
+	//s.updatePositions(frameDelta)
 
 }
 
@@ -266,79 +206,302 @@ func (s *Simulation) Step(frameDelta time.Duration) {
 func (s *Simulation) ClearForces() {
 
 	for i:=0; i < len(s.bodies); i++ {
-		s.bodies[i].force.Set(0,0,0)
-		s.bodies[i].torque.Set(0,0,0)
+		s.bodies[i].ClearForces()
 	}
 }
 
 // Add a constraint to the simulation.
-func (s *Simulation) AddConstraint(c *constraint.Constraint) {
+func (s *Simulation) AddConstraint(c constraint.IConstraint) {
 
 	s.constraints = append(s.constraints, c)
 }
 
-func (s *Simulation) RemoveConstraint(c *constraint.Constraint) {
+func (s *Simulation) RemoveConstraint(c constraint.IConstraint) {
 
 	// TODO
 }
 
-func (s *Simulation) AddMaterial(mat *Material) {
+func (s *Simulation) AddMaterial(mat *material.Material) {
 
 	s.materials = append(s.materials, mat)
 }
 
-func (s *Simulation) RemoveMaterial(mat *Material) {
+func (s *Simulation) RemoveMaterial(mat *material.Material) {
 
 	// TODO
 }
 
-func (s *Simulation) AddContactMaterial(cmat *ContactMaterial) {
+// Adds a contact material to the simulation
+func (s *Simulation) AddContactMaterial(cmat *material.ContactMaterial) {
 
 	s.cMaterials = append(s.cMaterials, cmat)
 
 	// TODO add contactMaterial materials to contactMaterialTable
+	// s.contactMaterialTable.set(cmat.materials[0].id, cmat.materials[1].id, cmat)
+}
+
+// GetContactMaterial returns the contact material between the specified bodies.
+func (s *Simulation) GetContactMaterial(bodyA, bodyB *object.Body) *material.ContactMaterial {
+
+	var cm *material.ContactMaterial
+	// TODO
+	//if bodyA.material != nil && bodyB.material != nil {
+	//	cm = s.contactMaterialTable.get(bodyA.material.id, bodyB.material.id)
+	//	if cm == nil {
+	//		cm = s.defaultContactMaterial
+	//	}
+	//} else {
+	cm = s.defaultContactMaterial
+	//}
+	return cm
 }
 
 
+// Events =====================
+
+type CollideEvent struct {
+	body      *object.Body
+	contactEq *equation.Contact
+}
+
+// TODO AddBodyEvent, RemoveBodyEvent
 type ContactEvent struct {
-	bodyA *Body
-	bodyB *Body
-	// shapeA
-	// shapeB
+	bodyA *object.Body
+	bodyB *object.Body
 }
 
 const (
 	BeginContactEvent = "physics.BeginContactEvent"
 	EndContactEvent   = "physics.EndContactEvent"
+	CollisionEv       = "physics.Collision"
 )
 
-func (s *Simulation) EmitContactEvents() {
-	//TODO
+// ===========================
+
+
+// Note - this method alters the solution arrays
+func (s *Simulation) ApplySolution(sol *solver.Solution) {
+
+	// Add results to velocity and angular velocity of bodies
+	for i := 0; i < len(s.bodies); i++ {
+		s.bodies[i].ApplyVelocityDeltas(&sol.VelocityDeltas[i], &sol.AngularVelocityDeltas[i])
+	}
+}
+
+// Store old collision state info
+func (s *Simulation) collisionMatrixTick() {
+
+	s.prevCollisionMatrix = s.collisionMatrix
+	s.collisionMatrix = collision.NewMatrix()
+
+	// TODO
+	//s.bodyOverlapKeeper.tick()
+	//s.shapeOverlapKeeper.tick()
 }
 
+// TODO read https://gafferongames.com/post/fix_your_timestep/
+func (s *Simulation) internalStep(dt float32) {
+
+	// Apply force fields and compute world normals/edges
+	for _, b := range s.bodies {
+
+		// Only apply to dynamic bodies
+		if b.BodyType() == object.Dynamic {
+			for _, ff := range s.forceFields {
+				pos := b.Position()
+				force := ff.ForceAt(&pos)
+				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
+	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
+
+	//
+    s.collisionMatrixTick()
+
+    // Generate contacts
+	s.generateContacts(pairs)
+
+    s.emitContactEvents()
+
+    // 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 user-added constraints
+	userAddedEquations := 0
+    for i := 0; i < len(s.constraints); i++ {
+        c := s.constraints[i]
+        c.Update()
+        eqs := c.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 {
+		for i := 0; i < len(s.bodies); i++ {
+			s.bodies[i].UpdateEffectiveMassProperties()
+		}
+	}
+
+    // 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)
+        }
+    }
+
+    // TODO s.Dispatch(World_step_preStepEvent)
+
+	// Integrate the forces into velocities into position deltas
+    quatNormalize := s.stepnumber % (s.quatNormalizeSkip + 1) == 0
+    for i := 0; i < len(s.bodies); i++ {
+		s.bodies[i].Integrate(dt, quatNormalize, s.quatNormalizeFast)
+    }
+    s.ClearForces()
+
+    // TODO s.broadphase.dirty = true ?
 
+    // Update world time
+    s.time += dt
+    s.stepnumber += 1
 
-func (s *Simulation) Solve() {
+    // TODO s.Dispatch(World_step_postStepEvent)
 
-	//// Update solve mass for all bodies
-	//if Neq > 0 {
-	//	for i := 0; i < Nbodies; i++ {
-	//		bodies[i].UpdateSolveMassProperties()
+    // Sleeping update
+    if s.allowSleep {
+        for i := 0; i < len(s.bodies); i++ {
+            s.bodies[i].SleepTick(s.time)
+        }
+    }
+
+}
+
+// TODO - REVIEW THIS
+func (s *Simulation) prunePairs(pairs []collision.Pair) []collision.Pair {
+
+	// TODO There is probably a bug here when the same body can have multiple constraints and appear in multiple pairs
+
+	//// Remove constrained pairs with collideConnected == false
+	//pairIdxsToRemove := []int
+	//for i := 0; i < len(s.constraints); i++ {
+	//	c := s.constraints[i]
+	//	cBodyA := s.bodies[c.BodyA().Index()]
+	//	cBodyB := s.bodies[c.BodyB().Index()]
+	//	if !c.CollideConnected() {
+	//		for i := range pairs {
+	//			if (pairs[i].BodyA == cBodyA && pairs[i].BodyB == cBodyB) ||
+	//				(pairs[i].BodyA == cBodyB && pairs[i].BodyB == cBodyA) {
+	//				pairIdxsToRemove = append(pairIdxsToRemove, i)
+	//
+	//			}
+	//		}
 	//	}
 	//}
 	//
-	//s.solver.Solve(frameDelta, len(bodies))
+	//// Remove pairs
+	////var prunedPairs []Pair
+	//for i := range pairs {
+	//	for _, idx := range pairIdxsToRemove {
+	//		copy(pairs[i:], pairs[i+1:])
+	//		//pairs[len(pairs)-1] = nil
+	//		pairs = pairs[:len(pairs)-1]
+	//	}
+	//}
 
+	return pairs
 }
 
+// generateContacts
+func (s *Simulation) generateContacts(pairs []collision.Pair) {
 
-// Note - this method alters the solution arrays
-func (s *Simulation) ApplySolution(solution *solver.Solution) {
+	// Find all contacts and generate contact and friction equations (narrowphase)
+	s.contactEqs, s.frictionEqs = s.narrowphase.GetContacts(pairs)
 
-	// Add results to velocity and angular velocity of bodies
-	for i := 0; i < len(s.bodies); i++ {
-		b := s.bodies[i]
-		b.velocity.Add(solution.VelocityDeltas[i].Multiply(b.LinearFactor()))
-		b.angularVelocity.Add(solution.AngularVelocityDeltas[i].Multiply(b.AngularFactor()))
+	// 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
+
+		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 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})
+		}
+
+		// TODO this is only for events
+		//s.bodyOverlapKeeper.set(bodyA.id, bodyB.id)
+		//s.shapeOverlapKeeper.set(si.id, sj.id)
 	}
+
 }
+
+func (s *Simulation) emitContactEvents() {
+	//TODO
+}

physics/solver/gs.go

@@ -6,7 +6,6 @@
 package solver
 
 import (
-	"time"
 	"github.com/g3n/engine/math32"
 )
 
@@ -16,7 +15,6 @@ import (
 // More iterations yield a better solution but require more computation.
 type GaussSeidel struct {
 	Solver
-	Solution
 	maxIter   int     // Number of solver iterations.
 	tolerance float32 // When the error is less than the tolerance, the system is assumed to be converged.
 
@@ -44,28 +42,25 @@ func NewGaussSeidel() *GaussSeidel {
 
 func (gs *GaussSeidel) Reset() {
 
+	// Reset solution
 	gs.VelocityDeltas = gs.VelocityDeltas[0:0]
 	gs.AngularVelocityDeltas = gs.AngularVelocityDeltas[0:0]
+	gs.Iterations = 0
 
+	// Reset internal arrays
 	gs.solveInvCs = gs.solveInvCs[0:0]
 	gs.solveBs = gs.solveBs[0:0]
 	gs.solveLambda = gs.solveLambda[0:0]
 }
 
 // Solve
-func (gs *GaussSeidel) Solve(frameDelta time.Duration, nBodies int) int {
+func (gs *GaussSeidel) Solve(dt float32, nBodies int) *Solution {
 
 	gs.Reset()
 
 	iter := 0
 	nEquations := len(gs.equations)
-	h := float32(frameDelta.Seconds())
-
-	// Reset deltas
-	for i := 0; i < nBodies; i++ {
-		gs.VelocityDeltas = append(gs.VelocityDeltas, math32.Vector3{0,0,0})
-		gs.AngularVelocityDeltas = append(gs.AngularVelocityDeltas, math32.Vector3{0,0,0})
-	}
+	h := dt
 
 	// Things that do not change during iteration can be computed once
 	for i := 0; i < nEquations; i++ {
@@ -106,7 +101,6 @@ func (gs *GaussSeidel) Solve(frameDelta time.Duration, nBodies int) int {
 				rotB := jeB.Rotational()
 
 				GWlambda := jeA.MultiplyVectors(&vA, &wA) + jeB.MultiplyVectors(&vB, &wB)
-
 				deltaLambda := gs.solveInvCs[j] * ( gs.solveBs[j]  - GWlambda - eq.Eps() *lambdaJ)
 
 				// Clamp if we are outside the min/max interval
@@ -119,13 +113,12 @@ func (gs *GaussSeidel) Solve(frameDelta time.Duration, nBodies int) int {
 				deltaLambdaTot += math32.Abs(deltaLambda)
 
 				// Add to velocity deltas
-				gs.VelocityDeltas[idxBodyA].Add(spatA.MultiplyScalar(eq.BodyA().InvMassSolve() * deltaLambda))
-				gs.VelocityDeltas[idxBodyB].Add(spatB.MultiplyScalar(eq.BodyB().InvMassSolve() * deltaLambda))
+				gs.VelocityDeltas[idxBodyA].Add(spatA.MultiplyScalar(eq.BodyA().InvMassEff() * deltaLambda))
+				gs.VelocityDeltas[idxBodyB].Add(spatB.MultiplyScalar(eq.BodyB().InvMassEff() * deltaLambda))
 
 				// Add to angular velocity deltas
-				gs.AngularVelocityDeltas[idxBodyA].Add(rotA.ApplyMatrix3(eq.BodyA().InvInertiaWorldSolve()).MultiplyScalar(deltaLambda))
-				gs.AngularVelocityDeltas[idxBodyB].Add(rotB.ApplyMatrix3(eq.BodyB().InvInertiaWorldSolve()).MultiplyScalar(deltaLambda))
-
+				gs.AngularVelocityDeltas[idxBodyA].Add(rotA.ApplyMatrix3(eq.BodyA().InvRotInertiaWorldEff()).MultiplyScalar(deltaLambda))
+				gs.AngularVelocityDeltas[idxBodyB].Add(rotB.ApplyMatrix3(eq.BodyB().InvRotInertiaWorldEff()).MultiplyScalar(deltaLambda))
 			}
 
 			// If the total error is small enough - stop iterating
@@ -138,9 +131,10 @@ func (gs *GaussSeidel) Solve(frameDelta time.Duration, nBodies int) int {
 		for i := range gs.equations {
 			gs.equations[i].SetMultiplier(gs.solveLambda[i] / h)
 		}
-
 		iter += 1
 	}
 
-	return iter
+	gs.Iterations = iter
+
+	return &gs.Solution
 }

physics/solver/solver.go

@@ -7,36 +7,39 @@ package solver
 
 import (
 	"github.com/g3n/engine/physics/equation"
-	"time"
 	"github.com/g3n/engine/math32"
 )
 
 // ISolver is the interface type for all constraint solvers.
 type ISolver interface {
-	// Solve should return the number of iterations performed
-	Solve(frameDelta time.Duration, nBodies int) int
+	Solve(dt float32, nBodies int) *Solution // Solve should return the number of iterations performed
+	AddEquation(eq equation.IEquation)
+	RemoveEquation(eq equation.IEquation) bool
+	ClearEquations()
 }
 
 // Solution represents a solver solution
 type Solution struct {
 	VelocityDeltas        []math32.Vector3
 	AngularVelocityDeltas []math32.Vector3
+	Iterations            int
 }
 
 // Constraint equation solver base class.
 type Solver struct {
-	equations []*equation.Equation // All equations to be solved
+	Solution
+	equations []equation.IEquation // All equations to be solved
 }
 
 // AddEquation adds an equation to the solver.
-func (s *Solver) AddEquation(eq *equation.Equation) {
+func (s *Solver) AddEquation(eq equation.IEquation) {
 
 	s.equations = append(s.equations, eq)
 }
 
 // RemoveEquation removes the specified equation from the solver.
 // Returns true if found, false otherwise.
-func (s *Solver) RemoveEquation(eq *equation.Equation) bool {
+func (s *Solver) RemoveEquation(eq equation.IEquation) bool {
 
 	for pos, current := range s.equations {
 		if current == eq {