physics dev (constraint, equation, solver)

core/node.go

@@ -12,6 +12,7 @@ import (
 
 // INode is the interface for all node types.
 type INode interface {
+	IDispatcher
 	GetNode() *Node
 	UpdateMatrixWorld()
 	Raycast(*Raycaster, *[]Intersect)

math32/matrix3.go

@@ -47,6 +47,18 @@ func (m *Matrix3) Identity() *Matrix3 {
 	return m
 }
 
+// Zero sets this matrix as the zero matrix.
+// Returns the pointer to this updated matrix.
+func (m *Matrix3) Zero() *Matrix3 {
+
+	m.Set(
+		0, 0, 0,
+		0, 0, 0,
+		0, 0, 0,
+	)
+	return m
+}
+
 // Copy copies src matrix into this one.
 // Returns the pointer to this updated matrix.
 func (m *Matrix3) Copy(src *Matrix3) *Matrix3 {
@@ -74,6 +86,52 @@ func (m *Matrix3) ApplyToVector3Array(array []float32, offset int, length int) [
 	return array
 }
 
+// Multiply multiply this matrix by the other matrix
+// Returns pointer to this updated matrix.
+func (m *Matrix3) Multiply(other *Matrix3) *Matrix3 {
+
+	return m.MultiplyMatrices(m, other)
+}
+
+// MultiplyMatrices multiply matrix a by b storing the result in this matrix.
+// Returns pointer to this updated matrix.
+func (m *Matrix3) MultiplyMatrices(a, b *Matrix3) *Matrix3 {
+
+	a11 := a[0]
+	a12 := a[3]
+	a13 := a[6]
+	a21 := a[1]
+	a22 := a[4]
+	a23 := a[7]
+	a31 := a[2]
+	a32 := a[5]
+	a33 := a[8]
+
+	b11 := b[0]
+	b12 := b[3]
+	b13 := b[6]
+	b21 := b[1]
+	b22 := b[4]
+	b23 := b[7]
+	b31 := b[2]
+	b32 := b[5]
+	b33 := b[8]
+
+	m[0] = a11*b11 + a12*b21 + a13*b31
+	m[3] = a11*b12 + a12*b22 + a13*b32
+	m[6] = a11*b13 + a12*b23 + a13*b33
+
+	m[1] = a21*b11 + a22*b21 + a23*b31
+	m[4] = a21*b12 + a22*b22 + a23*b32
+	m[7] = a21*b13 + a22*b23 + a23*b33
+
+	m[2] = a31*b11 + a32*b21 + a33*b31
+	m[5] = a31*b12 + a32*b22 + a33*b32
+	m[8] = a31*b13 + a32*b23 + a33*b33
+
+	return m
+}
+
 // MultiplyScalar multiplies each of this matrix's components by the specified scalar.
 // Returns pointer to this updated matrix.
 func (m *Matrix3) MultiplyScalar(s float32) *Matrix3 {

math32/matrix4.go

@@ -46,12 +46,25 @@ func (m *Matrix4) Set(n11, n12, n13, n14, n21, n22, n23, n24, n31, n32, n33, n34
 // Returns pointer to this updated matrix.
 func (m *Matrix4) Identity() *Matrix4 {
 
-	*m = Matrix4{
+	m.Set(
 		1, 0, 0, 0,
 		0, 1, 0, 0,
 		0, 0, 1, 0,
 		0, 0, 0, 1,
-	}
+	)
+	return m
+}
+
+// Zero sets this matrix as the zero matrix.
+// Returns the pointer to this updated matrix.
+func (m *Matrix4) Zero() *Matrix4 {
+
+	m.Set(
+		0, 0, 0, 0,
+		0, 0, 0, 0,
+		0, 0, 0, 0,
+		0, 0, 0, 0,
+	)
 	return m
 }
 

math32/vector2.go

@@ -17,6 +17,12 @@ func NewVector2(x, y float32) *Vector2 {
 	return &Vector2{X: x, Y: y}
 }
 
+// NewVec2 creates and returns a pointer to a new zero-ed Vector2.
+func NewVec2() *Vector2 {
+
+	return &Vector2{X: 0, Y: 0}
+}
+
 // Set sets this vector X and Y components.
 // Returns the pointer to this updated vector.
 func (v *Vector2) Set(x, y float32) *Vector2 {
@@ -70,6 +76,28 @@ func (v *Vector2) Component(index int) float32 {
 	}
 }
 
+// SetByName sets this vector component value by its case insensitive name: "x" or "y".
+func (v *Vector2) SetByName(name string, value float32) {
+
+	switch name {
+	case "x", "X":
+		v.X = value
+	case "y", "Y":
+		v.Y = value
+	default:
+		panic("Invalid Vector2 component name: " + name)
+	}
+}
+
+// Zero sets this vector X and Y components to be zero.
+// Returns the pointer to this updated vector.
+func (v *Vector2) Zero() *Vector2 {
+
+	v.X = 0
+	v.Y = 0
+	return v
+}
+
 // Copy copies other vector to this one.
 // It is equivalent to: *v = *other.
 // Returns the pointer to this updated vector.

math32/vector3.go

@@ -18,6 +18,12 @@ func NewVector3(x, y, z float32) *Vector3 {
 	return &Vector3{X: x, Y: y, Z: z}
 }
 
+// NewVec3 creates and returns a pointer to a new zero-ed Vector3.
+func NewVec3() *Vector3 {
+
+	return &Vector3{X: 0, Y: 0, Z: 0}
+}
+
 // Set sets this vector X, Y and Z components.
 // Returns the pointer to this updated vector.
 func (v *Vector3) Set(x, y, z float32) *Vector3 {
@@ -98,6 +104,16 @@ func (v *Vector3) SetByName(name string, value float32) {
 	}
 }
 
+// Zero sets this vector X, Y and Z components to be zero.
+// Returns the pointer to this updated vector.
+func (v *Vector3) Zero() *Vector3 {
+
+	v.X = 0
+	v.Y = 0
+	v.Z = 0
+	return v
+}
+
 // Copy copies other vector to this one.
 // It is equivalent to: *v = *other.
 // Returns the pointer to this updated vector.
@@ -622,3 +638,31 @@ func (v *Vector3) SetFromQuaternion(q *Quaternion) *Vector3 {
 	v.SetFromRotationMatrix(matrix)
 	return v
 }
+
+// RandomTangents computes and returns two arbitrary tangents to the vector.
+func (v *Vector3) RandomTangents() (*Vector3, *Vector3) {
+
+	t1 := NewVector3(0,0,0)
+	t2 := NewVector3(0,0,0)
+	length := v.Length()
+	if length > 0 {
+		n := NewVector3(v.X/length, v.Y/length, v.Z/length)
+		randVec := NewVector3(0,0,0)
+		if Abs(n.X) < 0.9 {
+			randVec.SetX(1)
+			t1.CrossVectors(n, randVec)
+		} else if Abs(n.Y) < 0.9 {
+			randVec.SetY(1)
+			t1.CrossVectors(n, randVec)
+		} else {
+			randVec.SetZ(1)
+			t1.CrossVectors(n, randVec)
+		}
+		t2.CrossVectors(n, t1)
+	} else {
+		t1.SetX(1)
+		t2.SetY(1)
+	}
+
+	return t1, t2
+}

math32/vector4.go

@@ -18,6 +18,12 @@ func NewVector4(x, y, z, w float32) *Vector4 {
 	return &Vector4{X: x, Y: y, Z: z, W: w}
 }
 
+// NewVec4 creates and returns a pointer to a new zero-ed Vector4 (with W=1).
+func NewVec4() *Vector4 {
+
+	return &Vector4{X: 0, Y: 0, Z: 0, W: 1}
+}
+
 // Set sets this vector X, Y, Z and W components.
 // Returns the pointer to this updated vector.
 func (v *Vector4) Set(x, y, z, w float32) *Vector4 {
@@ -124,6 +130,17 @@ func (v *Vector4) SetByName(name string, value float32) {
 	}
 }
 
+// Zero sets this vector X, Y and Z components to be zero and W to be one.
+// Returns the pointer to this updated vector.
+func (v *Vector4) Zero() *Vector4 {
+
+	v.X = 0
+	v.Y = 0
+	v.Z = 0
+	v.W = 1
+	return v
+}
+
 // Copy copies other vector to this one.
 // Returns the pointer to this updated vector.
 func (v *Vector4) Copy(other *Vector4) *Vector4 {

physics/body.go

@@ -0,0 +1,577 @@
+// 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/math32"
+	"github.com/g3n/engine/core"
+)
+
+// Body represents a physics-driven body.
+type Body struct {
+	core.INode // TODO instead of embedding INode - embed Node and have a method SetNode ?
+
+	mass            float32        // Total mass
+	invMass         float32
+	invMassSolve    float32
+
+	velocity        *math32.Vector3 // Linear velocity (World space velocity of the body.)
+	initVelocity    *math32.Vector3 // Initial linear velocity (World space velocity of the body.)
+	vLambda         *math32.Vector3
+
+	angularMass     *math32.Matrix3 // Angular mass i.e. moment of inertia
+
+	inertia              *math32.Vector3
+	invInertia           *math32.Vector3
+	invInertiaSolve      *math32.Vector3
+	invInertiaWorld      *math32.Matrix3
+	invInertiaWorldSolve *math32.Matrix3
+
+	fixedRotation    bool  // Set to true if you don't want the body to rotate. Make sure to run .updateMassProperties() after changing this.
+
+	angularVelocity     *math32.Vector3 // Angular velocity of the body, in world space. Think of the angular velocity as a vector, which the body rotates around. The length of this vector determines how fast (in radians per second) the body rotates.
+	initAngularVelocity *math32.Vector3
+	wLambda             *math32.Vector3
+
+
+	force           *math32.Vector3 // Linear force on the body in world space.
+	torque          *math32.Vector3 // World space rotational force on the body, around center of mass.
+
+	position        *math32.Vector3 // World position of the center of gravity (World space position of the body.)
+	prevPosition    *math32.Vector3 // Previous position
+	interpPosition  *math32.Vector3 // Interpolated position of the body.
+	initPosition    *math32.Vector3 // Initial position of the body.
+
+	quaternion       *math32.Quaternion // World space orientation of the body.
+	initQuaternion   *math32.Quaternion
+	prevQuaternion   *math32.Quaternion
+	interpQuaternion *math32.Quaternion // Interpolated orientation of the body.
+
+	bodyType        BodyType
+	sleepState      BodySleepState // Current sleep state.
+	allowSleep      bool           // If true, the body will automatically fall to sleep.
+	sleepSpeedLimit float32        // If the speed (the norm of the velocity) is smaller than this value, the body is considered sleepy.
+	sleepTimeLimit  float32        // If the body has been sleepy for this sleepTimeLimit seconds, it is considered sleeping.
+	timeLastSleepy  float32
+
+	simulation             *Simulation // Reference to the simulation the body is living in\
+	collisionFilterGroup   int
+	collisionFilterMask    int
+	collisionResponse      bool // Whether to produce contact forces when in contact with other bodies. Note that contacts will be generated, but they will be disabled.
+
+	wakeUpAfterNarrowphase bool
+	material               *Material
+
+	linearDamping          float32
+	angularDamping         float32
+
+	linearFactor           *math32.Vector3 // Use this property to limit the motion along any world axis. (1,1,1) will allow motion along all axes while (0,0,0) allows none.
+	angularFactor          *math32.Vector3 // Use this property to limit the rotational motion along any world axis. (1,1,1) will allow rotation along all axes while (0,0,0) allows none.
+
+	//aabb *AABB // World space bounding box of the body and its shapes.
+	//aabbNeedsUpdate bool // Indicates if the AABB needs to be updated before use.
+	// boundingRadius float32 // Total bounding radius of the Body including its shapes, relative to body.position.
+
+	// shapes          []*Shape
+	// shapeOffsets    []float32 // Position of each Shape in the body, given in local Body space.
+	// shapeOrientations [] ?
+
+	index int
+}
+
+// BodyType specifies how the body is affected during the simulation.
+type BodyType int
+
+const (
+	// A static body does not move during simulation and behaves as if it has infinite mass.
+	// Static bodies can be moved manually by setting the position of the body.
+	// The velocity of a static body is always zero.
+	// Static bodies do not collide with other static or kinematic bodies.
+	Static       = BodyType(iota)
+
+	// A kinematic body moves under simulation according to its velocity.
+	// They do not respond to forces.
+	// They can be moved manually, but normally a kinematic body is moved by setting its velocity.
+	// A kinematic body behaves as if it has infinite mass.
+	// Kinematic bodies do not collide with other static or kinematic bodies.
+	Kinematic
+
+	// A dynamic body is fully simulated.
+	// Can be moved manually by the user, but normally they move according to forces.
+	// A dynamic body can collide with all body types.
+	// A dynamic body always has finite, non-zero mass.
+	Dynamic
+)
+
+// BodyStatus specifies
+type BodySleepState int
+
+const (
+	Awake = BodySleepState(iota)
+	Sleepy
+	Sleeping
+)
+
+// Events
+const (
+	SleepyEvent  = "physics.SleepyEvent"  // Dispatched after a body has gone in to the sleepy state.
+	SleepEvent   = "physics.SleepEvent"   // Dispatched after a body has fallen asleep.
+	WakeUpEvent  = "physics.WakeUpEvent"  // Dispatched after a sleeping body has woken up.
+	CollideEvent = "physics.CollideEvent" // Dispatched after two bodies collide. This event is dispatched on each of the two bodies involved in the collision.
+)
+
+
+// NewBody creates and returns a pointer to a new RigidBody.
+func NewBody(inode core.INode) *Body {
+
+	b := new(Body)
+	b.INode = inode
+
+	// 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 // auto set to Static if mass == 0
+
+	b.collisionFilterGroup = 1
+	b.collisionFilterMask = -1
+
+	pos := inode.GetNode().Position()
+	b.position 			= math32.NewVector3(0,0,0).Copy(&pos)
+	b.prevPosition 		= math32.NewVector3(0,0,0).Copy(&pos)
+	b.interpPosition 	= math32.NewVector3(0,0,0).Copy(&pos)
+	b.initPosition 		= math32.NewVector3(0,0,0).Copy(&pos)
+
+	quat := inode.GetNode().Quaternion()
+	b.quaternion 		= math32.NewQuaternion(0,0,0,1).Copy(&quat)
+	b.prevQuaternion 	= math32.NewQuaternion(0,0,0,1).Copy(&quat)
+	b.interpQuaternion 	= math32.NewQuaternion(0,0,0,1).Copy(&quat)
+	b.initQuaternion 	= math32.NewQuaternion(0,0,0,1).Copy(&quat)
+
+	b.velocity = math32.NewVector3(0,0,0) // TODO copy options.velocity
+	b.initVelocity = math32.NewVector3(0,0,0) // don't copy
+
+	b.angularVelocity = math32.NewVector3(0,0,0)
+	b.initAngularVelocity = math32.NewVector3(0,0,0)
+
+	b.vLambda = math32.NewVector3(0,0,0)
+	b.wLambda = math32.NewVector3(0,0,0)
+
+	b.linearDamping = 0.01
+	b.angularDamping = 0.01
+
+	b.linearFactor = math32.NewVector3(1,1,1)
+	b.angularFactor = math32.NewVector3(1,1,1)
+
+	b.allowSleep = true
+	b.sleepState = Awake
+	b.sleepSpeedLimit = 0.1
+	b.sleepTimeLimit = 1
+	b.timeLastSleepy = 0
+
+	b.force = math32.NewVector3(0,0,0)
+	b.torque = math32.NewVector3(0,0,0)
+
+	b.wakeUpAfterNarrowphase = false
+
+	b.UpdateMassProperties()
+
+	return b
+}
+
+func (b *Body) Position() math32.Vector3 {
+
+	return *b.position
+}
+
+func (b *Body) SetVelocity(vel *math32.Vector3) {
+
+	b.velocity = vel
+}
+
+func (b *Body) AddToVelocity(delta *math32.Vector3) {
+
+	b.velocity.Add(delta)
+}
+
+func (b *Body) Velocity() math32.Vector3 {
+
+	return *b.velocity
+}
+
+func (b *Body) SetAngularVelocity(vel *math32.Vector3) {
+
+	b.angularVelocity = vel
+}
+
+func (b *Body) AddToAngularVelocity(delta *math32.Vector3) {
+
+	b.angularVelocity.Add(delta)
+}
+
+func (b *Body) AngularVelocity() math32.Vector3 {
+
+	return *b.angularVelocity
+}
+
+func (b *Body) Force() math32.Vector3 {
+
+	return *b.force
+}
+
+func (b *Body) Torque() math32.Vector3 {
+
+	return *b.torque
+}
+
+func (b *Body) SetVlambda(vLambda *math32.Vector3) {
+
+	b.vLambda = vLambda
+}
+
+func (b *Body) AddToVlambda(delta *math32.Vector3) {
+
+	b.vLambda.Add(delta)
+}
+
+func (b *Body) Vlambda() math32.Vector3 {
+
+	return *b.vLambda
+}
+
+func (b *Body) SetWlambda(wLambda *math32.Vector3) {
+
+	b.wLambda = wLambda
+}
+
+func (b *Body) AddToWlambda(delta *math32.Vector3) {
+
+	b.wLambda.Add(delta)
+}
+
+func (b *Body) Wlambda() math32.Vector3 {
+
+	return *b.wLambda
+}
+
+func (b *Body) InvMassSolve() float32 {
+
+	return b.invMassSolve
+}
+
+func (b *Body) InvInertiaWorldSolve() *math32.Matrix3 {
+
+	return b.invInertiaWorldSolve
+}
+
+func (b *Body) Quaternion() *math32.Quaternion {
+
+	return b.quaternion
+}
+
+func (b *Body) LinearFactor() *math32.Vector3 {
+
+	return b.linearFactor
+}
+
+func (b *Body) AngularFactor() *math32.Vector3 {
+
+	return b.angularFactor
+}
+
+// WakeUp wakes the body up.
+func (b *Body) WakeUp() {
+
+	state := b.sleepState
+	b.sleepState = Awake
+	b.wakeUpAfterNarrowphase = false
+	if state == Sleeping {
+		b.Dispatch(WakeUpEvent, nil)
+	}
+}
+
+// Sleep forces the body to sleep.
+func (b *Body) Sleep() {
+
+	b.sleepState = Sleeping
+	b.velocity.Set(0,0,0)
+	b.angularVelocity.Set(0,0,0)
+	b.wakeUpAfterNarrowphase = false
+}
+
+// Called every timestep to update internal sleep timer and change sleep state if needed.
+// time: The world time in seconds
+func (b *Body) SleepTick(time float32) {
+
+	if b.allowSleep {
+		speedSquared := b.velocity.LengthSq() + b.angularVelocity.LengthSq()
+		speedLimitSquared := math32.Pow(b.sleepSpeedLimit,2)
+		if b.sleepState == Awake && speedSquared < speedLimitSquared {
+			b.sleepState = Sleepy
+			b.timeLastSleepy = time
+			b.Dispatch(SleepyEvent, nil)
+		} else if b.sleepState == Sleepy && speedSquared > speedLimitSquared {
+			b.WakeUp() // Wake up
+		} else if b.sleepState == Sleepy && (time - b.timeLastSleepy ) > b.sleepTimeLimit {
+			b.Sleep() // Sleeping
+			b.Dispatch(SleepEvent, nil)
+		}
+	}
+}
+
+// TODO maybe return vector instead of pointer in below methods
+
+// PointToLocal converts a world point to local body frame. TODO maybe move to Node
+func (b *Body) PointToLocal(worldPoint *math32.Vector3) math32.Vector3 {
+
+	result := math32.NewVector3(0,0,0).SubVectors(worldPoint, b.position)
+	conj := b.quaternion.Conjugate()
+	result.ApplyQuaternion(conj)
+
+	return *result
+}
+
+// VectorToLocal converts a world vector to local body frame. TODO maybe move to Node
+func (b *Body) VectorToLocal(worldVector *math32.Vector3) math32.Vector3 {
+
+	result := math32.NewVector3(0,0,0).Copy(worldVector)
+	conj := b.quaternion.Conjugate()
+	result.ApplyQuaternion(conj)
+
+	return *result
+}
+
+// PointToWorld converts a local point to world frame. TODO maybe move to Node
+func (b *Body) PointToWorld(localPoint *math32.Vector3) math32.Vector3 {
+
+	result := math32.NewVector3(0,0,0).Copy(localPoint)
+	result.ApplyQuaternion(b.quaternion)
+	result.Add(b.position)
+
+	return *result
+}
+
+// VectorToWorld converts a local vector to world frame. TODO maybe move to Node
+func (b *Body) VectorToWorld(localVector *math32.Vector3) math32.Vector3 {
+
+	result := math32.NewVector3(0,0,0).Copy(localVector)
+	result.ApplyQuaternion(b.quaternion)
+
+	return *result
+}
+
+// UpdateSolveMassProperties
+// If the body is sleeping, it should be immovable / have infinite mass during solve. We solve it by having a separate "solve mass".
+func (b *Body) UpdateSolveMassProperties() {
+
+	if b.sleepState == Sleeping || b.bodyType == Kinematic {
+		b.invMassSolve = 0
+		b.invInertiaSolve.Zero()
+		b.invInertiaWorldSolve.Zero()
+	} else {
+		b.invMassSolve = b.invMass
+		b.invInertiaSolve.Copy(b.invInertia)
+		b.invInertiaWorldSolve.Copy(b.invInertiaWorld)
+	}
+}
+
+// UpdateMassProperties // TODO
+// Should be called whenever you change the body shape or mass.
+func (b *Body) UpdateMassProperties() {
+
+	//b.invMass = b.mass > 0 ? 1.0 / b.mass : 0 // TODO getter of invMass
+	//
+	//// Approximate with AABB box
+	//b.computeAABB()
+	//halfExtents := math32.NewVector3(0,0,0).Set(
+	//	(b.aabb.upperBound.x-b.aabb.lowerBound.x) / 2,
+	//	(b.aabb.upperBound.y-b.aabb.lowerBound.y) / 2,
+	//	(b.aabb.upperBound.z-b.aabb.lowerBound.z) / 2
+	//)
+	//Box.calculateInertia(halfExtents, b.mass, b.inertia)
+	//
+	//b.invInertia.set(
+	//	b.inertia.x > 0 && !b.fixedRotation ? 1.0 / b.inertia.x : 0,
+	//	b.inertia.y > 0 && !b.fixedRotation ? 1.0 / b.inertia.y : 0,
+	//	b.inertia.z > 0 && !b.fixedRotation ? 1.0 / b.inertia.z : 0
+	//)
+	//b.updateInertiaWorld(true)
+}
+
+// Update .inertiaWorld and .invInertiaWorld
+func (b *Body) UpdateInertiaWorld(force *math32.Vector3) {
+
+    I := b.invInertia
+    if I.X == I.Y && I.Y == I.Z && force == nil { // TODO clean
+        // If inertia M = s*I, where I is identity and s a scalar, then
+        //    R*M*R' = R*(s*I)*R' = s*R*I*R' = s*R*R' = s*I = M
+        // where R is the rotation matrix.
+        // In other words, we don't have to transform the inertia if all
+        // inertia diagonal entries are equal.
+    } else {
+        m1 := math32.NewMatrix3()
+        m2 := math32.NewMatrix3()
+
+        //m1.MakeRotationFromQuaternion(b.quaternion) // TODO
+		//m2.Copy(m1)
+		//m2.Transpose()
+        //m1.Scale(I,m1) // TODO scale matrix columns
+
+		b.invInertiaWorld.MultiplyMatrices(m1, m2)
+    }
+}
+
+// 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.
+// relativePoint: A point relative to the center of mass to apply the force on.
+func (b *Body) ApplyForce(force, relativePoint *math32.Vector3) {
+
+	if b.bodyType != Dynamic { // Needed?
+		return
+	}
+
+	// Compute produced rotational force
+	rotForce := math32.NewVector3(0,0,0)
+	rotForce.CrossVectors(relativePoint, force)
+
+	// Add linear force
+	b.force.Add(force) // TODO shouldn't rotational momentum be subtracted from linear momentum?
+
+	// Add rotational force
+	b.torque.Add(rotForce)
+}
+
+// Apply force to a local point in the body.
+// force: The force vector to apply, defined locally in the body frame.
+// localPoint: A local point in the body to apply the force on.
+func (b *Body) ApplyLocalForce(localForce, localPoint *math32.Vector3)  {
+
+	if b.bodyType != Dynamic {
+		return
+	}
+
+	// Transform the force vector to world space
+	worldForce := b.VectorToWorld(localForce)
+	relativePointWorld := b.VectorToWorld(localPoint)
+
+	b.ApplyForce(&worldForce, &relativePointWorld)
+}
+
+// Apply impulse to a world point.
+// This could for example be a point on the Body surface.
+// An impulse is a force added to a body during a short period of time (impulse = force * time).
+// Impulses will be added to Body.velocity and Body.angularVelocity.
+// impulse: The amount of impulse to add.
+// relativePoint: A point relative to the center of mass to apply the force on.
+func (b *Body) ApplyImpulse(impulse, relativePoint *math32.Vector3) {
+
+	if b.bodyType != Dynamic {
+		return
+	}
+
+    // Compute point position relative to the body center
+    r := relativePoint
+
+    // Compute produced central impulse velocity
+    velo := math32.NewVector3(0,0,0).Copy(impulse)
+    velo.MultiplyScalar(b.invMass)
+
+    // Add linear impulse
+    b.velocity.Add(velo)
+
+    // Compute produced rotational impulse velocity
+	rotVelo := math32.NewVector3(0,0,0).CrossVectors(r, impulse)
+
+    /*
+    rotVelo.x *= this.invInertia.x
+    rotVelo.y *= this.invInertia.y
+    rotVelo.z *= this.invInertia.z
+    */
+	rotVelo.ApplyMatrix3(b.invInertiaWorld)
+
+    // Add rotational Impulse
+    b.angularVelocity.Add(rotVelo)
+}
+
+// Apply locally-defined impulse to a local point in the body.
+// force: The force vector to apply, defined locally in the body frame.
+// localPoint: A local point in the body to apply the force on.
+func (b *Body) ApplyLocalImpulse(localImpulse, localPoint *math32.Vector3) {
+
+	if b.bodyType != Dynamic {
+		return
+	}
+
+	// Transform the force vector to world space
+	worldImpulse := b.VectorToWorld(localImpulse)
+	relativePointWorld := b.VectorToWorld(localPoint)
+
+	b.ApplyImpulse(&worldImpulse, &relativePointWorld)
+}
+
+// Get world velocity of a point in the body.
+func (b *Body) GetVelocityAtWorldPoint(worldPoint *math32.Vector3) *math32.Vector3 {
+
+	r := math32.NewVector3(0,0,0)
+	r.SubVectors(worldPoint, b.position)
+	r.CrossVectors(b.angularVelocity, r)
+	r.Add(b.velocity)
+
+	return r
+}
+
+// Move the body forward in time.
+// dt: Time step
+// quatNormalize: Set to true to normalize the body quaternion
+// quatNormalizeFast: If the quaternion should be normalized using "fast" quaternion normalization
+func (b *Body) Integrate(dt float32, quatNormalize, quatNormalizeFast bool) {
+
+
+    // Save previous position
+    b.prevPosition.Copy(b.position)
+    b.prevQuaternion.Copy(b.quaternion)
+
+    // If static or sleeping - skip
+    if !(b.bodyType == Dynamic || b.bodyType == Kinematic) || b.sleepState == Sleeping {
+        return
+    }
+
+    iMdt := b.invMass * dt
+    b.velocity.X += b.force.X * iMdt * b.linearFactor.X
+    b.velocity.Y += b.force.Y * iMdt * b.linearFactor.Y
+    b.velocity.Z += b.force.Z * iMdt * b.linearFactor.Z
+
+    //e := b.invInertiaWorld.elements // TODO
+    //tx := b.torque.X * b.angularFactor.X
+    //ty := b.torque.Y * b.angularFactor.Y
+    //tz := b.torque.Z * b.angularFactor.Z
+    //b.angularVelocity.X += dt * (e[0] * tx + e[1] * ty + e[2] * tz)
+    //b.angularVelocity.Y += dt * (e[3] * tx + e[4] * ty + e[5] * tz)
+    //b.angularVelocity.Z += dt * (e[6] * tx + e[7] * ty + e[8] * tz)
+	//
+    //// Use new velocity  - leap frog
+    //b.position.X += b.velocity.X * dt
+    //b.position.Y += b.velocity.Y * dt
+    //b.position.Z += b.velocity.Z * dt
+	//
+	//b.quaternion.integrate(b.angularVelocity, dt, b.angularFactor, b.quaternion) // TODO
+	//
+    //if quatNormalize {
+    //    if quatNormalizeFast {
+		//	b.quaternion.normalizeFast() // TODO
+    //    } else {
+		//	b.quaternion.normalize() // TODO
+    //    }
+    //}
+	//
+    //b.aabbNeedsUpdate = true  // TODO
+
+    // Update world inertia
+    b.UpdateInertiaWorld(nil)
+}

physics/constraint/conetwist.go

@@ -0,0 +1,84 @@
+// 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 constraint
+
+import (
+	"github.com/g3n/engine/math32"
+	"github.com/g3n/engine/physics/equation"
+)
+
+// ConeTwist constraint.
+type ConeTwist struct {
+	PointToPoint
+	axisA      *math32.Vector3 // Rotation axis, defined locally in bodyA.
+	axisB      *math32.Vector3 // Rotation axis, defined locally in bodyB.
+	coneEq     *equation.Cone
+	twistEq    *equation.Rotational
+	angle      float32
+	twistAngle float32
+}
+
+// NewConeTwist creates and returns a pointer to a new ConeTwist constraint object.
+func NewConeTwist(bodyA, bodyB IBody, pivotA, pivotB, axisA, axisB *math32.Vector3, angle, twistAngle, maxForce float32) *ConeTwist {
+
+	ctc := new(ConeTwist)
+
+	// Default of pivots and axes should be vec3(0)
+
+	ctc.initialize(bodyA, bodyB, pivotA, pivotB, maxForce)
+
+	ctc.axisA = axisA
+	ctc.axisB = axisB
+	ctc.axisA.Normalize()
+	ctc.axisB.Normalize()
+
+	ctc.angle = angle
+	ctc.twistAngle = twistAngle
+
+	ctc.coneEq = equation.NewCone(bodyA, bodyB, ctc.axisA, ctc.axisB, ctc.angle, maxForce)
+
+	ctc.twistEq = equation.NewRotational(bodyA, bodyB, maxForce)
+	ctc.twistEq.SetAxisA(ctc.axisA)
+	ctc.twistEq.SetAxisB(ctc.axisB)
+
+	// Make the cone equation push the bodies toward the cone axis, not outward
+	ctc.coneEq.SetMaxForce(0)
+	ctc.coneEq.SetMinForce(-maxForce)
+
+	// Make the twist equation add torque toward the initial position
+	ctc.twistEq.SetMaxForce(0)
+	ctc.twistEq.SetMinForce(-maxForce)
+
+	ctc.AddEquation(&ctc.coneEq.Equation)
+	ctc.AddEquation(&ctc.twistEq.Equation)
+
+	return ctc
+}
+
+// Update updates the equations with data.
+func (ctc *ConeTwist) Update() {
+
+	ctc.PointToPoint.Update()
+
+	// Update the axes to the cone constraint
+	worldAxisA := ctc.bodyA.VectorToWorld(ctc.axisA)
+	worldAxisB := ctc.bodyB.VectorToWorld(ctc.axisB)
+
+	ctc.coneEq.SetAxisA(&worldAxisA)
+	ctc.coneEq.SetAxisB(&worldAxisB)
+
+	// Update the world axes in the twist constraint
+	tA, _ := ctc.axisA.RandomTangents()
+	worldTA := ctc.bodyA.VectorToWorld(tA)
+	ctc.twistEq.SetAxisA(&worldTA)
+
+	tB, _ := ctc.axisB.RandomTangents()
+	worldTB := ctc.bodyB.VectorToWorld(tB)
+	ctc.twistEq.SetAxisB(&worldTB)
+
+	ctc.coneEq.SetAngle(ctc.angle)
+	ctc.twistEq.SetMaxAngle(ctc.twistAngle)
+}

physics/constraint/constraint.go

@@ -0,0 +1,71 @@
+// 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 constraint
+
+import (
+	"github.com/g3n/engine/physics/equation"
+	"github.com/g3n/engine/math32"
+)
+
+type IBody interface {
+	equation.IBody
+	WakeUp()
+	VectorToWorld(*math32.Vector3) math32.Vector3
+	PointToLocal(*math32.Vector3) math32.Vector3
+	VectorToLocal(*math32.Vector3) math32.Vector3
+	Quaternion() *math32.Quaternion
+}
+
+type IConstraint interface {
+	Update() // Update all the equations with data.
+}
+
+// Constraint base struct.
+type Constraint struct {
+	equations []*equation.Equation // 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.
+}
+
+// NewConstraint creates and returns a pointer to a new Constraint object.
+func NewConstraint(bodyA, bodyB IBody, collideConnected, wakeUpBodies bool) *Constraint {
+
+	c := new(Constraint)
+	c.initialize(bodyA, bodyB, collideConnected, wakeUpBodies)
+	return c
+}
+
+func (c *Constraint) initialize(bodyA, bodyB IBody, collideConnected, wakeUpBodies bool) {
+
+	c.bodyA = bodyA
+	c.bodyB = bodyB
+	c.collideConnected = collideConnected // true
+
+	if wakeUpBodies { // true
+		if bodyA != nil {
+			bodyA.WakeUp()
+		}
+		if bodyB != nil {
+			bodyB.WakeUp()
+		}
+	}
+}
+
+// AddEquation adds an equation to the constraint.
+func (c *Constraint) AddEquation(eq *equation.Equation) {
+
+	c.equations = append(c.equations, eq)
+}
+
+// SetEnable sets the enabled flag of the constraint equations.
+func (c *Constraint) SetEnabled(state bool) {
+
+	for i := range c.equations {
+		c.equations[i].SetEnabled(state)
+	}
+}

physics/constraint/distance.go

@@ -0,0 +1,51 @@
+// 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 constraint
+
+import (
+	"github.com/g3n/engine/physics/equation"
+)
+
+// Distance is a distance constraint.
+// Constrains two bodies to be at a constant distance from each others center of mass.
+type Distance struct {
+	Constraint
+	distance   float32 // Distance
+	equation   *equation.Contact
+}
+
+// NewDistance creates and returns a pointer to a new Distance constraint object.
+func NewDistance(bodyA, bodyB IBody, distance, maxForce float32) *Distance {
+
+	dc := new(Distance)
+	dc.initialize(bodyA, bodyB, true, true)
+
+	// Default distance should be: bodyA.position.distanceTo(bodyB.position)
+	// Default maxForce should be: 1e6
+
+	dc.distance = distance
+
+	dc.equation = equation.NewContact(bodyA, bodyB, -maxForce, maxForce) // Make it bidirectional
+	dc.equations = append(dc.equations, &dc.equation.Equation)
+
+
+	return dc
+}
+
+// Update updates the equation with data.
+func (dc *Distance) Update() {
+
+	halfDist := dc.distance * 0.5
+
+	posA := dc.bodyA.Position()
+	posB := dc.bodyB.Position()
+
+	normal := posB.Sub(&posA)
+	normal.Normalize()
+
+	dc.equation.SetRA(normal.Clone().MultiplyScalar(halfDist))
+	dc.equation.SetRB(normal.Clone().MultiplyScalar(-halfDist))
+}

physics/constraint/hinge.go

@@ -0,0 +1,88 @@
+// 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 constraint
+
+import (
+	"github.com/g3n/engine/physics/equation"
+	"github.com/g3n/engine/math32"
+)
+
+// Hinge constraint.
+// Think of it as a door hinge.
+// It tries to keep the door in the correct place and with the correct orientation.
+type Hinge struct {
+	PointToPoint
+	axisA   *math32.Vector3           // Rotation axis, defined locally in bodyA.
+	axisB   *math32.Vector3           // Rotation axis, defined locally in bodyB.
+	rotEq1  *equation.Rotational
+	rotEq2  *equation.Rotational
+	motorEq *equation.RotationalMotor
+
+}
+
+// NewHinge creates and returns a pointer to a new Hinge constraint object.
+func NewHinge(bodyA, bodyB IBody, pivotA, pivotB, axisA, axisB *math32.Vector3, maxForce float32) *Hinge {
+
+	hc := new(Hinge)
+
+	hc.initialize(bodyA, bodyB, pivotA, pivotB, maxForce)
+
+	hc.axisA = axisA
+	hc.axisB = axisB
+	hc.axisA.Normalize()
+	hc.axisB.Normalize()
+
+	hc.rotEq1 = equation.NewRotational(bodyA, bodyB, maxForce)
+	hc.rotEq2 = equation.NewRotational(bodyA, bodyB, maxForce)
+	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)
+
+	return hc
+}
+
+func (hc *Hinge) SetMotorEnabled(state bool) {
+
+	hc.motorEq.SetEnabled(state)
+}
+
+func (hc *Hinge) SetMotorSpeed(speed float32) {
+
+	hc.motorEq.SetTargetSpeed(speed)
+}
+
+func (hc *Hinge) SetMotorMaxForce(maxForce float32) {
+
+	hc.motorEq.SetMaxForce(maxForce)
+	hc.motorEq.SetMinForce(-maxForce)
+}
+
+// Update updates the equations with data.
+func (hc *Hinge) Update() {
+
+	hc.PointToPoint.Update()
+
+	// Get world axes
+	quatA := hc.bodyA.Quaternion()
+	quatB := hc.bodyB.Quaternion()
+
+	worldAxisA := hc.axisA.Clone().ApplyQuaternion(quatA)
+	worldAxisB := hc.axisB.Clone().ApplyQuaternion(quatB)
+
+	t1, t2 := worldAxisA.RandomTangents()
+	hc.rotEq1.SetAxisA(t1)
+	hc.rotEq2.SetAxisA(t2)
+	hc.rotEq1.SetAxisB(worldAxisB)
+	hc.rotEq2.SetAxisB(worldAxisB)
+
+	if hc.motorEq.Enabled() {
+		hc.motorEq.SetAxisA(hc.axisA.Clone().ApplyQuaternion(quatA))
+		hc.motorEq.SetAxisB(hc.axisB.Clone().ApplyQuaternion(quatB))
+	}
+}

physics/constraint/lock.go

@@ -0,0 +1,96 @@
+// 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 constraint
+
+import (
+	"github.com/g3n/engine/physics/equation"
+	"github.com/g3n/engine/math32"
+)
+
+// Lock constraint.
+// Removes all degrees of freedom between the bodies.
+type Lock struct {
+	PointToPoint
+	rotEq1 *equation.Rotational
+	rotEq2 *equation.Rotational
+	rotEq3 *equation.Rotational
+	xA     *math32.Vector3
+	xB     *math32.Vector3
+	yA     *math32.Vector3
+	yB     *math32.Vector3
+	zA     *math32.Vector3
+	zB     *math32.Vector3
+}
+
+// NewLock creates and returns a pointer to a new Lock constraint object.
+func NewLock(bodyA, bodyB IBody, maxForce float32) *Lock {
+
+	lc := new(Lock)
+
+	// Set pivot point in between
+	posA := bodyA.Position()
+	posB := bodyB.Position()
+
+	halfWay := math32.NewVec3().AddVectors(&posA, &posB)
+	halfWay.MultiplyScalar(0.5)
+
+	pivotB := bodyB.PointToLocal(halfWay)
+	pivotA := bodyA.PointToLocal(halfWay)
+
+	// The point-to-point constraint will keep a point shared between the bodies
+	lc.initialize(bodyA, bodyB, &pivotA, &pivotB, maxForce)
+
+	// Store initial rotation of the bodies as unit vectors in the local body spaces
+	UnitX := math32.NewVector3(1,0,0)
+
+	localA := bodyA.VectorToLocal(UnitX)
+	localB := bodyB.VectorToLocal(UnitX)
+
+	lc.xA = &localA
+	lc.xB = &localB
+	lc.yA = &localA
+	lc.yB = &localB
+	lc.zA = &localA
+	lc.zB = &localB
+
+	// ...and the following rotational equations will keep all rotational DOF's in place
+
+	lc.rotEq1 = equation.NewRotational(bodyA, bodyB, maxForce)
+	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)
+
+	return lc
+}
+
+// Update updates the equations with data.
+func (lc *Lock) Update() {
+
+	lc.PointToPoint.Update()
+
+	// These vector pairs must be orthogonal
+
+	xAw := lc.bodyA.VectorToWorld(lc.xA)
+	yBw := lc.bodyA.VectorToWorld(lc.yB)
+
+	yAw := lc.bodyA.VectorToWorld(lc.yA)
+	zBw := lc.bodyB.VectorToWorld(lc.zB)
+
+	zAw := lc.bodyA.VectorToWorld(lc.zA)
+	xBw := lc.bodyB.VectorToWorld(lc.xB)
+
+	lc.rotEq1.SetAxisA(&xAw)
+	lc.rotEq1.SetAxisB(&yBw)
+
+	lc.rotEq2.SetAxisA(&yAw)
+	lc.rotEq2.SetAxisB(&zBw)
+
+	lc.rotEq3.SetAxisA(&zAw)
+	lc.rotEq3.SetAxisB(&xBw)
+}

physics/constraint/pointtopoint.go

@@ -0,0 +1,66 @@
+// 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 constraint
+
+import (
+	"github.com/g3n/engine/physics/equation"
+	"github.com/g3n/engine/math32"
+)
+
+// PointToPoint is an offset constraint.
+// Connects two bodies at the specified offset points.
+type PointToPoint struct {
+	Constraint
+	pivotA *math32.Vector3   // Pivot, defined locally in bodyA.
+	pivotB *math32.Vector3   // Pivot, defined locally in bodyB.
+	eqX    *equation.Contact
+	eqY    *equation.Contact
+	eqZ    *equation.Contact
+}
+
+// NewPointToPoint creates and returns a pointer to a new PointToPoint constraint object.
+func NewPointToPoint(bodyA, bodyB IBody, pivotA, pivotB *math32.Vector3, maxForce float32) *PointToPoint {
+
+	ptpc := new(PointToPoint)
+	ptpc.initialize(bodyA, bodyB, pivotA, pivotB, maxForce)
+
+	return ptpc
+}
+
+func (ptpc *PointToPoint) initialize(bodyA, bodyB IBody, pivotA, pivotB *math32.Vector3, maxForce float32) {
+
+	ptpc.Constraint.initialize(bodyA, bodyB, true, true)
+
+	ptpc.pivotA = pivotA // default is zero vec3
+	ptpc.pivotB = pivotB // default is zero vec3
+
+	ptpc.eqX = equation.NewContact(bodyA, bodyB, -maxForce, maxForce)
+	ptpc.eqY = equation.NewContact(bodyA, bodyB, -maxForce, maxForce)
+	ptpc.eqZ = equation.NewContact(bodyA, bodyB, -maxForce, maxForce)
+
+	ptpc.eqX.SetNormal(&math32.Vector3{1,0,0})
+	ptpc.eqY.SetNormal(&math32.Vector3{0,1,0})
+	ptpc.eqZ.SetNormal(&math32.Vector3{0,0,1})
+
+	ptpc.AddEquation(&ptpc.eqX.Equation)
+	ptpc.AddEquation(&ptpc.eqY.Equation)
+	ptpc.AddEquation(&ptpc.eqZ.Equation)
+}
+
+// Update updates the equations with data.
+func (ptpc *PointToPoint) Update() {
+
+	// Rotate the pivots to world space
+	xRi := ptpc.pivotA.Clone().ApplyQuaternion(ptpc.bodyA.Quaternion())
+	xRj := ptpc.pivotA.Clone().ApplyQuaternion(ptpc.bodyA.Quaternion())
+
+	ptpc.eqX.SetRA(xRi)
+	ptpc.eqX.SetRB(xRj)
+	ptpc.eqY.SetRA(xRi)
+	ptpc.eqY.SetRB(xRj)
+	ptpc.eqZ.SetRA(xRi)
+	ptpc.eqZ.SetRB(xRj)
+}

physics/equation/cone.go

@@ -0,0 +1,89 @@
+// 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 equation
+
+import (
+	"github.com/g3n/engine/math32"
+)
+
+// Cone is a cone constraint equation.
+// Works to keep the given body world vectors aligned, or tilted within a given angle from each other.
+type Cone struct {
+	Equation
+	axisA    *math32.Vector3 // Local axis in A
+	axisB    *math32.Vector3 // Local axis in B
+	angle    float32         // The "cone angle" to keep
+}
+
+// NewCone creates and returns a pointer to a new Cone equation object.
+func NewCone(bodyA, bodyB IBody, axisA, axisB *math32.Vector3, angle, maxForce float32) *Cone {
+
+	ce := new(Cone)
+
+	ce.axisA = axisA // new Vec3(1, 0, 0)
+	ce.axisB = axisB // new Vec3(0, 1, 0)
+	ce.angle = angle // 0
+
+	ce.Equation.initialize(bodyA, bodyB, -maxForce, maxForce)
+
+	return ce
+}
+
+// SetAxisA sets the axis of body A.
+func (ce *Cone) SetAxisA(axisA *math32.Vector3) {
+
+	ce.axisA = axisA
+}
+
+// AxisA returns the axis of body A.
+func (ce *Cone) AxisA() math32.Vector3 {
+
+	return *ce.axisA
+}
+
+// SetAxisB sets the axis of body B.
+func (ce *Cone) SetAxisB(axisB *math32.Vector3) {
+
+	ce.axisB = axisB
+}
+
+// AxisB returns the axis of body B.
+func (ce *Cone) AxisB() math32.Vector3 {
+
+	return *ce.axisB
+}
+
+// SetAngle sets the cone angle.
+func (ce *Cone) SetAngle(angle float32) {
+
+	ce.angle = angle
+}
+
+// MaxAngle returns the cone angle.
+func (ce *Cone) Angle() float32 {
+
+	return ce.angle
+}
+
+// ComputeB
+func (ce *Cone) ComputeB(h float32) float32 {
+
+	// The angle between two vector is:
+	// cos(theta) = a * b / (length(a) * length(b) = { len(a) = len(b) = 1 } = a * b
+
+	// g = a * b
+	// gdot = (b x a) * wi + (a x b) * wj
+	// G = [0 bxa 0 axb]
+	// W = [vi wi vj wj]
+	ce.jeA.SetRotational(math32.NewVec3().CrossVectors(ce.axisB, ce.axisA))
+	ce.jeB.SetRotational(math32.NewVec3().CrossVectors(ce.axisA, ce.axisB))
+
+	g := math32.Cos(ce.angle) - ce.axisA.Dot(ce.axisB)
+	GW := ce.ComputeGW()
+	GiMf := ce.ComputeGiMf()
+
+	return -g*ce.a - GW*ce.b - h*GiMf
+}

physics/equation/contact.go

@@ -0,0 +1,117 @@
+// 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 equation
+
+import (
+	"github.com/g3n/engine/math32"
+)
+
+// Contact is a contact/non-penetration constraint equation.
+type Contact struct {
+	Equation
+	restitution float32         // "bounciness": u1 = -e*u0
+	rA          *math32.Vector3 // World-oriented vector that goes from the center of bA to the contact point.
+	rB          *math32.Vector3 // World-oriented vector that goes from the center of bB to the contact point.
+	nA          *math32.Vector3 // Contact normal, pointing out of body A.
+}
+
+// NewContact creates and returns a pointer to a new Contact equation object.
+func NewContact(bodyA, bodyB IBody, minForce, maxForce float32) *Contact {
+
+	ce := new(Contact)
+
+	// minForce default should be 0.
+
+	ce.restitution = 0
+	ce.rA = &math32.Vector3{0,0,0}
+	ce.rB = &math32.Vector3{0,0,0}
+	ce.nA = &math32.Vector3{0,0,0}
+
+	ce.Equation.initialize(bodyA, bodyB, minForce, maxForce)
+
+	return ce
+}
+
+func (ce *Contact) SetNormal(newNormal *math32.Vector3)  {
+
+	ce.nA = newNormal
+}
+
+func (ce *Contact) Normal() math32.Vector3 {
+
+	return *ce.nA
+}
+
+func (ce *Contact) SetRA(newRi *math32.Vector3)  {
+
+	ce.rA = newRi
+}
+
+func (ce *Contact) RA() math32.Vector3 {
+
+	return *ce.rA
+}
+
+func (ce *Contact) SetRB(newRj *math32.Vector3)  {
+
+	ce.rB = newRj
+}
+
+func (ce *Contact) RB() math32.Vector3 {
+
+	return *ce.rB
+}
+
+// ComputeB
+func (ce *Contact) ComputeB(h float32) float32 {
+
+	vA := ce.bA.Velocity()
+	wA := ce.bA.AngularVelocity()
+
+	vB := ce.bB.Velocity()
+	wB := ce.bB.AngularVelocity()
+
+	// Calculate cross products
+	rnA := math32.NewVec3().CrossVectors(ce.rA, ce.nA)
+	rnB := math32.NewVec3().CrossVectors(ce.rB, ce.nA)
+
+	// g = xj+rB -(xi+rA)
+	// G = [ -nA  -rnA  nA  rnB ]
+	ce.jeA.SetSpatial(ce.nA.Clone().Negate())
+	ce.jeA.SetRotational(rnA.Clone().Negate())
+	ce.jeB.SetSpatial(ce.nA.Clone())
+	ce.jeB.SetRotational(rnB.Clone())
+
+	// 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)
+
+	g := ce.nA.Dot(penetrationVec)
+
+	// Compute iteration
+	ePlusOne := ce.restitution + 1
+	GW := ePlusOne * vB.Dot(ce.nA) - ePlusOne * vA.Dot(ce.nA) + wB.Dot(rnB) - wA.Dot(rnA)
+	GiMf := ce.ComputeGiMf()
+
+	return -g*ce.a - GW*ce.b - h*GiMf
+}
+
+//// GetImpactVelocityAlongNormal returns the current relative velocity at the contact point.
+//func (ce *Contact) GetImpactVelocityAlongNormal() float32 {
+//
+//	xi := ce.bA.Position().Add(ce.rA)
+//	xj := ce.bB.Position().Add(ce.rB)
+//
+//	vi := ce.bA.GetVelocityAtWorldPoint(xi)
+//	vj := ce.bB.GetVelocityAtWorldPoint(xj)
+//
+//	relVel := math32.NewVec3().SubVectors(vi, vj)
+//
+//    return ce.nA.Dot(relVel)
+//}

physics/equation/equation.go

@@ -0,0 +1,218 @@
+// 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 equation
+
+import (
+	"github.com/g3n/engine/math32"
+)
+
+type IBody interface {
+	Position() math32.Vector3
+	Velocity() math32.Vector3
+	//Vlambda() math32.Vector3
+	//AddToVlambda(*math32.Vector3)
+	AngularVelocity() math32.Vector3
+	//Wlambda() math32.Vector3
+	//AddToWlambda(*math32.Vector3)
+	Force() math32.Vector3
+	Torque() math32.Vector3
+	InvMassSolve() float32
+	InvInertiaWorldSolve() *math32.Matrix3
+
+	Index() int
+}
+
+// Equation is a SPOOK constraint equation.
+type Equation struct {
+	id         int
+	minForce   float32       // Minimum (read: negative max) force to be applied by the constraint.
+	maxForce   float32       // Maximum (read: positive max) force to be applied by the constraint.
+	bA         IBody // Body "i"
+	bB         IBody // Body "j"
+	a          float32       // SPOOK parameter
+	b          float32       // SPOOK parameter
+	eps        float32       // SPOOK parameter
+	jeA        JacobianElement
+	jeB        JacobianElement
+	enabled    bool
+	multiplier float32 // A number, proportional to the force added to the bodies.
+}
+
+// NewEquation creates and returns a pointer to a new Equation object.
+func NewEquation(bi, bj IBody, minForce, maxForce float32) *Equation {
+
+	e := new(Equation)
+	e.initialize(bi, bj, minForce, maxForce)
+	return e
+}
+
+func (e *Equation) initialize(bi, bj IBody, minForce, maxForce float32) {
+
+	//e.id = Equation.id++
+	e.minForce = minForce //-1e6
+	e.maxForce = maxForce //1e6
+
+	e.bA = bi
+	e.bB = bj
+	e.a = 0
+	e.b = 0
+	e.eps = 0
+	e.enabled = true
+	e.multiplier = 0
+
+	// Set typical spook params
+	e.SetSpookParams(1e7, 4, 1/60)
+}
+
+func (e *Equation) BodyA() IBody {
+
+	return e.bA
+}
+
+func (e *Equation) BodyB() IBody {
+
+	return e.bB
+}
+
+func (e *Equation) JeA() JacobianElement {
+
+	return e.jeA
+}
+
+func (e *Equation) JeB() JacobianElement {
+
+	return e.jeB
+}
+
+// SetMinForce sets the minimum force to be applied by the constraint.
+func (e *Equation) SetMinForce(minForce float32) {
+
+	e.minForce = minForce
+}
+
+// MinForce returns the minimum force to be applied by the constraint.
+func (e *Equation) MinForce() float32 {
+
+	return e.minForce
+}
+
+// SetMaxForce sets the maximum force to be applied by the constraint.
+func (e *Equation) SetMaxForce(maxForce float32) {
+
+	e.maxForce = maxForce
+}
+
+// MaxForce returns the maximum force to be applied by the constraint.
+func (e *Equation) MaxForce() float32 {
+
+	return e.maxForce
+}
+
+// TODO
+func (e *Equation) Eps() float32 {
+
+	return e.eps
+}
+
+// SetMultiplier sets the multiplier.
+func (e *Equation) SetMultiplier(multiplier float32) {
+
+	e.multiplier = multiplier
+}
+
+// MaxForce returns the multiplier.
+func (e *Equation) Multiplier() float32 {
+
+	return e.multiplier
+}
+
+// SetEnable sets the enabled flag of the equation.
+func (e *Equation) SetEnabled(state bool) {
+
+	e.enabled = state
+}
+
+// Enabled returns the enabled flag of the equation.
+func (e *Equation) Enabled() bool {
+
+	return e.enabled
+}
+
+// SetSpookParams recalculates a, b, eps.
+func (e *Equation) SetSpookParams(stiffness, relaxation float32, timeStep float32) {
+
+	e.a = 4.0 / (timeStep * (1 + 4*relaxation))
+	e.b = (4.0 * relaxation) / (1 + 4*relaxation)
+	e.eps = 4.0 / (timeStep * timeStep * stiffness * (1 + 4*relaxation))
+}
+
+// ComputeB computes the RHS of the SPOOK equation.
+func (e *Equation) ComputeB(h float32) float32 {
+
+	GW := e.ComputeGW()
+	Gq := e.ComputeGq()
+	GiMf := e.ComputeGiMf()
+	return -Gq*e.a - GW*e.b - GiMf*h
+}
+
+// ComputeGq computes G*q, where q are the generalized body coordinates.
+func (e *Equation) ComputeGq() float32 {
+
+	xi := e.bA.Position()
+	xj := e.bB.Position()
+	spatA := e.jeA.Spatial()
+	spatB := e.jeB.Spatial()
+	return (&spatA).Dot(&xi) + (&spatB).Dot(&xj)
+}
+
+// ComputeGW computes G*W, where W are the body velocities.
+func (e *Equation) ComputeGW() float32 {
+
+	vA := e.bA.Velocity()
+	vB := e.bB.Velocity()
+	wA := e.bA.AngularVelocity()
+	wB := e.bB.AngularVelocity()
+	return e.jeA.MultiplyVectors(&vA, &wA) + e.jeB.MultiplyVectors(&vB, &wB)
+}
+
+// ComputeGiMf computes G*inv(M)*f, where M is the mass matrix with diagonal blocks for each body, and f are the forces on the bodies.
+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())
+
+	torqueA := e.bA.Torque()
+	torqueB := e.bB.Torque()
+
+	invIiTaui := torqueA.ApplyMatrix3(e.bA.InvInertiaWorldSolve())
+	invIjTauj := torqueB.ApplyMatrix3(e.bB.InvInertiaWorldSolve())
+
+	return e.jeA.MultiplyVectors(iMfA, invIiTaui) + e.jeB.MultiplyVectors(iMfB, invIjTauj)
+}
+
+// ComputeGiMGt computes G*inv(M)*G'.
+func (e *Equation) ComputeGiMGt() float32 {
+
+	rotA := e.jeA.Rotational()
+	rotB := e.jeB.Rotational()
+	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)
+
+	return result
+}
+
+// ComputeC computes the denominator part of the SPOOK equation: C = G*inv(M)*G' + eps.
+func (e *Equation) ComputeC() float32 {
+
+	return e.ComputeGiMGt() + e.eps
+}

