g3n-engine/physics/body.go

// 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)
}