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/graphic"
)

// Body represents a physics-driven body.
type Body struct {
	//core.INode // TODO instead of embedding INode - embed Node and have a method SetNode ?

	*graphic.Graphic

	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(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.collisionFilterGroup = 1
	b.collisionFilterMask = -1

	pos := igraphic.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 := igraphic.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
}



func (b *Body) ComputeAABB() {
	// TODO
}


// 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() {

	// TODO getter of invMass ?
	if b.mass > 0 {
		b.invMass = 1.0/b.mass
	} else {
		b.invMass = 0
	}

	// Approximate with AABB box
	b.ComputeAABB()
	//halfExtents := math32.NewVector3(0,0,0).Set( // TODO
	//	(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) // TODO

	if b.fixedRotation {
		b.invInertia.Zero()
	} else {
		if b.inertia.X > 0 {
			b.invInertia.SetX(1/b.inertia.X)
		} else {
			b.invInertia.SetX(0)
		}
		if b.inertia.Y > 0 {
			b.invInertia.SetY(1/b.inertia.Y)
		} else {
			b.invInertia.SetY(0)
		}
		if b.inertia.Z > 0 {
			b.invInertia.SetZ(1/b.inertia.Z)
		} else {
			b.invInertia.SetZ(0)
		}
	}

	b.UpdateInertiaWorld(true)
}

// Update .inertiaWorld and .invInertiaWorld
func (b *Body) UpdateInertiaWorld(force bool) {

    I := b.invInertia
	// If angular mass 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 do the transformation if all diagonal entries are equal.
    if I.X != I.Y || I.Y != I.Z || force {
    	//
    	// AngularMassWorld^(-1) = Rotation * AngularMassBody^(-1) * Rotation^(T)
    	//          3x3              3x3            3x3                  3x3
    	//
    	// Since AngularMassBodyTensor^(-1) is diagonal, then Rotation*AngularMassBodyTensor^(-1) is
    	// just scaling the columns of AngularMassBodyTensor by the diagonal components.
    	//
        m1 := math32.NewMatrix3()
        m2 := math32.NewMatrix3()

        m1.MakeRotationFromQuaternion(b.quaternion)
		m2.Copy(m1).Transpose()
        m1.ScaleColumns(I)

		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.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 and rotation
    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
    }

    // Integrate force over mass (acceleration) to obtain estimate for instantaneous velocities
    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

	// Integrate inverse angular mass times torque to obtain estimate for instantaneous angular velocities
    e := b.invInertiaWorld
    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[3]*ty + e[6]*tz)
    b.angularVelocity.Y += dt * (e[1]*tx + e[4]*ty + e[7]*tz)
    b.angularVelocity.Z += dt * (e[2]*tx + e[5]*ty + e[8]*tz)

	// Integrate velocity to obtain estimate for position
    b.position.X += b.velocity.X * dt
    b.position.Y += b.velocity.Y * dt
    b.position.Z += b.velocity.Z * dt

	// Integrate angular velocity to obtain estimate for rotation
	ax := b.angularVelocity.X * b.angularFactor.X
	ay := b.angularVelocity.Y * b.angularFactor.Y
	az := b.angularVelocity.Z * b.angularFactor.Z
	bx := b.quaternion.X
	by := b.quaternion.Y
	bz := b.quaternion.Z
	bw := b.quaternion.W
	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.W += halfDt * (- ax * bx - ay * by - az * bz)

	// Normalize quaternion
    if quatNormalize {
       if quatNormalizeFast {
			b.quaternion.NormalizeFast()
       } else {
			b.quaternion.Normalize()
       }
    }

    b.aabbNeedsUpdate = true  // TODO

    // Update world inertia
    b.UpdateInertiaWorld(false)
}