physics/equation/friction.go

@@ -0,0 +1,53 @@
+// 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 equation
+
+import (
+	"github.com/g3n/engine/math32"
+)
+
+// Friction is a friction constraint equation.
+type Friction struct {
+	Equation
+	rA *math32.Vector3 // World-oriented vector that goes from the center of bA to the contact point.
+	rB *math32.Vector3 // World-oriented vector that starts in body j position and goes to the contact point.
+	t  *math32.Vector3 // Contact tangent
+}
+
+// NewFriction creates and returns a pointer to a new Friction equation object.
+// slipForce should be +-F_friction = +-mu * F_normal = +-mu * m * g
+func NewFriction(bodyA, bodyB IBody, slipForce float32) *Friction {
+
+	fe := new(Friction)
+
+	fe.rA = math32.NewVec3()
+	fe.rB = math32.NewVec3()
+	fe.t = math32.NewVec3()
+
+	fe.Equation.initialize(bodyA, bodyB, -slipForce, slipForce)
+
+	return fe
+}
+
+// ComputeB
+func (fe *Friction) ComputeB(h float32) float32 {
+
+	// Calculate cross products
+	rtA := math32.NewVec3().CrossVectors(fe.rA, fe.t)
+	rtB := math32.NewVec3().CrossVectors(fe.rB, fe.t)
+
+	// G = [-t -rtA t rtB]
+	// And remember, this is a pure velocity constraint, g is always zero!
+	fe.jeA.SetSpatial(fe.t.Clone().Negate())
+	fe.jeA.SetRotational(rtA.Clone().Negate())
+	fe.jeB.SetSpatial(fe.t.Clone())
+	fe.jeB.SetRotational(rtB.Clone())
+
+	var GW = fe.ComputeGW()
+	var GiMf = fe.ComputeGiMf()
+
+	return -GW*fe.b - h*GiMf
+}

