Example #1
0
// 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)
}
Example #2
0
// CheckAgainstSphere checks the cube against a sphere to see if there's a collision.
func (cube *CollisionCube) CheckAgainstSphere(sphere *CollisionSphere, existingContacts []*Contact) (bool, []*Contact) {
	// transform the center of the sphere into cube coordinates
	position := sphere.transform.GetAxis(3)
	relCenter := cube.transform.TransformInverse(&position)
	// check to see if we can exclude contact
	if m.RealAbs(relCenter[0])-sphere.Radius > cube.HalfSize[0] ||
		m.RealAbs(relCenter[1])-sphere.Radius > cube.HalfSize[1] ||
		m.RealAbs(relCenter[2])-sphere.Radius > cube.HalfSize[2] {
		return false, existingContacts
	}

	var closestPoint m.Vector3

	// clamp the coordinates to the box
	for i := 0; i < 3; i++ {
		dist := relCenter[i]
		if dist > cube.HalfSize[i] {
			dist = cube.HalfSize[i]
		} else if dist < -cube.HalfSize[i] {
			dist = -cube.HalfSize[i]
		}
		closestPoint[i] = dist
	}

	// check to see if we're in contact
	distCheck := closestPoint
	distCheck.Sub(&relCenter)
	dist := distCheck.SquareMagnitude()
	if dist > sphere.Radius*sphere.Radius {
		return false, existingContacts
	}

	// transform the contact point
	closestPointWorld := cube.transform.MulVector3(&closestPoint)

	// we have contact
	c := NewContact()
	c.ContactPoint = closestPointWorld
	c.ContactNormal = closestPointWorld
	c.ContactNormal.Sub(&position)

	// if the sphere is small enough, or the engine doesn't process fast enough,
	// you can end up having a relCenter position that's the same as closestPoint --
	// meaning that closestPoint didn't need to be clamped to cube bounds.
	//
	// since closestPoint is relCenter at this point, transforming it back to
	// world coordinates makes it equal to the sphere position which will not
	// be able to produce a contact normal.
	if m.RealEqual(c.ContactNormal.Magnitude(), 0.0) {
		// our hack for this is to simply use the sphere's velocity as the contact
		// normal, which is probably not the correct thing to do, but looks okay.
		c.ContactNormal = sphere.Body.Velocity
	}
	c.ContactNormal.Normalize()

	c.Penetration = sphere.Radius
	if !m.RealEqual(dist, 0.0) {
		c.Penetration -= m.RealSqrt(dist)
	} else {
		c.Penetration = 0.0
	}
	c.Bodies[0] = cube.Body
	c.Bodies[1] = sphere.Body

	contacts := append(existingContacts, c)

	// FIXME:
	// TODO: c.Friction and c.Restitution set here are test constants
	c.Friction = 0.9
	c.Restitution = 0.1

	return true, contacts
}
Example #3
0
// 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
}