DynamicsB2Contact.go

package box2d

import (
	"math"
)

// Friction mixing law. The idea is to allow either fixture to drive the friction to zero.
// For example, anything slides on ice.
func B2MixFriction(friction1, friction2 float64) float64 {
	return math.Sqrt(friction1 * friction2)
}

// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface.
// For example, a superball bounces on anything.
func B2MixRestitution(restitution1, restitution2 float64) float64 {
	if restitution1 > restitution2 {
		return restitution1
	}

	return restitution2
}

// Restitution mixing law. This picks the lowest value.
func B2MixRestitutionThreshold(threshold1, threshold2 float64) float64 {
	if threshold1 < threshold2 {
		return threshold1
	}

	return threshold2
}

type B2ContactCreateFcn func(fixtureA *B2Fixture, indexA int, fixtureB *B2Fixture, indexB int) B2ContactInterface // returned contact should be a pointer
type B2ContactDestroyFcn func(contact B2ContactInterface)                                                         // contact should be a pointer

type B2ContactRegister struct {
	CreateFcn  B2ContactCreateFcn
	DestroyFcn B2ContactDestroyFcn
	Primary    bool
}

// A contact edge is used to connect bodies and contacts together
// in a contact graph where each body is a node and each contact
// is an edge. A contact edge belongs to a doubly linked list
// maintained in each attached body. Each contact has two contact
// nodes, one for each attached body.
type B2ContactEdge struct {
	Other   *B2Body            // provides quick access to the other body attached.
	Contact B2ContactInterface // the contact
	Prev    *B2ContactEdge     // the previous contact edge in the body's contact list
	Next    *B2ContactEdge     // the next contact edge in the body's contact list
}

func NewB2ContactEdge() *B2ContactEdge {
	return &B2ContactEdge{}
}

var B2Contact_Flag = struct {
	// Used when crawling contact graph when forming islands.
	E_islandFlag uint32

	// Set when the shapes are touching.
	E_touchingFlag uint32

	// This contact can be disabled (by user)
	E_enabledFlag uint32

	// This contact needs filtering because a fixture filter was changed.
	E_filterFlag uint32

	// This bullet contact had a TOI event
	E_bulletHitFlag uint32

	// This contact has a valid TOI in m_toi
	E_toiFlag uint32
}{
	E_islandFlag:    0x0001,
	E_touchingFlag:  0x0002,
	E_enabledFlag:   0x0004,
	E_filterFlag:    0x0008,
	E_bulletHitFlag: 0x0010,
	E_toiFlag:       0x0020,
}

// The class manages contact between two shapes. A contact exists for each overlapping
// AABB in the broad-phase (except if filtered). Therefore a contact object may exist
// that has no contact points.
var s_registers [][]B2ContactRegister
var s_initialized = false

type B2ContactInterface interface {
	GetFlags() uint32
	SetFlags(flags uint32)

	GetPrev() B2ContactInterface
	SetPrev(prev B2ContactInterface)

	GetNext() B2ContactInterface
	SetNext(prev B2ContactInterface)

	GetNodeA() *B2ContactEdge
	SetNodeA(node *B2ContactEdge)

	GetNodeB() *B2ContactEdge
	SetNodeB(node *B2ContactEdge)

	GetFixtureA() *B2Fixture
	SetFixtureA(fixture *B2Fixture)

	GetFixtureB() *B2Fixture
	SetFixtureB(fixture *B2Fixture)

	GetChildIndexA() int
	SetChildIndexA(index int)

	GetChildIndexB() int
	SetChildIndexB(index int)

	GetManifold() *B2Manifold
	SetManifold(manifold *B2Manifold)

	GetTOICount() int
	SetTOICount(toiCount int)

	GetTOI() float64
	SetTOI(toiCount float64)

	GetFriction() float64
	SetFriction(friction float64)
	ResetFriction()

	GetRestitution() float64
	SetRestitution(restitution float64)
	ResetRestitution()
	SetRestitutionThreshold(float64)
	GetRestitutionThreshold() float64
	ResetRestitutionThreshold()

	GetTangentSpeed() float64
	SetTangentSpeed(tangentSpeed float64)

	IsTouching() bool
	IsEnabled() bool
	SetEnabled(bool)

	Evaluate(manifold *B2Manifold, xfA B2Transform, xfB B2Transform)

	FlagForFiltering()

	GetWorldManifold(worldManifold *B2WorldManifold)
}