physics/equation/jacobian.go

@@ -0,0 +1,52 @@
+// 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 equation
+
+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
+}
+
+// SetSpatial sets the spatial component of the JacobianElement.
+func (je *JacobianElement) SetSpatial(spatial *math32.Vector3) {
+
+	je.spatial = spatial
+}
+
+// Spatial returns the spatial component of the JacobianElement.
+func (je *JacobianElement) Spatial() math32.Vector3 {
+
+	return *je.spatial
+}
+
+// Rotational sets the rotational component of the JacobianElement.
+func (je *JacobianElement) SetRotational(rotational *math32.Vector3) {
+
+	je.rotational = rotational
+}
+
+// Rotational returns the rotational component of the JacobianElement.
+func (je *JacobianElement) Rotational() math32.Vector3 {
+
+	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)
+}
+
+// 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)
+}

physics/equation/rotational.go

@@ -0,0 +1,90 @@
+// 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 equation
+
+import (
+	"github.com/g3n/engine/math32"
+)
+
+// Rotational is a rotational constraint equation.
+// Works to keep the local vectors orthogonal to each other in world space.
+type Rotational struct {
+	Equation
+	axisA    *math32.Vector3 // Local axis in A
+	axisB    *math32.Vector3 // Local axis in B
+	maxAngle float32        // Max angle
+}
+
+// NewRotational creates and returns a pointer to a new Rotational equation object.
+func NewRotational(bodyA, bodyB IBody, maxForce float32) *Rotational {
+
+	re := new(Rotational)
+
+	re.axisA = math32.NewVector3(1, 0, 0)
+	re.axisB = math32.NewVector3(0, 1, 0)
+	re.maxAngle = math32.Pi / 2
+
+	re.Equation.initialize(bodyA, bodyB, -maxForce, maxForce)
+
+	return re
+}
+
+// SetAxisA sets the axis of body A.
+func (re *Rotational) SetAxisA(axisA *math32.Vector3) {
+
+	re.axisA = axisA
+}
+
+// AxisA returns the axis of body A.
+func (re *Rotational) AxisA() math32.Vector3 {
+
+	return *re.axisA
+}
+
+// SetAxisB sets the axis of body B.
+func (re *Rotational) SetAxisB(axisB *math32.Vector3) {
+
+	re.axisB = axisB
+}
+
+// AxisB returns the axis of body B.
+func (re *Rotational) AxisB() math32.Vector3 {
+
+	return *re.axisB
+}
+
+// SetAngle sets the maximum angle.
+func (re *Rotational) SetMaxAngle(angle float32) {
+
+	re.maxAngle = angle
+}
+
+// MaxAngle returns the maximum angle.
+func (re *Rotational) MaxAngle() float32 {
+
+	return re.maxAngle
+}
+
+// ComputeB
+func (re *Rotational) ComputeB(h float32) float32 {
+
+	// Calculate cross products
+	nAnB := math32.NewVec3().CrossVectors(re.axisA, re.axisB)
+	nBnA := math32.NewVec3().CrossVectors(re.axisB, re.axisA)
+
+	// g = nA * nj
+	// gdot = (nj x nA) * wi + (nA x nj) * wj
+	// G = [0 nBnA 0 nAnB]
+	// W = [vi wi vj wj]
+	re.jeA.SetRotational(nBnA)
+	re.jeB.SetRotational(nAnB)
+
+	g := math32.Cos(re.maxAngle) - re.axisA.Dot(re.axisB)
+	GW := re.ComputeGW()
+	GiMf := re.ComputeGiMf()
+
+	return -g*re.a - GW*re.b - h*GiMf
+}

