Skip to content
This repository has been archived by the owner before Nov 9, 2022. It is now read-only.
Permalink
master
Switch branches/tags

Name already in use

A tag already exists with the provided branch name. Many Git commands accept both tag and branch names, so creating this branch may cause unexpected behavior. Are you sure you want to create this branch?
Go to file
 
 
Cannot retrieve contributors at this time
// Copyright 2015, Timothy Bogdala <tdb@animal-machine.com>
// See the LICENSE file for more details.
package cubez
import (
m "github.com/tbogdala/cubez/math"
)
// a few internal epsilon values
const (
velocityEpsilon m.Real = 0.01
positionEpsilon m.Real = 0.01
)
// Contact holds all of the data associated with a contact between two collider primitives.
type Contact struct {
// Bodies holds 1 or 2 bodies involved in the contact; second body can be nil
Bodies [2]*RigidBody
// Friction holds the lateral friction coefficient at the contact
Friction m.Real
// Restitution holdes the normal restitution coefficient at the contact
Restitution m.Real
// ContactPoint is the position of the contact in World Space
ContactPoint m.Vector3
// ContactNormal is the direction of the contact in World Space
ContactNormal m.Vector3
// Penetration is the depth of penetration at the contact point. If
// both Bodies are set, then this should be midway between the
// inter-penetrating points.
Penetration m.Real
// contactToWorld is a transform matrix that converts coordiantes in the contact's
// frame of reference to World coordinates. The columns are orthornomal vectors.
contactToWorld m.Matrix3
// relativeContactPosition holds the World Space position of the contact point
// relative to the center of each Body.
relativeContactPosition [2]m.Vector3
// contactVelocity holds the closing velocity at the point of contact.
contactVelocity m.Vector3
// desiredDeltaVelocity holds the required change in velocity for this contact to be resolved.
desiredDeltaVelocity m.Real
}
// NewContact returns a new Contact object.
func NewContact() *Contact {
c := new(Contact)
return c
}
func (c *Contact) calculateInternals(duration m.Real) {
// make sure that if there's only one body that it's in the first spot
if c.Bodies[0] == nil {
c.ContactNormal.MulWith(-1.0)
c.Bodies[0] = c.Bodies[1]
c.Bodies[1] = nil
}
// make the set of axis at the contact point
c.calculateContactBasis()
// store the relative position of the contact to each body
c.relativeContactPosition[0].Set(&c.ContactPoint)
c.relativeContactPosition[0].Sub(&c.Bodies[0].Position)
c.contactVelocity = c.calculateLocalVelocity(0, duration)
if c.Bodies[1] != nil {
c.relativeContactPosition[1].Set(&c.ContactPoint)
c.relativeContactPosition[1].Sub(&c.Bodies[1].Position)
contactVelocity1 := c.calculateLocalVelocity(1, duration)
c.contactVelocity.Sub(&contactVelocity1)
}
// calculate the desired change in velocity for resolution
c.calculateDesiredDeltaVelocity(duration)
}
func (c *Contact) calculateDesiredDeltaVelocity(duration m.Real) {
const velocityLimit m.Real = 0.25
var velocityFromAcc m.Real
// calculate the acceleration induced velocity accumlated this frame
var tempVelocity m.Vector3
if c.Bodies[0].IsAwake {
tempVelocity = c.Bodies[0].GetLastFrameAccelleration()
tempVelocity.MulWith(duration)
velocityFromAcc += tempVelocity.Dot(&c.ContactNormal)
}
if c.Bodies[1] != nil && c.Bodies[1].IsAwake {
tempVelocity = c.Bodies[1].GetLastFrameAccelleration()
tempVelocity.MulWith(duration)
velocityFromAcc -= tempVelocity.Dot(&c.ContactNormal)
}
// if the velocity is very slow, limit the restitution
restitution := c.Restitution
if m.RealAbs(c.contactVelocity[0]) < velocityLimit {
restitution = 0.0
}
// combine the bounce velocity with the removed acceleration velocity
c.desiredDeltaVelocity = -c.contactVelocity[0] - restitution*(c.contactVelocity[0]-velocityFromAcc)
}
// Constructs an arbitrary orthonormal basis for the contact. It's stored
// as a 3x3 matrix where each column is a vector for an axis. The x axis
// is based off of the contact normal and the y and z axis will be generated
// so that they are at right angles to it.
func (c *Contact) calculateContactBasis() {
var contactTangentY m.Vector3
var contactTangentZ m.Vector3
absContactNormalX := m.RealAbs(c.ContactNormal[0])
absContactNormalY := m.RealAbs(c.ContactNormal[1])
// check whether the z axis is nearer to the x or y axis
if absContactNormalX > absContactNormalY {
// generate a scaling factor to ensure results are normalized
s := m.Real(1.0) / m.RealSqrt(c.ContactNormal[2]*c.ContactNormal[2]+c.ContactNormal[0]*c.ContactNormal[0])
// the new x axis is at right angles to the world y axis
contactTangentY[0] = c.ContactNormal[2] * s
contactTangentY[1] = 0
contactTangentY[2] = c.ContactNormal[0] * -s
// the new y axis is at right angles to the new x and z axes
contactTangentZ[0] = c.ContactNormal[1] * contactTangentY[0]
contactTangentZ[1] = c.ContactNormal[2]*contactTangentY[0] - c.ContactNormal[0]*contactTangentY[2]
contactTangentZ[2] = -c.ContactNormal[1] * contactTangentY[0]
} else {
// generate a scaling factor to ensure results are normalized
s := m.Real(1.0) / m.RealSqrt(c.ContactNormal[2]*c.ContactNormal[2]+c.ContactNormal[1]*c.ContactNormal[1])
// the new x axis is at right angles to the world y axis
contactTangentY[0] = 0
contactTangentY[1] = -c.ContactNormal[2] * s
contactTangentY[2] = c.ContactNormal[1] * s
// the new y axis is at right angles to the new x and z axes
contactTangentZ[0] = c.ContactNormal[1]*contactTangentY[2] - c.ContactNormal[2]*contactTangentY[1]
contactTangentZ[1] = -c.ContactNormal[0] * contactTangentY[2]
contactTangentZ[2] = c.ContactNormal[0] * contactTangentY[1]
}
// now set the contactToWorld matrix based off of these three vectors
c.contactToWorld.SetComponents(&c.ContactNormal, &contactTangentY, &contactTangentZ)
}
// calculateLocalVelocity calculates the velocity of the contact point on th given body.
func (c *Contact) calculateLocalVelocity(bodyIndex int, duration m.Real) m.Vector3 {
body := c.Bodies[bodyIndex]
// work out the velocity of the contact point
velocity := body.Rotation.Cross(&c.relativeContactPosition[bodyIndex])
velocity.Add(&body.Velocity)
// turn the velocity into contact coordinates
contactVelocity := c.contactToWorld.TransformTranspose(&velocity)
// calculate the amount of velocity that is due to forces without reactions
accVelocity := body.GetLastFrameAccelleration()
accVelocity.MulWith(duration)
accVelocity = c.contactToWorld.TransformTranspose(&accVelocity)
// we ignore any component of acceleration in the contact normal direction
accVelocity[0] = 0.0
// add the planar velocity -- if there's enough friction they will
// be removed during the velocity resolution
contactVelocity.Add(&accVelocity)
return contactVelocity
}
// matchAwakeState wakes up bodies that are in contact with a body that is awake.
func (c *Contact) matchAwakeState() {
// solo body collisions don't trigger a wake-up
if c.Bodies[1] == nil {
return
}
b0Awake := c.Bodies[0].IsAwake
b1Awake := c.Bodies[1].IsAwake
// wake up only the sleeping one
if (b0Awake || b1Awake) && !(b0Awake && b1Awake) {
if b0Awake {
c.Bodies[1].SetAwake(true)
} else {
c.Bodies[0].SetAwake(true)
}
}
}
// ResolveContacts results a set of contacts for both penetration and velocity.
//
// NOTE: Contacts that cannot interact with each other should be passed to
// separate calls of ResolveContacts for performance reasons.
func ResolveContacts(maxIterations int, contacts []*Contact, duration m.Real) {
// start off with some sanity checks
if duration <= 0.0 || contacts == nil || len(contacts) == 0 {
return
}
// prepares the contacts for processing
prepareContacts(contacts, duration)
// resolve the interpenetration problems with the contacts
adjustPositions(maxIterations, contacts, duration)
// resolve the velocity problems with the contacts
adjustVelocities(maxIterations, contacts, duration)
}
// prepareContacts sets up contacts for processing by calculating internal data.
func prepareContacts(contacts []*Contact, duration m.Real) {
for _, e := range contacts {
e.calculateInternals(duration)
}
}
// adjustPositions resolves the positional issues with the given array of
// constraints using the given number of iterations.
func adjustPositions(maxIterations int, contacts []*Contact, duration m.Real) {
// iteratively resolve interpenetrations in order of severity
iterationsUsed := 0
for iterationsUsed < maxIterations {
// find the biggest penetration
max := positionEpsilon
index := len(contacts)
for i, c := range contacts {
if c.Penetration > max {
max = c.Penetration
index = i
}
}
if index == len(contacts) {
break
}
contact := contacts[index]
// match the awake state at the contact
contact.matchAwakeState()
// resolve the penetration
linearChange, angularChange := contact.applyPositionChange(max)
// again this action may have changed the penetration of other bodies,
// so we update contacts
for _, c := range contacts {
for b := 0; b < 2; b++ {
if c.Bodies[b] != nil {
// check for a match with each body in the newly resolved contact
for d := 0; d < 2; d++ {
if c.Bodies[b] == contact.Bodies[d] {
deltaPosition := angularChange[d].Cross(&c.relativeContactPosition[b])
deltaPosition.Add(&linearChange[d])
// the sign of the change is positive if we're dealing with the second body
// in a contact and negative otherwise (because we're subtracting the resolution).
var sign m.Real = 1.0
if b == 0 {
sign = -1.0
}
c.Penetration += deltaPosition.Dot(&c.ContactNormal) * sign
}
} // d
}
} // b
} // i,c
iterationsUsed++
}
}
// applyPositionChange performs an inertia weighted penetration resolution of this contact alone.
func (c *Contact) applyPositionChange(penetration m.Real) (linearChange, angularChange [2]m.Vector3) {
const angularLimit m.Real = 0.2
var angularInertia, linearInertia, angularMove, linearMove [2]m.Real
var totalInertia m.Real
// we need to work out the inertia of each object in the direction
// of the contact normal due to angular inertia only
for i := 0; i < 2; i++ {
body := c.Bodies[i]
if body == nil {
continue
}
inverseInertiaTensor := body.GetInverseInertiaTensorWorld()
// use the same procedure as for calculating frictionless velocity
// change to work out the angular inertia
angularInertiaWorld := c.relativeContactPosition[i].Cross(&c.ContactNormal)
angularInertiaWorld = inverseInertiaTensor.MulVector3(&angularInertiaWorld)
angularInertiaWorld = angularInertiaWorld.Cross(&c.relativeContactPosition[i])
angularInertia[i] = angularInertiaWorld.Dot(&c.ContactNormal)
// the linear component is simply the inverse mass
linearInertia[i] = body.GetInverseMass()
// keep track of the total inertia from all component-wise
totalInertia += linearInertia[i] + angularInertia[i]
}
// the logic is split into two loops so that totalInertia is completely calculated
for i := 0; i < 2; i++ {
body := c.Bodies[i]
if body == nil {
continue
}
// the linear and angular movements required are in proportion
// to the two inverse inertias
var sign m.Real = 1.0
if i != 0 {
sign = -1.0
}
angularMove[i] = sign * penetration * (angularInertia[i] / totalInertia)
linearMove[i] = sign * penetration * (linearInertia[i] / totalInertia)
// to avoid angular projections that are too great (when mass is large, but
// inertia tensor is small) limit the angular move.
projection := c.relativeContactPosition[i]
projection.AddScaled(&c.ContactNormal, -c.relativeContactPosition[i].Dot(&c.ContactNormal))
// use the small angle approximation for the sine of the angle (i.e. the
// magnitude would be sin(angularLimit) * projection.magnitude
// but we approximate sin(angularLimit) to angularLimit.
maxMagnitude := angularLimit * projection.Magnitude()
if angularMove[i] < -maxMagnitude {
totalMove := angularMove[i] + linearMove[i]
angularMove[i] = -maxMagnitude
linearMove[i] = totalMove - angularMove[i]
} else if angularMove[i] > maxMagnitude {
totalMove := angularMove[i] + linearMove[i]
angularMove[i] = maxMagnitude
linearMove[i] = totalMove - angularMove[i]
}
// we have the linear amount of movement required by turning the RigidBody
// (in angularMove[i]); we now need to calculate the desired rotation to achieve that.
if angularMove[i] == 0.0 {
// easy -- no movement means no rotation
angularChange[i].Clear()
} else {
// work out the direction we'd like to rotate in
targetAngularDirection := c.relativeContactPosition[i].Cross(&c.ContactNormal)
inverseInertiaTensor := body.GetInverseInertiaTensorWorld()
angularChange[i] = inverseInertiaTensor.MulVector3(&targetAngularDirection)
angularChange[i].MulWith(angularMove[i] / angularInertia[i])
}
// velocity change is easier -- it's just the linear movement along ContactNormal
linearChange[i] = c.ContactNormal
linearChange[i].MulWith(linearMove[i])
// now we can start to apply the values we've calculated, starting with linear movement
body.Position.AddScaled(&c.ContactNormal, linearMove[i])
// now we do the change in orientation
body.Orientation.AddScaledVector(&angularChange[i], 1.0)
body.Orientation.Normalize()
// we need to calculate the derived data for body that is asleep, so that
// the changes are reflected in the object's data. otherwise the resolution
// will not change the position of the object and the next collision detection
// round will have the same penetration.
if body.IsAwake == false {
body.CalculateDerivedData()
}
}
return
}
// adjustVelocities resolves the velocity issues with the given array of constraints,
// using the given number of iterations.
func adjustVelocities(maxIterations int, contacts []*Contact, duration m.Real) {
// iteratively handle impacts in order of severity
iterationsUsed := 0
for iterationsUsed < maxIterations {
max := velocityEpsilon
index := len(contacts)
for i, c := range contacts {
if c.desiredDeltaVelocity > max {
max = c.desiredDeltaVelocity
index = i
}
}
if index == len(contacts) {
break
}
contact := contacts[index]
// match the awake state at the contact
contact.matchAwakeState()
// do the resolution on the contact that came out on top
velocityChange, rotationChange := contact.applyVelocityChange()
// with the change in velocity of the two bodies, the update of contact
// velocities means that some of the relative closing velocities need recomputing.
for _, c2 := range contacts {
// check each body
for b := 0; b < 2; b++ {
if c2.Bodies[b] == nil {
continue
}
// check for a match with each body in the newly resolved contact
for d := 0; d < 2; d++ {
if c2.Bodies[b] == contact.Bodies[d] {
deltaVel := rotationChange[d].Cross(&c2.relativeContactPosition[b])
deltaVel.Add(&velocityChange[d])
// the sign of the change is negative if we're dealing with
// the second body in a contact.
var sign m.Real = 1.0
if b == 1 {
sign = -1.0
}
tmpContactVelocity := c2.contactToWorld.TransformTranspose(&deltaVel)
tmpContactVelocity.MulWith(sign)
c2.contactVelocity.Add(&tmpContactVelocity)
c2.calculateDesiredDeltaVelocity(duration)
}
} // d
} // b
} // c2
iterationsUsed++
}
}
// applyVelocityChange performs an inertia-weighted impulse based resolution of this contact alone
func (c *Contact) applyVelocityChange() (velocityChange, rotationChange [2]m.Vector3) {
// get hold of the inverse mass and inverse inertia tensor, both in World Space
var inverseInertiaTensors [2]m.Matrix3
inverseInertiaTensors[0] = c.Bodies[0].GetInverseInertiaTensorWorld()
if c.Bodies[1] != nil {
inverseInertiaTensors[1] = c.Bodies[1].GetInverseInertiaTensorWorld()
}
// we will calculate the impulse for each contact axis
var impulseContact m.Vector3
if c.Friction == 0.0 {
// use the short format for frictionless contacts
impulseContact = c.calculateFrictionlessImpulse(inverseInertiaTensors)
} else {
// otherwise we may have impulses that aren't in the direction of the
// contact, so we need the more complex version
impulseContact = c.calculateFrictionImpulse(inverseInertiaTensors)
}
// convert impulse to world coordinates
impulse := c.contactToWorld.MulVector3(&impulseContact)
// split in the impulse into linear and rotation component-wise
impulsiveTorque := c.relativeContactPosition[0].Cross(&impulse)
rotationChange[0] = inverseInertiaTensors[0].MulVector3(&impulsiveTorque)
velocityChange[0].Clear()
velocityChange[0].AddScaled(&impulse, c.Bodies[0].GetInverseMass())
// apply the changes
c.Bodies[0].AddVelocity(&velocityChange[0])
c.Bodies[0].AddRotation(&rotationChange[0])
if c.Bodies[1] != nil {
// work out the second body's linear and angular changes
impulsiveTorque = impulse.Cross(&c.relativeContactPosition[1])
rotationChange[1] = inverseInertiaTensors[1].MulVector3(&impulsiveTorque)
velocityChange[1].Clear()
velocityChange[1].AddScaled(&impulse, -c.Bodies[1].GetInverseMass())
// apply the changes
c.Bodies[1].AddVelocity(&velocityChange[1])
c.Bodies[1].AddRotation(&rotationChange[1])
}
return
}
// calculateFrictionlessImpulse calculates the impulse needed to resolve this contact,
// given that the contact has no friction.
func (c *Contact) calculateFrictionlessImpulse(inverseInertiaTensors [2]m.Matrix3) (impulseContact m.Vector3) {
// build a vector that shows the change in velocity in World Space for
// a unit impulse in the direction of the contact normal
deltaVelWorld := c.relativeContactPosition[0].Cross(&c.ContactNormal)
deltaVelWorld = inverseInertiaTensors[0].MulVector3(&deltaVelWorld)
deltaVelWorld = deltaVelWorld.Cross(&c.relativeContactPosition[0])
// work out the change in velocity in contact coordinates
deltaVelocity := deltaVelWorld.Dot(&c.ContactNormal)
// add the linear component of velocity change
deltaVelocity += c.Bodies[0].GetInverseMass()
// check if we need to process the second body's data
if c.Bodies[1] == nil {
// go through the same transformation sequence again
deltaVelWorld = c.relativeContactPosition[1].Cross(&c.ContactNormal)
deltaVelWorld = inverseInertiaTensors[1].MulVector3(&deltaVelWorld)
deltaVelWorld = deltaVelWorld.Cross(&c.relativeContactPosition[1])
// work out the change in velocity in contact coordinates
// NOTE: should this be a +=?
deltaVelocity += deltaVelWorld.Dot(&c.ContactNormal)
// add the linear component of velocity change
deltaVelocity += c.Bodies[1].GetInverseMass()
}
// calculate the required size of the impulse
impulseContact[0] = c.desiredDeltaVelocity / deltaVelocity
impulseContact[1] = 0
impulseContact[2] = 0
return
}
// calculateFrictionImpulse calculates the impulse needed to resolve this contact,
// given that the contact has a non-zero coefficient of friction.
func (c *Contact) calculateFrictionImpulse(inverseInertiaTensors [2]m.Matrix3) (impulseContact m.Vector3) {
inverseMass := c.Bodies[0].GetInverseMass()
// the equivalent of a cross product in matrices is multiplication
// by a skew symmetric matrix - we build the matrix for converting
// between linear and angular quantities.
var impulseToTorque m.Matrix3
setSkewSymmetric(&impulseToTorque, &c.relativeContactPosition[0])
// build the matrix to convert contact impulse to change in velocity in
// world coordinates
deltaVelWorld := impulseToTorque.MulMatrix3(&inverseInertiaTensors[0])
deltaVelWorld = deltaVelWorld.MulMatrix3(&impulseToTorque)
deltaVelWorld.MulWith(-1.0)
// check to see if we need to add the second body's data
if c.Bodies[1] != nil {
// set the cross product matrix
setSkewSymmetric(&impulseToTorque, &c.relativeContactPosition[1])
// calculate the velocity change matrix
deltaVelWorld2 := impulseToTorque.MulMatrix3(&inverseInertiaTensors[1])
deltaVelWorld2 = deltaVelWorld2.MulMatrix3(&impulseToTorque)
deltaVelWorld2.MulWith(-1.0)
// add to the total delta velocity
deltaVelWorld.Add(&deltaVelWorld2)
// add to the inverse mass
inverseMass += c.Bodies[1].GetInverseMass()
}
// do a change of basis to convert into contact coordinates
deltaVelocity := c.contactToWorld.Transpose()
deltaVelocity = deltaVelocity.MulMatrix3(&deltaVelWorld)
deltaVelocity = deltaVelocity.MulMatrix3(&c.contactToWorld)
// add in the linear velocity change
deltaVelocity[0] += inverseMass
deltaVelocity[4] += inverseMass
deltaVelocity[8] += inverseMass
// invert to get the impulse needed per unit velocity
impulseMatrix := deltaVelocity.Invert()
// find the target velocities to kill
velKill := m.Vector3{
c.desiredDeltaVelocity,
-c.contactVelocity[1],
-c.contactVelocity[2],
}
// find the impulse to kill target velocities
impulseContact = impulseMatrix.MulVector3(&velKill)
// check for exceeding friction
planarImpulse := m.RealSqrt(impulseContact[1]*impulseContact[1] + impulseContact[2]*impulseContact[2])
if planarImpulse > impulseContact[0]*c.Friction {
// we need to use dynamic friction
impulseContact[1] /= planarImpulse
impulseContact[2] /= planarImpulse
impulseContact[0] = deltaVelocity[0] +
deltaVelocity[3]*c.Friction*impulseContact[1] +
deltaVelocity[6]*c.Friction*impulseContact[2]
impulseContact[0] = c.desiredDeltaVelocity / impulseContact[0]
impulseContact[1] *= c.Friction * impulseContact[0]
impulseContact[2] *= c.Friction * impulseContact[0]
}
return
}
// setSkewSymmetric sets the matrix to be a skew symmetric matrix based on
// the given vector. The skew symmetric matrix is the equivalent of a vector
// cross prodcut. So if a,b are vectors. a x b = A_s b
// where A_s is the skew symmetrix form of a.
func setSkewSymmetric(m *m.Matrix3, v *m.Vector3) {
m[0], m[3], m[6] = 0.0, -v[2], v[1]
m[1], m[4], m[7] = v[2], 0.0, -v[0]
m[2], m[5], m[8] = -v[1], v[0], 0.0
}