physics dev (collision and solver working!)

core/node.go

@@ -415,6 +415,14 @@ func (n *Node) SetRotationVec(vrot *math32.Vector3) {
 	n.changed = true
 }
 
+// SetRotationQuat sets the rotation based on the specified quaternion pointer.
+// The stored quaternion is updated accordingly.
+func (n *Node) SetRotationQuat(quat *math32.Quaternion) {
+
+	n.quaternion = *quat
+	n.changed = true
+}
+
 // SetRotationX sets the X rotation to the specified angle in radians.
 // The stored quaternion is updated accordingly.
 func (n *Node) SetRotationX(x float32) {

physics/collision/broadphase.go

@@ -29,8 +29,8 @@ func (b *Broadphase) FindCollisionPairs(objects []*object.Body) []Pair {
 
 	pairs := make([]Pair,0)
 
-	for _, bodyA := range objects {
-		for _, bodyB := range objects {
+	for iA, bodyA := range objects {
+		for _, bodyB := range objects[iA+1:] {
 			if b.NeedTest(bodyA, bodyB) {
 				BBa := bodyA.BoundingBox()
 				BBb := bodyB.BoundingBox()

physics/debug.go

@@ -0,0 +1,93 @@
+// 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 implements a basic physics engine.
+package physics
+
+import (
+	"github.com/g3n/engine/core"
+	"github.com/g3n/engine/math32"
+	"github.com/g3n/engine/geometry"
+	"github.com/g3n/engine/gls"
+	"github.com/g3n/engine/graphic"
+	"github.com/g3n/engine/material"
+)
+
+// This file contains helpful infrastructure for debugging physics
+type DebugHelper struct {
+
+}
+
+func ShowWorldFace(scene *core.Node, face []math32.Vector3, color *math32.Color) {
+
+	if len(face) == 0 {
+		return
+	}
+
+	vertices := math32.NewArrayF32(0, 16)
+	for i := range face {
+		vertices.AppendVector3(&face[i])
+	}
+	vertices.AppendVector3(&face[0])
+
+	geom := geometry.NewGeometry()
+	geom.AddVBO(gls.NewVBO().AddAttrib("VertexPosition", 3).SetBuffer(vertices))
+
+	mat := material.NewStandard(color)
+	faceGraphic := graphic.NewLineStrip(geom, mat)
+	scene.Add(faceGraphic)
+}
+
+func ShowPenAxis(scene *core.Node, axis *math32.Vector3) {//}, min, max float32) {
+
+	vertices := math32.NewArrayF32(0, 16)
+
+	size := float32(100)
+	minPoint := axis.Clone().MultiplyScalar(size)
+	maxPoint := axis.Clone().MultiplyScalar(-size)
+	//vertices.AppendVector3(minPoint.Clone().SetX(minPoint.X - size))
+	//vertices.AppendVector3(minPoint.Clone().SetX(minPoint.X + size))
+	//vertices.AppendVector3(minPoint.Clone().SetY(minPoint.Y - size))
+	//vertices.AppendVector3(minPoint.Clone().SetY(minPoint.Y + size))
+	//vertices.AppendVector3(minPoint.Clone().SetZ(minPoint.Z - size))
+	//vertices.AppendVector3(minPoint.Clone().SetZ(minPoint.Z + size))
+	vertices.AppendVector3(minPoint)
+	//vertices.AppendVector3(maxPoint.Clone().SetX(maxPoint.X - size))
+	//vertices.AppendVector3(maxPoint.Clone().SetX(maxPoint.X + size))
+	//vertices.AppendVector3(maxPoint.Clone().SetY(maxPoint.Y - size))
+	//vertices.AppendVector3(maxPoint.Clone().SetY(maxPoint.Y + size))
+	//vertices.AppendVector3(maxPoint.Clone().SetZ(maxPoint.Z - size))
+	//vertices.AppendVector3(maxPoint.Clone().SetZ(maxPoint.Z + size))
+	vertices.AppendVector3(maxPoint)
+
+	geom := geometry.NewGeometry()
+	geom.AddVBO(gls.NewVBO().AddAttrib("VertexPosition", 3).SetBuffer(vertices))
+
+	mat := material.NewStandard(&math32.Color{1,1,1})
+	faceGraphic := graphic.NewLines(geom, mat)
+	scene.Add(faceGraphic)
+}
+
+func ShowContact(scene *core.Node, contact *Contact) {
+
+	vertices := math32.NewArrayF32(0, 16)
+
+	size := float32(0.0005)
+	otherPoint := contact.Point.Clone().Add(contact.Normal.MultiplyScalar(-contact.Depth))
+	vertices.AppendVector3(contact.Point.Clone().SetX(contact.Point.X - size))
+	vertices.AppendVector3(contact.Point.Clone().SetX(contact.Point.X + size))
+	vertices.AppendVector3(contact.Point.Clone().SetY(contact.Point.Y - size))
+	vertices.AppendVector3(contact.Point.Clone().SetY(contact.Point.Y + size))
+	vertices.AppendVector3(contact.Point.Clone().SetZ(contact.Point.Z - size))
+	vertices.AppendVector3(contact.Point.Clone().SetZ(contact.Point.Z + size))
+	vertices.AppendVector3(&contact.Point)
+	vertices.AppendVector3(otherPoint)
+
+	geom := geometry.NewGeometry()
+	geom.AddVBO(gls.NewVBO().AddAttrib("VertexPosition", 3).SetBuffer(vertices))
+
+	mat := material.NewStandard(&math32.Color{0,0,1})
+	faceGraphic := graphic.NewLines(geom, mat)
+	scene.Add(faceGraphic)
+}

physics/equation/contact.go

@@ -97,9 +97,7 @@ func (ce *Contact) ComputeB(h float32) float32 {
 	// Calculate the penetration vector
 	posA := ce.bA.Position()
 	posB := ce.bB.Position()
-	(&posB).Add(ce.rB)
-	x := (&posA).Sub(&posB)
-	penetrationVec := ce.rA.Clone().Sub(x)
+	penetrationVec := ce.rB.Clone().Add(&posB).Sub(ce.rA).Sub(&posA)
 
 	g := ce.nA.Dot(penetrationVec)
 

physics/equation/equation.go

@@ -80,7 +80,7 @@ func (e *Equation) initialize(bi, bj IBody, minForce, maxForce float32) {
 	e.enabled = true
 	e.multiplier = 0
 
-	// Set typical spook params
+	// Set typical spook params (k, d, dt)
 	e.SetSpookParams(1e7, 3, 1/60)
 }
 

physics/equation/jacobian.go

@@ -8,44 +8,44 @@ import "github.com/g3n/engine/math32"
 
 // JacobianElement contains 6 entries, 3 spatial and 3 rotational degrees of freedom.
 type JacobianElement struct {
-	spatial    *math32.Vector3
-	rotational *math32.Vector3
+	spatial    math32.Vector3
+	rotational math32.Vector3
 }
 
 // SetSpatial sets the spatial component of the JacobianElement.
 func (je *JacobianElement) SetSpatial(spatial *math32.Vector3) {
 
-	je.spatial = spatial
+	je.spatial = *spatial
 }
 
 // Spatial returns the spatial component of the JacobianElement.
 func (je *JacobianElement) Spatial() math32.Vector3 {
 
-	return *je.spatial
+	return je.spatial
 }
 
 // Rotational sets the rotational component of the JacobianElement.
 func (je *JacobianElement) SetRotational(rotational *math32.Vector3) {
 
-	je.rotational = rotational
+	je.rotational = *rotational
 }
 
 // Rotational returns the rotational component of the JacobianElement.
 func (je *JacobianElement) Rotational() math32.Vector3 {
 
-	return *je.rotational
+	return je.rotational
 }
 
 // MultiplyElement multiplies the JacobianElement with another JacobianElement.
 // None of the elements are changed.
 func (je *JacobianElement) MultiplyElement(je2 *JacobianElement) float32 {
 
-	return je.spatial.Dot(je2.spatial) + je.spatial.Dot(je2.spatial)
+	return je.spatial.Dot(&je2.spatial) + je.rotational.Dot(&je2.rotational)
 }
 
 // MultiplyElement multiplies the JacobianElement with two vectors.
 // None of the elements are changed.
 func (je *JacobianElement) MultiplyVectors(spatial *math32.Vector3, rotational *math32.Vector3) float32 {
 
-	return je.spatial.Dot(spatial) + je.spatial.Dot(spatial)
+	return je.spatial.Dot(spatial) + je.rotational.Dot(rotational)
 }

physics/narrowphase.go

@@ -18,6 +18,8 @@ type Narrowphase struct {
 	currentContactMaterial *material.ContactMaterial
 
 	enableFrictionReduction bool // If true friction is computed as average
+
+	debugging bool
 }
 
 type Pair struct {
@@ -30,7 +32,11 @@ func NewNarrowphase(simulation *Simulation) *Narrowphase {
 
 	n := new(Narrowphase)
 	n.simulation = simulation
-	n.enableFrictionReduction = true
+	//n.enableFrictionReduction = true
+
+	// FOR DEBUGGING
+	n.debugging = true
+
 	return n
 }
 
@@ -78,7 +84,10 @@ func (n *Narrowphase) Resolve(bodyA, bodyB *object.Body) (bool, []*equation.Cont
 
 	if penetrating {
 		// Colliding! Find contacts.
+		ShowPenAxis(n.simulation.Scene(), &penAxis) //, -1000, 1000)
+		log.Error("Colliding (%v|%v) penAxis: %v", bodyA.Name(), bodyB.Name(), penAxis)
 		contacts := n.ClipAgainstHull(bodyA, bodyB, &penAxis, -100, 100)
+		log.Error(" .... contacts: %v", contacts)
 
 		posA := bodyA.Position()
 		posB := bodyB.Position()
@@ -86,29 +95,39 @@ func (n *Narrowphase) Resolve(bodyA, bodyB *object.Body) (bool, []*equation.Cont
 		for j := 0; j < len(contacts); j++ {
 
 			contact := contacts[j]
+			ShowContact(n.simulation.Scene(), &contact) // TODO DEBUGGING
+			// Note - contact Normals point from B to A (contacts live in B)
 
 			// Create contact equation and append it
 			contactEq := equation.NewContact(bodyA, bodyB, 0, 1e6)
+			contactEq.SetSpookParams(1e6, 3, n.simulation.dt)
 			contactEq.SetEnabled(bodyA.CollisionResponse() && bodyB.CollisionResponse())
-			contactEq.SetNormal(penAxis.Negate())
-			contactEq.SetRA(contact.Normal.Negate().MultiplyScalar(contact.Depth).Add(&contact.Point).Sub(&posA))
+			contactEq.SetNormal(penAxis.Clone())
+
+			log.Error("contact.Depth: %v", contact.Depth)
+
+			//contactEq.SetRA(contact.Point.Clone().Sub(&posA).MultiplyScalar(1-contact.Depth))//.MultiplyScalar(2))
+			contactEq.SetRA(contact.Normal.Clone().MultiplyScalar(contact.Depth - 0).Add(&contact.Point).Sub(&posA))
+			//contactEq.SetRA(contact.Normal.MultiplyScalar(-contact.Depth*10).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
+			// TODO
 			if !n.enableFrictionReduction {
-				fEq1, fEq2 := n.createFrictionEquationsFromContact(contactEq)
-				frictionEqs = append(frictionEqs, fEq1, fEq2)
+				//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.
+		// TODO
 		if n.enableFrictionReduction && len(contactEqs) > 1 {
-			fEq1, fEq2 := n.createFrictionFromAverage(contactEqs)
-			frictionEqs = append(frictionEqs, fEq1, fEq2)
+			//fEq1, fEq2 := n.createFrictionFromAverage(contactEqs)
+			//frictionEqs = append(frictionEqs, fEq1, fEq2)
 		}
 	}
 
@@ -133,6 +152,9 @@ func (n *Narrowphase) createFrictionEquationsFromContact(contactEquation *equati
 	fricEq1 := equation.NewFriction(bodyA, bodyB, slipForce)
 	fricEq2 := equation.NewFriction(bodyA, bodyB, slipForce)
 
+	fricEq1.SetSpookParams(1e7, 3, n.simulation.dt)
+	fricEq2.SetSpookParams(1e7, 3, n.simulation.dt)
+
 	// Copy over the relative vectors
 	cRA := contactEquation.RA()
 	cRB := contactEquation.RB()
@@ -213,9 +235,9 @@ func (n *Narrowphase) createFrictionFromAverage(contactEqs []*equation.Contact)
 // 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
+// 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 (n *Narrowphase) FindPenetrationAxis(bodyA, bodyB *object.Body) (bool, math32.Vector3) {
 
 	// Keep track of the smaller depth found so far
@@ -287,7 +309,7 @@ func (n *Narrowphase) FindPenetrationAxis(bodyA, bodyB *object.Body) (bool, math
 	posA := bodyA.Position()
 	posB := bodyB.Position()
 
-	deltaC := math32.NewVec3().SubVectors(&posB, &posA)
+	deltaC := math32.NewVec3().SubVectors(&posA, &posB)
    	if deltaC.Dot(&penetrationAxis) > 0.0 {
        	penetrationAxis.Negate()
    	}
@@ -348,6 +370,10 @@ type Contact struct {
 //{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 {
 
+	var contacts []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
@@ -355,22 +381,23 @@ func (n *Narrowphase) ClipAgainstHull(bodyA, bodyB *object.Body, penAxis *math32
 	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)
+		d := worldFaceNormal.Dot(invPenAxis)
 		if d > dmax {
 			dmax = d
 			closestFaceBidx = i
 		}
 	}
 
-	// If found a closest face (TODO is this check necessary?)
-	//if closestFaceBidx >= 0 {
+	// If found a closest face (sometimes we don't find one)
+	if closestFaceBidx >= 0 {
 
-	// Copy and transform face vertices to world coordinates
-	faces := bodyB.Faces()
-	worldClosestFaceB := n.WorldFace(faces[closestFaceBidx], bodyB)
+		// Copy and transform face vertices to world coordinates
+		faces := bodyB.Faces()
+		worldClosestFaceB := n.WorldFace(faces[closestFaceBidx], bodyB)
+		contacts = n.ClipFaceAgainstHull(penAxis, bodyA, worldClosestFaceB, minDist, maxDist)
+	}
 
-	return n.ClipFaceAgainstHull(penAxis, bodyA, worldClosestFaceB, minDist, maxDist)
-	//}
+	return contacts
 }
 
 func (n *Narrowphase) WorldFace(face [3]math32.Vector3, body *object.Body) [3]math32.Vector3 {
@@ -400,22 +427,8 @@ func (n *Narrowphase) WorldFaceNormal(normal *math32.Vector3, body *object.Body)
 //@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)
@@ -434,14 +447,15 @@ func (n *Narrowphase) ClipFaceAgainstHull(penAxis *math32.Vector3, bodyA *object
 		return contacts
 	}
 
-	//console.log("closest A: ",closestFaceA);
+	//console.log("closest A: ",worldClosestFaceA);
 
 	// Get the face and construct connected faces
 	facesA := bodyA.Faces()
-	closestFaceA := n.WorldFace(facesA[closestFaceAidx], bodyA)
+	//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 closestFaceA
+		// Skip worldClosestFaceA
 		if faceIdx == closestFaceAidx {
 			continue
 		}
@@ -470,6 +484,24 @@ func (n *Narrowphase) ClipFaceAgainstHull(penAxis *math32.Vector3, bodyA *object
 	    }
 	}
 
+
+	// 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})
+	}
+	worldClosestFaceA := n.WorldFace(closestFaceA, bodyA)
+	//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
@@ -482,18 +514,20 @@ func (n *Narrowphase) ClipFaceAgainstHull(penAxis *math32.Vector3, bodyA *object
 		// 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)
+		clippedFace = n.ClipFaceAgainstPlane(clippedFace, connFaceNormal.Clone(), planeDelta)
 	}
 
-	//console.log("Resulting points after clip:",pVtxIn);
+	// Plot clipped face
+	log.Error("worldClosestFaceBClipped: %v", clippedFace)
+	ShowWorldFace(n.simulation.Scene(), clippedFace, &math32.Color{0,0,2})
+
 
 	closestFaceAnormal := worldFaceNormalsA[closestFaceAidx]
-	worldFirstVertex := closestFaceA[0].Clone().ApplyQuaternion(quatA).Add(&posA)
-	planeDelta := - worldFirstVertex.Dot(&closestFaceAnormal)
-	planeEqWS := planeDelta - closestFaceAnormal.Dot(&posA)
+	worldFirstVertex := worldClosestFaceA[0].Clone()//.ApplyQuaternion(quatA).Add(&posA)
+	planeDelta := -worldFirstVertex.Dot(&closestFaceAnormal)
 
-	for _, vertex := range worldClosestFaceB {
-		depth := closestFaceAnormal.Dot(&vertex) + planeEqWS // TODO verify MATH! Depth should be negative when penetrating (according to cannon.js)
+	for _, vertex := range clippedFace {
+		depth := closestFaceAnormal.Dot(&vertex) + planeDelta
 		// Cap distance
 		if depth <= minDist {
 			depth = minDist
@@ -516,36 +550,42 @@ func (n *Narrowphase) ClipFaceAgainstHull(penAxis *math32.Vector3, bodyA *object
 
 // 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 {
+func (n *Narrowphase) ClipFaceAgainstPlane(face []math32.Vector3, planeNormal *math32.Vector3, planeConstant float32) []math32.Vector3 {
 
 	// inVertices are the verts making up the face of hullB
-	var outVertices []math32.Vector3
+
+	clippedFace := make([]math32.Vector3, 0)
+
+	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 {
-	    	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
+		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 outVertices
+	return clippedFace
+
 }
 
 //// TODO ?

physics/object/body.go

@@ -16,6 +16,7 @@ type Body struct {
 
 	material             *material.Material   // Physics material specifying friction and restitution
 	index int
+	name string
 
 	// Mass properties
 	mass       float32 // Total mass
@@ -135,21 +136,23 @@ const (
 	CollideEvent = "physics.CollideEvent" // Dispatched after two bodies collide. This event is dispatched on each of the two bodies involved in the collision.
 )
 
+// TODO
+type HullType int
+const (
+	Sphere = HullType(iota)
+	Capsule
+	Mesh // use mesh itself
+)
+
+
 // 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)
 	b.Graphic = igraphic.GetGraphic()
-
-	// TODO mass setter/getter
-	b.mass = 1 // cannon.js default is 0
-	if b.mass > 0 {
-		b.invMass = 1.0 / b.mass
-	} else {
-		b.invMass = 0
-	}
-	b.bodyType = Dynamic // TODO auto set to Static if mass == 0
+	b.SetMass(1)
+	b.bodyType = Dynamic
 
 	// Rotational inertia and related properties
 	b.rotInertia = math32.NewMatrix3()
@@ -188,21 +191,26 @@ func NewBody(igraphic graphic.IGraphic) *Body {
 	b.linearFactor = math32.NewVector3(1, 1, 1)
 	b.angularFactor = math32.NewVector3(1, 1, 1)
 
+	// Sleep settings
 	b.allowSleep = true
 	b.sleepState = Awake
 	b.sleepSpeedLimit = 0.1
 	b.sleepTimeLimit = 1
 	b.timeLastSleepy = 0
 
+	// Collision filtering
 	b.colFilterGroup = 1
 	b.colFilterMask = -1
 
+	//b.fixedRotation = true
+
 	b.wakeUpAfterNarrowphase = false
 
 	// Perform single-time computations
 	b.computeFaceNormalsAndUniqueEdges()
 
 	b.UpdateMassProperties()
+	b.UpdateEffectiveMassProperties()
 
 	return b
 }
@@ -227,7 +235,7 @@ func (b *Body) computeFaceNormalsAndUniqueEdges() {
 		// Compute and store face normal in b.faceNormals
 		faceNormal := math32.NewVec3().CrossVectors(edge2, edge1)
 		if faceNormal.Length() > 0 {
-			faceNormal.Normalize()
+			faceNormal.Normalize().Negate()
 		}
 		b.faceNormals = append(b.faceNormals, *faceNormal)
 
@@ -299,11 +307,32 @@ func (b *Body) BoundingBox() math32.Box3 {
 	return b.GetGeometry().BoundingBox()
 }
 
+func (b *Body) SetMass(mass float32) {
+
+	b.mass = mass
+	if b.mass > 0 {
+		b.invMass = 1.0 / b.mass
+	} else {
+		b.invMass = 0
+		b.SetBodyType(Static)
+	}
+}
+
 func (b *Body) SetIndex(i int) {
 
 	b.index = i
 }
 
+func (b *Body) SetName(name string) {
+
+	b.name = name
+}
+
+func (b *Body) Name() string {
+
+	return b.name
+}
+
 func (b *Body) Index() int {
 
 	return b.index
@@ -334,6 +363,11 @@ func (b *Body) SleepState() BodySleepState {
 	return b.sleepState
 }
 
+func (b *Body) SetBodyType(bt BodyType) {
+
+	b.bodyType = bt
+}
+
 func (b *Body) BodyType() BodyType {
 
 	return b. bodyType
@@ -349,16 +383,10 @@ 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()))
+	b.velocity.Add(linearD.Multiply(b.linearFactor))
+	b.angularVelocity.Add(angularD.Multiply(b.angularFactor))
 }
 
 func (b *Body) ClearForces() {
@@ -384,7 +412,7 @@ func (b *Body) Position() math32.Vector3 {
 
 func (b *Body) Quaternion() *math32.Quaternion {
 
-	return b.quaternion
+	return b.quaternion.Clone()
 }
 
 func (b *Body) SetVelocity(vel *math32.Vector3) {
@@ -417,26 +445,54 @@ func (b *Body) Torque() math32.Vector3 {
 	return *b.torque
 }
 
+func (b *Body) SetLinearDamping(d float32) {
+
+	b.linearDamping = d
+}
+
 func (b *Body) LinearDamping() float32 {
 
 	return b.linearDamping
 }
 
+func (b *Body) SetAngularDamping(d float32) {
+
+	b.angularDamping = d
+}
+
 func (b *Body) AngularDamping() float32 {
 
 	return b.angularDamping
 }
 
-func (b *Body) LinearFactor() *math32.Vector3 {
+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) SetLinearFactor(factor *math32.Vector3) {
 
-	return b.linearFactor
+	b.linearFactor = factor
 }
 
-func (b *Body) AngularFactor() *math32.Vector3 {
+func (b *Body) LinearFactor() math32.Vector3 {
 
-	return b.angularFactor
+	return *b.linearFactor
 }
 
+func (b *Body) SetAngularFactor(factor *math32.Vector3) {
+
+	b.angularFactor = factor
+}
+
+func (b *Body) AngularFactor() math32.Vector3 {
+
+	return *b.angularFactor
+}
+
+
+
 // WakeUp wakes the body up.
 func (b *Body) WakeUp() {
 
@@ -556,8 +612,9 @@ func (b *Body) UpdateMassProperties() {
 		b.invRotInertia.Zero()
 	} else {
 		*b.rotInertia = b.GetGeometry().RotationalInertia()
+		b.rotInertia.MultiplyScalar(1000) // multiply by high density // TODO remove this ?
 		b.invRotInertia.GetInverse(b.rotInertia) // Note: rotInertia is always positive definite and thus always invertible
-}
+	}
 
 	b.UpdateInertiaWorld(true)
 }
@@ -719,22 +776,22 @@ func (b *Body) Integrate(dt float32, quatNormalize, quatNormalizeFast bool) {
 	halfDt := dt * 0.5
 	b.quaternion.X += halfDt * (ax*bw + ay*bz - az*by)
 	b.quaternion.Y += halfDt * (ay*bw + az*bx - ax*bz)
-	b.quaternion.X += halfDt * (az*bw + ax*by - ay*bx)
+	b.quaternion.Z += halfDt * (az*bw + ax*by - ay*bx)
 	b.quaternion.W += halfDt * (-ax*bx - ay*by - az*bz)
 
+	// Normalize quaternion
+	b.quaternion.Normalize()
+	//if quatNormalize { // TODO future
+	//	if quatNormalizeFast {
+	//		b.quaternion.NormalizeFast()
+	//	} else {
+	//		b.quaternion.Normalize()
+	//	}
+	//}
+
 	// 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 {
-			b.quaternion.NormalizeFast()
-		} else {
-			b.quaternion.Normalize()
-		}
-	}
+	b.GetNode().SetRotationQuat(b.quaternion)
 
 	b.aabbNeedsUpdate = true
 

physics/simulation.go

@@ -5,7 +5,6 @@
 package physics
 
 import (
-	"time"
 	"github.com/g3n/engine/physics/equation"
 	"github.com/g3n/engine/physics/solver"
 	"github.com/g3n/engine/physics/constraint"
@@ -13,6 +12,7 @@ import (
 	"github.com/g3n/engine/math32"
 	"github.com/g3n/engine/physics/object"
 	"github.com/g3n/engine/physics/material"
+	"github.com/g3n/engine/core"
 )
 
 type ICollidable interface {
@@ -38,6 +38,7 @@ type Simulation struct {
 	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.
@@ -69,21 +70,26 @@ type Simulation struct {
 
 
 	doProfiling      bool
+	scene *core.Node
 }
 
 // NewSimulation creates and returns a pointer to a new physics simulation.
-func NewSimulation() *Simulation {
+func NewSimulation(scene *core.Node) *Simulation {
 
 	s := new(Simulation)
 	s.time = 0
 	s.dt = -1
 	s.default_dt = 1/60
+	s.scene = scene
 
 	// Set up broadphase, narrowphase, and solver
 	s.broadphase = collision.NewBroadphase()
 	s.narrowphase = NewNarrowphase(s)
 	s.solver = solver.NewGaussSeidel()
 
+	s.collisionMatrix = collision.NewMatrix()
+	s.prevCollisionMatrix = collision.NewMatrix()
+
 	//s.contactMaterialTable = make(map[intPair]*ContactMaterial)
 	//s.defaultMaterial = NewMaterial
 	s.defaultContactMaterial = material.NewContactMaterial()
@@ -91,6 +97,11 @@ func NewSimulation() *Simulation {
 	return s
 }
 
+func (s *Simulation) Scene() *core.Node {
+
+	return s.scene
+}
+
 // AddForceField adds a force field to the simulation.
 func (s *Simulation) AddForceField(ff ForceField) {
 
@@ -113,7 +124,7 @@ func (s *Simulation) RemoveForceField(ff ForceField) bool {
 }
 
 // AddBody adds a body to the simulation.
-func (s *Simulation) AddBody(body *object.Body) {
+func (s *Simulation) AddBody(body *object.Body, name string) {
 
 	// Do nothing if body already present
 	for _, existingBody := range s.bodies {
@@ -128,10 +139,16 @@ func (s *Simulation) AddBody(body *object.Body) {
 		idx = s.nilBodies[nilLen]
 		s.nilBodies = s.nilBodies[0:nilLen-1]
 	} else {
+		idx = len(s.bodies)
 		s.bodies = append(s.bodies, body)
 	}
 
 	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})
@@ -179,29 +196,63 @@ func (s *Simulation) Bodies() []*object.Body{
 	return s.bodies
 }
 
+func (s *Simulation) Step(frameDelta float32) {
+
+	s.StepPlus(frameDelta, 0, 10)
+}
+
 
 // Step steps the simulation.
-func (s *Simulation) Step(frameDelta time.Duration) {
+// maxSubSteps should be 10 by default
+func (s *Simulation) StepPlus(frameDelta float32, timeSinceLastCalled float32, maxSubSteps int) {
 
-	// Check for collisions
-	//collisions := s.CheckCollisions()
-	//if len(collisions) > 0 {
-	//	// Backtrack to 0 penetration
-	//	s.Backtrack()
-	//}
+	if s.paused {
+		return
+	}
+
+	dt := frameDelta//float32(frameDelta.Seconds())
+
+    //if timeSinceLastCalled == 0 { // Fixed, simple stepping
 
-	// Apply static forces/inertia/impulses (only to objects that did not collide)
-	//s.applyForceFields()
-	// s.applyDrag(frameDelta) // TODO
+        s.internalStep(dt)
 
-	// Apply impact forces/inertia/impulses to objects that collided
-	//s.applyImpactForces(frameDelta)
+        // Increment time
+        //s.time += dt
 
-	// Update object positions based on calculated final speeds
-	//s.updatePositions(frameDelta)
+    //} else {
+	//
+    //    s.accumulator += timeSinceLastCalled
+    //    var substeps = 0
+    //    for s.accumulator >= dt && substeps < maxSubSteps {
+    //        // Do fixed steps to catch up
+    //        s.internalStep(dt)
+    //        s.accumulator -= dt
+    //        substeps++
+    //    }
+	//
+    //    var t = (s.accumulator % dt) / dt
+    //    for j := 0; j < len(s.bodies); j++ {
+    //        var b = s.bodies[j]
+    //        b.previousPosition.lerp(b.position, t, b.interpolatedPosition)
+    //        b.previousQuaternion.slerp(b.quaternion, t, b.interpolatedQuaternion)
+    //        b.previousQuaternion.normalize()
+    //    }
+    //    s.time += timeSinceLastCalled
+    //}
 
 }
 
+func (s *Simulation) SetPaused(state bool) {
+
+	s.paused = state
+}
+
+
+func (s *Simulation) Paused() bool {
+
+	return s.paused
+}
+
 // ClearForces sets all body forces in the world to zero.
 func (s *Simulation) ClearForces() {
 
@@ -294,6 +345,14 @@ func (s *Simulation) collisionMatrixTick() {
 	s.prevCollisionMatrix = s.collisionMatrix
 	s.collisionMatrix = collision.NewMatrix()
 
+	lb := len(s.bodies)
+	s.collisionMatrix.Set(lb, lb, false)
+
+	// TODO verify that the matrices are indeed different
+	//if s.prevCollisionMatrix == s.collisionMatrix {
+	//	log.Error("SAME")
+	//}
+
 	// TODO
 	//s.bodyOverlapKeeper.tick()
 	//s.shapeOverlapKeeper.tick()
@@ -302,6 +361,8 @@ func (s *Simulation) collisionMatrixTick() {
 // 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
 	for _, b := range s.bodies {
 
@@ -384,7 +445,7 @@ func (s *Simulation) internalStep(dt float32) {
     // TODO s.Dispatch(World_step_preStepEvent)
 
 	// Integrate the forces into velocities into position deltas
-    quatNormalize := s.stepnumber % (s.quatNormalizeSkip + 1) == 0
+    quatNormalize := true// s.stepnumber % (s.quatNormalizeSkip + 1) == 0
     for i := 0; i < len(s.bodies); i++ {
 		s.bodies[i].Integrate(dt, quatNormalize, s.quatNormalizeFast)
     }

physics/solver/gs.go

@@ -27,7 +27,7 @@ type GaussSeidel struct {
 func NewGaussSeidel() *GaussSeidel {
 
 	gs := new(GaussSeidel)
-	gs.maxIter = 10
+	gs.maxIter = 20
 	gs.tolerance = 1e-7
 
 	gs.VelocityDeltas = make([]math32.Vector3, 0)
@@ -40,11 +40,11 @@ func NewGaussSeidel() *GaussSeidel {
 	return gs
 }
 
-func (gs *GaussSeidel) Reset() {
+func (gs *GaussSeidel) Reset(numBodies int) {
 
 	// Reset solution
-	gs.VelocityDeltas = gs.VelocityDeltas[0:0]
-	gs.AngularVelocityDeltas = gs.AngularVelocityDeltas[0:0]
+	gs.VelocityDeltas = make([]math32.Vector3, numBodies)
+	gs.AngularVelocityDeltas = make([]math32.Vector3, numBodies)
 	gs.Iterations = 0
 
 	// Reset internal arrays
@@ -56,7 +56,7 @@ func (gs *GaussSeidel) Reset() {
 // Solve
 func (gs *GaussSeidel) Solve(dt float32, nBodies int) *Solution {
 
-	gs.Reset()
+	gs.Reset(nBodies)
 
 	iter := 0
 	nEquations := len(gs.equations)
@@ -92,31 +92,30 @@ func (gs *GaussSeidel) Solve(dt float32, nBodies int) *Solution {
 				vB := gs.VelocityDeltas[idxBodyB]
 				wA := gs.AngularVelocityDeltas[idxBodyA]
 				wB := gs.AngularVelocityDeltas[idxBodyB]
-
 				jeA := eq.JeA()
 				jeB := eq.JeB()
-				spatA := jeA.Spatial()
-				spatB := jeB.Spatial()
-				rotA := jeA.Rotational()
-				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
-				if lambdaJ+deltaLambda < eq.MinForce() {
+				if lambdaJ + deltaLambda < eq.MinForce() {
 					deltaLambda = eq.MinForce() - lambdaJ
-				} else if lambdaJ+deltaLambda > eq.MaxForce() {
+				} else if lambdaJ + deltaLambda > eq.MaxForce() {
 					deltaLambda = eq.MaxForce() - lambdaJ
 				}
 				gs.solveLambda[j] += deltaLambda
 				deltaLambdaTot += math32.Abs(deltaLambda)
 
 				// Add to velocity deltas
+				spatA := jeA.Spatial()
+				spatB := jeB.Spatial()
 				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
+				rotA := jeA.Rotational()
+				rotB := jeB.Rotational()
 				gs.AngularVelocityDeltas[idxBodyA].Add(rotA.ApplyMatrix3(eq.BodyA().InvRotInertiaWorldEff()).MultiplyScalar(deltaLambda))
 				gs.AngularVelocityDeltas[idxBodyB].Add(rotB.ApplyMatrix3(eq.BodyB().InvRotInertiaWorldEff()).MultiplyScalar(deltaLambda))
 			}