Esempio n. 1
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
}
Esempio n. 2
0
func tryAxis(one *CollisionCube, two *CollisionCube, axis m.Vector3, toCenter *m.Vector3,
	index int, smallestPenetration m.Real, smallestCase int) (bool, m.Real, int) {
	// make sure we have a normalized axis, and don't check almost parallel axes
	if axis.SquareMagnitude() < m.Epsilon {
		return true, smallestPenetration, smallestCase
	}

	axis.Normalize()

	penetration := penetrationOnAxis(one, two, &axis, toCenter)
	if penetration < 0 {
		return false, smallestPenetration, smallestCase
	}

	if penetration < smallestPenetration {
		return true, penetration, index
	}

	return true, smallestPenetration, smallestCase
}