Method Initialize

CollisionB2Collision.go:270–347

B2Collision.cpp

(manifold *B2Manifold, xfA B2Transform, radiusA float64, xfB B2Transform, radiusB float64)

///////////////////////////////////////////////////////////////////////////////

func (wm *B2WorldManifold) Initialize(manifold *B2Manifold, xfA B2Transform, radiusA float64, xfB B2Transform, radiusB float64) {
	if manifold.PointCount == 0 {
		return
	}

	switch manifold.Type {
	case B2Manifold_Type.E_circles:
		{
			wm.Normal.Set(1.0, 0.0)
			pointA := B2TransformVec2Mul(xfA, manifold.LocalPoint)
			pointB := B2TransformVec2Mul(xfB, manifold.Points[0].LocalPoint)
			if B2Vec2DistanceSquared(pointA, pointB) > B2_epsilon*B2_epsilon {
				wm.Normal = B2Vec2Sub(pointB, pointA)
				wm.Normal.Normalize()
			}

			cA := B2Vec2Add(pointA, B2Vec2MulScalar(radiusA, wm.Normal))
			cB := B2Vec2Sub(pointB, B2Vec2MulScalar(radiusB, wm.Normal))

			wm.Points[0] = B2Vec2MulScalar(0.5, B2Vec2Add(cA, cB))
			wm.Separations[0] = B2Vec2Dot(B2Vec2Sub(cB, cA), wm.Normal)
		}
		break

	case B2Manifold_Type.E_faceA:
		{
			wm.Normal = B2RotVec2Mul(xfA.Q, manifold.LocalNormal)
			planePoint := B2TransformVec2Mul(xfA, manifold.LocalPoint)

			for i := 0; i < manifold.PointCount; i++ {
				clipPoint := B2TransformVec2Mul(xfB, manifold.Points[i].LocalPoint)
				cA := B2Vec2Add(
					clipPoint,
					B2Vec2MulScalar(
						radiusA-B2Vec2Dot(
							B2Vec2Sub(clipPoint, planePoint),
							wm.Normal,
						),
						wm.Normal,
					),
				)
				cB := B2Vec2Sub(clipPoint, B2Vec2MulScalar(radiusB, wm.Normal))
				wm.Points[i] = B2Vec2MulScalar(0.5, B2Vec2Add(cA, cB))
				wm.Separations[i] = B2Vec2Dot(
					B2Vec2Sub(cB, cA),
					wm.Normal,
				)
			}
		}
		break

	case B2Manifold_Type.E_faceB:
		{
			wm.Normal = B2RotVec2Mul(xfB.Q, manifold.LocalNormal)
			planePoint := B2TransformVec2Mul(xfB, manifold.LocalPoint)

			for i := 0; i < manifold.PointCount; i++ {
				clipPoint := B2TransformVec2Mul(xfA, manifold.Points[i].LocalPoint)