Exemplo n.º 1
0
func transformToAxis(cube *CollisionCube, axis *m.Vector3) m.Real {
	cubeAxisX := cube.transform.GetAxis(0)
	cubeAxisY := cube.transform.GetAxis(1)
	cubeAxisZ := cube.transform.GetAxis(2)

	return cube.HalfSize[0]*m.RealAbs(axis.Dot(&cubeAxisX)) +
		cube.HalfSize[1]*m.RealAbs(axis.Dot(&cubeAxisY)) +
		cube.HalfSize[2]*m.RealAbs(axis.Dot(&cubeAxisZ))
}
Exemplo n.º 2
0
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)
}
Exemplo n.º 3
0
func contactPoint(pOne *m.Vector3, dOne *m.Vector3, oneSize m.Real,
	pTwo *m.Vector3, dTwo *m.Vector3, twoSize m.Real, useOne bool) m.Vector3 {
	// If useOne is true, and the contact point is outside
	// the edge (in the case of an edge-face contact) then
	// we use one's midpoint, otherwise we use two's.
	//Vector3 toSt, cOne, cTwo;
	//real dpStaOne, dpStaTwo, dpOneTwo, smOne, smTwo;
	//real denom, mua, mub;

	smOne := dOne.SquareMagnitude()
	smTwo := dTwo.SquareMagnitude()
	dpOneTwo := dTwo.Dot(dOne)

	toSt := *pOne
	toSt.Sub(pTwo)
	dpStaOne := dOne.Dot(&toSt)
	dpStaTwo := dTwo.Dot(&toSt)

	denom := smOne*smTwo - dpOneTwo*dpOneTwo

	// Zero denominator indicates parrallel lines
	if m.RealAbs(denom) < m.Epsilon {
		if useOne {
			return *pOne
		}
		return *pTwo
	}

	mua := (dpOneTwo*dpStaTwo - smTwo*dpStaOne) / denom
	mub := (smOne*dpStaTwo - dpOneTwo*dpStaOne) / denom

	// If either of the edges has the nearest point out
	// of bounds, then the edges aren't crossed, we have
	// an edge-face contact. Our point is on the edge, which
	// we know from the useOne parameter.
	if mua > oneSize || mua < -oneSize || mub > twoSize || mub < -twoSize {
		if useOne {
			return *pOne
		}
		return *pTwo
	}

	cOne := *dOne
	cOne.MulWith(mua)
	cOne.Add(pOne)

	cTwo := *dTwo
	cTwo.MulWith(mub)
	cTwo.Add(pTwo)

	cOne.MulWith(0.5)
	cTwo.MulWith(0.5)
	cOne.Add(&cTwo)
	return cOne
}
Exemplo n.º 4
0
// penetrationOnAxis checks if the two boxes overlap along a given axis and
// returns the amount of overlap.
func penetrationOnAxis(one *CollisionCube, two *CollisionCube, axis *m.Vector3, toCenter *m.Vector3) m.Real {
	// project the half-size of one onto axis
	oneProject := transformToAxis(one, axis)
	twoProject := transformToAxis(two, axis)

	// Project this onto the axis
	distance := m.RealAbs(toCenter.Dot(axis))

	// Return the overlap (i.e. positive indicates
	// overlap, negative indicates separation).
	return oneProject + twoProject - distance
}
Exemplo n.º 5
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)
}
Exemplo n.º 6
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
}