physics/equation/rotationalmotor.go

@@ -0,0 +1,81 @@
+// 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 equation
+
+import (
+	"github.com/g3n/engine/math32"
+)
+
+// RotationalMotor is a rotational motor constraint equation.
+// Tries to keep the relative angular velocity of the bodies to a given value.
+type RotationalMotor struct {
+	Equation
+	axisA       *math32.Vector3 // World oriented rotational axis
+	axisB       *math32.Vector3 // World oriented rotational axis
+	targetSpeed float32         // Target speed
+}
+
+// NewRotationalMotor creates and returns a pointer to a new RotationalMotor equation object.
+func NewRotationalMotor(bodyA, bodyB IBody, maxForce float32) *RotationalMotor {
+
+	re := new(RotationalMotor)
+	re.Equation.initialize(bodyA, bodyB, -maxForce, maxForce)
+
+	return re
+}
+
+// SetAxisA sets the axis of body A.
+func (ce *RotationalMotor) SetAxisA(axisA *math32.Vector3) {
+
+	ce.axisA = axisA
+}
+
+// AxisA returns the axis of body A.
+func (ce *RotationalMotor) AxisA() math32.Vector3 {
+
+	return *ce.axisA
+}
+
+// SetAxisB sets the axis of body B.
+func (ce *RotationalMotor) SetAxisB(axisB *math32.Vector3) {
+
+	ce.axisB = axisB
+}
+
+// AxisB returns the axis of body B.
+func (ce *RotationalMotor) AxisB() math32.Vector3 {
+
+	return *ce.axisB
+}
+
+// SetTargetSpeed sets the target speed.
+func (ce *RotationalMotor) SetTargetSpeed(speed float32) {
+
+	ce.targetSpeed = speed
+}
+
+// TargetSpeed returns the target speed.
+func (ce *RotationalMotor) TargetSpeed() float32 {
+
+	return ce.targetSpeed
+}
+
+// ComputeB
+func (re *RotationalMotor) ComputeB(h float32) float32 {
+
+	// g = 0
+	// gdot = axisA * wi - axisB * wj
+	// gdot = G * W = G * [vi wi vj wj]
+	// =>
+	// G = [0 axisA 0 -axisB]
+	re.jeA.SetRotational(re.axisA.Clone())
+	re.jeB.SetRotational(re.axisB.Clone().Negate())
+
+	GW := re.ComputeGW() - re.targetSpeed
+	GiMf := re.ComputeGiMf()
+
+	return -GW*re.b - h*GiMf
+}