type B2Contact struct {
	M_flags uint32

	// World pool and list pointers.
	M_prev B2ContactInterface //should be backed by a pointer
	M_next B2ContactInterface //should be backed by a pointer

	// Nodes for connecting bodies.
	M_nodeA *B2ContactEdge
	M_nodeB *B2ContactEdge

	M_fixtureA *B2Fixture
	M_fixtureB *B2Fixture

	M_indexA int
	M_indexB int

	M_manifold *B2Manifold

	M_toiCount             int
	M_toi                  float64
	M_friction             float64
	M_restitution          float64
	M_restitutionThreshold float64

	M_tangentSpeed float64
}

func (contact B2Contact) GetFlags() uint32 {
	return contact.M_flags
}

func (contact *B2Contact) SetFlags(flags uint32) {
	contact.M_flags = flags
}

func (contact B2Contact) GetPrev() B2ContactInterface {
	return contact.M_prev
}

func (contact *B2Contact) SetPrev(prev B2ContactInterface) {
	contact.M_prev = prev
}

func (contact B2Contact) GetNext() B2ContactInterface {
	return contact.M_next
}

func (contact *B2Contact) SetNext(next B2ContactInterface) {
	contact.M_next = next
}

func (contact B2Contact) GetNodeA() *B2ContactEdge {
	return contact.M_nodeA
}

func (contact *B2Contact) SetNodeA(node *B2ContactEdge) {
	contact.M_nodeA = node
}

func (contact B2Contact) GetNodeB() *B2ContactEdge {
	return contact.M_nodeB
}

func (contact *B2Contact) SetNodeB(node *B2ContactEdge) {
	contact.M_nodeB = node
}

func (contact B2Contact) GetFixtureA() *B2Fixture {
	return contact.M_fixtureA
}

func (contact *B2Contact) SetFixtureA(fixture *B2Fixture) {
	contact.M_fixtureA = fixture
}

func (contact B2Contact) GetFixtureB() *B2Fixture {
	return contact.M_fixtureB
}

func (contact *B2Contact) SetFixtureB(fixture *B2Fixture) {
	contact.M_fixtureB = fixture
}

func (contact B2Contact) GetChildIndexA() int {
	return contact.M_indexA
}

func (contact *B2Contact) SetChildIndexA(index int) {
	contact.M_indexA = index
}

func (contact B2Contact) GetChildIndexB() int {
	return contact.M_indexB
}

func (contact *B2Contact) SetChildIndexB(index int) {
	contact.M_indexB = index
}

func (contact B2Contact) GetManifold() *B2Manifold {
	return contact.M_manifold
}

func (contact *B2Contact) SetManifold(manifold *B2Manifold) {
	contact.M_manifold = manifold
}

func (contact B2Contact) GetTOICount() int {
	return contact.M_toiCount
}

func (contact *B2Contact) SetTOICount(toiCount int) {
	contact.M_toiCount = toiCount
}

func (contact B2Contact) GetTOI() float64 {
	return contact.M_toi
}

func (contact *B2Contact) SetTOI(toi float64) {
	contact.M_toi = toi
}

// Get the friction.
func (contact B2Contact) GetFriction() float64 {
	return contact.M_friction
}

// Override the default friction mixture. You can call this in b2ContactListener::PreSolve.
// This value persists until set or reset.
func (contact *B2Contact) SetFriction(friction float64) {
	contact.M_friction = friction
}