physics/forcefield.go

@@ -97,14 +97,14 @@ func (pa *AttractorForceField) ForceAt(pos *math32.Vector3) math32.Vector3 {
 	dist := dir.Length()
 	dir.Normalize()
 	var val float32
-	log.Error("dist %v", dist)
+	//log.Error("dist %v", dist)
 	if dist > 0 {
 		val = pa.mass/(dist*dist)
 	} else {
 		val = 0
 	}
 	val = math32.Min(val, 100) // TODO deal with instability
-	log.Error("%v", val)
+	//log.Error("%v", val)
 	dir.MultiplyScalar(val) // TODO multiply by gravitational constant: 6.673×10−11 (N–m2)/kg2
 	return *dir
 }

physics/material.go

@@ -0,0 +1,23 @@
+// 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
+
+type Material struct {
+	name        string
+	friction    float32
+	restitution float32
+}
+
+type ContactMaterial struct {
+	mat1                       *Material
+	mat2                       *Material
+	friction                   float32
+	restitution                float32
+	contactEquationStiffness   float32
+	contactEquationRelaxation  float32
+	frictionEquationStiffness  float32
+	frictionEquationRelaxation float32
+}

physics/rigidbody.go

@@ -1,30 +0,0 @@
-// Copyright 2016 The G3N Authors. All rights reserved.
-// Use of this source code is governed by a BSD-style
-// license that can be found in the LICENSE file.
-
-package physics
-
-import (
-	"github.com/g3n/engine/math32"
-	"github.com/g3n/engine/core"
-)
-
-// Body represents a physics-driven solid body.
-type Body struct {
-	core.INode
-	mass            float32        // Total mass
-	velocity        math32.Vector3 // Linear velocity
-	angularMass     math32.Matrix3 // Angular mass i.e. moment of inertia
-	angularVelocity math32.Vector3 // Angular velocity
-	position        math32.Vector3 // World position of the center of gravity
-	static          bool // If true - the rigidBody does not move or rotate
-}
-
-// NewBody creates and returns a pointer to a new RigidBody.
-func NewBody(inode core.INode) *Body {
-
-	b := new(Body)
-	b.INode = inode
-	b.mass = 1
-	return b
-}