// Reset the friction mixture to the default value.
func (contact *B2Contact) ResetFriction() {
	contact.M_friction = B2MixFriction(contact.M_fixtureA.M_friction, contact.M_fixtureB.M_friction)
}

// Get the restitution.
func (contact B2Contact) GetRestitution() float64 {
	return contact.M_restitution
}

// Override the default restitution mixture. You can call this in b2ContactListener::PreSolve.
// The value persists until you set or reset.
func (contact *B2Contact) SetRestitution(restitution float64) {
	contact.M_restitution = restitution
}

// Reset the restitution to the default value.
func (contact *B2Contact) ResetRestitution() {
	contact.M_restitution = B2MixRestitution(contact.M_fixtureA.M_restitution, contact.M_fixtureB.M_restitution)
}

// Override the default restitution velocity threshold mixture. You can call this in b2ContactListener::PreSolve.
// The value persists until you set or reset.
func (contact *B2Contact) SetRestitutionThreshold(threshold float64) {
	contact.M_restitutionThreshold = threshold
}

// Get the restitution threshold.
func (contact B2Contact) GetRestitutionThreshold() float64 {
	return contact.M_restitutionThreshold
}

// Reset the restitution threshold to the default value.
func (contact *B2Contact) ResetRestitutionThreshold() {
	contact.M_restitutionThreshold = B2MixRestitutionThreshold(contact.M_fixtureA.M_restitutionThreshold, contact.M_fixtureB.M_restitutionThreshold)
}

// Get the desired tangent speed. In meters per second.
func (contact B2Contact) GetTangentSpeed() float64 {
	return contact.M_tangentSpeed
}

// Set the desired tangent speed for a conveyor belt behavior. In meters per second.
func (contact *B2Contact) SetTangentSpeed(speed float64) {
	contact.M_tangentSpeed = speed
}

func (contact B2Contact) GetWorldManifold(worldManifold *B2WorldManifold) {
	bodyA := contact.M_fixtureA.GetBody()
	bodyB := contact.M_fixtureB.GetBody()
	shapeA := contact.M_fixtureA.GetShape()
	shapeB := contact.M_fixtureB.GetShape()

	worldManifold.Initialize(contact.M_manifold, bodyA.GetTransform(), shapeA.GetRadius(), bodyB.GetTransform(), shapeB.GetRadius())
}

func (contact *B2Contact) SetEnabled(flag bool) {
	if flag {
		contact.M_flags |= B2Contact_Flag.E_enabledFlag
	} else {
		contact.M_flags &= ^B2Contact_Flag.E_enabledFlag
	}
}

func (contact B2Contact) IsEnabled() bool {
	return (contact.M_flags & B2Contact_Flag.E_enabledFlag) == B2Contact_Flag.E_enabledFlag
}

func (contact B2Contact) IsTouching() bool {
	return (contact.M_flags & B2Contact_Flag.E_touchingFlag) == B2Contact_Flag.E_touchingFlag
}

func (contact *B2Contact) FlagForFiltering() {
	contact.M_flags |= B2Contact_Flag.E_filterFlag
}

///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
// B2Contact.cpp
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////