physics/simulation.go

@@ -6,6 +6,9 @@ package physics
 
 import (
 	"time"
+	"github.com/g3n/engine/physics/equation"
+	"github.com/g3n/engine/physics/solver"
+	"github.com/g3n/engine/physics/constraint"
 )
 
 type ICollidable interface {
@@ -27,13 +30,50 @@ type Simulation struct {
 	//dynamical   []int // non collidable but still affected by force fields
 	//collidables []ICollidable
 
+
+	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.
+	                      // If bodies tend to explode, set to a smaller value (zero to be sure nothing can go wrong).
+	quatNormalizeFast bool // Set to true to use fast quaternion normalization. It is often enough accurate to use. If bodies tend to explode, set to false.
+
+	time float32      // The wall-clock time since simulation start
+	stepnumber int    // Number of timesteps taken since start
+	default_dt float32 // Default and last timestep sizes
+
+	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
+	solver solver.ISolver    // The solver algorithm to use. Default is GSSolver
+
+	constraints       []*constraint.Constraint  // All constraints
+	materials         []*Material               // All added materials
+	cMaterials        []*ContactMaterial
+
+
+
+
+
+
+	doProfiling      bool
 }
 
 // NewSimulation creates and returns a pointer to a new physics simulation.
 func NewSimulation() *Simulation {
 
-	b := new(Simulation)
-	return b
+	s := new(Simulation)
+	s.time = 0
+	s.default_dt = 1/60
+
+	//s.broadphase = NewNaiveBroadphase()
+	//s.narrowphase = NewNarrowphase()
+	s.solver = solver.NewGaussSeidel()
+
+	return s
 }
 
 // AddForceField adds a force field to the simulation.
@@ -58,26 +98,68 @@ func (s *Simulation) RemoveForceField(ff ForceField) bool {
 }
 
 // AddBody adds a body to the simulation.
-func (s *Simulation) AddBody(rb *Body) {
-
-	s.bodies = append(s.bodies, rb)
+// @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(rb *Body) bool {
+func (s *Simulation) RemoveBody(body *Body) bool {
 
 	for pos, current := range s.bodies {
-		if current == rb {
+		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)
+
 			return true
 		}
 	}
+
 	return false
 }
 
+// Bodies returns the bodies under simulation.
+func (s *Simulation) Bodies() []*Body{
+
+	return s.bodies
+}
+
 // AddParticle adds a particle to the simulation.
 func (s *Simulation) AddParticle(rb *Particle) {
 
@@ -135,7 +217,7 @@ func (s *Simulation) updatePositions(frameDelta time.Duration) {
 		pos := rb.GetNode().Position()
 		posDelta := rb.velocity
 		posDelta.MultiplyScalar(float32(frameDelta.Seconds()))
-		pos.Add(&posDelta)
+		pos.Add(posDelta)
 		rb.GetNode().SetPositionVec(&pos)
 	}
 
@@ -179,3 +261,88 @@ func (s *Simulation) Step(frameDelta time.Duration) {
 	s.updatePositions(frameDelta)
 
 }
+
+// ClearForces sets all body forces in the world to zero.
+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)
+	}
+}
+
+// Add a constraint to the simulation.
+func (s *Simulation) AddConstraint(c *constraint.Constraint) {
+
+	s.constraints = append(s.constraints, c)
+}
+
+func (s *Simulation) RemoveConstraint(c *constraint.Constraint) {
+
+	// TODO
+}
+
+func (s *Simulation) AddMaterial(mat *Material) {
+
+	s.materials = append(s.materials, mat)
+}
+
+func (s *Simulation) RemoveMaterial(mat *Material) {
+
+	// TODO
+}
+
+func (s *Simulation) AddContactMaterial(cmat *ContactMaterial) {
+
+	s.cMaterials = append(s.cMaterials, cmat)
+
+	// TODO add contactMaterial materials to contactMaterialTable
+}
+
+
+type ContactEvent struct {
+	bodyA *Body
+	bodyB *Body
+	// shapeA
+	// shapeB
+}
+
+const (
+	BeginContactEvent = "physics.BeginContactEvent"
+	EndContactEvent   = "physics.EndContactEvent"
+)
+
+func (s *Simulation) EmitContactEvents() {
+	//TODO
+}
+
+
+
+func (s *Simulation) Solve() {
+
+	//// Update solve mass for all bodies
+	//if Neq > 0 {
+	//	for i := 0; i < Nbodies; i++ {
+	//		bodies[i].UpdateSolveMassProperties()
+	//	}
+	//}
+	//
+	//s.solver.Solve(frameDelta, len(bodies))
+
+}
+
+
+// Note - this method alters the solution arrays
+func (s *Simulation) ApplySolution(solution *solver.Solution) {
+
+	// Add results to velocity and angular velocity of bodies
+	for i := 0; i < len(s.bodies); i++ {
+		b := s.bodies[i]
+
+		vDelta := solution.VelocityDeltas[i].Multiply(b.LinearFactor())
+		b.AddToVelocity(vDelta)
+
+		wDelta := solution.AngularVelocityDeltas[i].Multiply(b.AngularFactor())
+		b.AddToAngularVelocity(wDelta)
+	}
+}