func B2ContactInitializeRegisters() {
	s_registers = make([][]B2ContactRegister, B2Shape_Type.E_typeCount)
	for i := 0; i < int(B2Shape_Type.E_typeCount); i++ {
		s_registers[i] = make([]B2ContactRegister, B2Shape_Type.E_typeCount)
	}

	AddType(B2CircleContact_Create, B2CircleContact_Destroy, B2Shape_Type.E_circle, B2Shape_Type.E_circle)
	AddType(B2PolygonAndCircleContact_Create, B2PolygonAndCircleContact_Destroy, B2Shape_Type.E_polygon, B2Shape_Type.E_circle)
	AddType(B2PolygonContact_Create, B2PolygonContact_Destroy, B2Shape_Type.E_polygon, B2Shape_Type.E_polygon)
	AddType(B2EdgeAndCircleContact_Create, B2EdgeAndCircleContact_Destroy, B2Shape_Type.E_edge, B2Shape_Type.E_circle)
	AddType(B2EdgeAndPolygonContact_Create, B2EdgeAndPolygonContact_Destroy, B2Shape_Type.E_edge, B2Shape_Type.E_polygon)
	AddType(B2ChainAndCircleContact_Create, B2ChainAndCircleContact_Destroy, B2Shape_Type.E_chain, B2Shape_Type.E_circle)
	AddType(B2ChainAndPolygonContact_Create, B2ChainAndPolygonContact_Destroy, B2Shape_Type.E_chain, B2Shape_Type.E_polygon)
}

func AddType(createFcn B2ContactCreateFcn, destroyFcn B2ContactDestroyFcn, type1 uint8, type2 uint8) {
	B2Assert(0 <= type1 && type1 < B2Shape_Type.E_typeCount)
	B2Assert(0 <= type2 && type2 < B2Shape_Type.E_typeCount)

	s_registers[type1][type2].CreateFcn = createFcn
	s_registers[type1][type2].DestroyFcn = destroyFcn
	s_registers[type1][type2].Primary = true

	if type1 != type2 {
		s_registers[type2][type1].CreateFcn = createFcn
		s_registers[type2][type1].DestroyFcn = destroyFcn
		s_registers[type2][type1].Primary = false
	}
}

func B2ContactFactory(fixtureA *B2Fixture, indexA int, fixtureB *B2Fixture, indexB int) B2ContactInterface { // returned contact should be a pointer

	if s_initialized == false {
		B2ContactInitializeRegisters()
		s_initialized = true
	}

	type1 := fixtureA.GetType()
	type2 := fixtureB.GetType()

	B2Assert(0 <= type1 && type1 < B2Shape_Type.E_typeCount)
	B2Assert(0 <= type2 && type2 < B2Shape_Type.E_typeCount)

	createFcn := s_registers[type1][type2].CreateFcn
	if createFcn != nil {
		if s_registers[type1][type2].Primary {
			return createFcn(fixtureA, indexA, fixtureB, indexB)
		} else {
			return createFcn(fixtureB, indexB, fixtureA, indexA)
		}
	}

	return nil
}

func B2ContactDestroy(contact B2ContactInterface) {
	B2Assert(s_initialized == true)

	fixtureA := contact.GetFixtureA()
	fixtureB := contact.GetFixtureB()

	if contact.GetManifold().PointCount > 0 && fixtureA.IsSensor() == false && fixtureB.IsSensor() == false {
		fixtureA.GetBody().SetAwake(true)
		fixtureB.GetBody().SetAwake(true)
	}

	typeA := fixtureA.GetType()
	typeB := fixtureB.GetType()

	B2Assert(0 <= typeA && typeA < B2Shape_Type.E_typeCount)
	B2Assert(0 <= typeB && typeB < B2Shape_Type.E_typeCount)

	destroyFcn := s_registers[typeA][typeB].DestroyFcn
	destroyFcn(contact)
}

func MakeB2Contact(fA *B2Fixture, indexA int, fB *B2Fixture, indexB int) B2Contact {
	contact := B2Contact{}
	contact.M_flags = B2Contact_Flag.E_enabledFlag

	contact.M_fixtureA = fA
	contact.M_fixtureB = fB

	contact.M_indexA = indexA
	contact.M_indexB = indexB

	contact.M_manifold = NewB2Manifold()
	contact.M_manifold.PointCount = 0

	contact.M_prev = nil
	contact.M_next = nil

	contact.M_nodeA = NewB2ContactEdge()

	contact.M_nodeA.Contact = nil
	contact.M_nodeA.Prev = nil
	contact.M_nodeA.Next = nil
	contact.M_nodeA.Other = nil

	contact.M_nodeB = NewB2ContactEdge()

	contact.M_nodeB.Contact = nil
	contact.M_nodeB.Prev = nil
	contact.M_nodeB.Next = nil
	contact.M_nodeB.Other = nil

	contact.M_toiCount = 0

	contact.M_friction = B2MixFriction(contact.M_fixtureA.M_friction, contact.M_fixtureB.M_friction)
	contact.M_restitution = B2MixRestitution(contact.M_fixtureA.M_restitution, contact.M_fixtureB.M_restitution)
	contact.M_restitutionThreshold = B2MixRestitutionThreshold(contact.M_fixtureA.M_restitutionThreshold, contact.M_fixtureB.M_restitutionThreshold)

	contact.M_tangentSpeed = 0.0

	return contact
}

// Update the contact manifold and touching status.
// Note: do not assume the fixture AABBs are overlapping or are valid.
func B2ContactUpdate(contact B2ContactInterface, listener B2ContactListenerInterface) {
	oldManifold := *contact.GetManifold()

	// Re-enable this contact.
	contact.SetFlags(contact.GetFlags() | B2Contact_Flag.E_enabledFlag)

	touching := false
	wasTouching := (contact.GetFlags() & B2Contact_Flag.E_touchingFlag) == B2Contact_Flag.E_touchingFlag

	sensorA := contact.GetFixtureA().IsSensor()
	sensorB := contact.GetFixtureB().IsSensor()
	sensor := sensorA || sensorB

	bodyA := contact.GetFixtureA().GetBody()
	bodyB := contact.GetFixtureB().GetBody()
	xfA := bodyA.GetTransform()
	xfB := bodyB.GetTransform()

	// Is this contact a sensor?
	if sensor {
		shapeA := contact.GetFixtureA().GetShape()
		shapeB := contact.GetFixtureB().GetShape()
		touching = B2TestOverlapShapes(shapeA, contact.GetChildIndexA(), shapeB, contact.GetChildIndexB(), xfA, xfB)

		// Sensors don't generate manifolds.
		contact.GetManifold().PointCount = 0
	} else {
		// *B2Contact is extended by specialized contact structs and mentionned by B2ContactInterface but not implemented on specialized structs
		// Thus when
		//spew.Dump("AVANT", contact.GetManifold())
		contact.Evaluate(contact.GetManifold(), xfA, xfB) // should be evaluated on specialisations of contact (like CircleContact)
		//spew.Dump("APRES", contact.GetManifold())
		touching = contact.GetManifold().PointCount > 0

		// Match old contact ids to new contact ids and copy the
		// stored impulses to warm start the solver.
		for i := 0; i < contact.GetManifold().PointCount; i++ {
			mp2 := &contact.GetManifold().Points[i]
			mp2.NormalImpulse = 0.0
			mp2.TangentImpulse = 0.0
			id2 := mp2.Id

			for j := 0; j < oldManifold.PointCount; j++ {
				mp1 := &oldManifold.Points[j]

				if mp1.Id.Key() == id2.Key() {
					mp2.NormalImpulse = mp1.NormalImpulse
					mp2.TangentImpulse = mp1.TangentImpulse
					break
				}
			}
		}

		if touching != wasTouching {
			bodyA.SetAwake(true)
			bodyB.SetAwake(true)
		}
	}

	if touching {
		contact.SetFlags(contact.GetFlags() | B2Contact_Flag.E_touchingFlag)
	} else {
		contact.SetFlags(contact.GetFlags() & ^B2Contact_Flag.E_touchingFlag)
	}

	if wasTouching == false && touching == true && listener != nil {
		listener.BeginContact(contact)
	}

	if wasTouching == true && touching == false && listener != nil {
		listener.EndContact(contact)
	}

	if sensor == false && touching && listener != nil {
		listener.PreSolve(contact, oldManifold)
	}
}