physics/solver/gs.go

@@ -0,0 +1,146 @@
+// 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 solver
+
+import (
+	"time"
+	"github.com/g3n/engine/math32"
+)
+
+// GaussSeidel equation solver.
+// See https://en.wikipedia.org/wiki/Gauss-Seidel_method.
+// The number of solver iterations determines the quality of the solution.
+// 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.
+
+	solveInvCs  []float32
+	solveBs     []float32
+	solveLambda []float32
+}
+
+// NewGaussSeidel creates and returns a pointer to a new GaussSeidel constraint equation solver.
+func NewGaussSeidel() *GaussSeidel {
+
+	gs := new(GaussSeidel)
+	gs.maxIter = 10
+	gs.tolerance = 1e-7
+
+	gs.VelocityDeltas = make([]math32.Vector3, 0)
+	gs.AngularVelocityDeltas = make([]math32.Vector3, 0)
+
+	gs.solveInvCs = make([]float32, 0)
+	gs.solveBs = make([]float32, 0)
+	gs.solveLambda = make([]float32, 0)
+
+	return gs
+}
+
+func (gs *GaussSeidel) Reset() {
+
+	gs.VelocityDeltas = gs.VelocityDeltas[0:0]
+	gs.AngularVelocityDeltas = gs.AngularVelocityDeltas[0:0]
+
+	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 {
+
+	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})
+	}
+
+	// Things that do not change during iteration can be computed once
+	for i := 0; i < nEquations; i++ {
+		eq := gs.equations[i]
+		gs.solveInvCs = append(gs.solveInvCs, 1.0 / eq.ComputeC())
+		gs.solveBs = append(gs.solveBs, eq.ComputeB(h))
+		gs.solveLambda = append(gs.solveLambda, 0.0)
+	}
+
+	if nEquations > 0 {
+		tolSquared := gs.tolerance*gs.tolerance
+
+		// Iterate over equations
+		for iter = 0; iter < gs.maxIter; iter++ {
+
+			// Accumulate the total error for each iteration.
+			deltaLambdaTot := float32(0)
+
+			for j := 0; j < nEquations; j++ {
+				eq := gs.equations[j]
+
+				// Compute iteration
+				lambdaJ := gs.solveLambda[j]
+
+				idxBodyA := eq.BodyA().Index()
+				idxBodyB := eq.BodyB().Index()
+
+				vA := gs.VelocityDeltas[idxBodyA]
+				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() {
+					deltaLambda = eq.MinForce() - lambdaJ
+				} else if lambdaJ+deltaLambda > eq.MaxForce() {
+					deltaLambda = eq.MaxForce() - lambdaJ
+				}
+				gs.solveLambda[j] += deltaLambda
+				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))
+
+				// 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))
+
+			}
+
+			// If the total error is small enough - stop iterating
+			if deltaLambdaTot*deltaLambdaTot < tolSquared {
+				break
+			}
+		}
+
+		// Set the .multiplier property of each equation
+		for i := range gs.equations {
+			gs.equations[i].SetMultiplier(gs.solveLambda[i] / h)
+		}
+
+		iter += 1
+	}
+
+	return iter
+}

physics/solver/solver.go

@@ -0,0 +1,56 @@
+// 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 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
+}
+
+// Solution represents a solver solution
+type Solution struct {
+	VelocityDeltas        []math32.Vector3
+	AngularVelocityDeltas []math32.Vector3
+}
+
+// Constraint equation solver base class.
+type Solver struct {
+	equations []*equation.Equation // All equations to be solved
+}
+
+// AddEquation adds an equation to the solver.
+func (s *Solver) AddEquation(eq *equation.Equation) {
+
+	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 {
+
+	for pos, current := range s.equations {
+		if current == eq {
+			copy(s.equations[pos:], s.equations[pos+1:])
+			s.equations[len(s.equations)-1] = nil
+			s.equations = s.equations[:len(s.equations)-1]
+			return true
+		}
+	}
+	return false
+}
+
+// ClearEquations removes all equations from the solver.
+func (s *Solver) ClearEquations() {
+
+	s.equations = s.equations[0:0]
+}