From e5767da8a94d96cf6fb3498aeb40c3a73299646c Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sat, 1 Aug 2026 15:16:15 +0300 Subject: [PATCH 01/85] WIP new body accelerator concept --- game/physics/accelerator_body.go | 149 +++++++++++++++++++++++++++++++ game/physics/body.go | 16 ++++ game/physics/scene.go | 33 ++++++- 3 files changed, 197 insertions(+), 1 deletion(-) create mode 100644 game/physics/accelerator_body.go diff --git a/game/physics/accelerator_body.go b/game/physics/accelerator_body.go new file mode 100644 index 00000000..8150f659 --- /dev/null +++ b/game/physics/accelerator_body.go @@ -0,0 +1,149 @@ +package physics + +type BodyAcceleratorID struct { + index int32 + revision int32 +} + +var NilBodyAcceleratorID = BodyAcceleratorID{} + +type BodyAcceleratorView struct { + scene *Scene +} + +func (s BodyAcceleratorView) Create(bodyID BodyID, solver AccelerationSolver) BodyAcceleratorID { + // TODO: Verify that the body exists! + bodyIndex := bodyID.index + + index := s.scene.allocateBodyAccelerator() + + accelerator := &s.scene.bodyAccelerators[index] + accelerator.solver = solver + accelerator.revision++ // progress ID to valid (odd) state + accelerator.isEnabled = true + + s.scene.attachBodyAccelerator(bodyIndex, index) + + return BodyAcceleratorID{ + index: index, + revision: accelerator.revision, + } +} + +func (s BodyAcceleratorView) Delete(id BodyAcceleratorID) { + // TODO: Should I allow the deletion of an invalid ID (noop)? + accelerator := s.resolve(id, true) + accelerator.solver = nil // allow the solver to be garbage collected + accelerator.revision++ // progress ID to invalid (event) state + + s.scene.detachBodyAccelerator(accelerator.bodyIndex, id.index) + s.scene.releaseBodyAccelerator(id.index) +} + +func (s BodyAcceleratorView) Handle(id BodyAcceleratorID) BodyAcceleratorHandle { + return BodyAcceleratorHandle{ + view: s, + id: id, + } +} + +func (s BodyAcceleratorView) IsValid(id BodyAcceleratorID) bool { + accelerator := s.resolve(id, false) + return accelerator != nil +} + +func (s BodyAcceleratorView) BodyID(id BodyAcceleratorID) BodyID { + accelerator := s.resolve(id, true) + bodyIndex := accelerator.bodyIndex + bodyRevision := s.scene.bodies[bodyIndex].reference.Revision + return BodyID{ + index: bodyIndex, + revision: int32(bodyRevision), + } +} + +func (s BodyAcceleratorView) Solver(id BodyAcceleratorID) AccelerationSolver { + accelerator := s.resolve(id, true) + return accelerator.solver +} + +func (s BodyAcceleratorView) SetSolver(id BodyAcceleratorID, solver AccelerationSolver) { + accelerator := s.resolve(id, true) + accelerator.solver = solver +} + +func (s BodyAcceleratorView) Enabled(id BodyAcceleratorID) bool { + accelerator := s.resolve(id, true) + return accelerator.isEnabled +} + +func (s BodyAcceleratorView) SetEnabled(id BodyAcceleratorID, enabled bool) { + accelerator := s.resolve(id, true) + accelerator.isEnabled = enabled +} + +func (s BodyAcceleratorView) resolve(id BodyAcceleratorID, required bool) *bodyAccelerator { + if id.revision == 0 { + if required { + panic("invalid body accelerator ID") + } + return nil + } + accelerator := &s.scene.bodyAccelerators[id.index] + if accelerator.revision != id.revision { + if required { + panic("invalid body accelerator ID") + } + return nil + } + return accelerator +} + +type BodyAcceleratorHandle struct { + view BodyAcceleratorView + id BodyAcceleratorID +} + +func (h BodyAcceleratorHandle) ID() BodyAcceleratorID { + return h.id +} + +func (h BodyAcceleratorHandle) Delete() { + h.view.Delete(h.id) +} + +func (h BodyAcceleratorHandle) IsValid() bool { + return h.view.IsValid(h.id) +} + +func (h BodyAcceleratorHandle) BodyID() BodyID { + return h.view.BodyID(h.id) +} + +func (h BodyAcceleratorHandle) Solver() AccelerationSolver { + return h.view.Solver(h.id) +} + +func (h BodyAcceleratorHandle) SetSolver(solver AccelerationSolver) { + h.view.SetSolver(h.id, solver) +} + +func (h BodyAcceleratorHandle) Enabled() bool { + return h.view.Enabled(h.id) +} + +func (h BodyAcceleratorHandle) SetEnabled(enabled bool) { + h.view.SetEnabled(h.id, enabled) +} + +type bodyAccelerator struct { + solver AccelerationSolver + revision int32 + bodyIndex int32 + nextIndex int32 + isEnabled bool +} + +func (s *bodyAccelerator) isValid() bool { + return s.revision%2 == 1 // only odd revisions are valid +} diff --git a/game/physics/body.go b/game/physics/body.go index 90d16eff..3d79b9b5 100644 --- a/game/physics/body.go +++ b/game/physics/body.go @@ -8,6 +8,13 @@ import ( "github.com/mokiat/lacking/game/physics/solver" ) +var NilBodyID = BodyID{} + +type BodyID struct { + index int32 + revision int32 +} + var invalidBodyState = &bodyState{} type BodyDefinitionInfo struct { @@ -82,6 +89,13 @@ type Body struct { reference indexReference } +func (b Body) ID() BodyID { + return BodyID{ + index: int32(b.reference.Index), + revision: int32(b.reference.Revision), + } +} + // Name returns the name of this body. func (b Body) Name() string { state := b.state() @@ -303,6 +317,8 @@ func (b Body) state() *bodyState { type bodyState struct { reference indexReference + firstBodyAcceleratorIndex int32 + objectID placement3d.ObjectID name string diff --git a/game/physics/scene.go b/game/physics/scene.go index 1be9808c..777bde54 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -91,7 +91,7 @@ type Scene struct { bodyConstraintPlaceholders []solver.Placeholder freeBodyIndices *ds.Stack[uint32] - // bodyAccelerators []any // TOOD + bodyAccelerators []bodyAccelerator freeBodyAcceleratorIndices *ds.Stack[uint32] // areaAccelerators []any // TODO @@ -953,6 +953,35 @@ func (s *Scene) deinitPlaceholder(placeholder *solver.Placeholder, body *bodySta }) } +// TODO +// func (s *Scene) GlobalAccelerators() *GlobalAcceleratorView { +// return GlobalAcceleratorView{ +// scene: s, +// } +// } + +func (s *Scene) BodyAccelerators() BodyAcceleratorView { + return BodyAcceleratorView{ + scene: s, + } +} + +func (s *Scene) allocateBodyAccelerator() int32 { + panic("TODO") +} + +func (s *Scene) releaseBodyAccelerator(index int32) { + panic("TODO") +} + +func (s *Scene) attachBodyAccelerator(bodyIndex, index int32) { + panic("TODO") +} + +func (s *Scene) detachBodyAccelerator(bodyIndex, index int32) { + panic("TODO") +} + type bodyRef struct { index uint32 } @@ -970,3 +999,5 @@ type dbCollisionPair struct { PrimaryRef indexReference SecondaryRef indexReference } + +var nilIndex int32 = -1 From 1843ca2ebe6126870467d04153d760819df51e0e Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sat, 1 Aug 2026 15:45:32 +0300 Subject: [PATCH 02/85] Rework global accelerators --- game/physics/accelerator.go | 82 ------------------- game/physics/accelerator_body.go | 54 ++++++------ game/physics/accelerator_global.go | 127 +++++++++++++++++++++++++++++ game/physics/scene.go | 67 +++++++++------ 4 files changed, 195 insertions(+), 135 deletions(-) delete mode 100644 game/physics/accelerator.go create mode 100644 game/physics/accelerator_global.go diff --git a/game/physics/accelerator.go b/game/physics/accelerator.go deleted file mode 100644 index 0ae051a6..00000000 --- a/game/physics/accelerator.go +++ /dev/null @@ -1,82 +0,0 @@ -package physics - -var invalidGlobalAcceleratorState = &globalAcceleratorState{} - -// GlobalAccelerator represents a force that is applied to all -// bodies in the scene. -type GlobalAccelerator struct { - scene *Scene - reference indexReference -} - -// Logic returns the acceleration solver that will be used to -// apply this global accelerator. -// -// TODO: Rename to Solver. -func (a GlobalAccelerator) Logic() AccelerationSolver { - state := a.state() - return state.logic -} - -// Enabled returns whether this global accelerator will be applied. -func (a GlobalAccelerator) Enabled() bool { - state := a.state() - return state.enabled -} - -// SetEnabled changes whether this global accelerator will be applied. -func (a GlobalAccelerator) SetEnabled(enabled bool) { - state := a.state() - state.enabled = enabled -} - -// Delete removes this global accelerator. -func (a GlobalAccelerator) Delete() { - deleteGlobalAccelerator(a.scene, a.reference) -} - -func (a GlobalAccelerator) state() *globalAcceleratorState { - index := a.reference.Index - state := &a.scene.globalAccelerators[index] - if state.reference != a.reference { - return invalidGlobalAcceleratorState - } - return state -} - -type globalAcceleratorState struct { - reference indexReference - logic AccelerationSolver - enabled bool -} - -func createGlobalAccelerator(scene *Scene, logic AccelerationSolver) GlobalAccelerator { - var freeIndex uint32 - if scene.freeGlobalAcceleratorIndices.IsEmpty() { - freeIndex = uint32(len(scene.globalAccelerators)) - scene.globalAccelerators = append(scene.globalAccelerators, globalAcceleratorState{}) - } else { - freeIndex = scene.freeGlobalAcceleratorIndices.Pop() - } - - reference := newIndexReference(freeIndex, scene.nextRevision()) - scene.globalAccelerators[freeIndex] = globalAcceleratorState{ - reference: reference, - logic: logic, - enabled: true, - } - return GlobalAccelerator{ - scene: scene, - reference: reference, - } -} - -func deleteGlobalAccelerator(scene *Scene, reference indexReference) { - index := reference.Index - state := &scene.globalAccelerators[index] - if state.reference == reference { - state.reference = newIndexReference(index, 0) - state.logic = nil - scene.freeGlobalAcceleratorIndices.Push(index) - } -} diff --git a/game/physics/accelerator_body.go b/game/physics/accelerator_body.go index 8150f659..65363d37 100644 --- a/game/physics/accelerator_body.go +++ b/game/physics/accelerator_body.go @@ -11,18 +11,18 @@ type BodyAcceleratorView struct { scene *Scene } -func (s BodyAcceleratorView) Create(bodyID BodyID, solver AccelerationSolver) BodyAcceleratorID { +func (v BodyAcceleratorView) Create(bodyID BodyID, solver AccelerationSolver) BodyAcceleratorID { // TODO: Verify that the body exists! bodyIndex := bodyID.index - index := s.scene.allocateBodyAccelerator() + index := v.scene.allocateBodyAccelerator() - accelerator := &s.scene.bodyAccelerators[index] + accelerator := &v.scene.bodyAccelerators[index] accelerator.solver = solver - accelerator.revision++ // progress ID to valid (odd) state + accelerator.revision++ // progress revision to valid (odd) value accelerator.isEnabled = true - s.scene.attachBodyAccelerator(bodyIndex, index) + v.scene.attachBodyAccelerator(bodyIndex, index) return BodyAcceleratorID{ index: index, @@ -30,66 +30,66 @@ func (s BodyAcceleratorView) Create(bodyID BodyID, solver AccelerationSolver) Bo } } -func (s BodyAcceleratorView) Delete(id BodyAcceleratorID) { +func (v BodyAcceleratorView) Delete(id BodyAcceleratorID) { // TODO: Should I allow the deletion of an invalid ID (noop)? - accelerator := s.resolve(id, true) + accelerator := v.resolve(id, true) accelerator.solver = nil // allow the solver to be garbage collected - accelerator.revision++ // progress ID to invalid (event) state + accelerator.revision++ // progress revision to invalid (even) value - s.scene.detachBodyAccelerator(accelerator.bodyIndex, id.index) - s.scene.releaseBodyAccelerator(id.index) + v.scene.detachBodyAccelerator(accelerator.bodyIndex, id.index) + v.scene.releaseBodyAccelerator(id.index) } -func (s BodyAcceleratorView) Handle(id BodyAcceleratorID) BodyAcceleratorHandle { +func (v BodyAcceleratorView) Handle(id BodyAcceleratorID) BodyAcceleratorHandle { return BodyAcceleratorHandle{ - view: s, + view: v, id: id, } } -func (s BodyAcceleratorView) IsValid(id BodyAcceleratorID) bool { - accelerator := s.resolve(id, false) +func (v BodyAcceleratorView) IsValid(id BodyAcceleratorID) bool { + accelerator := v.resolve(id, false) return accelerator != nil } -func (s BodyAcceleratorView) BodyID(id BodyAcceleratorID) BodyID { - accelerator := s.resolve(id, true) +func (v BodyAcceleratorView) BodyID(id BodyAcceleratorID) BodyID { + accelerator := v.resolve(id, true) bodyIndex := accelerator.bodyIndex - bodyRevision := s.scene.bodies[bodyIndex].reference.Revision + bodyRevision := v.scene.bodies[bodyIndex].reference.Revision return BodyID{ index: bodyIndex, revision: int32(bodyRevision), } } -func (s BodyAcceleratorView) Solver(id BodyAcceleratorID) AccelerationSolver { - accelerator := s.resolve(id, true) +func (v BodyAcceleratorView) Solver(id BodyAcceleratorID) AccelerationSolver { + accelerator := v.resolve(id, true) return accelerator.solver } -func (s BodyAcceleratorView) SetSolver(id BodyAcceleratorID, solver AccelerationSolver) { - accelerator := s.resolve(id, true) +func (v BodyAcceleratorView) SetSolver(id BodyAcceleratorID, solver AccelerationSolver) { + accelerator := v.resolve(id, true) accelerator.solver = solver } -func (s BodyAcceleratorView) Enabled(id BodyAcceleratorID) bool { - accelerator := s.resolve(id, true) +func (v BodyAcceleratorView) Enabled(id BodyAcceleratorID) bool { + accelerator := v.resolve(id, true) return accelerator.isEnabled } -func (s BodyAcceleratorView) SetEnabled(id BodyAcceleratorID, enabled bool) { - accelerator := s.resolve(id, true) +func (v BodyAcceleratorView) SetEnabled(id BodyAcceleratorID, enabled bool) { + accelerator := v.resolve(id, true) accelerator.isEnabled = enabled } -func (s BodyAcceleratorView) resolve(id BodyAcceleratorID, required bool) *bodyAccelerator { +func (v BodyAcceleratorView) resolve(id BodyAcceleratorID, required bool) *bodyAccelerator { if id.revision == 0 { if required { panic("invalid body accelerator ID") } return nil } - accelerator := &s.scene.bodyAccelerators[id.index] + accelerator := &v.scene.bodyAccelerators[id.index] if accelerator.revision != id.revision { if required { panic("invalid body accelerator ID") diff --git a/game/physics/accelerator_global.go b/game/physics/accelerator_global.go new file mode 100644 index 00000000..5489940a --- /dev/null +++ b/game/physics/accelerator_global.go @@ -0,0 +1,127 @@ +package physics + +type GlobalAcceleratorID struct { + index int32 + revision int32 +} + +var NilGlobalAcceleratorID = GlobalAcceleratorID{} + +type GlobalAcceleratorView struct { + scene *Scene +} + +func (v GlobalAcceleratorView) Create(solver AccelerationSolver) GlobalAcceleratorID { + index := v.scene.allocateGlobalAccelerator() + + accelerator := &v.scene.globalAccelerators[index] + accelerator.solver = solver + accelerator.revision++ // progress revision to valid (odd) value + accelerator.enabled = true + + return GlobalAcceleratorID{ + index: index, + revision: accelerator.revision, + } +} + +func (v GlobalAcceleratorView) Delete(id GlobalAcceleratorID) { + // TODO: Should I allow the deletion of an invalid ID (noop)? + accelerator := v.resolve(id, true) + accelerator.solver = nil // allow the solver to be garbage collected + accelerator.revision++ // progress revision to invalid (even) value + + v.scene.releaseGlobalAccelerator(id.index) +} + +func (v GlobalAcceleratorView) Handle(id GlobalAcceleratorID) GlobalAcceleratorHandle { + return GlobalAcceleratorHandle{ + view: v, + id: id, + } +} + +func (v GlobalAcceleratorView) IsValid(id GlobalAcceleratorID) bool { + accelerator := v.resolve(id, false) + return accelerator != nil +} + +func (v GlobalAcceleratorView) Solver(id GlobalAcceleratorID) AccelerationSolver { + accelerator := v.resolve(id, true) + return accelerator.solver +} + +func (v GlobalAcceleratorView) SetSolver(id GlobalAcceleratorID, solver AccelerationSolver) { + accelerator := v.resolve(id, true) + accelerator.solver = solver +} + +func (v GlobalAcceleratorView) Enabled(id GlobalAcceleratorID) bool { + accelerator := v.resolve(id, true) + return accelerator.enabled +} + +func (v GlobalAcceleratorView) SetEnabled(id GlobalAcceleratorID, enabled bool) { + accelerator := v.resolve(id, true) + accelerator.enabled = enabled +} + +func (v GlobalAcceleratorView) resolve(id GlobalAcceleratorID, required bool) *globalAccelerator { + if id.revision == 0 { + if required { + panic("invalid global accelerator ID") + } + return nil + } + accelerator := &v.scene.globalAccelerators[id.index] + if accelerator.revision != id.revision { + if required { + panic("invalid global accelerator ID") + } + return nil + } + return accelerator +} + +type GlobalAcceleratorHandle struct { + view GlobalAcceleratorView + id GlobalAcceleratorID +} + +func (h GlobalAcceleratorHandle) ID() GlobalAcceleratorID { + return h.id +} + +func (h GlobalAcceleratorHandle) Delete() { + h.view.Delete(h.id) +} + +func (h GlobalAcceleratorHandle) IsValid() bool { + return h.view.IsValid(h.id) +} + +func (h GlobalAcceleratorHandle) Solver() AccelerationSolver { + return h.view.Solver(h.id) +} + +func (h GlobalAcceleratorHandle) SetSolver(solver AccelerationSolver) { + h.view.SetSolver(h.id, solver) +} + +func (h GlobalAcceleratorHandle) Enabled() bool { + return h.view.Enabled(h.id) +} + +func (h GlobalAcceleratorHandle) SetEnabled(enabled bool) { + h.view.SetEnabled(h.id, enabled) +} + +type globalAccelerator struct { + solver AccelerationSolver + revision int32 + enabled bool +} + +func (s *globalAccelerator) isValid() bool { + return s.revision%2 == 1 // only odd revisions are valid +} diff --git a/game/physics/scene.go b/game/physics/scene.go index 777bde54..badadcfa 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -42,13 +42,14 @@ func NewScene() *Scene { bodyAccelerationTargets: make([]AccelerationTarget, 0, 64), bodyConstraintPlaceholders: make([]solver.Placeholder, 0, 64), + globalAccelerators: make([]globalAccelerator, 1), + freeGlobalAcceleratorIndices: ds.EmptyStack[int32](), + // bodyAccelerators []any // TOOD - // areaAccelerators []any // TODO - globalAccelerators: make([]globalAcceleratorState, 0, 64), + freeBodyAcceleratorIndices: ds.PreallocatedStack[uint32](16), - freeBodyAcceleratorIndices: ds.PreallocatedStack[uint32](16), - freeAreaAcceleratorIndices: ds.PreallocatedStack[uint32](16), - freeGlobalAcceleratorIndices: ds.PreallocatedStack[uint32](16), + // areaAccelerators []any // TODO + freeAreaAcceleratorIndices: ds.PreallocatedStack[uint32](16), sbConstraints: make([]sbConstraintState, 0, 64), dbConstraints: make([]dbConstraintState, 0, 64), @@ -97,8 +98,8 @@ type Scene struct { // areaAccelerators []any // TODO freeAreaAcceleratorIndices *ds.Stack[uint32] - globalAccelerators []globalAcceleratorState - freeGlobalAcceleratorIndices *ds.Stack[uint32] + globalAccelerators []globalAccelerator + freeGlobalAcceleratorIndices *ds.Stack[int32] sbConstraints []sbConstraintState freeSBConstraintIndices *ds.Stack[uint32] @@ -245,12 +246,6 @@ func (s *Scene) NextCollisionRejectGroup() uint32 { return s.freeCollisionRejectGroup } -// CreateGlobalAccelerator creates a new accelerator that affects the whole -// scene. -func (s *Scene) CreateGlobalAccelerator(logic AccelerationSolver) GlobalAccelerator { - return createGlobalAccelerator(s, logic) -} - // CreateProp creates a new static Prop. A prop is an object // that is static and rarely removed. func (s *Scene) CreateProp(info PropInfo) { @@ -426,17 +421,16 @@ func (s *Scene) applyAreaAccelerators() { func (s *Scene) applyGlobalAccelerators() { s.eachBodyState(func(index int, _ *bodyState) { target := &s.bodyAccelerationTargets[index] - position := target.Position() - for _, accelerator := range s.globalAccelerators { - if !accelerator.reference.IsValid() || !accelerator.enabled { - continue - } + s.eachGlobalAccelerator(func(_ int, accelerator *globalAccelerator) { + // TODO: Consider caching the following calculation, especially + // if the medium solver is expensive to compute. + position := target.Position() ctx := AccelerationContext{ MediumVelocity: s.mediumSolver.Velocity(position), MediumDensity: s.mediumSolver.Density(position), } - accelerator.logic.ApplyAcceleration(ctx, target) - } + accelerator.solver.ApplyAcceleration(ctx, target) + }) }) } @@ -953,12 +947,11 @@ func (s *Scene) deinitPlaceholder(placeholder *solver.Placeholder, body *bodySta }) } -// TODO -// func (s *Scene) GlobalAccelerators() *GlobalAcceleratorView { -// return GlobalAcceleratorView{ -// scene: s, -// } -// } +func (s *Scene) GlobalAccelerators() GlobalAcceleratorView { + return GlobalAcceleratorView{ + scene: s, + } +} func (s *Scene) BodyAccelerators() BodyAcceleratorView { return BodyAcceleratorView{ @@ -966,6 +959,28 @@ func (s *Scene) BodyAccelerators() BodyAcceleratorView { } } +func (s *Scene) allocateGlobalAccelerator() int32 { + if !s.freeGlobalAcceleratorIndices.IsEmpty() { + return s.freeGlobalAcceleratorIndices.Pop() + } + index := int32(len(s.globalAccelerators)) + s.globalAccelerators = append(s.globalAccelerators, globalAccelerator{}) + return index +} + +func (s *Scene) releaseGlobalAccelerator(index int32) { + s.freeGlobalAcceleratorIndices.Push(index) +} + +func (s *Scene) eachGlobalAccelerator(cb func(index int, accelerator *globalAccelerator)) { + for i := range s.globalAccelerators { + accelerator := &s.globalAccelerators[i] + if accelerator.isValid() { + cb(i, accelerator) + } + } +} + func (s *Scene) allocateBodyAccelerator() int32 { panic("TODO") } From c09fbc755429bb9e6cc40c32343f0789ab8bbad7 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sat, 1 Aug 2026 15:59:04 +0300 Subject: [PATCH 03/85] Check enabled state in global acceleration evaluation --- game/physics/scene.go | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/game/physics/scene.go b/game/physics/scene.go index badadcfa..2e1b8e49 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -422,14 +422,16 @@ func (s *Scene) applyGlobalAccelerators() { s.eachBodyState(func(index int, _ *bodyState) { target := &s.bodyAccelerationTargets[index] s.eachGlobalAccelerator(func(_ int, accelerator *globalAccelerator) { - // TODO: Consider caching the following calculation, especially - // if the medium solver is expensive to compute. - position := target.Position() - ctx := AccelerationContext{ - MediumVelocity: s.mediumSolver.Velocity(position), - MediumDensity: s.mediumSolver.Density(position), + if accelerator.enabled { + // TODO: Consider caching the following calculation, especially + // if the medium solver is expensive to compute. + position := target.Position() + ctx := AccelerationContext{ + MediumVelocity: s.mediumSolver.Velocity(position), + MediumDensity: s.mediumSolver.Density(position), + } + accelerator.solver.ApplyAcceleration(ctx, target) } - accelerator.solver.ApplyAcceleration(ctx, target) }) }) } From cb4f672af913b2a1261212e4c831ba01324b0a0b Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sat, 1 Aug 2026 15:59:16 +0300 Subject: [PATCH 04/85] Add godoc --- game/physics/accelerator_global.go | 74 +++++++++++++++++++++++++++++- game/physics/scene.go | 2 + 2 files changed, 75 insertions(+), 1 deletion(-) diff --git a/game/physics/accelerator_global.go b/game/physics/accelerator_global.go index 5489940a..de67b429 100644 --- a/game/physics/accelerator_global.go +++ b/game/physics/accelerator_global.go @@ -1,16 +1,34 @@ package physics +// GlobalAcceleratorID uniquely identifies a global accelerator that was +// created through [GlobalAcceleratorView.Create]. +// +// The zero value, also available as [NilGlobalAcceleratorID], does not +// reference a valid global accelerator. type GlobalAcceleratorID struct { index int32 revision int32 } +// NilGlobalAcceleratorID is a [GlobalAcceleratorID] that never references a +// valid global accelerator. var NilGlobalAcceleratorID = GlobalAcceleratorID{} +// GlobalAcceleratorView provides access to the global accelerators that +// belong to a [Scene]. +// +// A global accelerator evaluates its [AccelerationSolver] once for every +// body in the scene, on every simulation step, irrespective of the body's +// position. It is intended for scene-wide effects, such as gravity or wind, +// as opposed to a body accelerator, which affects a single, specific body. type GlobalAcceleratorView struct { scene *Scene } +// Create allocates a new global accelerator that uses the specified solver +// and returns its ID. +// +// The accelerator is enabled by default. func (v GlobalAcceleratorView) Create(solver AccelerationSolver) GlobalAcceleratorID { index := v.scene.allocateGlobalAccelerator() @@ -25,8 +43,13 @@ func (v GlobalAcceleratorView) Create(solver AccelerationSolver) GlobalAccelerat } } +// Delete removes the global accelerator with the specified ID. +// +// It panics if the ID does not reference a valid global accelerator, be it +// because it was never created, has already been deleted, or belongs to a +// different [Scene]. Use [GlobalAcceleratorView.IsValid] first if the ID's +// validity is not otherwise guaranteed. func (v GlobalAcceleratorView) Delete(id GlobalAcceleratorID) { - // TODO: Should I allow the deletion of an invalid ID (noop)? accelerator := v.resolve(id, true) accelerator.solver = nil // allow the solver to be garbage collected accelerator.revision++ // progress revision to invalid (even) value @@ -34,6 +57,9 @@ func (v GlobalAcceleratorView) Delete(id GlobalAcceleratorID) { v.scene.releaseGlobalAccelerator(id.index) } +// Handle returns a [GlobalAcceleratorHandle] that wraps the specified ID, +// as a more convenient means of repeatedly accessing the same global +// accelerator without having to pass its ID to this view on every call. func (v GlobalAcceleratorView) Handle(id GlobalAcceleratorID) GlobalAcceleratorHandle { return GlobalAcceleratorHandle{ view: v, @@ -41,26 +67,44 @@ func (v GlobalAcceleratorView) Handle(id GlobalAcceleratorID) GlobalAcceleratorH } } +// IsValid returns whether the specified ID references a global accelerator +// that has not been deleted. func (v GlobalAcceleratorView) IsValid(id GlobalAcceleratorID) bool { accelerator := v.resolve(id, false) return accelerator != nil } +// Solver returns the acceleration solver used by the specified global +// accelerator. +// +// It panics if the ID does not reference a valid global accelerator. func (v GlobalAcceleratorView) Solver(id GlobalAcceleratorID) AccelerationSolver { accelerator := v.resolve(id, true) return accelerator.solver } +// SetSolver changes the acceleration solver used by the specified global +// accelerator. +// +// It panics if the ID does not reference a valid global accelerator. func (v GlobalAcceleratorView) SetSolver(id GlobalAcceleratorID, solver AccelerationSolver) { accelerator := v.resolve(id, true) accelerator.solver = solver } +// Enabled returns whether the specified global accelerator is evaluated +// during the simulation. A global accelerator is enabled by default. +// +// It panics if the ID does not reference a valid global accelerator. func (v GlobalAcceleratorView) Enabled(id GlobalAcceleratorID) bool { accelerator := v.resolve(id, true) return accelerator.enabled } +// SetEnabled changes whether the specified global accelerator is evaluated +// during the simulation. +// +// It panics if the ID does not reference a valid global accelerator. func (v GlobalAcceleratorView) SetEnabled(id GlobalAcceleratorID, enabled bool) { accelerator := v.resolve(id, true) accelerator.enabled = enabled @@ -83,35 +127,63 @@ func (v GlobalAcceleratorView) resolve(id GlobalAcceleratorID, required bool) *g return accelerator } +// GlobalAcceleratorHandle is a convenience wrapper that binds together a +// [GlobalAcceleratorID] and the [GlobalAcceleratorView] needed to resolve +// it, so that callers that repeatedly act on the same global accelerator +// do not have to keep passing its ID around. +// +// It is created through [GlobalAcceleratorView.Handle]. type GlobalAcceleratorHandle struct { view GlobalAcceleratorView id GlobalAcceleratorID } +// ID returns the [GlobalAcceleratorID] wrapped by this handle. func (h GlobalAcceleratorHandle) ID() GlobalAcceleratorID { return h.id } +// Delete removes the wrapped global accelerator. +// +// It panics if the handle does not reference a valid global accelerator. func (h GlobalAcceleratorHandle) Delete() { h.view.Delete(h.id) } +// IsValid returns whether the wrapped global accelerator has not been +// deleted. func (h GlobalAcceleratorHandle) IsValid() bool { return h.view.IsValid(h.id) } +// Solver returns the acceleration solver used by the wrapped global +// accelerator. +// +// It panics if the handle does not reference a valid global accelerator. func (h GlobalAcceleratorHandle) Solver() AccelerationSolver { return h.view.Solver(h.id) } +// SetSolver changes the acceleration solver used by the wrapped global +// accelerator. +// +// It panics if the handle does not reference a valid global accelerator. func (h GlobalAcceleratorHandle) SetSolver(solver AccelerationSolver) { h.view.SetSolver(h.id, solver) } +// Enabled returns whether the wrapped global accelerator is evaluated +// during the simulation. A global accelerator is enabled by default. +// +// It panics if the handle does not reference a valid global accelerator. func (h GlobalAcceleratorHandle) Enabled() bool { return h.view.Enabled(h.id) } +// SetEnabled changes whether the wrapped global accelerator is evaluated +// during the simulation. +// +// It panics if the handle does not reference a valid global accelerator. func (h GlobalAcceleratorHandle) SetEnabled(enabled bool) { h.view.SetEnabled(h.id, enabled) } diff --git a/game/physics/scene.go b/game/physics/scene.go index 2e1b8e49..3d793648 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -949,6 +949,8 @@ func (s *Scene) deinitPlaceholder(placeholder *solver.Placeholder, body *bodySta }) } +// GlobalAccelerators returns a [GlobalAcceleratorView] through which the +// global accelerators of this scene can be created and managed. func (s *Scene) GlobalAccelerators() GlobalAcceleratorView { return GlobalAcceleratorView{ scene: s, From 8d1ae9eb274c79e64821596fe92fce23b64c0504 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sat, 1 Aug 2026 16:37:51 +0300 Subject: [PATCH 05/85] Remove unused constraints sets --- game/physics/constraint_set.go | 74 ---------------------------------- game/physics/scene.go | 7 ---- 2 files changed, 81 deletions(-) delete mode 100644 game/physics/constraint_set.go diff --git a/game/physics/constraint_set.go b/game/physics/constraint_set.go deleted file mode 100644 index fc098676..00000000 --- a/game/physics/constraint_set.go +++ /dev/null @@ -1,74 +0,0 @@ -package physics - -import "github.com/mokiat/lacking/game/physics/solver" - -// ConstraintSet represents a set of constraints. -// -// This type is useful when multiple constraints need to -// be managed (enabled,disabled,deleted) as a single unit. -type ConstraintSet struct { - scene *Scene - sbConstraints []SBConstraint - dbConstraints []DBConstraint -} - -// CreateSingleBodyConstraint creates a new physics constraint that acts on -// a single body and stores it in this set. -// -// Note: Constraints creates as part of this set should not be deleted -// individually. -func (s *ConstraintSet) CreateSingleBodyConstraint(body Body, solver solver.Constraint) SBConstraint { - constraint := s.scene.CreateSingleBodyConstraint(body, solver) - s.sbConstraints = append(s.sbConstraints, constraint) - return constraint -} - -// CreateDoubleBodyConstraint creates a new physics constraint that acts on -// two bodies and enables it for this scene. -// -// Note: Constraints creates as part of this set should not be deleted -// individually. -func (s *ConstraintSet) CreateDoubleBodyConstraint(primary, secondary Body, solver solver.PairConstraint) DBConstraint { - constraint := s.scene.CreateDoubleBodyConstraint(primary, secondary, solver) - s.dbConstraints = append(s.dbConstraints, constraint) - return constraint -} - -// Enabled returns whether at least one of the constraints -// in this set is enabled. -func (s *ConstraintSet) Enabled() bool { - for _, constraint := range s.sbConstraints { - if constraint.Enabled() { - return true - } - } - for _, constraint := range s.dbConstraints { - if constraint.Enabled() { - return true - } - } - return false -} - -// SetEnabled changes the enabled state of all -// constraints in this set. -func (s *ConstraintSet) SetEnabled(enabled bool) { - for _, constraint := range s.sbConstraints { - constraint.SetEnabled(enabled) - } - for _, constraint := range s.dbConstraints { - constraint.SetEnabled(enabled) - } -} - -// Delete deletes all contained constraints and this -// set. -func (s *ConstraintSet) Delete() { - for _, constraint := range s.sbConstraints { - constraint.Delete() - } - for _, constraint := range s.dbConstraints { - constraint.Delete() - } - s.scene = nil -} diff --git a/game/physics/scene.go b/game/physics/scene.go index 3d793648..8f1ef3bd 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -297,13 +297,6 @@ func (s *Scene) CreateBody(info BodyInfo) Body { return createBody(s, info) } -// CreateConstraintSet creates a new ConstraintSet. -func (s *Scene) CreateConstraintSet() *ConstraintSet { - return &ConstraintSet{ - scene: s, - } -} - // CreateSingleBodyConstraint creates a new physics constraint that acts on // a single body and enables it for this scene. func (s *Scene) CreateSingleBodyConstraint(body Body, logic solver.Constraint) SBConstraint { From 046ecfa439618e6855ef089debc03b49a5390c89 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sat, 1 Aug 2026 22:38:22 +0300 Subject: [PATCH 06/85] WIP: New solo constraint API --- game/physics/constraint_solo.go | 138 ++++++++++++++++++++++++++++ game/physics/scene.go | 156 +++++++++++++++++++------------- 2 files changed, 230 insertions(+), 64 deletions(-) create mode 100644 game/physics/constraint_solo.go diff --git a/game/physics/constraint_solo.go b/game/physics/constraint_solo.go new file mode 100644 index 00000000..aae8af62 --- /dev/null +++ b/game/physics/constraint_solo.go @@ -0,0 +1,138 @@ +package physics + +import "github.com/mokiat/lacking/game/physics/solver" + +type SoloConstraintContext struct { + DeltaSeconds float64 + ImpulseBeta float64 + NudgeBeta float64 + + Target *solver.Placeholder // TODO: use package-local ImpulseTarget instead of Placeholder +} + +type SoloConstraintSolver interface { + // Reset clears the internal cache state for this constraint solver. + // + // This is called at the start of every iteration. + Reset(ctx SoloConstraintContext) + + // ApplyImpulses is called by the physics engine to instruct the solver + // to apply the necessary impulses to its object. + // + // This is called multiple times per iteration. + ApplyImpulses(ctx SoloConstraintContext) + + // ApplyNudges is called by the physics engine to instruct the solver to + // apply the necessary nudges to its object. + // + // This is called multiple times per iteration. + ApplyNudges(ctx SoloConstraintContext) +} + +type SoloConstraintID struct { + index int32 + revision int32 +} + +var NilSoloConstraintID = SoloConstraintID{} + +type SoloConstraintView struct { + scene *Scene +} + +func (v SoloConstraintView) Create(bodyID BodyID, solver SoloConstraintSolver) SoloConstraintID { + // TODO: verify that bodyID is valid and belongs to this scene + + index := v.scene.allocateSoloConstraint() + + constraint := &v.scene.soloConstraints[index] + constraint.solver = solver + constraint.revision++ // progress revision to valid (odd) value + constraint.bodyIndex = bodyID.index + constraint.enabled = true + + return SoloConstraintID{ + index: index, + revision: constraint.revision, + } +} + +func (v SoloConstraintView) Delete(id SoloConstraintID) { + constraint := v.resolve(id, true) + constraint.solver = nil // allow the solver to be garbage collected + constraint.revision++ // progress revision to invalid (even) value + + v.scene.releaseSoloConstraint(id.index) +} + +func (v SoloConstraintView) Handle(id SoloConstraintID) SoloConstraintHandle { + return SoloConstraintHandle{ + view: v, + id: id, + } +} + +func (v SoloConstraintView) IsValid(id SoloConstraintID) bool { + constraint := v.resolve(id, false) + return constraint != nil +} + +func (v SoloConstraintView) BodyID(id SoloConstraintID) BodyID { + constraint := v.resolve(id, true) + body := &v.scene.bodies[constraint.bodyIndex] + return BodyID{ + index: constraint.bodyIndex, + revision: int32(body.reference.Revision), + } +} + +func (v SoloConstraintView) Solver(id SoloConstraintID) SoloConstraintSolver { + constraint := v.resolve(id, true) + return constraint.solver +} + +func (v SoloConstraintView) SetSolver(id SoloConstraintID, solver SoloConstraintSolver) { + constraint := v.resolve(id, true) + constraint.solver = solver +} + +func (v SoloConstraintView) Enabled(id SoloConstraintID) bool { + constraint := v.resolve(id, true) + return constraint.enabled +} + +func (v SoloConstraintView) SetEnabled(id SoloConstraintID, enabled bool) { + constraint := v.resolve(id, true) + constraint.enabled = enabled +} + +func (v SoloConstraintView) resolve(id SoloConstraintID, required bool) *soloConstraint { + if id.revision == 0 { + if required { + panic("invalid solo constraint ID") + } + return nil + } + constraint := &v.scene.soloConstraints[id.index] + if constraint.revision != id.revision { + if required { + panic("invalid solo constraint ID") + } + return nil + } + return constraint +} + +type SoloConstraintHandle struct { + view SoloConstraintView + id SoloConstraintID +} + +// TODO: Add methods to SoloConstraintHandle. + +type soloConstraint struct { + solver SoloConstraintSolver + revision int32 + bodyIndex int32 + enabled bool +} diff --git a/game/physics/scene.go b/game/physics/scene.go index 8f1ef3bd..c56b7820 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -14,59 +14,6 @@ import ( "github.com/mokiat/lacking/game/physics/solver" ) -func NewScene() *Scene { - return &Scene{ - shapeScene: placement3d.NewScene[bodyRef, struct{}, propRef](placement3d.SceneSettings{ - Size: opt.V(16384.0), - MaxDepth: opt.V[uint32](12), - InitialNodeCapacity: opt.V[uint32](1024), - InitialItemCapacity: opt.V[uint32](1024), - }), - - sbCollisionSubscriptions: NewSingleBodyCollisionSubscriptionSet(), - dbCollisionSubscriptions: NewDoubleBodyCollisionSubscriptionSet(), - - timeSpeed: 1.0, - - maxLinearAcceleration: 200.0, - maxAngularAcceleration: 200.0, - maxLinearVelocity: 2000.0, - maxAngularVelocity: 2000.0, - - mediumSolver: NewStaticAirSolver(), - - props: make([]propState, 0, 1024), - - freeBodyIndices: ds.PreallocatedStack[uint32](16), - bodies: make([]bodyState, 0, 64), - bodyAccelerationTargets: make([]AccelerationTarget, 0, 64), - bodyConstraintPlaceholders: make([]solver.Placeholder, 0, 64), - - globalAccelerators: make([]globalAccelerator, 1), - freeGlobalAcceleratorIndices: ds.EmptyStack[int32](), - - // bodyAccelerators []any // TOOD - freeBodyAcceleratorIndices: ds.PreallocatedStack[uint32](16), - - // areaAccelerators []any // TODO - freeAreaAcceleratorIndices: ds.PreallocatedStack[uint32](16), - - sbConstraints: make([]sbConstraintState, 0, 64), - dbConstraints: make([]dbConstraintState, 0, 64), - - freeSBConstraintIndices: ds.PreallocatedStack[uint32](16), - freeDBConstraintIndices: ds.PreallocatedStack[uint32](16), - - collisionSet: make(placement3d.ContactList, 0, 128), - - oldSBCollisions: make(map[sbCollisionPair]struct{}, 32), - newSBCollisions: make(map[sbCollisionPair]struct{}, 32), - - oldDBCollisions: make(map[dbCollisionPair]struct{}, 32), - newDBCollisions: make(map[dbCollisionPair]struct{}, 32), - } -} - // Scene represents a physics scene that contains // a number of bodies that are independent on any // bodies managed by other scene objects. @@ -83,8 +30,6 @@ type Scene struct { maxLinearVelocity float64 maxAngularVelocity float64 - mediumSolver MediumSolver - props []propState bodies []bodyState @@ -92,15 +37,6 @@ type Scene struct { bodyConstraintPlaceholders []solver.Placeholder freeBodyIndices *ds.Stack[uint32] - bodyAccelerators []bodyAccelerator - freeBodyAcceleratorIndices *ds.Stack[uint32] - - // areaAccelerators []any // TODO - freeAreaAcceleratorIndices *ds.Stack[uint32] - - globalAccelerators []globalAccelerator - freeGlobalAcceleratorIndices *ds.Stack[int32] - sbConstraints []sbConstraintState freeSBConstraintIndices *ds.Stack[uint32] @@ -123,6 +59,75 @@ type Scene struct { freeCollisionRejectGroup uint32 freeRevision uint32 + + // ---------- NEW BELOW---------- (TODO: REMOVE COMMENT) + mediumSolver MediumSolver + + freeGlobalAcceleratorIndices *ds.Stack[int32] + freeAreaAcceleratorIndices *ds.Stack[int32] + freeBodyAcceleratorIndices *ds.Stack[int32] + freeSoloConstraintIndices *ds.Stack[int32] + + globalAccelerators []globalAccelerator + bodyAccelerators []bodyAccelerator + soloConstraints []soloConstraint +} + +func NewScene() *Scene { + return &Scene{ + shapeScene: placement3d.NewScene[bodyRef, struct{}, propRef](placement3d.SceneSettings{ + Size: opt.V(16384.0), + MaxDepth: opt.V[uint32](12), + InitialNodeCapacity: opt.V[uint32](1024), + InitialItemCapacity: opt.V[uint32](1024), + }), + + sbCollisionSubscriptions: NewSingleBodyCollisionSubscriptionSet(), + dbCollisionSubscriptions: NewDoubleBodyCollisionSubscriptionSet(), + + timeSpeed: 1.0, + + maxLinearAcceleration: 200.0, + maxAngularAcceleration: 200.0, + maxLinearVelocity: 2000.0, + maxAngularVelocity: 2000.0, + + props: make([]propState, 0, 1024), + + freeBodyIndices: ds.PreallocatedStack[uint32](16), + bodies: make([]bodyState, 0, 64), + bodyAccelerationTargets: make([]AccelerationTarget, 0, 64), + bodyConstraintPlaceholders: make([]solver.Placeholder, 0, 64), + + // bodyAccelerators []any // TOOD + // areaAccelerators []any // TODO + + sbConstraints: make([]sbConstraintState, 0, 64), + dbConstraints: make([]dbConstraintState, 0, 64), + + freeSBConstraintIndices: ds.PreallocatedStack[uint32](16), + freeDBConstraintIndices: ds.PreallocatedStack[uint32](16), + + collisionSet: make(placement3d.ContactList, 0, 128), + + oldSBCollisions: make(map[sbCollisionPair]struct{}, 32), + newSBCollisions: make(map[sbCollisionPair]struct{}, 32), + + oldDBCollisions: make(map[dbCollisionPair]struct{}, 32), + newDBCollisions: make(map[dbCollisionPair]struct{}, 32), + + // ---------- NEW BELOW---------- (TODO: REMOVE COMMENT) + mediumSolver: NewStaticAirSolver(), + + freeGlobalAcceleratorIndices: ds.EmptyStack[int32](), + freeAreaAcceleratorIndices: ds.EmptyStack[int32](), + freeBodyAcceleratorIndices: ds.EmptyStack[int32](), + freeSoloConstraintIndices: ds.EmptyStack[int32](), + + globalAccelerators: make([]globalAccelerator, 0), + bodyAccelerators: make([]bodyAccelerator, 0), + soloConstraints: make([]soloConstraint, 0), + } } // Delete releases resources allocated by this scene. Users should not call @@ -950,12 +955,22 @@ func (s *Scene) GlobalAccelerators() GlobalAcceleratorView { } } +// func (s *Scene) AreaAccelerators() AreaAcceleratorView { +// panic("TODO") +// } + func (s *Scene) BodyAccelerators() BodyAcceleratorView { return BodyAcceleratorView{ scene: s, } } +func (s *Scene) SoloConstraints() SoloConstraintView { + return SoloConstraintView{ + scene: s, + } +} + func (s *Scene) allocateGlobalAccelerator() int32 { if !s.freeGlobalAcceleratorIndices.IsEmpty() { return s.freeGlobalAcceleratorIndices.Pop() @@ -994,6 +1009,19 @@ func (s *Scene) detachBodyAccelerator(bodyIndex, index int32) { panic("TODO") } +func (s *Scene) allocateSoloConstraint() int32 { + if !s.freeSoloConstraintIndices.IsEmpty() { + return s.freeSoloConstraintIndices.Pop() + } + index := int32(len(s.soloConstraints)) + s.soloConstraints = append(s.soloConstraints, soloConstraint{}) + return index +} + +func (s *Scene) releaseSoloConstraint(index int32) { + s.freeSoloConstraintIndices.Push(index) +} + type bodyRef struct { index uint32 } From db366e55e5013b16f8ce5e3903007d1e306bb504 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sat, 1 Aug 2026 23:55:43 +0300 Subject: [PATCH 07/85] WIP: Starting total physics API rework --- game/physics/accelerator_body.go | 5 +- game/physics/body.go | 594 ++++++++++++------------------- game/physics/constraint_solo.go | 7 +- game/physics/material.go | 44 --- game/physics/scene.go | 152 ++++---- 5 files changed, 319 insertions(+), 483 deletions(-) delete mode 100644 game/physics/material.go diff --git a/game/physics/accelerator_body.go b/game/physics/accelerator_body.go index 65363d37..9aa83d75 100644 --- a/game/physics/accelerator_body.go +++ b/game/physics/accelerator_body.go @@ -31,7 +31,6 @@ func (v BodyAcceleratorView) Create(bodyID BodyID, solver AccelerationSolver) Bo } func (v BodyAcceleratorView) Delete(id BodyAcceleratorID) { - // TODO: Should I allow the deletion of an invalid ID (noop)? accelerator := v.resolve(id, true) accelerator.solver = nil // allow the solver to be garbage collected accelerator.revision++ // progress revision to invalid (even) value @@ -55,10 +54,10 @@ func (v BodyAcceleratorView) IsValid(id BodyAcceleratorID) bool { func (v BodyAcceleratorView) BodyID(id BodyAcceleratorID) BodyID { accelerator := v.resolve(id, true) bodyIndex := accelerator.bodyIndex - bodyRevision := v.scene.bodies[bodyIndex].reference.Revision + body := &v.scene.bodies[bodyIndex] return BodyID{ index: bodyIndex, - revision: int32(bodyRevision), + revision: body.revision, } } diff --git a/game/physics/body.go b/game/physics/body.go index 3d79b9b5..a3237f4d 100644 --- a/game/physics/body.go +++ b/game/physics/body.go @@ -5,355 +5,317 @@ import ( "github.com/mokiat/gomath/dprec" "github.com/mokiat/lacking/core/spatial/placement3d" "github.com/mokiat/lacking/core/spatial/shape3d" - "github.com/mokiat/lacking/game/physics/solver" ) -var NilBodyID = BodyID{} - type BodyID struct { index int32 revision int32 } -var invalidBodyState = &bodyState{} +var NilBodyID = BodyID{} -type BodyDefinitionInfo struct { - Mass float64 - MomentOfInertia dprec.Mat3 - FrictionCoefficient float64 - RestitutionCoefficient float64 - DragFactor float64 - AngularDragFactor float64 - CollisionRejectGroup uint32 - CollisionSpheres []shape3d.Sphere - CollisionBoxes []shape3d.Box - CollisionMeshes []shape3d.Mesh - AerodynamicShapes []AerodynamicShape -} - -type BodyDefinition struct { - mass float64 - momentOfInertia dprec.Mat3 - frictionCoefficient float64 - restitutionCoefficient float64 - dragFactor float64 - angularDragFactor float64 - collisionRejectGroup uint32 - collisionSpheres []shape3d.Sphere - collisionBoxes []shape3d.Box - collisionMeshes []shape3d.Mesh - aerodynamicShapes []AerodynamicShape -} - -// NewBodyDefinition creates a new BodyDefinition that can be used -// to create Body instances. -func NewBodyDefinition(info BodyDefinitionInfo) *BodyDefinition { - return &BodyDefinition{ - mass: info.Mass, - momentOfInertia: info.MomentOfInertia, - frictionCoefficient: info.FrictionCoefficient, - restitutionCoefficient: info.RestitutionCoefficient, - dragFactor: info.DragFactor, - angularDragFactor: info.AngularDragFactor, - collisionRejectGroup: info.CollisionRejectGroup, - collisionSpheres: info.CollisionSpheres, - collisionBoxes: info.CollisionBoxes, - collisionMeshes: info.CollisionMeshes, - aerodynamicShapes: info.AerodynamicShapes, +type BodyView struct { + scene *Scene +} + +func (v BodyView) Create(position dprec.Vec3, rotation dprec.Quat) BodyID { + index := v.scene.allocateBody() + + objectID := v.scene.collisionScene.CreateObject(placement3d.ObjectInfo[bodyData]{ + Position: opt.V(position), + Rotation: opt.V(rotation), + UserData: bodyData{ + index: index, + }, + }) + + body := &v.scene.bodies[index] + body.objectID = objectID + body.revision++ // progress revision to valid (odd) value + body.invMass = 1.0 + body.invInertia = dprec.IdentityMat3() + body.linearVelocity = dprec.ZeroVec3() + body.angularVelocity = dprec.ZeroVec3() + body.position = position + body.rotation = rotation + + return BodyID{ + index: index, + revision: body.revision, } } -func (d *BodyDefinition) CollisionSpheres() []shape3d.Sphere { - return d.collisionSpheres +func (v BodyView) Delete(id BodyID) { + body := v.resolve(id, true) + v.scene.collisionScene.DeleteObject(body.objectID) + body.objectID = placement3d.InvalidObjectID + // TODO: delete all accelerators and constraints that reference this body + body.revision++ // progress revision to invalid (even) value + v.scene.releaseBody(id.index) } -func (d *BodyDefinition) CollisionBoxes() []shape3d.Box { - return d.collisionBoxes +func (v BodyView) Handle(id BodyID) BodyHandle { + return BodyHandle{ + view: v, + id: id, + } } -func (d *BodyDefinition) CollisionMeshes() []shape3d.Mesh { - return d.collisionMeshes +func (v BodyView) IsValid(id BodyID) bool { + body := v.resolve(id, false) + return body != nil } -type BodyInfo struct { - Name string - Definition *BodyDefinition - Position dprec.Vec3 - Rotation dprec.Quat +func (v BodyView) Mass(id BodyID) float64 { + body := v.resolve(id, true) + return 1.0 / body.invMass } -// Body represents a physical body that has physics -// act upon it. -type Body struct { - scene *Scene - reference indexReference +func (v BodyView) SetMass(id BodyID, mass float64) { + body := v.resolve(id, true) + body.invMass = 1.0 / mass } -func (b Body) ID() BodyID { - return BodyID{ - index: int32(b.reference.Index), - revision: int32(b.reference.Revision), - } +func (v BodyView) MomentOfInertia(id BodyID) dprec.Mat3 { + body := v.resolve(id, true) + return dprec.InverseMat3(body.invInertia) } -// Name returns the name of this body. -func (b Body) Name() string { - state := b.state() - return state.name +func (v BodyView) SetMomentOfInertia(id BodyID, inertia dprec.Mat3) { + body := v.resolve(id, true) + body.invInertia = dprec.InverseMat3(inertia) } -// SetName sets a new name for this body. -func (b Body) SetName(name string) { - state := b.state() - state.name = name +func (v BodyView) Velocity(id BodyID) dprec.Vec3 { + body := v.resolve(id, true) + return body.linearVelocity } -// Mass returns the mass of this body in kg. -func (b Body) Mass() float64 { - state := b.state() - return state.mass +func (v BodyView) SetVelocity(id BodyID, velocity dprec.Vec3) { + body := v.resolve(id, true) + body.linearVelocity = velocity } -// SetMass changes the mass of this body. -func (b Body) SetMass(mass float64) { - state := b.state() - state.mass = mass +func (v BodyView) AngularVelocity(id BodyID) dprec.Vec3 { + body := v.resolve(id, true) + return body.angularVelocity } -// MomentOfInertia returns the moment of inertia, or -// rotational inertia of this body. -func (b Body) MomentOfInertia() dprec.Mat3 { - state := b.state() - return state.momentOfInertia +func (v BodyView) SetAngularVelocity(id BodyID, angularVelocity dprec.Vec3) { + body := v.resolve(id, true) + body.angularVelocity = angularVelocity } -// SetMomentOfInertia changes the moment of inertia -// of this body. -func (b Body) SetMomentOfInertia(inertia dprec.Mat3) { - state := b.state() - state.momentOfInertia = inertia +func (v BodyView) Position(id BodyID) dprec.Vec3 { + body := v.resolve(id, true) + return body.position } -// // RestitutionCoefficient returns the restitution -// // coefficient of this body. Valid values are in -// // the range [0.0 - 1.0], where 0.0 means that the -// // body does not bounce and 1.0 means that it bounds -// // back with the same velocity. In reality the amount -// // that the body will bounce depends on the restitution -// // coefficients of both bodies colliding. Furthermore, -// // due to computational errors, the bounce will eventually -// // stop. -// func (b *Body) RestitutionCoefficient() float64 { -// return b.restitutionCoefficient -// } +func (v BodyView) SetPosition(id BodyID, position dprec.Vec3) { + body := v.resolve(id, true) + body.position = position + v.refreshPlacement(id, body) +} -// // SetRestitutionCoefficient changes the restitution -// // coefficient for this body. -// func (b *Body) SetRestitutionCoefficient(coefficient float64) { -// b.restitutionCoefficient = coefficient -// } +func (v BodyView) Rotation(id BodyID) dprec.Quat { + body := v.resolve(id, true) + return body.rotation +} -// // DragCoefficient returns the drag factor of this body. -// func (b *Body) DragFactor() float64 { -// return b.dragFactor -// } +func (v BodyView) SetRotation(id BodyID, rotation dprec.Quat) { + body := v.resolve(id, true) + body.rotation = rotation + v.refreshPlacement(id, body) +} -// // SetDragFactor sets the drag factor for this body. -// // The drag factor is the drag coefficient multiplied -// // by the area and divided in half. -// func (b *Body) SetDragFactor(factor float64) { -// b.dragFactor = factor -// } +func (v BodyView) AttachCollisionSphere(id BodyID, col CollisionSphere) CollisionShapeID { + body := v.resolve(id, true) + shapeID := v.scene.collisionScene.AttachSphere(body.objectID, placement3d.SphereInfo[shapeData]{ + Sphere: col.Shape, + Filtering: col.Filtering, + UserData: shapeData{ + frictionCoefficient: col.FrictionCoefficient, + restitutionCoefficient: col.RestitutionCoefficient, + }, + }) + return CollisionShapeID{ + bodyID: id, + shapeID: shapeID, + } +} -// // AngularDragFactor returns the angular drag factor -// // for this body. -// func (b *Body) AngularDragFactor() float64 { -// return b.angularDragFactor -// } +func (v BodyView) AttachCollisionBox(id BodyID, col CollisionBox) CollisionShapeID { + body := v.resolve(id, true) + shapeID := v.scene.collisionScene.AttachBox(body.objectID, placement3d.BoxInfo[shapeData]{ + Box: col.Shape, + Filtering: col.Filtering, + UserData: shapeData{ + frictionCoefficient: col.FrictionCoefficient, + restitutionCoefficient: col.RestitutionCoefficient, + }, + }) + return CollisionShapeID{ + bodyID: id, + shapeID: shapeID, + } +} -// // SetAngularDragFactor sets the angular factor for this body. -// // The angular factor is similar to the drag factor, except -// // that it deals with the drag induced by the rotation of -// // the body. -// func (b *Body) SetAngularDragFactor(factor float64) { -// b.angularDragFactor = factor -// } +func (v BodyView) DetachCollisionShape(id BodyID, shapeID CollisionShapeID) { + if id != shapeID.bodyID { + panic("invalid shape ID for body") + } + v.scene.collisionScene.DeleteShape(shapeID.shapeID) +} -// Position returns the body's position in world space. -func (b Body) Position() dprec.Vec3 { - state := b.state() - return state.position +func (v BodyView) refreshPlacement(id BodyID, body *bodyState) { + v.scene.collisionScene.SetObjectTransform(body.objectID, shape3d.Transform{ + Translation: body.position, + Rotation: shape3d.RotationFromQuat(body.rotation), + }) } -// SetPosition changes the position of this body. -func (b Body) SetPosition(position dprec.Vec3) { - state := b.state() - state.position = position +func (v BodyView) resolve(id BodyID, required bool) *bodyState { + if id.revision == 0 { + if required { + panic("invalid global accelerator ID") + } + return nil + } + body := &v.scene.bodies[id.index] + if body.revision != id.revision { + if required { + panic("invalid global accelerator ID") + } + return nil + } + return body +} - // FIXME: Invalidate shape placement! +type BodyHandle struct { + view BodyView + id BodyID } -// Rotation returns the quaternion rotation of this body. -func (b Body) Rotation() dprec.Quat { - state := b.state() - return state.rotation -} - -// SetRotation changes the quaterntion rotation of this body. -func (b Body) SetRotation(rotation dprec.Quat) { - state := b.state() - state.rotation = rotation - - // FIXME: Invalidate shape placement! -} - -// Velocity returns the velocity of this body. -func (b Body) Velocity() dprec.Vec3 { - state := b.state() - return state.velocity -} - -// SetVelocity changes the velocity of this body. -func (b Body) SetVelocity(velocity dprec.Vec3) { - state := b.state() - state.velocity = velocity -} - -// AngularVelocity returns the angular velocity -// of this body. -func (b Body) AngularVelocity() dprec.Vec3 { - state := b.state() - return state.angularVelocity -} - -// SetAngularVelocity changes the angular velocity -// of this body. -func (b Body) SetAngularVelocity(angularVelocity dprec.Vec3) { - state := b.state() - state.angularVelocity = angularVelocity -} - -// // CollisionGroup returns the collision group for this body. Two bodies -// // with the same collision group are not checked for collisions. -// func (b Body) CollisionGroup() int { -// state := b.state() -// return state.collisionGroup -// } - -// // SetCollisionGroup changes the collision group for this body. -// // -// // A value of 0 disables the collision group. -// func (b Body) SetCollisionGroup(group int) { -// state := b.state() -// state.collisionGroup = group -// } - -// // CollisionSet contains the collision shapes for this body. -// func (b Body) CollisionSet() collision.Set { -// state := b.state() -// return state.collisionSet -// } - -// // AerodynamicShapes returns a slice of shapes that -// // dictate how this body is affected by relative air -// // motion. -// func (b *Body) AerodynamicShapes() []AerodynamicShape { -// return b.aerodynamicShapes -// } - -// // SetAerodynamicShapes sets the aerodynamics shapes -// // to be used when calculating wind drag and lift. -// func (b *Body) SetAerodynamicShapes(shapes []AerodynamicShape) { -// b.aerodynamicShapes = shapes -// } +func (h BodyHandle) ID() BodyID { + return h.id +} -// Delete removes this physical body. -func (b Body) Delete() { - deleteBody(b.scene, b.reference) +func (h BodyHandle) Delete() { + h.view.Delete(h.id) } -func (b Body) state() *bodyState { - index := b.reference.Index - state := &b.scene.bodies[index] - if state.reference != b.reference { - return invalidBodyState - } - return state +func (h BodyHandle) IsValid() bool { + return h.view.IsValid(h.id) } -// func (b *Body) applyOffsetForce(offset, force dprec.Vec3) { -// b.applyForce(force) -// b.applyTorque(dprec.Vec3Cross(offset, force)) -// } +func (h BodyHandle) Mass() float64 { + return h.view.Mass(h.id) +} -// func (b *Body) applyImpulse(impulse dprec.Vec3) { -// b.addVelocity(dprec.Vec3Quot(impulse, b.mass)) -// } +func (h BodyHandle) SetMass(mass float64) { + h.view.SetMass(h.id, mass) +} -// func (b *Body) applyAngularImpulse(impulse dprec.Vec3) { -// // FIXME: the moment of intertia is in local space, whereas the impulse is in world space -// b.addAngularVelocity(dprec.Mat3Vec3Prod(dprec.InverseMat3(b.momentOfInertia), impulse)) -// } +func (h BodyHandle) MomentOfInertia() dprec.Mat3 { + return h.view.MomentOfInertia(h.id) +} -// func (b *Body) applyOffsetImpulse(offset, impulse dprec.Vec3) { -// b.applyImpulse(impulse) -// b.applyAngularImpulse(dprec.Vec3Cross(offset, impulse)) -// } +func (h BodyHandle) SetMomentOfInertia(inertia dprec.Mat3) { + h.view.SetMomentOfInertia(h.id, inertia) +} -// func (b *Body) applyNudge(nudge dprec.Vec3) { -// b.translate(dprec.Vec3Quot(nudge, b.mass)) -// } +func (h BodyHandle) Velocity() dprec.Vec3 { + return h.view.Velocity(h.id) +} -// func (b *Body) applyAngularNudge(nudge dprec.Vec3) { -// // FIXME: the moment of intertia is in local space, whereas the torque is in world space -// b.vectorRotate(dprec.Mat3Vec3Prod(dprec.InverseMat3(b.momentOfInertia), nudge)) -// } +func (h BodyHandle) SetVelocity(velocity dprec.Vec3) { + h.view.SetVelocity(h.id, velocity) +} -// func (b *Body) applyOffsetNudge(offset, nudge dprec.Vec3) { -// b.applyNudge(nudge) -// b.applyAngularNudge(dprec.Vec3Cross(offset, nudge)) -// } +func (h BodyHandle) AngularVelocity() dprec.Vec3 { + return h.view.AngularVelocity(h.id) +} -type bodyState struct { - reference indexReference +func (h BodyHandle) SetAngularVelocity(angularVelocity dprec.Vec3) { + h.view.SetAngularVelocity(h.id, angularVelocity) +} - firstBodyAcceleratorIndex int32 +func (h BodyHandle) Position() dprec.Vec3 { + return h.view.Position(h.id) +} - objectID placement3d.ObjectID +func (h BodyHandle) SetPosition(position dprec.Vec3) { + h.view.SetPosition(h.id, position) +} + +func (h BodyHandle) Rotation() dprec.Quat { + return h.view.Rotation(h.id) +} + +func (h BodyHandle) SetRotation(rotation dprec.Quat) { + h.view.SetRotation(h.id, rotation) +} + +func (h BodyHandle) AttachCollisionSphere(shape CollisionSphere) CollisionShapeID { + return h.view.AttachCollisionSphere(h.id, shape) +} + +func (h BodyHandle) AttachCollisionBox(shape CollisionBox) CollisionShapeID { + return h.view.AttachCollisionBox(h.id, shape) +} + +func (h BodyHandle) DetachCollisionShape(shapeID CollisionShapeID) { + h.view.DetachCollisionShape(h.id, shapeID) +} + +type CollisionShapeID struct { + bodyID BodyID + shapeID placement3d.ShapeID +} + +type CollisionShape[T any] struct { + Shape T + FrictionCoefficient float64 + RestitutionCoefficient float64 + Filtering placement3d.FilterInfo +} - name string - definition *BodyDefinition +type CollisionSphere CollisionShape[shape3d.Sphere] - mass float64 - momentOfInertia dprec.Mat3 +type CollisionBox CollisionShape[shape3d.Box] - // TODO: Move friction and restitution to the collision Set through - // a material. +type bodyData struct { + index int32 +} +type shapeData struct { frictionCoefficient float64 restitutionCoefficient float64 +} - // TODO: dragFactor and angularDragFactor should be moved to the - // aerodynamic shapes. +type bodyState struct { + objectID placement3d.ObjectID - dragFactor float64 - angularDragFactor float64 + revision int32 + firstBodyAcceleratorIndex int32 + firstSoloConstraintIndex int32 - position dprec.Vec3 - rotation dprec.Quat + invMass float64 + invInertia dprec.Mat3 - velocity dprec.Vec3 + linearVelocity dprec.Vec3 angularVelocity dprec.Vec3 - aerodynamicShapes []AerodynamicShape + position dprec.Vec3 + rotation dprec.Quat } func (s bodyState) IsActive() bool { - return s.reference.IsValid() + return s.revision%2 == 1 // only odd revisions are valid } func (b *bodyState) AddVelocity(amount dprec.Vec3) { - b.velocity = dprec.Vec3Sum(b.velocity, amount) + b.linearVelocity = dprec.Vec3Sum(b.linearVelocity, amount) } func (b *bodyState) AddAngularVelocity(amount dprec.Vec3) { @@ -361,8 +323,8 @@ func (b *bodyState) AddAngularVelocity(amount dprec.Vec3) { } func (b *bodyState) ClampVelocity(max float64) { - if b.velocity.SqrLength() > max*max { - b.velocity = dprec.ResizedVec3(b.velocity, max) + if b.linearVelocity.SqrLength() > max*max { + b.linearVelocity = dprec.ResizedVec3(b.linearVelocity, max) } } @@ -386,89 +348,3 @@ func (b *bodyState) VectorRotate(vector dprec.Vec3) { func (b *bodyState) Rotate(quat dprec.Quat) { b.rotation = dprec.UnitQuat(dprec.QuatProd(quat, b.rotation)) } - -func createBody(scene *Scene, info BodyInfo) Body { - var freeIndex uint32 - if scene.freeBodyIndices.IsEmpty() { - freeIndex = uint32(len(scene.bodies)) - scene.bodies = append(scene.bodies, bodyState{}) - scene.bodyAccelerationTargets = append(scene.bodyAccelerationTargets, AccelerationTarget{}) - scene.bodyConstraintPlaceholders = append(scene.bodyConstraintPlaceholders, solver.Placeholder{}) - } else { - freeIndex = scene.freeBodyIndices.Pop() - } - - objectID := scene.shapeScene.CreateObject(placement3d.ObjectInfo[bodyRef]{ - Position: opt.V(info.Position), - Rotation: opt.V(info.Rotation), - UserData: bodyRef{ - index: freeIndex, - }, - }) - for _, sphere := range info.Definition.collisionSpheres { - scene.shapeScene.AttachSphere(objectID, placement3d.SphereInfo[struct{}]{ - Filtering: placement3d.FilterInfo{ - RejectGroup: info.Definition.collisionRejectGroup, - }, - Sphere: sphere, - }) - } - for _, box := range info.Definition.collisionBoxes { - scene.shapeScene.AttachBox(objectID, placement3d.BoxInfo[struct{}]{ - Filtering: placement3d.FilterInfo{ - RejectGroup: info.Definition.collisionRejectGroup, - }, - Box: box, - }) - } - // for _, mesh := range info.Definition.collisionMeshes { - // scene.shapeScene.CreateMesh(placement3d.MeshInfo[struct{}]{ - // ShapeInfo: placement3d.ShapeInfo[struct{}]{ - // RejectGroup: uint32(info.Definition.collisionGroup), - // }, - // Mesh: mesh, - // }) - // } - - reference := newIndexReference(freeIndex, scene.nextRevision()) - body := bodyState{ - reference: reference, - - objectID: objectID, - - name: info.Name, - definition: info.Definition, - - mass: info.Definition.mass, - momentOfInertia: info.Definition.momentOfInertia, - - frictionCoefficient: info.Definition.frictionCoefficient, - restitutionCoefficient: info.Definition.restitutionCoefficient, - - dragFactor: info.Definition.dragFactor, - angularDragFactor: info.Definition.angularDragFactor, - - position: info.Position, - rotation: info.Rotation, - - aerodynamicShapes: info.Definition.aerodynamicShapes, - } - scene.bodies[freeIndex] = body - - return Body{ - scene: scene, - reference: reference, - } -} - -func deleteBody(scene *Scene, reference indexReference) { - index := reference.Index - state := &scene.bodies[index] - if state.reference == reference { - scene.shapeScene.DeleteObject(state.objectID) - state.reference = newIndexReference(index, 0) - state.definition = nil - state.aerodynamicShapes = nil - scene.freeBodyIndices.Push(index) - } -} diff --git a/game/physics/constraint_solo.go b/game/physics/constraint_solo.go index aae8af62..d417e198 100644 --- a/game/physics/constraint_solo.go +++ b/game/physics/constraint_solo.go @@ -79,10 +79,11 @@ func (v SoloConstraintView) IsValid(id SoloConstraintID) bool { func (v SoloConstraintView) BodyID(id SoloConstraintID) BodyID { constraint := v.resolve(id, true) - body := &v.scene.bodies[constraint.bodyIndex] + bodyIndex := constraint.bodyIndex + body := &v.scene.bodies[bodyIndex] return BodyID{ - index: constraint.bodyIndex, - revision: int32(body.reference.Revision), + index: bodyIndex, + revision: body.revision, } } diff --git a/game/physics/material.go b/game/physics/material.go deleted file mode 100644 index a3011232..00000000 --- a/game/physics/material.go +++ /dev/null @@ -1,44 +0,0 @@ -package physics - -// MaterialInfo contains the data necessary to create a Material. -type MaterialInfo struct { - FrictionCoefficient float64 - RestitutionCoefficient float64 -} - -// Material represents the surface properties of an object. -type Material struct { - frictionCoefficient float64 - restitutionCoefficient float64 -} - -// NewMaterial creates a new Material that can be used to describe an -// object's behavior. -func NewMaterial(info MaterialInfo) *Material { - return &Material{ - frictionCoefficient: info.FrictionCoefficient, - restitutionCoefficient: info.RestitutionCoefficient, - } -} - -// FrictionCoefficient returns the friction coefficient of this material. -func (m *Material) FrictionCoefficient() float64 { - return m.frictionCoefficient -} - -// SetFrictionCoefficient changes the friction coefficient of this material. -func (m *Material) SetFrictionCoefficient(coefficient float64) { - m.frictionCoefficient = coefficient -} - -// RestitutionCoefficient returns the coefficient of restitution of -// this material. -func (m *Material) RestitutionCoefficient() float64 { - return m.restitutionCoefficient -} - -// SetRestitutionCoefficient changes the coefficient of restitution of this -// material. -func (m *Material) SetRestitutionCoefficient(coefficient float64) { - m.restitutionCoefficient = coefficient -} diff --git a/game/physics/scene.go b/game/physics/scene.go index c56b7820..86bdf5bf 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -18,7 +18,7 @@ import ( // a number of bodies that are independent on any // bodies managed by other scene objects. type Scene struct { - shapeScene *placement3d.Scene[bodyRef, struct{}, propRef] + collisionScene *placement3d.Scene[bodyData, shapeData, propRef] sbCollisionSubscriptions *SingleBodyCollisionSubscriptionSet dbCollisionSubscriptions *DoubleBodyCollisionSubscriptionSet @@ -35,7 +35,6 @@ type Scene struct { bodies []bodyState bodyAccelerationTargets []AccelerationTarget bodyConstraintPlaceholders []solver.Placeholder - freeBodyIndices *ds.Stack[uint32] sbConstraints []sbConstraintState freeSBConstraintIndices *ds.Stack[uint32] @@ -58,7 +57,6 @@ type Scene struct { newDBCollisions map[dbCollisionPair]struct{} freeCollisionRejectGroup uint32 - freeRevision uint32 // ---------- NEW BELOW---------- (TODO: REMOVE COMMENT) mediumSolver MediumSolver @@ -67,6 +65,7 @@ type Scene struct { freeAreaAcceleratorIndices *ds.Stack[int32] freeBodyAcceleratorIndices *ds.Stack[int32] freeSoloConstraintIndices *ds.Stack[int32] + freeBodyIndices *ds.Stack[int32] globalAccelerators []globalAccelerator bodyAccelerators []bodyAccelerator @@ -75,7 +74,7 @@ type Scene struct { func NewScene() *Scene { return &Scene{ - shapeScene: placement3d.NewScene[bodyRef, struct{}, propRef](placement3d.SceneSettings{ + collisionScene: placement3d.NewScene[bodyData, shapeData, propRef](placement3d.SceneSettings{ Size: opt.V(16384.0), MaxDepth: opt.V[uint32](12), InitialNodeCapacity: opt.V[uint32](1024), @@ -94,7 +93,6 @@ func NewScene() *Scene { props: make([]propState, 0, 1024), - freeBodyIndices: ds.PreallocatedStack[uint32](16), bodies: make([]bodyState, 0, 64), bodyAccelerationTargets: make([]AccelerationTarget, 0, 64), bodyConstraintPlaceholders: make([]solver.Placeholder, 0, 64), @@ -123,6 +121,7 @@ func NewScene() *Scene { freeAreaAcceleratorIndices: ds.EmptyStack[int32](), freeBodyAcceleratorIndices: ds.EmptyStack[int32](), freeSoloConstraintIndices: ds.EmptyStack[int32](), + freeBodyIndices: ds.EmptyStack[int32](), globalAccelerators: make([]globalAccelerator, 0), bodyAccelerators: make([]bodyAccelerator, 0), @@ -279,7 +278,7 @@ func (s *Scene) CreateProp(info PropInfo) { for _, mesh := range info.CollisionMeshes { propIndex := uint32(len(s.props)) - meshID := s.shapeScene.CreateMesh(placement3d.MeshInfo[propRef]{ + meshID := s.collisionScene.CreateMesh(placement3d.MeshInfo[propRef]{ Position: info.Position, Rotation: info.Rotation, Mesh: mesh, @@ -296,12 +295,6 @@ func (s *Scene) CreateProp(info PropInfo) { } } -// CreateBody creates a new physics body and places -// it within this scene. -func (s *Scene) CreateBody(info BodyInfo) Body { - return createBody(s, info) -} - // CreateSingleBodyConstraint creates a new physics constraint that acts on // a single body and enables it for this scene. func (s *Scene) CreateSingleBodyConstraint(body Body, logic solver.Constraint) SBConstraint { @@ -332,22 +325,23 @@ func (s *Scene) Each(cb func(b Body)) { }) } -func (s *Scene) CheckSegmentIntersection(segment shape3d.Segment, mask uint32) (Body, bool) { - intersection, ok := s.shapeScene.CheckSegmentIntersection(segment, placement3d.Filter{ +func (s *Scene) CheckSegmentIntersection(segment shape3d.Segment, mask uint32) (BodyID, bool) { + intersection, ok := s.collisionScene.CheckSegmentIntersection(segment, placement3d.Filter{ Mask: opt.V(mask), }) if !ok { - return Body{}, false + return NilBodyID, false } if intersection.TargetShapeID == placement3d.InvalidShapeID { // A prop. - return Body{}, false // FIXME: This should handle props as well. + return NilBodyID, false // FIXME: This should handle props as well. } - objectID := s.shapeScene.GetShapeObject(intersection.TargetShapeID) - ref := s.shapeScene.GetObjectUserData(objectID) - return Body{ - scene: s, - reference: s.bodies[ref.index].reference, + objectID := s.collisionScene.GetShapeObject(intersection.TargetShapeID) + bData := s.collisionScene.GetObjectUserData(objectID) + body := &s.bodies[bData.index] + return BodyID{ + index: bData.index, + revision: body.revision, }, true } @@ -389,20 +383,18 @@ func (s *Scene) applyAcceleration(elapsedSeconds float64) { s.applyBodyAccelerators() s.applyAreaAccelerators() s.applyGlobalAccelerators() - s.applyAerodynamicAccelerations() + // s.applyAerodynamicAccelerations() s.applyAccelerationTargets(elapsedSeconds) } func (s *Scene) prepareAccelerationTargets() { s.eachBodyState(func(index int, body *bodyState) { s.bodyAccelerationTargets[index] = newAccelerationTarget( - 1.0/body.mass, - dprec.InverseMat3( - RotatedMomentOfInertia(body.momentOfInertia, body.rotation), - ), + body.invMass, + RotatedMomentOfInertia(body.invInertia, body.rotation), body.position, body.rotation, - body.velocity, + body.linearVelocity, body.angularVelocity, ) }) @@ -434,39 +426,39 @@ func (s *Scene) applyGlobalAccelerators() { }) } -func (s *Scene) applyAerodynamicAccelerations() { - s.eachBodyState(func(index int, body *bodyState) { - if len(body.aerodynamicShapes) == 0 { - return - } - target := &s.bodyAccelerationTargets[index] - mediumDensity := s.mediumSolver.Density(body.position) - mediumVelocity := s.mediumSolver.Velocity(body.position) +// func (s *Scene) applyAerodynamicAccelerations() { +// s.eachBodyState(func(index int, body *bodyState) { +// if len(body.aerodynamicShapes) == 0 { +// return +// } +// target := &s.bodyAccelerationTargets[index] +// mediumDensity := s.mediumSolver.Density(body.position) +// mediumVelocity := s.mediumSolver.Velocity(body.position) - deltaVelocity := dprec.Vec3Diff(mediumVelocity, body.velocity) - dragForce := dprec.Vec3Prod(deltaVelocity, deltaVelocity.Length()*mediumDensity*body.dragFactor) - target.ApplyForce(dragForce) +// deltaVelocity := dprec.Vec3Diff(mediumVelocity, body.velocity) +// dragForce := dprec.Vec3Prod(deltaVelocity, deltaVelocity.Length()*mediumDensity*body.dragFactor) +// target.ApplyForce(dragForce) - angularDragForce := dprec.Vec3Prod(body.angularVelocity, -body.angularVelocity.Length()*mediumDensity*body.angularDragFactor) - target.ApplyTorque(angularDragForce) +// angularDragForce := dprec.Vec3Prod(body.angularVelocity, -body.angularVelocity.Length()*mediumDensity*body.angularDragFactor) +// target.ApplyTorque(angularDragForce) - bodyTransform := NewTransform(body.position, body.rotation) - for _, aerodynamicShape := range body.aerodynamicShapes { - // TODO: Take shape velocity into account. This also means that wings should be - // split into two, to benefit from that. +// bodyTransform := NewTransform(body.position, body.rotation) +// for _, aerodynamicShape := range body.aerodynamicShapes { +// // TODO: Take shape velocity into account. This also means that wings should be +// // split into two, to benefit from that. - aerodynamicShape = aerodynamicShape.Transformed(bodyTransform) - relativeSpeed := dprec.QuatVec3Rotation(dprec.InverseQuat(aerodynamicShape.Rotation()), deltaVelocity) +// aerodynamicShape = aerodynamicShape.Transformed(bodyTransform) +// relativeSpeed := dprec.QuatVec3Rotation(dprec.InverseQuat(aerodynamicShape.Rotation()), deltaVelocity) - force := aerodynamicShape.solver.Force(relativeSpeed, mediumDensity) - absoluteForce := dprec.QuatVec3Rotation(aerodynamicShape.Rotation(), force) +// force := aerodynamicShape.solver.Force(relativeSpeed, mediumDensity) +// absoluteForce := dprec.QuatVec3Rotation(aerodynamicShape.Rotation(), force) - offset := dprec.Vec3Diff(aerodynamicShape.Position(), bodyTransform.Position()) - target.ApplyOffsetForce(offset, absoluteForce) - // target.ApplyOffsetForce(absoluteForce, aerodynamicShape.Position()) - } - }) -} +// offset := dprec.Vec3Diff(aerodynamicShape.Position(), bodyTransform.Position()) +// target.ApplyOffsetForce(offset, absoluteForce) +// // target.ApplyOffsetForce(absoluteForce, aerodynamicShape.Position()) +// } +// }) +// } func (s *Scene) applyAccelerationTargets(elapsedSeconds float64) { s.eachBodyState(func(index int, body *bodyState) { @@ -567,12 +559,12 @@ func (s *Scene) applyMotion(elapsedSeconds float64) { body.ClampVelocity(s.maxLinearVelocity) body.ClampAngularVelocity(s.maxAngularVelocity) - deltaPosition := dprec.Vec3Prod(body.velocity, elapsedSeconds) + deltaPosition := dprec.Vec3Prod(body.linearVelocity, elapsedSeconds) body.Translate(deltaPosition) deltaRotation := dprec.Vec3Prod(body.angularVelocity, elapsedSeconds) body.VectorRotate(deltaRotation) - s.shapeScene.SetObjectTransform(body.objectID, shape3d.Transform{ + s.collisionScene.SetObjectTransform(body.objectID, shape3d.Transform{ Translation: body.position, Rotation: shape3d.RotationFromQuat(body.rotation), }) @@ -649,23 +641,23 @@ func (s *Scene) detectCollisions() { s.dbCollisionSolvers = s.dbCollisionSolvers[:0] s.collisionSet.Reset() - s.shapeScene.CollectIntersections(s.collisionSet.AddContact) + s.collisionScene.CollectIntersections(s.collisionSet.AddContact) for _, intersection := range s.collisionSet.Contacts() { - srcBodyObject := s.shapeScene.GetShapeObject(intersection.SourceShapeID) - srcBodyRef := s.shapeScene.GetObjectUserData(srcBodyObject) + srcBodyObject := s.collisionScene.GetShapeObject(intersection.SourceShapeID) + srcBodyRef := s.collisionScene.GetObjectUserData(srcBodyObject) if intersection.TargetMeshID == placement3d.InvalidMeshID { - tgtBodyObject := s.shapeScene.GetShapeObject(intersection.TargetShapeID) - tgtBodyRef := s.shapeScene.GetObjectUserData(tgtBodyObject) + tgtBodyObject := s.collisionScene.GetShapeObject(intersection.TargetShapeID) + tgtBodyRef := s.collisionScene.GetObjectUserData(tgtBodyObject) s.detectBodyBodyCollision(srcBodyRef.index, tgtBodyRef.index, intersection) } else { - tgtPropMesh := s.shapeScene.GetMeshUserData(intersection.TargetMeshID) + tgtPropMesh := s.collisionScene.GetMeshUserData(intersection.TargetMeshID) s.detectBodyPropCollision(srcBodyRef.index, tgtPropMesh.index, intersection) } } } -func (s *Scene) detectBodyBodyCollision(primaryIndex, secondaryIndex uint32, intersection placement3d.Contact) { +func (s *Scene) detectBodyBodyCollision(primaryIndex, secondaryIndex int32, intersection placement3d.Contact) { primary := &s.bodies[primaryIndex] secondary := &s.bodies[secondaryIndex] @@ -701,7 +693,7 @@ func (s *Scene) detectBodyBodyCollision(primaryIndex, secondaryIndex uint32, int s.dbCollisionConstraints = append(s.dbCollisionConstraints, s.CreateDoubleBodyConstraint(primaryBody, secondaryBody, solver)) } -func (s *Scene) detectBodyPropCollision(bodyIndex, propIndex uint32, intersection placement3d.Contact) { +func (s *Scene) detectBodyPropCollision(bodyIndex, propIndex int32, intersection placement3d.Contact) { primary := &s.bodies[bodyIndex] secondary := &s.props[propIndex] @@ -817,11 +809,6 @@ func (s *Scene) allocateDualCollisionSolver() *constraint.PairCollision { // } // } -func (s *Scene) nextRevision() uint32 { - s.freeRevision++ - return s.freeRevision -} - func (s *Scene) notifySingleBodyCollisions() { for newCollision := range s.newSBCollisions { if _, ok := s.oldSBCollisions[newCollision]; !ok { @@ -928,7 +915,7 @@ func (s *Scene) initPlaceholder(placeholder *solver.Placeholder, body *bodyState placeholder.Init(solver.PlaceholderState{ Mass: body.mass, MomentOfInertia: body.momentOfInertia, - LinearVelocity: body.velocity, + LinearVelocity: body.linearVelocity, AngularVelocity: body.angularVelocity, Position: body.position, Rotation: body.rotation, @@ -936,12 +923,12 @@ func (s *Scene) initPlaceholder(placeholder *solver.Placeholder, body *bodyState } func (s *Scene) deinitPlaceholder(placeholder *solver.Placeholder, body *bodyState) { - body.velocity = placeholder.LinearVelocity() + body.linearVelocity = placeholder.LinearVelocity() body.angularVelocity = placeholder.AngularVelocity() body.position = placeholder.Position() body.rotation = placeholder.Rotation() - s.shapeScene.SetObjectTransform(body.objectID, shape3d.Transform{ + s.collisionScene.SetObjectTransform(body.objectID, shape3d.Transform{ Translation: body.position, Rotation: shape3d.RotationFromQuat(body.rotation), }) @@ -971,6 +958,12 @@ func (s *Scene) SoloConstraints() SoloConstraintView { } } +func (s *Scene) Bodies() BodyView { + return BodyView{ + scene: s, + } +} + func (s *Scene) allocateGlobalAccelerator() int32 { if !s.freeGlobalAcceleratorIndices.IsEmpty() { return s.freeGlobalAcceleratorIndices.Pop() @@ -1022,8 +1015,19 @@ func (s *Scene) releaseSoloConstraint(index int32) { s.freeSoloConstraintIndices.Push(index) } -type bodyRef struct { - index uint32 +func (s *Scene) allocateBody() int32 { + if !s.freeBodyIndices.IsEmpty() { + return s.freeBodyIndices.Pop() + } + index := int32(len(s.bodies)) + s.bodies = append(s.bodies, bodyState{}) + s.bodyAccelerationTargets = append(s.bodyAccelerationTargets, AccelerationTarget{}) + s.bodyConstraintPlaceholders = append(s.bodyConstraintPlaceholders, solver.Placeholder{}) + return index +} + +func (s *Scene) releaseBody(index int32) { + s.freeBodyIndices.Push(index) } type propRef struct { From d631e4c95ae5ea52c6f08e5dd76876d62c29e98d Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 15:44:24 +0300 Subject: [PATCH 08/85] WIP: More changes --- game/physics/accelerator_body.go | 57 ++++++++++++++----- game/physics/body.go | 20 ++++++- game/physics/constraint_solo.go | 96 ++++++++++++++++++++++++++------ game/physics/scene.go | 43 ++++++++------ 4 files changed, 168 insertions(+), 48 deletions(-) diff --git a/game/physics/accelerator_body.go b/game/physics/accelerator_body.go index 9aa83d75..21260db8 100644 --- a/game/physics/accelerator_body.go +++ b/game/physics/accelerator_body.go @@ -12,17 +12,18 @@ type BodyAcceleratorView struct { } func (v BodyAcceleratorView) Create(bodyID BodyID, solver AccelerationSolver) BodyAcceleratorID { - // TODO: Verify that the body exists! - bodyIndex := bodyID.index - - index := v.scene.allocateBodyAccelerator() - - accelerator := &v.scene.bodyAccelerators[index] - accelerator.solver = solver - accelerator.revision++ // progress revision to valid (odd) value - accelerator.isEnabled = true - - v.scene.attachBodyAccelerator(bodyIndex, index) + bodyView := v.scene.Bodies() + body := bodyView.resolve(bodyID, true) + + index, accelerator := v.scene.allocateBodyAccelerator() + *accelerator = bodyAccelerator{ + solver: solver, + revision: accelerator.revision + 1, // progress revision to valid (odd) value + bodyIndex: bodyID.index, + nextIndex: body.firstBodyAcceleratorIndex, + isEnabled: true, + } + body.firstBodyAcceleratorIndex = index return BodyAcceleratorID{ index: index, @@ -32,10 +33,30 @@ func (v BodyAcceleratorView) Create(bodyID BodyID, solver AccelerationSolver) Bo func (v BodyAcceleratorView) Delete(id BodyAcceleratorID) { accelerator := v.resolve(id, true) - accelerator.solver = nil // allow the solver to be garbage collected - accelerator.revision++ // progress revision to invalid (even) value - v.scene.detachBodyAccelerator(accelerator.bodyIndex, id.index) + body := &v.scene.bodies[accelerator.bodyIndex] + if body.firstBodyAcceleratorIndex == id.index { + body.firstBodyAcceleratorIndex = accelerator.nextIndex + } else { + prevIndex := body.firstBodyAcceleratorIndex + for prevIndex != nilIndex { + prev := &v.scene.bodyAccelerators[prevIndex] + if prev.nextIndex == id.index { + prev.nextIndex = accelerator.nextIndex + break + } + prevIndex = prev.nextIndex + } + } + + *accelerator = bodyAccelerator{ + solver: nil, // allow the solver to be garbage collected + revision: accelerator.revision + 1, // progress revision to invalid (even) value + bodyIndex: nilIndex, + nextIndex: nilIndex, + isEnabled: false, + } + v.scene.releaseBodyAccelerator(id.index) } @@ -81,6 +102,14 @@ func (v BodyAcceleratorView) SetEnabled(id BodyAcceleratorID, enabled bool) { accelerator.isEnabled = enabled } +func (v BodyAcceleratorView) idFromIndex(index int32) BodyAcceleratorID { + accelerator := &v.scene.bodyAccelerators[index] + return BodyAcceleratorID{ + index: index, + revision: accelerator.revision, + } +} + func (v BodyAcceleratorView) resolve(id BodyAcceleratorID, required bool) *bodyAccelerator { if id.revision == 0 { if required { diff --git a/game/physics/body.go b/game/physics/body.go index a3237f4d..94e75f3c 100644 --- a/game/physics/body.go +++ b/game/physics/body.go @@ -47,9 +47,19 @@ func (v BodyView) Create(position dprec.Vec3, rotation dprec.Quat) BodyID { func (v BodyView) Delete(id BodyID) { body := v.resolve(id, true) + + bodyAcceleratorView := v.scene.BodyAccelerators() + for body.firstBodyAcceleratorIndex != nilIndex { + bodyAcceleratorView.Delete(bodyAcceleratorView.idFromIndex(body.firstBodyAcceleratorIndex)) + } + soloConstraintView := v.scene.SoloConstraints() + for body.firstSoloConstraintIndex != nilIndex { + soloConstraintView.Delete(soloConstraintView.idFromIndex(body.firstSoloConstraintIndex)) + } + // TODO: delete pair constraints as well. + v.scene.collisionScene.DeleteObject(body.objectID) body.objectID = placement3d.InvalidObjectID - // TODO: delete all accelerators and constraints that reference this body body.revision++ // progress revision to invalid (even) value v.scene.releaseBody(id.index) } @@ -174,6 +184,14 @@ func (v BodyView) refreshPlacement(id BodyID, body *bodyState) { }) } +// func (v BodyView) idFromIndex(index int32) BodyID { +// body := &v.scene.bodies[index] +// return BodyID{ +// index: index, +// revision: body.revision, +// } +// } + func (v BodyView) resolve(id BodyID, required bool) *bodyState { if id.revision == 0 { if required { diff --git a/game/physics/constraint_solo.go b/game/physics/constraint_solo.go index d417e198..440d2393 100644 --- a/game/physics/constraint_solo.go +++ b/game/physics/constraint_solo.go @@ -6,8 +6,7 @@ type SoloConstraintContext struct { DeltaSeconds float64 ImpulseBeta float64 NudgeBeta float64 - - Target *solver.Placeholder // TODO: use package-local ImpulseTarget instead of Placeholder + Target *solver.Placeholder // TODO: use package-local ImpulseTarget instead of Placeholder } type SoloConstraintSolver interface { @@ -41,15 +40,18 @@ type SoloConstraintView struct { } func (v SoloConstraintView) Create(bodyID BodyID, solver SoloConstraintSolver) SoloConstraintID { - // TODO: verify that bodyID is valid and belongs to this scene - - index := v.scene.allocateSoloConstraint() - - constraint := &v.scene.soloConstraints[index] - constraint.solver = solver - constraint.revision++ // progress revision to valid (odd) value - constraint.bodyIndex = bodyID.index - constraint.enabled = true + bodyView := v.scene.Bodies() + body := bodyView.resolve(bodyID, true) + + index, constraint := v.scene.allocateSoloConstraint() + *constraint = soloConstraint{ + solver: solver, + revision: constraint.revision + 1, // progress revision to valid (odd) value + bodyIndex: bodyID.index, + nextIndex: body.firstSoloConstraintIndex, + isEnabled: true, + } + body.firstSoloConstraintIndex = index return SoloConstraintID{ index: index, @@ -59,8 +61,29 @@ func (v SoloConstraintView) Create(bodyID BodyID, solver SoloConstraintSolver) S func (v SoloConstraintView) Delete(id SoloConstraintID) { constraint := v.resolve(id, true) - constraint.solver = nil // allow the solver to be garbage collected - constraint.revision++ // progress revision to invalid (even) value + + body := &v.scene.bodies[constraint.bodyIndex] + if body.firstSoloConstraintIndex == id.index { + body.firstSoloConstraintIndex = constraint.nextIndex + } else { + prevIndex := body.firstSoloConstraintIndex + for prevIndex != nilIndex { + prev := &v.scene.soloConstraints[prevIndex] + if prev.nextIndex == id.index { + prev.nextIndex = constraint.nextIndex + break + } + prevIndex = prev.nextIndex + } + } + + *constraint = soloConstraint{ + solver: nil, // allow the solver to be garbage collected + revision: constraint.revision + 1, // progress revision to invalid (even) value + bodyIndex: nilIndex, + nextIndex: nilIndex, + isEnabled: false, + } v.scene.releaseSoloConstraint(id.index) } @@ -99,12 +122,20 @@ func (v SoloConstraintView) SetSolver(id SoloConstraintID, solver SoloConstraint func (v SoloConstraintView) Enabled(id SoloConstraintID) bool { constraint := v.resolve(id, true) - return constraint.enabled + return constraint.isEnabled } func (v SoloConstraintView) SetEnabled(id SoloConstraintID, enabled bool) { constraint := v.resolve(id, true) - constraint.enabled = enabled + constraint.isEnabled = enabled +} + +func (v SoloConstraintView) idFromIndex(index int32) SoloConstraintID { + constraint := &v.scene.soloConstraints[index] + return SoloConstraintID{ + index: index, + revision: constraint.revision, + } } func (v SoloConstraintView) resolve(id SoloConstraintID, required bool) *soloConstraint { @@ -129,11 +160,42 @@ type SoloConstraintHandle struct { id SoloConstraintID } -// TODO: Add methods to SoloConstraintHandle. +func (h SoloConstraintHandle) ID() SoloConstraintID { + return h.id +} + +func (h SoloConstraintHandle) Delete() { + h.view.Delete(h.id) +} + +func (h SoloConstraintHandle) IsValid() bool { + return h.view.IsValid(h.id) +} + +func (h SoloConstraintHandle) BodyID() BodyID { + return h.view.BodyID(h.id) +} + +func (h SoloConstraintHandle) Solver() SoloConstraintSolver { + return h.view.Solver(h.id) +} + +func (h SoloConstraintHandle) SetSolver(solver SoloConstraintSolver) { + h.view.SetSolver(h.id, solver) +} + +func (h SoloConstraintHandle) Enabled() bool { + return h.view.Enabled(h.id) +} + +func (h SoloConstraintHandle) SetEnabled(enabled bool) { + h.view.SetEnabled(h.id, enabled) +} type soloConstraint struct { solver SoloConstraintSolver revision int32 bodyIndex int32 - enabled bool + nextIndex int32 + isEnabled bool } diff --git a/game/physics/scene.go b/game/physics/scene.go index 86bdf5bf..07ae37a7 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -986,35 +986,46 @@ func (s *Scene) eachGlobalAccelerator(cb func(index int, accelerator *globalAcce } } -func (s *Scene) allocateBodyAccelerator() int32 { - panic("TODO") +func (s *Scene) allocateBodyAccelerator() (int32, *bodyAccelerator) { + var index int32 + if s.freeBodyAcceleratorIndices.IsEmpty() { + index = int32(len(s.bodyAccelerators)) + s.bodyAccelerators = append(s.bodyAccelerators, bodyAccelerator{}) + } else { + index = s.freeBodyAcceleratorIndices.Pop() + } + return index, &s.bodyAccelerators[index] } func (s *Scene) releaseBodyAccelerator(index int32) { panic("TODO") } -func (s *Scene) attachBodyAccelerator(bodyIndex, index int32) { - panic("TODO") -} - -func (s *Scene) detachBodyAccelerator(bodyIndex, index int32) { - panic("TODO") -} - -func (s *Scene) allocateSoloConstraint() int32 { - if !s.freeSoloConstraintIndices.IsEmpty() { - return s.freeSoloConstraintIndices.Pop() +func (s *Scene) allocateSoloConstraint() (int32, *soloConstraint) { + var index int32 + if s.freeSoloConstraintIndices.IsEmpty() { + index = int32(len(s.soloConstraints)) + s.soloConstraints = append(s.soloConstraints, soloConstraint{}) + } else { + index = s.freeSoloConstraintIndices.Pop() } - index := int32(len(s.soloConstraints)) - s.soloConstraints = append(s.soloConstraints, soloConstraint{}) - return index + return index, &s.soloConstraints[index] } func (s *Scene) releaseSoloConstraint(index int32) { s.freeSoloConstraintIndices.Push(index) } +func (s *Scene) verifySoloConstraintID(id SoloConstraintID) { + if id.revision == 0 { + panic("invalid solo constraint ID") + } + constraint := &s.soloConstraints[id.index] + if constraint.revision != id.revision { + panic("invalid solo constraint ID") + } +} + func (s *Scene) allocateBody() int32 { if !s.freeBodyIndices.IsEmpty() { return s.freeBodyIndices.Pop() From 77373a2e86aa47c1da699054eeed4359fee63ff7 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 15:50:54 +0300 Subject: [PATCH 09/85] WIP: Callback changes --- game/physics/callback.go | 60 ++++++++-------------------------------- game/physics/prop.go | 7 +++++ game/physics/scene.go | 21 +++++++------- 3 files changed, 29 insertions(+), 59 deletions(-) diff --git a/game/physics/callback.go b/game/physics/callback.go index 7aee2beb..2848ed2b 100644 --- a/game/physics/callback.go +++ b/game/physics/callback.go @@ -1,57 +1,19 @@ package physics -import ( - "time" +import "github.com/mokiat/lacking/util/observer" - "github.com/mokiat/lacking/util/observer" -) - -// UpdateCallback is a mechanism to receive update notifications. -type UpdateCallback func(elapsedTime time.Duration) - -// UpdateSubscription represents a notification subscription for updates. -type UpdateSubscription = observer.Subscription[UpdateCallback] - -// UpdateSubscriptionSet represents a set of update subscriptions. -type UpdateSubscriptionSet = observer.SubscriptionSet[UpdateCallback] - -// NewUpdateSubscriptionSet creates a new UpdateSubscriptionSet. -func NewUpdateSubscriptionSet() *UpdateSubscriptionSet { - return observer.NewSubscriptionSet[UpdateCallback]() -} - -// DoubleBodyCollisionCallback is a mechanism to receive notifications -// about collisions between two bodies. -type DoubleBodyCollisionCallback func(first, second Body, active bool) - -// DoubleBodyCollisionSubscription represents a notification subscription -// for double body collisions. -type DoubleBodyCollisionSubscription = observer.Subscription[DoubleBodyCollisionCallback] - -// DoubleBodyCollisionSubscriptionSet represents a set of double body -// collision subscriptions. -type DoubleBodyCollisionSubscriptionSet = observer.SubscriptionSet[DoubleBodyCollisionCallback] - -// NewDoubleBodyCollisionSubscriptionSet creates a new -// DoubleBodyCollisionSubscriptionSet. -func NewDoubleBodyCollisionSubscriptionSet() *DoubleBodyCollisionSubscriptionSet { - return observer.NewSubscriptionSet[DoubleBodyCollisionCallback]() -} - -// SingleBodyCollisionCallback is a mechanism to receive notifications +// SoloBodyCollisionCallback is a mechanism to receive notifications // about collisions between a body and a prop in the scene. -type SingleBodyCollisionCallback func(body Body, prop Prop, active bool) +type SoloBodyCollisionCallback func(bodyID BodyID, propID PropID, active bool) -// SingleBodyCollisionSubscription represents a notification subscription +// SoloBodyCollisionSubscription represents a notification subscription // for single body collisions. -type SingleBodyCollisionSubscription = observer.Subscription[SingleBodyCollisionCallback] +type SoloBodyCollisionSubscription = observer.Subscription[SoloBodyCollisionCallback] -// SingleBodyCollisionSubscriptionSet represents a set of single body -// collision subscriptions. -type SingleBodyCollisionSubscriptionSet = observer.SubscriptionSet[SingleBodyCollisionCallback] +// PairBodyCollisionCallback is a mechanism to receive notifications +// about collisions between two bodies. +type PairBodyCollisionCallback func(firstBodyID, secondBodyID BodyID, active bool) -// NewSingleBodyCollisionSubscriptionSet creates a new -// SingleBodyCollisionSubscriptionSet. -func NewSingleBodyCollisionSubscriptionSet() *SingleBodyCollisionSubscriptionSet { - return observer.NewSubscriptionSet[SingleBodyCollisionCallback]() -} +// PairBodyCollisionSubscription represents a notification subscription +// for double body collisions. +type PairBodyCollisionSubscription = observer.Subscription[PairBodyCollisionCallback] diff --git a/game/physics/prop.go b/game/physics/prop.go index 1ead4249..dbcbab22 100644 --- a/game/physics/prop.go +++ b/game/physics/prop.go @@ -7,6 +7,13 @@ import ( "github.com/mokiat/lacking/core/spatial/shape3d" ) +type PropID struct { + index int32 + revision int32 +} + +var NilPropID = PropID{} + type PropInfo struct { Name string Position opt.T[dprec.Vec3] diff --git a/game/physics/scene.go b/game/physics/scene.go index 07ae37a7..bea426e7 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -12,6 +12,7 @@ import ( "github.com/mokiat/lacking/debug/metric" "github.com/mokiat/lacking/game/physics/constraint" "github.com/mokiat/lacking/game/physics/solver" + "github.com/mokiat/lacking/util/observer" ) // Scene represents a physics scene that contains @@ -20,8 +21,8 @@ import ( type Scene struct { collisionScene *placement3d.Scene[bodyData, shapeData, propRef] - sbCollisionSubscriptions *SingleBodyCollisionSubscriptionSet - dbCollisionSubscriptions *DoubleBodyCollisionSubscriptionSet + sbCollisionSubscriptions *observer.SubscriptionSet[SoloBodyCollisionCallback] + dbCollisionSubscriptions *observer.SubscriptionSet[PairBodyCollisionCallback] timeSpeed float64 @@ -81,8 +82,8 @@ func NewScene() *Scene { InitialItemCapacity: opt.V[uint32](1024), }), - sbCollisionSubscriptions: NewSingleBodyCollisionSubscriptionSet(), - dbCollisionSubscriptions: NewDoubleBodyCollisionSubscriptionSet(), + sbCollisionSubscriptions: observer.NewSubscriptionSet[SoloBodyCollisionCallback](), + dbCollisionSubscriptions: observer.NewSubscriptionSet[PairBodyCollisionCallback](), timeSpeed: 1.0, @@ -167,13 +168,13 @@ func (s *Scene) Delete() { // SubscribeSingleBodyCollision registers a callback that is invoked when a body // collides with a static object. -func (s *Scene) SubscribeSingleBodyCollision(callback SingleBodyCollisionCallback) *SingleBodyCollisionSubscription { +func (s *Scene) SubscribeSingleBodyCollision(callback SoloBodyCollisionCallback) *SoloBodyCollisionSubscription { return s.sbCollisionSubscriptions.Subscribe(callback) } // SubscribeDoubleBodyCollision registers a callback that is invoked when two // bodies collide. -func (s *Scene) SubscribeDoubleBodyCollision(callback DoubleBodyCollisionCallback) *DoubleBodyCollisionSubscription { +func (s *Scene) SubscribeDoubleBodyCollision(callback PairBodyCollisionCallback) *PairBodyCollisionSubscription { return s.dbCollisionSubscriptions.Subscribe(callback) } @@ -819,7 +820,7 @@ func (s *Scene) notifySingleBodyCollisions() { prop := Prop{ name: s.props[newCollision.PropRef.Index].name, } - s.sbCollisionSubscriptions.Each(func(callback SingleBodyCollisionCallback) { + s.sbCollisionSubscriptions.Each(func(callback SoloBodyCollisionCallback) { callback(primary, prop, true) }) } @@ -833,7 +834,7 @@ func (s *Scene) notifySingleBodyCollisions() { prop := Prop{ name: s.props[oldCollision.PropRef.Index].name, } - s.sbCollisionSubscriptions.Each(func(callback SingleBodyCollisionCallback) { + s.sbCollisionSubscriptions.Each(func(callback SoloBodyCollisionCallback) { callback(primary, prop, false) }) } @@ -854,7 +855,7 @@ func (s *Scene) notifyDoubleBodyCollisions() { scene: s, reference: newCollision.SecondaryRef, } - s.dbCollisionSubscriptions.Each(func(callback DoubleBodyCollisionCallback) { + s.dbCollisionSubscriptions.Each(func(callback PairBodyCollisionCallback) { callback(primary, secondary, true) }) } @@ -869,7 +870,7 @@ func (s *Scene) notifyDoubleBodyCollisions() { scene: s, reference: oldCollision.SecondaryRef, } - s.dbCollisionSubscriptions.Each(func(callback DoubleBodyCollisionCallback) { + s.dbCollisionSubscriptions.Each(func(callback PairBodyCollisionCallback) { callback(primary, secondary, false) }) } From 8824f9e3af83688ad07731b7e738a75dc5251052 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 15:58:26 +0300 Subject: [PATCH 10/85] Minor changes --- game/physics/body.go | 53 ++++++++++++++++++++++++++----------------- game/physics/scene.go | 1 - 2 files changed, 32 insertions(+), 22 deletions(-) diff --git a/game/physics/body.go b/game/physics/body.go index 94e75f3c..c63c8582 100644 --- a/game/physics/body.go +++ b/game/physics/body.go @@ -30,14 +30,19 @@ func (v BodyView) Create(position dprec.Vec3, rotation dprec.Quat) BodyID { }) body := &v.scene.bodies[index] - body.objectID = objectID - body.revision++ // progress revision to valid (odd) value - body.invMass = 1.0 - body.invInertia = dprec.IdentityMat3() - body.linearVelocity = dprec.ZeroVec3() - body.angularVelocity = dprec.ZeroVec3() - body.position = position - body.rotation = rotation + *body = bodyState{ + objectID: objectID, + revision: body.revision + 1, // progress revision to valid (odd) value + firstBodyAcceleratorIndex: nilIndex, + firstSoloConstraintIndex: nilIndex, + firstPairConstraintIndex: nilIndex, + invMass: 1.0, + invInertia: dprec.IdentityMat3(), + linearVelocity: dprec.ZeroVec3(), + angularVelocity: dprec.ZeroVec3(), + position: position, + rotation: rotation, + } return BodyID{ index: index, @@ -48,6 +53,8 @@ func (v BodyView) Create(position dprec.Vec3, rotation dprec.Quat) BodyID { func (v BodyView) Delete(id BodyID) { body := v.resolve(id, true) + v.scene.collisionScene.DeleteObject(body.objectID) + bodyAcceleratorView := v.scene.BodyAccelerators() for body.firstBodyAcceleratorIndex != nilIndex { bodyAcceleratorView.Delete(bodyAcceleratorView.idFromIndex(body.firstBodyAcceleratorIndex)) @@ -58,9 +65,20 @@ func (v BodyView) Delete(id BodyID) { } // TODO: delete pair constraints as well. - v.scene.collisionScene.DeleteObject(body.objectID) - body.objectID = placement3d.InvalidObjectID - body.revision++ // progress revision to invalid (even) value + *body = bodyState{ + objectID: placement3d.InvalidObjectID, + revision: body.revision + 1, // progress revision to invalid (even) value + firstBodyAcceleratorIndex: nilIndex, + firstSoloConstraintIndex: nilIndex, + firstPairConstraintIndex: nilIndex, + invMass: 1.0, + invInertia: dprec.IdentityMat3(), + linearVelocity: dprec.ZeroVec3(), + angularVelocity: dprec.ZeroVec3(), + position: dprec.ZeroVec3(), + rotation: dprec.IdentityQuat(), + } + v.scene.releaseBody(id.index) } @@ -184,25 +202,17 @@ func (v BodyView) refreshPlacement(id BodyID, body *bodyState) { }) } -// func (v BodyView) idFromIndex(index int32) BodyID { -// body := &v.scene.bodies[index] -// return BodyID{ -// index: index, -// revision: body.revision, -// } -// } - func (v BodyView) resolve(id BodyID, required bool) *bodyState { if id.revision == 0 { if required { - panic("invalid global accelerator ID") + panic("invalid body ID") } return nil } body := &v.scene.bodies[id.index] if body.revision != id.revision { if required { - panic("invalid global accelerator ID") + panic("invalid body ID") } return nil } @@ -317,6 +327,7 @@ type bodyState struct { revision int32 firstBodyAcceleratorIndex int32 firstSoloConstraintIndex int32 + firstPairConstraintIndex int32 invMass float64 invInertia dprec.Mat3 diff --git a/game/physics/scene.go b/game/physics/scene.go index bea426e7..a71961e7 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -98,7 +98,6 @@ func NewScene() *Scene { bodyAccelerationTargets: make([]AccelerationTarget, 0, 64), bodyConstraintPlaceholders: make([]solver.Placeholder, 0, 64), - // bodyAccelerators []any // TOOD // areaAccelerators []any // TODO sbConstraints: make([]sbConstraintState, 0, 64), From b61e28a07fe2b7bc3c8bab0a401e8137a3c98e74 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 17:57:23 +0300 Subject: [PATCH 11/85] Minor code changes --- game/physics/constraint_sb.go | 94 ------------------------------- game/physics/scene.go | 102 +++++++++++----------------------- 2 files changed, 33 insertions(+), 163 deletions(-) delete mode 100644 game/physics/constraint_sb.go diff --git a/game/physics/constraint_sb.go b/game/physics/constraint_sb.go deleted file mode 100644 index 3de6ca26..00000000 --- a/game/physics/constraint_sb.go +++ /dev/null @@ -1,94 +0,0 @@ -package physics - -import "github.com/mokiat/lacking/game/physics/solver" - -var invalidSBConstraintState = &sbConstraintState{} - -// SBConstraint represents a restriction enforced on one body. -type SBConstraint struct { - scene *Scene - reference indexReference -} - -// Enabled returns whether this constraint will be enforced. -// By default a constraint is enabled. -func (c SBConstraint) Enabled() bool { - state := c.state() - return state.enabled -} - -// SetEnabled changes whether this constraint will be enforced. -func (c SBConstraint) SetEnabled(enabled bool) { - state := c.state() - state.enabled = enabled -} - -// Logic returns the constraint solver that will be used to enforce -// mathematically this constraint. -func (c SBConstraint) Logic() solver.Constraint { - state := c.state() - return state.logic -} - -// Body returns the body on which this constraint acts. -func (c SBConstraint) Body() Body { - state := c.state() - return state.body -} - -// Delete removes this constraint. -func (c SBConstraint) Delete() { - deleteSBConstraint(c.scene, c.reference) -} - -func (c SBConstraint) state() *sbConstraintState { - index := c.reference.Index - state := &c.scene.sbConstraints[index] - if state.reference != c.reference { - return invalidSBConstraintState - } - return state -} - -type sbConstraintState struct { - reference indexReference - logic solver.Constraint - body Body - enabled bool -} - -func (s sbConstraintState) IsActive() bool { - return s.reference.IsValid() && s.enabled -} - -func createSBConstraint(scene *Scene, logic solver.Constraint, body Body) SBConstraint { - var freeIndex uint32 - if scene.freeSBConstraintIndices.IsEmpty() { - freeIndex = uint32(len(scene.sbConstraints)) - scene.sbConstraints = append(scene.sbConstraints, sbConstraintState{}) - } else { - freeIndex = scene.freeSBConstraintIndices.Pop() - } - - reference := newIndexReference(freeIndex, scene.nextRevision()) - scene.sbConstraints[freeIndex] = sbConstraintState{ - reference: reference, - logic: logic, - body: body, - enabled: true, - } - return SBConstraint{ - scene: scene, - reference: reference, - } -} - -func deleteSBConstraint(scene *Scene, reference indexReference) { - index := reference.Index - state := &scene.sbConstraints[index] - if state.reference == reference { - state.reference = newIndexReference(index, 0) - state.logic = nil - scene.freeSBConstraintIndices.Push(index) - } -} diff --git a/game/physics/scene.go b/game/physics/scene.go index a71961e7..de730aa9 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -37,11 +37,7 @@ type Scene struct { bodyAccelerationTargets []AccelerationTarget bodyConstraintPlaceholders []solver.Placeholder - sbConstraints []sbConstraintState - freeSBConstraintIndices *ds.Stack[uint32] - - dbConstraints []dbConstraintState - freeDBConstraintIndices *ds.Stack[uint32] + dbConstraints []dbConstraintState sbCollisionConstraints []SBConstraint sbCollisionSolvers []constraint.Collision @@ -100,12 +96,8 @@ func NewScene() *Scene { // areaAccelerators []any // TODO - sbConstraints: make([]sbConstraintState, 0, 64), dbConstraints: make([]dbConstraintState, 0, 64), - freeSBConstraintIndices: ds.PreallocatedStack[uint32](16), - freeDBConstraintIndices: ds.PreallocatedStack[uint32](16), - collisionSet: make(placement3d.ContactList, 0, 128), oldSBCollisions: make(map[sbCollisionPair]struct{}, 32), @@ -134,21 +126,11 @@ func NewScene() *Scene { func (s *Scene) Delete() { s.props = nil - s.freeBodyIndices = nil s.bodies = nil s.bodyAccelerationTargets = nil s.bodyConstraintPlaceholders = nil - s.globalAccelerators = nil - s.freeBodyAcceleratorIndices = nil - s.freeAreaAcceleratorIndices = nil - s.freeGlobalAcceleratorIndices = nil - - s.sbConstraints = nil - s.freeSBConstraintIndices = nil - s.dbConstraints = nil - s.freeDBConstraintIndices = nil s.sbCollisionConstraints = nil s.sbCollisionSolvers = nil @@ -165,6 +147,38 @@ func (s *Scene) Delete() { s.newDBCollisions = nil } +// MediumSolver returns the solver that is used to calculate the medium +// properties of the scene. +// +// The returned solver is never nil. A scene starts off with a default +// [StaticAirSolver]. +func (s *Scene) MediumSolver() MediumSolver { + return s.mediumSolver +} + +// SetMediumSolver changes the solver that is used to calculate the medium +// properties of the scene. +// +// Passing nil is not an error and resets the scene to a default +// [StaticAirSolver], since the scene always needs a medium to sample. +func (s *Scene) SetMediumSolver(solver MediumSolver) { + if solver != nil { + s.mediumSolver = solver + } else { + s.mediumSolver = NewStaticAirSolver() + } +} + +// GlobalAccelerators returns a [GlobalAcceleratorView] through which the +// global accelerators of this scene can be created and managed. +func (s *Scene) GlobalAccelerators() GlobalAcceleratorView { + return GlobalAcceleratorView{ + scene: s, + } +} + +/////// OLD BELOW ------------ (TODO: DELETE COMMENT) + // SubscribeSingleBodyCollision registers a callback that is invoked when a body // collides with a static object. func (s *Scene) SubscribeSingleBodyCollision(callback SoloBodyCollisionCallback) *SoloBodyCollisionSubscription { @@ -212,28 +226,6 @@ func (s *Scene) SetMaxAngularAcceleration(acceleration float64) { s.maxAngularAcceleration = acceleration } -// MediumSolver returns the solver that is used to calculate the medium -// properties of the scene. -// -// The returned solver is never nil. A scene starts off with a default -// [StaticAirSolver]. -func (s *Scene) MediumSolver() MediumSolver { - return s.mediumSolver -} - -// SetMediumSolver changes the solver that is used to calculate the medium -// properties of the scene. -// -// Passing nil is not an error and resets the scene to a default -// [StaticAirSolver], since the scene always needs a medium to sample. -func (s *Scene) SetMediumSolver(solver MediumSolver) { - if solver != nil { - s.mediumSolver = solver - } else { - s.mediumSolver = NewStaticAirSolver() - } -} - // NextCollisionRejectGroup returns a collision reject group that is unique // within this Scene. Bodies that are assigned the same reject group do not // collide with each other, which is useful for objects that are meant to @@ -295,12 +287,6 @@ func (s *Scene) CreateProp(info PropInfo) { } } -// CreateSingleBodyConstraint creates a new physics constraint that acts on -// a single body and enables it for this scene. -func (s *Scene) CreateSingleBodyConstraint(body Body, logic solver.Constraint) SBConstraint { - return createSBConstraint(s, logic, body) -} - // CreateDoubleBodyConstraint creates a new physics constraint that acts on // two bodies and enables it for this scene. func (s *Scene) CreateDoubleBodyConstraint(primary, secondary Body, logic solver.PairConstraint) DBConstraint { @@ -486,12 +472,6 @@ func (s *Scene) applyImpulses(elapsedSeconds float64) { s.initPlaceholder(placeholder, body) }) - s.eachSBConstraintState(func(_ int, constraint *sbConstraintState) { - if s.resolveBodyState(constraint.body.reference) == nil { - deleteSBConstraint(s, constraint.reference) - return - } - }) s.eachDBConstraintState(func(_ int, constraint *dbConstraintState) { if s.resolveBodyState(constraint.primary.reference) == nil { deleteDBConstraint(s, constraint.reference) @@ -887,14 +867,6 @@ func (s *Scene) eachBodyState(cb func(index int, b *bodyState)) { } } -func (s *Scene) eachSBConstraintState(cb func(index int, constraint *sbConstraintState)) { - for i := range s.sbConstraints { - if constraint := &s.sbConstraints[i]; constraint.IsActive() { - cb(i, constraint) - } - } -} - func (s *Scene) eachDBConstraintState(cb func(index int, constraint *dbConstraintState)) { for i := range s.dbConstraints { if constraint := &s.dbConstraints[i]; constraint.IsActive() { @@ -934,14 +906,6 @@ func (s *Scene) deinitPlaceholder(placeholder *solver.Placeholder, body *bodySta }) } -// GlobalAccelerators returns a [GlobalAcceleratorView] through which the -// global accelerators of this scene can be created and managed. -func (s *Scene) GlobalAccelerators() GlobalAcceleratorView { - return GlobalAcceleratorView{ - scene: s, - } -} - // func (s *Scene) AreaAccelerators() AreaAcceleratorView { // panic("TODO") // } From 9a8372fdcb031eba2d364f6d83103a635aa9d61a Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 18:04:47 +0300 Subject: [PATCH 12/85] Add godoc for solo constraints --- game/physics/constraint_solo.go | 132 +++++++++++++++++++++++++++++--- 1 file changed, 122 insertions(+), 10 deletions(-) diff --git a/game/physics/constraint_solo.go b/game/physics/constraint_solo.go index 440d2393..fe51a967 100644 --- a/game/physics/constraint_solo.go +++ b/game/physics/constraint_solo.go @@ -2,43 +2,91 @@ package physics import "github.com/mokiat/lacking/game/physics/solver" +// SoloConstraintContext contains the information that a [SoloConstraintSolver] +// needs in order to process the single body it acts upon during a physics +// simulation step. type SoloConstraintContext struct { + + // DeltaSeconds is the amount of time, in seconds, covered by the + // current physics simulation step. DeltaSeconds float64 - ImpulseBeta float64 - NudgeBeta float64 - Target *solver.Placeholder // TODO: use package-local ImpulseTarget instead of Placeholder + + // ImpulseBeta is the Baumgarte stabilization factor to be used when + // correcting positional drift through impulses. + ImpulseBeta float64 + + // NudgeBeta is the Baumgarte stabilization factor to be used when + // correcting positional drift through nudges. + NudgeBeta float64 + + // Target is the placeholder representation of the body that is + // constrained. + Target *solver.Placeholder // TODO: use package-local ImpulseTarget instead of Placeholder } +// SoloConstraintSolver implements the mathematical logic that enforces a +// constraint acting on a single body. +// +// Instances are registered with a [Scene] through [SoloConstraintView.Create] +// and are subsequently driven by the physics engine through the methods +// below during each simulation step. type SoloConstraintSolver interface { - // Reset clears the internal cache state for this constraint solver. + + // Reset clears any internal cache state held by the solver, in + // preparation for a new physics simulation step. // - // This is called at the start of every iteration. + // This is called once at the start of every step, before + // ApplyImpulses or ApplyNudges are invoked. Reset(ctx SoloConstraintContext) // ApplyImpulses is called by the physics engine to instruct the solver - // to apply the necessary impulses to its object. + // to apply the necessary impulses to its target body, in order to + // correct its velocity so that the constraint is satisfied. // - // This is called multiple times per iteration. + // This is called multiple times per step, once for each impulse + // resolution iteration. ApplyImpulses(ctx SoloConstraintContext) - // ApplyNudges is called by the physics engine to instruct the solver to - // apply the necessary nudges to its object. + // ApplyNudges is called by the physics engine to instruct the solver + // to apply the necessary nudges to its target body, in order to + // correct its position so that the constraint is satisfied. // - // This is called multiple times per iteration. + // This is called multiple times per step, once for each nudge + // resolution iteration. ApplyNudges(ctx SoloConstraintContext) } +// SoloConstraintID uniquely identifies a solo constraint that has been +// created through [SoloConstraintView.Create]. +// +// The zero value is not a valid ID; use [NilSoloConstraintID] to represent +// the absence of a solo constraint. type SoloConstraintID struct { index int32 revision int32 } +// NilSoloConstraintID is a [SoloConstraintID] that is guaranteed to never +// reference a valid solo constraint. var NilSoloConstraintID = SoloConstraintID{} +// SoloConstraintView provides access to the solo constraints (i.e. +// constraints that act on a single body) that belong to a [Scene]. +// +// A SoloConstraintView is a lightweight accessor around a [Scene] and can +// be obtained through [Scene.SoloConstraints]. type SoloConstraintView struct { scene *Scene } +// Create registers solver as a new solo constraint that acts on the body +// identified by bodyID, and returns an ID through which the constraint can +// be referenced in the future. +// +// The returned constraint is enabled by default. It is automatically +// deleted whenever the target body is deleted. +// +// Create panics if bodyID does not reference a valid body. func (v SoloConstraintView) Create(bodyID BodyID, solver SoloConstraintSolver) SoloConstraintID { bodyView := v.scene.Bodies() body := bodyView.resolve(bodyID, true) @@ -59,9 +107,15 @@ func (v SoloConstraintView) Create(bodyID BodyID, solver SoloConstraintSolver) S } } +// Delete removes the solo constraint identified by id, unlinking it from +// its target body and releasing the underlying storage for reuse. +// +// Delete panics if id does not reference a valid solo constraint. func (v SoloConstraintView) Delete(id SoloConstraintID) { constraint := v.resolve(id, true) + // Unlink the constraint from its body's singly-linked list of solo + // constraints, which may require patching up a preceding sibling. body := &v.scene.bodies[constraint.bodyIndex] if body.firstSoloConstraintIndex == id.index { body.firstSoloConstraintIndex = constraint.nextIndex @@ -88,6 +142,9 @@ func (v SoloConstraintView) Delete(id SoloConstraintID) { v.scene.releaseSoloConstraint(id.index) } +// Handle returns a [SoloConstraintHandle] that wraps id, offering a more +// convenient, object-oriented way to interact with the referenced solo +// constraint. func (v SoloConstraintView) Handle(id SoloConstraintID) SoloConstraintHandle { return SoloConstraintHandle{ view: v, @@ -95,11 +152,17 @@ func (v SoloConstraintView) Handle(id SoloConstraintID) SoloConstraintHandle { } } +// IsValid returns whether id references a solo constraint that is still +// alive within the [Scene]. func (v SoloConstraintView) IsValid(id SoloConstraintID) bool { constraint := v.resolve(id, false) return constraint != nil } +// BodyID returns the ID of the body on which the solo constraint +// identified by id acts. +// +// BodyID panics if id does not reference a valid solo constraint. func (v SoloConstraintView) BodyID(id SoloConstraintID) BodyID { constraint := v.resolve(id, true) bodyIndex := constraint.bodyIndex @@ -110,26 +173,44 @@ func (v SoloConstraintView) BodyID(id SoloConstraintID) BodyID { } } +// Solver returns the [SoloConstraintSolver] that implements the solo +// constraint identified by id. +// +// Solver panics if id does not reference a valid solo constraint. func (v SoloConstraintView) Solver(id SoloConstraintID) SoloConstraintSolver { constraint := v.resolve(id, true) return constraint.solver } +// SetSolver changes the [SoloConstraintSolver] that implements the solo +// constraint identified by id. +// +// SetSolver panics if id does not reference a valid solo constraint. func (v SoloConstraintView) SetSolver(id SoloConstraintID, solver SoloConstraintSolver) { constraint := v.resolve(id, true) constraint.solver = solver } +// Enabled returns whether the solo constraint identified by id is +// currently enforced by the physics engine. +// +// Enabled panics if id does not reference a valid solo constraint. func (v SoloConstraintView) Enabled(id SoloConstraintID) bool { constraint := v.resolve(id, true) return constraint.isEnabled } +// SetEnabled changes whether the solo constraint identified by id is +// enforced by the physics engine. +// +// SetEnabled panics if id does not reference a valid solo constraint. func (v SoloConstraintView) SetEnabled(id SoloConstraintID, enabled bool) { constraint := v.resolve(id, true) constraint.isEnabled = enabled } +// idFromIndex builds the current [SoloConstraintID] for the solo +// constraint stored at the given slice index. func (v SoloConstraintView) idFromIndex(index int32) SoloConstraintID { constraint := &v.scene.soloConstraints[index] return SoloConstraintID{ @@ -138,6 +219,9 @@ func (v SoloConstraintView) idFromIndex(index int32) SoloConstraintID { } } +// resolve looks up the soloConstraint referenced by id. If id is stale or +// otherwise invalid, resolve panics when required is true, or returns nil +// otherwise. func (v SoloConstraintView) resolve(id SoloConstraintID, required bool) *soloConstraint { if id.revision == 0 { if required { @@ -155,43 +239,71 @@ func (v SoloConstraintView) resolve(id SoloConstraintID, required bool) *soloCon return constraint } +// SoloConstraintHandle is an object-oriented alternative to +// [SoloConstraintView] that is bound to a specific [SoloConstraintID]. +// +// It is obtained through [SoloConstraintView.Handle]. type SoloConstraintHandle struct { view SoloConstraintView id SoloConstraintID } +// ID returns the identifier of the solo constraint targeted by this +// handle. func (h SoloConstraintHandle) ID() SoloConstraintID { return h.id } +// Delete removes the solo constraint targeted by this handle. +// +// See [SoloConstraintView.Delete] for further details. func (h SoloConstraintHandle) Delete() { h.view.Delete(h.id) } +// IsValid returns whether this handle still references a solo constraint +// that is alive within the [Scene]. func (h SoloConstraintHandle) IsValid() bool { return h.view.IsValid(h.id) } +// BodyID returns the ID of the body on which the targeted solo constraint +// acts. func (h SoloConstraintHandle) BodyID() BodyID { return h.view.BodyID(h.id) } +// Solver returns the [SoloConstraintSolver] that implements the targeted +// solo constraint. func (h SoloConstraintHandle) Solver() SoloConstraintSolver { return h.view.Solver(h.id) } +// SetSolver changes the [SoloConstraintSolver] that implements the +// targeted solo constraint. func (h SoloConstraintHandle) SetSolver(solver SoloConstraintSolver) { h.view.SetSolver(h.id, solver) } +// Enabled returns whether the targeted solo constraint is currently +// enforced by the physics engine. func (h SoloConstraintHandle) Enabled() bool { return h.view.Enabled(h.id) } +// SetEnabled changes whether the targeted solo constraint is enforced by +// the physics engine. func (h SoloConstraintHandle) SetEnabled(enabled bool) { h.view.SetEnabled(h.id, enabled) } +// soloConstraint holds the internal state of a single solo constraint, as +// tracked by a [Scene]. +// +// Instances form a singly-linked list (through nextIndex) per body, rooted +// at the owning body's firstSoloConstraintIndex, so that all the solo +// constraints acting on a given body can be enumerated or deleted +// together. type soloConstraint struct { solver SoloConstraintSolver revision int32 From fcf767b1aa2c84cf66d4ddd9a2fb182fbd33d31e Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 18:09:38 +0300 Subject: [PATCH 13/85] Minor code alignment changes --- game/physics/accelerator_global.go | 35 ++++++++++++--------- game/physics/constraint_solo.go | 10 +++--- game/physics/scene.go | 50 ++++++++++++++---------------- 3 files changed, 48 insertions(+), 47 deletions(-) diff --git a/game/physics/accelerator_global.go b/game/physics/accelerator_global.go index de67b429..745f2773 100644 --- a/game/physics/accelerator_global.go +++ b/game/physics/accelerator_global.go @@ -30,12 +30,13 @@ type GlobalAcceleratorView struct { // // The accelerator is enabled by default. func (v GlobalAcceleratorView) Create(solver AccelerationSolver) GlobalAcceleratorID { - index := v.scene.allocateGlobalAccelerator() + index, accelerator := v.scene.allocateGlobalAccelerator() - accelerator := &v.scene.globalAccelerators[index] - accelerator.solver = solver - accelerator.revision++ // progress revision to valid (odd) value - accelerator.enabled = true + *accelerator = globalAcceleratorState{ + solver: solver, + revision: accelerator.revision + 1, // progress revision to valid (odd) value + isEnabled: true, + } return GlobalAcceleratorID{ index: index, @@ -51,8 +52,12 @@ func (v GlobalAcceleratorView) Create(solver AccelerationSolver) GlobalAccelerat // validity is not otherwise guaranteed. func (v GlobalAcceleratorView) Delete(id GlobalAcceleratorID) { accelerator := v.resolve(id, true) - accelerator.solver = nil // allow the solver to be garbage collected - accelerator.revision++ // progress revision to invalid (even) value + + *accelerator = globalAcceleratorState{ + solver: nil, // allow the solver to be garbage collected + revision: accelerator.revision + 1, // progress revision to invalid (even) value + isEnabled: false, + } v.scene.releaseGlobalAccelerator(id.index) } @@ -98,7 +103,7 @@ func (v GlobalAcceleratorView) SetSolver(id GlobalAcceleratorID, solver Accelera // It panics if the ID does not reference a valid global accelerator. func (v GlobalAcceleratorView) Enabled(id GlobalAcceleratorID) bool { accelerator := v.resolve(id, true) - return accelerator.enabled + return accelerator.isEnabled } // SetEnabled changes whether the specified global accelerator is evaluated @@ -107,10 +112,10 @@ func (v GlobalAcceleratorView) Enabled(id GlobalAcceleratorID) bool { // It panics if the ID does not reference a valid global accelerator. func (v GlobalAcceleratorView) SetEnabled(id GlobalAcceleratorID, enabled bool) { accelerator := v.resolve(id, true) - accelerator.enabled = enabled + accelerator.isEnabled = enabled } -func (v GlobalAcceleratorView) resolve(id GlobalAcceleratorID, required bool) *globalAccelerator { +func (v GlobalAcceleratorView) resolve(id GlobalAcceleratorID, required bool) *globalAcceleratorState { if id.revision == 0 { if required { panic("invalid global accelerator ID") @@ -188,12 +193,12 @@ func (h GlobalAcceleratorHandle) SetEnabled(enabled bool) { h.view.SetEnabled(h.id, enabled) } -type globalAccelerator struct { - solver AccelerationSolver - revision int32 - enabled bool +type globalAcceleratorState struct { + solver AccelerationSolver + revision int32 + isEnabled bool } -func (s *globalAccelerator) isValid() bool { +func (s *globalAcceleratorState) isValid() bool { return s.revision%2 == 1 // only odd revisions are valid } diff --git a/game/physics/constraint_solo.go b/game/physics/constraint_solo.go index fe51a967..5a85871e 100644 --- a/game/physics/constraint_solo.go +++ b/game/physics/constraint_solo.go @@ -92,7 +92,7 @@ func (v SoloConstraintView) Create(bodyID BodyID, solver SoloConstraintSolver) S body := bodyView.resolve(bodyID, true) index, constraint := v.scene.allocateSoloConstraint() - *constraint = soloConstraint{ + *constraint = soloConstraintState{ solver: solver, revision: constraint.revision + 1, // progress revision to valid (odd) value bodyIndex: bodyID.index, @@ -131,7 +131,7 @@ func (v SoloConstraintView) Delete(id SoloConstraintID) { } } - *constraint = soloConstraint{ + *constraint = soloConstraintState{ solver: nil, // allow the solver to be garbage collected revision: constraint.revision + 1, // progress revision to invalid (even) value bodyIndex: nilIndex, @@ -222,7 +222,7 @@ func (v SoloConstraintView) idFromIndex(index int32) SoloConstraintID { // resolve looks up the soloConstraint referenced by id. If id is stale or // otherwise invalid, resolve panics when required is true, or returns nil // otherwise. -func (v SoloConstraintView) resolve(id SoloConstraintID, required bool) *soloConstraint { +func (v SoloConstraintView) resolve(id SoloConstraintID, required bool) *soloConstraintState { if id.revision == 0 { if required { panic("invalid solo constraint ID") @@ -297,14 +297,14 @@ func (h SoloConstraintHandle) SetEnabled(enabled bool) { h.view.SetEnabled(h.id, enabled) } -// soloConstraint holds the internal state of a single solo constraint, as +// soloConstraintState holds the internal state of a single solo constraint, as // tracked by a [Scene]. // // Instances form a singly-linked list (through nextIndex) per body, rooted // at the owning body's firstSoloConstraintIndex, so that all the solo // constraints acting on a given body can be enumerated or deleted // together. -type soloConstraint struct { +type soloConstraintState struct { solver SoloConstraintSolver revision int32 bodyIndex int32 diff --git a/game/physics/scene.go b/game/physics/scene.go index de730aa9..a76ec24a 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -59,14 +59,13 @@ type Scene struct { mediumSolver MediumSolver freeGlobalAcceleratorIndices *ds.Stack[int32] - freeAreaAcceleratorIndices *ds.Stack[int32] freeBodyAcceleratorIndices *ds.Stack[int32] freeSoloConstraintIndices *ds.Stack[int32] freeBodyIndices *ds.Stack[int32] - globalAccelerators []globalAccelerator + globalAccelerators []globalAcceleratorState bodyAccelerators []bodyAccelerator - soloConstraints []soloConstraint + soloConstraints []soloConstraintState } func NewScene() *Scene { @@ -110,14 +109,13 @@ func NewScene() *Scene { mediumSolver: NewStaticAirSolver(), freeGlobalAcceleratorIndices: ds.EmptyStack[int32](), - freeAreaAcceleratorIndices: ds.EmptyStack[int32](), freeBodyAcceleratorIndices: ds.EmptyStack[int32](), freeSoloConstraintIndices: ds.EmptyStack[int32](), freeBodyIndices: ds.EmptyStack[int32](), - globalAccelerators: make([]globalAccelerator, 0), + globalAccelerators: make([]globalAcceleratorState, 0), bodyAccelerators: make([]bodyAccelerator, 0), - soloConstraints: make([]soloConstraint, 0), + soloConstraints: make([]soloConstraintState, 0), } } @@ -177,6 +175,12 @@ func (s *Scene) GlobalAccelerators() GlobalAcceleratorView { } } +func (s *Scene) BodyAccelerators() BodyAcceleratorView { + return BodyAcceleratorView{ + scene: s, + } +} + /////// OLD BELOW ------------ (TODO: DELETE COMMENT) // SubscribeSingleBodyCollision registers a callback that is invoked when a body @@ -397,8 +401,8 @@ func (s *Scene) applyAreaAccelerators() { func (s *Scene) applyGlobalAccelerators() { s.eachBodyState(func(index int, _ *bodyState) { target := &s.bodyAccelerationTargets[index] - s.eachGlobalAccelerator(func(_ int, accelerator *globalAccelerator) { - if accelerator.enabled { + s.eachGlobalAccelerator(func(_ int, accelerator *globalAcceleratorState) { + if accelerator.isEnabled { // TODO: Consider caching the following calculation, especially // if the medium solver is expensive to compute. position := target.Position() @@ -906,16 +910,6 @@ func (s *Scene) deinitPlaceholder(placeholder *solver.Placeholder, body *bodySta }) } -// func (s *Scene) AreaAccelerators() AreaAcceleratorView { -// panic("TODO") -// } - -func (s *Scene) BodyAccelerators() BodyAcceleratorView { - return BodyAcceleratorView{ - scene: s, - } -} - func (s *Scene) SoloConstraints() SoloConstraintView { return SoloConstraintView{ scene: s, @@ -928,20 +922,22 @@ func (s *Scene) Bodies() BodyView { } } -func (s *Scene) allocateGlobalAccelerator() int32 { - if !s.freeGlobalAcceleratorIndices.IsEmpty() { - return s.freeGlobalAcceleratorIndices.Pop() +func (s *Scene) allocateGlobalAccelerator() (int32, *globalAcceleratorState) { + var index int32 + if s.freeGlobalAcceleratorIndices.IsEmpty() { + index = int32(len(s.globalAccelerators)) + s.globalAccelerators = append(s.globalAccelerators, globalAcceleratorState{}) + } else { + index = s.freeGlobalAcceleratorIndices.Pop() } - index := int32(len(s.globalAccelerators)) - s.globalAccelerators = append(s.globalAccelerators, globalAccelerator{}) - return index + return index, &s.globalAccelerators[index] } func (s *Scene) releaseGlobalAccelerator(index int32) { s.freeGlobalAcceleratorIndices.Push(index) } -func (s *Scene) eachGlobalAccelerator(cb func(index int, accelerator *globalAccelerator)) { +func (s *Scene) eachGlobalAccelerator(cb func(index int, accelerator *globalAcceleratorState)) { for i := range s.globalAccelerators { accelerator := &s.globalAccelerators[i] if accelerator.isValid() { @@ -965,11 +961,11 @@ func (s *Scene) releaseBodyAccelerator(index int32) { panic("TODO") } -func (s *Scene) allocateSoloConstraint() (int32, *soloConstraint) { +func (s *Scene) allocateSoloConstraint() (int32, *soloConstraintState) { var index int32 if s.freeSoloConstraintIndices.IsEmpty() { index = int32(len(s.soloConstraints)) - s.soloConstraints = append(s.soloConstraints, soloConstraint{}) + s.soloConstraints = append(s.soloConstraints, soloConstraintState{}) } else { index = s.freeSoloConstraintIndices.Pop() } From f6c3c6fffb102413f7260822142c2eb867ef64a3 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 18:11:41 +0300 Subject: [PATCH 14/85] Minor godoc adjustments --- game/physics/accelerator_global.go | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/game/physics/accelerator_global.go b/game/physics/accelerator_global.go index 745f2773..4cae6e8b 100644 --- a/game/physics/accelerator_global.go +++ b/game/physics/accelerator_global.go @@ -115,6 +115,9 @@ func (v GlobalAcceleratorView) SetEnabled(id GlobalAcceleratorID, enabled bool) accelerator.isEnabled = enabled } +// resolve looks up the globalAcceleratorState referenced by id. If id is +// stale or otherwise invalid, resolve panics when required is true, or +// returns nil otherwise. func (v GlobalAcceleratorView) resolve(id GlobalAcceleratorID, required bool) *globalAcceleratorState { if id.revision == 0 { if required { @@ -193,12 +196,16 @@ func (h GlobalAcceleratorHandle) SetEnabled(enabled bool) { h.view.SetEnabled(h.id, enabled) } +// globalAcceleratorState holds the internal state of a single global +// accelerator, as tracked by a [Scene]. type globalAcceleratorState struct { solver AccelerationSolver revision int32 isEnabled bool } +// isValid returns whether this state is currently backing a live global +// accelerator, as opposed to a freed slot awaiting reuse. func (s *globalAcceleratorState) isValid() bool { return s.revision%2 == 1 // only odd revisions are valid } From e251a68fb7f84fd60f457a995a0f4f97330c5b47 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 18:16:38 +0300 Subject: [PATCH 15/85] More adjustments and godoc --- game/physics/accelerator_body.go | 110 +++++++++++++++++++++++++++++-- game/physics/scene.go | 10 +-- 2 files changed, 111 insertions(+), 9 deletions(-) diff --git a/game/physics/accelerator_body.go b/game/physics/accelerator_body.go index 21260db8..4c1d38ec 100644 --- a/game/physics/accelerator_body.go +++ b/game/physics/accelerator_body.go @@ -1,22 +1,44 @@ package physics +// BodyAcceleratorID uniquely identifies a body accelerator that was created +// through [BodyAcceleratorView.Create]. +// +// The zero value, also available as [NilBodyAcceleratorID], does not +// reference a valid body accelerator. type BodyAcceleratorID struct { index int32 revision int32 } +// NilBodyAcceleratorID is a [BodyAcceleratorID] that never references a +// valid body accelerator. var NilBodyAcceleratorID = BodyAcceleratorID{} +// BodyAcceleratorView provides access to the body accelerators that belong +// to a [Scene]. +// +// A body accelerator evaluates its [AccelerationSolver] once for a single, +// specific body, on every simulation step. It is intended for effects that +// are tied to a particular body, as opposed to a global accelerator, which +// affects every body in the scene. type BodyAcceleratorView struct { scene *Scene } +// Create allocates a new body accelerator that uses the specified solver to +// act on the body identified by bodyID, and returns its ID. +// +// The accelerator is enabled by default. It is automatically deleted +// whenever the target body is deleted. +// +// Create panics if bodyID does not reference a valid body. func (v BodyAcceleratorView) Create(bodyID BodyID, solver AccelerationSolver) BodyAcceleratorID { bodyView := v.scene.Bodies() body := bodyView.resolve(bodyID, true) index, accelerator := v.scene.allocateBodyAccelerator() - *accelerator = bodyAccelerator{ + + *accelerator = bodyAcceleratorState{ solver: solver, revision: accelerator.revision + 1, // progress revision to valid (odd) value bodyIndex: bodyID.index, @@ -31,9 +53,18 @@ func (v BodyAcceleratorView) Create(bodyID BodyID, solver AccelerationSolver) Bo } } +// Delete removes the body accelerator with the specified ID, unlinking it +// from its target body and releasing the underlying storage for reuse. +// +// It panics if the ID does not reference a valid body accelerator, be it +// because it was never created, has already been deleted, or belongs to a +// different [Scene]. Use [BodyAcceleratorView.IsValid] first if the ID's +// validity is not otherwise guaranteed. func (v BodyAcceleratorView) Delete(id BodyAcceleratorID) { accelerator := v.resolve(id, true) + // Unlink the accelerator from its body's singly-linked list of body + // accelerators, which may require patching up a preceding sibling. body := &v.scene.bodies[accelerator.bodyIndex] if body.firstBodyAcceleratorIndex == id.index { body.firstBodyAcceleratorIndex = accelerator.nextIndex @@ -49,7 +80,7 @@ func (v BodyAcceleratorView) Delete(id BodyAcceleratorID) { } } - *accelerator = bodyAccelerator{ + *accelerator = bodyAcceleratorState{ solver: nil, // allow the solver to be garbage collected revision: accelerator.revision + 1, // progress revision to invalid (even) value bodyIndex: nilIndex, @@ -60,6 +91,9 @@ func (v BodyAcceleratorView) Delete(id BodyAcceleratorID) { v.scene.releaseBodyAccelerator(id.index) } +// Handle returns a [BodyAcceleratorHandle] that wraps the specified ID, as +// a more convenient means of repeatedly accessing the same body accelerator +// without having to pass its ID to this view on every call. func (v BodyAcceleratorView) Handle(id BodyAcceleratorID) BodyAcceleratorHandle { return BodyAcceleratorHandle{ view: v, @@ -67,11 +101,17 @@ func (v BodyAcceleratorView) Handle(id BodyAcceleratorID) BodyAcceleratorHandle } } +// IsValid returns whether the specified ID references a body accelerator +// that has not been deleted. func (v BodyAcceleratorView) IsValid(id BodyAcceleratorID) bool { accelerator := v.resolve(id, false) return accelerator != nil } +// BodyID returns the ID of the body on which the specified body +// accelerator acts. +// +// It panics if the ID does not reference a valid body accelerator. func (v BodyAcceleratorView) BodyID(id BodyAcceleratorID) BodyID { accelerator := v.resolve(id, true) bodyIndex := accelerator.bodyIndex @@ -82,26 +122,44 @@ func (v BodyAcceleratorView) BodyID(id BodyAcceleratorID) BodyID { } } +// Solver returns the acceleration solver used by the specified body +// accelerator. +// +// It panics if the ID does not reference a valid body accelerator. func (v BodyAcceleratorView) Solver(id BodyAcceleratorID) AccelerationSolver { accelerator := v.resolve(id, true) return accelerator.solver } +// SetSolver changes the acceleration solver used by the specified body +// accelerator. +// +// It panics if the ID does not reference a valid body accelerator. func (v BodyAcceleratorView) SetSolver(id BodyAcceleratorID, solver AccelerationSolver) { accelerator := v.resolve(id, true) accelerator.solver = solver } +// Enabled returns whether the specified body accelerator is evaluated +// during the simulation. A body accelerator is enabled by default. +// +// It panics if the ID does not reference a valid body accelerator. func (v BodyAcceleratorView) Enabled(id BodyAcceleratorID) bool { accelerator := v.resolve(id, true) return accelerator.isEnabled } +// SetEnabled changes whether the specified body accelerator is evaluated +// during the simulation. +// +// It panics if the ID does not reference a valid body accelerator. func (v BodyAcceleratorView) SetEnabled(id BodyAcceleratorID, enabled bool) { accelerator := v.resolve(id, true) accelerator.isEnabled = enabled } +// idFromIndex builds the current [BodyAcceleratorID] for the body +// accelerator stored at the given slice index. func (v BodyAcceleratorView) idFromIndex(index int32) BodyAcceleratorID { accelerator := &v.scene.bodyAccelerators[index] return BodyAcceleratorID{ @@ -110,7 +168,10 @@ func (v BodyAcceleratorView) idFromIndex(index int32) BodyAcceleratorID { } } -func (v BodyAcceleratorView) resolve(id BodyAcceleratorID, required bool) *bodyAccelerator { +// resolve looks up the bodyAcceleratorState referenced by id. If id is +// stale or otherwise invalid, resolve panics when required is true, or +// returns nil otherwise. +func (v BodyAcceleratorView) resolve(id BodyAcceleratorID, required bool) *bodyAcceleratorState { if id.revision == 0 { if required { panic("invalid body accelerator ID") @@ -127,44 +188,81 @@ func (v BodyAcceleratorView) resolve(id BodyAcceleratorID, required bool) *bodyA return accelerator } +// BodyAcceleratorHandle is a convenience wrapper that binds together a +// [BodyAcceleratorID] and the [BodyAcceleratorView] needed to resolve it, +// so that callers that repeatedly act on the same body accelerator do not +// have to keep passing its ID around. +// +// It is created through [BodyAcceleratorView.Handle]. type BodyAcceleratorHandle struct { view BodyAcceleratorView id BodyAcceleratorID } +// ID returns the [BodyAcceleratorID] wrapped by this handle. func (h BodyAcceleratorHandle) ID() BodyAcceleratorID { return h.id } +// Delete removes the wrapped body accelerator. +// +// It panics if the handle does not reference a valid body accelerator. func (h BodyAcceleratorHandle) Delete() { h.view.Delete(h.id) } +// IsValid returns whether the wrapped body accelerator has not been +// deleted. func (h BodyAcceleratorHandle) IsValid() bool { return h.view.IsValid(h.id) } +// BodyID returns the ID of the body on which the wrapped body accelerator +// acts. func (h BodyAcceleratorHandle) BodyID() BodyID { return h.view.BodyID(h.id) } +// Solver returns the acceleration solver used by the wrapped body +// accelerator. +// +// It panics if the handle does not reference a valid body accelerator. func (h BodyAcceleratorHandle) Solver() AccelerationSolver { return h.view.Solver(h.id) } +// SetSolver changes the acceleration solver used by the wrapped body +// accelerator. +// +// It panics if the handle does not reference a valid body accelerator. func (h BodyAcceleratorHandle) SetSolver(solver AccelerationSolver) { h.view.SetSolver(h.id, solver) } +// Enabled returns whether the wrapped body accelerator is evaluated during +// the simulation. A body accelerator is enabled by default. +// +// It panics if the handle does not reference a valid body accelerator. func (h BodyAcceleratorHandle) Enabled() bool { return h.view.Enabled(h.id) } +// SetEnabled changes whether the wrapped body accelerator is evaluated +// during the simulation. +// +// It panics if the handle does not reference a valid body accelerator. func (h BodyAcceleratorHandle) SetEnabled(enabled bool) { h.view.SetEnabled(h.id, enabled) } -type bodyAccelerator struct { +// bodyAcceleratorState holds the internal state of a single body +// accelerator, as tracked by a [Scene]. +// +// Instances form a singly-linked list (through nextIndex) per body, rooted +// at the owning body's firstBodyAcceleratorIndex, so that all the body +// accelerators acting on a given body can be enumerated or deleted +// together. +type bodyAcceleratorState struct { solver AccelerationSolver revision int32 bodyIndex int32 @@ -172,6 +270,8 @@ type bodyAccelerator struct { isEnabled bool } -func (s *bodyAccelerator) isValid() bool { +// isValid returns whether this state is currently backing a live body +// accelerator, as opposed to a freed slot awaiting reuse. +func (s *bodyAcceleratorState) isValid() bool { return s.revision%2 == 1 // only odd revisions are valid } diff --git a/game/physics/scene.go b/game/physics/scene.go index a76ec24a..e595d112 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -64,7 +64,7 @@ type Scene struct { freeBodyIndices *ds.Stack[int32] globalAccelerators []globalAcceleratorState - bodyAccelerators []bodyAccelerator + bodyAccelerators []bodyAcceleratorState soloConstraints []soloConstraintState } @@ -114,7 +114,7 @@ func NewScene() *Scene { freeBodyIndices: ds.EmptyStack[int32](), globalAccelerators: make([]globalAcceleratorState, 0), - bodyAccelerators: make([]bodyAccelerator, 0), + bodyAccelerators: make([]bodyAcceleratorState, 0), soloConstraints: make([]soloConstraintState, 0), } } @@ -175,6 +175,8 @@ func (s *Scene) GlobalAccelerators() GlobalAcceleratorView { } } +// BodyAccelerators returns a [BodyAcceleratorView] through which the body +// accelerators of this scene can be created and managed. func (s *Scene) BodyAccelerators() BodyAcceleratorView { return BodyAcceleratorView{ scene: s, @@ -946,11 +948,11 @@ func (s *Scene) eachGlobalAccelerator(cb func(index int, accelerator *globalAcce } } -func (s *Scene) allocateBodyAccelerator() (int32, *bodyAccelerator) { +func (s *Scene) allocateBodyAccelerator() (int32, *bodyAcceleratorState) { var index int32 if s.freeBodyAcceleratorIndices.IsEmpty() { index = int32(len(s.bodyAccelerators)) - s.bodyAccelerators = append(s.bodyAccelerators, bodyAccelerator{}) + s.bodyAccelerators = append(s.bodyAccelerators, bodyAcceleratorState{}) } else { index = s.freeBodyAcceleratorIndices.Pop() } From 7828db97ebaff079b4dcf8bd062b7edb45b53dc8 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 18:18:14 +0300 Subject: [PATCH 16/85] Minor code change --- game/physics/scene.go | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/game/physics/scene.go b/game/physics/scene.go index e595d112..c808ae9d 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -93,8 +93,6 @@ func NewScene() *Scene { bodyAccelerationTargets: make([]AccelerationTarget, 0, 64), bodyConstraintPlaceholders: make([]solver.Placeholder, 0, 64), - // areaAccelerators []any // TODO - dbConstraints: make([]dbConstraintState, 0, 64), collisionSet: make(placement3d.ContactList, 0, 128), @@ -183,6 +181,14 @@ func (s *Scene) BodyAccelerators() BodyAcceleratorView { } } +// SoloConstraints returns a [SoloConstraintView] through which the solo +// constraints of this scene can be created and managed. +func (s *Scene) SoloConstraints() SoloConstraintView { + return SoloConstraintView{ + scene: s, + } +} + /////// OLD BELOW ------------ (TODO: DELETE COMMENT) // SubscribeSingleBodyCollision registers a callback that is invoked when a body @@ -912,12 +918,6 @@ func (s *Scene) deinitPlaceholder(placeholder *solver.Placeholder, body *bodySta }) } -func (s *Scene) SoloConstraints() SoloConstraintView { - return SoloConstraintView{ - scene: s, - } -} - func (s *Scene) Bodies() BodyView { return BodyView{ scene: s, From 492cfe0f7aea653c57db62b33e949166a4607b69 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 18:45:44 +0300 Subject: [PATCH 17/85] Further code changes --- game/physics/acceleration.go | 95 ++++++++++-------------- game/physics/body.go | 50 ++++++++++++- game/physics/scene.go | 128 +++++++++------------------------ game/physics/solver_gravity.go | 2 +- 4 files changed, 121 insertions(+), 154 deletions(-) diff --git a/game/physics/acceleration.go b/game/physics/acceleration.go index e93a8446..94e369f1 100644 --- a/game/physics/acceleration.go +++ b/game/physics/acceleration.go @@ -13,17 +13,7 @@ import "github.com/mokiat/gomath/dprec" // All vectors and tensors are expressed in world space, with offsets being // relative to the object's center of mass. type AccelerationTarget struct { - invMass float64 - invInertia dprec.Mat3 - - position dprec.Vec3 - rotation dprec.Quat - - linearVelocity dprec.Vec3 - angularVelocity dprec.Vec3 - - linearAcceleration dprec.Vec3 - angularAcceleration dprec.Vec3 + body *bodyState } // newAccelerationTarget creates a new [AccelerationTarget] with the specified @@ -36,21 +26,9 @@ type AccelerationTarget struct { // The inverse moment of inertia has to be expressed in world space. Use // [RotatedMomentOfInertia] with the object's rotation to bring a tensor that // is expressed in local space into world space before inverting it. -func newAccelerationTarget( - invMass float64, - invInertia dprec.Mat3, - position dprec.Vec3, - rotation dprec.Quat, - linearVelocity dprec.Vec3, - angularVelocity dprec.Vec3, -) AccelerationTarget { +func newAccelerationTarget(body *bodyState) AccelerationTarget { return AccelerationTarget{ - invMass: invMass, - invInertia: invInertia, - position: position, - rotation: rotation, - linearVelocity: linearVelocity, - angularVelocity: angularVelocity, + body: body, } } @@ -59,16 +37,16 @@ func newAccelerationTarget( // // Prefer this method over [AccelerationTarget.Mass], since the physics // engine works with the inverse mass internally and no division is needed. -func (t *AccelerationTarget) InverseMass() float64 { - return t.invMass +func (t AccelerationTarget) InverseMass() float64 { + return t.body.invMass } // Mass returns the mass of the target. // // This method performs a division and returns positive infinity for an // immovable object. Prefer [AccelerationTarget.InverseMass] where possible. -func (t *AccelerationTarget) Mass() float64 { - return 1.0 / t.invMass +func (t AccelerationTarget) Mass() float64 { + return t.body.mass() } // InverseInertia returns the reciprocal of the moment of inertia tensor of @@ -78,8 +56,8 @@ func (t *AccelerationTarget) Mass() float64 { // Prefer this method over [AccelerationTarget.Inertia], since the physics // engine works with the inverse tensor internally and no matrix inversion // is needed. -func (t *AccelerationTarget) InverseInertia() dprec.Mat3 { - return t.invInertia +func (t AccelerationTarget) InverseInertia() dprec.Mat3 { + return t.body.invInertia } // Inertia returns the moment of inertia tensor of the target in world space. @@ -87,36 +65,36 @@ func (t *AccelerationTarget) InverseInertia() dprec.Mat3 { // This method performs a matrix inversion and is undefined for an object // that cannot be rotated. Prefer [AccelerationTarget.InverseInertia] where // possible. -func (t *AccelerationTarget) Inertia() dprec.Mat3 { - return dprec.InverseMat3(t.invInertia) +func (t AccelerationTarget) Inertia() dprec.Mat3 { + return t.body.inertia() } // Position returns the world position of the center of mass of the target. -func (t *AccelerationTarget) Position() dprec.Vec3 { - return t.position +func (t AccelerationTarget) Position() dprec.Vec3 { + return t.body.position } // Rotation returns the world orientation of the target. -func (t *AccelerationTarget) Rotation() dprec.Quat { - return t.rotation +func (t AccelerationTarget) Rotation() dprec.Quat { + return t.body.rotation } // LinearVelocity returns the world velocity of the center of mass of the // target. -func (t *AccelerationTarget) LinearVelocity() dprec.Vec3 { - return t.linearVelocity +func (t AccelerationTarget) LinearVelocity() dprec.Vec3 { + return t.body.linearVelocity } // AngularVelocity returns the world angular velocity of the target, in // radians per second around each axis. -func (t *AccelerationTarget) AngularVelocity() dprec.Vec3 { - return t.angularVelocity +func (t AccelerationTarget) AngularVelocity() dprec.Vec3 { + return t.body.angularVelocity } // LinearAcceleration returns the linear acceleration that has been // accumulated on the target so far. -func (t *AccelerationTarget) LinearAcceleration() dprec.Vec3 { - return t.linearAcceleration +func (t AccelerationTarget) LinearAcceleration() dprec.Vec3 { + return t.body.linearAcceleration } // AddLinearAcceleration accumulates the specified linear acceleration on the @@ -124,8 +102,8 @@ func (t *AccelerationTarget) LinearAcceleration() dprec.Vec3 { // // Since acceleration is independent of mass, this is the correct way to // model effects like gravity, which pull on all objects equally. -func (t *AccelerationTarget) AddLinearAcceleration(acceleration dprec.Vec3) { - t.linearAcceleration = dprec.Vec3Sum(t.linearAcceleration, acceleration) +func (t AccelerationTarget) AddLinearAcceleration(acceleration dprec.Vec3) { + t.body.addLinearAcceleration(acceleration) } // ApplyForce accumulates the linear acceleration that results from the @@ -133,34 +111,33 @@ func (t *AccelerationTarget) AddLinearAcceleration(acceleration dprec.Vec3) { // // Use [AccelerationTarget.ApplyOffsetForce] instead if the force does not // act on the center of mass and should induce rotation. -func (t *AccelerationTarget) ApplyForce(force dprec.Vec3) { - t.AddLinearAcceleration(dprec.Vec3Prod(force, t.invMass)) +func (t AccelerationTarget) ApplyForce(force dprec.Vec3) { + t.body.applyForce(force) } // AngularAcceleration returns the angular acceleration that has been // accumulated on the target so far. -func (t *AccelerationTarget) AngularAcceleration() dprec.Vec3 { - return t.angularAcceleration +func (t AccelerationTarget) AngularAcceleration() dprec.Vec3 { + return t.body.angularAcceleration } // AddAngularAcceleration accumulates the specified angular acceleration on // the target. -func (t *AccelerationTarget) AddAngularAcceleration(acceleration dprec.Vec3) { - t.angularAcceleration = dprec.Vec3Sum(t.angularAcceleration, acceleration) +func (t AccelerationTarget) AddAngularAcceleration(acceleration dprec.Vec3) { + t.body.addAngularAcceleration(acceleration) } // ApplyTorque accumulates the angular acceleration that results from the // specified torque acting on the target. -func (t *AccelerationTarget) ApplyTorque(torque dprec.Vec3) { - t.AddAngularAcceleration(dprec.Mat3Vec3Prod(t.invInertia, torque)) +func (t AccelerationTarget) ApplyTorque(torque dprec.Vec3) { + t.body.applyTorque(torque) } // ApplyOffsetForce accumulates the linear and angular acceleration that // result from the specified force acting on the target at the specified // offset from its center of mass. -func (t *AccelerationTarget) ApplyOffsetForce(offset, force dprec.Vec3) { - t.ApplyForce(force) - t.ApplyTorque(dprec.Vec3Cross(offset, force)) +func (t AccelerationTarget) ApplyOffsetForce(offset, force dprec.Vec3) { + t.body.applyOffsetForce(offset, force) } // AccelerationContext describes the surrounding medium at the location of @@ -171,6 +148,10 @@ func (t *AccelerationTarget) ApplyOffsetForce(offset, force dprec.Vec3) { // its own. type AccelerationContext struct { + // DeltaSeconds is the time step of the simulation, in seconds. Use this + // in case of any time-dependent effects, like drag. + DeltaSeconds float64 + // MediumVelocity is the velocity of the medium in world space, in m/s. MediumVelocity dprec.Vec3 @@ -190,5 +171,5 @@ type AccelerationSolver interface { // Implementations must not retain the target, since it is only valid for // the duration of the call, and must not mutate any state that other // contributors observe, since the evaluation order is unspecified. - ApplyAcceleration(ctx AccelerationContext, target *AccelerationTarget) + ApplyAcceleration(ctx AccelerationContext, target AccelerationTarget) } diff --git a/game/physics/body.go b/game/physics/body.go index c63c8582..0b0912b6 100644 --- a/game/physics/body.go +++ b/game/physics/body.go @@ -332,6 +332,9 @@ type bodyState struct { invMass float64 invInertia dprec.Mat3 + linearAcceleration dprec.Vec3 + angularAcceleration dprec.Vec3 + linearVelocity dprec.Vec3 angularVelocity dprec.Vec3 @@ -339,15 +342,56 @@ type bodyState struct { rotation dprec.Quat } -func (s bodyState) IsActive() bool { +func (s bodyState) isValid() bool { return s.revision%2 == 1 // only odd revisions are valid } -func (b *bodyState) AddVelocity(amount dprec.Vec3) { +func (s *bodyState) mass() float64 { + return 1.0 / s.invMass +} + +func (s *bodyState) inertia() dprec.Mat3 { + return dprec.InverseMat3(s.invInertia) +} + +func (b *bodyState) addLinearAcceleration(amount dprec.Vec3) { + b.linearAcceleration = dprec.Vec3Sum(b.linearAcceleration, amount) +} + +func (b *bodyState) addAngularAcceleration(amount dprec.Vec3) { + b.angularAcceleration = dprec.Vec3Sum(b.angularAcceleration, amount) +} + +func (b *bodyState) clampLinearAcceleration(max float64) { + if b.linearAcceleration.SqrLength() > max*max { + b.linearAcceleration = dprec.ResizedVec3(b.linearAcceleration, max) + } +} + +func (b *bodyState) clampAngularAcceleration(max float64) { + if b.angularAcceleration.SqrLength() > max*max { + b.angularAcceleration = dprec.ResizedVec3(b.angularAcceleration, max) + } +} + +func (b *bodyState) applyForce(force dprec.Vec3) { + b.addLinearAcceleration(dprec.Vec3Prod(force, b.invMass)) +} + +func (b *bodyState) applyTorque(torque dprec.Vec3) { + b.addAngularAcceleration(dprec.Mat3Vec3Prod(b.invInertia, torque)) +} + +func (b *bodyState) applyOffsetForce(force dprec.Vec3, offset dprec.Vec3) { + b.applyForce(force) + b.applyTorque(dprec.Vec3Cross(offset, force)) +} + +func (b *bodyState) addLinearVelocity(amount dprec.Vec3) { b.linearVelocity = dprec.Vec3Sum(b.linearVelocity, amount) } -func (b *bodyState) AddAngularVelocity(amount dprec.Vec3) { +func (b *bodyState) addAngularVelocity(amount dprec.Vec3) { b.angularVelocity = dprec.Vec3Sum(b.angularVelocity, amount) } diff --git a/game/physics/scene.go b/game/physics/scene.go index c808ae9d..ec1f8ac7 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -33,10 +33,6 @@ type Scene struct { props []propState - bodies []bodyState - bodyAccelerationTargets []AccelerationTarget - bodyConstraintPlaceholders []solver.Placeholder - dbConstraints []dbConstraintState sbCollisionConstraints []SBConstraint @@ -66,6 +62,7 @@ type Scene struct { globalAccelerators []globalAcceleratorState bodyAccelerators []bodyAcceleratorState soloConstraints []soloConstraintState + bodies []bodyState } func NewScene() *Scene { @@ -89,10 +86,6 @@ func NewScene() *Scene { props: make([]propState, 0, 1024), - bodies: make([]bodyState, 0, 64), - bodyAccelerationTargets: make([]AccelerationTarget, 0, 64), - bodyConstraintPlaceholders: make([]solver.Placeholder, 0, 64), - dbConstraints: make([]dbConstraintState, 0, 64), collisionSet: make(placement3d.ContactList, 0, 128), @@ -114,35 +107,10 @@ func NewScene() *Scene { globalAccelerators: make([]globalAcceleratorState, 0), bodyAccelerators: make([]bodyAcceleratorState, 0), soloConstraints: make([]soloConstraintState, 0), + bodies: make([]bodyState, 0), } } -// Delete releases resources allocated by this scene. Users should not call -// any further methods on this object. -func (s *Scene) Delete() { - s.props = nil - - s.bodies = nil - s.bodyAccelerationTargets = nil - s.bodyConstraintPlaceholders = nil - - s.dbConstraints = nil - - s.sbCollisionConstraints = nil - s.sbCollisionSolvers = nil - - s.dbCollisionConstraints = nil - s.dbCollisionSolvers = nil - - s.collisionSet = nil - - s.oldSBCollisions = nil - s.newSBCollisions = nil - - s.oldDBCollisions = nil - s.newDBCollisions = nil -} - // MediumSolver returns the solver that is used to calculate the medium // properties of the scene. // @@ -364,8 +332,6 @@ func (s *Scene) CheckSegmentIntersection(segment shape3d.Segment, mask uint32) ( // } func (s *Scene) runSimulation(elapsedSeconds float64) { - // TODO: body -> acceleration targets -> impulse targets -> positioning targets -> body -> check for collisions (maybe reposition to first) - if elapsedSeconds > 0.0001 { s.applyAcceleration(elapsedSeconds) s.applyImpulses(elapsedSeconds) @@ -377,50 +343,35 @@ func (s *Scene) runSimulation(elapsedSeconds float64) { func (s *Scene) applyAcceleration(elapsedSeconds float64) { defer metric.BeginRegion("acceleration").End() - s.prepareAccelerationTargets() - s.applyBodyAccelerators() - s.applyAreaAccelerators() - s.applyGlobalAccelerators() - // s.applyAerodynamicAccelerations() - s.applyAccelerationTargets(elapsedSeconds) -} -func (s *Scene) prepareAccelerationTargets() { s.eachBodyState(func(index int, body *bodyState) { - s.bodyAccelerationTargets[index] = newAccelerationTarget( - body.invMass, - RotatedMomentOfInertia(body.invInertia, body.rotation), - body.position, - body.rotation, - body.linearVelocity, - body.angularVelocity, - ) - }) -} - -func (s *Scene) applyBodyAccelerators() { - // TODO -} + // Create acceleration context. + ctx := AccelerationContext{ + DeltaSeconds: elapsedSeconds, + MediumVelocity: s.mediumSolver.Velocity(body.position), + MediumDensity: s.mediumSolver.Density(body.position), + } + target := newAccelerationTarget(body) -func (s *Scene) applyAreaAccelerators() { - // TODO -} + // Reset accumulated accelerations. + body.linearAcceleration = dprec.ZeroVec3() + body.angularAcceleration = dprec.ZeroVec3() -func (s *Scene) applyGlobalAccelerators() { - s.eachBodyState(func(index int, _ *bodyState) { - target := &s.bodyAccelerationTargets[index] - s.eachGlobalAccelerator(func(_ int, accelerator *globalAcceleratorState) { - if accelerator.isEnabled { - // TODO: Consider caching the following calculation, especially - // if the medium solver is expensive to compute. - position := target.Position() - ctx := AccelerationContext{ - MediumVelocity: s.mediumSolver.Velocity(position), - MediumDensity: s.mediumSolver.Density(position), - } - accelerator.solver.ApplyAcceleration(ctx, target) - } + // Apply global accelerators. + s.eachEnabledGlobalAccelerator(func(_ int, accelerator *globalAcceleratorState) { + accelerator.solver.ApplyAcceleration(ctx, target) }) + + // Apply body accelerators. + // TODO: Implement body accelerators. + + // Constrain the accumulated accelerations to the maximum allowed values. + body.clampLinearAcceleration(s.maxLinearAcceleration) + body.clampAngularAcceleration(s.maxAngularAcceleration) + + // Update the body's velocity based on the accumulated accelerations. + body.addLinearVelocity(dprec.Vec3Prod(body.linearAcceleration, elapsedSeconds)) + body.addAngularVelocity(dprec.Vec3Prod(body.angularAcceleration, elapsedSeconds)) }) } @@ -458,24 +409,6 @@ func (s *Scene) applyGlobalAccelerators() { // }) // } -func (s *Scene) applyAccelerationTargets(elapsedSeconds float64) { - s.eachBodyState(func(index int, body *bodyState) { - target := s.bodyAccelerationTargets[index] - - linearAcceleration := target.LinearAcceleration() - if linearAcceleration.Length() > s.maxLinearAcceleration { - linearAcceleration = dprec.ResizedVec3(linearAcceleration, s.maxLinearAcceleration) - } - body.AddVelocity(dprec.Vec3Prod(linearAcceleration, elapsedSeconds)) - - angularAcceleration := target.AngularAcceleration() - if angularAcceleration.Length() > s.maxAngularAcceleration { - angularAcceleration = dprec.ResizedVec3(angularAcceleration, s.maxAngularAcceleration) - } - body.AddAngularVelocity(dprec.Vec3Prod(angularAcceleration, elapsedSeconds)) - }) -} - func (s *Scene) applyImpulses(elapsedSeconds float64) { defer metric.BeginRegion("impulses").End() @@ -948,6 +881,15 @@ func (s *Scene) eachGlobalAccelerator(cb func(index int, accelerator *globalAcce } } +func (s *Scene) eachEnabledGlobalAccelerator(cb func(index int, accelerator *globalAcceleratorState)) { + for i := range s.globalAccelerators { + accelerator := &s.globalAccelerators[i] + if accelerator.isValid() && accelerator.isEnabled { + cb(i, accelerator) + } + } +} + func (s *Scene) allocateBodyAccelerator() (int32, *bodyAcceleratorState) { var index int32 if s.freeBodyAcceleratorIndices.IsEmpty() { diff --git a/game/physics/solver_gravity.go b/game/physics/solver_gravity.go index 55528c77..0277d8cd 100644 --- a/game/physics/solver_gravity.go +++ b/game/physics/solver_gravity.go @@ -62,7 +62,7 @@ func (s *GravitySolver) SetMagnitude(magnitude float64) *GravitySolver { // ApplyAcceleration accumulates the gravitational acceleration on the // target. The medium of the context is not taken into account, meaning that // buoyancy is not modeled. -func (s *GravitySolver) ApplyAcceleration(ctx AccelerationContext, target *AccelerationTarget) { +func (s *GravitySolver) ApplyAcceleration(ctx AccelerationContext, target AccelerationTarget) { target.AddLinearAcceleration(s.acceleration) } From 0d94c50b158e05e3e8564159b2770c976e7af4c3 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 18:57:53 +0300 Subject: [PATCH 18/85] Further improvements to code --- game/physics/body.go | 18 +++---- game/physics/scene.go | 120 ++++++++++-------------------------------- 2 files changed, 36 insertions(+), 102 deletions(-) diff --git a/game/physics/body.go b/game/physics/body.go index 0b0912b6..94c9f9d0 100644 --- a/game/physics/body.go +++ b/game/physics/body.go @@ -395,29 +395,29 @@ func (b *bodyState) addAngularVelocity(amount dprec.Vec3) { b.angularVelocity = dprec.Vec3Sum(b.angularVelocity, amount) } -func (b *bodyState) ClampVelocity(max float64) { +func (b *bodyState) clampLinearVelocity(max float64) { if b.linearVelocity.SqrLength() > max*max { b.linearVelocity = dprec.ResizedVec3(b.linearVelocity, max) } } -func (b *bodyState) ClampAngularVelocity(max float64) { +func (b *bodyState) clampAngularVelocity(max float64) { if b.angularVelocity.SqrLength() > max*max { b.angularVelocity = dprec.ResizedVec3(b.angularVelocity, max) } } -func (b *bodyState) Translate(offset dprec.Vec3) { +func (b *bodyState) translate(offset dprec.Vec3) { b.position = dprec.Vec3Sum(b.position, offset) } -func (b *bodyState) VectorRotate(vector dprec.Vec3) { +func (b *bodyState) rotate(quat dprec.Quat) { + b.rotation = dprec.UnitQuat(dprec.QuatProd(quat, b.rotation)) +} + +func (b *bodyState) rotateVector(vector dprec.Vec3) { const angularEpsilon = float64(0.00001) if radians := vector.Length(); dprec.Abs(radians) > angularEpsilon { - b.Rotate(dprec.RotationQuat(dprec.Radians(radians), vector)) + b.rotate(dprec.RotationQuat(dprec.Radians(radians), vector)) } } - -func (b *bodyState) Rotate(quat dprec.Quat) { - b.rotation = dprec.UnitQuat(dprec.QuatProd(quat, b.rotation)) -} diff --git a/game/physics/scene.go b/game/physics/scene.go index ec1f8ac7..cc83ff55 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -157,6 +157,12 @@ func (s *Scene) SoloConstraints() SoloConstraintView { } } +func (s *Scene) Bodies() BodyView { + return BodyView{ + scene: s, + } +} + /////// OLD BELOW ------------ (TODO: DELETE COMMENT) // SubscribeSingleBodyCollision registers a callback that is invoked when a body @@ -225,8 +231,6 @@ func (s *Scene) NextCollisionRejectGroup() uint32 { // CreateProp creates a new static Prop. A prop is an object // that is static and rarely removed. func (s *Scene) CreateProp(info PropInfo) { - // TODO: createProp(s, info) - // objectID := s.shapeScene.CreateObject(placement3d.ObjectInfo[internalRef]{ // Position: info.Position, // Rotation: info.Rotation, @@ -235,18 +239,6 @@ func (s *Scene) CreateProp(info PropInfo) { // isProp: true, // }, // }) - // for _, sphere := range info.CollisionSpheres { - // s.shapeScene.AttachSphere(objectID, placement3d.SphereInfo[struct{}]{ - // ShapeInfo: placement3d.ShapeInfo[struct{}]{}, - // Sphere: sphere, - // }) - // } - // for _, box := range info.CollisionBoxes { - // s.shapeScene.AttachBox(objectID, placement3d.BoxInfo[struct{}]{ - // ShapeInfo: placement3d.ShapeInfo[struct{}]{}, - // Box: box, - // }) - // } for _, mesh := range info.CollisionMeshes { propIndex := uint32(len(s.props)) @@ -337,6 +329,7 @@ func (s *Scene) runSimulation(elapsedSeconds float64) { s.applyImpulses(elapsedSeconds) s.applyMotion(elapsedSeconds) s.applyNudges(elapsedSeconds) + s.applyPlacement() s.detectCollisions() } } @@ -412,22 +405,6 @@ func (s *Scene) applyAcceleration(elapsedSeconds float64) { func (s *Scene) applyImpulses(elapsedSeconds float64) { defer metric.BeginRegion("impulses").End() - s.eachBodyState(func(i int, body *bodyState) { - placeholder := &s.bodyConstraintPlaceholders[i] - s.initPlaceholder(placeholder, body) - }) - - s.eachDBConstraintState(func(_ int, constraint *dbConstraintState) { - if s.resolveBodyState(constraint.primary.reference) == nil { - deleteDBConstraint(s, constraint.reference) - return - } - if s.resolveBodyState(constraint.secondary.reference) == nil { - deleteDBConstraint(s, constraint.reference) - return - } - }) - s.eachDBConstraintState(func(_ int, constraint *dbConstraintState) { target := &s.bodyConstraintPlaceholders[constraint.primary.reference.Index] source := &s.bodyConstraintPlaceholders[constraint.secondary.reference.Index] @@ -449,7 +426,7 @@ func (s *Scene) applyImpulses(elapsedSeconds float64) { }) }) - for i := 0; i < ImpulseIterationCount; i++ { + for range ImpulseIterationCount { s.eachDBConstraintState(func(_ int, constraint *dbConstraintState) { target := &s.bodyConstraintPlaceholders[constraint.primary.reference.Index] source := &s.bodyConstraintPlaceholders[constraint.secondary.reference.Index] @@ -471,45 +448,26 @@ func (s *Scene) applyImpulses(elapsedSeconds float64) { }) }) } - - s.eachBodyState(func(i int, body *bodyState) { - placeholder := &s.bodyConstraintPlaceholders[i] - s.deinitPlaceholder(placeholder, body) - }) } func (s *Scene) applyMotion(elapsedSeconds float64) { defer metric.BeginRegion("motion").End() - s.eachBodyState(func(_ int, body *bodyState) { - body.ClampVelocity(s.maxLinearVelocity) - body.ClampAngularVelocity(s.maxAngularVelocity) - deltaPosition := dprec.Vec3Prod(body.linearVelocity, elapsedSeconds) - body.Translate(deltaPosition) - deltaRotation := dprec.Vec3Prod(body.angularVelocity, elapsedSeconds) - body.VectorRotate(deltaRotation) + s.eachBodyState(func(_ int, body *bodyState) { + // Clamp the velocity to the maximum allowed values. + body.clampLinearVelocity(s.maxLinearVelocity) + body.clampAngularVelocity(s.maxAngularVelocity) - s.collisionScene.SetObjectTransform(body.objectID, shape3d.Transform{ - Translation: body.position, - Rotation: shape3d.RotationFromQuat(body.rotation), - }) + // Apply the velocity to the body's position and rotation. + body.translate(dprec.Vec3Prod(body.linearVelocity, elapsedSeconds)) + body.rotateVector(dprec.Vec3Prod(body.angularVelocity, elapsedSeconds)) }) } func (s *Scene) applyNudges(elapsedSeconds float64) { defer metric.BeginRegion("nudges").End() - // TODO: Use Grow instead - s.bodyConstraintPlaceholders = s.bodyConstraintPlaceholders[:0] - for i := range s.bodies { - placeholder := solver.Placeholder{} - if body := &s.bodies[i]; body.IsActive() { - s.initPlaceholder(&placeholder, body) - } - s.bodyConstraintPlaceholders = append(s.bodyConstraintPlaceholders, placeholder) - } - - for i := 0; i < NudgeIterationCount; i++ { + for range NudgeIterationCount { for _, constraint := range s.dbConstraints { if !constraint.IsActive() { continue @@ -541,13 +499,18 @@ func (s *Scene) applyNudges(elapsedSeconds float64) { constraint.logic.ApplyNudges(ctx) } } +} - for i := range s.bodies { - placeholder := s.bodyConstraintPlaceholders[i] - if body := &s.bodies[i]; body.IsActive() { - s.deinitPlaceholder(&placeholder, body) - } - } +func (s *Scene) applyPlacement() { + defer metric.BeginRegion("placement").End() + + s.eachBodyState(func(_ int, body *bodyState) { + // Update the collision scene with the new position and rotation of the body. + s.collisionScene.SetObjectTransform(body.objectID, shape3d.Transform{ + Translation: body.position, + Rotation: shape3d.RotationFromQuat(body.rotation), + }) + }) } func (s *Scene) detectCollisions() { @@ -828,35 +791,6 @@ func (s *Scene) resolveBodyState(reference indexReference) *bodyState { return state } -func (s *Scene) initPlaceholder(placeholder *solver.Placeholder, body *bodyState) { - placeholder.Init(solver.PlaceholderState{ - Mass: body.mass, - MomentOfInertia: body.momentOfInertia, - LinearVelocity: body.linearVelocity, - AngularVelocity: body.angularVelocity, - Position: body.position, - Rotation: body.rotation, - }) -} - -func (s *Scene) deinitPlaceholder(placeholder *solver.Placeholder, body *bodyState) { - body.linearVelocity = placeholder.LinearVelocity() - body.angularVelocity = placeholder.AngularVelocity() - body.position = placeholder.Position() - body.rotation = placeholder.Rotation() - - s.collisionScene.SetObjectTransform(body.objectID, shape3d.Transform{ - Translation: body.position, - Rotation: shape3d.RotationFromQuat(body.rotation), - }) -} - -func (s *Scene) Bodies() BodyView { - return BodyView{ - scene: s, - } -} - func (s *Scene) allocateGlobalAccelerator() (int32, *globalAcceleratorState) { var index int32 if s.freeGlobalAcceleratorIndices.IsEmpty() { From 53120fd3b87c0a60d313cf5a8d8733116bc7b2c0 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 19:03:04 +0300 Subject: [PATCH 19/85] Bug fix --- game/physics/acceleration.go | 12 ++---------- game/physics/body.go | 2 +- 2 files changed, 3 insertions(+), 11 deletions(-) diff --git a/game/physics/acceleration.go b/game/physics/acceleration.go index 94e369f1..b704fa6c 100644 --- a/game/physics/acceleration.go +++ b/game/physics/acceleration.go @@ -16,16 +16,8 @@ type AccelerationTarget struct { body *bodyState } -// newAccelerationTarget creates a new [AccelerationTarget] with the specified -// state. -// -// The invMass and invInertia parameters are the reciprocals of the mass and -// of the moment of inertia tensor respectively. A value of zero (or a zero -// matrix) represents an immovable object of infinite mass or inertia. -// -// The inverse moment of inertia has to be expressed in world space. Use -// [RotatedMomentOfInertia] with the object's rotation to bring a tensor that -// is expressed in local space into world space before inverting it. +// newAccelerationTarget creates a new [AccelerationTarget] backed by the +// specified body state. func newAccelerationTarget(body *bodyState) AccelerationTarget { return AccelerationTarget{ body: body, diff --git a/game/physics/body.go b/game/physics/body.go index 94c9f9d0..01a38300 100644 --- a/game/physics/body.go +++ b/game/physics/body.go @@ -382,7 +382,7 @@ func (b *bodyState) applyTorque(torque dprec.Vec3) { b.addAngularAcceleration(dprec.Mat3Vec3Prod(b.invInertia, torque)) } -func (b *bodyState) applyOffsetForce(force dprec.Vec3, offset dprec.Vec3) { +func (b *bodyState) applyOffsetForce(offset, force dprec.Vec3) { b.applyForce(force) b.applyTorque(dprec.Vec3Cross(offset, force)) } From 0cbe3fcd67aa4c2b2fb3647719a11cb17be83b33 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 21:38:24 +0300 Subject: [PATCH 20/85] More code changes in new direction --- game/physics/body.go | 17 +++--- game/physics/constraint.go | 97 ++++++++++++++++++++++++++++++ game/physics/constraint_pair.go | 75 +++++++++++++++++++++++ game/physics/constraint_solo.go | 4 +- game/physics/index.go | 23 ------- game/physics/prop.go | 5 +- game/physics/scene.go | 57 ++++++++++-------- game/physics/solver/change.go | 12 ---- game/physics/solver/constraint.go | 43 ------------- game/physics/solver/placeholder.go | 88 --------------------------- game/physics/util.go | 14 +++++ 11 files changed, 227 insertions(+), 208 deletions(-) create mode 100644 game/physics/constraint.go create mode 100644 game/physics/constraint_pair.go delete mode 100644 game/physics/index.go delete mode 100644 game/physics/solver/constraint.go delete mode 100644 game/physics/solver/placeholder.go create mode 100644 game/physics/util.go diff --git a/game/physics/body.go b/game/physics/body.go index 01a38300..91977715 100644 --- a/game/physics/body.go +++ b/game/physics/body.go @@ -19,7 +19,7 @@ type BodyView struct { } func (v BodyView) Create(position dprec.Vec3, rotation dprec.Quat) BodyID { - index := v.scene.allocateBody() + index, body := v.scene.allocateBody() objectID := v.scene.collisionScene.CreateObject(placement3d.ObjectInfo[bodyData]{ Position: opt.V(position), @@ -29,7 +29,6 @@ func (v BodyView) Create(position dprec.Vec3, rotation dprec.Quat) BodyID { }, }) - body := &v.scene.bodies[index] *body = bodyState{ objectID: objectID, revision: body.revision + 1, // progress revision to valid (odd) value @@ -63,7 +62,10 @@ func (v BodyView) Delete(id BodyID) { for body.firstSoloConstraintIndex != nilIndex { soloConstraintView.Delete(soloConstraintView.idFromIndex(body.firstSoloConstraintIndex)) } - // TODO: delete pair constraints as well. + pairConstraintView := v.scene.PairConstraints() + for body.firstPairConstraintIndex != nilIndex { + pairConstraintView.Delete(pairConstraintView.idFromIndex(body.firstPairConstraintIndex)) + } *body = bodyState{ objectID: placement3d.InvalidObjectID, @@ -296,6 +298,8 @@ func (h BodyHandle) DetachCollisionShape(shapeID CollisionShapeID) { h.view.DetachCollisionShape(h.id, shapeID) } +// TODO: Relocate collision-related code to separate file. + type CollisionShapeID struct { bodyID BodyID shapeID placement3d.ShapeID @@ -414,10 +418,3 @@ func (b *bodyState) translate(offset dprec.Vec3) { func (b *bodyState) rotate(quat dprec.Quat) { b.rotation = dprec.UnitQuat(dprec.QuatProd(quat, b.rotation)) } - -func (b *bodyState) rotateVector(vector dprec.Vec3) { - const angularEpsilon = float64(0.00001) - if radians := vector.Length(); dprec.Abs(radians) > angularEpsilon { - b.rotate(dprec.RotationQuat(dprec.Radians(radians), vector)) - } -} diff --git a/game/physics/constraint.go b/game/physics/constraint.go new file mode 100644 index 00000000..88f3d0cc --- /dev/null +++ b/game/physics/constraint.go @@ -0,0 +1,97 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +type Impulse struct { + Linear dprec.Vec3 + Angular dprec.Vec3 +} + +type Nudge struct { + Linear dprec.Vec3 + Angular dprec.Vec3 +} + +type ConstraintTarget struct { + body *bodyState +} + +func newConstraintTarget(body *bodyState) ConstraintTarget { + return ConstraintTarget{ + body: body, + } +} + +func (t ConstraintTarget) InverseMass() float64 { + return t.body.invMass +} + +func (t ConstraintTarget) Mass() float64 { + return t.body.mass() +} + +func (t ConstraintTarget) InverseInertia() dprec.Mat3 { + return t.body.invInertia +} + +func (t ConstraintTarget) Inertia() dprec.Mat3 { + return t.body.inertia() +} + +func (t ConstraintTarget) LinearVelocity() dprec.Vec3 { + return t.body.linearVelocity +} + +func (t ConstraintTarget) SetLinearVelocity(velocity dprec.Vec3) { + t.body.linearVelocity = velocity +} + +func (t ConstraintTarget) AddLinearVelocity(delta dprec.Vec3) { + t.body.addLinearVelocity(delta) +} + +func (t ConstraintTarget) AngularVelocity() dprec.Vec3 { + return t.body.angularVelocity +} + +func (t ConstraintTarget) SetAngularVelocity(velocity dprec.Vec3) { + t.body.angularVelocity = velocity +} + +func (t ConstraintTarget) AddAngularVelocity(delta dprec.Vec3) { + t.body.addAngularVelocity(delta) +} + +func (t ConstraintTarget) ApplyImpulse(impulse Impulse) { + t.body.addLinearVelocity(dprec.Vec3Prod(impulse.Linear, t.body.invMass)) + t.body.addAngularVelocity(dprec.Mat3Vec3Prod(t.body.invInertia, impulse.Angular)) +} + +func (t ConstraintTarget) Position() dprec.Vec3 { + return t.body.position +} + +func (t ConstraintTarget) SetPosition(position dprec.Vec3) { + t.body.position = position +} + +func (t ConstraintTarget) Translate(delta dprec.Vec3) { + t.body.translate(delta) +} + +func (t ConstraintTarget) Rotation() dprec.Quat { + return t.body.rotation +} + +func (t ConstraintTarget) SetRotation(rotation dprec.Quat) { + t.body.rotation = rotation +} + +func (t ConstraintTarget) Rotate(rotation dprec.Quat) { + t.body.rotate(rotation) +} + +func (t ConstraintTarget) ApplyNudge(nudge Nudge) { + t.body.translate(dprec.Vec3Prod(nudge.Linear, t.body.invMass)) + t.body.rotate(QuatFromVector(dprec.Mat3Vec3Prod(t.body.invInertia, nudge.Angular))) +} diff --git a/game/physics/constraint_pair.go b/game/physics/constraint_pair.go new file mode 100644 index 00000000..20b555ae --- /dev/null +++ b/game/physics/constraint_pair.go @@ -0,0 +1,75 @@ +package physics + +type PairConstraintContext struct { + DeltaSeconds float64 + ImpulseBeta float64 + NudgeBeta float64 + PrimaryTarget ConstraintTarget + SecondaryTarget ConstraintTarget +} + +type PairConstraintSolver interface { + Reset(ctx PairConstraintContext) + + ApplyImpulses(ctx PairConstraintContext) + + ApplyNudges(ctx PairConstraintContext) +} + +type PairConstraintID struct { + index int32 + revision int32 +} + +var NilPairConstraintID = PairConstraintID{} + +type PairConstraintView struct { + scene *Scene +} + +func (v PairConstraintView) Create(primaryID, secondaryID BodyID, solver PairConstraintSolver) PairConstraintID { + panic("TODO") +} + +func (v PairConstraintView) Delete(id PairConstraintID) { + panic("TODO") +} + +func (v PairConstraintView) idFromIndex(index int32) PairConstraintID { + state := &v.scene.pairConstraints[index] + return PairConstraintID{ + index: index, + revision: state.revision, + } +} + +func (v PairConstraintView) resolve(id PairConstraintID, required bool) *pairConstraintState { + if id.revision == 0 { + if required { + panic("invalid pair constraint ID") + } + return nil + } + constraint := &v.scene.pairConstraints[id.index] + if constraint.revision != id.revision { + if required { + panic("invalid pair constraint ID") + } + return nil + } + return constraint +} + +type PairConstraintHandle struct { + view PairConstraintView + id PairConstraintID +} + +type pairConstraintState struct { + solver PairConstraintSolver + revision int32 + primaryBodyIndex int32 + secondaryBodyIndex int32 + nextIndex int32 + isEnabled bool +} diff --git a/game/physics/constraint_solo.go b/game/physics/constraint_solo.go index 5a85871e..90cb6dea 100644 --- a/game/physics/constraint_solo.go +++ b/game/physics/constraint_solo.go @@ -1,7 +1,5 @@ package physics -import "github.com/mokiat/lacking/game/physics/solver" - // SoloConstraintContext contains the information that a [SoloConstraintSolver] // needs in order to process the single body it acts upon during a physics // simulation step. @@ -21,7 +19,7 @@ type SoloConstraintContext struct { // Target is the placeholder representation of the body that is // constrained. - Target *solver.Placeholder // TODO: use package-local ImpulseTarget instead of Placeholder + Target ConstraintTarget } // SoloConstraintSolver implements the mathematical logic that enforces a diff --git a/game/physics/index.go b/game/physics/index.go deleted file mode 100644 index ed35e70d..00000000 --- a/game/physics/index.go +++ /dev/null @@ -1,23 +0,0 @@ -package physics - -import "fmt" - -func newIndexReference(index, revision uint32) indexReference { - return indexReference{ - Index: index, - Revision: revision, - } -} - -type indexReference struct { - Index uint32 - Revision uint32 -} - -func (r indexReference) IsValid() bool { - return r.Revision > 0 -} - -func (r indexReference) String() string { - return fmt.Sprintf("%d:%d", r.Index, r.Revision) -} diff --git a/game/physics/prop.go b/game/physics/prop.go index dbcbab22..398bef8e 100644 --- a/game/physics/prop.go +++ b/game/physics/prop.go @@ -32,7 +32,6 @@ func (p Prop) Name() string { } type propState struct { - reference indexReference - meshID placement3d.MeshID - name string + meshID placement3d.MeshID + revision int32 } diff --git a/game/physics/scene.go b/game/physics/scene.go index cc83ff55..375aece6 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -33,8 +33,6 @@ type Scene struct { props []propState - dbConstraints []dbConstraintState - sbCollisionConstraints []SBConstraint sbCollisionSolvers []constraint.Collision @@ -57,11 +55,13 @@ type Scene struct { freeGlobalAcceleratorIndices *ds.Stack[int32] freeBodyAcceleratorIndices *ds.Stack[int32] freeSoloConstraintIndices *ds.Stack[int32] + freePairConstraintIndices *ds.Stack[int32] freeBodyIndices *ds.Stack[int32] globalAccelerators []globalAcceleratorState bodyAccelerators []bodyAcceleratorState soloConstraints []soloConstraintState + pairConstraints []pairConstraintState bodies []bodyState } @@ -86,8 +86,6 @@ func NewScene() *Scene { props: make([]propState, 0, 1024), - dbConstraints: make([]dbConstraintState, 0, 64), - collisionSet: make(placement3d.ContactList, 0, 128), oldSBCollisions: make(map[sbCollisionPair]struct{}, 32), @@ -102,11 +100,13 @@ func NewScene() *Scene { freeGlobalAcceleratorIndices: ds.EmptyStack[int32](), freeBodyAcceleratorIndices: ds.EmptyStack[int32](), freeSoloConstraintIndices: ds.EmptyStack[int32](), + freePairConstraintIndices: ds.EmptyStack[int32](), freeBodyIndices: ds.EmptyStack[int32](), globalAccelerators: make([]globalAcceleratorState, 0), bodyAccelerators: make([]bodyAcceleratorState, 0), soloConstraints: make([]soloConstraintState, 0), + pairConstraints: make([]pairConstraintState, 0), bodies: make([]bodyState, 0), } } @@ -157,6 +157,12 @@ func (s *Scene) SoloConstraints() SoloConstraintView { } } +func (s *Scene) PairConstraints() PairConstraintView { + return PairConstraintView{ + scene: s, + } +} + func (s *Scene) Bodies() BodyView { return BodyView{ scene: s, @@ -259,12 +265,6 @@ func (s *Scene) CreateProp(info PropInfo) { } } -// CreateDoubleBodyConstraint creates a new physics constraint that acts on -// two bodies and enables it for this scene. -func (s *Scene) CreateDoubleBodyConstraint(primary, secondary Body, logic solver.PairConstraint) DBConstraint { - return createDBConstraint(s, logic, primary, secondary) -} - // Update runs a single physics iteration. This method should be called with // fixed elapsed times, otherwise the physics may break. func (s *Scene) Update(elapsedTime time.Duration) { @@ -460,7 +460,7 @@ func (s *Scene) applyMotion(elapsedSeconds float64) { // Apply the velocity to the body's position and rotation. body.translate(dprec.Vec3Prod(body.linearVelocity, elapsedSeconds)) - body.rotateVector(dprec.Vec3Prod(body.angularVelocity, elapsedSeconds)) + body.rotate(QuatFromVector(dprec.Vec3Prod(body.angularVelocity, elapsedSeconds))) }) } @@ -854,25 +854,30 @@ func (s *Scene) releaseSoloConstraint(index int32) { s.freeSoloConstraintIndices.Push(index) } -func (s *Scene) verifySoloConstraintID(id SoloConstraintID) { - if id.revision == 0 { - panic("invalid solo constraint ID") - } - constraint := &s.soloConstraints[id.index] - if constraint.revision != id.revision { - panic("invalid solo constraint ID") +func (s *Scene) allocatePairConstraint() (int32, *pairConstraintState) { + var index int32 + if s.freePairConstraintIndices.IsEmpty() { + index = int32(len(s.pairConstraints)) + s.pairConstraints = append(s.pairConstraints, pairConstraintState{}) + } else { + index = s.freePairConstraintIndices.Pop() } + return index, &s.pairConstraints[index] } -func (s *Scene) allocateBody() int32 { - if !s.freeBodyIndices.IsEmpty() { - return s.freeBodyIndices.Pop() +func (s *Scene) releasePairConstraint(index int32) { + s.freePairConstraintIndices.Push(index) +} + +func (s *Scene) allocateBody() (int32, *bodyState) { + var index int32 + if s.freeBodyIndices.IsEmpty() { + index = int32(len(s.bodies)) + s.bodies = append(s.bodies, bodyState{}) + } else { + index = s.freeBodyIndices.Pop() } - index := int32(len(s.bodies)) - s.bodies = append(s.bodies, bodyState{}) - s.bodyAccelerationTargets = append(s.bodyAccelerationTargets, AccelerationTarget{}) - s.bodyConstraintPlaceholders = append(s.bodyConstraintPlaceholders, solver.Placeholder{}) - return index + return index, &s.bodies[index] } func (s *Scene) releaseBody(index int32) { diff --git a/game/physics/solver/change.go b/game/physics/solver/change.go index 3d6ac7c8..80d1e0b8 100644 --- a/game/physics/solver/change.go +++ b/game/physics/solver/change.go @@ -1,22 +1,10 @@ package solver -import "github.com/mokiat/gomath/dprec" - -type Impulse struct { - Linear dprec.Vec3 - Angular dprec.Vec3 -} - type PairImpulse struct { Target Impulse Source Impulse } -type Nudge struct { - Linear dprec.Vec3 - Angular dprec.Vec3 -} - type PairNudge struct { Target Nudge Source Nudge diff --git a/game/physics/solver/constraint.go b/game/physics/solver/constraint.go deleted file mode 100644 index 5e884d78..00000000 --- a/game/physics/solver/constraint.go +++ /dev/null @@ -1,43 +0,0 @@ -package solver - -// Constraint represents the algorithm necessary to solve a single-object -// constraint. -type Constraint interface { - // Reset clears the internal cache state for this constraint solver. - // - // This is called at the start of every iteration. - Reset(ctx Context) - - // ApplyImpulses is called by the physics engine to instruct the solver - // to apply the necessary impulses to its object. - // - // This is called multiple times per iteration. - ApplyImpulses(ctx Context) - - // ApplyNudges is called by the physics engine to instruct the solver to - // apply the necessary nudges to its object. - // - // This is called multiple times per iteration. - ApplyNudges(ctx Context) -} - -// PairConstraint represents the algorithm necessary to solve -// a double-object constraint. -type PairConstraint interface { - // Reset clears the internal cache state for this constraint solver. - // - // This is called at the start of every iteration. - Reset(ctx PairContext) - - // ApplyImpulses is called by the physics engine to instruct the solver - // to apply the necessary impulses to its objects. - // - // This is called multiple times per iteration. - ApplyImpulses(ctx PairContext) - - // ApplyNudges is called by the physics engine to instruct the solver to - // apply the necessary nudges to its objects. - // - // This is called multiple times per iteration. - ApplyNudges(ctx PairContext) -} diff --git a/game/physics/solver/placeholder.go b/game/physics/solver/placeholder.go deleted file mode 100644 index e441659c..00000000 --- a/game/physics/solver/placeholder.go +++ /dev/null @@ -1,88 +0,0 @@ -package solver - -import "github.com/mokiat/gomath/dprec" - -type PlaceholderState struct { - Mass float64 - MomentOfInertia dprec.Mat3 - - LinearVelocity dprec.Vec3 - AngularVelocity dprec.Vec3 - - Position dprec.Vec3 - Rotation dprec.Quat -} - -type Placeholder struct { - inverseMass float64 - inverseMomentOfInertia dprec.Mat3 - - linearVelocity dprec.Vec3 - angularVelocity dprec.Vec3 - - position dprec.Vec3 - rotation dprec.Quat -} - -func (p *Placeholder) Init(state PlaceholderState) { - p.inverseMass = 1.0 / state.Mass - // TODO: First rotate the moment of inertia according to the object's rotation - p.inverseMomentOfInertia = dprec.InverseMat3(state.MomentOfInertia) - - p.linearVelocity = state.LinearVelocity - p.angularVelocity = state.AngularVelocity - - p.position = state.Position - p.rotation = state.Rotation -} - -func (p *Placeholder) LinearVelocity() dprec.Vec3 { - return p.linearVelocity -} - -func (p *Placeholder) SetLinearVelocity(velocity dprec.Vec3) { - p.linearVelocity = velocity -} - -func (p *Placeholder) AngularVelocity() dprec.Vec3 { - return p.angularVelocity -} - -func (p *Placeholder) SetAngularVelocity(velocity dprec.Vec3) { - p.angularVelocity = velocity -} - -func (p *Placeholder) ApplyImpulse(impulse Impulse) { - linearChange := dprec.Vec3Prod(impulse.Linear, p.inverseMass) - p.linearVelocity = dprec.Vec3Sum(p.linearVelocity, linearChange) - - angularChange := dprec.Mat3Vec3Prod(p.inverseMomentOfInertia, impulse.Angular) - p.angularVelocity = dprec.Vec3Sum(p.angularVelocity, angularChange) -} - -func (p *Placeholder) Position() dprec.Vec3 { - return p.position -} - -func (p *Placeholder) SetPosition(position dprec.Vec3) { - p.position = position -} - -func (p *Placeholder) Rotation() dprec.Quat { - return p.rotation -} - -func (p *Placeholder) SetRotation(rotation dprec.Quat) { - p.rotation = rotation -} - -func (p *Placeholder) ApplyNudge(nudge Nudge) { - linearChange := dprec.Vec3Prod(nudge.Linear, p.inverseMass) - p.position = dprec.Vec3Sum(p.position, linearChange) - - angularChange := dprec.Mat3Vec3Prod(p.inverseMomentOfInertia, nudge.Angular) - if radians := angularChange.Length(); radians > Epsilon { - rotationChange := dprec.RotationQuat(dprec.Radians(radians), angularChange) - p.rotation = dprec.UnitQuat(dprec.QuatProd(rotationChange, p.rotation)) - } -} diff --git a/game/physics/util.go b/game/physics/util.go new file mode 100644 index 00000000..1f9a0676 --- /dev/null +++ b/game/physics/util.go @@ -0,0 +1,14 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +func QuatFromVector(vector dprec.Vec3) dprec.Quat { + radians := vector.Length() + + const angularEpsilon = float64(0.00001) + if dprec.Abs(radians) < angularEpsilon { + return dprec.IdentityQuat() + } + + return dprec.RotationQuat(dprec.Radians(radians), vector) +} From 76096b1e6b98e2457b2a41fc6629267972101df1 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 21:54:53 +0300 Subject: [PATCH 21/85] More code changes --- game/physics/constraint_db.go | 103 -------------------- game/physics/constraint_pair.go | 164 +++++++++++++++++++++++++++++++- game/physics/constraint_solo.go | 5 + game/physics/scene.go | 103 +++++++++++--------- 4 files changed, 222 insertions(+), 153 deletions(-) delete mode 100644 game/physics/constraint_db.go diff --git a/game/physics/constraint_db.go b/game/physics/constraint_db.go deleted file mode 100644 index fd22363e..00000000 --- a/game/physics/constraint_db.go +++ /dev/null @@ -1,103 +0,0 @@ -package physics - -import "github.com/mokiat/lacking/game/physics/solver" - -var invalidDBConstraintState = &dbConstraintState{} - -// DBConstraint represents a restriction enforced on two bodies in conjunction. -type DBConstraint struct { - scene *Scene - reference indexReference -} - -// Enabled returns whether this constraint will be enforced. -func (c DBConstraint) Enabled() bool { - state := c.state() - return state.enabled -} - -// SetEnabled changes whether this constraint will be enforced. -func (c DBConstraint) SetEnabled(enabled bool) { - state := c.state() - state.enabled = enabled -} - -// Logic returns the constraint solver that will be used to enforce -// mathematically this constraint. -func (c DBConstraint) Logic() solver.PairConstraint { - state := c.state() - return state.logic -} - -// PrimaryBody returns the primary body on which this constraint -// acts. -func (c DBConstraint) PrimaryBody() Body { - state := c.state() - return state.primary -} - -// SecondaryBody returns the secondary body on which this constraint -// acts. -func (c DBConstraint) SecondaryBody() Body { - state := c.state() - return state.secondary -} - -// Delete removes this constraint. -func (c DBConstraint) Delete() { - deleteDBConstraint(c.scene, c.reference) -} - -func (c DBConstraint) state() *dbConstraintState { - index := c.reference.Index - state := &c.scene.dbConstraints[index] - if state.reference != c.reference { - return invalidDBConstraintState - } - return state -} - -type dbConstraintState struct { - reference indexReference - logic solver.PairConstraint - primary Body - secondary Body - enabled bool -} - -func (s dbConstraintState) IsActive() bool { - return s.reference.IsValid() && s.enabled -} - -func createDBConstraint(scene *Scene, logic solver.PairConstraint, primary, secondary Body) DBConstraint { - var freeIndex uint32 - if scene.freeDBConstraintIndices.IsEmpty() { - freeIndex = uint32(len(scene.dbConstraints)) - scene.dbConstraints = append(scene.dbConstraints, dbConstraintState{}) - } else { - freeIndex = scene.freeDBConstraintIndices.Pop() - } - - reference := newIndexReference(freeIndex, scene.nextRevision()) - scene.dbConstraints[freeIndex] = dbConstraintState{ - reference: reference, - logic: logic, - primary: primary, - secondary: secondary, - enabled: true, - } - return DBConstraint{ - scene: scene, - reference: reference, - } -} - -func deleteDBConstraint(scene *Scene, reference indexReference) { - index := reference.Index - state := &scene.dbConstraints[index] - if state.reference == reference { - state.reference = newIndexReference(index, 0) - state.logic = nil - scene.freeDBConstraintIndices.Push(index) - } -} diff --git a/game/physics/constraint_pair.go b/game/physics/constraint_pair.go index 20b555ae..356c485a 100644 --- a/game/physics/constraint_pair.go +++ b/game/physics/constraint_pair.go @@ -28,11 +28,128 @@ type PairConstraintView struct { } func (v PairConstraintView) Create(primaryID, secondaryID BodyID, solver PairConstraintSolver) PairConstraintID { - panic("TODO") + bodyView := v.scene.Bodies() + primaryBody := bodyView.resolve(primaryID, true) + secondaryBody := bodyView.resolve(secondaryID, true) + + index, constraint := v.scene.allocatePairConstraint() + + *constraint = pairConstraintState{ + solver: solver, + revision: constraint.revision + 1, // progress revision to valid (odd) value + primaryBodyIndex: primaryID.index, + secondaryBodyIndex: secondaryID.index, + primaryNextIndex: primaryBody.firstSoloConstraintIndex, + secondaryNextIndex: secondaryBody.firstSoloConstraintIndex, + isEnabled: true, + } + primaryBody.firstSoloConstraintIndex = index + secondaryBody.firstSoloConstraintIndex = index + + return PairConstraintID{ + index: index, + revision: constraint.revision, + } } func (v PairConstraintView) Delete(id PairConstraintID) { - panic("TODO") + constraint := v.resolve(id, true) + + // Unlink the constraint from the primary body. + primaryBody := &v.scene.bodies[constraint.primaryBodyIndex] + if primaryBody.firstPairConstraintIndex == constraint.primaryNextIndex { + primaryBody.firstPairConstraintIndex = constraint.primaryNextIndex + } else { + prevIndex := primaryBody.firstPairConstraintIndex + for prevIndex != -1 { + prevConstraint := &v.scene.pairConstraints[prevIndex] + if prevConstraint.primaryNextIndex == constraint.primaryNextIndex { + prevConstraint.primaryNextIndex = constraint.primaryNextIndex + break + } + prevIndex = prevConstraint.primaryNextIndex + } + } + + // Unlink the constraint from the secondary body. + secondaryBody := &v.scene.bodies[constraint.secondaryBodyIndex] + if secondaryBody.firstPairConstraintIndex == constraint.secondaryNextIndex { + secondaryBody.firstPairConstraintIndex = constraint.secondaryNextIndex + } else { + prevIndex := secondaryBody.firstPairConstraintIndex + for prevIndex != -1 { + prevConstraint := &v.scene.pairConstraints[prevIndex] + if prevConstraint.secondaryNextIndex == constraint.secondaryNextIndex { + prevConstraint.secondaryNextIndex = constraint.secondaryNextIndex + break + } + prevIndex = prevConstraint.secondaryNextIndex + } + } + + *constraint = pairConstraintState{ + solver: nil, // allow the solver to be garbage collected + revision: constraint.revision + 1, // progress revision to invalid (even) value + primaryBodyIndex: nilIndex, + secondaryBodyIndex: nilIndex, + primaryNextIndex: nilIndex, + secondaryNextIndex: nilIndex, + isEnabled: false, + } + + v.scene.releasePairConstraint(id.index) +} + +func (v PairConstraintView) Handle(id PairConstraintID) PairConstraintHandle { + return PairConstraintHandle{ + view: v, + id: id, + } +} + +func (v PairConstraintView) IsValid(id PairConstraintID) bool { + constraint := v.resolve(id, false) + return constraint != nil +} + +func (v PairConstraintView) PrimaryBodyID(id PairConstraintID) BodyID { + constraint := v.resolve(id, true) + bodyIndex := constraint.primaryBodyIndex + body := &v.scene.bodies[bodyIndex] + return BodyID{ + index: bodyIndex, + revision: body.revision, + } +} + +func (v PairConstraintView) SecondaryBodyID(id PairConstraintID) BodyID { + constraint := v.resolve(id, true) + bodyIndex := constraint.secondaryBodyIndex + body := &v.scene.bodies[bodyIndex] + return BodyID{ + index: bodyIndex, + revision: body.revision, + } +} + +func (v PairConstraintView) Solver(id PairConstraintID) PairConstraintSolver { + constraint := v.resolve(id, true) + return constraint.solver +} + +func (v PairConstraintView) SetSolver(id PairConstraintID, solver PairConstraintSolver) { + constraint := v.resolve(id, true) + constraint.solver = solver +} + +func (v PairConstraintView) Enabled(id PairConstraintID) bool { + constraint := v.resolve(id, true) + return constraint.isEnabled +} + +func (v PairConstraintView) SetEnabled(id PairConstraintID, enabled bool) { + constraint := v.resolve(id, true) + constraint.isEnabled = enabled } func (v PairConstraintView) idFromIndex(index int32) PairConstraintID { @@ -65,11 +182,52 @@ type PairConstraintHandle struct { id PairConstraintID } +func (h PairConstraintHandle) ID() PairConstraintID { + return h.id +} + +func (h PairConstraintHandle) Delete() { + h.view.Delete(h.id) +} + +func (h PairConstraintHandle) IsValid() bool { + return h.view.IsValid(h.id) +} + +func (h PairConstraintHandle) PrimaryBodyID() BodyID { + return h.view.PrimaryBodyID(h.id) +} + +func (h PairConstraintHandle) SecondaryBodyID() BodyID { + return h.view.SecondaryBodyID(h.id) +} + +func (h PairConstraintHandle) Solver() PairConstraintSolver { + return h.view.Solver(h.id) +} + +func (h PairConstraintHandle) SetSolver(solver PairConstraintSolver) { + h.view.SetSolver(h.id, solver) +} + +func (h PairConstraintHandle) Enabled() bool { + return h.view.Enabled(h.id) +} + +func (h PairConstraintHandle) SetEnabled(enabled bool) { + h.view.SetEnabled(h.id, enabled) +} + type pairConstraintState struct { solver PairConstraintSolver revision int32 primaryBodyIndex int32 secondaryBodyIndex int32 - nextIndex int32 + primaryNextIndex int32 + secondaryNextIndex int32 isEnabled bool } + +func (s *pairConstraintState) isValid() bool { + return s.revision%2 == 1 // only odd revisions are valid +} diff --git a/game/physics/constraint_solo.go b/game/physics/constraint_solo.go index 90cb6dea..1cc938f7 100644 --- a/game/physics/constraint_solo.go +++ b/game/physics/constraint_solo.go @@ -90,6 +90,7 @@ func (v SoloConstraintView) Create(bodyID BodyID, solver SoloConstraintSolver) S body := bodyView.resolve(bodyID, true) index, constraint := v.scene.allocateSoloConstraint() + *constraint = soloConstraintState{ solver: solver, revision: constraint.revision + 1, // progress revision to valid (odd) value @@ -309,3 +310,7 @@ type soloConstraintState struct { nextIndex int32 isEnabled bool } + +func (s *soloConstraintState) isValid() bool { + return s.revision%2 == 1 // only odd revisions are valid +} diff --git a/game/physics/scene.go b/game/physics/scene.go index 375aece6..3fe1c9b9 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -2,6 +2,7 @@ package physics import ( "maps" + "math" "time" "github.com/mokiat/gog/ds" @@ -26,11 +27,6 @@ type Scene struct { timeSpeed float64 - maxLinearAcceleration float64 - maxAngularAcceleration float64 - maxLinearVelocity float64 - maxAngularVelocity float64 - props []propState sbCollisionConstraints []SBConstraint @@ -63,6 +59,11 @@ type Scene struct { soloConstraints []soloConstraintState pairConstraints []pairConstraintState bodies []bodyState + + maxLinearAcceleration float64 + maxAngularAcceleration float64 + maxLinearVelocity float64 + maxAngularVelocity float64 } func NewScene() *Scene { @@ -79,11 +80,6 @@ func NewScene() *Scene { timeSpeed: 1.0, - maxLinearAcceleration: 200.0, - maxAngularAcceleration: 200.0, - maxLinearVelocity: 2000.0, - maxAngularVelocity: 2000.0, - props: make([]propState, 0, 1024), collisionSet: make(placement3d.ContactList, 0, 128), @@ -108,6 +104,11 @@ func NewScene() *Scene { soloConstraints: make([]soloConstraintState, 0), pairConstraints: make([]pairConstraintState, 0), bodies: make([]bodyState, 0), + + maxLinearAcceleration: math.MaxFloat64, + maxAngularAcceleration: math.MaxFloat64, + maxLinearVelocity: math.MaxFloat64, + maxAngularVelocity: math.MaxFloat64, } } @@ -169,6 +170,38 @@ func (s *Scene) Bodies() BodyView { } } +func (s *Scene) MaxLinearAcceleration() float64 { + return s.maxLinearAcceleration +} + +func (s *Scene) SetMaxLinearAcceleration(acceleration float64) { + s.maxLinearAcceleration = acceleration +} + +func (s *Scene) MaxAngularAcceleration() float64 { + return s.maxAngularAcceleration +} + +func (s *Scene) SetMaxAngularAcceleration(acceleration float64) { + s.maxAngularAcceleration = acceleration +} + +func (s *Scene) MaxLinearVelocity() float64 { + return s.maxLinearVelocity +} + +func (s *Scene) SetMaxLinearVelocity(velocity float64) { + s.maxLinearVelocity = velocity +} + +func (s *Scene) MaxAngularVelocity() float64 { + return s.maxAngularVelocity +} + +func (s *Scene) SetMaxAngularVelocity(velocity float64) { + s.maxAngularVelocity = velocity +} + /////// OLD BELOW ------------ (TODO: DELETE COMMENT) // SubscribeSingleBodyCollision registers a callback that is invoked when a body @@ -194,30 +227,6 @@ func (s *Scene) SetTimeSpeed(timeSpeed float64) { s.timeSpeed = timeSpeed } -// MaxLinearAcceleration returns the maximum linear acceleration that a body -// can have. -func (s *Scene) MaxLinearAcceleration() float64 { - return s.maxLinearAcceleration -} - -// SetMaxLinearAcceleration changes the maximum linear acceleration that a body -// can have. -func (s *Scene) SetMaxLinearAcceleration(acceleration float64) { - s.maxLinearAcceleration = acceleration -} - -// MaxAngularAcceleration returns the maximum angular acceleration that a body -// can have. -func (s *Scene) MaxAngularAcceleration() float64 { - return s.maxAngularAcceleration -} - -// SetMaxAngularAcceleration changes the maximum angular acceleration that a -// body can have. -func (s *Scene) SetMaxAngularAcceleration(acceleration float64) { - s.maxAngularAcceleration = acceleration -} - // NextCollisionRejectGroup returns a collision reject group that is unique // within this Scene. Bodies that are assigned the same reject group do not // collide with each other, which is useful for objects that are meant to @@ -275,7 +284,7 @@ func (s *Scene) Update(elapsedTime time.Duration) { } func (s *Scene) Each(cb func(b Body)) { - s.eachBodyState(func(_ int, b *bodyState) { + s.eachBody(func(_ int, b *bodyState) { cb(Body{ scene: s, reference: b.reference, @@ -337,7 +346,7 @@ func (s *Scene) runSimulation(elapsedSeconds float64) { func (s *Scene) applyAcceleration(elapsedSeconds float64) { defer metric.BeginRegion("acceleration").End() - s.eachBodyState(func(index int, body *bodyState) { + s.eachBody(func(index int, body *bodyState) { // Create acceleration context. ctx := AccelerationContext{ DeltaSeconds: elapsedSeconds, @@ -369,7 +378,7 @@ func (s *Scene) applyAcceleration(elapsedSeconds float64) { } // func (s *Scene) applyAerodynamicAccelerations() { -// s.eachBodyState(func(index int, body *bodyState) { +// s.eachBody(func(index int, body *bodyState) { // if len(body.aerodynamicShapes) == 0 { // return // } @@ -453,7 +462,7 @@ func (s *Scene) applyImpulses(elapsedSeconds float64) { func (s *Scene) applyMotion(elapsedSeconds float64) { defer metric.BeginRegion("motion").End() - s.eachBodyState(func(_ int, body *bodyState) { + s.eachBody(func(_ int, body *bodyState) { // Clamp the velocity to the maximum allowed values. body.clampLinearVelocity(s.maxLinearVelocity) body.clampAngularVelocity(s.maxAngularVelocity) @@ -504,7 +513,7 @@ func (s *Scene) applyNudges(elapsedSeconds float64) { func (s *Scene) applyPlacement() { defer metric.BeginRegion("placement").End() - s.eachBodyState(func(_ int, body *bodyState) { + s.eachBody(func(_ int, body *bodyState) { // Update the collision scene with the new position and rotation of the body. s.collisionScene.SetObjectTransform(body.objectID, shape3d.Transform{ Translation: body.position, @@ -767,14 +776,6 @@ func (s *Scene) notifyDoubleBodyCollisions() { clear(s.newDBCollisions) } -func (s *Scene) eachBodyState(cb func(index int, b *bodyState)) { - for i := range s.bodies { - if body := &s.bodies[i]; body.IsActive() { - cb(i, body) - } - } -} - func (s *Scene) eachDBConstraintState(cb func(index int, constraint *dbConstraintState)) { for i := range s.dbConstraints { if constraint := &s.dbConstraints[i]; constraint.IsActive() { @@ -884,6 +885,14 @@ func (s *Scene) releaseBody(index int32) { s.freeBodyIndices.Push(index) } +func (s *Scene) eachBody(cb func(index int, b *bodyState)) { + for i := range s.bodies { + if body := &s.bodies[i]; body.isValid() { + cb(i, body) + } + } +} + type propRef struct { index uint32 } From 79d4b96698ad2acb009045411816d5c4ff7045d2 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 22:18:29 +0300 Subject: [PATCH 22/85] Bug fixes and godoc --- game/physics/constraint_pair.go | 171 +++++++++++++++++++++++++++++--- game/physics/constraint_solo.go | 19 ++-- game/physics/scene.go | 32 ++++++ 3 files changed, 201 insertions(+), 21 deletions(-) diff --git a/game/physics/constraint_pair.go b/game/physics/constraint_pair.go index 356c485a..98c0b764 100644 --- a/game/physics/constraint_pair.go +++ b/game/physics/constraint_pair.go @@ -1,32 +1,100 @@ package physics +// PairConstraintContext contains the information that a [PairConstraintSolver] +// needs in order to process the two bodies it acts upon during a physics +// simulation step. type PairConstraintContext struct { - DeltaSeconds float64 - ImpulseBeta float64 - NudgeBeta float64 - PrimaryTarget ConstraintTarget + + // DeltaSeconds is the amount of time, in seconds, covered by the + // current physics simulation step. + DeltaSeconds float64 + + // ImpulseBeta is the Baumgarte stabilization factor to be used when + // correcting positional drift through impulses. + ImpulseBeta float64 + + // NudgeBeta is the Baumgarte stabilization factor to be used when + // correcting positional drift through nudges. + NudgeBeta float64 + + // PrimaryTarget is the [ConstraintTarget] for the primary body that + // is constrained, through which the solver reads its motion state + // and applies corrective impulses and nudges to it. + PrimaryTarget ConstraintTarget + + // SecondaryTarget is the [ConstraintTarget] for the secondary body + // that is constrained, through which the solver reads its motion + // state and applies corrective impulses and nudges to it. SecondaryTarget ConstraintTarget } +// PairConstraintSolver implements the mathematical logic that enforces a +// constraint acting on two bodies simultaneously. +// +// Instances are registered with a [Scene] through [PairConstraintView.Create] +// and are subsequently driven by the physics engine through the methods +// below during each simulation step. type PairConstraintSolver interface { + + // Reset clears any internal cache state held by the solver, in + // preparation for a new physics simulation step. + // + // This is called once at the start of every step, before + // [PairConstraintSolver.ApplyImpulses] or [PairConstraintSolver.ApplyNudges] + // are invoked. Reset(ctx PairConstraintContext) + // ApplyImpulses is called by the physics engine to instruct the + // solver to apply the necessary impulses to its target bodies, in + // order to correct their velocities so that the constraint is + // satisfied. + // + // This is called multiple times per step, once for each impulse + // resolution iteration. ApplyImpulses(ctx PairConstraintContext) + // ApplyNudges is called by the physics engine to instruct the solver + // to apply the necessary nudges to its target bodies, in order to + // correct their positions so that the constraint is satisfied. + // + // This is called multiple times per step, once for each nudge + // resolution iteration. ApplyNudges(ctx PairConstraintContext) } +// PairConstraintID uniquely identifies a pair constraint that has been +// created through [PairConstraintView.Create]. +// +// The zero value is not a valid ID; use [NilPairConstraintID] to represent +// the absence of a pair constraint. type PairConstraintID struct { index int32 revision int32 } +// NilPairConstraintID is a [PairConstraintID] that is guaranteed to never +// reference a valid pair constraint. var NilPairConstraintID = PairConstraintID{} +// PairConstraintView provides access to the pair constraints (i.e. +// constraints that act on two bodies simultaneously) that belong to a +// [Scene], as opposed to a solo constraint, which acts on a single body. +// +// A PairConstraintView is a lightweight accessor around a [Scene] and can +// be obtained through [Scene.PairConstraints]. type PairConstraintView struct { scene *Scene } +// Create registers solver as a new pair constraint that acts on the +// bodies identified by primaryID and secondaryID, and returns an ID +// through which the constraint can be referenced in the future. +// +// The returned constraint is enabled by default. It is automatically +// deleted whenever either of its two target bodies is deleted. +// +// Create panics if primaryID or secondaryID does not reference a valid +// body. func (v PairConstraintView) Create(primaryID, secondaryID BodyID, solver PairConstraintSolver) PairConstraintID { bodyView := v.scene.Bodies() primaryBody := bodyView.resolve(primaryID, true) @@ -39,12 +107,12 @@ func (v PairConstraintView) Create(primaryID, secondaryID BodyID, solver PairCon revision: constraint.revision + 1, // progress revision to valid (odd) value primaryBodyIndex: primaryID.index, secondaryBodyIndex: secondaryID.index, - primaryNextIndex: primaryBody.firstSoloConstraintIndex, - secondaryNextIndex: secondaryBody.firstSoloConstraintIndex, + primaryNextIndex: primaryBody.firstPairConstraintIndex, + secondaryNextIndex: secondaryBody.firstPairConstraintIndex, isEnabled: true, } - primaryBody.firstSoloConstraintIndex = index - secondaryBody.firstSoloConstraintIndex = index + primaryBody.firstPairConstraintIndex = index + secondaryBody.firstPairConstraintIndex = index return PairConstraintID{ index: index, @@ -52,18 +120,23 @@ func (v PairConstraintView) Create(primaryID, secondaryID BodyID, solver PairCon } } +// Delete removes the pair constraint identified by id, unlinking it from +// both of its target bodies and releasing the underlying storage for +// reuse. +// +// Delete panics if id does not reference a valid pair constraint. func (v PairConstraintView) Delete(id PairConstraintID) { constraint := v.resolve(id, true) // Unlink the constraint from the primary body. primaryBody := &v.scene.bodies[constraint.primaryBodyIndex] - if primaryBody.firstPairConstraintIndex == constraint.primaryNextIndex { + if primaryBody.firstPairConstraintIndex == id.index { primaryBody.firstPairConstraintIndex = constraint.primaryNextIndex } else { prevIndex := primaryBody.firstPairConstraintIndex - for prevIndex != -1 { + for prevIndex != nilIndex { prevConstraint := &v.scene.pairConstraints[prevIndex] - if prevConstraint.primaryNextIndex == constraint.primaryNextIndex { + if prevConstraint.primaryNextIndex == id.index { prevConstraint.primaryNextIndex = constraint.primaryNextIndex break } @@ -73,13 +146,13 @@ func (v PairConstraintView) Delete(id PairConstraintID) { // Unlink the constraint from the secondary body. secondaryBody := &v.scene.bodies[constraint.secondaryBodyIndex] - if secondaryBody.firstPairConstraintIndex == constraint.secondaryNextIndex { + if secondaryBody.firstPairConstraintIndex == id.index { secondaryBody.firstPairConstraintIndex = constraint.secondaryNextIndex } else { prevIndex := secondaryBody.firstPairConstraintIndex - for prevIndex != -1 { + for prevIndex != nilIndex { prevConstraint := &v.scene.pairConstraints[prevIndex] - if prevConstraint.secondaryNextIndex == constraint.secondaryNextIndex { + if prevConstraint.secondaryNextIndex == id.index { prevConstraint.secondaryNextIndex = constraint.secondaryNextIndex break } @@ -100,6 +173,9 @@ func (v PairConstraintView) Delete(id PairConstraintID) { v.scene.releasePairConstraint(id.index) } +// Handle returns a [PairConstraintHandle] that wraps id, offering a more +// convenient, object-oriented way to interact with the referenced pair +// constraint. func (v PairConstraintView) Handle(id PairConstraintID) PairConstraintHandle { return PairConstraintHandle{ view: v, @@ -107,11 +183,17 @@ func (v PairConstraintView) Handle(id PairConstraintID) PairConstraintHandle { } } +// IsValid returns whether id references a pair constraint that is still +// alive within the [Scene]. func (v PairConstraintView) IsValid(id PairConstraintID) bool { constraint := v.resolve(id, false) return constraint != nil } +// PrimaryBodyID returns the ID of the primary body on which the pair +// constraint identified by id acts. +// +// PrimaryBodyID panics if id does not reference a valid pair constraint. func (v PairConstraintView) PrimaryBodyID(id PairConstraintID) BodyID { constraint := v.resolve(id, true) bodyIndex := constraint.primaryBodyIndex @@ -122,6 +204,11 @@ func (v PairConstraintView) PrimaryBodyID(id PairConstraintID) BodyID { } } +// SecondaryBodyID returns the ID of the secondary body on which the pair +// constraint identified by id acts. +// +// SecondaryBodyID panics if id does not reference a valid pair +// constraint. func (v PairConstraintView) SecondaryBodyID(id PairConstraintID) BodyID { constraint := v.resolve(id, true) bodyIndex := constraint.secondaryBodyIndex @@ -132,26 +219,44 @@ func (v PairConstraintView) SecondaryBodyID(id PairConstraintID) BodyID { } } +// Solver returns the [PairConstraintSolver] that implements the pair +// constraint identified by id. +// +// Solver panics if id does not reference a valid pair constraint. func (v PairConstraintView) Solver(id PairConstraintID) PairConstraintSolver { constraint := v.resolve(id, true) return constraint.solver } +// SetSolver changes the [PairConstraintSolver] that implements the pair +// constraint identified by id. +// +// SetSolver panics if id does not reference a valid pair constraint. func (v PairConstraintView) SetSolver(id PairConstraintID, solver PairConstraintSolver) { constraint := v.resolve(id, true) constraint.solver = solver } +// Enabled returns whether the pair constraint identified by id is +// currently enforced by the physics engine. +// +// Enabled panics if id does not reference a valid pair constraint. func (v PairConstraintView) Enabled(id PairConstraintID) bool { constraint := v.resolve(id, true) return constraint.isEnabled } +// SetEnabled changes whether the pair constraint identified by id is +// enforced by the physics engine. +// +// SetEnabled panics if id does not reference a valid pair constraint. func (v PairConstraintView) SetEnabled(id PairConstraintID, enabled bool) { constraint := v.resolve(id, true) constraint.isEnabled = enabled } +// idFromIndex builds the current [PairConstraintID] for the pair +// constraint stored at the given slice index. func (v PairConstraintView) idFromIndex(index int32) PairConstraintID { state := &v.scene.pairConstraints[index] return PairConstraintID{ @@ -160,6 +265,9 @@ func (v PairConstraintView) idFromIndex(index int32) PairConstraintID { } } +// resolve looks up the pairConstraintState referenced by id. If id is +// stale or otherwise invalid, resolve panics when required is true, or +// returns nil otherwise. func (v PairConstraintView) resolve(id PairConstraintID, required bool) *pairConstraintState { if id.revision == 0 { if required { @@ -177,47 +285,80 @@ func (v PairConstraintView) resolve(id PairConstraintID, required bool) *pairCon return constraint } +// PairConstraintHandle is an object-oriented alternative to +// [PairConstraintView] that is bound to a specific [PairConstraintID]. +// +// It is obtained through [PairConstraintView.Handle]. type PairConstraintHandle struct { view PairConstraintView id PairConstraintID } +// ID returns the identifier of the pair constraint targeted by this +// handle. func (h PairConstraintHandle) ID() PairConstraintID { return h.id } +// Delete removes the pair constraint targeted by this handle. +// +// See [PairConstraintView.Delete] for further details. func (h PairConstraintHandle) Delete() { h.view.Delete(h.id) } +// IsValid returns whether this handle still references a pair constraint +// that is alive within the [Scene]. func (h PairConstraintHandle) IsValid() bool { return h.view.IsValid(h.id) } +// PrimaryBodyID returns the ID of the primary body on which the targeted +// pair constraint acts. func (h PairConstraintHandle) PrimaryBodyID() BodyID { return h.view.PrimaryBodyID(h.id) } +// SecondaryBodyID returns the ID of the secondary body on which the +// targeted pair constraint acts. func (h PairConstraintHandle) SecondaryBodyID() BodyID { return h.view.SecondaryBodyID(h.id) } +// Solver returns the [PairConstraintSolver] that implements the targeted +// pair constraint. func (h PairConstraintHandle) Solver() PairConstraintSolver { return h.view.Solver(h.id) } +// SetSolver changes the [PairConstraintSolver] that implements the +// targeted pair constraint. func (h PairConstraintHandle) SetSolver(solver PairConstraintSolver) { h.view.SetSolver(h.id, solver) } +// Enabled returns whether the targeted pair constraint is currently +// enforced by the physics engine. func (h PairConstraintHandle) Enabled() bool { return h.view.Enabled(h.id) } +// SetEnabled changes whether the targeted pair constraint is enforced by +// the physics engine. func (h PairConstraintHandle) SetEnabled(enabled bool) { h.view.SetEnabled(h.id, enabled) } +// pairConstraintState holds the internal state of a single pair +// constraint, as tracked by a [Scene]. +// +// Instances are threaded into two independent singly-linked lists, one +// per participating body, each rooted at that body's +// firstPairConstraintIndex. Within a given body's list, a node's link to +// the next entry is held in primaryNextIndex if that body is the node's +// primary body, or in secondaryNextIndex if it is the node's secondary +// body, since the same body can be the primary of one constraint and the +// secondary of another. type pairConstraintState struct { solver PairConstraintSolver revision int32 @@ -228,6 +369,8 @@ type pairConstraintState struct { isEnabled bool } +// isValid returns whether this state is currently backing a live pair +// constraint, as opposed to a freed slot awaiting reuse. func (s *pairConstraintState) isValid() bool { return s.revision%2 == 1 // only odd revisions are valid } diff --git a/game/physics/constraint_solo.go b/game/physics/constraint_solo.go index 1cc938f7..919a81da 100644 --- a/game/physics/constraint_solo.go +++ b/game/physics/constraint_solo.go @@ -17,8 +17,9 @@ type SoloConstraintContext struct { // correcting positional drift through nudges. NudgeBeta float64 - // Target is the placeholder representation of the body that is - // constrained. + // Target is the [ConstraintTarget] for the body that is constrained, + // through which the solver reads its motion state and applies + // corrective impulses and nudges to it. Target ConstraintTarget } @@ -34,7 +35,8 @@ type SoloConstraintSolver interface { // preparation for a new physics simulation step. // // This is called once at the start of every step, before - // ApplyImpulses or ApplyNudges are invoked. + // [SoloConstraintSolver.ApplyImpulses] or [SoloConstraintSolver.ApplyNudges] + // are invoked. Reset(ctx SoloConstraintContext) // ApplyImpulses is called by the physics engine to instruct the solver @@ -69,7 +71,8 @@ type SoloConstraintID struct { var NilSoloConstraintID = SoloConstraintID{} // SoloConstraintView provides access to the solo constraints (i.e. -// constraints that act on a single body) that belong to a [Scene]. +// constraints that act on a single body) that belong to a [Scene], as +// opposed to a pair constraint, which acts on two bodies simultaneously. // // A SoloConstraintView is a lightweight accessor around a [Scene] and can // be obtained through [Scene.SoloConstraints]. @@ -218,9 +221,9 @@ func (v SoloConstraintView) idFromIndex(index int32) SoloConstraintID { } } -// resolve looks up the soloConstraint referenced by id. If id is stale or -// otherwise invalid, resolve panics when required is true, or returns nil -// otherwise. +// resolve looks up the soloConstraintState referenced by id. If id is +// stale or otherwise invalid, resolve panics when required is true, or +// returns nil otherwise. func (v SoloConstraintView) resolve(id SoloConstraintID, required bool) *soloConstraintState { if id.revision == 0 { if required { @@ -311,6 +314,8 @@ type soloConstraintState struct { isEnabled bool } +// isValid returns whether this state is currently backing a live solo +// constraint, as opposed to a freed slot awaiting reuse. func (s *soloConstraintState) isValid() bool { return s.revision%2 == 1 // only odd revisions are valid } diff --git a/game/physics/scene.go b/game/physics/scene.go index 3fe1c9b9..861d015c 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -158,46 +158,78 @@ func (s *Scene) SoloConstraints() SoloConstraintView { } } +// PairConstraints returns a [PairConstraintView] through which the pair +// constraints of this scene can be created and managed. func (s *Scene) PairConstraints() PairConstraintView { return PairConstraintView{ scene: s, } } +// Bodies returns a [BodyView] through which the bodies of this scene can be +// created and managed. func (s *Scene) Bodies() BodyView { return BodyView{ scene: s, } } +// MaxLinearAcceleration returns the maximum magnitude that the linear +// acceleration of a body can reach. Accelerations that exceed it are +// clamped on every simulation step. +// +// Defaults to math.MaxFloat64, which is effectively unbounded. func (s *Scene) MaxLinearAcceleration() float64 { return s.maxLinearAcceleration } +// SetMaxLinearAcceleration changes the maximum magnitude that the linear +// acceleration of a body can reach. func (s *Scene) SetMaxLinearAcceleration(acceleration float64) { s.maxLinearAcceleration = acceleration } +// MaxAngularAcceleration returns the maximum magnitude that the angular +// acceleration of a body can reach. Accelerations that exceed it are +// clamped on every simulation step. +// +// Defaults to math.MaxFloat64, which is effectively unbounded. func (s *Scene) MaxAngularAcceleration() float64 { return s.maxAngularAcceleration } +// SetMaxAngularAcceleration changes the maximum magnitude that the angular +// acceleration of a body can reach. func (s *Scene) SetMaxAngularAcceleration(acceleration float64) { s.maxAngularAcceleration = acceleration } +// MaxLinearVelocity returns the maximum magnitude that the linear velocity +// of a body can reach. Velocities that exceed it are clamped on every +// simulation step. +// +// Defaults to math.MaxFloat64, which is effectively unbounded. func (s *Scene) MaxLinearVelocity() float64 { return s.maxLinearVelocity } +// SetMaxLinearVelocity changes the maximum magnitude that the linear +// velocity of a body can reach. func (s *Scene) SetMaxLinearVelocity(velocity float64) { s.maxLinearVelocity = velocity } +// MaxAngularVelocity returns the maximum magnitude that the angular +// velocity of a body can reach. Velocities that exceed it are clamped on +// every simulation step. +// +// Defaults to math.MaxFloat64, which is effectively unbounded. func (s *Scene) MaxAngularVelocity() float64 { return s.maxAngularVelocity } +// SetMaxAngularVelocity changes the maximum magnitude that the angular +// velocity of a body can reach. func (s *Scene) SetMaxAngularVelocity(velocity float64) { s.maxAngularVelocity = velocity } From 400f18e708cbe78bcf3b878867498fc92a42e917 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 22:40:31 +0300 Subject: [PATCH 23/85] Bug fix to pair constraint deletion --- game/physics/constraint_pair.go | 70 +++++++++++++++++++++++++++------ 1 file changed, 58 insertions(+), 12 deletions(-) diff --git a/game/physics/constraint_pair.go b/game/physics/constraint_pair.go index 98c0b764..ba18c3ea 100644 --- a/game/physics/constraint_pair.go +++ b/game/physics/constraint_pair.go @@ -129,34 +129,62 @@ func (v PairConstraintView) Delete(id PairConstraintID) { constraint := v.resolve(id, true) // Unlink the constraint from the primary body. - primaryBody := &v.scene.bodies[constraint.primaryBodyIndex] + primaryBodyIndex := constraint.primaryBodyIndex + primaryBody := &v.scene.bodies[primaryBodyIndex] if primaryBody.firstPairConstraintIndex == id.index { - primaryBody.firstPairConstraintIndex = constraint.primaryNextIndex + primaryBody.firstPairConstraintIndex = constraint.nextIndexForBody(primaryBodyIndex) } else { prevIndex := primaryBody.firstPairConstraintIndex for prevIndex != nilIndex { prevConstraint := &v.scene.pairConstraints[prevIndex] - if prevConstraint.primaryNextIndex == id.index { - prevConstraint.primaryNextIndex = constraint.primaryNextIndex - break + switch { + case prevConstraint.primaryBodyIndex == primaryBodyIndex: // body follows primary chain + if prevConstraint.primaryNextIndex == id.index { + prevConstraint.primaryNextIndex = constraint.nextIndexForBody(primaryBodyIndex) + prevIndex = nilIndex // break the loop + } else { + prevIndex = prevConstraint.primaryNextIndex + } + case prevConstraint.secondaryBodyIndex == primaryBodyIndex: // body follows secondary chain + if prevConstraint.secondaryNextIndex == id.index { + prevConstraint.secondaryNextIndex = constraint.nextIndexForBody(primaryBodyIndex) + prevIndex = nilIndex // break the loop + } else { + prevIndex = prevConstraint.secondaryNextIndex + } + default: + panic("body index does not match either primary or secondary body") } - prevIndex = prevConstraint.primaryNextIndex } } // Unlink the constraint from the secondary body. - secondaryBody := &v.scene.bodies[constraint.secondaryBodyIndex] + secondaryBodyIndex := constraint.secondaryBodyIndex + secondaryBody := &v.scene.bodies[secondaryBodyIndex] if secondaryBody.firstPairConstraintIndex == id.index { - secondaryBody.firstPairConstraintIndex = constraint.secondaryNextIndex + secondaryBody.firstPairConstraintIndex = constraint.nextIndexForBody(secondaryBodyIndex) } else { prevIndex := secondaryBody.firstPairConstraintIndex for prevIndex != nilIndex { prevConstraint := &v.scene.pairConstraints[prevIndex] - if prevConstraint.secondaryNextIndex == id.index { - prevConstraint.secondaryNextIndex = constraint.secondaryNextIndex - break + switch { + case prevConstraint.primaryBodyIndex == secondaryBodyIndex: // body follows primary chain + if prevConstraint.primaryNextIndex == id.index { + prevConstraint.primaryNextIndex = constraint.nextIndexForBody(secondaryBodyIndex) + prevIndex = nilIndex // break the loop + } else { + prevIndex = prevConstraint.primaryNextIndex + } + case prevConstraint.secondaryBodyIndex == secondaryBodyIndex: // body follows secondary chain + if prevConstraint.secondaryNextIndex == id.index { + prevConstraint.secondaryNextIndex = constraint.nextIndexForBody(secondaryBodyIndex) + prevIndex = nilIndex // break the loop + } else { + prevIndex = prevConstraint.secondaryNextIndex + } + default: + panic("body index does not match either primary or secondary body") } - prevIndex = prevConstraint.secondaryNextIndex } } @@ -369,6 +397,24 @@ type pairConstraintState struct { isEnabled bool } +// nextIndexForBody returns the index of the next entry in bodyIndex's +// singly-linked list of pair constraints, following primaryNextIndex or +// secondaryNextIndex depending on whether bodyIndex is this state's +// primary or secondary body. +// +// nextIndexForBody panics if bodyIndex is neither the primary nor the +// secondary body of this state. +func (s *pairConstraintState) nextIndexForBody(bodyIndex int32) int32 { + switch bodyIndex { + case s.primaryBodyIndex: + return s.primaryNextIndex + case s.secondaryBodyIndex: + return s.secondaryNextIndex + default: + panic("body index does not match either primary or secondary body") + } +} + // isValid returns whether this state is currently backing a live pair // constraint, as opposed to a freed slot awaiting reuse. func (s *pairConstraintState) isValid() bool { From fa150c3b5ceaf667cdcef5a58c554f8156e2de65 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 22:45:25 +0300 Subject: [PATCH 24/85] Prevent pair constraint for single body --- game/physics/constraint_pair.go | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/game/physics/constraint_pair.go b/game/physics/constraint_pair.go index ba18c3ea..117b5454 100644 --- a/game/physics/constraint_pair.go +++ b/game/physics/constraint_pair.go @@ -94,8 +94,13 @@ type PairConstraintView struct { // deleted whenever either of its two target bodies is deleted. // // Create panics if primaryID or secondaryID does not reference a valid -// body. +// body, or if they both reference the same body, since a pair constraint +// cannot act on a single body twice. func (v PairConstraintView) Create(primaryID, secondaryID BodyID, solver PairConstraintSolver) PairConstraintID { + if primaryID == secondaryID { + panic("pair constraint cannot be created between a body and itself") + } + bodyView := v.scene.Bodies() primaryBody := bodyView.resolve(primaryID, true) secondaryBody := bodyView.resolve(secondaryID, true) From a24a964866dbbc6e3c3310179cddb7b09179fccd Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 22:59:18 +0300 Subject: [PATCH 25/85] Make impulse and nudge settings per-scene --- game/physics/scene.go | 74 ++++++++++++++++++++++++++++++++++++++++++ game/physics/solver.go | 28 ---------------- 2 files changed, 74 insertions(+), 28 deletions(-) delete mode 100644 game/physics/solver.go diff --git a/game/physics/scene.go b/game/physics/scene.go index 861d015c..b229989d 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -64,6 +64,11 @@ type Scene struct { maxAngularAcceleration float64 maxLinearVelocity float64 maxAngularVelocity float64 + + impulseIterationCount int + impulseDriftAdjustmentRatio float64 + nudgeIterationCount int + nudgeDriftAdjustmentRatio float64 } func NewScene() *Scene { @@ -109,6 +114,11 @@ func NewScene() *Scene { maxAngularAcceleration: math.MaxFloat64, maxLinearVelocity: math.MaxFloat64, maxAngularVelocity: math.MaxFloat64, + + impulseIterationCount: 8, + impulseDriftAdjustmentRatio: 0.2, + nudgeIterationCount: 8, + nudgeDriftAdjustmentRatio: 0.2, } } @@ -234,6 +244,70 @@ func (s *Scene) SetMaxAngularVelocity(velocity float64) { s.maxAngularVelocity = velocity } +// ImpulseIterationCount returns the number of impulse resolution +// iterations performed per physics simulation step. A higher iteration +// count improves the accuracy with which constraints are jointly +// satisfied, at the cost of extra computation. +// +// Defaults to 8. +func (s *Scene) ImpulseIterationCount() int { + return s.impulseIterationCount +} + +// SetImpulseIterationCount changes the number of impulse resolution +// iterations performed per physics simulation step. +func (s *Scene) SetImpulseIterationCount(count int) { + s.impulseIterationCount = count +} + +// ImpulseDriftAdjustmentRatio returns the Baumgarte stabilization factor +// used to correct positional drift through impulses, i.e. the value +// passed as [SoloConstraintContext.ImpulseBeta] and +// [PairConstraintContext.ImpulseBeta] to constraint solvers. +// +// Defaults to 0.2. +func (s *Scene) ImpulseDriftAdjustmentRatio() float64 { + return s.impulseDriftAdjustmentRatio +} + +// SetImpulseDriftAdjustmentRatio changes the Baumgarte stabilization +// factor used to correct positional drift through impulses. +func (s *Scene) SetImpulseDriftAdjustmentRatio(ratio float64) { + s.impulseDriftAdjustmentRatio = ratio +} + +// NudgeIterationCount returns the number of nudge resolution iterations +// performed per physics simulation step. A higher iteration count +// improves the accuracy with which constraints are jointly satisfied, at +// the cost of extra computation. +// +// Defaults to 8. +func (s *Scene) NudgeIterationCount() int { + return s.nudgeIterationCount +} + +// SetNudgeIterationCount changes the number of nudge resolution +// iterations performed per physics simulation step. +func (s *Scene) SetNudgeIterationCount(count int) { + s.nudgeIterationCount = count +} + +// NudgeDriftAdjustmentRatio returns the Baumgarte stabilization factor +// used to correct positional drift through nudges, i.e. the value passed +// as [SoloConstraintContext.NudgeBeta] and [PairConstraintContext.NudgeBeta] +// to constraint solvers. +// +// Defaults to 0.2. +func (s *Scene) NudgeDriftAdjustmentRatio() float64 { + return s.nudgeDriftAdjustmentRatio +} + +// SetNudgeDriftAdjustmentRatio changes the Baumgarte stabilization factor +// used to correct positional drift through nudges. +func (s *Scene) SetNudgeDriftAdjustmentRatio(ratio float64) { + s.nudgeDriftAdjustmentRatio = ratio +} + /////// OLD BELOW ------------ (TODO: DELETE COMMENT) // SubscribeSingleBodyCollision registers a callback that is invoked when a body diff --git a/game/physics/solver.go b/game/physics/solver.go deleted file mode 100644 index 1e56196f..00000000 --- a/game/physics/solver.go +++ /dev/null @@ -1,28 +0,0 @@ -package physics - -var ( - // ImpulseIterationCount controls the number of iterations for impulse - // solutions by the solvers. - ImpulseIterationCount = 8 - - // NudgeIterationCount controls the number of iterations for nudge - // solutions by the solvers. - NudgeIterationCount = 8 - - // ImpulseDriftAdjustmentRatio controls the amount by which impulses should - // try to correct positional drift. - // - // This is the `beta` coefficient in the Baumgarte stabilization approach. - ImpulseDriftAdjustmentRatio = 0.2 - - // NudgeDriftAdjustmentRatio controls the amount by which nudges should - // try to correct positional drift. - // - // The value here is accumulated over all iterations. In fact, the total - // remaining error is proportional to (1.0 - ratio) ^ iterations. - // - // Some error should be left in order to avoid jitters due to imprecise - // integration of the correction and to leave some drift for the - // impulse solution. - NudgeDriftAdjustmentRatio = 0.2 -) From 70b896e95bf6834f3db28d298225690d470f4065 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 23:20:34 +0300 Subject: [PATCH 26/85] More code rework --- game/physics/body.go | 27 ---------- game/physics/callback.go | 2 +- game/physics/collision.go | 24 +++++++++ game/physics/prop.go | 37 -------------- game/physics/scene.go | 93 +++++++++++++++++----------------- game/physics/terrain.go | 102 ++++++++++++++++++++++++++++++++++++++ 6 files changed, 175 insertions(+), 110 deletions(-) create mode 100644 game/physics/collision.go delete mode 100644 game/physics/prop.go create mode 100644 game/physics/terrain.go diff --git a/game/physics/body.go b/game/physics/body.go index 91977715..0bc1ac6e 100644 --- a/game/physics/body.go +++ b/game/physics/body.go @@ -298,33 +298,6 @@ func (h BodyHandle) DetachCollisionShape(shapeID CollisionShapeID) { h.view.DetachCollisionShape(h.id, shapeID) } -// TODO: Relocate collision-related code to separate file. - -type CollisionShapeID struct { - bodyID BodyID - shapeID placement3d.ShapeID -} - -type CollisionShape[T any] struct { - Shape T - FrictionCoefficient float64 - RestitutionCoefficient float64 - Filtering placement3d.FilterInfo -} - -type CollisionSphere CollisionShape[shape3d.Sphere] - -type CollisionBox CollisionShape[shape3d.Box] - -type bodyData struct { - index int32 -} - -type shapeData struct { - frictionCoefficient float64 - restitutionCoefficient float64 -} - type bodyState struct { objectID placement3d.ObjectID diff --git a/game/physics/callback.go b/game/physics/callback.go index 2848ed2b..81d643c1 100644 --- a/game/physics/callback.go +++ b/game/physics/callback.go @@ -4,7 +4,7 @@ import "github.com/mokiat/lacking/util/observer" // SoloBodyCollisionCallback is a mechanism to receive notifications // about collisions between a body and a prop in the scene. -type SoloBodyCollisionCallback func(bodyID BodyID, propID PropID, active bool) +type SoloBodyCollisionCallback func(bodyID BodyID, terrainID TerrainID, active bool) // SoloBodyCollisionSubscription represents a notification subscription // for single body collisions. diff --git a/game/physics/collision.go b/game/physics/collision.go new file mode 100644 index 00000000..e9658802 --- /dev/null +++ b/game/physics/collision.go @@ -0,0 +1,24 @@ +package physics + +import ( + "github.com/mokiat/lacking/core/spatial/placement3d" + "github.com/mokiat/lacking/core/spatial/shape3d" +) + +type CollisionShapeID struct { + bodyID BodyID + shapeID placement3d.ShapeID +} + +type CollisionShape[T any] struct { + Shape T + FrictionCoefficient float64 + RestitutionCoefficient float64 + Filtering placement3d.FilterInfo +} + +type CollisionSphere CollisionShape[shape3d.Sphere] + +type CollisionBox CollisionShape[shape3d.Box] + +type CollisionMesh CollisionShape[shape3d.Mesh] diff --git a/game/physics/prop.go b/game/physics/prop.go deleted file mode 100644 index 398bef8e..00000000 --- a/game/physics/prop.go +++ /dev/null @@ -1,37 +0,0 @@ -package physics - -import ( - "github.com/mokiat/gog/opt" - "github.com/mokiat/gomath/dprec" - "github.com/mokiat/lacking/core/spatial/placement3d" - "github.com/mokiat/lacking/core/spatial/shape3d" -) - -type PropID struct { - index int32 - revision int32 -} - -var NilPropID = PropID{} - -type PropInfo struct { - Name string - Position opt.T[dprec.Vec3] - Rotation opt.T[dprec.Quat] - CollisionSpheres []shape3d.Sphere - CollisionBoxes []shape3d.Box - CollisionMeshes []shape3d.Mesh -} - -type Prop struct { - name string -} - -func (p Prop) Name() string { - return p.name -} - -type propState struct { - meshID placement3d.MeshID - revision int32 -} diff --git a/game/physics/scene.go b/game/physics/scene.go index b229989d..b745d0c4 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -20,15 +20,13 @@ import ( // a number of bodies that are independent on any // bodies managed by other scene objects. type Scene struct { - collisionScene *placement3d.Scene[bodyData, shapeData, propRef] + collisionScene *placement3d.Scene[bodyData, shapeData, terrainData] sbCollisionSubscriptions *observer.SubscriptionSet[SoloBodyCollisionCallback] dbCollisionSubscriptions *observer.SubscriptionSet[PairBodyCollisionCallback] timeSpeed float64 - props []propState - sbCollisionConstraints []SBConstraint sbCollisionSolvers []constraint.Collision @@ -53,12 +51,14 @@ type Scene struct { freeSoloConstraintIndices *ds.Stack[int32] freePairConstraintIndices *ds.Stack[int32] freeBodyIndices *ds.Stack[int32] + freeTerrainIndices *ds.Stack[int32] globalAccelerators []globalAcceleratorState bodyAccelerators []bodyAcceleratorState soloConstraints []soloConstraintState pairConstraints []pairConstraintState bodies []bodyState + terrains []terrainState maxLinearAcceleration float64 maxAngularAcceleration float64 @@ -73,7 +73,7 @@ type Scene struct { func NewScene() *Scene { return &Scene{ - collisionScene: placement3d.NewScene[bodyData, shapeData, propRef](placement3d.SceneSettings{ + collisionScene: placement3d.NewScene[bodyData, shapeData, terrainData](placement3d.SceneSettings{ Size: opt.V(16384.0), MaxDepth: opt.V[uint32](12), InitialNodeCapacity: opt.V[uint32](1024), @@ -85,8 +85,6 @@ func NewScene() *Scene { timeSpeed: 1.0, - props: make([]propState, 0, 1024), - collisionSet: make(placement3d.ContactList, 0, 128), oldSBCollisions: make(map[sbCollisionPair]struct{}, 32), @@ -103,12 +101,14 @@ func NewScene() *Scene { freeSoloConstraintIndices: ds.EmptyStack[int32](), freePairConstraintIndices: ds.EmptyStack[int32](), freeBodyIndices: ds.EmptyStack[int32](), + freeTerrainIndices: ds.EmptyStack[int32](), globalAccelerators: make([]globalAcceleratorState, 0), bodyAccelerators: make([]bodyAcceleratorState, 0), soloConstraints: make([]soloConstraintState, 0), pairConstraints: make([]pairConstraintState, 0), bodies: make([]bodyState, 0), + terrains: make([]terrainState, 0), maxLinearAcceleration: math.MaxFloat64, maxAngularAcceleration: math.MaxFloat64, @@ -184,6 +184,14 @@ func (s *Scene) Bodies() BodyView { } } +// Terrains returns a [TerrainView] through which the terrains of this +// scene can be created and managed. +func (s *Scene) Terrains() TerrainView { + return TerrainView{ + scene: s, + } +} + // MaxLinearAcceleration returns the maximum magnitude that the linear // acceleration of a body can reach. Accelerations that exceed it are // clamped on every simulation step. @@ -349,37 +357,6 @@ func (s *Scene) NextCollisionRejectGroup() uint32 { return s.freeCollisionRejectGroup } -// CreateProp creates a new static Prop. A prop is an object -// that is static and rarely removed. -func (s *Scene) CreateProp(info PropInfo) { - // objectID := s.shapeScene.CreateObject(placement3d.ObjectInfo[internalRef]{ - // Position: info.Position, - // Rotation: info.Rotation, - // UserData: internalRef{ - // index: propIndex, - // isProp: true, - // }, - // }) - for _, mesh := range info.CollisionMeshes { - propIndex := uint32(len(s.props)) - - meshID := s.collisionScene.CreateMesh(placement3d.MeshInfo[propRef]{ - Position: info.Position, - Rotation: info.Rotation, - Mesh: mesh, - UserData: propRef{ - index: propIndex, - }, - }) - - s.props = append(s.props, propState{ - reference: newIndexReference(propIndex, s.nextRevision()), - meshID: meshID, - name: info.Name, - }) - } -} - // Update runs a single physics iteration. This method should be called with // fixed elapsed times, otherwise the physics may break. func (s *Scene) Update(elapsedTime time.Duration) { @@ -582,7 +559,7 @@ func (s *Scene) applyMotion(elapsedSeconds float64) { func (s *Scene) applyNudges(elapsedSeconds float64) { defer metric.BeginRegion("nudges").End() - for range NudgeIterationCount { + for range s.nudgeIterationCount { for _, constraint := range s.dbConstraints { if !constraint.IsActive() { continue @@ -593,8 +570,8 @@ func (s *Scene) applyNudges(elapsedSeconds float64) { Target: target, Source: source, DeltaTime: elapsedSeconds, - ImpulseBeta: ImpulseDriftAdjustmentRatio, - NudgeBeta: NudgeDriftAdjustmentRatio, + ImpulseBeta: s.impulseDriftAdjustmentRatio, + NudgeBeta: s.nudgeDriftAdjustmentRatio, } constraint.logic.Reset(ctx) constraint.logic.ApplyNudges(ctx) @@ -607,8 +584,8 @@ func (s *Scene) applyNudges(elapsedSeconds float64) { ctx := solver.Context{ Target: target, DeltaTime: elapsedSeconds, - ImpulseBeta: ImpulseDriftAdjustmentRatio, - NudgeBeta: NudgeDriftAdjustmentRatio, + ImpulseBeta: s.impulseDriftAdjustmentRatio, + NudgeBeta: s.nudgeDriftAdjustmentRatio, } constraint.logic.Reset(ctx) constraint.logic.ApplyNudges(ctx) @@ -943,7 +920,7 @@ func (s *Scene) allocateBodyAccelerator() (int32, *bodyAcceleratorState) { } func (s *Scene) releaseBodyAccelerator(index int32) { - panic("TODO") + s.freeBodyAcceleratorIndices.Push(index) } func (s *Scene) allocateSoloConstraint() (int32, *soloConstraintState) { @@ -999,8 +976,19 @@ func (s *Scene) eachBody(cb func(index int, b *bodyState)) { } } -type propRef struct { - index uint32 +func (s *Scene) allocateTerrain() (int32, *terrainState) { + var index int32 + if s.freeTerrainIndices.IsEmpty() { + index = int32(len(s.terrains)) + s.terrains = append(s.terrains, terrainState{}) + } else { + index = s.freeTerrainIndices.Pop() + } + return index, &s.terrains[index] +} + +func (s *Scene) releaseTerrain(index int32) { + s.freeTerrainIndices.Push(index) } type sbCollisionPair struct { @@ -1014,3 +1002,18 @@ type dbCollisionPair struct { } var nilIndex int32 = -1 + +type bodyData struct { + index int32 +} + +type shapeData struct { + frictionCoefficient float64 + restitutionCoefficient float64 +} + +type terrainData struct { + index int32 + frictionCoefficient float64 + restitutionCoefficient float64 +} diff --git a/game/physics/terrain.go b/game/physics/terrain.go new file mode 100644 index 00000000..202ca61d --- /dev/null +++ b/game/physics/terrain.go @@ -0,0 +1,102 @@ +package physics + +import ( + "github.com/mokiat/gog/opt" + "github.com/mokiat/gomath/dprec" + "github.com/mokiat/lacking/core/spatial/placement3d" +) + +type TerrainID struct { + index int32 + revision int32 +} + +var NilTerrainID = TerrainID{} + +type TerrainView struct { + scene *Scene +} + +// TODO: Rework the Terrain such that the Mesh is attached similar to how +// collision shapes are attached to bodies. This should open the door for +// other types of terrain, such as heightmaps, etc. +// +// However, this requires rework of placement3d API. + +func (v TerrainView) Create(position dprec.Vec3, rotation dprec.Quat, mesh CollisionMesh) TerrainID { + index, terrain := v.scene.allocateTerrain() + + meshID := v.scene.collisionScene.CreateMesh(placement3d.MeshInfo[terrainData]{ + Position: opt.V(position), + Rotation: opt.V(rotation), + Mesh: mesh.Shape, + Filtering: mesh.Filtering, + UserData: terrainData{ + index: index, + frictionCoefficient: mesh.FrictionCoefficient, + restitutionCoefficient: mesh.RestitutionCoefficient, + }, + }) + + *terrain = terrainState{ + meshID: meshID, + revision: terrain.revision + 1, // progress revision to valid (odd) value + } + + return TerrainID{ + index: index, + revision: terrain.revision, + } +} + +func (v TerrainView) Delete(id TerrainID) { + terrain := v.resolve(id, true) + + v.scene.collisionScene.DeleteMesh(terrain.meshID) + + *terrain = terrainState{ + meshID: placement3d.InvalidMeshID, + revision: terrain.revision + 1, // progress revision to invalid (even) value + } + + v.scene.releaseTerrain(id.index) +} + +func (v TerrainView) Handle(id TerrainID) TerrainHandle { + return TerrainHandle{ + view: v, + id: id, + } +} + +func (v TerrainView) IsValid(id TerrainID) bool { + terrain := v.resolve(id, false) + return terrain != nil +} + +func (v TerrainView) resolve(id TerrainID, required bool) *terrainState { + if id.revision == 0 { + if required { + panic("invalid terrain ID") + } + return nil + } + terrain := &v.scene.terrains[id.index] + if terrain.revision != id.revision { + if required { + panic("invalid terrain ID") + } + return nil + } + return terrain +} + +type TerrainHandle struct { + view TerrainView + id TerrainID +} + +type terrainState struct { + meshID placement3d.MeshID + revision int32 +} From 4f78ba71e45f37dcd6be70c5848ddb2ee35def2f Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 23:32:35 +0300 Subject: [PATCH 27/85] More code changes --- game/physics/scene.go | 194 ++++++++++++++++++++++++------------------ 1 file changed, 112 insertions(+), 82 deletions(-) diff --git a/game/physics/scene.go b/game/physics/scene.go index b745d0c4..09c75289 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -12,7 +12,6 @@ import ( "github.com/mokiat/lacking/core/spatial/shape3d" "github.com/mokiat/lacking/debug/metric" "github.com/mokiat/lacking/game/physics/constraint" - "github.com/mokiat/lacking/game/physics/solver" "github.com/mokiat/lacking/util/observer" ) @@ -366,6 +365,7 @@ func (s *Scene) Update(elapsedTime time.Duration) { s.notifyDoubleBodyCollisions() } +// TODO: Move to BodyView func (s *Scene) Each(cb func(b Body)) { s.eachBody(func(_ int, b *bodyState) { cb(Body{ @@ -497,47 +497,68 @@ func (s *Scene) applyAcceleration(elapsedSeconds float64) { func (s *Scene) applyImpulses(elapsedSeconds float64) { defer metric.BeginRegion("impulses").End() - s.eachDBConstraintState(func(_ int, constraint *dbConstraintState) { - target := &s.bodyConstraintPlaceholders[constraint.primary.reference.Index] - source := &s.bodyConstraintPlaceholders[constraint.secondary.reference.Index] - constraint.logic.Reset(solver.PairContext{ - Target: target, - Source: source, - DeltaTime: elapsedSeconds, - ImpulseBeta: ImpulseDriftAdjustmentRatio, - NudgeBeta: NudgeDriftAdjustmentRatio, - }) + // TODO: If it is decided that the Reset should only be called once per + // iteration, then extract the reset logic into a separate method. + + // Reset constraints. + s.eachEnabledPairConstraint(func(_ int, constraint *pairConstraintState) { + primaryBody := &s.bodies[constraint.primaryBodyIndex] + secondaryBody := &s.bodies[constraint.secondaryBodyIndex] + + ctx := PairConstraintContext{ + DeltaSeconds: elapsedSeconds, + ImpulseBeta: s.impulseDriftAdjustmentRatio, + NudgeBeta: s.nudgeDriftAdjustmentRatio, + PrimaryTarget: newConstraintTarget(primaryBody), + SecondaryTarget: newConstraintTarget(secondaryBody), + } + + constraint.solver.Reset(ctx) }) - s.eachSBConstraintState(func(_ int, constraint *sbConstraintState) { - target := &s.bodyConstraintPlaceholders[constraint.body.reference.Index] - constraint.logic.Reset(solver.Context{ - Target: target, - DeltaTime: elapsedSeconds, - ImpulseBeta: ImpulseDriftAdjustmentRatio, - NudgeBeta: NudgeDriftAdjustmentRatio, - }) + + s.eachEnabledSoloConstraint(func(index int, constraint *soloConstraintState) { + body := &s.bodies[constraint.bodyIndex] + + ctx := SoloConstraintContext{ + DeltaSeconds: elapsedSeconds, + ImpulseBeta: s.impulseDriftAdjustmentRatio, + NudgeBeta: s.nudgeDriftAdjustmentRatio, + Target: newConstraintTarget(body), + } + + constraint.solver.Reset(ctx) }) - for range ImpulseIterationCount { - s.eachDBConstraintState(func(_ int, constraint *dbConstraintState) { - target := &s.bodyConstraintPlaceholders[constraint.primary.reference.Index] - source := &s.bodyConstraintPlaceholders[constraint.secondary.reference.Index] - constraint.logic.ApplyImpulses(solver.PairContext{ - Target: target, - Source: source, - DeltaTime: elapsedSeconds, - ImpulseBeta: ImpulseDriftAdjustmentRatio, - NudgeBeta: NudgeDriftAdjustmentRatio, - }) + for range s.impulseIterationCount { + // Apply pair constraints first on purpose. We want since solo constraints + // to have the last word. + + s.eachEnabledPairConstraint(func(index int, constraint *pairConstraintState) { + primaryBody := &s.bodies[constraint.primaryBodyIndex] + secondaryBody := &s.bodies[constraint.secondaryBodyIndex] + + ctx := PairConstraintContext{ + DeltaSeconds: elapsedSeconds, + ImpulseBeta: s.impulseDriftAdjustmentRatio, + NudgeBeta: s.nudgeDriftAdjustmentRatio, + PrimaryTarget: newConstraintTarget(primaryBody), + SecondaryTarget: newConstraintTarget(secondaryBody), + } + + constraint.solver.ApplyImpulses(ctx) }) - s.eachSBConstraintState(func(_ int, constraint *sbConstraintState) { - target := &s.bodyConstraintPlaceholders[constraint.body.reference.Index] - constraint.logic.ApplyImpulses(solver.Context{ - Target: target, - DeltaTime: elapsedSeconds, - ImpulseBeta: ImpulseDriftAdjustmentRatio, - NudgeBeta: NudgeDriftAdjustmentRatio, - }) + + s.eachEnabledSoloConstraint(func(index int, constraint *soloConstraintState) { + body := &s.bodies[constraint.bodyIndex] + + ctx := SoloConstraintContext{ + DeltaSeconds: elapsedSeconds, + ImpulseBeta: s.impulseDriftAdjustmentRatio, + NudgeBeta: s.nudgeDriftAdjustmentRatio, + Target: newConstraintTarget(body), + } + + constraint.solver.ApplyImpulses(ctx) }) } } @@ -559,37 +580,44 @@ func (s *Scene) applyMotion(elapsedSeconds float64) { func (s *Scene) applyNudges(elapsedSeconds float64) { defer metric.BeginRegion("nudges").End() + // TODO: Figure out if the Reset calls below are really necessary. + // On one side, it is true that each Nudge repositions the bodies and + // a Reset allows for a more correct Jacobian. On the other hand it + // might be wasteful to do so and contradicts the godoc for the method. + for range s.nudgeIterationCount { - for _, constraint := range s.dbConstraints { - if !constraint.IsActive() { - continue - } - target := &s.bodyConstraintPlaceholders[constraint.primary.reference.Index] - source := &s.bodyConstraintPlaceholders[constraint.secondary.reference.Index] - ctx := solver.PairContext{ - Target: target, - Source: source, - DeltaTime: elapsedSeconds, - ImpulseBeta: s.impulseDriftAdjustmentRatio, - NudgeBeta: s.nudgeDriftAdjustmentRatio, + // Apply pair constraints first on purpose. We want since solo constraints + // to have the last word. + + s.eachEnabledPairConstraint(func(index int, constraint *pairConstraintState) { + primaryBody := &s.bodies[constraint.primaryBodyIndex] + secondaryBody := &s.bodies[constraint.secondaryBodyIndex] + + ctx := PairConstraintContext{ + DeltaSeconds: elapsedSeconds, + ImpulseBeta: s.impulseDriftAdjustmentRatio, + NudgeBeta: s.nudgeDriftAdjustmentRatio, + PrimaryTarget: newConstraintTarget(primaryBody), + SecondaryTarget: newConstraintTarget(secondaryBody), } - constraint.logic.Reset(ctx) - constraint.logic.ApplyNudges(ctx) - } - for _, constraint := range s.sbConstraints { - if !constraint.IsActive() { - continue - } - target := &s.bodyConstraintPlaceholders[constraint.body.reference.Index] - ctx := solver.Context{ - Target: target, - DeltaTime: elapsedSeconds, - ImpulseBeta: s.impulseDriftAdjustmentRatio, - NudgeBeta: s.nudgeDriftAdjustmentRatio, + + constraint.solver.Reset(ctx) + constraint.solver.ApplyNudges(ctx) + }) + + s.eachEnabledSoloConstraint(func(index int, constraint *soloConstraintState) { + body := &s.bodies[constraint.bodyIndex] + + ctx := SoloConstraintContext{ + DeltaSeconds: elapsedSeconds, + ImpulseBeta: s.impulseDriftAdjustmentRatio, + NudgeBeta: s.nudgeDriftAdjustmentRatio, + Target: newConstraintTarget(body), } - constraint.logic.Reset(ctx) - constraint.logic.ApplyNudges(ctx) - } + + constraint.solver.Reset(ctx) + constraint.solver.ApplyNudges(ctx) + }) } } @@ -859,22 +887,6 @@ func (s *Scene) notifyDoubleBodyCollisions() { clear(s.newDBCollisions) } -func (s *Scene) eachDBConstraintState(cb func(index int, constraint *dbConstraintState)) { - for i := range s.dbConstraints { - if constraint := &s.dbConstraints[i]; constraint.IsActive() { - cb(i, constraint) - } - } -} - -func (s *Scene) resolveBodyState(reference indexReference) *bodyState { - state := &s.bodies[reference.Index] - if !state.IsActive() || state.reference.Revision != reference.Revision { - return nil - } - return state -} - func (s *Scene) allocateGlobalAccelerator() (int32, *globalAcceleratorState) { var index int32 if s.freeGlobalAcceleratorIndices.IsEmpty() { @@ -938,6 +950,15 @@ func (s *Scene) releaseSoloConstraint(index int32) { s.freeSoloConstraintIndices.Push(index) } +func (s *Scene) eachEnabledSoloConstraint(cb func(index int, constraint *soloConstraintState)) { + for i := range s.soloConstraints { + constraint := &s.soloConstraints[i] + if constraint.isValid() && constraint.isEnabled { + cb(i, constraint) + } + } +} + func (s *Scene) allocatePairConstraint() (int32, *pairConstraintState) { var index int32 if s.freePairConstraintIndices.IsEmpty() { @@ -953,6 +974,15 @@ func (s *Scene) releasePairConstraint(index int32) { s.freePairConstraintIndices.Push(index) } +func (s *Scene) eachEnabledPairConstraint(cb func(index int, constraint *pairConstraintState)) { + for i := range s.pairConstraints { + constraint := &s.pairConstraints[i] + if constraint.isValid() && constraint.isEnabled { + cb(i, constraint) + } + } +} + func (s *Scene) allocateBody() (int32, *bodyState) { var index int32 if s.freeBodyIndices.IsEmpty() { From 8503235291672e7a0bf0fa79ef62ada052899464 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 2 Aug 2026 23:43:16 +0300 Subject: [PATCH 28/85] Minor comment adjustments --- game/physics/constraint_pair.go | 17 ++++++++++++----- game/physics/constraint_solo.go | 17 ++++++++++++----- game/physics/scene.go | 17 ++--------------- 3 files changed, 26 insertions(+), 25 deletions(-) diff --git a/game/physics/constraint_pair.go b/game/physics/constraint_pair.go index 117b5454..2e4536d1 100644 --- a/game/physics/constraint_pair.go +++ b/game/physics/constraint_pair.go @@ -36,12 +36,19 @@ type PairConstraintContext struct { // below during each simulation step. type PairConstraintSolver interface { - // Reset clears any internal cache state held by the solver, in - // preparation for a new physics simulation step. + // Reset clears any internal cache state held by the solver and + // recomputes any data (e.g. Jacobians) that is derived from the + // current position and orientation of the target bodies. // - // This is called once at the start of every step, before - // [PairConstraintSolver.ApplyImpulses] or [PairConstraintSolver.ApplyNudges] - // are invoked. + // This is called once before the first + // [PairConstraintSolver.ApplyImpulses] iteration of a step, since the + // target bodies' positions and orientations remain unchanged + // throughout that loop. + // + // This is also called before every single + // [PairConstraintSolver.ApplyNudges] invocation, since nudges + // reposition the target bodies and would otherwise leave the solver's + // cached, position-derived data stale for subsequent iterations. Reset(ctx PairConstraintContext) // ApplyImpulses is called by the physics engine to instruct the diff --git a/game/physics/constraint_solo.go b/game/physics/constraint_solo.go index 919a81da..916c2f40 100644 --- a/game/physics/constraint_solo.go +++ b/game/physics/constraint_solo.go @@ -31,12 +31,19 @@ type SoloConstraintContext struct { // below during each simulation step. type SoloConstraintSolver interface { - // Reset clears any internal cache state held by the solver, in - // preparation for a new physics simulation step. + // Reset clears any internal cache state held by the solver and + // recomputes any data (e.g. Jacobians) that is derived from the + // current position and orientation of the target body. // - // This is called once at the start of every step, before - // [SoloConstraintSolver.ApplyImpulses] or [SoloConstraintSolver.ApplyNudges] - // are invoked. + // This is called once before the first + // [SoloConstraintSolver.ApplyImpulses] iteration of a step, since the + // target body's position and orientation remain unchanged throughout + // that loop. + // + // This is also called before every single + // [SoloConstraintSolver.ApplyNudges] invocation, since nudges + // reposition the target body and would otherwise leave the solver's + // cached, position-derived data stale for subsequent iterations. Reset(ctx SoloConstraintContext) // ApplyImpulses is called by the physics engine to instruct the solver diff --git a/game/physics/scene.go b/game/physics/scene.go index 09c75289..311f4e94 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -497,10 +497,7 @@ func (s *Scene) applyAcceleration(elapsedSeconds float64) { func (s *Scene) applyImpulses(elapsedSeconds float64) { defer metric.BeginRegion("impulses").End() - // TODO: If it is decided that the Reset should only be called once per - // iteration, then extract the reset logic into a separate method. - - // Reset constraints. + // Reset constraint solvers. s.eachEnabledPairConstraint(func(_ int, constraint *pairConstraintState) { primaryBody := &s.bodies[constraint.primaryBodyIndex] secondaryBody := &s.bodies[constraint.secondaryBodyIndex] @@ -529,10 +526,8 @@ func (s *Scene) applyImpulses(elapsedSeconds float64) { constraint.solver.Reset(ctx) }) + // Apply impulses multiple times in a row. for range s.impulseIterationCount { - // Apply pair constraints first on purpose. We want since solo constraints - // to have the last word. - s.eachEnabledPairConstraint(func(index int, constraint *pairConstraintState) { primaryBody := &s.bodies[constraint.primaryBodyIndex] secondaryBody := &s.bodies[constraint.secondaryBodyIndex] @@ -580,15 +575,7 @@ func (s *Scene) applyMotion(elapsedSeconds float64) { func (s *Scene) applyNudges(elapsedSeconds float64) { defer metric.BeginRegion("nudges").End() - // TODO: Figure out if the Reset calls below are really necessary. - // On one side, it is true that each Nudge repositions the bodies and - // a Reset allows for a more correct Jacobian. On the other hand it - // might be wasteful to do so and contradicts the godoc for the method. - for range s.nudgeIterationCount { - // Apply pair constraints first on purpose. We want since solo constraints - // to have the last word. - s.eachEnabledPairConstraint(func(index int, constraint *pairConstraintState) { primaryBody := &s.bodies[constraint.primaryBodyIndex] secondaryBody := &s.bodies[constraint.secondaryBodyIndex] From bfdb2de6b9f0eba34994e57eef219e81b64e0b21 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Mon, 3 Aug 2026 00:11:40 +0300 Subject: [PATCH 29/85] More code adjustments --- game/physics/callback.go | 18 ++--- game/physics/constraint.go | 41 +++++++++++ game/physics/scene.go | 119 +++++++++++++++++++------------- game/physics/solver/jacobian.go | 41 ----------- 4 files changed, 120 insertions(+), 99 deletions(-) diff --git a/game/physics/callback.go b/game/physics/callback.go index 81d643c1..0b637011 100644 --- a/game/physics/callback.go +++ b/game/physics/callback.go @@ -2,18 +2,18 @@ package physics import "github.com/mokiat/lacking/util/observer" -// SoloBodyCollisionCallback is a mechanism to receive notifications -// about collisions between a body and a prop in the scene. -type SoloBodyCollisionCallback func(bodyID BodyID, terrainID TerrainID, active bool) +// SoloCollisionCallback is a mechanism to receive notifications +// about collisions between a body and a terrain in the scene. +type SoloCollisionCallback func(bodyID BodyID, terrainID TerrainID, active bool) -// SoloBodyCollisionSubscription represents a notification subscription +// SoloCollisionSubscription represents a notification subscription // for single body collisions. -type SoloBodyCollisionSubscription = observer.Subscription[SoloBodyCollisionCallback] +type SoloCollisionSubscription = observer.Subscription[SoloCollisionCallback] -// PairBodyCollisionCallback is a mechanism to receive notifications +// PairCollisionCallback is a mechanism to receive notifications // about collisions between two bodies. -type PairBodyCollisionCallback func(firstBodyID, secondBodyID BodyID, active bool) +type PairCollisionCallback func(firstBodyID, secondBodyID BodyID, active bool) -// PairBodyCollisionSubscription represents a notification subscription +// PairCollisionSubscription represents a notification subscription // for double body collisions. -type PairBodyCollisionSubscription = observer.Subscription[PairBodyCollisionCallback] +type PairCollisionSubscription = observer.Subscription[PairCollisionCallback] diff --git a/game/physics/constraint.go b/game/physics/constraint.go index 88f3d0cc..15a08d35 100644 --- a/game/physics/constraint.go +++ b/game/physics/constraint.go @@ -95,3 +95,44 @@ func (t ConstraintTarget) ApplyNudge(nudge Nudge) { t.body.translate(dprec.Vec3Prod(nudge.Linear, t.body.invMass)) t.body.rotate(QuatFromVector(dprec.Mat3Vec3Prod(t.body.invInertia, nudge.Angular))) } + +// Jacobian represents the 1x6 Jacobian matrix of a single-object velocity +// constraint. +type Jacobian struct { + LinearSlope dprec.Vec3 + AngularSlope dprec.Vec3 +} + +// EffectiveVelocity returns the amount of velocity in the wrong direction +// of the target. +func (j Jacobian) EffectiveVelocity(target ConstraintTarget) float64 { + linear := dprec.Vec3Dot(j.LinearSlope, target.LinearVelocity()) + angular := dprec.Vec3Dot(j.AngularSlope, target.AngularVelocity()) + return linear + angular +} + +// InverseEffectiveMass returns the inverse of the effective mass with which +// the target affects the constraint. +func (j Jacobian) InverseEffectiveMass(target ConstraintTarget) float64 { + linear := dprec.Vec3Dot(j.LinearSlope, j.LinearSlope) * target.InverseMass() + angular := dprec.Vec3Dot(dprec.Mat3Vec3Prod(target.InverseInertia(), j.AngularSlope), j.AngularSlope) + return linear + angular +} + +// Impulse returns an Impulse solution based on the lambda impulse +// amount applied according to this Jacobian. +func (j Jacobian) Impulse(lambda float64) Impulse { + return Impulse{ + Linear: dprec.Vec3Prod(j.LinearSlope, lambda), + Angular: dprec.Vec3Prod(j.AngularSlope, lambda), + } +} + +// Nudge returns a nudge solution based on the lambda nudge amount +// applied according to this Jacobian. +func (j Jacobian) Nudge(lambda float64) Nudge { + return Nudge{ + Linear: dprec.Vec3Prod(j.LinearSlope, lambda), + Angular: dprec.Vec3Prod(j.AngularSlope, lambda), + } +} diff --git a/game/physics/scene.go b/game/physics/scene.go index 311f4e94..fc0fb8ca 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -19,11 +19,6 @@ import ( // a number of bodies that are independent on any // bodies managed by other scene objects. type Scene struct { - collisionScene *placement3d.Scene[bodyData, shapeData, terrainData] - - sbCollisionSubscriptions *observer.SubscriptionSet[SoloBodyCollisionCallback] - dbCollisionSubscriptions *observer.SubscriptionSet[PairBodyCollisionCallback] - timeSpeed float64 sbCollisionConstraints []SBConstraint @@ -40,9 +35,14 @@ type Scene struct { oldDBCollisions map[dbCollisionPair]struct{} newDBCollisions map[dbCollisionPair]struct{} + // ---------- NEW BELOW---------- (TODO: REMOVE COMMENT) + collisionScene *placement3d.Scene[bodyData, shapeData, terrainData] + + soloCollisionSubscriptions *observer.SubscriptionSet[SoloCollisionCallback] + pairCollisionSubscriptions *observer.SubscriptionSet[PairCollisionCallback] + freeCollisionRejectGroup uint32 - // ---------- NEW BELOW---------- (TODO: REMOVE COMMENT) mediumSolver MediumSolver freeGlobalAcceleratorIndices *ds.Stack[int32] @@ -72,16 +72,6 @@ type Scene struct { func NewScene() *Scene { return &Scene{ - collisionScene: placement3d.NewScene[bodyData, shapeData, terrainData](placement3d.SceneSettings{ - Size: opt.V(16384.0), - MaxDepth: opt.V[uint32](12), - InitialNodeCapacity: opt.V[uint32](1024), - InitialItemCapacity: opt.V[uint32](1024), - }), - - sbCollisionSubscriptions: observer.NewSubscriptionSet[SoloBodyCollisionCallback](), - dbCollisionSubscriptions: observer.NewSubscriptionSet[PairBodyCollisionCallback](), - timeSpeed: 1.0, collisionSet: make(placement3d.ContactList, 0, 128), @@ -93,6 +83,18 @@ func NewScene() *Scene { newDBCollisions: make(map[dbCollisionPair]struct{}, 32), // ---------- NEW BELOW---------- (TODO: REMOVE COMMENT) + collisionScene: placement3d.NewScene[bodyData, shapeData, terrainData](placement3d.SceneSettings{ + Size: opt.V(16384.0), + MaxDepth: opt.V[uint32](12), + InitialNodeCapacity: opt.V[uint32](1024), + InitialItemCapacity: opt.V[uint32](1024), + }), + + soloCollisionSubscriptions: observer.NewSubscriptionSet[SoloCollisionCallback](), + pairCollisionSubscriptions: observer.NewSubscriptionSet[PairCollisionCallback](), + + freeCollisionRejectGroup: 0, + mediumSolver: NewStaticAirSolver(), freeGlobalAcceleratorIndices: ds.EmptyStack[int32](), @@ -121,6 +123,40 @@ func NewScene() *Scene { } } +// SubscribeSoloCollision registers a callback that is invoked whenever +// a body starts or stops colliding with a terrain in the scene. +// +// Call [SoloCollisionSubscription.Delete] on the returned subscription +// to stop receiving notifications. +func (s *Scene) SubscribeSoloCollision(callback SoloCollisionCallback) *SoloCollisionSubscription { + return s.soloCollisionSubscriptions.Subscribe(callback) +} + +// SubscribePairCollision registers a callback that is invoked whenever +// two bodies start or stop colliding with each other. +// +// Call [PairCollisionSubscription.Delete] on the returned subscription +// to stop receiving notifications. +func (s *Scene) SubscribePairCollision(callback PairCollisionCallback) *PairCollisionSubscription { + return s.pairCollisionSubscriptions.Subscribe(callback) +} + +// NextCollisionRejectGroup returns a collision reject group that is unique +// within this Scene. Bodies that are assigned the same reject group do not +// collide with each other, which is useful for objects that are meant to +// overlap, such as the chassis and the wheels of a vehicle. +// +// The returned value is always larger than zero, since zero indicates that a +// body does not belong to any reject group and hence can collide with +// everything. +// +// Reject groups are never recycled. Each call returns a new value, even if all +// bodies that used a previously returned group have been deleted. +func (s *Scene) NextCollisionRejectGroup() uint32 { + s.freeCollisionRejectGroup++ + return s.freeCollisionRejectGroup +} + // MediumSolver returns the solver that is used to calculate the medium // properties of the scene. // @@ -317,18 +353,6 @@ func (s *Scene) SetNudgeDriftAdjustmentRatio(ratio float64) { /////// OLD BELOW ------------ (TODO: DELETE COMMENT) -// SubscribeSingleBodyCollision registers a callback that is invoked when a body -// collides with a static object. -func (s *Scene) SubscribeSingleBodyCollision(callback SoloBodyCollisionCallback) *SoloBodyCollisionSubscription { - return s.sbCollisionSubscriptions.Subscribe(callback) -} - -// SubscribeDoubleBodyCollision registers a callback that is invoked when two -// bodies collide. -func (s *Scene) SubscribeDoubleBodyCollision(callback PairBodyCollisionCallback) *PairBodyCollisionSubscription { - return s.dbCollisionSubscriptions.Subscribe(callback) -} - // TimeSpeed returns the speed at which time runs, where 1.0 is the default // and 0.0 is stopped. func (s *Scene) TimeSpeed() float64 { @@ -340,22 +364,6 @@ func (s *Scene) SetTimeSpeed(timeSpeed float64) { s.timeSpeed = timeSpeed } -// NextCollisionRejectGroup returns a collision reject group that is unique -// within this Scene. Bodies that are assigned the same reject group do not -// collide with each other, which is useful for objects that are meant to -// overlap, such as the chassis and the wheels of a vehicle. -// -// The returned value is always larger than zero, since zero indicates that a -// body does not belong to any reject group and hence can collide with -// everything. -// -// Reject groups are never recycled. Each call returns a new value, even if all -// bodies that used a previously returned group have been deleted. -func (s *Scene) NextCollisionRejectGroup() uint32 { - s.freeCollisionRejectGroup++ - return s.freeCollisionRejectGroup -} - // Update runs a single physics iteration. This method should be called with // fixed elapsed times, otherwise the physics may break. func (s *Scene) Update(elapsedTime time.Duration) { @@ -448,7 +456,9 @@ func (s *Scene) applyAcceleration(elapsedSeconds float64) { }) // Apply body accelerators. - // TODO: Implement body accelerators. + s.eachEnabledBodyAccelerator(body, func(_ int, accelerator *bodyAcceleratorState) { + accelerator.solver.ApplyAcceleration(ctx, target) + }) // Constrain the accumulated accelerations to the maximum allowed values. body.clampLinearAcceleration(s.maxLinearAcceleration) @@ -814,7 +824,7 @@ func (s *Scene) notifySingleBodyCollisions() { prop := Prop{ name: s.props[newCollision.PropRef.Index].name, } - s.sbCollisionSubscriptions.Each(func(callback SoloBodyCollisionCallback) { + s.soloCollisionSubscriptions.Each(func(callback SoloCollisionCallback) { callback(primary, prop, true) }) } @@ -828,7 +838,7 @@ func (s *Scene) notifySingleBodyCollisions() { prop := Prop{ name: s.props[oldCollision.PropRef.Index].name, } - s.sbCollisionSubscriptions.Each(func(callback SoloBodyCollisionCallback) { + s.soloCollisionSubscriptions.Each(func(callback SoloCollisionCallback) { callback(primary, prop, false) }) } @@ -849,7 +859,7 @@ func (s *Scene) notifyDoubleBodyCollisions() { scene: s, reference: newCollision.SecondaryRef, } - s.dbCollisionSubscriptions.Each(func(callback PairBodyCollisionCallback) { + s.pairCollisionSubscriptions.Each(func(callback PairCollisionCallback) { callback(primary, secondary, true) }) } @@ -864,7 +874,7 @@ func (s *Scene) notifyDoubleBodyCollisions() { scene: s, reference: oldCollision.SecondaryRef, } - s.dbCollisionSubscriptions.Each(func(callback PairBodyCollisionCallback) { + s.pairCollisionSubscriptions.Each(func(callback PairCollisionCallback) { callback(primary, secondary, false) }) } @@ -922,6 +932,17 @@ func (s *Scene) releaseBodyAccelerator(index int32) { s.freeBodyAcceleratorIndices.Push(index) } +func (s *Scene) eachEnabledBodyAccelerator(body *bodyState, cb func(index int, accelerator *bodyAcceleratorState)) { + index := body.firstBodyAcceleratorIndex + if index != nilIndex { + accelerator := &s.bodyAccelerators[index] + if accelerator.isValid() && accelerator.isEnabled { + cb(int(index), accelerator) + } + index = accelerator.nextIndex + } +} + func (s *Scene) allocateSoloConstraint() (int32, *soloConstraintState) { var index int32 if s.freeSoloConstraintIndices.IsEmpty() { diff --git a/game/physics/solver/jacobian.go b/game/physics/solver/jacobian.go index 1955b275..03566f0a 100644 --- a/game/physics/solver/jacobian.go +++ b/game/physics/solver/jacobian.go @@ -2,47 +2,6 @@ package solver import "github.com/mokiat/gomath/dprec" -// Jacobian represents the 1x6 Jacobian matrix of a single-object velocity -// constraint. -type Jacobian struct { - LinearSlope dprec.Vec3 - AngularSlope dprec.Vec3 -} - -// EffectiveVelocity returns the amount of velocity in the wrong direction -// of the target. -func (j Jacobian) EffectiveVelocity(target *Placeholder) float64 { - linear := dprec.Vec3Dot(j.LinearSlope, target.linearVelocity) - angular := dprec.Vec3Dot(j.AngularSlope, target.angularVelocity) - return linear + angular -} - -// InverseEffectiveMass returns the inverse of the effective mass with which -// the target affects the constraint. -func (j Jacobian) InverseEffectiveMass(target *Placeholder) float64 { - linear := dprec.Vec3Dot(j.LinearSlope, j.LinearSlope) * target.inverseMass - angular := dprec.Vec3Dot(dprec.Mat3Vec3Prod(target.inverseMomentOfInertia, j.AngularSlope), j.AngularSlope) - return linear + angular -} - -// Impulse returns an Impulse solution based on the lambda impulse -// amount applied according to this Jacobian. -func (j Jacobian) Impulse(lambda float64) Impulse { - return Impulse{ - Linear: dprec.Vec3Prod(j.LinearSlope, lambda), - Angular: dprec.Vec3Prod(j.AngularSlope, lambda), - } -} - -// Nudge returns a nudge solution based on the lambda nudge amount -// applied according to this Jacobian. -func (j Jacobian) Nudge(lambda float64) Nudge { - return Nudge{ - Linear: dprec.Vec3Prod(j.LinearSlope, lambda), - Angular: dprec.Vec3Prod(j.AngularSlope, lambda), - } -} - // PairJacobian represents the 1x12 Jacobian matrix of a double-object velocity // constraint. type PairJacobian struct { From 18743b6fda892e862b9167230fe01a03225a89e6 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Mon, 3 Aug 2026 00:26:56 +0300 Subject: [PATCH 30/85] More code rework --- game/physics/constraint.go | 72 +++++++++++++++++++++++++++++++++----- game/physics/scene.go | 69 +++++++++++++++++------------------- 2 files changed, 97 insertions(+), 44 deletions(-) diff --git a/game/physics/constraint.go b/game/physics/constraint.go index 15a08d35..69dcae08 100644 --- a/game/physics/constraint.go +++ b/game/physics/constraint.go @@ -2,109 +2,164 @@ package physics import "github.com/mokiat/gomath/dprec" +// Impulse represents a velocity-space correction, split into a linear and +// an angular component, that can be applied to a [ConstraintTarget] +// through [ConstraintTarget.ApplyImpulse]. type Impulse struct { - Linear dprec.Vec3 + + // Linear is the change in linear velocity to be applied. + Linear dprec.Vec3 + + // Angular is the change in angular velocity to be applied. Angular dprec.Vec3 } +// Nudge represents a position-space correction, split into a linear and +// an angular component, that can be applied to a [ConstraintTarget] +// through [ConstraintTarget.ApplyNudge]. +// +// It mirrors [Impulse], but acts on position and rotation directly +// instead of on velocity. type Nudge struct { - Linear dprec.Vec3 + + // Linear is the positional offset to be applied. + Linear dprec.Vec3 + + // Angular is the rotational offset to be applied, expressed as a + // scaled rotation vector (see [QuatFromVector]). Angular dprec.Vec3 } +// ConstraintTarget represents the body acted upon by a +// [SoloConstraintSolver] or [PairConstraintSolver]. It exposes the +// body's motion state to the solver and lets the solver correct that +// state through impulses and nudges. type ConstraintTarget struct { body *bodyState } +// newConstraintTarget creates a new ConstraintTarget that wraps body. func newConstraintTarget(body *bodyState) ConstraintTarget { return ConstraintTarget{ body: body, } } +// InverseMass returns the inverse of the target's mass. func (t ConstraintTarget) InverseMass() float64 { return t.body.invMass } +// Mass returns the target's mass. func (t ConstraintTarget) Mass() float64 { return t.body.mass() } +// InverseInertia returns the inverse of the target's world-space +// inertia tensor. func (t ConstraintTarget) InverseInertia() dprec.Mat3 { return t.body.invInertia } +// Inertia returns the target's world-space inertia tensor. func (t ConstraintTarget) Inertia() dprec.Mat3 { return t.body.inertia() } +// LinearVelocity returns the target's current linear velocity. func (t ConstraintTarget) LinearVelocity() dprec.Vec3 { return t.body.linearVelocity } +// SetLinearVelocity changes the target's linear velocity. func (t ConstraintTarget) SetLinearVelocity(velocity dprec.Vec3) { t.body.linearVelocity = velocity } +// AddLinearVelocity adds delta to the target's linear velocity. func (t ConstraintTarget) AddLinearVelocity(delta dprec.Vec3) { t.body.addLinearVelocity(delta) } +// AngularVelocity returns the target's current angular velocity. func (t ConstraintTarget) AngularVelocity() dprec.Vec3 { return t.body.angularVelocity } +// SetAngularVelocity changes the target's angular velocity. func (t ConstraintTarget) SetAngularVelocity(velocity dprec.Vec3) { t.body.angularVelocity = velocity } +// AddAngularVelocity adds delta to the target's angular velocity. func (t ConstraintTarget) AddAngularVelocity(delta dprec.Vec3) { t.body.addAngularVelocity(delta) } +// ApplyImpulse adjusts the target's linear and angular velocity +// according to impulse, scaled by the target's inverse mass and +// inverse inertia respectively. func (t ConstraintTarget) ApplyImpulse(impulse Impulse) { t.body.addLinearVelocity(dprec.Vec3Prod(impulse.Linear, t.body.invMass)) t.body.addAngularVelocity(dprec.Mat3Vec3Prod(t.body.invInertia, impulse.Angular)) } +// Position returns the target's current position. func (t ConstraintTarget) Position() dprec.Vec3 { return t.body.position } +// SetPosition changes the target's position. func (t ConstraintTarget) SetPosition(position dprec.Vec3) { t.body.position = position } +// Translate offsets the target's position by delta. func (t ConstraintTarget) Translate(delta dprec.Vec3) { t.body.translate(delta) } +// Rotation returns the target's current rotation. func (t ConstraintTarget) Rotation() dprec.Quat { return t.body.rotation } +// SetRotation changes the target's rotation. func (t ConstraintTarget) SetRotation(rotation dprec.Quat) { t.body.rotation = rotation } +// Rotate applies rotation on top of the target's current rotation. func (t ConstraintTarget) Rotate(rotation dprec.Quat) { t.body.rotate(rotation) } +// ApplyNudge adjusts the target's position and rotation according to +// nudge, scaled by the target's inverse mass and inverse inertia +// respectively, the same way [ConstraintTarget.ApplyImpulse] adjusts +// velocity. func (t ConstraintTarget) ApplyNudge(nudge Nudge) { t.body.translate(dprec.Vec3Prod(nudge.Linear, t.body.invMass)) t.body.rotate(QuatFromVector(dprec.Mat3Vec3Prod(t.body.invInertia, nudge.Angular))) } // Jacobian represents the 1x6 Jacobian matrix of a single-object velocity -// constraint. +// constraint, split into the 1x3 blocks that act on linear and angular +// velocity respectively. type Jacobian struct { - LinearSlope dprec.Vec3 + + // LinearSlope is the block of the Jacobian that acts on linear + // velocity. + LinearSlope dprec.Vec3 + + // AngularSlope is the block of the Jacobian that acts on angular + // velocity. AngularSlope dprec.Vec3 } // EffectiveVelocity returns the amount of velocity in the wrong direction -// of the target. +// of the constraint, i.e. the constraint equation's velocity error, +// evaluated against target's current linear and angular velocity. func (j Jacobian) EffectiveVelocity(target ConstraintTarget) float64 { linear := dprec.Vec3Dot(j.LinearSlope, target.LinearVelocity()) angular := dprec.Vec3Dot(j.AngularSlope, target.AngularVelocity()) @@ -112,14 +167,15 @@ func (j Jacobian) EffectiveVelocity(target ConstraintTarget) float64 { } // InverseEffectiveMass returns the inverse of the effective mass with which -// the target affects the constraint. +// target affects the constraint (i.e. J * M^-1 * J^T, where J is this +// Jacobian and M is target's mass-inertia matrix). func (j Jacobian) InverseEffectiveMass(target ConstraintTarget) float64 { linear := dprec.Vec3Dot(j.LinearSlope, j.LinearSlope) * target.InverseMass() angular := dprec.Vec3Dot(dprec.Mat3Vec3Prod(target.InverseInertia(), j.AngularSlope), j.AngularSlope) return linear + angular } -// Impulse returns an Impulse solution based on the lambda impulse +// Impulse returns an [Impulse] solution based on the lambda impulse // amount applied according to this Jacobian. func (j Jacobian) Impulse(lambda float64) Impulse { return Impulse{ @@ -128,7 +184,7 @@ func (j Jacobian) Impulse(lambda float64) Impulse { } } -// Nudge returns a nudge solution based on the lambda nudge amount +// Nudge returns a [Nudge] solution based on the lambda nudge amount // applied according to this Jacobian. func (j Jacobian) Nudge(lambda float64) Nudge { return Nudge{ diff --git a/game/physics/scene.go b/game/physics/scene.go index fc0fb8ca..ba95f992 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -19,8 +19,6 @@ import ( // a number of bodies that are independent on any // bodies managed by other scene objects. type Scene struct { - timeSpeed float64 - sbCollisionConstraints []SBConstraint sbCollisionSolvers []constraint.Collision @@ -68,12 +66,12 @@ type Scene struct { impulseDriftAdjustmentRatio float64 nudgeIterationCount int nudgeDriftAdjustmentRatio float64 + + timeScale float64 } func NewScene() *Scene { return &Scene{ - timeSpeed: 1.0, - collisionSet: make(placement3d.ContactList, 0, 128), oldSBCollisions: make(map[sbCollisionPair]struct{}, 32), @@ -120,6 +118,8 @@ func NewScene() *Scene { impulseDriftAdjustmentRatio: 0.2, nudgeIterationCount: 8, nudgeDriftAdjustmentRatio: 0.2, + + timeScale: 1.0, } } @@ -351,28 +351,45 @@ func (s *Scene) SetNudgeDriftAdjustmentRatio(ratio float64) { s.nudgeDriftAdjustmentRatio = ratio } -/////// OLD BELOW ------------ (TODO: DELETE COMMENT) - -// TimeSpeed returns the speed at which time runs, where 1.0 is the default -// and 0.0 is stopped. -func (s *Scene) TimeSpeed() float64 { - return s.timeSpeed +// TimeScale returns the multiplier applied to elapsed real time before it +// is fed into the physics simulation through [Scene.Update], where 1.0 is +// the default (real-time) rate and 0.0 pauses the simulation. +// +// The returned value is never negative. +func (s *Scene) TimeScale() float64 { + return s.timeScale } -// SetTimeSpeed changes the rate at which time runs. -func (s *Scene) SetTimeSpeed(timeSpeed float64) { - s.timeSpeed = timeSpeed +// SetTimeScale changes the multiplier applied to elapsed real time before +// it is fed into the physics simulation through [Scene.Update]. +// +// Negative values are clamped to 0, since the simulation does not support +// running time backwards. +func (s *Scene) SetTimeScale(scale float64) { + s.timeScale = max(0.0, scale) } -// Update runs a single physics iteration. This method should be called with -// fixed elapsed times, otherwise the physics may break. +// Update advances the physics simulation by elapsedTime, scaled by +// [Scene.TimeScale], and notifies any collision subscribers registered +// through [Scene.SubscribeSoloCollision] and [Scene.SubscribePairCollision] +// of collisions that started or stopped as a result. +// +// elapsedTime must be a fixed, consistent duration across calls (e.g. the +// ticks produced by a fixed-interval segmenter), since the impulse and +// nudge resolution assume a stable step size; a varying elapsedTime will +// make the simulation inaccurate or unstable. +// +// A [Scene.TimeScale] of 0 effectively pauses the simulation: Update can +// still be called on a fixed schedule, but no motion is integrated. func (s *Scene) Update(elapsedTime time.Duration) { elapsedSeconds := elapsedTime.Seconds() - s.runSimulation(elapsedSeconds * s.timeSpeed) + s.runSimulation(elapsedSeconds * s.timeScale) s.notifySingleBodyCollisions() s.notifyDoubleBodyCollisions() } +/////// OLD BELOW ------------ (TODO: DELETE COMMENT) + // TODO: Move to BodyView func (s *Scene) Each(cb func(b Body)) { s.eachBody(func(_ int, b *bodyState) { @@ -403,26 +420,6 @@ func (s *Scene) CheckSegmentIntersection(segment shape3d.Segment, mask uint32) ( }, true } -// func (s *Scene) Nearby(body Body, distance float64, cb func(b Body)) { -// state := s.resolveBodyState(body.reference) -// if state == nil { -// return -// } -// region := spatial.CuboidRegion( -// state.position, -// dprec.NewVec3(distance, distance, distance), -// ) -// s.bodyOctree.VisitHexahedronRegion(®ion, spatial.VisitorFunc[uint32](func(candidate uint32) { -// candidateState := &s.bodies[candidate] -// if candidateState != state { -// cb(Body{ -// scene: s, -// reference: candidateState.reference, -// }) -// } -// })) -// } - func (s *Scene) runSimulation(elapsedSeconds float64) { if elapsedSeconds > 0.0001 { s.applyAcceleration(elapsedSeconds) From 0f9f87846460aa0e37d408ac62b6685b26efe7ad Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Mon, 3 Aug 2026 00:36:35 +0300 Subject: [PATCH 31/85] Add Each mothods to views --- game/physics/accelerator_body.go | 9 + game/physics/accelerator_global.go | 9 + game/physics/body.go | 9 + game/physics/constraint_pair.go | 9 + game/physics/constraint_solo.go | 9 + game/physics/scene.go | 335 ++++++++++++++++------------- game/physics/terrain.go | 13 ++ 7 files changed, 239 insertions(+), 154 deletions(-) diff --git a/game/physics/accelerator_body.go b/game/physics/accelerator_body.go index 4c1d38ec..a31ad39e 100644 --- a/game/physics/accelerator_body.go +++ b/game/physics/accelerator_body.go @@ -91,6 +91,15 @@ func (v BodyAcceleratorView) Delete(id BodyAcceleratorID) { v.scene.releaseBodyAccelerator(id.index) } +func (v BodyAcceleratorView) Each(cb func(id BodyAcceleratorID)) { + v.scene.eachBodyAccelerator(func(index int, accelerator *bodyAcceleratorState) { + cb(BodyAcceleratorID{ + index: int32(index), + revision: accelerator.revision, + }) + }) +} + // Handle returns a [BodyAcceleratorHandle] that wraps the specified ID, as // a more convenient means of repeatedly accessing the same body accelerator // without having to pass its ID to this view on every call. diff --git a/game/physics/accelerator_global.go b/game/physics/accelerator_global.go index 4cae6e8b..7138a114 100644 --- a/game/physics/accelerator_global.go +++ b/game/physics/accelerator_global.go @@ -62,6 +62,15 @@ func (v GlobalAcceleratorView) Delete(id GlobalAcceleratorID) { v.scene.releaseGlobalAccelerator(id.index) } +func (v GlobalAcceleratorView) Each(cb func(id GlobalAcceleratorID)) { + v.scene.eachGlobalAccelerator(func(index int, accelerator *globalAcceleratorState) { + cb(GlobalAcceleratorID{ + index: int32(index), + revision: accelerator.revision, + }) + }) +} + // Handle returns a [GlobalAcceleratorHandle] that wraps the specified ID, // as a more convenient means of repeatedly accessing the same global // accelerator without having to pass its ID to this view on every call. diff --git a/game/physics/body.go b/game/physics/body.go index 0bc1ac6e..1412a8b5 100644 --- a/game/physics/body.go +++ b/game/physics/body.go @@ -84,6 +84,15 @@ func (v BodyView) Delete(id BodyID) { v.scene.releaseBody(id.index) } +func (v BodyView) Each(cb func(BodyID)) { + v.scene.eachBody(func(index int, body *bodyState) { + cb(BodyID{ + index: int32(index), + revision: body.revision, + }) + }) +} + func (v BodyView) Handle(id BodyID) BodyHandle { return BodyHandle{ view: v, diff --git a/game/physics/constraint_pair.go b/game/physics/constraint_pair.go index 2e4536d1..55e02716 100644 --- a/game/physics/constraint_pair.go +++ b/game/physics/constraint_pair.go @@ -213,6 +213,15 @@ func (v PairConstraintView) Delete(id PairConstraintID) { v.scene.releasePairConstraint(id.index) } +func (v PairConstraintView) Each(cb func(id PairConstraintID)) { + v.scene.eachPairConstraint(func(index int, constraint *pairConstraintState) { + cb(PairConstraintID{ + index: int32(index), + revision: constraint.revision, + }) + }) +} + // Handle returns a [PairConstraintHandle] that wraps id, offering a more // convenient, object-oriented way to interact with the referenced pair // constraint. diff --git a/game/physics/constraint_solo.go b/game/physics/constraint_solo.go index 916c2f40..057eb9dd 100644 --- a/game/physics/constraint_solo.go +++ b/game/physics/constraint_solo.go @@ -151,6 +151,15 @@ func (v SoloConstraintView) Delete(id SoloConstraintID) { v.scene.releaseSoloConstraint(id.index) } +func (v SoloConstraintView) Each(cb func(id SoloConstraintID)) { + v.scene.eachSoloConstraint(func(index int, constraint *soloConstraintState) { + cb(SoloConstraintID{ + index: int32(index), + revision: constraint.revision, + }) + }) +} + // Handle returns a [SoloConstraintHandle] that wraps id, offering a more // convenient, object-oriented way to interact with the referenced solo // constraint. diff --git a/game/physics/scene.go b/game/physics/scene.go index ba95f992..01add9b9 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -388,18 +388,190 @@ func (s *Scene) Update(elapsedTime time.Duration) { s.notifyDoubleBodyCollisions() } -/////// OLD BELOW ------------ (TODO: DELETE COMMENT) +func (s *Scene) allocateGlobalAccelerator() (int32, *globalAcceleratorState) { + var index int32 + if s.freeGlobalAcceleratorIndices.IsEmpty() { + index = int32(len(s.globalAccelerators)) + s.globalAccelerators = append(s.globalAccelerators, globalAcceleratorState{}) + } else { + index = s.freeGlobalAcceleratorIndices.Pop() + } + return index, &s.globalAccelerators[index] +} -// TODO: Move to BodyView -func (s *Scene) Each(cb func(b Body)) { - s.eachBody(func(_ int, b *bodyState) { - cb(Body{ - scene: s, - reference: b.reference, - }) - }) +func (s *Scene) releaseGlobalAccelerator(index int32) { + s.freeGlobalAcceleratorIndices.Push(index) +} + +func (s *Scene) eachGlobalAccelerator(cb func(index int, accelerator *globalAcceleratorState)) { + for i := range s.globalAccelerators { + accelerator := &s.globalAccelerators[i] + if accelerator.isValid() { + cb(i, accelerator) + } + } +} + +func (s *Scene) eachEnabledGlobalAccelerator(cb func(index int, accelerator *globalAcceleratorState)) { + for i := range s.globalAccelerators { + accelerator := &s.globalAccelerators[i] + if accelerator.isValid() && accelerator.isEnabled { + cb(i, accelerator) + } + } +} + +func (s *Scene) allocateBodyAccelerator() (int32, *bodyAcceleratorState) { + var index int32 + if s.freeBodyAcceleratorIndices.IsEmpty() { + index = int32(len(s.bodyAccelerators)) + s.bodyAccelerators = append(s.bodyAccelerators, bodyAcceleratorState{}) + } else { + index = s.freeBodyAcceleratorIndices.Pop() + } + return index, &s.bodyAccelerators[index] +} + +func (s *Scene) releaseBodyAccelerator(index int32) { + s.freeBodyAcceleratorIndices.Push(index) +} + +func (s *Scene) eachBodyAccelerator(cb func(index int, accelerator *bodyAcceleratorState)) { + for i := range s.bodyAccelerators { + accelerator := &s.bodyAccelerators[i] + if accelerator.isValid() { + cb(i, accelerator) + } + } +} + +func (s *Scene) eachEnabledBodyAccelerator(body *bodyState, cb func(index int, accelerator *bodyAcceleratorState)) { + index := body.firstBodyAcceleratorIndex + if index != nilIndex { + accelerator := &s.bodyAccelerators[index] + if accelerator.isValid() && accelerator.isEnabled { + cb(int(index), accelerator) + } + index = accelerator.nextIndex + } +} + +func (s *Scene) allocateSoloConstraint() (int32, *soloConstraintState) { + var index int32 + if s.freeSoloConstraintIndices.IsEmpty() { + index = int32(len(s.soloConstraints)) + s.soloConstraints = append(s.soloConstraints, soloConstraintState{}) + } else { + index = s.freeSoloConstraintIndices.Pop() + } + return index, &s.soloConstraints[index] +} + +func (s *Scene) releaseSoloConstraint(index int32) { + s.freeSoloConstraintIndices.Push(index) +} + +func (s *Scene) eachSoloConstraint(cb func(index int, constraint *soloConstraintState)) { + for i := range s.soloConstraints { + constraint := &s.soloConstraints[i] + if constraint.isValid() { + cb(i, constraint) + } + } +} + +func (s *Scene) eachEnabledSoloConstraint(cb func(index int, constraint *soloConstraintState)) { + for i := range s.soloConstraints { + constraint := &s.soloConstraints[i] + if constraint.isValid() && constraint.isEnabled { + cb(i, constraint) + } + } +} + +func (s *Scene) allocatePairConstraint() (int32, *pairConstraintState) { + var index int32 + if s.freePairConstraintIndices.IsEmpty() { + index = int32(len(s.pairConstraints)) + s.pairConstraints = append(s.pairConstraints, pairConstraintState{}) + } else { + index = s.freePairConstraintIndices.Pop() + } + return index, &s.pairConstraints[index] +} + +func (s *Scene) releasePairConstraint(index int32) { + s.freePairConstraintIndices.Push(index) +} + +func (s *Scene) eachPairConstraint(cb func(index int, constraint *pairConstraintState)) { + for i := range s.pairConstraints { + constraint := &s.pairConstraints[i] + if constraint.isValid() { + cb(i, constraint) + } + } +} + +func (s *Scene) eachEnabledPairConstraint(cb func(index int, constraint *pairConstraintState)) { + for i := range s.pairConstraints { + constraint := &s.pairConstraints[i] + if constraint.isValid() && constraint.isEnabled { + cb(i, constraint) + } + } +} + +func (s *Scene) allocateBody() (int32, *bodyState) { + var index int32 + if s.freeBodyIndices.IsEmpty() { + index = int32(len(s.bodies)) + s.bodies = append(s.bodies, bodyState{}) + } else { + index = s.freeBodyIndices.Pop() + } + return index, &s.bodies[index] +} + +func (s *Scene) releaseBody(index int32) { + s.freeBodyIndices.Push(index) +} + +func (s *Scene) eachBody(cb func(index int, b *bodyState)) { + for i := range s.bodies { + body := &s.bodies[i] + if body.isValid() { + cb(i, body) + } + } +} + +func (s *Scene) allocateTerrain() (int32, *terrainState) { + var index int32 + if s.freeTerrainIndices.IsEmpty() { + index = int32(len(s.terrains)) + s.terrains = append(s.terrains, terrainState{}) + } else { + index = s.freeTerrainIndices.Pop() + } + return index, &s.terrains[index] +} + +func (s *Scene) releaseTerrain(index int32) { + s.freeTerrainIndices.Push(index) +} + +func (s *Scene) eachTerrain(cb func(index int, t *terrainState)) { + for i := range s.terrains { + terrain := &s.terrains[i] + if terrain.isValid() { + cb(i, terrain) + } + } } +/////// OLD BELOW ------------ (TODO: DELETE COMMENT) + func (s *Scene) CheckSegmentIntersection(segment shape3d.Segment, mask uint32) (BodyID, bool) { intersection, ok := s.collisionScene.CheckSegmentIntersection(segment, placement3d.Filter{ Mask: opt.V(mask), @@ -881,151 +1053,6 @@ func (s *Scene) notifyDoubleBodyCollisions() { clear(s.newDBCollisions) } -func (s *Scene) allocateGlobalAccelerator() (int32, *globalAcceleratorState) { - var index int32 - if s.freeGlobalAcceleratorIndices.IsEmpty() { - index = int32(len(s.globalAccelerators)) - s.globalAccelerators = append(s.globalAccelerators, globalAcceleratorState{}) - } else { - index = s.freeGlobalAcceleratorIndices.Pop() - } - return index, &s.globalAccelerators[index] -} - -func (s *Scene) releaseGlobalAccelerator(index int32) { - s.freeGlobalAcceleratorIndices.Push(index) -} - -func (s *Scene) eachGlobalAccelerator(cb func(index int, accelerator *globalAcceleratorState)) { - for i := range s.globalAccelerators { - accelerator := &s.globalAccelerators[i] - if accelerator.isValid() { - cb(i, accelerator) - } - } -} - -func (s *Scene) eachEnabledGlobalAccelerator(cb func(index int, accelerator *globalAcceleratorState)) { - for i := range s.globalAccelerators { - accelerator := &s.globalAccelerators[i] - if accelerator.isValid() && accelerator.isEnabled { - cb(i, accelerator) - } - } -} - -func (s *Scene) allocateBodyAccelerator() (int32, *bodyAcceleratorState) { - var index int32 - if s.freeBodyAcceleratorIndices.IsEmpty() { - index = int32(len(s.bodyAccelerators)) - s.bodyAccelerators = append(s.bodyAccelerators, bodyAcceleratorState{}) - } else { - index = s.freeBodyAcceleratorIndices.Pop() - } - return index, &s.bodyAccelerators[index] -} - -func (s *Scene) releaseBodyAccelerator(index int32) { - s.freeBodyAcceleratorIndices.Push(index) -} - -func (s *Scene) eachEnabledBodyAccelerator(body *bodyState, cb func(index int, accelerator *bodyAcceleratorState)) { - index := body.firstBodyAcceleratorIndex - if index != nilIndex { - accelerator := &s.bodyAccelerators[index] - if accelerator.isValid() && accelerator.isEnabled { - cb(int(index), accelerator) - } - index = accelerator.nextIndex - } -} - -func (s *Scene) allocateSoloConstraint() (int32, *soloConstraintState) { - var index int32 - if s.freeSoloConstraintIndices.IsEmpty() { - index = int32(len(s.soloConstraints)) - s.soloConstraints = append(s.soloConstraints, soloConstraintState{}) - } else { - index = s.freeSoloConstraintIndices.Pop() - } - return index, &s.soloConstraints[index] -} - -func (s *Scene) releaseSoloConstraint(index int32) { - s.freeSoloConstraintIndices.Push(index) -} - -func (s *Scene) eachEnabledSoloConstraint(cb func(index int, constraint *soloConstraintState)) { - for i := range s.soloConstraints { - constraint := &s.soloConstraints[i] - if constraint.isValid() && constraint.isEnabled { - cb(i, constraint) - } - } -} - -func (s *Scene) allocatePairConstraint() (int32, *pairConstraintState) { - var index int32 - if s.freePairConstraintIndices.IsEmpty() { - index = int32(len(s.pairConstraints)) - s.pairConstraints = append(s.pairConstraints, pairConstraintState{}) - } else { - index = s.freePairConstraintIndices.Pop() - } - return index, &s.pairConstraints[index] -} - -func (s *Scene) releasePairConstraint(index int32) { - s.freePairConstraintIndices.Push(index) -} - -func (s *Scene) eachEnabledPairConstraint(cb func(index int, constraint *pairConstraintState)) { - for i := range s.pairConstraints { - constraint := &s.pairConstraints[i] - if constraint.isValid() && constraint.isEnabled { - cb(i, constraint) - } - } -} - -func (s *Scene) allocateBody() (int32, *bodyState) { - var index int32 - if s.freeBodyIndices.IsEmpty() { - index = int32(len(s.bodies)) - s.bodies = append(s.bodies, bodyState{}) - } else { - index = s.freeBodyIndices.Pop() - } - return index, &s.bodies[index] -} - -func (s *Scene) releaseBody(index int32) { - s.freeBodyIndices.Push(index) -} - -func (s *Scene) eachBody(cb func(index int, b *bodyState)) { - for i := range s.bodies { - if body := &s.bodies[i]; body.isValid() { - cb(i, body) - } - } -} - -func (s *Scene) allocateTerrain() (int32, *terrainState) { - var index int32 - if s.freeTerrainIndices.IsEmpty() { - index = int32(len(s.terrains)) - s.terrains = append(s.terrains, terrainState{}) - } else { - index = s.freeTerrainIndices.Pop() - } - return index, &s.terrains[index] -} - -func (s *Scene) releaseTerrain(index int32) { - s.freeTerrainIndices.Push(index) -} - type sbCollisionPair struct { BodyRef indexReference PropRef indexReference diff --git a/game/physics/terrain.go b/game/physics/terrain.go index 202ca61d..ffb6c91f 100644 --- a/game/physics/terrain.go +++ b/game/physics/terrain.go @@ -62,6 +62,15 @@ func (v TerrainView) Delete(id TerrainID) { v.scene.releaseTerrain(id.index) } +func (v TerrainView) Each(cb func(id TerrainID)) { + v.scene.eachTerrain(func(index int, terrain *terrainState) { + cb(TerrainID{ + index: int32(index), + revision: terrain.revision, + }) + }) +} + func (v TerrainView) Handle(id TerrainID) TerrainHandle { return TerrainHandle{ view: v, @@ -100,3 +109,7 @@ type terrainState struct { meshID placement3d.MeshID revision int32 } + +func (s *terrainState) isValid() bool { + return s.revision%2 == 1 // only odd revisions are valid +} From 9240a99dcd8162dae9b035a4b64ace8c3ba9c732 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Mon, 3 Aug 2026 00:41:49 +0300 Subject: [PATCH 32/85] Bug fix and godoc --- game/physics/accelerator_body.go | 2 ++ game/physics/accelerator_global.go | 2 ++ game/physics/body.go | 2 ++ game/physics/constraint_pair.go | 2 ++ game/physics/constraint_solo.go | 2 ++ game/physics/scene.go | 2 +- game/physics/terrain.go | 2 ++ 7 files changed, 13 insertions(+), 1 deletion(-) diff --git a/game/physics/accelerator_body.go b/game/physics/accelerator_body.go index a31ad39e..e9f130b1 100644 --- a/game/physics/accelerator_body.go +++ b/game/physics/accelerator_body.go @@ -91,6 +91,8 @@ func (v BodyAcceleratorView) Delete(id BodyAcceleratorID) { v.scene.releaseBodyAccelerator(id.index) } +// Each calls cb once for every body accelerator that is currently alive +// within this Scene, in unspecified order. func (v BodyAcceleratorView) Each(cb func(id BodyAcceleratorID)) { v.scene.eachBodyAccelerator(func(index int, accelerator *bodyAcceleratorState) { cb(BodyAcceleratorID{ diff --git a/game/physics/accelerator_global.go b/game/physics/accelerator_global.go index 7138a114..7a6b8199 100644 --- a/game/physics/accelerator_global.go +++ b/game/physics/accelerator_global.go @@ -62,6 +62,8 @@ func (v GlobalAcceleratorView) Delete(id GlobalAcceleratorID) { v.scene.releaseGlobalAccelerator(id.index) } +// Each calls cb once for every global accelerator that is currently alive +// within this Scene, in unspecified order. func (v GlobalAcceleratorView) Each(cb func(id GlobalAcceleratorID)) { v.scene.eachGlobalAccelerator(func(index int, accelerator *globalAcceleratorState) { cb(GlobalAcceleratorID{ diff --git a/game/physics/body.go b/game/physics/body.go index 1412a8b5..e29a36cc 100644 --- a/game/physics/body.go +++ b/game/physics/body.go @@ -84,6 +84,8 @@ func (v BodyView) Delete(id BodyID) { v.scene.releaseBody(id.index) } +// Each calls cb once for every body that is currently alive within this +// Scene, in unspecified order. func (v BodyView) Each(cb func(BodyID)) { v.scene.eachBody(func(index int, body *bodyState) { cb(BodyID{ diff --git a/game/physics/constraint_pair.go b/game/physics/constraint_pair.go index 55e02716..291d946c 100644 --- a/game/physics/constraint_pair.go +++ b/game/physics/constraint_pair.go @@ -213,6 +213,8 @@ func (v PairConstraintView) Delete(id PairConstraintID) { v.scene.releasePairConstraint(id.index) } +// Each calls cb once for every pair constraint that is currently alive +// within this Scene, in unspecified order. func (v PairConstraintView) Each(cb func(id PairConstraintID)) { v.scene.eachPairConstraint(func(index int, constraint *pairConstraintState) { cb(PairConstraintID{ diff --git a/game/physics/constraint_solo.go b/game/physics/constraint_solo.go index 057eb9dd..50127091 100644 --- a/game/physics/constraint_solo.go +++ b/game/physics/constraint_solo.go @@ -151,6 +151,8 @@ func (v SoloConstraintView) Delete(id SoloConstraintID) { v.scene.releaseSoloConstraint(id.index) } +// Each calls cb once for every solo constraint that is currently alive +// within this Scene, in unspecified order. func (v SoloConstraintView) Each(cb func(id SoloConstraintID)) { v.scene.eachSoloConstraint(func(index int, constraint *soloConstraintState) { cb(SoloConstraintID{ diff --git a/game/physics/scene.go b/game/physics/scene.go index 01add9b9..bc9ba7e2 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -447,7 +447,7 @@ func (s *Scene) eachBodyAccelerator(cb func(index int, accelerator *bodyAccelera func (s *Scene) eachEnabledBodyAccelerator(body *bodyState, cb func(index int, accelerator *bodyAcceleratorState)) { index := body.firstBodyAcceleratorIndex - if index != nilIndex { + for index != nilIndex { accelerator := &s.bodyAccelerators[index] if accelerator.isValid() && accelerator.isEnabled { cb(int(index), accelerator) diff --git a/game/physics/terrain.go b/game/physics/terrain.go index ffb6c91f..e4fb564e 100644 --- a/game/physics/terrain.go +++ b/game/physics/terrain.go @@ -62,6 +62,8 @@ func (v TerrainView) Delete(id TerrainID) { v.scene.releaseTerrain(id.index) } +// Each calls cb once for every terrain that is currently alive within +// this Scene, in unspecified order. func (v TerrainView) Each(cb func(id TerrainID)) { v.scene.eachTerrain(func(index int, terrain *terrainState) { cb(TerrainID{ From 4a87b75683b471e84b7d62b30617d5abc1318d35 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Mon, 3 Aug 2026 00:49:15 +0300 Subject: [PATCH 33/85] Changes to util --- game/physics/solver/precision.go | 25 -------------------- game/physics/util.go | 40 ++++++++++++++++++++++++++++---- 2 files changed, 36 insertions(+), 29 deletions(-) delete mode 100644 game/physics/solver/precision.go diff --git a/game/physics/solver/precision.go b/game/physics/solver/precision.go deleted file mode 100644 index 31c93ade..00000000 --- a/game/physics/solver/precision.go +++ /dev/null @@ -1,25 +0,0 @@ -package solver - -import "github.com/mokiat/gomath/dprec" - -// Epsilon indicates a small enough amount that something could be ignored. -const Epsilon = float64(0.00001) - -// RestitutionClamp specifies a ratio that describes how much the restitution -// coefficient should be allowed to apply. -// -// The goal of this clamp is to reduce bounciness of objects when they are -// barely moving. -func RestitutionClamp(effectiveVelocity float64) float64 { - absEffectiveVelocity := dprec.Abs(effectiveVelocity) - switch { - case absEffectiveVelocity < 0.5: - return 0.0 - case absEffectiveVelocity < 1.0: - return 0.05 - case absEffectiveVelocity < 2.0: - return 0.1 - default: - return 1.0 - } -} diff --git a/game/physics/util.go b/game/physics/util.go index 1f9a0676..7b4896f8 100644 --- a/game/physics/util.go +++ b/game/physics/util.go @@ -2,13 +2,45 @@ package physics import "github.com/mokiat/gomath/dprec" +// Epsilon is a threshold below which a quantity is small enough to be +// treated as zero, in order to avoid degenerate behavior such as +// normalizing a near-zero-length vector. +const Epsilon = float64(0.00001) + +// QuatFromVector returns the quaternion that represents a rotation of +// vector's length, in radians, around vector's direction as the rotation +// axis. +// +// This is the standard way to turn a rotation vector (e.g. an angular +// velocity scaled by elapsed time, or an angular nudge) into a quaternion +// that can be composed with an existing orientation. +// +// If vector's length is smaller than [Epsilon], the identity quaternion +// is returned instead, since the direction of a near-zero vector is not +// meaningful as a rotation axis. func QuatFromVector(vector dprec.Vec3) dprec.Quat { radians := vector.Length() - - const angularEpsilon = float64(0.00001) - if dprec.Abs(radians) < angularEpsilon { + if dprec.Abs(radians) < Epsilon { return dprec.IdentityQuat() } - return dprec.RotationQuat(dprec.Radians(radians), vector) } + +// RestitutionClamp specifies a ratio that describes how much the restitution +// coefficient should be allowed to apply. +// +// The goal of this clamp is to reduce bounciness of objects when they are +// barely moving. +func RestitutionClamp(effectiveVelocity float64) float64 { + absEffectiveVelocity := dprec.Abs(effectiveVelocity) + switch { + case absEffectiveVelocity < 0.5: + return 0.0 + case absEffectiveVelocity < 1.0: + return 0.05 + case absEffectiveVelocity < 2.0: + return 0.1 + default: + return 1.0 + } +} From 66a96602c336ab2cb9e4217c02edbefc36b89a87 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Mon, 3 Aug 2026 22:44:02 +0300 Subject: [PATCH 34/85] More code changes --- game/asset_model.go | 81 ++- game/asset_physics.go | 544 +++++++++--------- game/binding.go | 18 +- game/physics/body.go | 12 + game/physics/constraint/chandelier.go | 166 +++--- .../constraint/clamp_direction_offset.go | 284 ++++----- game/physics/constraint/coilover.go | 274 ++++----- game/physics/constraint/collision.go | 450 +++++++-------- game/physics/constraint/combined.go | 114 ++-- game/physics/constraint/copy_direction.go | 126 ++-- game/physics/constraint/copy_position.go | 36 +- game/physics/constraint/copy_rotation.go | 36 +- game/physics/constraint/differential.go | 110 ++-- game/physics/constraint/hinged_rod.go | 196 +++---- .../constraint/limit_relative_angle.go | 234 ++++---- game/physics/constraint/match_direction.go | 184 +++--- .../constraint/match_direction_offset.go | 236 ++++---- game/physics/constraint/match_rotation.go | 34 +- game/physics/constraint/pair_attachment.go | 120 ++-- game/physics/constraint/static_position.go | 84 +-- game/physics/constraint/static_rotation.go | 84 +-- game/physics/scene.go | 438 ++++++-------- game/physics/solver/change.go | 16 +- game/physics/solver/context.go | 166 +++--- game/physics/solver/jacobian.go | 90 ++- game/physics/solver_collision_pair.go | 15 + game/physics/solver_collision_solo.go | 34 ++ game/physics/terrain.go | 8 + game/scene.go | 10 +- 29 files changed, 2107 insertions(+), 2093 deletions(-) create mode 100644 game/physics/solver_collision_pair.go create mode 100644 game/physics/solver_collision_solo.go diff --git a/game/asset_model.go b/game/asset_model.go index 4613ce15..1a9f7f87 100644 --- a/game/asset_model.go +++ b/game/asset_model.go @@ -11,24 +11,23 @@ import ( "github.com/mokiat/lacking/game/asset/dto" "github.com/mokiat/lacking/game/graphics" "github.com/mokiat/lacking/game/hierarchy" - "github.com/mokiat/lacking/game/physics" "github.com/mokiat/lacking/render" ) // ModelTemplate represents a template for a model that can be instantiated // in a Scene. type ModelTemplate struct { - Recordings IdentifiableList[*animation.Recording] - Shaders IdentifiableList[*graphics.Shader] - Textures IdentifiableList[render.Texture] - Materials IdentifiableList[*graphics.Material] - BodyMaterials IdentifiableList[*physics.Material] - BodyDefinitions IdentifiableList[*physics.BodyDefinition] + Recordings IdentifiableList[*animation.Recording] + Shaders IdentifiableList[*graphics.Shader] + Textures IdentifiableList[render.Texture] + Materials IdentifiableList[*graphics.Material] + // BodyMaterials IdentifiableList[*physics.Material] + // BodyDefinitions IdentifiableList[*physics.BodyDefinition] MeshGeometries IdentifiableList[*graphics.MeshGeometry] MeshDefinitions IdentifiableList[*graphics.MeshDefinition] - Nodes IdentifiableList[NodeTemplate] - Bodies IdentifiableList[BodyTemplate] + Nodes IdentifiableList[NodeTemplate] + // Bodies IdentifiableList[BodyTemplate] Armatures IdentifiableList[ArmatureTemplate] Meshes IdentifiableList[MeshTemplate] AmbientLights IdentifiableList[AmbientLightTemplate] @@ -71,15 +70,15 @@ func LoadModelTemplate(loader *AssetLoader, assetModel dto.Model) (*ModelTemplat return nil, fmt.Errorf("failed to resolve materials: %w", err) } - bodyMaterials, err := LoadPhysicsMaterials(loader, assetModel.PhysicsChunk.BodyMaterials) - if err != nil { - return nil, fmt.Errorf("failed to resolve body materials: %w", err) - } + // bodyMaterials, err := LoadPhysicsMaterials(loader, assetModel.PhysicsChunk.BodyMaterials) + // if err != nil { + // return nil, fmt.Errorf("failed to resolve body materials: %w", err) + // } - bodyDefinitions, err := LoadPhysicsBodyDefinitions(loader, assetModel.PhysicsChunk.BodyDefinitions, bodyMaterials) - if err != nil { - return nil, fmt.Errorf("failed to resolve body definitions: %w", err) - } + // bodyDefinitions, err := LoadPhysicsBodyDefinitions(loader, assetModel.PhysicsChunk.BodyDefinitions, bodyMaterials) + // if err != nil { + // return nil, fmt.Errorf("failed to resolve body definitions: %w", err) + // } meshGeometries, err := LoadMeshGeometries(loader, assetModel.MeshChunk.Geometries) if err != nil { @@ -96,10 +95,10 @@ func LoadModelTemplate(loader *AssetLoader, assetModel dto.Model) (*ModelTemplat return nil, fmt.Errorf("failed to resolve node templates: %w", err) } - bodies, err := LoadPhysicsBodyTemplates(loader, assetModel.PhysicsChunk.Bodies, bodyDefinitions) - if err != nil { - return nil, fmt.Errorf("failed to resolve physics body templates: %w", err) - } + // bodies, err := LoadPhysicsBodyTemplates(loader, assetModel.PhysicsChunk.Bodies, bodyDefinitions) + // if err != nil { + // return nil, fmt.Errorf("failed to resolve physics body templates: %w", err) + // } armatures, err := LoadArmatureTemplates(loader, assetModel.MeshChunk.Armatures) if err != nil { @@ -139,17 +138,17 @@ func LoadModelTemplate(loader *AssetLoader, assetModel dto.Model) (*ModelTemplat } return &ModelTemplate{ - Recordings: recordings, - Shaders: shaders, - Textures: textures, - Materials: materials, - BodyMaterials: bodyMaterials, - BodyDefinitions: bodyDefinitions, + Recordings: recordings, + Shaders: shaders, + Textures: textures, + Materials: materials, + // BodyMaterials: bodyMaterials, + // BodyDefinitions: bodyDefinitions, MeshGeometries: meshGeometries, MeshDefinitions: meshDefinitions, - Nodes: nodes, - Bodies: bodies, + Nodes: nodes, + // Bodies: bodies, Armatures: armatures, Meshes: meshes, AmbientLights: ambientLights, @@ -169,13 +168,13 @@ func UnloadModelTemplate(loader *AssetLoader, template *ModelTemplate) error { UnloadShaders(loader, template.Shaders), UnloadTextures(loader, template.Textures), UnloadMaterials(loader, template.Materials), - UnloadPhysicsMaterials(loader, template.BodyMaterials), - UnloadPhysicsBodyDefinitions(loader, template.BodyDefinitions), + // UnloadPhysicsMaterials(loader, template.BodyMaterials), + // UnloadPhysicsBodyDefinitions(loader, template.BodyDefinitions), UnloadMeshGeometries(loader, template.MeshGeometries), UnloadMeshDefinitions(loader, template.MeshDefinitions), UnloadNodeTemplates(loader, template.Nodes), - UnloadPhysicsBodyTemplates(loader, template.Bodies), + // UnloadPhysicsBodyTemplates(loader, template.Bodies), UnloadArmatureTemplates(loader, template.Armatures), UnloadMeshTemplates(loader, template.Meshes), UnloadAmbientLightTemplates(loader, template.AmbientLights), @@ -316,15 +315,15 @@ func InstantiateModel(scene *Scene, info ModelInfo) *Model { recordings := definition.Recordings meshDefinitions := definition.MeshDefinitions - for template := range definition.Bodies.Values() { - if nodes.HasID(template.NodeID) { - if info.IsDynamic { - InstantiatePhysicsBodyTemplateDynamic(scene, template, nodes) - } else { - InstantiatePhysicsBodyTemplateStatic(scene, template, nodes) - } - } - } + // for template := range definition.Bodies.Values() { + // if nodes.HasID(template.NodeID) { + // if info.IsDynamic { + // InstantiatePhysicsBodyTemplateDynamic(scene, template, nodes) + // } else { + // InstantiatePhysicsBodyTemplateStatic(scene, template, nodes) + // } + // } + // } armatures := make(IdentifiableList[*graphics.Armature], 0, len(definition.Armatures)) for id, template := range definition.Armatures.Iter() { diff --git a/game/asset_physics.go b/game/asset_physics.go index d2aaa503..c92e18c3 100644 --- a/game/asset_physics.go +++ b/game/asset_physics.go @@ -1,298 +1,298 @@ package game -import ( - "fmt" +// import ( +// "fmt" - "github.com/mokiat/gog/opt" - "github.com/mokiat/lacking/core/spatial/shape3d" - "github.com/mokiat/lacking/game/asset/dto" - "github.com/mokiat/lacking/game/hierarchy" - "github.com/mokiat/lacking/game/physics" - "golang.org/x/sync/errgroup" -) +// "github.com/mokiat/gog/opt" +// "github.com/mokiat/lacking/core/spatial/shape3d" +// "github.com/mokiat/lacking/game/asset/dto" +// "github.com/mokiat/lacking/game/hierarchy" +// "github.com/mokiat/lacking/game/physics" +// "golang.org/x/sync/errgroup" +// ) -// LoadPhysicsMaterial loads a physics material from the given asset data. -// -// This is a blocking operation and should be called from a worker thread. -func LoadPhysicsMaterial(loader *AssetLoader, assetMaterial dto.BodyMaterial) (Identifiable[*physics.Material], error) { - materialInfo := physics.MaterialInfo{ - FrictionCoefficient: assetMaterial.FrictionCoefficient, - RestitutionCoefficient: assetMaterial.RestitutionCoefficient, - } +// // LoadPhysicsMaterial loads a physics material from the given asset data. +// // +// // This is a blocking operation and should be called from a worker thread. +// func LoadPhysicsMaterial(loader *AssetLoader, assetMaterial dto.BodyMaterial) (Identifiable[*physics.Material], error) { +// materialInfo := physics.MaterialInfo{ +// FrictionCoefficient: assetMaterial.FrictionCoefficient, +// RestitutionCoefficient: assetMaterial.RestitutionCoefficient, +// } - var material *physics.Material - allocateMaterial := func() error { - material = physics.NewMaterial(materialInfo) - return nil - } - if err := loader.ScheduleMain(allocateMaterial).Wait(); err != nil { - return Identifiable[*physics.Material]{}, err - } +// var material *physics.Material +// allocateMaterial := func() error { +// material = physics.NewMaterial(materialInfo) +// return nil +// } +// if err := loader.ScheduleMain(allocateMaterial).Wait(); err != nil { +// return Identifiable[*physics.Material]{}, err +// } - return Identifiable[*physics.Material]{ - ID: assetMaterial.ID, - Value: material, - }, nil -} +// return Identifiable[*physics.Material]{ +// ID: assetMaterial.ID, +// Value: material, +// }, nil +// } -// LoadPhysicsMaterials loads a list of physics materials from the given asset -// materials. -// -// This is a blocking operation and should be called from a worker thread. -func LoadPhysicsMaterials(loader *AssetLoader, assetMaterials []dto.BodyMaterial) (IdentifiableList[*physics.Material], error) { - materials := make(IdentifiableList[*physics.Material], len(assetMaterials)) - var group errgroup.Group - for i, assetMaterial := range assetMaterials { - group.Go(func() error { - material, err := LoadPhysicsMaterial(loader, assetMaterial) - materials[i] = material - return err - }) - } - return materials, group.Wait() -} +// // LoadPhysicsMaterials loads a list of physics materials from the given asset +// // materials. +// // +// // This is a blocking operation and should be called from a worker thread. +// func LoadPhysicsMaterials(loader *AssetLoader, assetMaterials []dto.BodyMaterial) (IdentifiableList[*physics.Material], error) { +// materials := make(IdentifiableList[*physics.Material], len(assetMaterials)) +// var group errgroup.Group +// for i, assetMaterial := range assetMaterials { +// group.Go(func() error { +// material, err := LoadPhysicsMaterial(loader, assetMaterial) +// materials[i] = material +// return err +// }) +// } +// return materials, group.Wait() +// } -// UnloadPhysicsMaterial unloads a physics material from the asset loader. -// -// This is a blocking operation and should be called from a worker thread. -func UnloadPhysicsMaterial(loader *AssetLoader, idMaterial Identifiable[*physics.Material]) error { - // At the time being this is a no-op. - return nil -} +// // UnloadPhysicsMaterial unloads a physics material from the asset loader. +// // +// // This is a blocking operation and should be called from a worker thread. +// func UnloadPhysicsMaterial(loader *AssetLoader, idMaterial Identifiable[*physics.Material]) error { +// // At the time being this is a no-op. +// return nil +// } -// UnloadPhysicsMaterials unloads a list of physics materials from the asset -// loader. -// -// This is a blocking operation and should be called from a worker thread. -func UnloadPhysicsMaterials(loader *AssetLoader, idMaterials IdentifiableList[*physics.Material]) error { - for _, idMaterial := range idMaterials { - if err := UnloadPhysicsMaterial(loader, idMaterial); err != nil { - return err - } - } - return nil -} +// // UnloadPhysicsMaterials unloads a list of physics materials from the asset +// // loader. +// // +// // This is a blocking operation and should be called from a worker thread. +// func UnloadPhysicsMaterials(loader *AssetLoader, idMaterials IdentifiableList[*physics.Material]) error { +// for _, idMaterial := range idMaterials { +// if err := UnloadPhysicsMaterial(loader, idMaterial); err != nil { +// return err +// } +// } +// return nil +// } -// LoadPhysicsBodyDefinition loads a physics body definition from the given -// asset data. -// -// This is a blocking operation and should be called from a worker thread. -func LoadPhysicsBodyDefinition(loader *AssetLoader, assetBodyDefinition dto.BodyDefinition, materials IdentifiableList[*physics.Material]) (Identifiable[*physics.BodyDefinition], error) { - material, ok := materials.FindByID(assetBodyDefinition.MaterialID) - if !ok { - return Identifiable[*physics.BodyDefinition]{}, fmt.Errorf("physics material with ID %d not found", assetBodyDefinition.MaterialID) - } +// // LoadPhysicsBodyDefinition loads a physics body definition from the given +// // asset data. +// // +// // This is a blocking operation and should be called from a worker thread. +// func LoadPhysicsBodyDefinition(loader *AssetLoader, assetBodyDefinition dto.BodyDefinition, materials IdentifiableList[*physics.Material]) (Identifiable[*physics.BodyDefinition], error) { +// material, ok := materials.FindByID(assetBodyDefinition.MaterialID) +// if !ok { +// return Identifiable[*physics.BodyDefinition]{}, fmt.Errorf("physics material with ID %d not found", assetBodyDefinition.MaterialID) +// } - bodyDefinitionInfo := physics.BodyDefinitionInfo{ - Mass: assetBodyDefinition.Mass, - MomentOfInertia: assetBodyDefinition.MomentOfInertia, - FrictionCoefficient: material.FrictionCoefficient(), - RestitutionCoefficient: material.RestitutionCoefficient(), - DragFactor: assetBodyDefinition.DragFactor, - AngularDragFactor: assetBodyDefinition.AngularDragFactor, - AerodynamicShapes: nil, // TODO - CollisionSpheres: resolveCollisionSpheres(assetBodyDefinition), - CollisionBoxes: resolveCollisionBoxes(assetBodyDefinition), - CollisionMeshes: resolveCollisionMeshes(assetBodyDefinition), - } +// bodyDefinitionInfo := physics.BodyDefinitionInfo{ +// Mass: assetBodyDefinition.Mass, +// MomentOfInertia: assetBodyDefinition.MomentOfInertia, +// FrictionCoefficient: material.FrictionCoefficient(), +// RestitutionCoefficient: material.RestitutionCoefficient(), +// DragFactor: assetBodyDefinition.DragFactor, +// AngularDragFactor: assetBodyDefinition.AngularDragFactor, +// AerodynamicShapes: nil, // TODO +// CollisionSpheres: resolveCollisionSpheres(assetBodyDefinition), +// CollisionBoxes: resolveCollisionBoxes(assetBodyDefinition), +// CollisionMeshes: resolveCollisionMeshes(assetBodyDefinition), +// } - var bodyDefinition *physics.BodyDefinition - allocateDefinition := func() error { - bodyDefinition = physics.NewBodyDefinition(bodyDefinitionInfo) - return nil - } - if err := loader.ScheduleMain(allocateDefinition).Wait(); err != nil { - return Identifiable[*physics.BodyDefinition]{}, err - } +// var bodyDefinition *physics.BodyDefinition +// allocateDefinition := func() error { +// bodyDefinition = physics.NewBodyDefinition(bodyDefinitionInfo) +// return nil +// } +// if err := loader.ScheduleMain(allocateDefinition).Wait(); err != nil { +// return Identifiable[*physics.BodyDefinition]{}, err +// } - return Identifiable[*physics.BodyDefinition]{ - ID: assetBodyDefinition.ID, - Value: bodyDefinition, - }, nil -} +// return Identifiable[*physics.BodyDefinition]{ +// ID: assetBodyDefinition.ID, +// Value: bodyDefinition, +// }, nil +// } -// LoadPhysicsBodyDefinitions loads a list of physics body definitions from the -// given asset body definitions. -// -// This is a blocking operation and should be called from a worker thread. -func LoadPhysicsBodyDefinitions(loader *AssetLoader, assetBodyDefinitions []dto.BodyDefinition, materials IdentifiableList[*physics.Material]) (IdentifiableList[*physics.BodyDefinition], error) { - bodyDefinitions := make(IdentifiableList[*physics.BodyDefinition], len(assetBodyDefinitions)) - var group errgroup.Group - for i, assetBodyDefinition := range assetBodyDefinitions { - group.Go(func() error { - bodyDefinition, err := LoadPhysicsBodyDefinition(loader, assetBodyDefinition, materials) - bodyDefinitions[i] = bodyDefinition - return err - }) - } - return bodyDefinitions, group.Wait() -} +// // LoadPhysicsBodyDefinitions loads a list of physics body definitions from the +// // given asset body definitions. +// // +// // This is a blocking operation and should be called from a worker thread. +// func LoadPhysicsBodyDefinitions(loader *AssetLoader, assetBodyDefinitions []dto.BodyDefinition, materials IdentifiableList[*physics.Material]) (IdentifiableList[*physics.BodyDefinition], error) { +// bodyDefinitions := make(IdentifiableList[*physics.BodyDefinition], len(assetBodyDefinitions)) +// var group errgroup.Group +// for i, assetBodyDefinition := range assetBodyDefinitions { +// group.Go(func() error { +// bodyDefinition, err := LoadPhysicsBodyDefinition(loader, assetBodyDefinition, materials) +// bodyDefinitions[i] = bodyDefinition +// return err +// }) +// } +// return bodyDefinitions, group.Wait() +// } -// UnloadPhysicsBodyDefinition unloads a physics body definition from the asset -// loader. -// -// This is a blocking operation and should be called from a worker thread. -func UnloadPhysicsBodyDefinition(loader *AssetLoader, idBodyDefinition Identifiable[*physics.BodyDefinition]) error { - // At the time being this is a no-op. - return nil -} +// // UnloadPhysicsBodyDefinition unloads a physics body definition from the asset +// // loader. +// // +// // This is a blocking operation and should be called from a worker thread. +// func UnloadPhysicsBodyDefinition(loader *AssetLoader, idBodyDefinition Identifiable[*physics.BodyDefinition]) error { +// // At the time being this is a no-op. +// return nil +// } -// UnloadPhysicsBodyDefinitions unloads a list of physics body definitions from -// the asset loader. -// -// This is a blocking operation and should be called from a worker thread. -func UnloadPhysicsBodyDefinitions(loader *AssetLoader, idBodyDefinitions IdentifiableList[*physics.BodyDefinition]) error { - for _, idBodyDefinition := range idBodyDefinitions { - if err := UnloadPhysicsBodyDefinition(loader, idBodyDefinition); err != nil { - return err - } - } - return nil -} +// // UnloadPhysicsBodyDefinitions unloads a list of physics body definitions from +// // the asset loader. +// // +// // This is a blocking operation and should be called from a worker thread. +// func UnloadPhysicsBodyDefinitions(loader *AssetLoader, idBodyDefinitions IdentifiableList[*physics.BodyDefinition]) error { +// for _, idBodyDefinition := range idBodyDefinitions { +// if err := UnloadPhysicsBodyDefinition(loader, idBodyDefinition); err != nil { +// return err +// } +// } +// return nil +// } -func resolveCollisionSpheres(bodyDef dto.BodyDefinition) []shape3d.Sphere { - result := make([]shape3d.Sphere, len(bodyDef.CollisionSpheres)) - for i, collisionSphereAsset := range bodyDef.CollisionSpheres { - result[i] = shape3d.Sphere{ - Center: collisionSphereAsset.Translation, - Radius: collisionSphereAsset.Radius, - } - } - return result -} +// func resolveCollisionSpheres(bodyDef dto.BodyDefinition) []shape3d.Sphere { +// result := make([]shape3d.Sphere, len(bodyDef.CollisionSpheres)) +// for i, collisionSphereAsset := range bodyDef.CollisionSpheres { +// result[i] = shape3d.Sphere{ +// Center: collisionSphereAsset.Translation, +// Radius: collisionSphereAsset.Radius, +// } +// } +// return result +// } -func resolveCollisionBoxes(bodyDef dto.BodyDefinition) []shape3d.Box { - result := make([]shape3d.Box, len(bodyDef.CollisionBoxes)) - for i, collisionBoxAsset := range bodyDef.CollisionBoxes { - result[i] = shape3d.Box{ - Center: collisionBoxAsset.Translation, - Rotation: shape3d.RotationFromQuat(collisionBoxAsset.Rotation), - HalfWidth: collisionBoxAsset.Width / 2.0, - HalfHeight: collisionBoxAsset.Height / 2.0, - HalfLength: collisionBoxAsset.Length / 2.0, - } - } - return result -} +// func resolveCollisionBoxes(bodyDef dto.BodyDefinition) []shape3d.Box { +// result := make([]shape3d.Box, len(bodyDef.CollisionBoxes)) +// for i, collisionBoxAsset := range bodyDef.CollisionBoxes { +// result[i] = shape3d.Box{ +// Center: collisionBoxAsset.Translation, +// Rotation: shape3d.RotationFromQuat(collisionBoxAsset.Rotation), +// HalfWidth: collisionBoxAsset.Width / 2.0, +// HalfHeight: collisionBoxAsset.Height / 2.0, +// HalfLength: collisionBoxAsset.Length / 2.0, +// } +// } +// return result +// } -func resolveCollisionMeshes(bodyDef dto.BodyDefinition) []shape3d.Mesh { - result := make([]shape3d.Mesh, len(bodyDef.CollisionMeshes)) - for i, collisionMeshAsset := range bodyDef.CollisionMeshes { - transform := shape3d.TRTransform( - collisionMeshAsset.Translation, - shape3d.RotationFromQuat(collisionMeshAsset.Rotation), - ) - triangles := make([]shape3d.Triangle, len(collisionMeshAsset.Triangles)) - for j, triangleAsset := range collisionMeshAsset.Triangles { - triangles[j] = shape3d.Triangle{ - A: transform.Apply(triangleAsset.A), - B: transform.Apply(triangleAsset.B), - C: transform.Apply(triangleAsset.C), - } - } - result[i] = shape3d.Mesh{ - Triangles: triangles, - } - } - return result -} +// func resolveCollisionMeshes(bodyDef dto.BodyDefinition) []shape3d.Mesh { +// result := make([]shape3d.Mesh, len(bodyDef.CollisionMeshes)) +// for i, collisionMeshAsset := range bodyDef.CollisionMeshes { +// transform := shape3d.TRTransform( +// collisionMeshAsset.Translation, +// shape3d.RotationFromQuat(collisionMeshAsset.Rotation), +// ) +// triangles := make([]shape3d.Triangle, len(collisionMeshAsset.Triangles)) +// for j, triangleAsset := range collisionMeshAsset.Triangles { +// triangles[j] = shape3d.Triangle{ +// A: transform.Apply(triangleAsset.A), +// B: transform.Apply(triangleAsset.B), +// C: transform.Apply(triangleAsset.C), +// } +// } +// result[i] = shape3d.Mesh{ +// Triangles: triangles, +// } +// } +// return result +// } -// BodyTemplate represents a template for physics body that can be -// instantiated in a scene. -type BodyTemplate struct { - NodeID uint32 - Definition *physics.BodyDefinition -} +// // BodyTemplate represents a template for physics body that can be +// // instantiated in a scene. +// type BodyTemplate struct { +// NodeID uint32 +// Definition *physics.BodyDefinition +// } -// LoadPhysicsBodyTemplate resolves a physics body template from the given asset -// data. -// -// This is a blocking operation and should be called from a worker thread. -func LoadPhysicsBodyTemplate(loader *AssetLoader, assetBody dto.Body, bodyDefinitions IdentifiableList[*physics.BodyDefinition]) (Identifiable[BodyTemplate], error) { - bodyDefinition, ok := bodyDefinitions.FindByID(assetBody.BodyDefinitionID) - if !ok { - return Identifiable[BodyTemplate]{}, fmt.Errorf("body definition with ID %d not found", assetBody.BodyDefinitionID) - } - return Identifiable[BodyTemplate]{ - ID: assetBody.ID, - Value: BodyTemplate{ - NodeID: assetBody.NodeID, - Definition: bodyDefinition, - }, - }, nil -} +// // LoadPhysicsBodyTemplate resolves a physics body template from the given asset +// // data. +// // +// // This is a blocking operation and should be called from a worker thread. +// func LoadPhysicsBodyTemplate(loader *AssetLoader, assetBody dto.Body, bodyDefinitions IdentifiableList[*physics.BodyDefinition]) (Identifiable[BodyTemplate], error) { +// bodyDefinition, ok := bodyDefinitions.FindByID(assetBody.BodyDefinitionID) +// if !ok { +// return Identifiable[BodyTemplate]{}, fmt.Errorf("body definition with ID %d not found", assetBody.BodyDefinitionID) +// } +// return Identifiable[BodyTemplate]{ +// ID: assetBody.ID, +// Value: BodyTemplate{ +// NodeID: assetBody.NodeID, +// Definition: bodyDefinition, +// }, +// }, nil +// } -// LoadPhysicsBodyTemplates resolves a list of physics body templates from the -// given asset bodies. -// -// This is a blocking operation and should be called from a worker thread. -func LoadPhysicsBodyTemplates(loader *AssetLoader, assetBodies []dto.Body, bodyDefinitions IdentifiableList[*physics.BodyDefinition]) (IdentifiableList[BodyTemplate], error) { - bodyTemplates := make(IdentifiableList[BodyTemplate], len(assetBodies)) - for i, assetBody := range assetBodies { - template, err := LoadPhysicsBodyTemplate(loader, assetBody, bodyDefinitions) - if err != nil { - return IdentifiableList[BodyTemplate]{}, err - } - bodyTemplates[i] = template - } - return bodyTemplates, nil -} +// // LoadPhysicsBodyTemplates resolves a list of physics body templates from the +// // given asset bodies. +// // +// // This is a blocking operation and should be called from a worker thread. +// func LoadPhysicsBodyTemplates(loader *AssetLoader, assetBodies []dto.Body, bodyDefinitions IdentifiableList[*physics.BodyDefinition]) (IdentifiableList[BodyTemplate], error) { +// bodyTemplates := make(IdentifiableList[BodyTemplate], len(assetBodies)) +// for i, assetBody := range assetBodies { +// template, err := LoadPhysicsBodyTemplate(loader, assetBody, bodyDefinitions) +// if err != nil { +// return IdentifiableList[BodyTemplate]{}, err +// } +// bodyTemplates[i] = template +// } +// return bodyTemplates, nil +// } -// UnloadPhysicsBodyTemplate unloads a physics body template from the asset -// loader. -// -// This is a blocking operation and should be called from a worker thread. -func UnloadPhysicsBodyTemplate(loader *AssetLoader, idBody Identifiable[BodyTemplate]) error { - // At the time being this is a no-op. - return nil -} +// // UnloadPhysicsBodyTemplate unloads a physics body template from the asset +// // loader. +// // +// // This is a blocking operation and should be called from a worker thread. +// func UnloadPhysicsBodyTemplate(loader *AssetLoader, idBody Identifiable[BodyTemplate]) error { +// // At the time being this is a no-op. +// return nil +// } -// UnloadPhysicsBodyTemplates unloads a list of physics body templates from the -// asset loader. -// -// This is a blocking operation and should be called from a worker thread. -func UnloadPhysicsBodyTemplates(loader *AssetLoader, idBodies IdentifiableList[BodyTemplate]) error { - for _, idBody := range idBodies { - if err := UnloadPhysicsBodyTemplate(loader, idBody); err != nil { - return err - } - } - return nil -} +// // UnloadPhysicsBodyTemplates unloads a list of physics body templates from the +// // asset loader. +// // +// // This is a blocking operation and should be called from a worker thread. +// func UnloadPhysicsBodyTemplates(loader *AssetLoader, idBodies IdentifiableList[BodyTemplate]) error { +// for _, idBody := range idBodies { +// if err := UnloadPhysicsBodyTemplate(loader, idBody); err != nil { +// return err +// } +// } +// return nil +// } -// InstantiatePhysicsBodyTemplateStatic creates a static physics body in the -// given scene from the provided body template. -// -// This operation needs to be called from the main thread. -func InstantiatePhysicsBodyTemplateStatic(scene *Scene, template BodyTemplate, nodes IdentifiableList[hierarchy.NodeID]) { - node := nodes.GetByID(template.NodeID) - absMatrix := scene.Hierarchy().NodeAbsoluteMatrix(node) - nodeName := scene.Hierarchy().NodeName(node) - scene.physicsScene.CreateProp(physics.PropInfo{ - Name: nodeName, - Position: opt.V(absMatrix.Translation()), - Rotation: opt.V(absMatrix.Rotation()), - CollisionSpheres: template.Definition.CollisionSpheres(), - CollisionBoxes: template.Definition.CollisionBoxes(), - CollisionMeshes: template.Definition.CollisionMeshes(), - }) -} +// // InstantiatePhysicsBodyTemplateStatic creates a static physics body in the +// // given scene from the provided body template. +// // +// // This operation needs to be called from the main thread. +// func InstantiatePhysicsBodyTemplateStatic(scene *Scene, template BodyTemplate, nodes IdentifiableList[hierarchy.NodeID]) { +// node := nodes.GetByID(template.NodeID) +// absMatrix := scene.Hierarchy().NodeAbsoluteMatrix(node) +// nodeName := scene.Hierarchy().NodeName(node) +// scene.physicsScene.CreateProp(physics.PropInfo{ +// Name: nodeName, +// Position: opt.V(absMatrix.Translation()), +// Rotation: opt.V(absMatrix.Rotation()), +// CollisionSpheres: template.Definition.CollisionSpheres(), +// CollisionBoxes: template.Definition.CollisionBoxes(), +// CollisionMeshes: template.Definition.CollisionMeshes(), +// }) +// } -// InstantiatePhysicsBodyTemplateDynamic creates a dynamic physics body in the -// given scene from the provided body template and returns it. -// -// This operation needs to be called from the main thread. -func InstantiatePhysicsBodyTemplateDynamic(scene *Scene, template BodyTemplate, nodes IdentifiableList[hierarchy.NodeID]) physics.Body { - node := nodes.GetByID(template.NodeID) - absMatrix := scene.Hierarchy().NodeAbsoluteMatrix(node) - nodeName := scene.Hierarchy().NodeName(node) - translation, rotation, _ := absMatrix.TRS() - body := scene.physicsScene.CreateBody(physics.BodyInfo{ - Name: nodeName, - Definition: template.Definition, - Position: translation, - Rotation: rotation, - }) - scene.bodyBindingSet.Bind(node, body) - return body -} +// // InstantiatePhysicsBodyTemplateDynamic creates a dynamic physics body in the +// // given scene from the provided body template and returns it. +// // +// // This operation needs to be called from the main thread. +// func InstantiatePhysicsBodyTemplateDynamic(scene *Scene, template BodyTemplate, nodes IdentifiableList[hierarchy.NodeID]) physics.Body { +// node := nodes.GetByID(template.NodeID) +// absMatrix := scene.Hierarchy().NodeAbsoluteMatrix(node) +// nodeName := scene.Hierarchy().NodeName(node) +// translation, rotation, _ := absMatrix.TRS() +// body := scene.physicsScene.CreateBody(physics.BodyInfo{ +// Name: nodeName, +// Definition: template.Definition, +// Position: translation, +// Rotation: rotation, +// }) +// scene.bodyBindingSet.Bind(node, body) +// return body +// } diff --git a/game/binding.go b/game/binding.go index 10a5e93e..471d8aac 100644 --- a/game/binding.go +++ b/game/binding.go @@ -45,13 +45,19 @@ func (b *animationBinding) OnStaleBinding(scene *hierarchy.Scene, player *animat } // NewBodyBinding creates a new binding for physics bodies. -func NewBodyBinding() hierarchy.SourceBinding[physics.Body] { - return &bodyBinding{} +func NewBodyBinding(physicsScene *physics.Scene) hierarchy.SourceBinding[physics.BodyID] { + return &bodyBinding{ + physicsScene: physicsScene, + } +} + +type bodyBinding struct { + physicsScene *physics.Scene } -type bodyBinding struct{} +func (b *bodyBinding) OnSourceToNode(scene *hierarchy.Scene, bodyID physics.BodyID, id hierarchy.NodeID) { + body := b.physicsScene.Bodies().Handle(bodyID) -func (b *bodyBinding) OnSourceToNode(scene *hierarchy.Scene, body physics.Body, id hierarchy.NodeID) { currentTranslation := body.Position() currentRotation := body.Rotation() @@ -62,8 +68,8 @@ func (b *bodyBinding) OnSourceToNode(scene *hierarchy.Scene, body physics.Body, )) } -func (b *bodyBinding) OnStaleBinding(scene *hierarchy.Scene, body physics.Body) { - body.Delete() +func (b *bodyBinding) OnStaleBinding(scene *hierarchy.Scene, bodyID physics.BodyID) { + b.physicsScene.Bodies().Delete(bodyID) } // NewSkyBinding creates a new binding for skies. diff --git a/game/physics/body.go b/game/physics/body.go index e29a36cc..167f98d4 100644 --- a/game/physics/body.go +++ b/game/physics/body.go @@ -49,6 +49,10 @@ func (v BodyView) Create(position dprec.Vec3, rotation dprec.Quat) BodyID { } } +func (v BodyView) CreateHandle(position dprec.Vec3, rotation dprec.Quat) BodyHandle { + return v.Handle(v.Create(position, rotation)) +} + func (v BodyView) Delete(id BodyID) { body := v.resolve(id, true) @@ -215,6 +219,14 @@ func (v BodyView) refreshPlacement(id BodyID, body *bodyState) { }) } +func (v BodyView) idFromIndex(index int32) BodyID { + body := &v.scene.bodies[index] + return BodyID{ + index: index, + revision: body.revision, + } +} + func (v BodyView) resolve(id BodyID, required bool) *bodyState { if id.revision == 0 { if required { diff --git a/game/physics/constraint/chandelier.go b/game/physics/constraint/chandelier.go index c0a92b4e..3d3d2884 100644 --- a/game/physics/constraint/chandelier.go +++ b/game/physics/constraint/chandelier.go @@ -1,98 +1,98 @@ package constraint -import ( - "github.com/mokiat/gomath/dprec" - "github.com/mokiat/lacking/game/physics/solver" -) +// import ( +// "github.com/mokiat/gomath/dprec" +// "github.com/mokiat/lacking/game/physics/solver" +// ) -// NewChandelier creates a new Chandelier constraint solver. -func NewChandelier() *Chandelier { - return &Chandelier{ - fixture: dprec.ZeroVec3(), - radius: dprec.ZeroVec3(), - length: 1.0, - } -} +// // NewChandelier creates a new Chandelier constraint solver. +// func NewChandelier() *Chandelier { +// return &Chandelier{ +// fixture: dprec.ZeroVec3(), +// radius: dprec.ZeroVec3(), +// length: 1.0, +// } +// } -var _ solver.Constraint = (*Chandelier)(nil) +// var _ solver.Constraint = (*Chandelier)(nil) -// Chandelier represents the solution for a constraint -// that keeps a body hanging off of a fixture location similar -// to a chandelier. -type Chandelier struct { - fixture dprec.Vec3 - radius dprec.Vec3 - length float64 +// // Chandelier represents the solution for a constraint +// // that keeps a body hanging off of a fixture location similar +// // to a chandelier. +// type Chandelier struct { +// fixture dprec.Vec3 +// radius dprec.Vec3 +// length float64 - jacobian solver.Jacobian - drift float64 -} +// jacobian solver.Jacobian +// drift float64 +// } -// Fixture returns the fixture location for the chandelier hook. -func (s *Chandelier) Fixture() dprec.Vec3 { - return s.fixture -} +// // Fixture returns the fixture location for the chandelier hook. +// func (s *Chandelier) Fixture() dprec.Vec3 { +// return s.fixture +// } -// SetFixture changes the fixture location for the chandelier hook. -func (s *Chandelier) SetFixture(fixture dprec.Vec3) *Chandelier { - s.fixture = fixture - return s -} +// // SetFixture changes the fixture location for the chandelier hook. +// func (s *Chandelier) SetFixture(fixture dprec.Vec3) *Chandelier { +// s.fixture = fixture +// return s +// } -// Radius returns the radius vector of the contact point on the object. -// -// The vector is in the object's local space. -func (s *Chandelier) Radius() dprec.Vec3 { - return s.radius -} +// // Radius returns the radius vector of the contact point on the object. +// // +// // The vector is in the object's local space. +// func (s *Chandelier) Radius() dprec.Vec3 { +// return s.radius +// } -// SetRadius changes the radius vector of the contact point on the object. -// -// The vector is in the object's local space. -func (s *Chandelier) SetRadius(radius dprec.Vec3) *Chandelier { - s.radius = radius - return s -} +// // SetRadius changes the radius vector of the contact point on the object. +// // +// // The vector is in the object's local space. +// func (s *Chandelier) SetRadius(radius dprec.Vec3) *Chandelier { +// s.radius = radius +// return s +// } -// Length returns the chandelier length. -func (s *Chandelier) Length() float64 { - return s.length -} +// // Length returns the chandelier length. +// func (s *Chandelier) Length() float64 { +// return s.length +// } -// SetLength changes the chandelier length. -func (s *Chandelier) SetLength(length float64) *Chandelier { - s.length = length - return s -} +// // SetLength changes the chandelier length. +// func (s *Chandelier) SetLength(length float64) *Chandelier { +// s.length = length +// return s +// } -// Reset re-evaluates the constraint. -func (s *Chandelier) Reset(ctx solver.Context) { - radiusWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.radius) - pointWS := dprec.Vec3Sum(ctx.Target.Position(), radiusWS) - deltaPositionWS := dprec.Vec3Diff(pointWS, s.fixture) - if distance := deltaPositionWS.Length(); distance > solver.Epsilon { - normalWS := dprec.Vec3Quot(deltaPositionWS, distance) - s.jacobian = solver.Jacobian{ - LinearSlope: normalWS, - AngularSlope: dprec.Vec3Cross(radiusWS, normalWS), - } - s.drift = distance - s.length - } else { - s.jacobian = solver.Jacobian{} - s.drift = -s.length - } -} +// // Reset re-evaluates the constraint. +// func (s *Chandelier) Reset(ctx solver.Context) { +// radiusWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.radius) +// pointWS := dprec.Vec3Sum(ctx.Target.Position(), radiusWS) +// deltaPositionWS := dprec.Vec3Diff(pointWS, s.fixture) +// if distance := deltaPositionWS.Length(); distance > solver.Epsilon { +// normalWS := dprec.Vec3Quot(deltaPositionWS, distance) +// s.jacobian = solver.Jacobian{ +// LinearSlope: normalWS, +// AngularSlope: dprec.Vec3Cross(radiusWS, normalWS), +// } +// s.drift = distance - s.length +// } else { +// s.jacobian = solver.Jacobian{} +// s.drift = -s.length +// } +// } -// ApplyImpulses applies impulses in order to keep the velocity part of -// the constraint satisfied. -func (s *Chandelier) ApplyImpulses(ctx solver.Context) { - solution := ctx.JacobianImpulseSolution(s.jacobian, s.drift, 0.0) - ctx.Target.ApplyImpulse(solution) -} +// // ApplyImpulses applies impulses in order to keep the velocity part of +// // the constraint satisfied. +// func (s *Chandelier) ApplyImpulses(ctx solver.Context) { +// solution := ctx.JacobianImpulseSolution(s.jacobian, s.drift, 0.0) +// ctx.Target.ApplyImpulse(solution) +// } -// ApplyNudges applies nudges in order to keep the positional part of the -// constraint satisfied. -func (s *Chandelier) ApplyNudges(ctx solver.Context) { - solution := ctx.JacobianNudgeSolution(s.jacobian, s.drift) - ctx.Target.ApplyNudge(solution) -} +// // ApplyNudges applies nudges in order to keep the positional part of the +// // constraint satisfied. +// func (s *Chandelier) ApplyNudges(ctx solver.Context) { +// solution := ctx.JacobianNudgeSolution(s.jacobian, s.drift) +// ctx.Target.ApplyNudge(solution) +// } diff --git a/game/physics/constraint/clamp_direction_offset.go b/game/physics/constraint/clamp_direction_offset.go index 7090c766..5a76d5fa 100644 --- a/game/physics/constraint/clamp_direction_offset.go +++ b/game/physics/constraint/clamp_direction_offset.go @@ -1,144 +1,144 @@ package constraint -import ( - "github.com/mokiat/gomath/dprec" - "github.com/mokiat/lacking/game/physics/solver" -) - -// NewClampDirectionOffset creates a new ClampDirectionOffset constraint solver. -func NewClampDirectionOffset() *ClampDirectionOffset { - return &ClampDirectionOffset{ - direction: dprec.BasisYVec3(), - min: -1.0, - max: 1.0, - restitution: 0.0, - } -} - -var _ solver.PairConstraint = (*ClampDirectionOffset)(nil) - -// ClampDirectionOffset represents the solution for a constraint which ensures that -// the second body is within certain min and max bounds relative to the first -// body along a certain direction of the first body. -type ClampDirectionOffset struct { - direction dprec.Vec3 - min float64 - max float64 - restitution float64 - - jacobian solver.PairJacobian - drift float64 -} - -// Direction returns the constraint direction, which is in local space of -// the first body. -func (s *ClampDirectionOffset) Direction() dprec.Vec3 { - return s.direction -} - -// SetDirection changes the constraint direction, which must be in local space -// of the first body. -func (s *ClampDirectionOffset) SetDirection(direction dprec.Vec3) *ClampDirectionOffset { - s.direction = dprec.UnitVec3(direction) - return s -} - -// Min returns the lower bounds limit. -func (s *ClampDirectionOffset) Min() float64 { - return s.min -} - -// SetMin changes the lower bounds limit. -func (s *ClampDirectionOffset) SetMin(min float64) *ClampDirectionOffset { - s.min = min - return s -} - -// Max returns the upper bounds limit. -func (s *ClampDirectionOffset) Max() float64 { - return s.max -} - -// SetMax changes the upper bounds limit. -func (s *ClampDirectionOffset) SetMax(max float64) *ClampDirectionOffset { - s.max = max - return s -} - -// Restitution returns the restitution to be used when adjusting the -// two bodies when the constraint is not met. -func (s *ClampDirectionOffset) Restitution() float64 { - return s.restitution -} - -// SetRestitution changes the restitution to be used when adjusting the -// two bodies when the constraint is not met. -func (s *ClampDirectionOffset) SetRestitution(restitution float64) *ClampDirectionOffset { - s.restitution = restitution - return s -} - -func (s *ClampDirectionOffset) Reset(ctx solver.PairContext) { - dirWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.direction) - deltaPosition := dprec.Vec3Diff(ctx.Source.Position(), ctx.Target.Position()) - dirDistance := dprec.Vec3Dot(deltaPosition, dirWS) - - switch { - case dirDistance > s.max: - radius := dprec.Vec3Diff( - deltaPosition, - dprec.Vec3Prod(dirWS, dirDistance-s.max), - ) - s.jacobian = solver.PairJacobian{ - Target: solver.Jacobian{ - LinearSlope: dprec.InverseVec3(dirWS), - AngularSlope: dprec.Vec3Cross(dirWS, radius), - }, - Source: solver.Jacobian{ - LinearSlope: dirWS, - AngularSlope: dprec.ZeroVec3(), - }, - } - s.drift = dirDistance - s.max - - case dirDistance < s.min: - radius := dprec.Vec3Sum( - deltaPosition, - dprec.Vec3Prod(dirWS, s.min-dirDistance), - ) - s.jacobian = solver.PairJacobian{ - Target: solver.Jacobian{ - LinearSlope: dirWS, - AngularSlope: dprec.Vec3Cross(radius, dirWS), - }, - Source: solver.Jacobian{ - LinearSlope: dprec.InverseVec3(dirWS), - AngularSlope: dprec.ZeroVec3(), - }, - } - s.drift = s.min - dirDistance - - default: - s.jacobian = solver.PairJacobian{} - s.drift = 0 - } -} - -func (s *ClampDirectionOffset) ApplyImpulses(ctx solver.PairContext) { - // TODO: Should drift be passed to this check? - lambda := ctx.JacobianImpulseLambda(s.jacobian, s.drift, s.restitution) - if lambda > 0.0 { - return // moving away - } - solution := ctx.JacobianImpulseSolution(s.jacobian, s.drift, 0.0) - ctx.Target.ApplyImpulse(solution.Target) - ctx.Source.ApplyImpulse(solution.Source) -} - -func (s *ClampDirectionOffset) ApplyNudges(ctx solver.PairContext) { - if s.drift > 0 { - solution := ctx.JacobianNudgeSolution(s.jacobian, s.drift) - ctx.Target.ApplyNudge(solution.Target) - ctx.Source.ApplyNudge(solution.Source) - } -} +// import ( +// "github.com/mokiat/gomath/dprec" +// "github.com/mokiat/lacking/game/physics/solver" +// ) + +// // NewClampDirectionOffset creates a new ClampDirectionOffset constraint solver. +// func NewClampDirectionOffset() *ClampDirectionOffset { +// return &ClampDirectionOffset{ +// direction: dprec.BasisYVec3(), +// min: -1.0, +// max: 1.0, +// restitution: 0.0, +// } +// } + +// var _ solver.PairConstraint = (*ClampDirectionOffset)(nil) + +// // ClampDirectionOffset represents the solution for a constraint which ensures that +// // the second body is within certain min and max bounds relative to the first +// // body along a certain direction of the first body. +// type ClampDirectionOffset struct { +// direction dprec.Vec3 +// min float64 +// max float64 +// restitution float64 + +// jacobian solver.PairJacobian +// drift float64 +// } + +// // Direction returns the constraint direction, which is in local space of +// // the first body. +// func (s *ClampDirectionOffset) Direction() dprec.Vec3 { +// return s.direction +// } + +// // SetDirection changes the constraint direction, which must be in local space +// // of the first body. +// func (s *ClampDirectionOffset) SetDirection(direction dprec.Vec3) *ClampDirectionOffset { +// s.direction = dprec.UnitVec3(direction) +// return s +// } + +// // Min returns the lower bounds limit. +// func (s *ClampDirectionOffset) Min() float64 { +// return s.min +// } + +// // SetMin changes the lower bounds limit. +// func (s *ClampDirectionOffset) SetMin(min float64) *ClampDirectionOffset { +// s.min = min +// return s +// } + +// // Max returns the upper bounds limit. +// func (s *ClampDirectionOffset) Max() float64 { +// return s.max +// } + +// // SetMax changes the upper bounds limit. +// func (s *ClampDirectionOffset) SetMax(max float64) *ClampDirectionOffset { +// s.max = max +// return s +// } + +// // Restitution returns the restitution to be used when adjusting the +// // two bodies when the constraint is not met. +// func (s *ClampDirectionOffset) Restitution() float64 { +// return s.restitution +// } + +// // SetRestitution changes the restitution to be used when adjusting the +// // two bodies when the constraint is not met. +// func (s *ClampDirectionOffset) SetRestitution(restitution float64) *ClampDirectionOffset { +// s.restitution = restitution +// return s +// } + +// func (s *ClampDirectionOffset) Reset(ctx solver.PairContext) { +// dirWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.direction) +// deltaPosition := dprec.Vec3Diff(ctx.Source.Position(), ctx.Target.Position()) +// dirDistance := dprec.Vec3Dot(deltaPosition, dirWS) + +// switch { +// case dirDistance > s.max: +// radius := dprec.Vec3Diff( +// deltaPosition, +// dprec.Vec3Prod(dirWS, dirDistance-s.max), +// ) +// s.jacobian = solver.PairJacobian{ +// Target: solver.Jacobian{ +// LinearSlope: dprec.InverseVec3(dirWS), +// AngularSlope: dprec.Vec3Cross(dirWS, radius), +// }, +// Source: solver.Jacobian{ +// LinearSlope: dirWS, +// AngularSlope: dprec.ZeroVec3(), +// }, +// } +// s.drift = dirDistance - s.max + +// case dirDistance < s.min: +// radius := dprec.Vec3Sum( +// deltaPosition, +// dprec.Vec3Prod(dirWS, s.min-dirDistance), +// ) +// s.jacobian = solver.PairJacobian{ +// Target: solver.Jacobian{ +// LinearSlope: dirWS, +// AngularSlope: dprec.Vec3Cross(radius, dirWS), +// }, +// Source: solver.Jacobian{ +// LinearSlope: dprec.InverseVec3(dirWS), +// AngularSlope: dprec.ZeroVec3(), +// }, +// } +// s.drift = s.min - dirDistance + +// default: +// s.jacobian = solver.PairJacobian{} +// s.drift = 0 +// } +// } + +// func (s *ClampDirectionOffset) ApplyImpulses(ctx solver.PairContext) { +// // TODO: Should drift be passed to this check? +// lambda := ctx.JacobianImpulseLambda(s.jacobian, s.drift, s.restitution) +// if lambda > 0.0 { +// return // moving away +// } +// solution := ctx.JacobianImpulseSolution(s.jacobian, s.drift, 0.0) +// ctx.Target.ApplyImpulse(solution.Target) +// ctx.Source.ApplyImpulse(solution.Source) +// } + +// func (s *ClampDirectionOffset) ApplyNudges(ctx solver.PairContext) { +// if s.drift > 0 { +// solution := ctx.JacobianNudgeSolution(s.jacobian, s.drift) +// ctx.Target.ApplyNudge(solution.Target) +// ctx.Source.ApplyNudge(solution.Source) +// } +// } diff --git a/game/physics/constraint/coilover.go b/game/physics/constraint/coilover.go index 71e23898..91e4872a 100644 --- a/game/physics/constraint/coilover.go +++ b/game/physics/constraint/coilover.go @@ -1,139 +1,139 @@ package constraint -import ( - "github.com/mokiat/gomath/dprec" - "github.com/mokiat/lacking/game/physics/solver" -) - -// NewCoilover creates a new Coilover constraint solver. -func NewCoilover() *Coilover { - return &Coilover{ - primaryRadius: dprec.ZeroVec3(), - secondaryRadius: dprec.ZeroVec3(), - frequency: 1.0, - damping: 0.5, - } -} - -var _ solver.PairConstraint = (*Coilover)(nil) - -// Coilover represents the solution for a constraint that immitates -// a car coilover through a damped harmonic oscillator. -type Coilover struct { - primaryRadius dprec.Vec3 - secondaryRadius dprec.Vec3 - frequency float64 - damping float64 - - appliedLambda float64 - jacobian solver.PairJacobian - drift float64 -} - -// PrimaryRadius returns the radius vector of the contact point -// on the primary object. -// -// The vector is in the object's local space. -func (s *Coilover) PrimaryRadius() dprec.Vec3 { - return s.primaryRadius -} - -// SetPrimaryRadius changes the radius vector of the contact point -// on the primary object. -// -// The vector is in the object's local space. -func (s *Coilover) SetPrimaryRadius(radius dprec.Vec3) *Coilover { - s.primaryRadius = radius - return s -} - -// SecondaryRadius returns the radius vector of the contact point -// on the secondary object. -// -// The vector is in the object's local space. -func (s *Coilover) SecondaryRadius() dprec.Vec3 { - return s.secondaryRadius -} - -// SetSecondaryRadius changes the radius vector of the contact point -// on the secondary object. -// -// The vector is in the object's local space. -func (s *Coilover) SetSecondaryRadius(radius dprec.Vec3) *Coilover { - s.secondaryRadius = radius - return s -} - -// Frequency returns the frequency (in Hz) of the damped harmonic -// oscillator that represents this coilover. -func (s *Coilover) Frequency() float64 { - return s.frequency -} - -// SetFrequency changes the frequency (in Hz) of the damped harmonic -// oscillator that represents this coilover. -func (s *Coilover) SetFrequency(frequency float64) *Coilover { - s.frequency = frequency - return s -} - -// Damping returns the damping ratio of the damped harmonic oscillator -// that represents this coilover. -func (s *Coilover) Damping() float64 { - return s.damping -} - -// SetDamping changes the damping ratio of the damped harmonic oscillator -// that represents this coilover. -func (s *Coilover) SetDamping(damping float64) *Coilover { - s.damping = damping - return s -} - -func (s *Coilover) Reset(ctx solver.PairContext) { - s.appliedLambda = 0.0 - - primaryRadiusWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.primaryRadius) - primaryPointWS := dprec.Vec3Sum(ctx.Target.Position(), primaryRadiusWS) - secondaryRadiusWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.secondaryRadius) - secondaryPointWS := dprec.Vec3Sum(ctx.Source.Position(), secondaryRadiusWS) - - deltaPosition := dprec.Vec3Diff(secondaryPointWS, primaryPointWS) - s.drift = deltaPosition.Length() - normal := dprec.BasisYVec3() - if s.drift > solver.Epsilon { - normal = dprec.UnitVec3(deltaPosition) - } - s.jacobian = solver.PairJacobian{ - Target: solver.Jacobian{ - LinearSlope: dprec.InverseVec3(normal), - AngularSlope: dprec.Vec3Cross(normal, primaryRadiusWS), - }, - Source: solver.Jacobian{ - LinearSlope: normal, - AngularSlope: dprec.Vec3Cross(secondaryRadiusWS, normal), - }, - } -} - -func (s *Coilover) ApplyImpulses(ctx solver.PairContext) { - if s.drift < solver.Epsilon { - return - } - invertedEffectiveMass := s.jacobian.InverseEffectiveMass(ctx.Target, ctx.Source) - w := 2.0 * dprec.Pi * s.frequency - dc := 2.0 * s.damping * w / invertedEffectiveMass - k := w * w / invertedEffectiveMass - - gamma := 1.0 / (ctx.DeltaTime * (dc + ctx.DeltaTime*k)) - beta := ctx.DeltaTime * k * gamma - - effectiveVelocity := s.jacobian.EffectiveVelocity(ctx.Target, ctx.Source) - lambda := -(effectiveVelocity + beta*s.drift + gamma*s.appliedLambda) / (invertedEffectiveMass + gamma) - solution := s.jacobian.Impulse(lambda) - ctx.Target.ApplyImpulse(solution.Target) - ctx.Source.ApplyImpulse(solution.Source) - s.appliedLambda += lambda -} - -func (s *Coilover) ApplyNudges(ctx solver.PairContext) {} +// import ( +// "github.com/mokiat/gomath/dprec" +// "github.com/mokiat/lacking/game/physics/solver" +// ) + +// // NewCoilover creates a new Coilover constraint solver. +// func NewCoilover() *Coilover { +// return &Coilover{ +// primaryRadius: dprec.ZeroVec3(), +// secondaryRadius: dprec.ZeroVec3(), +// frequency: 1.0, +// damping: 0.5, +// } +// } + +// var _ solver.PairConstraint = (*Coilover)(nil) + +// // Coilover represents the solution for a constraint that immitates +// // a car coilover through a damped harmonic oscillator. +// type Coilover struct { +// primaryRadius dprec.Vec3 +// secondaryRadius dprec.Vec3 +// frequency float64 +// damping float64 + +// appliedLambda float64 +// jacobian solver.PairJacobian +// drift float64 +// } + +// // PrimaryRadius returns the radius vector of the contact point +// // on the primary object. +// // +// // The vector is in the object's local space. +// func (s *Coilover) PrimaryRadius() dprec.Vec3 { +// return s.primaryRadius +// } + +// // SetPrimaryRadius changes the radius vector of the contact point +// // on the primary object. +// // +// // The vector is in the object's local space. +// func (s *Coilover) SetPrimaryRadius(radius dprec.Vec3) *Coilover { +// s.primaryRadius = radius +// return s +// } + +// // SecondaryRadius returns the radius vector of the contact point +// // on the secondary object. +// // +// // The vector is in the object's local space. +// func (s *Coilover) SecondaryRadius() dprec.Vec3 { +// return s.secondaryRadius +// } + +// // SetSecondaryRadius changes the radius vector of the contact point +// // on the secondary object. +// // +// // The vector is in the object's local space. +// func (s *Coilover) SetSecondaryRadius(radius dprec.Vec3) *Coilover { +// s.secondaryRadius = radius +// return s +// } + +// // Frequency returns the frequency (in Hz) of the damped harmonic +// // oscillator that represents this coilover. +// func (s *Coilover) Frequency() float64 { +// return s.frequency +// } + +// // SetFrequency changes the frequency (in Hz) of the damped harmonic +// // oscillator that represents this coilover. +// func (s *Coilover) SetFrequency(frequency float64) *Coilover { +// s.frequency = frequency +// return s +// } + +// // Damping returns the damping ratio of the damped harmonic oscillator +// // that represents this coilover. +// func (s *Coilover) Damping() float64 { +// return s.damping +// } + +// // SetDamping changes the damping ratio of the damped harmonic oscillator +// // that represents this coilover. +// func (s *Coilover) SetDamping(damping float64) *Coilover { +// s.damping = damping +// return s +// } + +// func (s *Coilover) Reset(ctx solver.PairContext) { +// s.appliedLambda = 0.0 + +// primaryRadiusWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.primaryRadius) +// primaryPointWS := dprec.Vec3Sum(ctx.Target.Position(), primaryRadiusWS) +// secondaryRadiusWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.secondaryRadius) +// secondaryPointWS := dprec.Vec3Sum(ctx.Source.Position(), secondaryRadiusWS) + +// deltaPosition := dprec.Vec3Diff(secondaryPointWS, primaryPointWS) +// s.drift = deltaPosition.Length() +// normal := dprec.BasisYVec3() +// if s.drift > solver.Epsilon { +// normal = dprec.UnitVec3(deltaPosition) +// } +// s.jacobian = solver.PairJacobian{ +// Target: solver.Jacobian{ +// LinearSlope: dprec.InverseVec3(normal), +// AngularSlope: dprec.Vec3Cross(normal, primaryRadiusWS), +// }, +// Source: solver.Jacobian{ +// LinearSlope: normal, +// AngularSlope: dprec.Vec3Cross(secondaryRadiusWS, normal), +// }, +// } +// } + +// func (s *Coilover) ApplyImpulses(ctx solver.PairContext) { +// if s.drift < solver.Epsilon { +// return +// } +// invertedEffectiveMass := s.jacobian.InverseEffectiveMass(ctx.Target, ctx.Source) +// w := 2.0 * dprec.Pi * s.frequency +// dc := 2.0 * s.damping * w / invertedEffectiveMass +// k := w * w / invertedEffectiveMass + +// gamma := 1.0 / (ctx.DeltaTime * (dc + ctx.DeltaTime*k)) +// beta := ctx.DeltaTime * k * gamma + +// effectiveVelocity := s.jacobian.EffectiveVelocity(ctx.Target, ctx.Source) +// lambda := -(effectiveVelocity + beta*s.drift + gamma*s.appliedLambda) / (invertedEffectiveMass + gamma) +// solution := s.jacobian.Impulse(lambda) +// ctx.Target.ApplyImpulse(solution.Target) +// ctx.Source.ApplyImpulse(solution.Source) +// s.appliedLambda += lambda +// } + +// func (s *Coilover) ApplyNudges(ctx solver.PairContext) {} diff --git a/game/physics/constraint/collision.go b/game/physics/constraint/collision.go index b17192ff..d2b7e110 100644 --- a/game/physics/constraint/collision.go +++ b/game/physics/constraint/collision.go @@ -1,227 +1,227 @@ package constraint -import ( - "github.com/mokiat/gomath/dprec" - "github.com/mokiat/lacking/game/physics/solver" -) - -type CollisionState struct { - PropFrictionCoefficient float64 - PropRestitutionCoefficient float64 - - BodyNormal dprec.Vec3 - BodyPoint dprec.Vec3 - BodyFrictionCoefficient float64 - BodyRestitutionCoefficient float64 - - Depth float64 -} - -var _ solver.Constraint = (*Collision)(nil) - -type Collision struct { - propFrictionCoefficient float64 - propRestitutionCoefficient float64 - - bodyCollisionNormal dprec.Vec3 - bodyCollisionPoint dprec.Vec3 - bodyFrictionCoefficient float64 - bodyRestitutionCoefficient float64 - - collisionDepth float64 - - radius dprec.Vec3 - jacobian solver.Jacobian - drift float64 -} - -func (s *Collision) Init(state CollisionState) { - s.propFrictionCoefficient = state.PropFrictionCoefficient - s.propRestitutionCoefficient = state.PropRestitutionCoefficient - - s.bodyCollisionNormal = state.BodyNormal - s.bodyCollisionPoint = state.BodyPoint - s.bodyFrictionCoefficient = state.BodyFrictionCoefficient - s.bodyRestitutionCoefficient = state.BodyRestitutionCoefficient - - s.collisionDepth = state.Depth -} - -func (s *Collision) Reset(ctx solver.Context) { - radiusWS := dprec.Vec3Diff(s.bodyCollisionPoint, ctx.Target.Position()) - s.radius = dprec.QuatVec3Rotation(dprec.ConjugateQuat(ctx.Target.Rotation()), radiusWS) - s.jacobian = solver.Jacobian{ - LinearSlope: dprec.InverseVec3(s.bodyCollisionNormal), - AngularSlope: dprec.Vec3Cross(s.bodyCollisionNormal, radiusWS), - } - s.drift = s.collisionDepth -} - -func (s *Collision) ApplyImpulses(ctx solver.Context) { - // NOTE: We include the bounce force in the max friction calculation. - // This might actually be accurate, since you have both the force of - // the object pushing down, as well as the elastic force pushing further - // down, trying to bounce the object up. - restitution := s.propRestitutionCoefficient * s.bodyRestitutionCoefficient - - // Bounce solution - pressureLambda := ctx.JacobianImpulseLambda(s.jacobian, 0.0, restitution) - if pressureLambda > 0 { - return // moving away - } - bounceSolution := ctx.JacobianImpulseSolution(s.jacobian, s.collisionDepth, restitution) - - // Friction solution - radiusWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.radius) - pointVelocity := dprec.Vec3Sum(ctx.Target.LinearVelocity(), dprec.Vec3Cross(ctx.Target.AngularVelocity(), radiusWS)) - verticalVelocity := dprec.Vec3Prod(s.bodyCollisionNormal, dprec.Vec3Dot(s.bodyCollisionNormal, pointVelocity)) - lateralVelocity := dprec.Vec3Diff(pointVelocity, verticalVelocity) - frictionSolution := solver.Impulse{} - if lng := lateralVelocity.Length(); lng > solver.Epsilon { - lateralDirection := dprec.UnitVec3(lateralVelocity) - frictionJacobian := solver.Jacobian{ - LinearSlope: lateralDirection, - AngularSlope: dprec.Vec3Cross(radiusWS, lateralDirection), - } - frictionLambda := ctx.JacobianImpulseLambda(frictionJacobian, 0.0, 0.0) - // TODO: Have friction coefficient configurable - // const frictionCoefficient = 0.9 // around 0.7 to 0.9 is realistic for dry asphalt - const frictionCoefficient = 1.2 - maxFrictionLambda := pressureLambda * frictionCoefficient - if -frictionLambda > -maxFrictionLambda { - frictionLambda = maxFrictionLambda - } - frictionSolution = frictionJacobian.Impulse(frictionLambda) - } - - // Note: Make sure to apply these as late as possible, otherwise you are - // introducing noise that is picked up by subsequent calculations. - ctx.Target.ApplyImpulse(bounceSolution) - ctx.Target.ApplyImpulse(frictionSolution) -} - -func (s *Collision) ApplyNudges(ctx solver.Context) { - // TODO: Add nudge solution -} - -type PairCollisionState struct { - PrimaryNormal dprec.Vec3 - PrimaryPoint dprec.Vec3 - PrimaryFrictionCoefficient float64 - PrimaryRestitutionCoefficient float64 - - SecondaryNormal dprec.Vec3 - SecondaryPoint dprec.Vec3 - SecondaryFrictionCoefficient float64 - SecondaryRestitutionCoefficient float64 - - Depth float64 -} - -var _ solver.PairConstraint = (*PairCollision)(nil) - -type PairCollision struct { - primaryCollisionNormal dprec.Vec3 - primaryCollisionPoint dprec.Vec3 - primaryFrictionCoefficient float64 - primaryRestitutionCoefficient float64 - - secondaryCollisionNormal dprec.Vec3 - secondaryCollisionPoint dprec.Vec3 - secondaryFrictionCoefficient float64 - secondaryRestitutionCoefficient float64 - - collisionDepth float64 - - primaryRadius dprec.Vec3 - secondaryRadius dprec.Vec3 - jacobian solver.PairJacobian -} - -func (s *PairCollision) Init(state PairCollisionState) { - s.primaryCollisionNormal = state.PrimaryNormal - s.primaryCollisionPoint = state.PrimaryPoint - s.primaryFrictionCoefficient = state.PrimaryFrictionCoefficient - s.primaryRestitutionCoefficient = state.PrimaryRestitutionCoefficient - - s.secondaryCollisionNormal = state.SecondaryNormal - s.secondaryCollisionPoint = state.SecondaryPoint - s.secondaryFrictionCoefficient = state.SecondaryFrictionCoefficient - s.secondaryRestitutionCoefficient = state.SecondaryRestitutionCoefficient - - s.collisionDepth = state.Depth -} - -func (s *PairCollision) Reset(ctx solver.PairContext) { - primaryRadiusWS := dprec.Vec3Diff(s.primaryCollisionPoint, ctx.Target.Position()) - s.primaryRadius = dprec.QuatVec3Rotation(dprec.ConjugateQuat(ctx.Target.Rotation()), primaryRadiusWS) - secondaryRadiusWS := dprec.Vec3Diff(s.secondaryCollisionPoint, ctx.Source.Position()) - s.secondaryRadius = dprec.QuatVec3Rotation(dprec.ConjugateQuat(ctx.Source.Rotation()), secondaryRadiusWS) - s.jacobian = solver.PairJacobian{ - Target: solver.Jacobian{ - LinearSlope: dprec.InverseVec3(s.primaryCollisionNormal), - AngularSlope: dprec.Vec3Cross(s.primaryCollisionNormal, primaryRadiusWS), - }, - Source: solver.Jacobian{ - LinearSlope: dprec.InverseVec3(s.secondaryCollisionNormal), - AngularSlope: dprec.Vec3Cross(s.secondaryCollisionNormal, secondaryRadiusWS), - }, - } -} - -func (s *PairCollision) ApplyImpulses(ctx solver.PairContext) { - // NOTE: We include the bounce force in the max friction calculation. - // This might actually be accurate, since you have both the force of - // the object pushing down, as well as the elastic force pushing further - // down, trying to bounce the object up. - restitution := s.primaryRestitutionCoefficient * s.secondaryRestitutionCoefficient - - // Bounce solution - pressureLambda := ctx.JacobianImpulseLambda(s.jacobian, 0.0, restitution) - if pressureLambda > 0 { - return // moving away - } - bounceSolution := ctx.JacobianImpulseSolution(s.jacobian, s.collisionDepth, restitution) - - // Friction solution - primaryRadiusWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.primaryRadius) - primaryPointVelocity := dprec.Vec3Sum(ctx.Target.LinearVelocity(), dprec.Vec3Cross(ctx.Target.AngularVelocity(), primaryRadiusWS)) - secondaryRadiusWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.secondaryRadius) - secondaryPointVelocity := dprec.Vec3Sum(ctx.Source.LinearVelocity(), dprec.Vec3Cross(ctx.Source.AngularVelocity(), secondaryRadiusWS)) - deltaPointVelocity := dprec.Vec3Diff(primaryPointVelocity, secondaryPointVelocity) - verticalVelocity := dprec.Vec3Prod(s.secondaryCollisionNormal, dprec.Vec3Dot(s.secondaryCollisionNormal, deltaPointVelocity)) - lateralVelocity := dprec.Vec3Diff(deltaPointVelocity, verticalVelocity) - frictionSolution := solver.PairImpulse{} - if lng := lateralVelocity.Length(); lng > solver.Epsilon { - lateralDirection := dprec.UnitVec3(lateralVelocity) - frictionJacobian := solver.PairJacobian{ - Target: solver.Jacobian{ - LinearSlope: lateralDirection, - AngularSlope: dprec.Vec3Cross(primaryRadiusWS, lateralDirection), - }, - Source: solver.Jacobian{ - LinearSlope: dprec.InverseVec3(lateralDirection), - AngularSlope: dprec.Vec3Cross(lateralDirection, secondaryRadiusWS), - }, - } - frictionLambda := ctx.JacobianImpulseLambda(frictionJacobian, 0.0, 0.0) - // TODO: Have friction coefficient configurable - const frictionCoefficient = 0.9 // around 0.7 to 0.9 is realistic for dry asphalt - maxFrictionLambda := pressureLambda * frictionCoefficient - if -frictionLambda > -maxFrictionLambda { - frictionLambda = maxFrictionLambda - } - frictionSolution = frictionJacobian.Impulse(frictionLambda) - } - - // Note: Make sure to apply these as late as possible, otherwise you are - // introducing noise that is picked up by subsequent calculations. - ctx.Target.ApplyImpulse(bounceSolution.Target) - ctx.Source.ApplyImpulse(bounceSolution.Source) - ctx.Target.ApplyImpulse(frictionSolution.Target) - ctx.Source.ApplyImpulse(frictionSolution.Source) -} - -func (s *PairCollision) ApplyNudges(ctx solver.PairContext) { - // TODO: Add nudge solution -} +// import ( +// "github.com/mokiat/gomath/dprec" +// "github.com/mokiat/lacking/game/physics/solver" +// ) + +// type CollisionState struct { +// PropFrictionCoefficient float64 +// PropRestitutionCoefficient float64 + +// BodyNormal dprec.Vec3 +// BodyPoint dprec.Vec3 +// BodyFrictionCoefficient float64 +// BodyRestitutionCoefficient float64 + +// Depth float64 +// } + +// var _ solver.Constraint = (*Collision)(nil) + +// type Collision struct { +// propFrictionCoefficient float64 +// propRestitutionCoefficient float64 + +// bodyCollisionNormal dprec.Vec3 +// bodyCollisionPoint dprec.Vec3 +// bodyFrictionCoefficient float64 +// bodyRestitutionCoefficient float64 + +// collisionDepth float64 + +// radius dprec.Vec3 +// jacobian solver.Jacobian +// drift float64 +// } + +// func (s *Collision) Init(state CollisionState) { +// s.propFrictionCoefficient = state.PropFrictionCoefficient +// s.propRestitutionCoefficient = state.PropRestitutionCoefficient + +// s.bodyCollisionNormal = state.BodyNormal +// s.bodyCollisionPoint = state.BodyPoint +// s.bodyFrictionCoefficient = state.BodyFrictionCoefficient +// s.bodyRestitutionCoefficient = state.BodyRestitutionCoefficient + +// s.collisionDepth = state.Depth +// } + +// func (s *Collision) Reset(ctx solver.Context) { +// radiusWS := dprec.Vec3Diff(s.bodyCollisionPoint, ctx.Target.Position()) +// s.radius = dprec.QuatVec3Rotation(dprec.ConjugateQuat(ctx.Target.Rotation()), radiusWS) +// s.jacobian = solver.Jacobian{ +// LinearSlope: dprec.InverseVec3(s.bodyCollisionNormal), +// AngularSlope: dprec.Vec3Cross(s.bodyCollisionNormal, radiusWS), +// } +// s.drift = s.collisionDepth +// } + +// func (s *Collision) ApplyImpulses(ctx solver.Context) { +// // NOTE: We include the bounce force in the max friction calculation. +// // This might actually be accurate, since you have both the force of +// // the object pushing down, as well as the elastic force pushing further +// // down, trying to bounce the object up. +// restitution := s.propRestitutionCoefficient * s.bodyRestitutionCoefficient + +// // Bounce solution +// pressureLambda := ctx.JacobianImpulseLambda(s.jacobian, 0.0, restitution) +// if pressureLambda > 0 { +// return // moving away +// } +// bounceSolution := ctx.JacobianImpulseSolution(s.jacobian, s.collisionDepth, restitution) + +// // Friction solution +// radiusWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.radius) +// pointVelocity := dprec.Vec3Sum(ctx.Target.LinearVelocity(), dprec.Vec3Cross(ctx.Target.AngularVelocity(), radiusWS)) +// verticalVelocity := dprec.Vec3Prod(s.bodyCollisionNormal, dprec.Vec3Dot(s.bodyCollisionNormal, pointVelocity)) +// lateralVelocity := dprec.Vec3Diff(pointVelocity, verticalVelocity) +// frictionSolution := solver.Impulse{} +// if lng := lateralVelocity.Length(); lng > solver.Epsilon { +// lateralDirection := dprec.UnitVec3(lateralVelocity) +// frictionJacobian := solver.Jacobian{ +// LinearSlope: lateralDirection, +// AngularSlope: dprec.Vec3Cross(radiusWS, lateralDirection), +// } +// frictionLambda := ctx.JacobianImpulseLambda(frictionJacobian, 0.0, 0.0) +// // TODO: Have friction coefficient configurable +// // const frictionCoefficient = 0.9 // around 0.7 to 0.9 is realistic for dry asphalt +// const frictionCoefficient = 1.2 +// maxFrictionLambda := pressureLambda * frictionCoefficient +// if -frictionLambda > -maxFrictionLambda { +// frictionLambda = maxFrictionLambda +// } +// frictionSolution = frictionJacobian.Impulse(frictionLambda) +// } + +// // Note: Make sure to apply these as late as possible, otherwise you are +// // introducing noise that is picked up by subsequent calculations. +// ctx.Target.ApplyImpulse(bounceSolution) +// ctx.Target.ApplyImpulse(frictionSolution) +// } + +// func (s *Collision) ApplyNudges(ctx solver.Context) { +// // TODO: Add nudge solution +// } + +// type PairCollisionState struct { +// PrimaryNormal dprec.Vec3 +// PrimaryPoint dprec.Vec3 +// PrimaryFrictionCoefficient float64 +// PrimaryRestitutionCoefficient float64 + +// SecondaryNormal dprec.Vec3 +// SecondaryPoint dprec.Vec3 +// SecondaryFrictionCoefficient float64 +// SecondaryRestitutionCoefficient float64 + +// Depth float64 +// } + +// var _ solver.PairConstraint = (*PairCollision)(nil) + +// type PairCollision struct { +// primaryCollisionNormal dprec.Vec3 +// primaryCollisionPoint dprec.Vec3 +// primaryFrictionCoefficient float64 +// primaryRestitutionCoefficient float64 + +// secondaryCollisionNormal dprec.Vec3 +// secondaryCollisionPoint dprec.Vec3 +// secondaryFrictionCoefficient float64 +// secondaryRestitutionCoefficient float64 + +// collisionDepth float64 + +// primaryRadius dprec.Vec3 +// secondaryRadius dprec.Vec3 +// jacobian solver.PairJacobian +// } + +// func (s *PairCollision) Init(state PairCollisionState) { +// s.primaryCollisionNormal = state.PrimaryNormal +// s.primaryCollisionPoint = state.PrimaryPoint +// s.primaryFrictionCoefficient = state.PrimaryFrictionCoefficient +// s.primaryRestitutionCoefficient = state.PrimaryRestitutionCoefficient + +// s.secondaryCollisionNormal = state.SecondaryNormal +// s.secondaryCollisionPoint = state.SecondaryPoint +// s.secondaryFrictionCoefficient = state.SecondaryFrictionCoefficient +// s.secondaryRestitutionCoefficient = state.SecondaryRestitutionCoefficient + +// s.collisionDepth = state.Depth +// } + +// func (s *PairCollision) Reset(ctx solver.PairContext) { +// primaryRadiusWS := dprec.Vec3Diff(s.primaryCollisionPoint, ctx.Target.Position()) +// s.primaryRadius = dprec.QuatVec3Rotation(dprec.ConjugateQuat(ctx.Target.Rotation()), primaryRadiusWS) +// secondaryRadiusWS := dprec.Vec3Diff(s.secondaryCollisionPoint, ctx.Source.Position()) +// s.secondaryRadius = dprec.QuatVec3Rotation(dprec.ConjugateQuat(ctx.Source.Rotation()), secondaryRadiusWS) +// s.jacobian = solver.PairJacobian{ +// Target: solver.Jacobian{ +// LinearSlope: dprec.InverseVec3(s.primaryCollisionNormal), +// AngularSlope: dprec.Vec3Cross(s.primaryCollisionNormal, primaryRadiusWS), +// }, +// Source: solver.Jacobian{ +// LinearSlope: dprec.InverseVec3(s.secondaryCollisionNormal), +// AngularSlope: dprec.Vec3Cross(s.secondaryCollisionNormal, secondaryRadiusWS), +// }, +// } +// } + +// func (s *PairCollision) ApplyImpulses(ctx solver.PairContext) { +// // NOTE: We include the bounce force in the max friction calculation. +// // This might actually be accurate, since you have both the force of +// // the object pushing down, as well as the elastic force pushing further +// // down, trying to bounce the object up. +// restitution := s.primaryRestitutionCoefficient * s.secondaryRestitutionCoefficient + +// // Bounce solution +// pressureLambda := ctx.JacobianImpulseLambda(s.jacobian, 0.0, restitution) +// if pressureLambda > 0 { +// return // moving away +// } +// bounceSolution := ctx.JacobianImpulseSolution(s.jacobian, s.collisionDepth, restitution) + +// // Friction solution +// primaryRadiusWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.primaryRadius) +// primaryPointVelocity := dprec.Vec3Sum(ctx.Target.LinearVelocity(), dprec.Vec3Cross(ctx.Target.AngularVelocity(), primaryRadiusWS)) +// secondaryRadiusWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.secondaryRadius) +// secondaryPointVelocity := dprec.Vec3Sum(ctx.Source.LinearVelocity(), dprec.Vec3Cross(ctx.Source.AngularVelocity(), secondaryRadiusWS)) +// deltaPointVelocity := dprec.Vec3Diff(primaryPointVelocity, secondaryPointVelocity) +// verticalVelocity := dprec.Vec3Prod(s.secondaryCollisionNormal, dprec.Vec3Dot(s.secondaryCollisionNormal, deltaPointVelocity)) +// lateralVelocity := dprec.Vec3Diff(deltaPointVelocity, verticalVelocity) +// frictionSolution := solver.PairImpulse{} +// if lng := lateralVelocity.Length(); lng > solver.Epsilon { +// lateralDirection := dprec.UnitVec3(lateralVelocity) +// frictionJacobian := solver.PairJacobian{ +// Target: solver.Jacobian{ +// LinearSlope: lateralDirection, +// AngularSlope: dprec.Vec3Cross(primaryRadiusWS, lateralDirection), +// }, +// Source: solver.Jacobian{ +// LinearSlope: dprec.InverseVec3(lateralDirection), +// AngularSlope: dprec.Vec3Cross(lateralDirection, secondaryRadiusWS), +// }, +// } +// frictionLambda := ctx.JacobianImpulseLambda(frictionJacobian, 0.0, 0.0) +// // TODO: Have friction coefficient configurable +// const frictionCoefficient = 0.9 // around 0.7 to 0.9 is realistic for dry asphalt +// maxFrictionLambda := pressureLambda * frictionCoefficient +// if -frictionLambda > -maxFrictionLambda { +// frictionLambda = maxFrictionLambda +// } +// frictionSolution = frictionJacobian.Impulse(frictionLambda) +// } + +// // Note: Make sure to apply these as late as possible, otherwise you are +// // introducing noise that is picked up by subsequent calculations. +// ctx.Target.ApplyImpulse(bounceSolution.Target) +// ctx.Source.ApplyImpulse(bounceSolution.Source) +// ctx.Target.ApplyImpulse(frictionSolution.Target) +// ctx.Source.ApplyImpulse(frictionSolution.Source) +// } + +// func (s *PairCollision) ApplyNudges(ctx solver.PairContext) { +// // TODO: Add nudge solution +// } diff --git a/game/physics/constraint/combined.go b/game/physics/constraint/combined.go index 7f1ef084..c10fd7cb 100644 --- a/game/physics/constraint/combined.go +++ b/game/physics/constraint/combined.go @@ -1,71 +1,71 @@ package constraint -import "github.com/mokiat/lacking/game/physics/solver" +// import "github.com/mokiat/lacking/game/physics/solver" -// NewCombined creates a new Combined solver based on the specified -// sub-solvers. -func NewCombined(delegates ...solver.Constraint) *Combined { - return &Combined{ - delegates: delegates, - } -} +// // NewCombined creates a new Combined solver based on the specified +// // sub-solvers. +// func NewCombined(delegates ...solver.Constraint) *Combined { +// return &Combined{ +// delegates: delegates, +// } +// } -var _ solver.Constraint = (*Combined)(nil) +// var _ solver.Constraint = (*Combined)(nil) -// Combined is a single-object solver that delegates its logic to a -// number of sub-solvers. -type Combined struct { - delegates []solver.Constraint -} +// // Combined is a single-object solver that delegates its logic to a +// // number of sub-solvers. +// type Combined struct { +// delegates []solver.Constraint +// } -func (s *Combined) Reset(ctx solver.Context) { - for _, delegate := range s.delegates { - delegate.Reset(ctx) - } -} +// func (s *Combined) Reset(ctx solver.Context) { +// for _, delegate := range s.delegates { +// delegate.Reset(ctx) +// } +// } -func (s *Combined) ApplyImpulses(ctx solver.Context) { - for _, delegate := range s.delegates { - delegate.ApplyImpulses(ctx) - } -} +// func (s *Combined) ApplyImpulses(ctx solver.Context) { +// for _, delegate := range s.delegates { +// delegate.ApplyImpulses(ctx) +// } +// } -func (s *Combined) ApplyNudges(ctx solver.Context) { - for _, delegate := range s.delegates { - delegate.ApplyNudges(ctx) - } -} +// func (s *Combined) ApplyNudges(ctx solver.Context) { +// for _, delegate := range s.delegates { +// delegate.ApplyNudges(ctx) +// } +// } -// NewPairCombined creates a new PairCombined solver based on the specified -// sub-solvers. -func NewPairCombined(delegates ...solver.PairConstraint) *PairCombined { - return &PairCombined{ - delegates: delegates, - } -} +// // NewPairCombined creates a new PairCombined solver based on the specified +// // sub-solvers. +// func NewPairCombined(delegates ...solver.PairConstraint) *PairCombined { +// return &PairCombined{ +// delegates: delegates, +// } +// } -var _ solver.PairConstraint = (*PairCombined)(nil) +// var _ solver.PairConstraint = (*PairCombined)(nil) -// PairCombined is a double-object solver that delegates its logic to a -// number of sub-solvers. -type PairCombined struct { - delegates []solver.PairConstraint -} +// // PairCombined is a double-object solver that delegates its logic to a +// // number of sub-solvers. +// type PairCombined struct { +// delegates []solver.PairConstraint +// } -func (s *PairCombined) Reset(ctx solver.PairContext) { - for _, delegate := range s.delegates { - delegate.Reset(ctx) - } -} +// func (s *PairCombined) Reset(ctx solver.PairContext) { +// for _, delegate := range s.delegates { +// delegate.Reset(ctx) +// } +// } -func (s *PairCombined) ApplyImpulses(ctx solver.PairContext) { - for _, delegate := range s.delegates { - delegate.ApplyImpulses(ctx) - } -} +// func (s *PairCombined) ApplyImpulses(ctx solver.PairContext) { +// for _, delegate := range s.delegates { +// delegate.ApplyImpulses(ctx) +// } +// } -func (s *PairCombined) ApplyNudges(ctx solver.PairContext) { - for _, delegate := range s.delegates { - delegate.ApplyNudges(ctx) - } -} +// func (s *PairCombined) ApplyNudges(ctx solver.PairContext) { +// for _, delegate := range s.delegates { +// delegate.ApplyNudges(ctx) +// } +// } diff --git a/game/physics/constraint/copy_direction.go b/game/physics/constraint/copy_direction.go index cfccb65e..14ba7322 100644 --- a/game/physics/constraint/copy_direction.go +++ b/game/physics/constraint/copy_direction.go @@ -1,79 +1,79 @@ package constraint -import ( - "math" +// import ( +// "math" - "github.com/mokiat/gomath/dprec" - "github.com/mokiat/lacking/game/physics/solver" -) +// "github.com/mokiat/gomath/dprec" +// "github.com/mokiat/lacking/game/physics/solver" +// ) -// NewCopyDirection creates a new CopyDirection constraint solver. -func NewCopyDirection() *CopyDirection { - return &CopyDirection{ - primaryDirection: dprec.BasisYVec3(), - secondaryDirection: dprec.BasisYVec3(), - } -} +// // NewCopyDirection creates a new CopyDirection constraint solver. +// func NewCopyDirection() *CopyDirection { +// return &CopyDirection{ +// primaryDirection: dprec.BasisYVec3(), +// secondaryDirection: dprec.BasisYVec3(), +// } +// } -var _ solver.PairConstraint = (*CopyDirection)(nil) +// var _ solver.PairConstraint = (*CopyDirection)(nil) -// CopyDirection ensures that the second body has the same direction as -// the first one. -// This solver is immediate - it does not use impulses or nudges. -type CopyDirection struct { - primaryDirection dprec.Vec3 - secondaryDirection dprec.Vec3 -} +// // CopyDirection ensures that the second body has the same direction as +// // the first one. +// // This solver is immediate - it does not use impulses or nudges. +// type CopyDirection struct { +// primaryDirection dprec.Vec3 +// secondaryDirection dprec.Vec3 +// } -// PrimaryDirection returns the direction of the primary body. -func (s *CopyDirection) PrimaryDirection() dprec.Vec3 { - return s.primaryDirection -} +// // PrimaryDirection returns the direction of the primary body. +// func (s *CopyDirection) PrimaryDirection() dprec.Vec3 { +// return s.primaryDirection +// } -// SetPrimaryDirection changes the direction of the primary body. -func (s *CopyDirection) SetPrimaryDirection(direction dprec.Vec3) *CopyDirection { - s.primaryDirection = dprec.UnitVec3(direction) - return s -} +// // SetPrimaryDirection changes the direction of the primary body. +// func (s *CopyDirection) SetPrimaryDirection(direction dprec.Vec3) *CopyDirection { +// s.primaryDirection = dprec.UnitVec3(direction) +// return s +// } -// SecondaryDirection returns the direction of the secondary body. -func (s *CopyDirection) SecondaryDirection() dprec.Vec3 { - return s.secondaryDirection -} +// // SecondaryDirection returns the direction of the secondary body. +// func (s *CopyDirection) SecondaryDirection() dprec.Vec3 { +// return s.secondaryDirection +// } -// SetSecondaryDirection changes the direction of the secondary body. -func (s *CopyDirection) SetSecondaryDirection(direction dprec.Vec3) *CopyDirection { - s.secondaryDirection = dprec.UnitVec3(direction) - return s -} +// // SetSecondaryDirection changes the direction of the secondary body. +// func (s *CopyDirection) SetSecondaryDirection(direction dprec.Vec3) *CopyDirection { +// s.secondaryDirection = dprec.UnitVec3(direction) +// return s +// } -func (s *CopyDirection) Reset(ctx solver.PairContext) {} +// func (s *CopyDirection) Reset(ctx solver.PairContext) {} -func (s *CopyDirection) ApplyImpulses(ctx solver.PairContext) { - // The secondary body will have its direction aligned with the primary body's - // direction. As such, we need to ensure that the secondary's body angular - // velocity is only aligned with the primary body's direction (i.e. there is - // no rotation component that tries to move it away). +// func (s *CopyDirection) ApplyImpulses(ctx solver.PairContext) { +// // The secondary body will have its direction aligned with the primary body's +// // direction. As such, we need to ensure that the secondary's body angular +// // velocity is only aligned with the primary body's direction (i.e. there is +// // no rotation component that tries to move it away). - primaryDirWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.primaryDirection) - angularVelocityAmount := dprec.Vec3Dot(primaryDirWS, ctx.Target.AngularVelocity()) - ctx.Target.SetAngularVelocity(dprec.Vec3Prod(primaryDirWS, angularVelocityAmount)) -} +// primaryDirWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.primaryDirection) +// angularVelocityAmount := dprec.Vec3Dot(primaryDirWS, ctx.Target.AngularVelocity()) +// ctx.Target.SetAngularVelocity(dprec.Vec3Prod(primaryDirWS, angularVelocityAmount)) +// } -func (s *CopyDirection) ApplyNudges(ctx solver.PairContext) { - primaryDirWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.primaryDirection) - secondaryDirWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.secondaryDirection) +// func (s *CopyDirection) ApplyNudges(ctx solver.PairContext) { +// primaryDirWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.primaryDirection) +// secondaryDirWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.secondaryDirection) - rotationAxis := dprec.Vec3Cross(secondaryDirWS, primaryDirWS) - cos := dprec.Vec3Dot(secondaryDirWS, primaryDirWS) - sin := rotationAxis.Length() +// rotationAxis := dprec.Vec3Cross(secondaryDirWS, primaryDirWS) +// cos := dprec.Vec3Dot(secondaryDirWS, primaryDirWS) +// sin := rotationAxis.Length() - angle := dprec.Abs(dprec.Radians(math.Atan2(sin, cos))) - if angle > dprec.Angle(solver.Epsilon) { - rotation := dprec.RotationQuat(angle, dprec.UnitVec3(rotationAxis)) - ctx.Target.SetRotation(dprec.UnitQuat(dprec.QuatProd( - rotation, - ctx.Target.Rotation(), - ))) - } -} +// angle := dprec.Abs(dprec.Radians(math.Atan2(sin, cos))) +// if angle > dprec.Angle(solver.Epsilon) { +// rotation := dprec.RotationQuat(angle, dprec.UnitVec3(rotationAxis)) +// ctx.Target.SetRotation(dprec.UnitQuat(dprec.QuatProd( +// rotation, +// ctx.Target.Rotation(), +// ))) +// } +// } diff --git a/game/physics/constraint/copy_position.go b/game/physics/constraint/copy_position.go index 84ac70bc..b5419482 100644 --- a/game/physics/constraint/copy_position.go +++ b/game/physics/constraint/copy_position.go @@ -1,26 +1,26 @@ package constraint -import "github.com/mokiat/lacking/game/physics/solver" +// import "github.com/mokiat/lacking/game/physics/solver" -// NewCopyPosition creates a new CopyPosition constraint solver. -func NewCopyPosition() *CopyPosition { - return &CopyPosition{} -} +// // NewCopyPosition creates a new CopyPosition constraint solver. +// func NewCopyPosition() *CopyPosition { +// return &CopyPosition{} +// } -var _ solver.PairConstraint = (*CopyPosition)(nil) +// var _ solver.PairConstraint = (*CopyPosition)(nil) -// CopyPosition ensures that the target object has the same position as -// the source one. -// -// This solver is immediate - it converges in a single step. -type CopyPosition struct{} +// // CopyPosition ensures that the target object has the same position as +// // the source one. +// // +// // This solver is immediate - it converges in a single step. +// type CopyPosition struct{} -func (s *CopyPosition) Reset(ctx solver.PairContext) {} +// func (s *CopyPosition) Reset(ctx solver.PairContext) {} -func (s *CopyPosition) ApplyImpulses(ctx solver.PairContext) { - ctx.Target.SetLinearVelocity(ctx.Source.LinearVelocity()) -} +// func (s *CopyPosition) ApplyImpulses(ctx solver.PairContext) { +// ctx.Target.SetLinearVelocity(ctx.Source.LinearVelocity()) +// } -func (s *CopyPosition) ApplyNudges(ctx solver.PairContext) { - ctx.Target.SetPosition(ctx.Source.Position()) -} +// func (s *CopyPosition) ApplyNudges(ctx solver.PairContext) { +// ctx.Target.SetPosition(ctx.Source.Position()) +// } diff --git a/game/physics/constraint/copy_rotation.go b/game/physics/constraint/copy_rotation.go index aa62e080..75cca887 100644 --- a/game/physics/constraint/copy_rotation.go +++ b/game/physics/constraint/copy_rotation.go @@ -1,26 +1,26 @@ package constraint -import "github.com/mokiat/lacking/game/physics/solver" +// import "github.com/mokiat/lacking/game/physics/solver" -// NewCopyRotation creates a new CopyRotation constraint solver. -func NewCopyRotation() *CopyRotation { - return &CopyRotation{} -} +// // NewCopyRotation creates a new CopyRotation constraint solver. +// func NewCopyRotation() *CopyRotation { +// return &CopyRotation{} +// } -var _ solver.PairConstraint = (*CopyRotation)(nil) +// var _ solver.PairConstraint = (*CopyRotation)(nil) -// CopyRotation ensures that the target body has exactly the same rotation -// as the source one. -// -// This solver is immediate - it converges in a single step. -type CopyRotation struct{} +// // CopyRotation ensures that the target body has exactly the same rotation +// // as the source one. +// // +// // This solver is immediate - it converges in a single step. +// type CopyRotation struct{} -func (s *CopyRotation) Reset(ctx solver.PairContext) {} +// func (s *CopyRotation) Reset(ctx solver.PairContext) {} -func (s *CopyRotation) ApplyImpulses(ctx solver.PairContext) { - ctx.Target.SetAngularVelocity(ctx.Source.AngularVelocity()) -} +// func (s *CopyRotation) ApplyImpulses(ctx solver.PairContext) { +// ctx.Target.SetAngularVelocity(ctx.Source.AngularVelocity()) +// } -func (s *CopyRotation) ApplyNudges(ctx solver.PairContext) { - ctx.Target.SetRotation(ctx.Source.Rotation()) -} +// func (s *CopyRotation) ApplyNudges(ctx solver.PairContext) { +// ctx.Target.SetRotation(ctx.Source.Rotation()) +// } diff --git a/game/physics/constraint/differential.go b/game/physics/constraint/differential.go index 4b57030c..7ced8b3b 100644 --- a/game/physics/constraint/differential.go +++ b/game/physics/constraint/differential.go @@ -1,57 +1,57 @@ package constraint -import ( - "github.com/mokiat/gomath/dprec" - "github.com/mokiat/lacking/game/physics/solver" -) - -// NewDifferential creates a new Differential constraint solver. -func NewDifferential() *Differential { - return &Differential{ - maxDelta: 20.0, - } -} - -var _ solver.PairConstraint = (*Differential)(nil) - -// Differential represents the solution for a constraint that keeps two -// objects from rotating too much relative to one another over the local X -// axis. -type Differential struct { - maxDelta float64 -} - -// MaxDelta returns the maximum difference in velocity that is allowed. -func (d *Differential) MaxDelta() float64 { - return d.maxDelta -} - -// SetMaxDelta changes the maximum difference in velocity that is allowed. -func (d *Differential) SetMaxDelta(maxDelta float64) *Differential { - d.maxDelta = maxDelta - return d -} - -func (d *Differential) Reset(ctx solver.PairContext) {} - -func (d *Differential) ApplyImpulses(ctx solver.PairContext) { - targetAxisX := ctx.Target.Rotation().OrientationX() - targetVelocity := dprec.Vec3Dot(targetAxisX, ctx.Target.AngularVelocity()) - sourceAxisX := ctx.Source.Rotation().OrientationX() - sourceVelocity := dprec.Vec3Dot(sourceAxisX, ctx.Source.AngularVelocity()) - - var targetCorrection dprec.Vec3 - var sourceCorrection dprec.Vec3 - if delta := targetVelocity - sourceVelocity; delta > d.maxDelta { - targetCorrection = dprec.Vec3Prod(targetAxisX, (d.maxDelta-delta)/2.0) - sourceCorrection = dprec.Vec3Prod(sourceAxisX, (delta-d.maxDelta)/2.0) - } - if delta := sourceVelocity - targetVelocity; delta > d.maxDelta { - sourceCorrection = dprec.Vec3Prod(sourceAxisX, (d.maxDelta-delta)/2.0) - targetCorrection = dprec.Vec3Prod(targetAxisX, (delta-d.maxDelta)/2.0) - } - ctx.Target.SetAngularVelocity(dprec.Vec3Sum(ctx.Target.AngularVelocity(), targetCorrection)) - ctx.Source.SetAngularVelocity(dprec.Vec3Sum(ctx.Source.AngularVelocity(), sourceCorrection)) -} - -func (d *Differential) ApplyNudges(ctx solver.PairContext) {} +// import ( +// "github.com/mokiat/gomath/dprec" +// "github.com/mokiat/lacking/game/physics/solver" +// ) + +// // NewDifferential creates a new Differential constraint solver. +// func NewDifferential() *Differential { +// return &Differential{ +// maxDelta: 20.0, +// } +// } + +// var _ solver.PairConstraint = (*Differential)(nil) + +// // Differential represents the solution for a constraint that keeps two +// // objects from rotating too much relative to one another over the local X +// // axis. +// type Differential struct { +// maxDelta float64 +// } + +// // MaxDelta returns the maximum difference in velocity that is allowed. +// func (d *Differential) MaxDelta() float64 { +// return d.maxDelta +// } + +// // SetMaxDelta changes the maximum difference in velocity that is allowed. +// func (d *Differential) SetMaxDelta(maxDelta float64) *Differential { +// d.maxDelta = maxDelta +// return d +// } + +// func (d *Differential) Reset(ctx solver.PairContext) {} + +// func (d *Differential) ApplyImpulses(ctx solver.PairContext) { +// targetAxisX := ctx.Target.Rotation().OrientationX() +// targetVelocity := dprec.Vec3Dot(targetAxisX, ctx.Target.AngularVelocity()) +// sourceAxisX := ctx.Source.Rotation().OrientationX() +// sourceVelocity := dprec.Vec3Dot(sourceAxisX, ctx.Source.AngularVelocity()) + +// var targetCorrection dprec.Vec3 +// var sourceCorrection dprec.Vec3 +// if delta := targetVelocity - sourceVelocity; delta > d.maxDelta { +// targetCorrection = dprec.Vec3Prod(targetAxisX, (d.maxDelta-delta)/2.0) +// sourceCorrection = dprec.Vec3Prod(sourceAxisX, (delta-d.maxDelta)/2.0) +// } +// if delta := sourceVelocity - targetVelocity; delta > d.maxDelta { +// sourceCorrection = dprec.Vec3Prod(sourceAxisX, (d.maxDelta-delta)/2.0) +// targetCorrection = dprec.Vec3Prod(targetAxisX, (delta-d.maxDelta)/2.0) +// } +// ctx.Target.SetAngularVelocity(dprec.Vec3Sum(ctx.Target.AngularVelocity(), targetCorrection)) +// ctx.Source.SetAngularVelocity(dprec.Vec3Sum(ctx.Source.AngularVelocity(), sourceCorrection)) +// } + +// func (d *Differential) ApplyNudges(ctx solver.PairContext) {} diff --git a/game/physics/constraint/hinged_rod.go b/game/physics/constraint/hinged_rod.go index 26abdd09..6a9f0ccc 100644 --- a/game/physics/constraint/hinged_rod.go +++ b/game/physics/constraint/hinged_rod.go @@ -1,113 +1,113 @@ package constraint -import ( - "github.com/mokiat/gomath/dprec" - "github.com/mokiat/lacking/game/physics/solver" -) +// import ( +// "github.com/mokiat/gomath/dprec" +// "github.com/mokiat/lacking/game/physics/solver" +// ) -// NewHingedRod creates a new HingedRod constraint solver. -func NewHingedRod() *HingedRod { - return &HingedRod{ - primaryRadius: dprec.ZeroVec3(), - secondaryRadius: dprec.ZeroVec3(), - length: 1.0, - } -} +// // NewHingedRod creates a new HingedRod constraint solver. +// func NewHingedRod() *HingedRod { +// return &HingedRod{ +// primaryRadius: dprec.ZeroVec3(), +// secondaryRadius: dprec.ZeroVec3(), +// length: 1.0, +// } +// } -var _ solver.PairConstraint = (*HingedRod)(nil) +// var _ solver.PairConstraint = (*HingedRod)(nil) -// HingedRod represents the solution for a constraint that keeps two bodies -// tied together with a hard link of specific length. -type HingedRod struct { - primaryRadius dprec.Vec3 - secondaryRadius dprec.Vec3 - length float64 +// // HingedRod represents the solution for a constraint that keeps two bodies +// // tied together with a hard link of specific length. +// type HingedRod struct { +// primaryRadius dprec.Vec3 +// secondaryRadius dprec.Vec3 +// length float64 - jacobian solver.PairJacobian - drift float64 -} +// jacobian solver.PairJacobian +// drift float64 +// } -// PrimaryRadius returns the radius vector of the contact point -// on the primary object. -// -// The vector is in the object's local space. -func (s *HingedRod) PrimaryRadius() dprec.Vec3 { - return s.primaryRadius -} +// // PrimaryRadius returns the radius vector of the contact point +// // on the primary object. +// // +// // The vector is in the object's local space. +// func (s *HingedRod) PrimaryRadius() dprec.Vec3 { +// return s.primaryRadius +// } -// SetPrimaryRadius changes the attachment point of the link -// on the primary body. -func (s *HingedRod) SetPrimaryRadius(radius dprec.Vec3) *HingedRod { - s.primaryRadius = radius - return s -} +// // SetPrimaryRadius changes the attachment point of the link +// // on the primary body. +// func (s *HingedRod) SetPrimaryRadius(radius dprec.Vec3) *HingedRod { +// s.primaryRadius = radius +// return s +// } -// SecondaryRadius returns the radius vector of the contact point -// on the secondary object. -// -// The vector is in the object's local space. -func (s *HingedRod) SecondaryRadius() dprec.Vec3 { - return s.secondaryRadius -} +// // SecondaryRadius returns the radius vector of the contact point +// // on the secondary object. +// // +// // The vector is in the object's local space. +// func (s *HingedRod) SecondaryRadius() dprec.Vec3 { +// return s.secondaryRadius +// } -// SetSecondaryRadius changes the radius vector of the contact point -// on the secondary object. -// -// The vector is in the object's local space. -func (s *HingedRod) SetSecondaryRadius(radius dprec.Vec3) *HingedRod { - s.secondaryRadius = radius - return s -} +// // SetSecondaryRadius changes the radius vector of the contact point +// // on the secondary object. +// // +// // The vector is in the object's local space. +// func (s *HingedRod) SetSecondaryRadius(radius dprec.Vec3) *HingedRod { +// s.secondaryRadius = radius +// return s +// } -// Length returns the link length. -func (s *HingedRod) Length() float64 { - return s.length -} +// // Length returns the link length. +// func (s *HingedRod) Length() float64 { +// return s.length +// } -// SetLength changes the link length. -func (s *HingedRod) SetLength(length float64) *HingedRod { - s.length = length - return s -} +// // SetLength changes the link length. +// func (s *HingedRod) SetLength(length float64) *HingedRod { +// s.length = length +// return s +// } -// Reset re-evaluates the constraint. -func (s *HingedRod) Reset(ctx solver.PairContext) { - primaryRadiusWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.primaryRadius) - primaryAnchorWS := dprec.Vec3Sum(ctx.Target.Position(), primaryRadiusWS) - secondaryRadiusWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.secondaryRadius) - secondaryAnchorWS := dprec.Vec3Sum(ctx.Source.Position(), secondaryRadiusWS) - deltaPosition := dprec.Vec3Diff(secondaryAnchorWS, primaryAnchorWS) - if lng := deltaPosition.Length(); lng > solver.Epsilon { - normal := dprec.Vec3Quot(deltaPosition, lng) - s.jacobian = solver.PairJacobian{ - Target: solver.Jacobian{ - LinearSlope: dprec.InverseVec3(normal), - AngularSlope: dprec.Vec3Cross(normal, primaryRadiusWS), - }, - Source: solver.Jacobian{ - LinearSlope: normal, - AngularSlope: dprec.Vec3Cross(secondaryRadiusWS, normal), - }, - } - s.drift = lng - s.length - } else { - s.jacobian = solver.PairJacobian{} - s.drift = 0.0 - } -} +// // Reset re-evaluates the constraint. +// func (s *HingedRod) Reset(ctx solver.PairContext) { +// primaryRadiusWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.primaryRadius) +// primaryAnchorWS := dprec.Vec3Sum(ctx.Target.Position(), primaryRadiusWS) +// secondaryRadiusWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.secondaryRadius) +// secondaryAnchorWS := dprec.Vec3Sum(ctx.Source.Position(), secondaryRadiusWS) +// deltaPosition := dprec.Vec3Diff(secondaryAnchorWS, primaryAnchorWS) +// if lng := deltaPosition.Length(); lng > solver.Epsilon { +// normal := dprec.Vec3Quot(deltaPosition, lng) +// s.jacobian = solver.PairJacobian{ +// Target: solver.Jacobian{ +// LinearSlope: dprec.InverseVec3(normal), +// AngularSlope: dprec.Vec3Cross(normal, primaryRadiusWS), +// }, +// Source: solver.Jacobian{ +// LinearSlope: normal, +// AngularSlope: dprec.Vec3Cross(secondaryRadiusWS, normal), +// }, +// } +// s.drift = lng - s.length +// } else { +// s.jacobian = solver.PairJacobian{} +// s.drift = 0.0 +// } +// } -// ApplyImpulses applies impulses in order to keep the velocity part of -// the constraint satisfied. -func (s *HingedRod) ApplyImpulses(ctx solver.PairContext) { - solution := ctx.JacobianImpulseSolution(s.jacobian, s.drift, 0.0) - ctx.Target.ApplyImpulse(solution.Target) - ctx.Source.ApplyImpulse(solution.Source) -} +// // ApplyImpulses applies impulses in order to keep the velocity part of +// // the constraint satisfied. +// func (s *HingedRod) ApplyImpulses(ctx solver.PairContext) { +// solution := ctx.JacobianImpulseSolution(s.jacobian, s.drift, 0.0) +// ctx.Target.ApplyImpulse(solution.Target) +// ctx.Source.ApplyImpulse(solution.Source) +// } -// ApplyNudges applies nudges in order to keep the positional part of the -// constraint satisfied. -func (s *HingedRod) ApplyNudges(ctx solver.PairContext) { - solution := ctx.JacobianNudgeSolution(s.jacobian, s.drift) - ctx.Target.ApplyNudge(solution.Target) - ctx.Source.ApplyNudge(solution.Source) -} +// // ApplyNudges applies nudges in order to keep the positional part of the +// // constraint satisfied. +// func (s *HingedRod) ApplyNudges(ctx solver.PairContext) { +// solution := ctx.JacobianNudgeSolution(s.jacobian, s.drift) +// ctx.Target.ApplyNudge(solution.Target) +// ctx.Source.ApplyNudge(solution.Source) +// } diff --git a/game/physics/constraint/limit_relative_angle.go b/game/physics/constraint/limit_relative_angle.go index 1e403a4d..c83a51d2 100644 --- a/game/physics/constraint/limit_relative_angle.go +++ b/game/physics/constraint/limit_relative_angle.go @@ -1,119 +1,119 @@ package constraint -import ( - "github.com/mokiat/gomath/dprec" - "github.com/mokiat/lacking/game/physics/solver" -) - -func NewLimitRelativeAngle() *LimitRelativeAngle { - return &LimitRelativeAngle{} -} - -var _ solver.PairConstraint = (*LimitRelativeAngle)(nil) - -type LimitRelativeAngle struct { - primaryDirection dprec.Vec3 - secondaryDirection dprec.Vec3 - axis dprec.Vec3 - minAngle dprec.Angle - maxAngle dprec.Angle - - jacobian solver.PairJacobian - drift float64 -} - -func (c *LimitRelativeAngle) PrimaryDirection() dprec.Vec3 { - return c.primaryDirection -} - -func (c *LimitRelativeAngle) SetPrimaryDirection(direction dprec.Vec3) *LimitRelativeAngle { - c.primaryDirection = dprec.UnitVec3(direction) - return c -} - -func (c *LimitRelativeAngle) SecondaryDirection() dprec.Vec3 { - return c.secondaryDirection -} - -func (c *LimitRelativeAngle) SetSecondaryDirection(direction dprec.Vec3) *LimitRelativeAngle { - c.secondaryDirection = dprec.UnitVec3(direction) - return c -} - -func (c *LimitRelativeAngle) Axis() dprec.Vec3 { - return c.axis -} - -func (c *LimitRelativeAngle) SetAxis(axis dprec.Vec3) *LimitRelativeAngle { - c.axis = dprec.UnitVec3(axis) - return c -} - -func (c *LimitRelativeAngle) MinAngle() dprec.Angle { - return c.minAngle -} - -func (c *LimitRelativeAngle) SetMinAngle(angle dprec.Angle) *LimitRelativeAngle { - c.minAngle = angle - return c -} - -func (c *LimitRelativeAngle) MaxAngle() dprec.Angle { - return c.maxAngle -} - -func (c *LimitRelativeAngle) SetMaxAngle(angle dprec.Angle) *LimitRelativeAngle { - c.maxAngle = angle - return c -} - -func (c *LimitRelativeAngle) Reset(ctx solver.PairContext) { - axisWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), c.axis) - primaryDirectionWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), c.primaryDirection) - secondaryDirectionWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), c.secondaryDirection) - - if dprec.Abs(dprec.Vec3Dot(axisWS, secondaryDirectionWS)) > 0.99 { - c.jacobian = solver.PairJacobian{} - c.drift = 0.0 - return // secondary direction is parallel to axis - } - angle := dprec.Vec3ProjectionAngle(primaryDirectionWS, secondaryDirectionWS, axisWS) - - switch { - case angle > c.maxAngle: - c.drift = (angle - c.maxAngle).Radians() - c.jacobian = solver.PairJacobian{ - Target: solver.Jacobian{ - AngularSlope: dprec.InverseVec3(axisWS), - }, - Source: solver.Jacobian{ - AngularSlope: axisWS, - }, - } - case angle < c.minAngle: - c.drift = (c.minAngle - angle).Radians() - c.jacobian = solver.PairJacobian{ - Target: solver.Jacobian{ - AngularSlope: axisWS, - }, - Source: solver.Jacobian{ - AngularSlope: dprec.InverseVec3(axisWS), - }, - } - default: - c.drift = 0.0 - c.jacobian = solver.PairJacobian{} - } -} - -func (c *LimitRelativeAngle) ApplyImpulses(ctx solver.PairContext) { - if lambda := ctx.JacobianImpulseLambda(c.jacobian, 0.0, 0.0); lambda >= 0.0 { - return // moving away - } - solution := ctx.JacobianImpulseSolution(c.jacobian, c.drift, 0.0) - ctx.Target.ApplyImpulse(solution.Target) - ctx.Source.ApplyImpulse(solution.Source) -} - -func (c *LimitRelativeAngle) ApplyNudges(ctx solver.PairContext) { -} +// import ( +// "github.com/mokiat/gomath/dprec" +// "github.com/mokiat/lacking/game/physics/solver" +// ) + +// func NewLimitRelativeAngle() *LimitRelativeAngle { +// return &LimitRelativeAngle{} +// } + +// var _ solver.PairConstraint = (*LimitRelativeAngle)(nil) + +// type LimitRelativeAngle struct { +// primaryDirection dprec.Vec3 +// secondaryDirection dprec.Vec3 +// axis dprec.Vec3 +// minAngle dprec.Angle +// maxAngle dprec.Angle + +// jacobian solver.PairJacobian +// drift float64 +// } + +// func (c *LimitRelativeAngle) PrimaryDirection() dprec.Vec3 { +// return c.primaryDirection +// } + +// func (c *LimitRelativeAngle) SetPrimaryDirection(direction dprec.Vec3) *LimitRelativeAngle { +// c.primaryDirection = dprec.UnitVec3(direction) +// return c +// } + +// func (c *LimitRelativeAngle) SecondaryDirection() dprec.Vec3 { +// return c.secondaryDirection +// } + +// func (c *LimitRelativeAngle) SetSecondaryDirection(direction dprec.Vec3) *LimitRelativeAngle { +// c.secondaryDirection = dprec.UnitVec3(direction) +// return c +// } + +// func (c *LimitRelativeAngle) Axis() dprec.Vec3 { +// return c.axis +// } + +// func (c *LimitRelativeAngle) SetAxis(axis dprec.Vec3) *LimitRelativeAngle { +// c.axis = dprec.UnitVec3(axis) +// return c +// } + +// func (c *LimitRelativeAngle) MinAngle() dprec.Angle { +// return c.minAngle +// } + +// func (c *LimitRelativeAngle) SetMinAngle(angle dprec.Angle) *LimitRelativeAngle { +// c.minAngle = angle +// return c +// } + +// func (c *LimitRelativeAngle) MaxAngle() dprec.Angle { +// return c.maxAngle +// } + +// func (c *LimitRelativeAngle) SetMaxAngle(angle dprec.Angle) *LimitRelativeAngle { +// c.maxAngle = angle +// return c +// } + +// func (c *LimitRelativeAngle) Reset(ctx solver.PairContext) { +// axisWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), c.axis) +// primaryDirectionWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), c.primaryDirection) +// secondaryDirectionWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), c.secondaryDirection) + +// if dprec.Abs(dprec.Vec3Dot(axisWS, secondaryDirectionWS)) > 0.99 { +// c.jacobian = solver.PairJacobian{} +// c.drift = 0.0 +// return // secondary direction is parallel to axis +// } +// angle := dprec.Vec3ProjectionAngle(primaryDirectionWS, secondaryDirectionWS, axisWS) + +// switch { +// case angle > c.maxAngle: +// c.drift = (angle - c.maxAngle).Radians() +// c.jacobian = solver.PairJacobian{ +// Target: solver.Jacobian{ +// AngularSlope: dprec.InverseVec3(axisWS), +// }, +// Source: solver.Jacobian{ +// AngularSlope: axisWS, +// }, +// } +// case angle < c.minAngle: +// c.drift = (c.minAngle - angle).Radians() +// c.jacobian = solver.PairJacobian{ +// Target: solver.Jacobian{ +// AngularSlope: axisWS, +// }, +// Source: solver.Jacobian{ +// AngularSlope: dprec.InverseVec3(axisWS), +// }, +// } +// default: +// c.drift = 0.0 +// c.jacobian = solver.PairJacobian{} +// } +// } + +// func (c *LimitRelativeAngle) ApplyImpulses(ctx solver.PairContext) { +// if lambda := ctx.JacobianImpulseLambda(c.jacobian, 0.0, 0.0); lambda >= 0.0 { +// return // moving away +// } +// solution := ctx.JacobianImpulseSolution(c.jacobian, c.drift, 0.0) +// ctx.Target.ApplyImpulse(solution.Target) +// ctx.Source.ApplyImpulse(solution.Source) +// } + +// func (c *LimitRelativeAngle) ApplyNudges(ctx solver.PairContext) { +// } diff --git a/game/physics/constraint/match_direction.go b/game/physics/constraint/match_direction.go index 9e751a63..bcd1a191 100644 --- a/game/physics/constraint/match_direction.go +++ b/game/physics/constraint/match_direction.go @@ -1,107 +1,107 @@ package constraint -import ( - "github.com/mokiat/gomath/dprec" - "github.com/mokiat/lacking/game/physics/solver" -) +// import ( +// "github.com/mokiat/gomath/dprec" +// "github.com/mokiat/lacking/game/physics/solver" +// ) -// NewMatchDirections creates a new MatchDirections constraint solver. -func NewMatchDirections() *MatchDirections { - return &MatchDirections{ - primaryDirection: dprec.BasisYVec3(), - secondaryDirection: dprec.BasisYVec3(), - } -} +// // NewMatchDirections creates a new MatchDirections constraint solver. +// func NewMatchDirections() *MatchDirections { +// return &MatchDirections{ +// primaryDirection: dprec.BasisYVec3(), +// secondaryDirection: dprec.BasisYVec3(), +// } +// } -var _ solver.PairConstraint = (*MatchDirections)(nil) +// var _ solver.PairConstraint = (*MatchDirections)(nil) -// MatchDirections represents the solution for a constraint -// that keeps the direction of two bodies pointing in the same -// direction. -type MatchDirections struct { - primaryDirection dprec.Vec3 - secondaryDirection dprec.Vec3 +// // MatchDirections represents the solution for a constraint +// // that keeps the direction of two bodies pointing in the same +// // direction. +// type MatchDirections struct { +// primaryDirection dprec.Vec3 +// secondaryDirection dprec.Vec3 - jacobian1 solver.PairJacobian - jacobian2 solver.PairJacobian - drift1 float64 - drift2 float64 -} +// jacobian1 solver.PairJacobian +// jacobian2 solver.PairJacobian +// drift1 float64 +// drift2 float64 +// } -// PrimaryDirection returns the direction of the primary body that will be -// used in the alignment. -func (s *MatchDirections) PrimaryDirection() dprec.Vec3 { - return s.primaryDirection -} +// // PrimaryDirection returns the direction of the primary body that will be +// // used in the alignment. +// func (s *MatchDirections) PrimaryDirection() dprec.Vec3 { +// return s.primaryDirection +// } -// SetPrimaryDirection changes the direction of the primary body to be used -// in the alignment. -func (s *MatchDirections) SetPrimaryDirection(direction dprec.Vec3) *MatchDirections { - s.primaryDirection = dprec.UnitVec3(direction) - return s -} +// // SetPrimaryDirection changes the direction of the primary body to be used +// // in the alignment. +// func (s *MatchDirections) SetPrimaryDirection(direction dprec.Vec3) *MatchDirections { +// s.primaryDirection = dprec.UnitVec3(direction) +// return s +// } -// SecondaryDirection returns the direction of the secondary body that will be -// used in the alignment. -func (s *MatchDirections) SecondaryDirection() dprec.Vec3 { - return s.secondaryDirection -} +// // SecondaryDirection returns the direction of the secondary body that will be +// // used in the alignment. +// func (s *MatchDirections) SecondaryDirection() dprec.Vec3 { +// return s.secondaryDirection +// } -// SetSecondaryDirection changes the direction of the secondary body to be -// used in the alignment. -func (s *MatchDirections) SetSecondaryDirection(direction dprec.Vec3) *MatchDirections { - s.secondaryDirection = dprec.UnitVec3(direction) - return s -} +// // SetSecondaryDirection changes the direction of the secondary body to be +// // used in the alignment. +// func (s *MatchDirections) SetSecondaryDirection(direction dprec.Vec3) *MatchDirections { +// s.secondaryDirection = dprec.UnitVec3(direction) +// return s +// } -func (s *MatchDirections) Reset(ctx solver.PairContext) { - primaryDirWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.primaryDirection) - secondaryDirWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.secondaryDirection) - secondaryNorm1 := dprec.NormalVec3(secondaryDirWS) - secondaryNorm2 := dprec.Vec3Cross(secondaryDirWS, secondaryNorm1) +// func (s *MatchDirections) Reset(ctx solver.PairContext) { +// primaryDirWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.primaryDirection) +// secondaryDirWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.secondaryDirection) +// secondaryNorm1 := dprec.NormalVec3(secondaryDirWS) +// secondaryNorm2 := dprec.Vec3Cross(secondaryDirWS, secondaryNorm1) - // FIXME: This jacobian converges better than the original one-tier - // but produces a wrong result if the second object flips all the way - // around. - s.jacobian1 = solver.PairJacobian{ - Target: solver.Jacobian{ - LinearSlope: dprec.ZeroVec3(), - AngularSlope: dprec.Vec3Cross(primaryDirWS, secondaryNorm1), - }, - Source: solver.Jacobian{ - LinearSlope: dprec.ZeroVec3(), - AngularSlope: dprec.Vec3Cross(secondaryNorm1, primaryDirWS), - }, - } - s.jacobian2 = solver.PairJacobian{ - Target: solver.Jacobian{ - LinearSlope: dprec.ZeroVec3(), - AngularSlope: dprec.Vec3Cross(primaryDirWS, secondaryNorm2), - }, - Source: solver.Jacobian{ - LinearSlope: dprec.ZeroVec3(), - AngularSlope: dprec.Vec3Cross(secondaryNorm2, primaryDirWS), - }, - } +// // FIXME: This jacobian converges better than the original one-tier +// // but produces a wrong result if the second object flips all the way +// // around. +// s.jacobian1 = solver.PairJacobian{ +// Target: solver.Jacobian{ +// LinearSlope: dprec.ZeroVec3(), +// AngularSlope: dprec.Vec3Cross(primaryDirWS, secondaryNorm1), +// }, +// Source: solver.Jacobian{ +// LinearSlope: dprec.ZeroVec3(), +// AngularSlope: dprec.Vec3Cross(secondaryNorm1, primaryDirWS), +// }, +// } +// s.jacobian2 = solver.PairJacobian{ +// Target: solver.Jacobian{ +// LinearSlope: dprec.ZeroVec3(), +// AngularSlope: dprec.Vec3Cross(primaryDirWS, secondaryNorm2), +// }, +// Source: solver.Jacobian{ +// LinearSlope: dprec.ZeroVec3(), +// AngularSlope: dprec.Vec3Cross(secondaryNorm2, primaryDirWS), +// }, +// } - s.drift1 = dprec.Vec3Dot(primaryDirWS, secondaryNorm1) - s.drift2 = dprec.Vec3Dot(primaryDirWS, secondaryNorm2) -} +// s.drift1 = dprec.Vec3Dot(primaryDirWS, secondaryNorm1) +// s.drift2 = dprec.Vec3Dot(primaryDirWS, secondaryNorm2) +// } -func (s *MatchDirections) ApplyImpulses(ctx solver.PairContext) { - solution := ctx.JacobianImpulseSolution(s.jacobian1, s.drift1, 0.0) - ctx.Target.ApplyImpulse(solution.Target) - ctx.Source.ApplyImpulse(solution.Source) - solution = ctx.JacobianImpulseSolution(s.jacobian2, s.drift2, 0.0) - ctx.Target.ApplyImpulse(solution.Target) - ctx.Source.ApplyImpulse(solution.Source) -} +// func (s *MatchDirections) ApplyImpulses(ctx solver.PairContext) { +// solution := ctx.JacobianImpulseSolution(s.jacobian1, s.drift1, 0.0) +// ctx.Target.ApplyImpulse(solution.Target) +// ctx.Source.ApplyImpulse(solution.Source) +// solution = ctx.JacobianImpulseSolution(s.jacobian2, s.drift2, 0.0) +// ctx.Target.ApplyImpulse(solution.Target) +// ctx.Source.ApplyImpulse(solution.Source) +// } -func (s *MatchDirections) ApplyNudges(ctx solver.PairContext) { - solution := ctx.JacobianNudgeSolution(s.jacobian1, s.drift1) - ctx.Target.ApplyNudge(solution.Target) - ctx.Source.ApplyNudge(solution.Source) - solution = ctx.JacobianNudgeSolution(s.jacobian2, s.drift2) - ctx.Target.ApplyNudge(solution.Target) - ctx.Source.ApplyNudge(solution.Source) -} +// func (s *MatchDirections) ApplyNudges(ctx solver.PairContext) { +// solution := ctx.JacobianNudgeSolution(s.jacobian1, s.drift1) +// ctx.Target.ApplyNudge(solution.Target) +// ctx.Source.ApplyNudge(solution.Source) +// solution = ctx.JacobianNudgeSolution(s.jacobian2, s.drift2) +// ctx.Target.ApplyNudge(solution.Target) +// ctx.Source.ApplyNudge(solution.Source) +// } diff --git a/game/physics/constraint/match_direction_offset.go b/game/physics/constraint/match_direction_offset.go index 2460fd39..8d100a8b 100644 --- a/game/physics/constraint/match_direction_offset.go +++ b/game/physics/constraint/match_direction_offset.go @@ -1,120 +1,120 @@ package constraint -import ( - "github.com/mokiat/gomath/dprec" - "github.com/mokiat/lacking/game/physics/solver" -) - -// NewMatchDirectionOffset creates a new MatchDirectionOffset constraint solver. -func NewMatchDirectionOffset() *MatchDirectionOffset { - return &MatchDirectionOffset{ - primaryRadius: dprec.ZeroVec3(), - secondaryRadius: dprec.ZeroVec3(), - direction: dprec.BasisYVec3(), - offset: 0.0, - } -} - -var _ solver.PairConstraint = (*MatchDirectionOffset)(nil) - -// MatchDirectionOffset represents the solution for a constraint which ensures that -// the second body is at an exact distance away from the first body along -// some direction of the first body. -type MatchDirectionOffset struct { - primaryRadius dprec.Vec3 - secondaryRadius dprec.Vec3 - direction dprec.Vec3 - offset float64 - - jacobian solver.PairJacobian - drift float64 -} - -// PrimaryRadius returns the radius vector of the contact point -// on the primary object. -// -// The vector is in the object's local space. -func (s *MatchDirectionOffset) PrimaryRadius() dprec.Vec3 { - return s.primaryRadius -} - -// SetPrimaryRadius changes the attachment point of the link -// on the primary body. -func (s *MatchDirectionOffset) SetPrimaryRadius(radius dprec.Vec3) *MatchDirectionOffset { - s.primaryRadius = radius - return s -} - -// SecondaryRadius returns the radius vector of the contact point -// on the secondary object. -// -// The vector is in the object's local space. -func (s *MatchDirectionOffset) SecondaryRadius() dprec.Vec3 { - return s.secondaryRadius -} - -// SetSecondaryRadius changes the radius vector of the contact point -// on the secondary object. -// -// The vector is in the object's local space. -func (s *MatchDirectionOffset) SetSecondaryRadius(radius dprec.Vec3) *MatchDirectionOffset { - s.secondaryRadius = radius - return s -} - -// Direction returns the constraint direction, which is in local space of -// the first body. -func (s *MatchDirectionOffset) Direction() dprec.Vec3 { - return s.direction -} - -// SetDirection changes the constraint direction, which must be in local space -// of the first body. -func (s *MatchDirectionOffset) SetDirection(direction dprec.Vec3) *MatchDirectionOffset { - s.direction = dprec.UnitVec3(direction) - return s -} - -// Offset returns the directional offset. -func (s *MatchDirectionOffset) Offset() float64 { - return s.offset -} - -// SetOffset changes the directional offset. -func (s *MatchDirectionOffset) SetOffset(offset float64) *MatchDirectionOffset { - s.offset = offset - return s -} - -func (s *MatchDirectionOffset) Reset(ctx solver.PairContext) { - dirWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.direction) - primaryRadiusWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.primaryRadius) - secondaryRadiusWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.secondaryRadius) - s.jacobian = solver.PairJacobian{ - Target: solver.Jacobian{ - LinearSlope: dprec.InverseVec3(dirWS), - AngularSlope: dprec.Vec3Cross(dirWS, primaryRadiusWS), - }, - Source: solver.Jacobian{ - LinearSlope: dirWS, - AngularSlope: dprec.Vec3Cross(secondaryRadiusWS, dirWS), - }, - } - deltaPosition := dprec.Vec3Diff( - dprec.Vec3Sum(ctx.Source.Position(), secondaryRadiusWS), - dprec.Vec3Sum(ctx.Target.Position(), primaryRadiusWS), - ) - s.drift = dprec.Vec3Dot(dirWS, deltaPosition) -} - -func (s *MatchDirectionOffset) ApplyImpulses(ctx solver.PairContext) { - solution := ctx.JacobianImpulseSolution(s.jacobian, s.drift, 0.0) - ctx.Target.ApplyImpulse(solution.Target) - ctx.Source.ApplyImpulse(solution.Source) -} - -func (s *MatchDirectionOffset) ApplyNudges(ctx solver.PairContext) { - solution := ctx.JacobianNudgeSolution(s.jacobian, s.drift) - ctx.Target.ApplyNudge(solution.Target) - ctx.Source.ApplyNudge(solution.Source) -} +// import ( +// "github.com/mokiat/gomath/dprec" +// "github.com/mokiat/lacking/game/physics/solver" +// ) + +// // NewMatchDirectionOffset creates a new MatchDirectionOffset constraint solver. +// func NewMatchDirectionOffset() *MatchDirectionOffset { +// return &MatchDirectionOffset{ +// primaryRadius: dprec.ZeroVec3(), +// secondaryRadius: dprec.ZeroVec3(), +// direction: dprec.BasisYVec3(), +// offset: 0.0, +// } +// } + +// var _ solver.PairConstraint = (*MatchDirectionOffset)(nil) + +// // MatchDirectionOffset represents the solution for a constraint which ensures that +// // the second body is at an exact distance away from the first body along +// // some direction of the first body. +// type MatchDirectionOffset struct { +// primaryRadius dprec.Vec3 +// secondaryRadius dprec.Vec3 +// direction dprec.Vec3 +// offset float64 + +// jacobian solver.PairJacobian +// drift float64 +// } + +// // PrimaryRadius returns the radius vector of the contact point +// // on the primary object. +// // +// // The vector is in the object's local space. +// func (s *MatchDirectionOffset) PrimaryRadius() dprec.Vec3 { +// return s.primaryRadius +// } + +// // SetPrimaryRadius changes the attachment point of the link +// // on the primary body. +// func (s *MatchDirectionOffset) SetPrimaryRadius(radius dprec.Vec3) *MatchDirectionOffset { +// s.primaryRadius = radius +// return s +// } + +// // SecondaryRadius returns the radius vector of the contact point +// // on the secondary object. +// // +// // The vector is in the object's local space. +// func (s *MatchDirectionOffset) SecondaryRadius() dprec.Vec3 { +// return s.secondaryRadius +// } + +// // SetSecondaryRadius changes the radius vector of the contact point +// // on the secondary object. +// // +// // The vector is in the object's local space. +// func (s *MatchDirectionOffset) SetSecondaryRadius(radius dprec.Vec3) *MatchDirectionOffset { +// s.secondaryRadius = radius +// return s +// } + +// // Direction returns the constraint direction, which is in local space of +// // the first body. +// func (s *MatchDirectionOffset) Direction() dprec.Vec3 { +// return s.direction +// } + +// // SetDirection changes the constraint direction, which must be in local space +// // of the first body. +// func (s *MatchDirectionOffset) SetDirection(direction dprec.Vec3) *MatchDirectionOffset { +// s.direction = dprec.UnitVec3(direction) +// return s +// } + +// // Offset returns the directional offset. +// func (s *MatchDirectionOffset) Offset() float64 { +// return s.offset +// } + +// // SetOffset changes the directional offset. +// func (s *MatchDirectionOffset) SetOffset(offset float64) *MatchDirectionOffset { +// s.offset = offset +// return s +// } + +// func (s *MatchDirectionOffset) Reset(ctx solver.PairContext) { +// dirWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.direction) +// primaryRadiusWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.primaryRadius) +// secondaryRadiusWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.secondaryRadius) +// s.jacobian = solver.PairJacobian{ +// Target: solver.Jacobian{ +// LinearSlope: dprec.InverseVec3(dirWS), +// AngularSlope: dprec.Vec3Cross(dirWS, primaryRadiusWS), +// }, +// Source: solver.Jacobian{ +// LinearSlope: dirWS, +// AngularSlope: dprec.Vec3Cross(secondaryRadiusWS, dirWS), +// }, +// } +// deltaPosition := dprec.Vec3Diff( +// dprec.Vec3Sum(ctx.Source.Position(), secondaryRadiusWS), +// dprec.Vec3Sum(ctx.Target.Position(), primaryRadiusWS), +// ) +// s.drift = dprec.Vec3Dot(dirWS, deltaPosition) +// } + +// func (s *MatchDirectionOffset) ApplyImpulses(ctx solver.PairContext) { +// solution := ctx.JacobianImpulseSolution(s.jacobian, s.drift, 0.0) +// ctx.Target.ApplyImpulse(solution.Target) +// ctx.Source.ApplyImpulse(solution.Source) +// } + +// func (s *MatchDirectionOffset) ApplyNudges(ctx solver.PairContext) { +// solution := ctx.JacobianNudgeSolution(s.jacobian, s.drift) +// ctx.Target.ApplyNudge(solution.Target) +// ctx.Source.ApplyNudge(solution.Source) +// } diff --git a/game/physics/constraint/match_rotation.go b/game/physics/constraint/match_rotation.go index 8abba7b9..d60accd0 100644 --- a/game/physics/constraint/match_rotation.go +++ b/game/physics/constraint/match_rotation.go @@ -1,20 +1,20 @@ package constraint -import ( - "github.com/mokiat/gomath/dprec" - "github.com/mokiat/lacking/game/physics/solver" -) +// import ( +// "github.com/mokiat/gomath/dprec" +// "github.com/mokiat/lacking/game/physics/solver" +// ) -// NewMatchRotation creates a new constraint solver that keeps -// two bodies oriented in the same direction on all axis. -func NewMatchRotation() solver.PairConstraint { - // TODO: Do a three-jacobian solution here - return NewPairCombined( - NewMatchDirections(). - SetPrimaryDirection(dprec.BasisXVec3()). - SetSecondaryDirection(dprec.BasisXVec3()), - NewMatchDirections(). - SetPrimaryDirection(dprec.BasisZVec3()). - SetSecondaryDirection(dprec.BasisZVec3()), - ) -} +// // NewMatchRotation creates a new constraint solver that keeps +// // two bodies oriented in the same direction on all axis. +// func NewMatchRotation() solver.PairConstraint { +// // TODO: Do a three-jacobian solution here +// return NewPairCombined( +// NewMatchDirections(). +// SetPrimaryDirection(dprec.BasisXVec3()). +// SetSecondaryDirection(dprec.BasisXVec3()), +// NewMatchDirections(). +// SetPrimaryDirection(dprec.BasisZVec3()). +// SetSecondaryDirection(dprec.BasisZVec3()), +// ) +// } diff --git a/game/physics/constraint/pair_attachment.go b/game/physics/constraint/pair_attachment.go index abede2c7..1934b5ac 100644 --- a/game/physics/constraint/pair_attachment.go +++ b/game/physics/constraint/pair_attachment.go @@ -1,62 +1,62 @@ package constraint -import ( - "github.com/mokiat/gomath/dprec" - "github.com/mokiat/lacking/game/physics/solver" -) - -// NewPairAttachment creates a new PairAttachment constraint solver, which -// can be used to attach two bodies together at given offsets. Each body -// is still free to rotate independently. -func NewPairAttachment() *PairAttachment { - solverX := NewMatchDirectionOffset().SetDirection(dprec.BasisXVec3()).SetOffset(0.0) - solverY := NewMatchDirectionOffset().SetDirection(dprec.BasisYVec3()).SetOffset(0.0) - solverZ := NewMatchDirectionOffset().SetDirection(dprec.BasisZVec3()).SetOffset(0.0) - return &PairAttachment{ - solverX: *solverX, - solverY: *solverY, - solverZ: *solverZ, - } -} - -var _ solver.PairConstraint = (*PairAttachment)(nil) - -// TODO: Implement the following constraint independently. - -type PairAttachment struct { - solverX MatchDirectionOffset - solverY MatchDirectionOffset - solverZ MatchDirectionOffset -} - -func (s *PairAttachment) SetPrimaryOffset(offset dprec.Vec3) *PairAttachment { - s.solverX.SetPrimaryRadius(offset) - s.solverY.SetPrimaryRadius(offset) - s.solverZ.SetPrimaryRadius(offset) - return s -} - -func (s *PairAttachment) SetSecondaryOffset(offset dprec.Vec3) *PairAttachment { - s.solverX.SetSecondaryRadius(offset) - s.solverY.SetSecondaryRadius(offset) - s.solverZ.SetSecondaryRadius(offset) - return s -} - -func (s *PairAttachment) Reset(ctx solver.PairContext) { - s.solverX.Reset(ctx) - s.solverY.Reset(ctx) - s.solverZ.Reset(ctx) -} - -func (s *PairAttachment) ApplyImpulses(ctx solver.PairContext) { - s.solverX.ApplyImpulses(ctx) - s.solverY.ApplyImpulses(ctx) - s.solverZ.ApplyImpulses(ctx) -} - -func (s *PairAttachment) ApplyNudges(ctx solver.PairContext) { - s.solverX.ApplyNudges(ctx) - s.solverY.ApplyNudges(ctx) - s.solverZ.ApplyNudges(ctx) -} +// import ( +// "github.com/mokiat/gomath/dprec" +// "github.com/mokiat/lacking/game/physics/solver" +// ) + +// // NewPairAttachment creates a new PairAttachment constraint solver, which +// // can be used to attach two bodies together at given offsets. Each body +// // is still free to rotate independently. +// func NewPairAttachment() *PairAttachment { +// solverX := NewMatchDirectionOffset().SetDirection(dprec.BasisXVec3()).SetOffset(0.0) +// solverY := NewMatchDirectionOffset().SetDirection(dprec.BasisYVec3()).SetOffset(0.0) +// solverZ := NewMatchDirectionOffset().SetDirection(dprec.BasisZVec3()).SetOffset(0.0) +// return &PairAttachment{ +// solverX: *solverX, +// solverY: *solverY, +// solverZ: *solverZ, +// } +// } + +// var _ solver.PairConstraint = (*PairAttachment)(nil) + +// // TODO: Implement the following constraint independently. + +// type PairAttachment struct { +// solverX MatchDirectionOffset +// solverY MatchDirectionOffset +// solverZ MatchDirectionOffset +// } + +// func (s *PairAttachment) SetPrimaryOffset(offset dprec.Vec3) *PairAttachment { +// s.solverX.SetPrimaryRadius(offset) +// s.solverY.SetPrimaryRadius(offset) +// s.solverZ.SetPrimaryRadius(offset) +// return s +// } + +// func (s *PairAttachment) SetSecondaryOffset(offset dprec.Vec3) *PairAttachment { +// s.solverX.SetSecondaryRadius(offset) +// s.solverY.SetSecondaryRadius(offset) +// s.solverZ.SetSecondaryRadius(offset) +// return s +// } + +// func (s *PairAttachment) Reset(ctx solver.PairContext) { +// s.solverX.Reset(ctx) +// s.solverY.Reset(ctx) +// s.solverZ.Reset(ctx) +// } + +// func (s *PairAttachment) ApplyImpulses(ctx solver.PairContext) { +// s.solverX.ApplyImpulses(ctx) +// s.solverY.ApplyImpulses(ctx) +// s.solverZ.ApplyImpulses(ctx) +// } + +// func (s *PairAttachment) ApplyNudges(ctx solver.PairContext) { +// s.solverX.ApplyNudges(ctx) +// s.solverY.ApplyNudges(ctx) +// s.solverZ.ApplyNudges(ctx) +// } diff --git a/game/physics/constraint/static_position.go b/game/physics/constraint/static_position.go index b77d39e1..206b4719 100644 --- a/game/physics/constraint/static_position.go +++ b/game/physics/constraint/static_position.go @@ -1,44 +1,44 @@ package constraint -import ( - "github.com/mokiat/gomath/dprec" - "github.com/mokiat/lacking/game/physics/solver" -) - -// NewStaticPosition creates a new StaticPosition constraint solver. -func NewStaticPosition() *StaticPosition { - return &StaticPosition{ - position: dprec.ZeroVec3(), - } -} - -var _ solver.Constraint = (*StaticPosition)(nil) - -// StaticPosition represents the solution for a constraint -// that keeps a body positioned at the specified fixture location. -// -// This solver is immediate - it converges in a single step. -type StaticPosition struct { - position dprec.Vec3 -} - -// Position returns the location to which the body will be constrained. -func (t *StaticPosition) Position() dprec.Vec3 { - return t.position -} - -// SetPosition changes the location to which the body will be constrained. -func (t *StaticPosition) SetPosition(position dprec.Vec3) *StaticPosition { - t.position = position - return t -} - -func (s *StaticPosition) Reset(ctx solver.Context) {} - -func (s *StaticPosition) ApplyImpulses(ctx solver.Context) { - ctx.Target.SetLinearVelocity(dprec.ZeroVec3()) -} - -func (s *StaticPosition) ApplyNudges(ctx solver.Context) { - ctx.Target.SetPosition(s.position) -} +// import ( +// "github.com/mokiat/gomath/dprec" +// "github.com/mokiat/lacking/game/physics/solver" +// ) + +// // NewStaticPosition creates a new StaticPosition constraint solver. +// func NewStaticPosition() *StaticPosition { +// return &StaticPosition{ +// position: dprec.ZeroVec3(), +// } +// } + +// var _ solver.Constraint = (*StaticPosition)(nil) + +// // StaticPosition represents the solution for a constraint +// // that keeps a body positioned at the specified fixture location. +// // +// // This solver is immediate - it converges in a single step. +// type StaticPosition struct { +// position dprec.Vec3 +// } + +// // Position returns the location to which the body will be constrained. +// func (t *StaticPosition) Position() dprec.Vec3 { +// return t.position +// } + +// // SetPosition changes the location to which the body will be constrained. +// func (t *StaticPosition) SetPosition(position dprec.Vec3) *StaticPosition { +// t.position = position +// return t +// } + +// func (s *StaticPosition) Reset(ctx solver.Context) {} + +// func (s *StaticPosition) ApplyImpulses(ctx solver.Context) { +// ctx.Target.SetLinearVelocity(dprec.ZeroVec3()) +// } + +// func (s *StaticPosition) ApplyNudges(ctx solver.Context) { +// ctx.Target.SetPosition(s.position) +// } diff --git a/game/physics/constraint/static_rotation.go b/game/physics/constraint/static_rotation.go index 866ad20e..4d10ecf0 100644 --- a/game/physics/constraint/static_rotation.go +++ b/game/physics/constraint/static_rotation.go @@ -1,44 +1,44 @@ package constraint -import ( - "github.com/mokiat/gomath/dprec" - "github.com/mokiat/lacking/game/physics/solver" -) - -// NewStaticRotation creates a new StaticRotation constraint solver. -func NewStaticRotation() *StaticRotation { - return &StaticRotation{ - rotation: dprec.IdentityQuat(), - } -} - -var _ solver.Constraint = (*StaticRotation)(nil) - -// StaticRotation represents the solution for a constraint -// that keeps a body positioned at the specified fixture location. -// -// This solver is immediate - it converges in a single step. -type StaticRotation struct { - rotation dprec.Quat -} - -// Rotation returns the orientation to which the body will be constrained. -func (t *StaticRotation) Rotation() dprec.Quat { - return t.rotation -} - -// SetRotation changes the orientation to which the body will be constrained. -func (t *StaticRotation) SetRotation(rotation dprec.Quat) *StaticRotation { - t.rotation = rotation - return t -} - -func (s *StaticRotation) Reset(ctx solver.Context) {} - -func (s *StaticRotation) ApplyImpulses(ctx solver.Context) { - ctx.Target.SetAngularVelocity(dprec.ZeroVec3()) -} - -func (s *StaticRotation) ApplyNudges(ctx solver.Context) { - ctx.Target.SetRotation(s.rotation) -} +// import ( +// "github.com/mokiat/gomath/dprec" +// "github.com/mokiat/lacking/game/physics/solver" +// ) + +// // NewStaticRotation creates a new StaticRotation constraint solver. +// func NewStaticRotation() *StaticRotation { +// return &StaticRotation{ +// rotation: dprec.IdentityQuat(), +// } +// } + +// var _ solver.Constraint = (*StaticRotation)(nil) + +// // StaticRotation represents the solution for a constraint +// // that keeps a body positioned at the specified fixture location. +// // +// // This solver is immediate - it converges in a single step. +// type StaticRotation struct { +// rotation dprec.Quat +// } + +// // Rotation returns the orientation to which the body will be constrained. +// func (t *StaticRotation) Rotation() dprec.Quat { +// return t.rotation +// } + +// // SetRotation changes the orientation to which the body will be constrained. +// func (t *StaticRotation) SetRotation(rotation dprec.Quat) *StaticRotation { +// t.rotation = rotation +// return t +// } + +// func (s *StaticRotation) Reset(ctx solver.Context) {} + +// func (s *StaticRotation) ApplyImpulses(ctx solver.Context) { +// ctx.Target.SetAngularVelocity(dprec.ZeroVec3()) +// } + +// func (s *StaticRotation) ApplyNudges(ctx solver.Context) { +// ctx.Target.SetRotation(s.rotation) +// } diff --git a/game/physics/scene.go b/game/physics/scene.go index bc9ba7e2..0053c8a7 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -11,7 +11,6 @@ import ( "github.com/mokiat/lacking/core/spatial/placement3d" "github.com/mokiat/lacking/core/spatial/shape3d" "github.com/mokiat/lacking/debug/metric" - "github.com/mokiat/lacking/game/physics/constraint" "github.com/mokiat/lacking/util/observer" ) @@ -19,25 +18,24 @@ import ( // a number of bodies that are independent on any // bodies managed by other scene objects. type Scene struct { - sbCollisionConstraints []SBConstraint - sbCollisionSolvers []constraint.Collision + collisionScene *placement3d.Scene[bodyData, shapeData, terrainData] - dbCollisionConstraints []DBConstraint - dbCollisionSolvers []constraint.PairCollision + soloCollisionSubscriptions *observer.SubscriptionSet[SoloCollisionCallback] + pairCollisionSubscriptions *observer.SubscriptionSet[PairCollisionCallback] - collisionSet placement3d.ContactList + soloCollisionConstraintIDs []SoloConstraintID + pairCollisionConstraintIDs []PairConstraintID - oldSBCollisions map[sbCollisionPair]struct{} - newSBCollisions map[sbCollisionPair]struct{} + soloCollisionSolvers []SoloCollisionSolver + pairCollisionSolvers []PairCollisionSolver - oldDBCollisions map[dbCollisionPair]struct{} - newDBCollisions map[dbCollisionPair]struct{} + oldSoloCollisionRefs map[soloCollisionRef]struct{} + oldPairCollisionRefs map[pairCollisionRef]struct{} - // ---------- NEW BELOW---------- (TODO: REMOVE COMMENT) - collisionScene *placement3d.Scene[bodyData, shapeData, terrainData] + newSoloCollisionRefs map[soloCollisionRef]struct{} + newPairCollisionRefs map[pairCollisionRef]struct{} - soloCollisionSubscriptions *observer.SubscriptionSet[SoloCollisionCallback] - pairCollisionSubscriptions *observer.SubscriptionSet[PairCollisionCallback] + collisionContacts placement3d.ContactList freeCollisionRejectGroup uint32 @@ -72,15 +70,6 @@ type Scene struct { func NewScene() *Scene { return &Scene{ - collisionSet: make(placement3d.ContactList, 0, 128), - - oldSBCollisions: make(map[sbCollisionPair]struct{}, 32), - newSBCollisions: make(map[sbCollisionPair]struct{}, 32), - - oldDBCollisions: make(map[dbCollisionPair]struct{}, 32), - newDBCollisions: make(map[dbCollisionPair]struct{}, 32), - - // ---------- NEW BELOW---------- (TODO: REMOVE COMMENT) collisionScene: placement3d.NewScene[bodyData, shapeData, terrainData](placement3d.SceneSettings{ Size: opt.V(16384.0), MaxDepth: opt.V[uint32](12), @@ -91,6 +80,20 @@ func NewScene() *Scene { soloCollisionSubscriptions: observer.NewSubscriptionSet[SoloCollisionCallback](), pairCollisionSubscriptions: observer.NewSubscriptionSet[PairCollisionCallback](), + soloCollisionConstraintIDs: make([]SoloConstraintID, 0), + pairCollisionConstraintIDs: make([]PairConstraintID, 0), + + soloCollisionSolvers: make([]SoloCollisionSolver, 0), + pairCollisionSolvers: make([]PairCollisionSolver, 0), + + oldSoloCollisionRefs: make(map[soloCollisionRef]struct{}), + oldPairCollisionRefs: make(map[pairCollisionRef]struct{}), + + newSoloCollisionRefs: make(map[soloCollisionRef]struct{}), + newPairCollisionRefs: make(map[pairCollisionRef]struct{}), + + collisionContacts: make(placement3d.ContactList, 0), + freeCollisionRejectGroup: 0, mediumSolver: NewStaticAirSolver(), @@ -384,8 +387,8 @@ func (s *Scene) SetTimeScale(scale float64) { func (s *Scene) Update(elapsedTime time.Duration) { elapsedSeconds := elapsedTime.Seconds() s.runSimulation(elapsedSeconds * s.timeScale) - s.notifySingleBodyCollisions() - s.notifyDoubleBodyCollisions() + s.notifySoloCollisions() + s.notifyPairCollisions() } func (s *Scene) allocateGlobalAccelerator() (int32, *globalAcceleratorState) { @@ -802,265 +805,184 @@ func (s *Scene) applyPlacement() { func (s *Scene) detectCollisions() { defer metric.BeginRegion("collision").End() - for _, constraint := range s.sbCollisionConstraints { - constraint.Delete() + // Purge old collision constraints and solvers. + soloConstraints := s.SoloConstraints() + for _, constraintID := range s.soloCollisionConstraintIDs { + soloConstraints.Delete(constraintID) } - s.sbCollisionConstraints = s.sbCollisionConstraints[:0] - s.sbCollisionSolvers = s.sbCollisionSolvers[:0] - for _, constraint := range s.dbCollisionConstraints { - constraint.Delete() + pairConstraints := s.PairConstraints() + for _, constraintID := range s.pairCollisionConstraintIDs { + pairConstraints.Delete(constraintID) } - s.dbCollisionConstraints = s.dbCollisionConstraints[:0] - s.dbCollisionSolvers = s.dbCollisionSolvers[:0] - - s.collisionSet.Reset() - s.collisionScene.CollectIntersections(s.collisionSet.AddContact) - for _, intersection := range s.collisionSet.Contacts() { - srcBodyObject := s.collisionScene.GetShapeObject(intersection.SourceShapeID) - srcBodyRef := s.collisionScene.GetObjectUserData(srcBodyObject) - - if intersection.TargetMeshID == placement3d.InvalidMeshID { - tgtBodyObject := s.collisionScene.GetShapeObject(intersection.TargetShapeID) - tgtBodyRef := s.collisionScene.GetObjectUserData(tgtBodyObject) - s.detectBodyBodyCollision(srcBodyRef.index, tgtBodyRef.index, intersection) + + s.soloCollisionConstraintIDs = s.soloCollisionConstraintIDs[:0] + s.pairCollisionConstraintIDs = s.pairCollisionConstraintIDs[:0] + + s.soloCollisionSolvers = s.soloCollisionSolvers[:0] + s.pairCollisionSolvers = s.pairCollisionSolvers[:0] + + // Collect new contacts. + s.collisionContacts.Reset() + s.collisionScene.CollectIntersections(s.collisionContacts.AddContact) + + // Handle contacts. + for _, contact := range s.collisionContacts.Contacts() { + srcBodyObject := s.collisionScene.GetShapeObject(contact.SourceShapeID) + srcShapeData := s.collisionScene.GetShapeUserData(contact.SourceShapeID) + srcBodyData := s.collisionScene.GetObjectUserData(srcBodyObject) + + if contact.TargetMeshID == placement3d.InvalidMeshID { + tgtBodyObject := s.collisionScene.GetShapeObject(contact.TargetShapeID) + tgtShapeData := s.collisionScene.GetShapeUserData(contact.TargetShapeID) + tgtBodyData := s.collisionScene.GetObjectUserData(tgtBodyObject) + s.handlePairCollision( + bodyCollisionData{ + srcBodyData.index, + srcShapeData.frictionCoefficient, + srcShapeData.restitutionCoefficient, + }, + bodyCollisionData{ + tgtBodyData.index, + tgtShapeData.frictionCoefficient, + tgtShapeData.restitutionCoefficient, + }, + contact, + ) } else { - tgtPropMesh := s.collisionScene.GetMeshUserData(intersection.TargetMeshID) - s.detectBodyPropCollision(srcBodyRef.index, tgtPropMesh.index, intersection) + tgtTerrainData := s.collisionScene.GetMeshUserData(contact.TargetMeshID) + s.handleSoloCollision( + bodyCollisionData{ + srcBodyData.index, + srcShapeData.frictionCoefficient, + srcShapeData.restitutionCoefficient, + }, + terrainCollisionData{ + tgtTerrainData.index, + tgtTerrainData.frictionCoefficient, + tgtTerrainData.restitutionCoefficient, + }, + contact, + ) } } } -func (s *Scene) detectBodyBodyCollision(primaryIndex, secondaryIndex int32, intersection placement3d.Contact) { - primary := &s.bodies[primaryIndex] - secondary := &s.bodies[secondaryIndex] - - solver := s.allocateDualCollisionSolver() - solver.Init(constraint.PairCollisionState{ - PrimaryNormal: intersection.TargetNormal, - PrimaryPoint: intersection.EvalSourcePoint(), - PrimaryFrictionCoefficient: primary.frictionCoefficient, - PrimaryRestitutionCoefficient: primary.restitutionCoefficient, - - SecondaryNormal: intersection.EvalSourceNormal(), - SecondaryPoint: intersection.TargetPoint, - SecondaryFrictionCoefficient: secondary.frictionCoefficient, - SecondaryRestitutionCoefficient: secondary.restitutionCoefficient, - - Depth: intersection.Depth, +func (s *Scene) handlePairCollision(primaryBodyData, secondaryBodyData bodyCollisionData, contact placement3d.Contact) { + // primary := &s.bodies[primaryIndex] + // secondary := &s.bodies[secondaryIndex] + + // solver := s.allocateDualCollisionSolver() + // solver.Init(constraint.PairCollisionState{ + // PrimaryNormal: intersection.TargetNormal, + // PrimaryPoint: intersection.EvalSourcePoint(), + // PrimaryFrictionCoefficient: primary.frictionCoefficient, + // PrimaryRestitutionCoefficient: primary.restitutionCoefficient, + + // SecondaryNormal: intersection.EvalSourceNormal(), + // SecondaryPoint: intersection.TargetPoint, + // SecondaryFrictionCoefficient: secondary.frictionCoefficient, + // SecondaryRestitutionCoefficient: secondary.restitutionCoefficient, + + // Depth: intersection.Depth, + // }) + + // pair := dbCollisionPair{ + // PrimaryRef: primary.reference, + // SecondaryRef: secondary.reference, + // } + // s.newDBCollisions[pair] = struct{}{} + + // primaryBody := Body{ + // scene: s, + // reference: primary.reference, + // } + // + // secondaryBody := Body{ + // scene: s, + // reference: secondary.reference, + // } + // + // s.dbCollisionConstraints = append(s.dbCollisionConstraints, s.CreateDoubleBodyConstraint(primaryBody, secondaryBody, solver)) +} + +func (s *Scene) handleSoloCollision(bodyData bodyCollisionData, terrainData terrainCollisionData, contact placement3d.Contact) { + solver := s.allocateSoloCollisionSolver() + solver.Init(SoloCollisionSolverConfig{ + TerrainFrictionCoefficient: terrainData.frictionCoefficient, + TerrainRestitutionCoefficient: terrainData.restitutionCoefficient, + TerrainContactNormal: contact.TargetNormal, + + BodyFrictionCoefficient: bodyData.frictionCoefficient, + BodyRestitutionCoefficient: bodyData.restitutionCoefficient, + BodyContactPoint: contact.EvalSourcePoint(), + + Depth: contact.Depth, }) - pair := dbCollisionPair{ - PrimaryRef: primary.reference, - SecondaryRef: secondary.reference, - } - s.newDBCollisions[pair] = struct{}{} + bodyID := s.Bodies().idFromIndex(bodyData.index) + terrainID := s.Terrains().idFromIndex(terrainData.index) - primaryBody := Body{ - scene: s, - reference: primary.reference, - } - secondaryBody := Body{ - scene: s, - reference: secondary.reference, - } - s.dbCollisionConstraints = append(s.dbCollisionConstraints, s.CreateDoubleBodyConstraint(primaryBody, secondaryBody, solver)) -} - -func (s *Scene) detectBodyPropCollision(bodyIndex, propIndex int32, intersection placement3d.Contact) { - primary := &s.bodies[bodyIndex] - secondary := &s.props[propIndex] - - solver := s.allocateGroundCollisionSolver() - solver.Init(constraint.CollisionState{ - BodyNormal: intersection.TargetNormal, - BodyPoint: intersection.EvalSourcePoint(), - BodyFrictionCoefficient: primary.frictionCoefficient, - BodyRestitutionCoefficient: primary.restitutionCoefficient, - - PropFrictionCoefficient: 1.0, // TODO: Take from prop or shape material - PropRestitutionCoefficient: 0.5, // TODO: Take from prop or shape material - - Depth: intersection.Depth, - }) - - pair := sbCollisionPair{ - BodyRef: primary.reference, - PropRef: secondary.reference, - } - s.newSBCollisions[pair] = struct{}{} + constraintID := s.SoloConstraints().Create(bodyID, solver) + s.soloCollisionConstraintIDs = append(s.soloCollisionConstraintIDs, constraintID) - primaryBody := Body{ - scene: s, - reference: primary.reference, + ref := soloCollisionRef{ + bodyID: bodyID, + terrainID: terrainID, } - s.sbCollisionConstraints = append(s.sbCollisionConstraints, s.CreateSingleBodyConstraint(primaryBody, solver)) + s.newSoloCollisionRefs[ref] = struct{}{} } -func (s *Scene) allocateGroundCollisionSolver() *constraint.Collision { - if len(s.sbCollisionSolvers) < cap(s.sbCollisionSolvers) { - s.sbCollisionSolvers = s.sbCollisionSolvers[:len(s.sbCollisionSolvers)+1] - } else { - s.sbCollisionSolvers = append(s.sbCollisionSolvers, constraint.Collision{}) - } - return &s.sbCollisionSolvers[len(s.sbCollisionSolvers)-1] +func (s *Scene) allocatePairCollisionSolver() *PairCollisionSolver { + index := len(s.pairCollisionSolvers) + s.pairCollisionSolvers = append(s.pairCollisionSolvers, PairCollisionSolver{}) + return &s.pairCollisionSolvers[index] } -func (s *Scene) allocateDualCollisionSolver() *constraint.PairCollision { - if len(s.dbCollisionSolvers) < cap(s.dbCollisionSolvers) { - s.dbCollisionSolvers = s.dbCollisionSolvers[:len(s.dbCollisionSolvers)+1] - } else { - s.dbCollisionSolvers = append(s.dbCollisionSolvers, constraint.PairCollision{}) - } - return &s.dbCollisionSolvers[len(s.dbCollisionSolvers)-1] +func (s *Scene) allocateSoloCollisionSolver() *SoloCollisionSolver { + index := len(s.soloCollisionSolvers) + s.soloCollisionSolvers = append(s.soloCollisionSolvers, SoloCollisionSolver{}) + return &s.soloCollisionSolvers[index] } -// func (s *Scene) checkCollisionBodyWithProp(primary *bodyState, prop *propState) { -// s.collisionSet.Reset() -// collision.CheckIntersectionSetWithSet(primary.collisionSet, prop.collisionSet, s.collisionSet) -// for _, intersection := range s.collisionSet.Intersections() { -// solver := s.allocateGroundCollisionSolver() -// solver.Init(constraint.CollisionState{ -// BodyNormal: intersection.FirstDisplaceNormal, -// BodyPoint: intersection.FirstContact, -// BodyFrictionCoefficient: primary.frictionCoefficient, -// BodyRestitutionCoefficient: primary.restitutionCoefficient, - -// PropFrictionCoefficient: 1.0, // TODO: Take from prop or shape material -// PropRestitutionCoefficient: 0.5, // TODO: Take from prop or shape material - -// Depth: intersection.Depth, -// }) - -// pair := sbCollisionPair{ -// BodyRef: primary.reference, -// PropRef: prop.reference, -// } -// s.newSBCollisions[pair] = struct{}{} - -// primaryBody := Body{ -// scene: s, -// reference: primary.reference, -// } -// s.sbCollisionConstraints = append(s.sbCollisionConstraints, s.CreateSingleBodyConstraint(primaryBody, solver)) -// } -// } - -// func (s *Scene) checkCollisionTwoBodies(primary, secondary *bodyState) { -// s.collisionSet.Reset() -// collision.CheckIntersectionSetWithSet(primary.collisionSet, secondary.collisionSet, s.collisionSet) -// for _, intersection := range s.collisionSet.Intersections() { -// solver := s.allocateDualCollisionSolver() -// solver.Init(constraint.PairCollisionState{ -// PrimaryNormal: intersection.FirstDisplaceNormal, -// PrimaryPoint: intersection.FirstContact, -// PrimaryFrictionCoefficient: primary.frictionCoefficient, -// PrimaryRestitutionCoefficient: primary.restitutionCoefficient, - -// SecondaryNormal: intersection.SecondDisplaceNormal, -// SecondaryPoint: intersection.SecondContact, -// SecondaryFrictionCoefficient: secondary.frictionCoefficient, -// SecondaryRestitutionCoefficient: secondary.restitutionCoefficient, - -// Depth: intersection.Depth, -// }) - -// pair := dbCollisionPair{ -// PrimaryRef: primary.reference, -// SecondaryRef: secondary.reference, -// } -// s.newDBCollisions[pair] = struct{}{} - -// primaryBody := Body{ -// scene: s, -// reference: primary.reference, -// } -// secondaryBody := Body{ -// scene: s, -// reference: secondary.reference, -// } -// s.dbCollisionConstraints = append(s.dbCollisionConstraints, s.CreateDoubleBodyConstraint(primaryBody, secondaryBody, solver)) -// } -// } - -func (s *Scene) notifySingleBodyCollisions() { - for newCollision := range s.newSBCollisions { - if _, ok := s.oldSBCollisions[newCollision]; !ok { - primary := Body{ - scene: s, - reference: newCollision.BodyRef, - } - prop := Prop{ - name: s.props[newCollision.PropRef.Index].name, - } +func (s *Scene) notifySoloCollisions() { + for newRef := range s.newSoloCollisionRefs { + if _, ok := s.oldSoloCollisionRefs[newRef]; !ok { s.soloCollisionSubscriptions.Each(func(callback SoloCollisionCallback) { - callback(primary, prop, true) + callback(newRef.bodyID, newRef.terrainID, true) }) } } - for oldCollision := range s.oldSBCollisions { - if _, ok := s.newSBCollisions[oldCollision]; !ok { - primary := Body{ - scene: s, - reference: oldCollision.BodyRef, - } - prop := Prop{ - name: s.props[oldCollision.PropRef.Index].name, - } + for oldRef := range s.oldSoloCollisionRefs { + if _, ok := s.newSoloCollisionRefs[oldRef]; !ok { s.soloCollisionSubscriptions.Each(func(callback SoloCollisionCallback) { - callback(primary, prop, false) + callback(oldRef.bodyID, oldRef.terrainID, false) }) } } - clear(s.oldSBCollisions) - maps.Copy(s.oldSBCollisions, s.newSBCollisions) - clear(s.newSBCollisions) + clear(s.oldSoloCollisionRefs) + maps.Copy(s.oldSoloCollisionRefs, s.newSoloCollisionRefs) + clear(s.newSoloCollisionRefs) } -func (s *Scene) notifyDoubleBodyCollisions() { - for newCollision := range s.newDBCollisions { - if _, ok := s.oldDBCollisions[newCollision]; !ok { - primary := Body{ - scene: s, - reference: newCollision.PrimaryRef, - } - secondary := Body{ - scene: s, - reference: newCollision.SecondaryRef, - } +func (s *Scene) notifyPairCollisions() { + for newRef := range s.newPairCollisionRefs { + if _, ok := s.oldPairCollisionRefs[newRef]; !ok { s.pairCollisionSubscriptions.Each(func(callback PairCollisionCallback) { - callback(primary, secondary, true) + callback(newRef.primaryBodyID, newRef.secondaryBodyID, true) }) } } - for oldCollision := range s.oldDBCollisions { - if _, ok := s.newDBCollisions[oldCollision]; !ok { - primary := Body{ - scene: s, - reference: oldCollision.PrimaryRef, - } - secondary := Body{ - scene: s, - reference: oldCollision.SecondaryRef, - } + for oldRef := range s.oldPairCollisionRefs { + if _, ok := s.newPairCollisionRefs[oldRef]; !ok { s.pairCollisionSubscriptions.Each(func(callback PairCollisionCallback) { - callback(primary, secondary, false) + callback(oldRef.primaryBodyID, oldRef.secondaryBodyID, false) }) } } - clear(s.oldDBCollisions) - maps.Copy(s.oldDBCollisions, s.newDBCollisions) - clear(s.newDBCollisions) -} - -type sbCollisionPair struct { - BodyRef indexReference - PropRef indexReference -} - -type dbCollisionPair struct { - PrimaryRef indexReference - SecondaryRef indexReference + clear(s.oldPairCollisionRefs) + maps.Copy(s.oldPairCollisionRefs, s.newPairCollisionRefs) + clear(s.newPairCollisionRefs) } var nilIndex int32 = -1 @@ -1079,3 +1001,25 @@ type terrainData struct { frictionCoefficient float64 restitutionCoefficient float64 } + +type bodyCollisionData struct { + index int32 + frictionCoefficient float64 + restitutionCoefficient float64 +} + +type terrainCollisionData struct { + index int32 + frictionCoefficient float64 + restitutionCoefficient float64 +} + +type soloCollisionRef struct { + bodyID BodyID + terrainID TerrainID +} + +type pairCollisionRef struct { + primaryBodyID BodyID + secondaryBodyID BodyID +} diff --git a/game/physics/solver/change.go b/game/physics/solver/change.go index 80d1e0b8..f2d45e55 100644 --- a/game/physics/solver/change.go +++ b/game/physics/solver/change.go @@ -1,11 +1,11 @@ package solver -type PairImpulse struct { - Target Impulse - Source Impulse -} +// type PairImpulse struct { +// Target Impulse +// Source Impulse +// } -type PairNudge struct { - Target Nudge - Source Nudge -} +// type PairNudge struct { +// Target Nudge +// Source Nudge +// } diff --git a/game/physics/solver/context.go b/game/physics/solver/context.go index c2c8a1e6..ffa7f363 100644 --- a/game/physics/solver/context.go +++ b/game/physics/solver/context.go @@ -1,96 +1,96 @@ package solver -// Context contains information related to single-object constraint -// processing. -type Context struct { - DeltaTime float64 - ImpulseBeta float64 - NudgeBeta float64 +// // Context contains information related to single-object constraint +// // processing. +// type Context struct { +// DeltaTime float64 +// ImpulseBeta float64 +// NudgeBeta float64 - Target *Placeholder -} +// Target *Placeholder +// } -// JacobianImpulseLambda returns the impulse lambda for the specified -// constraint Jacobian, positional drift and restitution. -func (c Context) JacobianImpulseLambda(jacobian Jacobian, drift, restitution float64) float64 { - effMass := jacobian.InverseEffectiveMass(c.Target) - if effMass < Epsilon { - return 0.0 - } - effVelocity := jacobian.EffectiveVelocity(c.Target) - restitutionClamp := RestitutionClamp(effVelocity) - baumgarte := c.ImpulseBeta * drift / c.DeltaTime - return -((1+restitution*restitutionClamp)*effVelocity + baumgarte) / effMass -} +// // JacobianImpulseLambda returns the impulse lambda for the specified +// // constraint Jacobian, positional drift and restitution. +// func (c Context) JacobianImpulseLambda(jacobian Jacobian, drift, restitution float64) float64 { +// effMass := jacobian.InverseEffectiveMass(c.Target) +// if effMass < Epsilon { +// return 0.0 +// } +// effVelocity := jacobian.EffectiveVelocity(c.Target) +// restitutionClamp := RestitutionClamp(effVelocity) +// baumgarte := c.ImpulseBeta * drift / c.DeltaTime +// return -((1+restitution*restitutionClamp)*effVelocity + baumgarte) / effMass +// } -// JacobianNudgeLambda returns the nudge lambda for the specified -// constraint Jacobian and positional drift. -func (c Context) JacobianNudgeLambda(jacobian Jacobian, drift float64) float64 { - effMass := jacobian.InverseEffectiveMass(c.Target) - if effMass < Epsilon { - return 0.0 - } - return -c.NudgeBeta * drift / effMass -} +// // JacobianNudgeLambda returns the nudge lambda for the specified +// // constraint Jacobian and positional drift. +// func (c Context) JacobianNudgeLambda(jacobian Jacobian, drift float64) float64 { +// effMass := jacobian.InverseEffectiveMass(c.Target) +// if effMass < Epsilon { +// return 0.0 +// } +// return -c.NudgeBeta * drift / effMass +// } -// JacobianImpulseSolution returns an impulse solution based on the specified -// constraint Jacobian, positional drift and restitution. -func (c Context) JacobianImpulseSolution(jacobian Jacobian, drift, restitution float64) Impulse { - lambda := c.JacobianImpulseLambda(jacobian, drift, restitution) - return jacobian.Impulse(lambda) -} +// // JacobianImpulseSolution returns an impulse solution based on the specified +// // constraint Jacobian, positional drift and restitution. +// func (c Context) JacobianImpulseSolution(jacobian Jacobian, drift, restitution float64) Impulse { +// lambda := c.JacobianImpulseLambda(jacobian, drift, restitution) +// return jacobian.Impulse(lambda) +// } -// JacobianNudgeSolution returns a nudge solution based on the specified -// constraint Jacobian and positional drift. -func (c Context) JacobianNudgeSolution(jacobian Jacobian, drift float64) Nudge { - lambda := c.JacobianNudgeLambda(jacobian, drift) - return jacobian.Nudge(lambda) -} +// // JacobianNudgeSolution returns a nudge solution based on the specified +// // constraint Jacobian and positional drift. +// func (c Context) JacobianNudgeSolution(jacobian Jacobian, drift float64) Nudge { +// lambda := c.JacobianNudgeLambda(jacobian, drift) +// return jacobian.Nudge(lambda) +// } -// PairContext contains information related to double-object constraint -// processing. -type PairContext struct { - DeltaTime float64 - ImpulseBeta float64 - NudgeBeta float64 +// // PairContext contains information related to double-object constraint +// // processing. +// type PairContext struct { +// DeltaTime float64 +// ImpulseBeta float64 +// NudgeBeta float64 - Target *Placeholder - Source *Placeholder -} +// Target *Placeholder +// Source *Placeholder +// } -// JacobianImpulseLambda returns the impulse lambda for the specified -// constraint Jacobian, positional drift and restitution. -func (c PairContext) JacobianImpulseLambda(jacobian PairJacobian, drift, restitution float64) float64 { - effMass := jacobian.InverseEffectiveMass(c.Target, c.Source) - if effMass < Epsilon { - return 0.0 - } - effVelocity := jacobian.EffectiveVelocity(c.Target, c.Source) - restitutionClamp := RestitutionClamp(effVelocity) - baumgarte := c.ImpulseBeta * drift / c.DeltaTime - return -((1+restitution*restitutionClamp)*effVelocity + baumgarte) / effMass -} +// // JacobianImpulseLambda returns the impulse lambda for the specified +// // constraint Jacobian, positional drift and restitution. +// func (c PairContext) JacobianImpulseLambda(jacobian PairJacobian, drift, restitution float64) float64 { +// effMass := jacobian.InverseEffectiveMass(c.Target, c.Source) +// if effMass < Epsilon { +// return 0.0 +// } +// effVelocity := jacobian.EffectiveVelocity(c.Target, c.Source) +// restitutionClamp := RestitutionClamp(effVelocity) +// baumgarte := c.ImpulseBeta * drift / c.DeltaTime +// return -((1+restitution*restitutionClamp)*effVelocity + baumgarte) / effMass +// } -// JacobianNudgeLambda returns the nudge lambda for the specified -// constraint Jacobian and positional drift. -func (c PairContext) JacobianNudgeLambda(jacobian PairJacobian, drift float64) float64 { - effMass := jacobian.InverseEffectiveMass(c.Target, c.Source) - if effMass < Epsilon { - return 0.0 - } - return -c.NudgeBeta * drift / effMass -} +// // JacobianNudgeLambda returns the nudge lambda for the specified +// // constraint Jacobian and positional drift. +// func (c PairContext) JacobianNudgeLambda(jacobian PairJacobian, drift float64) float64 { +// effMass := jacobian.InverseEffectiveMass(c.Target, c.Source) +// if effMass < Epsilon { +// return 0.0 +// } +// return -c.NudgeBeta * drift / effMass +// } -// JacobianImpulseSolution returns an impulse solution based on the specified -// constraint Jacobian, positional drift and restitution. -func (c PairContext) JacobianImpulseSolution(jacobian PairJacobian, drift, restitution float64) PairImpulse { - lambda := c.JacobianImpulseLambda(jacobian, drift, restitution) - return jacobian.Impulse(lambda) -} +// // JacobianImpulseSolution returns an impulse solution based on the specified +// // constraint Jacobian, positional drift and restitution. +// func (c PairContext) JacobianImpulseSolution(jacobian PairJacobian, drift, restitution float64) PairImpulse { +// lambda := c.JacobianImpulseLambda(jacobian, drift, restitution) +// return jacobian.Impulse(lambda) +// } -// JacobianNudgeSolution returns a nudge solution based on the specified -// constraint Jacobian and positional drift. -func (c PairContext) JacobianNudgeSolution(jacobian PairJacobian, drift float64) PairNudge { - lambda := c.JacobianNudgeLambda(jacobian, drift) - return jacobian.Nudge(lambda) -} +// // JacobianNudgeSolution returns a nudge solution based on the specified +// // constraint Jacobian and positional drift. +// func (c PairContext) JacobianNudgeSolution(jacobian PairJacobian, drift float64) PairNudge { +// lambda := c.JacobianNudgeLambda(jacobian, drift) +// return jacobian.Nudge(lambda) +// } diff --git a/game/physics/solver/jacobian.go b/game/physics/solver/jacobian.go index 03566f0a..076058a2 100644 --- a/game/physics/solver/jacobian.go +++ b/game/physics/solver/jacobian.go @@ -1,52 +1,50 @@ package solver -import "github.com/mokiat/gomath/dprec" +// // PairJacobian represents the 1x12 Jacobian matrix of a double-object velocity +// // constraint. +// type PairJacobian struct { +// Target Jacobian +// Source Jacobian +// } -// PairJacobian represents the 1x12 Jacobian matrix of a double-object velocity -// constraint. -type PairJacobian struct { - Target Jacobian - Source Jacobian -} +// // EffectiveVelocity returns the amount of the combined velocities of the two +// // objects that is going in the wrong direction. +// func (j PairJacobian) EffectiveVelocity(target, source *Placeholder) float64 { +// return j.Target.EffectiveVelocity(target) + j.Source.EffectiveVelocity(source) +// } -// EffectiveVelocity returns the amount of the combined velocities of the two -// objects that is going in the wrong direction. -func (j PairJacobian) EffectiveVelocity(target, source *Placeholder) float64 { - return j.Target.EffectiveVelocity(target) + j.Source.EffectiveVelocity(source) -} +// // InverseEffectiveMass returns the inverse of the effective mass with which +// // the two bodies affect the constraint. +// func (j PairJacobian) InverseEffectiveMass(target, source *Placeholder) float64 { +// return j.Target.InverseEffectiveMass(target) + j.Source.InverseEffectiveMass(source) +// } -// InverseEffectiveMass returns the inverse of the effective mass with which -// the two bodies affect the constraint. -func (j PairJacobian) InverseEffectiveMass(target, source *Placeholder) float64 { - return j.Target.InverseEffectiveMass(target) + j.Source.InverseEffectiveMass(source) -} +// // Impulse returns an impulse solution based on the lambda impulse +// // amount applied according to this Jacobian. +// func (j PairJacobian) Impulse(lambda float64) PairImpulse { +// return PairImpulse{ +// Target: Impulse{ +// Linear: dprec.Vec3Prod(j.Target.LinearSlope, lambda), +// Angular: dprec.Vec3Prod(j.Target.AngularSlope, lambda), +// }, +// Source: Impulse{ +// Linear: dprec.Vec3Prod(j.Source.LinearSlope, lambda), +// Angular: dprec.Vec3Prod(j.Source.AngularSlope, lambda), +// }, +// } +// } -// Impulse returns an impulse solution based on the lambda impulse -// amount applied according to this Jacobian. -func (j PairJacobian) Impulse(lambda float64) PairImpulse { - return PairImpulse{ - Target: Impulse{ - Linear: dprec.Vec3Prod(j.Target.LinearSlope, lambda), - Angular: dprec.Vec3Prod(j.Target.AngularSlope, lambda), - }, - Source: Impulse{ - Linear: dprec.Vec3Prod(j.Source.LinearSlope, lambda), - Angular: dprec.Vec3Prod(j.Source.AngularSlope, lambda), - }, - } -} - -// Nudge returns a nudge solution based on the lambda nudge amount -// applied according to this Jacobian. -func (j PairJacobian) Nudge(lambda float64) PairNudge { - return PairNudge{ - Target: Nudge{ - Linear: dprec.Vec3Prod(j.Target.LinearSlope, lambda), - Angular: dprec.Vec3Prod(j.Target.AngularSlope, lambda), - }, - Source: Nudge{ - Linear: dprec.Vec3Prod(j.Source.LinearSlope, lambda), - Angular: dprec.Vec3Prod(j.Source.AngularSlope, lambda), - }, - } -} +// // Nudge returns a nudge solution based on the lambda nudge amount +// // applied according to this Jacobian. +// func (j PairJacobian) Nudge(lambda float64) PairNudge { +// return PairNudge{ +// Target: Nudge{ +// Linear: dprec.Vec3Prod(j.Target.LinearSlope, lambda), +// Angular: dprec.Vec3Prod(j.Target.AngularSlope, lambda), +// }, +// Source: Nudge{ +// Linear: dprec.Vec3Prod(j.Source.LinearSlope, lambda), +// Angular: dprec.Vec3Prod(j.Source.AngularSlope, lambda), +// }, +// } +// } diff --git a/game/physics/solver_collision_pair.go b/game/physics/solver_collision_pair.go new file mode 100644 index 00000000..4fb56138 --- /dev/null +++ b/game/physics/solver_collision_pair.go @@ -0,0 +1,15 @@ +package physics + +var _ PairConstraintSolver = (*PairCollisionSolver)(nil) + +type PairCollisionSolver struct{} + +func (s *PairCollisionSolver) Reset(ctx PairConstraintContext) { + +} +func (s *PairCollisionSolver) ApplyImpulses(ctx PairConstraintContext) { + +} +func (s *PairCollisionSolver) ApplyNudges(ctx PairConstraintContext) { + +} diff --git a/game/physics/solver_collision_solo.go b/game/physics/solver_collision_solo.go new file mode 100644 index 00000000..8e5f6b2e --- /dev/null +++ b/game/physics/solver_collision_solo.go @@ -0,0 +1,34 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +var _ SoloConstraintSolver = (*SoloCollisionSolver)(nil) + +type SoloCollisionSolverConfig struct { + TerrainFrictionCoefficient float64 + TerrainRestitutionCoefficient float64 + TerrainContactNormal dprec.Vec3 + + BodyFrictionCoefficient float64 + BodyRestitutionCoefficient float64 + BodyContactPoint dprec.Vec3 + + Depth float64 +} + +type SoloCollisionSolver struct { +} + +func (s *SoloCollisionSolver) Init(config SoloCollisionSolverConfig) { + +} + +func (s *SoloCollisionSolver) Reset(ctx SoloConstraintContext) { + +} +func (s *SoloCollisionSolver) ApplyImpulses(ctx SoloConstraintContext) { + +} +func (s *SoloCollisionSolver) ApplyNudges(ctx SoloConstraintContext) { + +} diff --git a/game/physics/terrain.go b/game/physics/terrain.go index e4fb564e..6e464150 100644 --- a/game/physics/terrain.go +++ b/game/physics/terrain.go @@ -85,6 +85,14 @@ func (v TerrainView) IsValid(id TerrainID) bool { return terrain != nil } +func (v TerrainView) idFromIndex(index int32) TerrainID { + terrain := &v.scene.terrains[index] + return TerrainID{ + index: index, + revision: terrain.revision, + } +} + func (v TerrainView) resolve(id TerrainID, required bool) *terrainState { if id.revision == 0 { if required { diff --git a/game/scene.go b/game/scene.go index 4a5b024f..1260b1f3 100644 --- a/game/scene.go +++ b/game/scene.go @@ -77,7 +77,7 @@ func newScene(engine *Engine, info SceneInfo) *Scene { // source binding sets armatureBindingSet := hierarchy.NewSourceBindingSet(hierarchyScene, NewAnimationBinding()) - bodyBindingSet := hierarchy.NewSourceBindingSet(hierarchyScene, NewBodyBinding()) + bodyBindingSet := hierarchy.NewSourceBindingSet(hierarchyScene, NewBodyBinding(physicsScene)) // target binding sets skyBindingSet := hierarchy.NewInterpolationBindingSet(hierarchyScene, NewSkyBinding()) ambientLightBindingSet := hierarchy.NewInterpolationBindingSet(hierarchyScene, NewAmbientLightBinding()) @@ -132,7 +132,7 @@ type Scene struct { // source binding sets armatureBindingSet *hierarchy.SourceBindingSet[*animation.Player] - bodyBindingSet *hierarchy.SourceBindingSet[physics.Body] + bodyBindingSet *hierarchy.SourceBindingSet[physics.BodyID] // target binding sets skyBindingSet *hierarchy.InterpolationBindingSet[*graphics.Sky] ambientLightBindingSet *hierarchy.InterpolationBindingSet[*graphics.AmbientLight] @@ -160,9 +160,7 @@ func (s *Scene) Delete() { if s.ecsScene != nil { defer s.ecsScene.Delete() } - if s.physicsScene != nil { - defer s.physicsScene.Delete() - } + s.physicsScene = nil if s.gfxScene != nil { defer s.gfxScene.Delete() } @@ -231,7 +229,7 @@ func (s *Scene) ArmatureBindingSet() *hierarchy.SourceBindingSet[*animation.Play } // BodyBindingSet returns the binding set that binds physics bodies. -func (s *Scene) BodyBindingSet() *hierarchy.SourceBindingSet[physics.Body] { +func (s *Scene) BodyBindingSet() *hierarchy.SourceBindingSet[physics.BodyID] { return s.bodyBindingSet } From 6a1fdbb6240c9ec83dc283509e7d10b07f06cc47 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Mon, 3 Aug 2026 22:53:07 +0300 Subject: [PATCH 35/85] Mode core rework --- game/physics/scene.go | 175 ++++++++++++-------------- game/physics/solver_collision_pair.go | 22 +++- game/physics/solver_collision_solo.go | 4 +- 3 files changed, 106 insertions(+), 95 deletions(-) diff --git a/game/physics/scene.go b/game/physics/scene.go index 0053c8a7..31623e10 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -573,28 +573,6 @@ func (s *Scene) eachTerrain(cb func(index int, t *terrainState)) { } } -/////// OLD BELOW ------------ (TODO: DELETE COMMENT) - -func (s *Scene) CheckSegmentIntersection(segment shape3d.Segment, mask uint32) (BodyID, bool) { - intersection, ok := s.collisionScene.CheckSegmentIntersection(segment, placement3d.Filter{ - Mask: opt.V(mask), - }) - if !ok { - return NilBodyID, false - } - if intersection.TargetShapeID == placement3d.InvalidShapeID { - // A prop. - return NilBodyID, false // FIXME: This should handle props as well. - } - objectID := s.collisionScene.GetShapeObject(intersection.TargetShapeID) - bData := s.collisionScene.GetObjectUserData(objectID) - body := &s.bodies[bData.index] - return BodyID{ - index: bData.index, - revision: body.revision, - }, true -} - func (s *Scene) runSimulation(elapsedSeconds float64) { if elapsedSeconds > 0.0001 { s.applyAcceleration(elapsedSeconds) @@ -642,40 +620,6 @@ func (s *Scene) applyAcceleration(elapsedSeconds float64) { }) } -// func (s *Scene) applyAerodynamicAccelerations() { -// s.eachBody(func(index int, body *bodyState) { -// if len(body.aerodynamicShapes) == 0 { -// return -// } -// target := &s.bodyAccelerationTargets[index] -// mediumDensity := s.mediumSolver.Density(body.position) -// mediumVelocity := s.mediumSolver.Velocity(body.position) - -// deltaVelocity := dprec.Vec3Diff(mediumVelocity, body.velocity) -// dragForce := dprec.Vec3Prod(deltaVelocity, deltaVelocity.Length()*mediumDensity*body.dragFactor) -// target.ApplyForce(dragForce) - -// angularDragForce := dprec.Vec3Prod(body.angularVelocity, -body.angularVelocity.Length()*mediumDensity*body.angularDragFactor) -// target.ApplyTorque(angularDragForce) - -// bodyTransform := NewTransform(body.position, body.rotation) -// for _, aerodynamicShape := range body.aerodynamicShapes { -// // TODO: Take shape velocity into account. This also means that wings should be -// // split into two, to benefit from that. - -// aerodynamicShape = aerodynamicShape.Transformed(bodyTransform) -// relativeSpeed := dprec.QuatVec3Rotation(dprec.InverseQuat(aerodynamicShape.Rotation()), deltaVelocity) - -// force := aerodynamicShape.solver.Force(relativeSpeed, mediumDensity) -// absoluteForce := dprec.QuatVec3Rotation(aerodynamicShape.Rotation(), force) - -// offset := dprec.Vec3Diff(aerodynamicShape.Position(), bodyTransform.Position()) -// target.ApplyOffsetForce(offset, absoluteForce) -// // target.ApplyOffsetForce(absoluteForce, aerodynamicShape.Position()) -// } -// }) -// } - func (s *Scene) applyImpulses(elapsedSeconds float64) { defer metric.BeginRegion("impulses").End() @@ -868,42 +812,33 @@ func (s *Scene) detectCollisions() { } } -func (s *Scene) handlePairCollision(primaryBodyData, secondaryBodyData bodyCollisionData, contact placement3d.Contact) { - // primary := &s.bodies[primaryIndex] - // secondary := &s.bodies[secondaryIndex] - - // solver := s.allocateDualCollisionSolver() - // solver.Init(constraint.PairCollisionState{ - // PrimaryNormal: intersection.TargetNormal, - // PrimaryPoint: intersection.EvalSourcePoint(), - // PrimaryFrictionCoefficient: primary.frictionCoefficient, - // PrimaryRestitutionCoefficient: primary.restitutionCoefficient, - - // SecondaryNormal: intersection.EvalSourceNormal(), - // SecondaryPoint: intersection.TargetPoint, - // SecondaryFrictionCoefficient: secondary.frictionCoefficient, - // SecondaryRestitutionCoefficient: secondary.restitutionCoefficient, - - // Depth: intersection.Depth, - // }) - - // pair := dbCollisionPair{ - // PrimaryRef: primary.reference, - // SecondaryRef: secondary.reference, - // } - // s.newDBCollisions[pair] = struct{}{} - - // primaryBody := Body{ - // scene: s, - // reference: primary.reference, - // } - // - // secondaryBody := Body{ - // scene: s, - // reference: secondary.reference, - // } - // - // s.dbCollisionConstraints = append(s.dbCollisionConstraints, s.CreateDoubleBodyConstraint(primaryBody, secondaryBody, solver)) +func (s *Scene) handlePairCollision(primaryData, secondaryData bodyCollisionData, contact placement3d.Contact) { + solver := s.allocatePairCollisionSolver() + solver.Init(PairCollisionSolverConfig{ + PrimaryFrictionCoefficient: primaryData.frictionCoefficient, + PrimaryRestitutionCoefficient: primaryData.restitutionCoefficient, + PrimaryContactNormal: contact.EvalSourceNormal(), + PrimaryContactPoint: contact.EvalSourcePoint(), + + SecondaryFrictionCoefficient: secondaryData.frictionCoefficient, + SecondaryRestitutionCoefficient: secondaryData.restitutionCoefficient, + SecondaryContactNormal: contact.TargetNormal, + SecondaryContactPoint: contact.TargetPoint, + + Depth: contact.Depth, + }) + + primaryID := s.Bodies().idFromIndex(primaryData.index) + secondaryID := s.Bodies().idFromIndex(secondaryData.index) + + constraintID := s.PairConstraints().Create(primaryID, secondaryID, solver) + s.pairCollisionConstraintIDs = append(s.pairCollisionConstraintIDs, constraintID) + + ref := pairCollisionRef{ + primaryBodyID: primaryID, + secondaryBodyID: secondaryID, + } + s.newPairCollisionRefs[ref] = struct{}{} } func (s *Scene) handleSoloCollision(bodyData bodyCollisionData, terrainData terrainCollisionData, contact placement3d.Contact) { @@ -1023,3 +958,59 @@ type pairCollisionRef struct { primaryBodyID BodyID secondaryBodyID BodyID } + +/////// OLD BELOW ------------ (TODO: DELETE COMMENT) + +// func (s *Scene) applyAerodynamicAccelerations() { +// s.eachBody(func(index int, body *bodyState) { +// if len(body.aerodynamicShapes) == 0 { +// return +// } +// target := &s.bodyAccelerationTargets[index] +// mediumDensity := s.mediumSolver.Density(body.position) +// mediumVelocity := s.mediumSolver.Velocity(body.position) + +// deltaVelocity := dprec.Vec3Diff(mediumVelocity, body.velocity) +// dragForce := dprec.Vec3Prod(deltaVelocity, deltaVelocity.Length()*mediumDensity*body.dragFactor) +// target.ApplyForce(dragForce) + +// angularDragForce := dprec.Vec3Prod(body.angularVelocity, -body.angularVelocity.Length()*mediumDensity*body.angularDragFactor) +// target.ApplyTorque(angularDragForce) + +// bodyTransform := NewTransform(body.position, body.rotation) +// for _, aerodynamicShape := range body.aerodynamicShapes { +// // TODO: Take shape velocity into account. This also means that wings should be +// // split into two, to benefit from that. + +// aerodynamicShape = aerodynamicShape.Transformed(bodyTransform) +// relativeSpeed := dprec.QuatVec3Rotation(dprec.InverseQuat(aerodynamicShape.Rotation()), deltaVelocity) + +// force := aerodynamicShape.solver.Force(relativeSpeed, mediumDensity) +// absoluteForce := dprec.QuatVec3Rotation(aerodynamicShape.Rotation(), force) + +// offset := dprec.Vec3Diff(aerodynamicShape.Position(), bodyTransform.Position()) +// target.ApplyOffsetForce(offset, absoluteForce) +// // target.ApplyOffsetForce(absoluteForce, aerodynamicShape.Position()) +// } +// }) +// } + +func (s *Scene) CheckSegmentIntersection(segment shape3d.Segment, mask uint32) (BodyID, bool) { + intersection, ok := s.collisionScene.CheckSegmentIntersection(segment, placement3d.Filter{ + Mask: opt.V(mask), + }) + if !ok { + return NilBodyID, false + } + if intersection.TargetShapeID == placement3d.InvalidShapeID { + // A prop. + return NilBodyID, false // FIXME: This should handle props as well. + } + objectID := s.collisionScene.GetShapeObject(intersection.TargetShapeID) + bData := s.collisionScene.GetObjectUserData(objectID) + body := &s.bodies[bData.index] + return BodyID{ + index: bData.index, + revision: body.revision, + }, true +} diff --git a/game/physics/solver_collision_pair.go b/game/physics/solver_collision_pair.go index 4fb56138..41cd12b2 100644 --- a/game/physics/solver_collision_pair.go +++ b/game/physics/solver_collision_pair.go @@ -1,9 +1,29 @@ package physics -var _ PairConstraintSolver = (*PairCollisionSolver)(nil) +import "github.com/mokiat/gomath/dprec" + +type PairCollisionSolverConfig struct { + PrimaryFrictionCoefficient float64 + PrimaryRestitutionCoefficient float64 + PrimaryContactNormal dprec.Vec3 + PrimaryContactPoint dprec.Vec3 + + SecondaryFrictionCoefficient float64 + SecondaryRestitutionCoefficient float64 + SecondaryContactNormal dprec.Vec3 + SecondaryContactPoint dprec.Vec3 + + Depth float64 +} type PairCollisionSolver struct{} +var _ PairConstraintSolver = (*PairCollisionSolver)(nil) + +func (s *PairCollisionSolver) Init(config PairCollisionSolverConfig) { + +} + func (s *PairCollisionSolver) Reset(ctx PairConstraintContext) { } diff --git a/game/physics/solver_collision_solo.go b/game/physics/solver_collision_solo.go index 8e5f6b2e..8acb46df 100644 --- a/game/physics/solver_collision_solo.go +++ b/game/physics/solver_collision_solo.go @@ -2,8 +2,6 @@ package physics import "github.com/mokiat/gomath/dprec" -var _ SoloConstraintSolver = (*SoloCollisionSolver)(nil) - type SoloCollisionSolverConfig struct { TerrainFrictionCoefficient float64 TerrainRestitutionCoefficient float64 @@ -19,6 +17,8 @@ type SoloCollisionSolverConfig struct { type SoloCollisionSolver struct { } +var _ SoloConstraintSolver = (*SoloCollisionSolver)(nil) + func (s *SoloCollisionSolver) Init(config SoloCollisionSolverConfig) { } From f1208733717435bb8eb8787d9a02d6299e4510a7 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Mon, 3 Aug 2026 23:03:26 +0300 Subject: [PATCH 36/85] Minor refactoring --- game/physics/body.go | 6 +++--- game/physics/scene.go | 7 ++----- 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/game/physics/body.go b/game/physics/body.go index 167f98d4..0579db8b 100644 --- a/game/physics/body.go +++ b/game/physics/body.go @@ -159,7 +159,7 @@ func (v BodyView) Position(id BodyID) dprec.Vec3 { func (v BodyView) SetPosition(id BodyID, position dprec.Vec3) { body := v.resolve(id, true) body.position = position - v.refreshPlacement(id, body) + v.refreshPlacement(body) } func (v BodyView) Rotation(id BodyID) dprec.Quat { @@ -170,7 +170,7 @@ func (v BodyView) Rotation(id BodyID) dprec.Quat { func (v BodyView) SetRotation(id BodyID, rotation dprec.Quat) { body := v.resolve(id, true) body.rotation = rotation - v.refreshPlacement(id, body) + v.refreshPlacement(body) } func (v BodyView) AttachCollisionSphere(id BodyID, col CollisionSphere) CollisionShapeID { @@ -212,7 +212,7 @@ func (v BodyView) DetachCollisionShape(id BodyID, shapeID CollisionShapeID) { v.scene.collisionScene.DeleteShape(shapeID.shapeID) } -func (v BodyView) refreshPlacement(id BodyID, body *bodyState) { +func (v BodyView) refreshPlacement(body *bodyState) { v.scene.collisionScene.SetObjectTransform(body.objectID, shape3d.Transform{ Translation: body.position, Rotation: shape3d.RotationFromQuat(body.rotation), diff --git a/game/physics/scene.go b/game/physics/scene.go index 31623e10..4dd98103 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -737,12 +737,9 @@ func (s *Scene) applyNudges(elapsedSeconds float64) { func (s *Scene) applyPlacement() { defer metric.BeginRegion("placement").End() + bodies := s.Bodies() s.eachBody(func(_ int, body *bodyState) { - // Update the collision scene with the new position and rotation of the body. - s.collisionScene.SetObjectTransform(body.objectID, shape3d.Transform{ - Translation: body.position, - Rotation: shape3d.RotationFromQuat(body.rotation), - }) + bodies.refreshPlacement(body) }) } From 805722dc43aeef06ce26046517d67b9292c37ed5 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Mon, 3 Aug 2026 23:42:35 +0300 Subject: [PATCH 37/85] Use world-space inertia tensor --- game/physics/body.go | 18 +++++++++++++----- game/physics/scene.go | 5 +++++ 2 files changed, 18 insertions(+), 5 deletions(-) diff --git a/game/physics/body.go b/game/physics/body.go index 0579db8b..446ef0b2 100644 --- a/game/physics/body.go +++ b/game/physics/body.go @@ -35,8 +35,9 @@ func (v BodyView) Create(position dprec.Vec3, rotation dprec.Quat) BodyID { firstBodyAcceleratorIndex: nilIndex, firstSoloConstraintIndex: nilIndex, firstPairConstraintIndex: nilIndex, - invMass: 1.0, + invInertiaLocal: dprec.IdentityMat3(), invInertia: dprec.IdentityMat3(), + invMass: 1.0, linearVelocity: dprec.ZeroVec3(), angularVelocity: dprec.ZeroVec3(), position: position, @@ -123,12 +124,13 @@ func (v BodyView) SetMass(id BodyID, mass float64) { func (v BodyView) MomentOfInertia(id BodyID) dprec.Mat3 { body := v.resolve(id, true) - return dprec.InverseMat3(body.invInertia) + return dprec.InverseMat3(body.invInertiaLocal) } func (v BodyView) SetMomentOfInertia(id BodyID, inertia dprec.Mat3) { body := v.resolve(id, true) - body.invInertia = dprec.InverseMat3(inertia) + body.invInertiaLocal = dprec.InverseMat3(inertia) + body.recalculateInertia() } func (v BodyView) Velocity(id BodyID) dprec.Vec3 { @@ -170,6 +172,7 @@ func (v BodyView) Rotation(id BodyID) dprec.Quat { func (v BodyView) SetRotation(id BodyID, rotation dprec.Quat) { body := v.resolve(id, true) body.rotation = rotation + body.recalculateInertia() v.refreshPlacement(body) } @@ -329,8 +332,9 @@ type bodyState struct { firstSoloConstraintIndex int32 firstPairConstraintIndex int32 - invMass float64 - invInertia dprec.Mat3 + invInertiaLocal dprec.Mat3 + invInertia dprec.Mat3 + invMass float64 linearAcceleration dprec.Vec3 angularAcceleration dprec.Vec3 @@ -354,6 +358,10 @@ func (s *bodyState) inertia() dprec.Mat3 { return dprec.InverseMat3(s.invInertia) } +func (b *bodyState) recalculateInertia() { + b.invInertia = RotatedMomentOfInertia(b.invInertiaLocal, b.rotation) +} + func (b *bodyState) addLinearAcceleration(amount dprec.Vec3) { b.linearAcceleration = dprec.Vec3Sum(b.linearAcceleration, amount) } diff --git a/game/physics/scene.go b/game/physics/scene.go index 4dd98103..d4cc4915 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -695,6 +695,7 @@ func (s *Scene) applyMotion(elapsedSeconds float64) { // Apply the velocity to the body's position and rotation. body.translate(dprec.Vec3Prod(body.linearVelocity, elapsedSeconds)) body.rotate(QuatFromVector(dprec.Vec3Prod(body.angularVelocity, elapsedSeconds))) + body.recalculateInertia() }) } @@ -731,6 +732,10 @@ func (s *Scene) applyNudges(elapsedSeconds float64) { constraint.solver.Reset(ctx) constraint.solver.ApplyNudges(ctx) }) + + s.eachBody(func(_ int, body *bodyState) { + body.recalculateInertia() + }) } } From c69ccc40c4ccc001a13662f213ea1e7de1ee9ce6 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Tue, 4 Aug 2026 01:16:16 +0300 Subject: [PATCH 38/85] Implement solo collision solver --- game/physics/constraint_solo.go | 40 +++++++++++++++++++ game/physics/scene.go | 6 +-- game/physics/solver_collision_pair.go | 10 +++-- game/physics/solver_collision_solo.go | 57 +++++++++++++++++++++++++-- 4 files changed, 103 insertions(+), 10 deletions(-) diff --git a/game/physics/constraint_solo.go b/game/physics/constraint_solo.go index 50127091..92498d32 100644 --- a/game/physics/constraint_solo.go +++ b/game/physics/constraint_solo.go @@ -23,6 +23,46 @@ type SoloConstraintContext struct { Target ConstraintTarget } +func (c SoloConstraintContext) ImpulseLambda(jacobian Jacobian, drift, restitutionCoef float64) float64 { + effMass := jacobian.InverseEffectiveMass(c.Target) + if effMass < Epsilon { + return 0.0 + } + effVelocity := jacobian.EffectiveVelocity(c.Target) + restitution := 1 + restitutionCoef*RestitutionClamp(effVelocity) + baumgarte := c.ImpulseBeta * drift / c.DeltaSeconds + return -(restitution*effVelocity - baumgarte) / effMass +} + +func (c SoloConstraintContext) ImpulseLambdaSplit(jacobian Jacobian, drift, restitutionCoef float64) (float64, float64) { + effMass := jacobian.InverseEffectiveMass(c.Target) + if effMass < Epsilon { + return 0.0, 0.0 + } + effVelocity := jacobian.EffectiveVelocity(c.Target) + restitution := 1 + restitutionCoef*RestitutionClamp(effVelocity) + baumgarte := c.ImpulseBeta * drift / c.DeltaSeconds + return -restitution * effVelocity / effMass, baumgarte / effMass +} + +func (c SoloConstraintContext) ImpulseSolution(jacobian Jacobian, drift, restitutionCoef float64) Impulse { + lambda := c.ImpulseLambda(jacobian, drift, restitutionCoef) + return jacobian.Impulse(lambda) +} + +func (c SoloConstraintContext) NudgeLambda(jacobian Jacobian, drift float64) float64 { + effMass := jacobian.InverseEffectiveMass(c.Target) + if effMass < Epsilon { + return 0.0 + } + return c.NudgeBeta * drift / effMass +} + +func (c SoloConstraintContext) NudgeSolution(jacobian Jacobian, drift float64) Nudge { + lambda := c.NudgeLambda(jacobian, drift) + return jacobian.Nudge(lambda) +} + // SoloConstraintSolver implements the mathematical logic that enforces a // constraint acting on a single body. // diff --git a/game/physics/scene.go b/game/physics/scene.go index d4cc4915..08ee4b45 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -119,7 +119,7 @@ func NewScene() *Scene { impulseIterationCount: 8, impulseDriftAdjustmentRatio: 0.2, - nudgeIterationCount: 8, + nudgeIterationCount: 4, nudgeDriftAdjustmentRatio: 0.2, timeScale: 1.0, @@ -827,7 +827,7 @@ func (s *Scene) handlePairCollision(primaryData, secondaryData bodyCollisionData SecondaryContactNormal: contact.TargetNormal, SecondaryContactPoint: contact.TargetPoint, - Depth: contact.Depth, + ContactDepth: contact.Depth, }) primaryID := s.Bodies().idFromIndex(primaryData.index) @@ -854,7 +854,7 @@ func (s *Scene) handleSoloCollision(bodyData bodyCollisionData, terrainData terr BodyRestitutionCoefficient: bodyData.restitutionCoefficient, BodyContactPoint: contact.EvalSourcePoint(), - Depth: contact.Depth, + ContactDepth: contact.Depth, }) bodyID := s.Bodies().idFromIndex(bodyData.index) diff --git a/game/physics/solver_collision_pair.go b/game/physics/solver_collision_pair.go index 41cd12b2..3b13a262 100644 --- a/game/physics/solver_collision_pair.go +++ b/game/physics/solver_collision_pair.go @@ -13,7 +13,7 @@ type PairCollisionSolverConfig struct { SecondaryContactNormal dprec.Vec3 SecondaryContactPoint dprec.Vec3 - Depth float64 + ContactDepth float64 } type PairCollisionSolver struct{} @@ -25,11 +25,13 @@ func (s *PairCollisionSolver) Init(config PairCollisionSolverConfig) { } func (s *PairCollisionSolver) Reset(ctx PairConstraintContext) { - + // TODO } -func (s *PairCollisionSolver) ApplyImpulses(ctx PairConstraintContext) { +func (s *PairCollisionSolver) ApplyImpulses(ctx PairConstraintContext) { + // TODO } -func (s *PairCollisionSolver) ApplyNudges(ctx PairConstraintContext) { +func (s *PairCollisionSolver) ApplyNudges(ctx PairConstraintContext) { + // TODO } diff --git a/game/physics/solver_collision_solo.go b/game/physics/solver_collision_solo.go index 8acb46df..391b068f 100644 --- a/game/physics/solver_collision_solo.go +++ b/game/physics/solver_collision_solo.go @@ -11,24 +11,75 @@ type SoloCollisionSolverConfig struct { BodyRestitutionCoefficient float64 BodyContactPoint dprec.Vec3 - Depth float64 + ContactDepth float64 } type SoloCollisionSolver struct { + terrainContactNormal dprec.Vec3 + bodyContactPoint dprec.Vec3 + contactDepth float64 + + frictionCoefficient float64 + restitutionCoefficient float64 + + pointOffsetWS dprec.Vec3 + jacobian Jacobian + drift float64 } var _ SoloConstraintSolver = (*SoloCollisionSolver)(nil) func (s *SoloCollisionSolver) Init(config SoloCollisionSolverConfig) { + s.terrainContactNormal = config.TerrainContactNormal + s.bodyContactPoint = config.BodyContactPoint + s.contactDepth = config.ContactDepth + s.frictionCoefficient = dprec.Sqrt(config.BodyFrictionCoefficient * config.TerrainFrictionCoefficient) + s.restitutionCoefficient = max(config.BodyRestitutionCoefficient, config.TerrainRestitutionCoefficient) } func (s *SoloCollisionSolver) Reset(ctx SoloConstraintContext) { - + s.pointOffsetWS = dprec.Vec3Diff(s.bodyContactPoint, ctx.Target.Position()) + s.jacobian = Jacobian{ + LinearSlope: s.terrainContactNormal, + AngularSlope: dprec.Vec3Cross(s.pointOffsetWS, s.terrainContactNormal), + } + s.drift = s.contactDepth } + func (s *SoloCollisionSolver) ApplyImpulses(ctx SoloConstraintContext) { + // Bounce solution + bounceLambda, baumgarteLambda := ctx.ImpulseLambdaSplit(s.jacobian, s.drift, s.restitutionCoefficient) + if bounceLambda < 0.0 { + return // moving away + } + bounceImpulse := s.jacobian.Impulse(bounceLambda + baumgarteLambda) + // Friction solution + pointVelocity := dprec.Vec3Sum(ctx.Target.LinearVelocity(), dprec.Vec3Cross(ctx.Target.AngularVelocity(), s.pointOffsetWS)) + pointLateralVelocity := dprec.Vec3Projection(pointVelocity, s.terrainContactNormal) + var frictionSolution Impulse + if lng := pointLateralVelocity.Length(); lng > Epsilon { + pointLateralDirection := dprec.UnitVec3(pointLateralVelocity) + frictionJacobian := Jacobian{ + LinearSlope: dprec.InverseVec3(pointLateralDirection), + AngularSlope: dprec.Vec3Cross(pointLateralDirection, s.pointOffsetWS), + } + frictionLambda := ctx.ImpulseLambda(frictionJacobian, 0.0, 0.0) + maxFrictionLambda := bounceLambda * s.frictionCoefficient + frictionLambda = min(frictionLambda, maxFrictionLambda) + frictionSolution = frictionJacobian.Impulse(frictionLambda) + } + + // Note: Make sure to apply these as late as possible, otherwise you are + // introducing noise that is picked up by friction calculations. + ctx.Target.ApplyImpulse(bounceImpulse) + ctx.Target.ApplyImpulse(frictionSolution) } -func (s *SoloCollisionSolver) ApplyNudges(ctx SoloConstraintContext) { +func (s *SoloCollisionSolver) ApplyNudges(ctx SoloConstraintContext) { + if s.drift > 0.0 { + nudge := ctx.NudgeSolution(s.jacobian, s.drift) + ctx.Target.ApplyNudge(nudge) + } } From 3ffdb7cf0beaec7525750edc08684a0d0a5481ee Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Tue, 4 Aug 2026 19:56:45 +0300 Subject: [PATCH 39/85] Minor adjustments and godoc to SoloConstraintContext --- game/physics/constraint_solo.go | 58 ++++++++++++++++++++++----- game/physics/solver_collision_solo.go | 2 +- 2 files changed, 48 insertions(+), 12 deletions(-) diff --git a/game/physics/constraint_solo.go b/game/physics/constraint_solo.go index 92498d32..071e6fd3 100644 --- a/game/physics/constraint_solo.go +++ b/game/physics/constraint_solo.go @@ -23,41 +23,77 @@ type SoloConstraintContext struct { Target ConstraintTarget } +// ImpulseLambda returns the impulse magnitude (lambda) that needs to be +// applied along jacobian in order to correct its velocity error, taking +// into account both restitution and Baumgarte positional-drift +// stabilization. +// +// drift is the current positional error of the constraint (e.g. a +// penetration depth), and restitutionCoef is the coefficient of +// restitution to apply, in the [0.0, 1.0] range. +// +// The result is intended to be passed to [Jacobian.Impulse]. func (c SoloConstraintContext) ImpulseLambda(jacobian Jacobian, drift, restitutionCoef float64) float64 { - effMass := jacobian.InverseEffectiveMass(c.Target) - if effMass < Epsilon { + invEffMass := jacobian.InverseEffectiveMass(c.Target) + if invEffMass < Epsilon { return 0.0 } effVelocity := jacobian.EffectiveVelocity(c.Target) restitution := 1 + restitutionCoef*RestitutionClamp(effVelocity) baumgarte := c.ImpulseBeta * drift / c.DeltaSeconds - return -(restitution*effVelocity - baumgarte) / effMass + return -(restitution*effVelocity - baumgarte) / invEffMass } -func (c SoloConstraintContext) ImpulseLambdaSplit(jacobian Jacobian, drift, restitutionCoef float64) (float64, float64) { - effMass := jacobian.InverseEffectiveMass(c.Target) - if effMass < Epsilon { +// ImpulseLambdaComponents behaves like [SoloConstraintContext.ImpulseLambda] +// but returns its two additive components separately instead of their sum: +// bounce is the restitution-only component, derived purely from the +// jacobian's current velocity error, while baumgarte is the +// positional-drift-correction component, derived from drift. +// +// bounce + baumgarte is equal to the value that +// [SoloConstraintContext.ImpulseLambda] would return for the same +// arguments. Callers that need to reason about the velocity-only portion +// in isolation - for example to detect separating contacts, or to derive +// a friction bound from it - should use this method instead. +func (c SoloConstraintContext) ImpulseLambdaComponents(jacobian Jacobian, drift, restitutionCoef float64) (bounce, baumgarte float64) { + invEffMass := jacobian.InverseEffectiveMass(c.Target) + if invEffMass < Epsilon { return 0.0, 0.0 } effVelocity := jacobian.EffectiveVelocity(c.Target) restitution := 1 + restitutionCoef*RestitutionClamp(effVelocity) - baumgarte := c.ImpulseBeta * drift / c.DeltaSeconds - return -restitution * effVelocity / effMass, baumgarte / effMass + driftBias := c.ImpulseBeta * drift / c.DeltaSeconds + return -restitution * effVelocity / invEffMass, driftBias / invEffMass } +// ImpulseSolution returns the [Impulse] that needs to be applied to +// correct the velocity error of jacobian, combining restitution and +// Baumgarte positional-drift stabilization. +// +// See [SoloConstraintContext.ImpulseLambda] for details on drift and +// restitutionCoef. func (c SoloConstraintContext) ImpulseSolution(jacobian Jacobian, drift, restitutionCoef float64) Impulse { lambda := c.ImpulseLambda(jacobian, drift, restitutionCoef) return jacobian.Impulse(lambda) } +// NudgeLambda returns the nudge magnitude (lambda) that needs to be +// applied along jacobian in order to correct drift, the current +// positional error of the constraint (e.g. a penetration depth), through +// Baumgarte positional-drift stabilization. +// +// The result is intended to be passed to [Jacobian.Nudge]. func (c SoloConstraintContext) NudgeLambda(jacobian Jacobian, drift float64) float64 { - effMass := jacobian.InverseEffectiveMass(c.Target) - if effMass < Epsilon { + invEffMass := jacobian.InverseEffectiveMass(c.Target) + if invEffMass < Epsilon { return 0.0 } - return c.NudgeBeta * drift / effMass + return c.NudgeBeta * drift / invEffMass } +// NudgeSolution returns the [Nudge] that needs to be applied to correct +// drift, the current positional error of the constraint (e.g. a +// penetration depth). func (c SoloConstraintContext) NudgeSolution(jacobian Jacobian, drift float64) Nudge { lambda := c.NudgeLambda(jacobian, drift) return jacobian.Nudge(lambda) diff --git a/game/physics/solver_collision_solo.go b/game/physics/solver_collision_solo.go index 391b068f..e1a34964 100644 --- a/game/physics/solver_collision_solo.go +++ b/game/physics/solver_collision_solo.go @@ -49,7 +49,7 @@ func (s *SoloCollisionSolver) Reset(ctx SoloConstraintContext) { func (s *SoloCollisionSolver) ApplyImpulses(ctx SoloConstraintContext) { // Bounce solution - bounceLambda, baumgarteLambda := ctx.ImpulseLambdaSplit(s.jacobian, s.drift, s.restitutionCoefficient) + bounceLambda, baumgarteLambda := ctx.ImpulseLambdaComponents(s.jacobian, s.drift, s.restitutionCoefficient) if bounceLambda < 0.0 { return // moving away } From 4867c1c8c1a8900857e4c819c6fe27c5b4ffca2a Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Tue, 4 Aug 2026 20:12:45 +0300 Subject: [PATCH 40/85] Add godoc to SoloCollisionSolver --- game/physics/constraint/collision.go | 104 -------------------------- game/physics/solver/context.go | 47 ------------ game/physics/solver_collision_solo.go | 77 ++++++++++++++++++- 3 files changed, 73 insertions(+), 155 deletions(-) diff --git a/game/physics/constraint/collision.go b/game/physics/constraint/collision.go index d2b7e110..4feeab49 100644 --- a/game/physics/constraint/collision.go +++ b/game/physics/constraint/collision.go @@ -1,109 +1,5 @@ package constraint -// import ( -// "github.com/mokiat/gomath/dprec" -// "github.com/mokiat/lacking/game/physics/solver" -// ) - -// type CollisionState struct { -// PropFrictionCoefficient float64 -// PropRestitutionCoefficient float64 - -// BodyNormal dprec.Vec3 -// BodyPoint dprec.Vec3 -// BodyFrictionCoefficient float64 -// BodyRestitutionCoefficient float64 - -// Depth float64 -// } - -// var _ solver.Constraint = (*Collision)(nil) - -// type Collision struct { -// propFrictionCoefficient float64 -// propRestitutionCoefficient float64 - -// bodyCollisionNormal dprec.Vec3 -// bodyCollisionPoint dprec.Vec3 -// bodyFrictionCoefficient float64 -// bodyRestitutionCoefficient float64 - -// collisionDepth float64 - -// radius dprec.Vec3 -// jacobian solver.Jacobian -// drift float64 -// } - -// func (s *Collision) Init(state CollisionState) { -// s.propFrictionCoefficient = state.PropFrictionCoefficient -// s.propRestitutionCoefficient = state.PropRestitutionCoefficient - -// s.bodyCollisionNormal = state.BodyNormal -// s.bodyCollisionPoint = state.BodyPoint -// s.bodyFrictionCoefficient = state.BodyFrictionCoefficient -// s.bodyRestitutionCoefficient = state.BodyRestitutionCoefficient - -// s.collisionDepth = state.Depth -// } - -// func (s *Collision) Reset(ctx solver.Context) { -// radiusWS := dprec.Vec3Diff(s.bodyCollisionPoint, ctx.Target.Position()) -// s.radius = dprec.QuatVec3Rotation(dprec.ConjugateQuat(ctx.Target.Rotation()), radiusWS) -// s.jacobian = solver.Jacobian{ -// LinearSlope: dprec.InverseVec3(s.bodyCollisionNormal), -// AngularSlope: dprec.Vec3Cross(s.bodyCollisionNormal, radiusWS), -// } -// s.drift = s.collisionDepth -// } - -// func (s *Collision) ApplyImpulses(ctx solver.Context) { -// // NOTE: We include the bounce force in the max friction calculation. -// // This might actually be accurate, since you have both the force of -// // the object pushing down, as well as the elastic force pushing further -// // down, trying to bounce the object up. -// restitution := s.propRestitutionCoefficient * s.bodyRestitutionCoefficient - -// // Bounce solution -// pressureLambda := ctx.JacobianImpulseLambda(s.jacobian, 0.0, restitution) -// if pressureLambda > 0 { -// return // moving away -// } -// bounceSolution := ctx.JacobianImpulseSolution(s.jacobian, s.collisionDepth, restitution) - -// // Friction solution -// radiusWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.radius) -// pointVelocity := dprec.Vec3Sum(ctx.Target.LinearVelocity(), dprec.Vec3Cross(ctx.Target.AngularVelocity(), radiusWS)) -// verticalVelocity := dprec.Vec3Prod(s.bodyCollisionNormal, dprec.Vec3Dot(s.bodyCollisionNormal, pointVelocity)) -// lateralVelocity := dprec.Vec3Diff(pointVelocity, verticalVelocity) -// frictionSolution := solver.Impulse{} -// if lng := lateralVelocity.Length(); lng > solver.Epsilon { -// lateralDirection := dprec.UnitVec3(lateralVelocity) -// frictionJacobian := solver.Jacobian{ -// LinearSlope: lateralDirection, -// AngularSlope: dprec.Vec3Cross(radiusWS, lateralDirection), -// } -// frictionLambda := ctx.JacobianImpulseLambda(frictionJacobian, 0.0, 0.0) -// // TODO: Have friction coefficient configurable -// // const frictionCoefficient = 0.9 // around 0.7 to 0.9 is realistic for dry asphalt -// const frictionCoefficient = 1.2 -// maxFrictionLambda := pressureLambda * frictionCoefficient -// if -frictionLambda > -maxFrictionLambda { -// frictionLambda = maxFrictionLambda -// } -// frictionSolution = frictionJacobian.Impulse(frictionLambda) -// } - -// // Note: Make sure to apply these as late as possible, otherwise you are -// // introducing noise that is picked up by subsequent calculations. -// ctx.Target.ApplyImpulse(bounceSolution) -// ctx.Target.ApplyImpulse(frictionSolution) -// } - -// func (s *Collision) ApplyNudges(ctx solver.Context) { -// // TODO: Add nudge solution -// } - // type PairCollisionState struct { // PrimaryNormal dprec.Vec3 // PrimaryPoint dprec.Vec3 diff --git a/game/physics/solver/context.go b/game/physics/solver/context.go index ffa7f363..cb7b335c 100644 --- a/game/physics/solver/context.go +++ b/game/physics/solver/context.go @@ -1,52 +1,5 @@ package solver -// // Context contains information related to single-object constraint -// // processing. -// type Context struct { -// DeltaTime float64 -// ImpulseBeta float64 -// NudgeBeta float64 - -// Target *Placeholder -// } - -// // JacobianImpulseLambda returns the impulse lambda for the specified -// // constraint Jacobian, positional drift and restitution. -// func (c Context) JacobianImpulseLambda(jacobian Jacobian, drift, restitution float64) float64 { -// effMass := jacobian.InverseEffectiveMass(c.Target) -// if effMass < Epsilon { -// return 0.0 -// } -// effVelocity := jacobian.EffectiveVelocity(c.Target) -// restitutionClamp := RestitutionClamp(effVelocity) -// baumgarte := c.ImpulseBeta * drift / c.DeltaTime -// return -((1+restitution*restitutionClamp)*effVelocity + baumgarte) / effMass -// } - -// // JacobianNudgeLambda returns the nudge lambda for the specified -// // constraint Jacobian and positional drift. -// func (c Context) JacobianNudgeLambda(jacobian Jacobian, drift float64) float64 { -// effMass := jacobian.InverseEffectiveMass(c.Target) -// if effMass < Epsilon { -// return 0.0 -// } -// return -c.NudgeBeta * drift / effMass -// } - -// // JacobianImpulseSolution returns an impulse solution based on the specified -// // constraint Jacobian, positional drift and restitution. -// func (c Context) JacobianImpulseSolution(jacobian Jacobian, drift, restitution float64) Impulse { -// lambda := c.JacobianImpulseLambda(jacobian, drift, restitution) -// return jacobian.Impulse(lambda) -// } - -// // JacobianNudgeSolution returns a nudge solution based on the specified -// // constraint Jacobian and positional drift. -// func (c Context) JacobianNudgeSolution(jacobian Jacobian, drift float64) Nudge { -// lambda := c.JacobianNudgeLambda(jacobian, drift) -// return jacobian.Nudge(lambda) -// } - // // PairContext contains information related to double-object constraint // // processing. // type PairContext struct { diff --git a/game/physics/solver_collision_solo.go b/game/physics/solver_collision_solo.go index e1a34964..503274a8 100644 --- a/game/physics/solver_collision_solo.go +++ b/game/physics/solver_collision_solo.go @@ -2,18 +2,60 @@ package physics import "github.com/mokiat/gomath/dprec" +// SoloCollisionSolverConfig holds the parameters with which a +// [SoloCollisionSolver] is configured through [SoloCollisionSolver.Init]. +// +// It describes a single contact between a body and static terrain - +// the contact geometry (normals, point, penetration depth) as well as +// the per-surface material properties (friction, restitution) that are +// combined into the solver's effective coefficients. type SoloCollisionSolverConfig struct { - TerrainFrictionCoefficient float64 + + // TerrainFrictionCoefficient is the friction coefficient of the + // terrain surface at the contact point. + TerrainFrictionCoefficient float64 + + // TerrainRestitutionCoefficient is the restitution (bounciness) + // coefficient of the terrain surface at the contact point. TerrainRestitutionCoefficient float64 - TerrainContactNormal dprec.Vec3 - BodyFrictionCoefficient float64 + // TerrainContactNormal is the unit-length surface normal of the + // terrain at the contact point, expressed in world space and + // pointing away from the terrain (i.e. towards the body). + TerrainContactNormal dprec.Vec3 + + // BodyFrictionCoefficient is the friction coefficient of the body's + // surface at the contact point. + BodyFrictionCoefficient float64 + + // BodyRestitutionCoefficient is the restitution (bounciness) + // coefficient of the body's surface at the contact point. BodyRestitutionCoefficient float64 - BodyContactPoint dprec.Vec3 + // BodyContactPoint is the position, in world space, of the point on + // the body's surface where the contact occurs. + BodyContactPoint dprec.Vec3 + + // ContactDepth is the penetration depth between the body and the + // terrain, as measured along TerrainContactNormal at the moment the + // contact was detected. It is expected to be positive while the two + // are overlapping. ContactDepth float64 } +// SoloCollisionSolver is a [SoloConstraintSolver] that resolves a single +// contact between a body and static terrain. +// +// Through [SoloCollisionSolver.ApplyImpulses] it applies a normal +// ("bounce") impulse - which prevents the body from moving further into +// the terrain and accounts for restitution and positional-drift +// stabilization - together with a Coulomb friction impulse bounded by +// that normal impulse. Through [SoloCollisionSolver.ApplyNudges] it +// separately corrects any remaining penetration at the position level. +// +// A SoloCollisionSolver must be configured through +// [SoloCollisionSolver.Init] before being registered with a [Scene] +// through [SoloConstraintView.Create]. type SoloCollisionSolver struct { terrainContactNormal dprec.Vec3 bodyContactPoint dprec.Vec3 @@ -29,6 +71,14 @@ type SoloCollisionSolver struct { var _ SoloConstraintSolver = (*SoloCollisionSolver)(nil) +// Init configures this solver according to config. +// +// The body's and terrain's friction coefficients are combined into a +// single coefficient through their geometric mean, and their restitution +// coefficients through their maximum. +// +// Init must be called once, before this solver is registered with a +// [Scene] through [SoloConstraintView.Create]. func (s *SoloCollisionSolver) Init(config SoloCollisionSolverConfig) { s.terrainContactNormal = config.TerrainContactNormal s.bodyContactPoint = config.BodyContactPoint @@ -38,6 +88,11 @@ func (s *SoloCollisionSolver) Init(config SoloCollisionSolverConfig) { s.restitutionCoefficient = max(config.BodyRestitutionCoefficient, config.TerrainRestitutionCoefficient) } +// Reset implements [SoloConstraintSolver.Reset]. +// +// It recomputes the contact's [Jacobian], along with the world-space +// offset from the target's center of mass to the contact point that it +// is derived from, based on the target's current position. func (s *SoloCollisionSolver) Reset(ctx SoloConstraintContext) { s.pointOffsetWS = dprec.Vec3Diff(s.bodyContactPoint, ctx.Target.Position()) s.jacobian = Jacobian{ @@ -47,6 +102,16 @@ func (s *SoloCollisionSolver) Reset(ctx SoloConstraintContext) { s.drift = s.contactDepth } +// ApplyImpulses implements [SoloConstraintSolver.ApplyImpulses]. +// +// It first resolves the contact's normal impulse, combining restitution +// with Baumgarte positional-drift stabilization. If the target is +// already moving away from the terrain, it returns without applying +// anything, leaving any remaining penetration to +// [SoloCollisionSolver.ApplyNudges]. Otherwise, it additionally resolves +// a Coulomb friction impulse that opposes the target's lateral +// (tangential) velocity at the contact point, clamped to a fraction of +// the normal impulse's restitution-only component. func (s *SoloCollisionSolver) ApplyImpulses(ctx SoloConstraintContext) { // Bounce solution bounceLambda, baumgarteLambda := ctx.ImpulseLambdaComponents(s.jacobian, s.drift, s.restitutionCoefficient) @@ -77,6 +142,10 @@ func (s *SoloCollisionSolver) ApplyImpulses(ctx SoloConstraintContext) { ctx.Target.ApplyImpulse(frictionSolution) } +// ApplyNudges implements [SoloConstraintSolver.ApplyNudges]. +// +// If the contact is still penetrating, it nudges the target along the +// terrain's contact normal to reduce the penetration. func (s *SoloCollisionSolver) ApplyNudges(ctx SoloConstraintContext) { if s.drift > 0.0 { nudge := ctx.NudgeSolution(s.jacobian, s.drift) From ef27d84175bcb9195e6c138f11c286d799270f29 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Tue, 4 Aug 2026 22:08:17 +0300 Subject: [PATCH 41/85] Implement pair collision solver --- game/physics/constraint/collision.go | 75 ------------------------ game/physics/constraint_pair.go | 40 +++++++++++++ game/physics/constraint_solo.go | 4 +- game/physics/solver/change.go | 11 ---- game/physics/solver/context.go | 49 ---------------- game/physics/solver/jacobian.go | 50 ---------------- game/physics/solver_collision_pair.go | 82 +++++++++++++++++++++++++-- game/physics/solver_collision_solo.go | 14 +++-- 8 files changed, 128 insertions(+), 197 deletions(-) delete mode 100644 game/physics/solver/change.go delete mode 100644 game/physics/solver/context.go delete mode 100644 game/physics/solver/jacobian.go diff --git a/game/physics/constraint/collision.go b/game/physics/constraint/collision.go index 4feeab49..8bbbe9e8 100644 --- a/game/physics/constraint/collision.go +++ b/game/physics/constraint/collision.go @@ -1,77 +1,6 @@ package constraint -// type PairCollisionState struct { -// PrimaryNormal dprec.Vec3 -// PrimaryPoint dprec.Vec3 -// PrimaryFrictionCoefficient float64 -// PrimaryRestitutionCoefficient float64 - -// SecondaryNormal dprec.Vec3 -// SecondaryPoint dprec.Vec3 -// SecondaryFrictionCoefficient float64 -// SecondaryRestitutionCoefficient float64 - -// Depth float64 -// } - -// var _ solver.PairConstraint = (*PairCollision)(nil) - -// type PairCollision struct { -// primaryCollisionNormal dprec.Vec3 -// primaryCollisionPoint dprec.Vec3 -// primaryFrictionCoefficient float64 -// primaryRestitutionCoefficient float64 - -// secondaryCollisionNormal dprec.Vec3 -// secondaryCollisionPoint dprec.Vec3 -// secondaryFrictionCoefficient float64 -// secondaryRestitutionCoefficient float64 - -// collisionDepth float64 - -// primaryRadius dprec.Vec3 -// secondaryRadius dprec.Vec3 -// jacobian solver.PairJacobian -// } - -// func (s *PairCollision) Init(state PairCollisionState) { -// s.primaryCollisionNormal = state.PrimaryNormal -// s.primaryCollisionPoint = state.PrimaryPoint -// s.primaryFrictionCoefficient = state.PrimaryFrictionCoefficient -// s.primaryRestitutionCoefficient = state.PrimaryRestitutionCoefficient - -// s.secondaryCollisionNormal = state.SecondaryNormal -// s.secondaryCollisionPoint = state.SecondaryPoint -// s.secondaryFrictionCoefficient = state.SecondaryFrictionCoefficient -// s.secondaryRestitutionCoefficient = state.SecondaryRestitutionCoefficient - -// s.collisionDepth = state.Depth -// } - -// func (s *PairCollision) Reset(ctx solver.PairContext) { -// primaryRadiusWS := dprec.Vec3Diff(s.primaryCollisionPoint, ctx.Target.Position()) -// s.primaryRadius = dprec.QuatVec3Rotation(dprec.ConjugateQuat(ctx.Target.Rotation()), primaryRadiusWS) -// secondaryRadiusWS := dprec.Vec3Diff(s.secondaryCollisionPoint, ctx.Source.Position()) -// s.secondaryRadius = dprec.QuatVec3Rotation(dprec.ConjugateQuat(ctx.Source.Rotation()), secondaryRadiusWS) -// s.jacobian = solver.PairJacobian{ -// Target: solver.Jacobian{ -// LinearSlope: dprec.InverseVec3(s.primaryCollisionNormal), -// AngularSlope: dprec.Vec3Cross(s.primaryCollisionNormal, primaryRadiusWS), -// }, -// Source: solver.Jacobian{ -// LinearSlope: dprec.InverseVec3(s.secondaryCollisionNormal), -// AngularSlope: dprec.Vec3Cross(s.secondaryCollisionNormal, secondaryRadiusWS), -// }, -// } -// } - // func (s *PairCollision) ApplyImpulses(ctx solver.PairContext) { -// // NOTE: We include the bounce force in the max friction calculation. -// // This might actually be accurate, since you have both the force of -// // the object pushing down, as well as the elastic force pushing further -// // down, trying to bounce the object up. -// restitution := s.primaryRestitutionCoefficient * s.secondaryRestitutionCoefficient - // // Bounce solution // pressureLambda := ctx.JacobianImpulseLambda(s.jacobian, 0.0, restitution) // if pressureLambda > 0 { @@ -117,7 +46,3 @@ package constraint // ctx.Target.ApplyImpulse(frictionSolution.Target) // ctx.Source.ApplyImpulse(frictionSolution.Source) // } - -// func (s *PairCollision) ApplyNudges(ctx solver.PairContext) { -// // TODO: Add nudge solution -// } diff --git a/game/physics/constraint_pair.go b/game/physics/constraint_pair.go index 291d946c..5de0a2ea 100644 --- a/game/physics/constraint_pair.go +++ b/game/physics/constraint_pair.go @@ -28,6 +28,46 @@ type PairConstraintContext struct { SecondaryTarget ConstraintTarget } +func (c PairConstraintContext) ImpulseLambda(primaryJacobian, secondaryJacobian Jacobian, drift, restitutionCoef float64) float64 { + invEffMass := primaryJacobian.InverseEffectiveMass(c.PrimaryTarget) + secondaryJacobian.InverseEffectiveMass(c.SecondaryTarget) + if invEffMass < Epsilon { + return 0.0 + } + effVelocity := primaryJacobian.EffectiveVelocity(c.PrimaryTarget) + secondaryJacobian.EffectiveVelocity(c.SecondaryTarget) + restitution := 1 + restitutionCoef*RestitutionClamp(effVelocity) + driftBias := c.ImpulseBeta * drift / c.DeltaSeconds + return -(restitution*effVelocity - driftBias) / invEffMass +} + +func (c PairConstraintContext) ImpulseLambdaComponents(primaryJacobian, secondaryJacobian Jacobian, drift, restitutionCoef float64) (bounce, baumgarte float64) { + invEffMass := primaryJacobian.InverseEffectiveMass(c.PrimaryTarget) + secondaryJacobian.InverseEffectiveMass(c.SecondaryTarget) + if invEffMass < Epsilon { + return 0.0, 0.0 + } + effVelocity := primaryJacobian.EffectiveVelocity(c.PrimaryTarget) + secondaryJacobian.EffectiveVelocity(c.SecondaryTarget) + restitution := 1 + restitutionCoef*RestitutionClamp(effVelocity) + driftBias := c.ImpulseBeta * drift / c.DeltaSeconds + return -restitution * effVelocity / invEffMass, driftBias / invEffMass +} + +func (c PairConstraintContext) ImpulseSolution(primaryJacobian, secondaryJacobian Jacobian, drift, restitutionCoef float64) (Impulse, Impulse) { + lambda := c.ImpulseLambda(primaryJacobian, secondaryJacobian, drift, restitutionCoef) + return primaryJacobian.Impulse(lambda), secondaryJacobian.Impulse(lambda) +} + +func (c PairConstraintContext) NudgeLambda(primaryJacobian, secondaryJacobian Jacobian, drift float64) float64 { + invEffMass := primaryJacobian.InverseEffectiveMass(c.PrimaryTarget) + secondaryJacobian.InverseEffectiveMass(c.SecondaryTarget) + if invEffMass < Epsilon { + return 0.0 + } + return c.NudgeBeta * drift / invEffMass +} + +func (c PairConstraintContext) NudgeSolution(primaryJacobian, secondaryJacobian Jacobian, drift float64) (Nudge, Nudge) { + lambda := c.NudgeLambda(primaryJacobian, secondaryJacobian, drift) + return primaryJacobian.Nudge(lambda), secondaryJacobian.Nudge(lambda) +} + // PairConstraintSolver implements the mathematical logic that enforces a // constraint acting on two bodies simultaneously. // diff --git a/game/physics/constraint_solo.go b/game/physics/constraint_solo.go index 071e6fd3..2fd6ca99 100644 --- a/game/physics/constraint_solo.go +++ b/game/physics/constraint_solo.go @@ -40,8 +40,8 @@ func (c SoloConstraintContext) ImpulseLambda(jacobian Jacobian, drift, restituti } effVelocity := jacobian.EffectiveVelocity(c.Target) restitution := 1 + restitutionCoef*RestitutionClamp(effVelocity) - baumgarte := c.ImpulseBeta * drift / c.DeltaSeconds - return -(restitution*effVelocity - baumgarte) / invEffMass + driftBias := c.ImpulseBeta * drift / c.DeltaSeconds + return -(restitution*effVelocity - driftBias) / invEffMass } // ImpulseLambdaComponents behaves like [SoloConstraintContext.ImpulseLambda] diff --git a/game/physics/solver/change.go b/game/physics/solver/change.go deleted file mode 100644 index f2d45e55..00000000 --- a/game/physics/solver/change.go +++ /dev/null @@ -1,11 +0,0 @@ -package solver - -// type PairImpulse struct { -// Target Impulse -// Source Impulse -// } - -// type PairNudge struct { -// Target Nudge -// Source Nudge -// } diff --git a/game/physics/solver/context.go b/game/physics/solver/context.go deleted file mode 100644 index cb7b335c..00000000 --- a/game/physics/solver/context.go +++ /dev/null @@ -1,49 +0,0 @@ -package solver - -// // PairContext contains information related to double-object constraint -// // processing. -// type PairContext struct { -// DeltaTime float64 -// ImpulseBeta float64 -// NudgeBeta float64 - -// Target *Placeholder -// Source *Placeholder -// } - -// // JacobianImpulseLambda returns the impulse lambda for the specified -// // constraint Jacobian, positional drift and restitution. -// func (c PairContext) JacobianImpulseLambda(jacobian PairJacobian, drift, restitution float64) float64 { -// effMass := jacobian.InverseEffectiveMass(c.Target, c.Source) -// if effMass < Epsilon { -// return 0.0 -// } -// effVelocity := jacobian.EffectiveVelocity(c.Target, c.Source) -// restitutionClamp := RestitutionClamp(effVelocity) -// baumgarte := c.ImpulseBeta * drift / c.DeltaTime -// return -((1+restitution*restitutionClamp)*effVelocity + baumgarte) / effMass -// } - -// // JacobianNudgeLambda returns the nudge lambda for the specified -// // constraint Jacobian and positional drift. -// func (c PairContext) JacobianNudgeLambda(jacobian PairJacobian, drift float64) float64 { -// effMass := jacobian.InverseEffectiveMass(c.Target, c.Source) -// if effMass < Epsilon { -// return 0.0 -// } -// return -c.NudgeBeta * drift / effMass -// } - -// // JacobianImpulseSolution returns an impulse solution based on the specified -// // constraint Jacobian, positional drift and restitution. -// func (c PairContext) JacobianImpulseSolution(jacobian PairJacobian, drift, restitution float64) PairImpulse { -// lambda := c.JacobianImpulseLambda(jacobian, drift, restitution) -// return jacobian.Impulse(lambda) -// } - -// // JacobianNudgeSolution returns a nudge solution based on the specified -// // constraint Jacobian and positional drift. -// func (c PairContext) JacobianNudgeSolution(jacobian PairJacobian, drift float64) PairNudge { -// lambda := c.JacobianNudgeLambda(jacobian, drift) -// return jacobian.Nudge(lambda) -// } diff --git a/game/physics/solver/jacobian.go b/game/physics/solver/jacobian.go deleted file mode 100644 index 076058a2..00000000 --- a/game/physics/solver/jacobian.go +++ /dev/null @@ -1,50 +0,0 @@ -package solver - -// // PairJacobian represents the 1x12 Jacobian matrix of a double-object velocity -// // constraint. -// type PairJacobian struct { -// Target Jacobian -// Source Jacobian -// } - -// // EffectiveVelocity returns the amount of the combined velocities of the two -// // objects that is going in the wrong direction. -// func (j PairJacobian) EffectiveVelocity(target, source *Placeholder) float64 { -// return j.Target.EffectiveVelocity(target) + j.Source.EffectiveVelocity(source) -// } - -// // InverseEffectiveMass returns the inverse of the effective mass with which -// // the two bodies affect the constraint. -// func (j PairJacobian) InverseEffectiveMass(target, source *Placeholder) float64 { -// return j.Target.InverseEffectiveMass(target) + j.Source.InverseEffectiveMass(source) -// } - -// // Impulse returns an impulse solution based on the lambda impulse -// // amount applied according to this Jacobian. -// func (j PairJacobian) Impulse(lambda float64) PairImpulse { -// return PairImpulse{ -// Target: Impulse{ -// Linear: dprec.Vec3Prod(j.Target.LinearSlope, lambda), -// Angular: dprec.Vec3Prod(j.Target.AngularSlope, lambda), -// }, -// Source: Impulse{ -// Linear: dprec.Vec3Prod(j.Source.LinearSlope, lambda), -// Angular: dprec.Vec3Prod(j.Source.AngularSlope, lambda), -// }, -// } -// } - -// // Nudge returns a nudge solution based on the lambda nudge amount -// // applied according to this Jacobian. -// func (j PairJacobian) Nudge(lambda float64) PairNudge { -// return PairNudge{ -// Target: Nudge{ -// Linear: dprec.Vec3Prod(j.Target.LinearSlope, lambda), -// Angular: dprec.Vec3Prod(j.Target.AngularSlope, lambda), -// }, -// Source: Nudge{ -// Linear: dprec.Vec3Prod(j.Source.LinearSlope, lambda), -// Angular: dprec.Vec3Prod(j.Source.AngularSlope, lambda), -// }, -// } -// } diff --git a/game/physics/solver_collision_pair.go b/game/physics/solver_collision_pair.go index 3b13a262..854f3f06 100644 --- a/game/physics/solver_collision_pair.go +++ b/game/physics/solver_collision_pair.go @@ -16,22 +16,96 @@ type PairCollisionSolverConfig struct { ContactDepth float64 } -type PairCollisionSolver struct{} +type PairCollisionSolver struct { + primaryContactNormal dprec.Vec3 + primaryContactPoint dprec.Vec3 + secondaryContactNormal dprec.Vec3 + secondaryContactPoint dprec.Vec3 + contactDepth float64 + + frictionCoefficient float64 + restitutionCoefficient float64 + + primaryPointOffsetWS dprec.Vec3 + secondaryPointOffsetWS dprec.Vec3 + primaryJacobian Jacobian + secondaryJacobian Jacobian + drift float64 +} var _ PairConstraintSolver = (*PairCollisionSolver)(nil) func (s *PairCollisionSolver) Init(config PairCollisionSolverConfig) { + s.primaryContactNormal = config.PrimaryContactNormal + s.primaryContactPoint = config.PrimaryContactPoint + s.secondaryContactNormal = config.SecondaryContactNormal + s.secondaryContactPoint = config.SecondaryContactPoint + s.contactDepth = config.ContactDepth + s.frictionCoefficient = dprec.Sqrt(config.PrimaryFrictionCoefficient * config.SecondaryFrictionCoefficient) + s.restitutionCoefficient = max(config.PrimaryRestitutionCoefficient, config.SecondaryRestitutionCoefficient) } func (s *PairCollisionSolver) Reset(ctx PairConstraintContext) { - // TODO + s.primaryPointOffsetWS = dprec.Vec3Diff(s.primaryContactPoint, ctx.PrimaryTarget.Position()) + s.secondaryPointOffsetWS = dprec.Vec3Diff(s.secondaryContactPoint, ctx.SecondaryTarget.Position()) + + s.primaryJacobian = Jacobian{ + LinearSlope: s.secondaryContactNormal, + AngularSlope: dprec.Vec3Cross(s.primaryPointOffsetWS, s.secondaryContactNormal), + } + s.secondaryJacobian = Jacobian{ + LinearSlope: s.primaryContactNormal, + AngularSlope: dprec.Vec3Cross(s.secondaryPointOffsetWS, s.primaryContactNormal), + } + + s.drift = s.contactDepth } func (s *PairCollisionSolver) ApplyImpulses(ctx PairConstraintContext) { - // TODO + // Bounce solution + bounceLambda, baumgarteLambda := ctx.ImpulseLambdaComponents(s.primaryJacobian, s.secondaryJacobian, s.drift, s.restitutionCoefficient) + if bounceLambda < 0.0 { + return // moving away + } + primaryBounceImpulse := s.primaryJacobian.Impulse(bounceLambda + baumgarteLambda) + secondaryBounceImpulse := s.secondaryJacobian.Impulse(bounceLambda + baumgarteLambda) + + // Friction solution + primaryPointVelocity := dprec.Vec3Sum(ctx.PrimaryTarget.LinearVelocity(), dprec.Vec3Cross(ctx.PrimaryTarget.AngularVelocity(), s.primaryPointOffsetWS)) + secondaryPointVelocity := dprec.Vec3Sum(ctx.SecondaryTarget.LinearVelocity(), dprec.Vec3Cross(ctx.SecondaryTarget.AngularVelocity(), s.secondaryPointOffsetWS)) + deltaPointVelocity := dprec.Vec3Diff(primaryPointVelocity, secondaryPointVelocity) + pointsLateralVelocity := dprec.Vec3Projection(deltaPointVelocity, s.secondaryContactNormal) + var primaryFrictionImpulse, secondaryFrictionImpulse Impulse + if lng := pointsLateralVelocity.Length(); lng > Epsilon { + velocityLateralDirection := dprec.UnitVec3(pointsLateralVelocity) + primaryFrictionJacobian := Jacobian{ + LinearSlope: dprec.InverseVec3(velocityLateralDirection), + AngularSlope: dprec.Vec3Cross(velocityLateralDirection, s.primaryPointOffsetWS), + } + secondaryFrictionJacobian := Jacobian{ + LinearSlope: velocityLateralDirection, + AngularSlope: dprec.Vec3Cross(s.secondaryPointOffsetWS, velocityLateralDirection), + } + frictionLambda := ctx.ImpulseLambda(primaryFrictionJacobian, secondaryFrictionJacobian, 0.0, 0.0) + maxFrictionLambda := bounceLambda * s.frictionCoefficient + frictionLambda = min(frictionLambda, maxFrictionLambda) + primaryFrictionImpulse = primaryFrictionJacobian.Impulse(frictionLambda) + secondaryFrictionImpulse = secondaryFrictionJacobian.Impulse(frictionLambda) + } + + // Note: Make sure to apply these as late as possible, otherwise you are + // introducing noise that is picked up by friction calculations. + ctx.PrimaryTarget.ApplyImpulse(primaryBounceImpulse) + ctx.SecondaryTarget.ApplyImpulse(secondaryBounceImpulse) + ctx.PrimaryTarget.ApplyImpulse(primaryFrictionImpulse) + ctx.SecondaryTarget.ApplyImpulse(secondaryFrictionImpulse) } func (s *PairCollisionSolver) ApplyNudges(ctx PairConstraintContext) { - // TODO + if s.drift > 0.0 { + primaryNudge, secondaryNudge := ctx.NudgeSolution(s.primaryJacobian, s.secondaryJacobian, s.drift) + ctx.PrimaryTarget.ApplyNudge(primaryNudge) + ctx.SecondaryTarget.ApplyNudge(secondaryNudge) + } } diff --git a/game/physics/solver_collision_solo.go b/game/physics/solver_collision_solo.go index 503274a8..26244535 100644 --- a/game/physics/solver_collision_solo.go +++ b/game/physics/solver_collision_solo.go @@ -95,10 +95,12 @@ func (s *SoloCollisionSolver) Init(config SoloCollisionSolverConfig) { // is derived from, based on the target's current position. func (s *SoloCollisionSolver) Reset(ctx SoloConstraintContext) { s.pointOffsetWS = dprec.Vec3Diff(s.bodyContactPoint, ctx.Target.Position()) + s.jacobian = Jacobian{ LinearSlope: s.terrainContactNormal, AngularSlope: dprec.Vec3Cross(s.pointOffsetWS, s.terrainContactNormal), } + s.drift = s.contactDepth } @@ -123,23 +125,23 @@ func (s *SoloCollisionSolver) ApplyImpulses(ctx SoloConstraintContext) { // Friction solution pointVelocity := dprec.Vec3Sum(ctx.Target.LinearVelocity(), dprec.Vec3Cross(ctx.Target.AngularVelocity(), s.pointOffsetWS)) pointLateralVelocity := dprec.Vec3Projection(pointVelocity, s.terrainContactNormal) - var frictionSolution Impulse + var frictionImpulse Impulse if lng := pointLateralVelocity.Length(); lng > Epsilon { - pointLateralDirection := dprec.UnitVec3(pointLateralVelocity) + velocityLateralDirection := dprec.UnitVec3(pointLateralVelocity) frictionJacobian := Jacobian{ - LinearSlope: dprec.InverseVec3(pointLateralDirection), - AngularSlope: dprec.Vec3Cross(pointLateralDirection, s.pointOffsetWS), + LinearSlope: dprec.InverseVec3(velocityLateralDirection), + AngularSlope: dprec.Vec3Cross(velocityLateralDirection, s.pointOffsetWS), } frictionLambda := ctx.ImpulseLambda(frictionJacobian, 0.0, 0.0) maxFrictionLambda := bounceLambda * s.frictionCoefficient frictionLambda = min(frictionLambda, maxFrictionLambda) - frictionSolution = frictionJacobian.Impulse(frictionLambda) + frictionImpulse = frictionJacobian.Impulse(frictionLambda) } // Note: Make sure to apply these as late as possible, otherwise you are // introducing noise that is picked up by friction calculations. ctx.Target.ApplyImpulse(bounceImpulse) - ctx.Target.ApplyImpulse(frictionSolution) + ctx.Target.ApplyImpulse(frictionImpulse) } // ApplyNudges implements [SoloConstraintSolver.ApplyNudges]. From 91f57a0b7f49bc42342b5de27d4e1ae9a75fd572 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Tue, 4 Aug 2026 22:18:24 +0300 Subject: [PATCH 42/85] Add godoc and remove old file --- game/physics/constraint/collision.go | 48 ------------ game/physics/constraint_pair.go | 59 +++++++++++++++ game/physics/solver_collision_pair.go | 103 ++++++++++++++++++++++++-- 3 files changed, 156 insertions(+), 54 deletions(-) delete mode 100644 game/physics/constraint/collision.go diff --git a/game/physics/constraint/collision.go b/game/physics/constraint/collision.go deleted file mode 100644 index 8bbbe9e8..00000000 --- a/game/physics/constraint/collision.go +++ /dev/null @@ -1,48 +0,0 @@ -package constraint - -// func (s *PairCollision) ApplyImpulses(ctx solver.PairContext) { -// // Bounce solution -// pressureLambda := ctx.JacobianImpulseLambda(s.jacobian, 0.0, restitution) -// if pressureLambda > 0 { -// return // moving away -// } -// bounceSolution := ctx.JacobianImpulseSolution(s.jacobian, s.collisionDepth, restitution) - -// // Friction solution -// primaryRadiusWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.primaryRadius) -// primaryPointVelocity := dprec.Vec3Sum(ctx.Target.LinearVelocity(), dprec.Vec3Cross(ctx.Target.AngularVelocity(), primaryRadiusWS)) -// secondaryRadiusWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.secondaryRadius) -// secondaryPointVelocity := dprec.Vec3Sum(ctx.Source.LinearVelocity(), dprec.Vec3Cross(ctx.Source.AngularVelocity(), secondaryRadiusWS)) -// deltaPointVelocity := dprec.Vec3Diff(primaryPointVelocity, secondaryPointVelocity) -// verticalVelocity := dprec.Vec3Prod(s.secondaryCollisionNormal, dprec.Vec3Dot(s.secondaryCollisionNormal, deltaPointVelocity)) -// lateralVelocity := dprec.Vec3Diff(deltaPointVelocity, verticalVelocity) -// frictionSolution := solver.PairImpulse{} -// if lng := lateralVelocity.Length(); lng > solver.Epsilon { -// lateralDirection := dprec.UnitVec3(lateralVelocity) -// frictionJacobian := solver.PairJacobian{ -// Target: solver.Jacobian{ -// LinearSlope: lateralDirection, -// AngularSlope: dprec.Vec3Cross(primaryRadiusWS, lateralDirection), -// }, -// Source: solver.Jacobian{ -// LinearSlope: dprec.InverseVec3(lateralDirection), -// AngularSlope: dprec.Vec3Cross(lateralDirection, secondaryRadiusWS), -// }, -// } -// frictionLambda := ctx.JacobianImpulseLambda(frictionJacobian, 0.0, 0.0) -// // TODO: Have friction coefficient configurable -// const frictionCoefficient = 0.9 // around 0.7 to 0.9 is realistic for dry asphalt -// maxFrictionLambda := pressureLambda * frictionCoefficient -// if -frictionLambda > -maxFrictionLambda { -// frictionLambda = maxFrictionLambda -// } -// frictionSolution = frictionJacobian.Impulse(frictionLambda) -// } - -// // Note: Make sure to apply these as late as possible, otherwise you are -// // introducing noise that is picked up by subsequent calculations. -// ctx.Target.ApplyImpulse(bounceSolution.Target) -// ctx.Source.ApplyImpulse(bounceSolution.Source) -// ctx.Target.ApplyImpulse(frictionSolution.Target) -// ctx.Source.ApplyImpulse(frictionSolution.Source) -// } diff --git a/game/physics/constraint_pair.go b/game/physics/constraint_pair.go index 5de0a2ea..b58aa94b 100644 --- a/game/physics/constraint_pair.go +++ b/game/physics/constraint_pair.go @@ -28,6 +28,26 @@ type PairConstraintContext struct { SecondaryTarget ConstraintTarget } +// ImpulseLambda returns the impulse magnitude (lambda) that needs to be +// applied along primaryJacobian to [PairConstraintContext.PrimaryTarget] +// and along secondaryJacobian to +// [PairConstraintContext.SecondaryTarget], in order to correct their +// combined velocity error, taking into account both restitution and +// Baumgarte positional-drift stabilization. +// +// primaryJacobian and secondaryJacobian must describe the same scalar +// constraint from each body's own point of view - typically with +// secondaryJacobian's slopes being the negation of primaryJacobian's - +// so that applying the same lambda to both targets, as +// [PairConstraintContext.ImpulseSolution] does, is consistent with +// Newton's third law. +// +// drift is the current positional error of the constraint (e.g. a +// penetration depth), and restitutionCoef is the coefficient of +// restitution to apply, in the [0.0, 1.0] range. +// +// The result is intended to be passed to [Jacobian.Impulse] for each of +// primaryJacobian and secondaryJacobian. func (c PairConstraintContext) ImpulseLambda(primaryJacobian, secondaryJacobian Jacobian, drift, restitutionCoef float64) float64 { invEffMass := primaryJacobian.InverseEffectiveMass(c.PrimaryTarget) + secondaryJacobian.InverseEffectiveMass(c.SecondaryTarget) if invEffMass < Epsilon { @@ -39,6 +59,18 @@ func (c PairConstraintContext) ImpulseLambda(primaryJacobian, secondaryJacobian return -(restitution*effVelocity - driftBias) / invEffMass } +// ImpulseLambdaComponents behaves like +// [PairConstraintContext.ImpulseLambda] but returns its two additive +// components separately instead of their sum: bounce is the +// restitution-only component, derived purely from the two jacobians' +// current combined velocity error, while baumgarte is the +// positional-drift-correction component, derived from drift. +// +// bounce + baumgarte is equal to the value that +// [PairConstraintContext.ImpulseLambda] would return for the same +// arguments. Callers that need to reason about the velocity-only portion +// in isolation - for example to detect separating contacts, or to derive +// a friction bound from it - should use this method instead. func (c PairConstraintContext) ImpulseLambdaComponents(primaryJacobian, secondaryJacobian Jacobian, drift, restitutionCoef float64) (bounce, baumgarte float64) { invEffMass := primaryJacobian.InverseEffectiveMass(c.PrimaryTarget) + secondaryJacobian.InverseEffectiveMass(c.SecondaryTarget) if invEffMass < Epsilon { @@ -50,11 +82,32 @@ func (c PairConstraintContext) ImpulseLambdaComponents(primaryJacobian, secondar return -restitution * effVelocity / invEffMass, driftBias / invEffMass } +// ImpulseSolution returns the [Impulse] that needs to be applied to +// [PairConstraintContext.PrimaryTarget] along primaryJacobian, and the +// [Impulse] that needs to be applied to +// [PairConstraintContext.SecondaryTarget] along secondaryJacobian, in +// order to correct their combined velocity error, combining restitution +// and Baumgarte positional-drift stabilization. +// +// See [PairConstraintContext.ImpulseLambda] for details on +// primaryJacobian, secondaryJacobian, drift and restitutionCoef. func (c PairConstraintContext) ImpulseSolution(primaryJacobian, secondaryJacobian Jacobian, drift, restitutionCoef float64) (Impulse, Impulse) { lambda := c.ImpulseLambda(primaryJacobian, secondaryJacobian, drift, restitutionCoef) return primaryJacobian.Impulse(lambda), secondaryJacobian.Impulse(lambda) } +// NudgeLambda returns the nudge magnitude (lambda) that needs to be +// applied along primaryJacobian to [PairConstraintContext.PrimaryTarget] +// and along secondaryJacobian to [PairConstraintContext.SecondaryTarget], +// in order to correct drift, the current combined positional error of +// the constraint (e.g. a penetration depth), through Baumgarte +// positional-drift stabilization. +// +// See [PairConstraintContext.ImpulseLambda] for the expected relationship +// between primaryJacobian and secondaryJacobian. +// +// The result is intended to be passed to [Jacobian.Nudge] for each of +// primaryJacobian and secondaryJacobian. func (c PairConstraintContext) NudgeLambda(primaryJacobian, secondaryJacobian Jacobian, drift float64) float64 { invEffMass := primaryJacobian.InverseEffectiveMass(c.PrimaryTarget) + secondaryJacobian.InverseEffectiveMass(c.SecondaryTarget) if invEffMass < Epsilon { @@ -63,6 +116,12 @@ func (c PairConstraintContext) NudgeLambda(primaryJacobian, secondaryJacobian Ja return c.NudgeBeta * drift / invEffMass } +// NudgeSolution returns the [Nudge] that needs to be applied to +// [PairConstraintContext.PrimaryTarget] along primaryJacobian, and the +// [Nudge] that needs to be applied to +// [PairConstraintContext.SecondaryTarget] along secondaryJacobian, in +// order to correct drift, the current combined positional error of the +// constraint (e.g. a penetration depth). func (c PairConstraintContext) NudgeSolution(primaryJacobian, secondaryJacobian Jacobian, drift float64) (Nudge, Nudge) { lambda := c.NudgeLambda(primaryJacobian, secondaryJacobian, drift) return primaryJacobian.Nudge(lambda), secondaryJacobian.Nudge(lambda) diff --git a/game/physics/solver_collision_pair.go b/game/physics/solver_collision_pair.go index 854f3f06..2f0b55fd 100644 --- a/game/physics/solver_collision_pair.go +++ b/game/physics/solver_collision_pair.go @@ -2,20 +2,75 @@ package physics import "github.com/mokiat/gomath/dprec" +// PairCollisionSolverConfig holds the parameters with which a +// [PairCollisionSolver] is configured through [PairCollisionSolver.Init]. +// +// It describes a single contact between two dynamic bodies - the contact +// geometry (normals, points, penetration depth), each as seen from its +// own body's point of view, as well as the per-surface material +// properties (friction, restitution) that are combined into the +// solver's effective coefficients. type PairCollisionSolverConfig struct { - PrimaryFrictionCoefficient float64 + + // PrimaryFrictionCoefficient is the friction coefficient of the + // primary body's surface at the contact point. + PrimaryFrictionCoefficient float64 + + // PrimaryRestitutionCoefficient is the restitution (bounciness) + // coefficient of the primary body's surface at the contact point. PrimaryRestitutionCoefficient float64 - PrimaryContactNormal dprec.Vec3 - PrimaryContactPoint dprec.Vec3 - SecondaryFrictionCoefficient float64 + // PrimaryContactNormal is the unit-length surface normal of the + // primary body at the contact point, expressed in world space and + // pointing away from the primary body (i.e. towards the secondary + // body). It is expected to be approximately the negation of + // SecondaryContactNormal. + PrimaryContactNormal dprec.Vec3 + + // PrimaryContactPoint is the position, in world space, of the point + // on the primary body's surface where the contact occurs. + PrimaryContactPoint dprec.Vec3 + + // SecondaryFrictionCoefficient is the friction coefficient of the + // secondary body's surface at the contact point. + SecondaryFrictionCoefficient float64 + + // SecondaryRestitutionCoefficient is the restitution (bounciness) + // coefficient of the secondary body's surface at the contact point. SecondaryRestitutionCoefficient float64 - SecondaryContactNormal dprec.Vec3 - SecondaryContactPoint dprec.Vec3 + // SecondaryContactNormal is the unit-length surface normal of the + // secondary body at the contact point, expressed in world space and + // pointing away from the secondary body (i.e. towards the primary + // body). It is expected to be approximately the negation of + // PrimaryContactNormal. + SecondaryContactNormal dprec.Vec3 + + // SecondaryContactPoint is the position, in world space, of the + // point on the secondary body's surface where the contact occurs. + SecondaryContactPoint dprec.Vec3 + + // ContactDepth is the penetration depth between the primary and + // secondary bodies, as measured along the contact normals at the + // moment the contact was detected. It is expected to be positive + // while the two are overlapping. ContactDepth float64 } +// PairCollisionSolver is a [PairConstraintSolver] that resolves a single +// contact between two dynamic bodies. +// +// Through [PairCollisionSolver.ApplyImpulses] it applies a pair of +// normal ("bounce") impulses - which prevent the bodies from moving +// further into one another and account for restitution and +// positional-drift stabilization - together with a pair of Coulomb +// friction impulses bounded by that normal impulse. Through +// [PairCollisionSolver.ApplyNudges] it separately corrects any +// remaining penetration at the position level. +// +// A PairCollisionSolver must be configured through +// [PairCollisionSolver.Init] before being registered with a [Scene] +// through [PairConstraintView.Create]. type PairCollisionSolver struct { primaryContactNormal dprec.Vec3 primaryContactPoint dprec.Vec3 @@ -35,6 +90,14 @@ type PairCollisionSolver struct { var _ PairConstraintSolver = (*PairCollisionSolver)(nil) +// Init configures this solver according to config. +// +// The two bodies' friction coefficients are combined into a single +// coefficient through their geometric mean, and their restitution +// coefficients through their maximum. +// +// Init must be called once, before this solver is registered with a +// [Scene] through [PairConstraintView.Create]. func (s *PairCollisionSolver) Init(config PairCollisionSolverConfig) { s.primaryContactNormal = config.PrimaryContactNormal s.primaryContactPoint = config.PrimaryContactPoint @@ -46,6 +109,20 @@ func (s *PairCollisionSolver) Init(config PairCollisionSolverConfig) { s.restitutionCoefficient = max(config.PrimaryRestitutionCoefficient, config.SecondaryRestitutionCoefficient) } +// Reset implements [PairConstraintSolver.Reset]. +// +// It recomputes the contact's primary and secondary [Jacobian]s, along +// with the world-space offsets from each target's center of mass to its +// respective contact point that they are derived from, based on the +// targets' current positions. +// +// Each jacobian's linear slope is built from the other body's contact +// normal (e.g. the primary jacobian uses SecondaryContactNormal, not +// PrimaryContactNormal) rather than from an explicit negation, relying +// on the two normals being approximately antiparallel. This is what +// allows a single lambda, as computed by [PairConstraintContext], to be +// applied to both targets - see +// [PairConstraintContext.ImpulseLambda] for that requirement. func (s *PairCollisionSolver) Reset(ctx PairConstraintContext) { s.primaryPointOffsetWS = dprec.Vec3Diff(s.primaryContactPoint, ctx.PrimaryTarget.Position()) s.secondaryPointOffsetWS = dprec.Vec3Diff(s.secondaryContactPoint, ctx.SecondaryTarget.Position()) @@ -62,6 +139,16 @@ func (s *PairCollisionSolver) Reset(ctx PairConstraintContext) { s.drift = s.contactDepth } +// ApplyImpulses implements [PairConstraintSolver.ApplyImpulses]. +// +// It first resolves the contact's normal impulse pair, combining +// restitution with Baumgarte positional-drift stabilization. If the two +// targets are already moving apart, it returns without applying +// anything, leaving any remaining penetration to +// [PairCollisionSolver.ApplyNudges]. Otherwise, it additionally resolves +// a Coulomb friction impulse pair that opposes the targets' relative +// lateral (tangential) velocity at the contact, clamped to a fraction of +// the normal impulse's restitution-only component. func (s *PairCollisionSolver) ApplyImpulses(ctx PairConstraintContext) { // Bounce solution bounceLambda, baumgarteLambda := ctx.ImpulseLambdaComponents(s.primaryJacobian, s.secondaryJacobian, s.drift, s.restitutionCoefficient) @@ -102,6 +189,10 @@ func (s *PairCollisionSolver) ApplyImpulses(ctx PairConstraintContext) { ctx.SecondaryTarget.ApplyImpulse(secondaryFrictionImpulse) } +// ApplyNudges implements [PairConstraintSolver.ApplyNudges]. +// +// If the contact is still penetrating, it nudges the two targets apart +// along their contact normals to reduce the penetration. func (s *PairCollisionSolver) ApplyNudges(ctx PairConstraintContext) { if s.drift > 0.0 { primaryNudge, secondaryNudge := ctx.NudgeSolution(s.primaryJacobian, s.secondaryJacobian, s.drift) From 04422a85134f87295192ee579f7ad7d2aaa25193 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Wed, 5 Aug 2026 20:46:48 +0300 Subject: [PATCH 43/85] Add fixed position constraint solver --- game/physics/constraint/static_position.go | 44 ----------- game/physics/scene.go | 4 +- game/physics/solver_collision_pair.go | 28 +++++-- game/physics/solver_collision_solo.go | 28 +++++-- game/physics/solver_fixed_position.go | 87 ++++++++++++++++++++++ 5 files changed, 129 insertions(+), 62 deletions(-) delete mode 100644 game/physics/constraint/static_position.go create mode 100644 game/physics/solver_fixed_position.go diff --git a/game/physics/constraint/static_position.go b/game/physics/constraint/static_position.go deleted file mode 100644 index 206b4719..00000000 --- a/game/physics/constraint/static_position.go +++ /dev/null @@ -1,44 +0,0 @@ -package constraint - -// import ( -// "github.com/mokiat/gomath/dprec" -// "github.com/mokiat/lacking/game/physics/solver" -// ) - -// // NewStaticPosition creates a new StaticPosition constraint solver. -// func NewStaticPosition() *StaticPosition { -// return &StaticPosition{ -// position: dprec.ZeroVec3(), -// } -// } - -// var _ solver.Constraint = (*StaticPosition)(nil) - -// // StaticPosition represents the solution for a constraint -// // that keeps a body positioned at the specified fixture location. -// // -// // This solver is immediate - it converges in a single step. -// type StaticPosition struct { -// position dprec.Vec3 -// } - -// // Position returns the location to which the body will be constrained. -// func (t *StaticPosition) Position() dprec.Vec3 { -// return t.position -// } - -// // SetPosition changes the location to which the body will be constrained. -// func (t *StaticPosition) SetPosition(position dprec.Vec3) *StaticPosition { -// t.position = position -// return t -// } - -// func (s *StaticPosition) Reset(ctx solver.Context) {} - -// func (s *StaticPosition) ApplyImpulses(ctx solver.Context) { -// ctx.Target.SetLinearVelocity(dprec.ZeroVec3()) -// } - -// func (s *StaticPosition) ApplyNudges(ctx solver.Context) { -// ctx.Target.SetPosition(s.position) -// } diff --git a/game/physics/scene.go b/game/physics/scene.go index 08ee4b45..06100005 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -816,7 +816,7 @@ func (s *Scene) detectCollisions() { func (s *Scene) handlePairCollision(primaryData, secondaryData bodyCollisionData, contact placement3d.Contact) { solver := s.allocatePairCollisionSolver() - solver.Init(PairCollisionSolverConfig{ + solver.Configure(PairCollisionSolverConfig{ PrimaryFrictionCoefficient: primaryData.frictionCoefficient, PrimaryRestitutionCoefficient: primaryData.restitutionCoefficient, PrimaryContactNormal: contact.EvalSourceNormal(), @@ -845,7 +845,7 @@ func (s *Scene) handlePairCollision(primaryData, secondaryData bodyCollisionData func (s *Scene) handleSoloCollision(bodyData bodyCollisionData, terrainData terrainCollisionData, contact placement3d.Contact) { solver := s.allocateSoloCollisionSolver() - solver.Init(SoloCollisionSolverConfig{ + solver.Configure(SoloCollisionSolverConfig{ TerrainFrictionCoefficient: terrainData.frictionCoefficient, TerrainRestitutionCoefficient: terrainData.restitutionCoefficient, TerrainContactNormal: contact.TargetNormal, diff --git a/game/physics/solver_collision_pair.go b/game/physics/solver_collision_pair.go index 2f0b55fd..db226478 100644 --- a/game/physics/solver_collision_pair.go +++ b/game/physics/solver_collision_pair.go @@ -3,7 +3,8 @@ package physics import "github.com/mokiat/gomath/dprec" // PairCollisionSolverConfig holds the parameters with which a -// [PairCollisionSolver] is configured through [PairCollisionSolver.Init]. +// [PairCollisionSolver] is configured, either through +// [NewPairCollisionSolver] or [PairCollisionSolver.Configure]. // // It describes a single contact between two dynamic bodies - the contact // geometry (normals, points, penetration depth), each as seen from its @@ -68,9 +69,9 @@ type PairCollisionSolverConfig struct { // [PairCollisionSolver.ApplyNudges] it separately corrects any // remaining penetration at the position level. // -// A PairCollisionSolver must be configured through -// [PairCollisionSolver.Init] before being registered with a [Scene] -// through [PairConstraintView.Create]. +// A PairCollisionSolver must be configured, either through +// [NewPairCollisionSolver] or [PairCollisionSolver.Configure], before +// being registered with a [Scene] through [PairConstraintView.Create]. type PairCollisionSolver struct { primaryContactNormal dprec.Vec3 primaryContactPoint dprec.Vec3 @@ -90,15 +91,26 @@ type PairCollisionSolver struct { var _ PairConstraintSolver = (*PairCollisionSolver)(nil) -// Init configures this solver according to config. +// NewPairCollisionSolver creates a new [PairCollisionSolver] configured +// according to config. See [PairCollisionSolver.Configure] for details. +func NewPairCollisionSolver(config PairCollisionSolverConfig) *PairCollisionSolver { + result := &PairCollisionSolver{} + result.Configure(config) + return result +} + +// Configure configures this solver according to config. // // The two bodies' friction coefficients are combined into a single // coefficient through their geometric mean, and their restitution // coefficients through their maximum. // -// Init must be called once, before this solver is registered with a -// [Scene] through [PairConstraintView.Create]. -func (s *PairCollisionSolver) Init(config PairCollisionSolverConfig) { +// Configure must be called before this solver is registered with a +// [Scene] through [PairConstraintView.Create]. Unlike +// [NewPairCollisionSolver], it can be called on an already-allocated +// solver, which allows solvers to be cached (e.g. in a slice) and +// configured on demand as new contacts are detected. +func (s *PairCollisionSolver) Configure(config PairCollisionSolverConfig) { s.primaryContactNormal = config.PrimaryContactNormal s.primaryContactPoint = config.PrimaryContactPoint s.secondaryContactNormal = config.SecondaryContactNormal diff --git a/game/physics/solver_collision_solo.go b/game/physics/solver_collision_solo.go index 26244535..686517d8 100644 --- a/game/physics/solver_collision_solo.go +++ b/game/physics/solver_collision_solo.go @@ -3,7 +3,8 @@ package physics import "github.com/mokiat/gomath/dprec" // SoloCollisionSolverConfig holds the parameters with which a -// [SoloCollisionSolver] is configured through [SoloCollisionSolver.Init]. +// [SoloCollisionSolver] is configured, either through +// [NewSoloCollisionSolver] or [SoloCollisionSolver.Configure]. // // It describes a single contact between a body and static terrain - // the contact geometry (normals, point, penetration depth) as well as @@ -53,9 +54,9 @@ type SoloCollisionSolverConfig struct { // that normal impulse. Through [SoloCollisionSolver.ApplyNudges] it // separately corrects any remaining penetration at the position level. // -// A SoloCollisionSolver must be configured through -// [SoloCollisionSolver.Init] before being registered with a [Scene] -// through [SoloConstraintView.Create]. +// A SoloCollisionSolver must be configured, either through +// [NewSoloCollisionSolver] or [SoloCollisionSolver.Configure], before +// being registered with a [Scene] through [SoloConstraintView.Create]. type SoloCollisionSolver struct { terrainContactNormal dprec.Vec3 bodyContactPoint dprec.Vec3 @@ -71,15 +72,26 @@ type SoloCollisionSolver struct { var _ SoloConstraintSolver = (*SoloCollisionSolver)(nil) -// Init configures this solver according to config. +// NewSoloCollisionSolver creates a new [SoloCollisionSolver] configured +// according to config. See [SoloCollisionSolver.Configure] for details. +func NewSoloCollisionSolver(config SoloCollisionSolverConfig) *SoloCollisionSolver { + result := &SoloCollisionSolver{} + result.Configure(config) + return result +} + +// Configure configures this solver according to config. // // The body's and terrain's friction coefficients are combined into a // single coefficient through their geometric mean, and their restitution // coefficients through their maximum. // -// Init must be called once, before this solver is registered with a -// [Scene] through [SoloConstraintView.Create]. -func (s *SoloCollisionSolver) Init(config SoloCollisionSolverConfig) { +// Configure must be called before this solver is registered with a +// [Scene] through [SoloConstraintView.Create]. Unlike +// [NewSoloCollisionSolver], it can be called on an already-allocated +// solver, which allows solvers to be cached (e.g. in a slice) and +// configured on demand as new contacts are detected. +func (s *SoloCollisionSolver) Configure(config SoloCollisionSolverConfig) { s.terrainContactNormal = config.TerrainContactNormal s.bodyContactPoint = config.BodyContactPoint s.contactDepth = config.ContactDepth diff --git a/game/physics/solver_fixed_position.go b/game/physics/solver_fixed_position.go new file mode 100644 index 00000000..712cb4c3 --- /dev/null +++ b/game/physics/solver_fixed_position.go @@ -0,0 +1,87 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +// FixedPositionSolverConfig holds the parameters with which a +// [FixedPositionSolver] is configured, either through +// [NewFixedPositionSolver] or [FixedPositionSolver.Configure]. +type FixedPositionSolverConfig struct { + + // Position is the world-space location at which the target is to be + // held fixed. + Position dprec.Vec3 +} + +// FixedPositionSolver is a [SoloConstraintSolver] that pins a target to a +// fixed position in world space, regardless of any forces or impulses +// acting on it. +// +// Unlike [SoloCollisionSolver], which only nudges its target enough to +// resolve penetration, FixedPositionSolver is a kinematic constraint - it +// unconditionally zeroes the target's linear velocity through +// [FixedPositionSolver.ApplyImpulses] and snaps it to Position through +// [FixedPositionSolver.ApplyNudges] on every step. It does not affect +// angular velocity or orientation. +// +// A FixedPositionSolver must be configured, either through +// [NewFixedPositionSolver] or [FixedPositionSolver.Configure], before +// being registered with a [Scene] through [SoloConstraintView.Create]. +type FixedPositionSolver struct { + position dprec.Vec3 +} + +var _ SoloConstraintSolver = (*FixedPositionSolver)(nil) + +// NewFixedPositionSolver creates a new [FixedPositionSolver] configured +// according to config. +func NewFixedPositionSolver(config FixedPositionSolverConfig) *FixedPositionSolver { + result := &FixedPositionSolver{} + result.Configure(config) + return result +} + +// Configure configures this solver according to config. +// +// Configure must be called before this solver is registered with a +// [Scene] through [SoloConstraintView.Create]. Unlike +// [NewFixedPositionSolver], it can be called on an already-allocated +// solver, which allows solvers to be cached (e.g. in a slice) and +// configured on demand. +func (s *FixedPositionSolver) Configure(config FixedPositionSolverConfig) { + s.position = config.Position +} + +// Position returns the world-space location at which the target is held +// fixed. +func (s *FixedPositionSolver) Position() dprec.Vec3 { + return s.position +} + +// SetPosition changes the world-space location at which the target is +// held fixed. +// +// It returns the solver itself, so that calls can be chained. +func (s *FixedPositionSolver) SetPosition(position dprec.Vec3) *FixedPositionSolver { + s.position = position + return s +} + +// Reset implements [SoloConstraintSolver.Reset]. +// +// It is a no-op, since this solver holds no per-step state that needs to +// be derived from the target's current position or velocity. +func (s *FixedPositionSolver) Reset(ctx SoloConstraintContext) {} + +// ApplyImpulses implements [SoloConstraintSolver.ApplyImpulses]. +// +// It unconditionally zeroes the target's linear velocity. +func (s *FixedPositionSolver) ApplyImpulses(ctx SoloConstraintContext) { + ctx.Target.SetLinearVelocity(dprec.ZeroVec3()) +} + +// ApplyNudges implements [SoloConstraintSolver.ApplyNudges]. +// +// It unconditionally sets the target's position to Position. +func (s *FixedPositionSolver) ApplyNudges(ctx SoloConstraintContext) { + ctx.Target.SetPosition(s.position) +} From d0793067a3f317496c71064ab243c9104e9dc3af Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Wed, 5 Aug 2026 20:50:19 +0300 Subject: [PATCH 44/85] Add FixedRotation constraint solver --- game/physics/constraint/static_rotation.go | 44 ----------- game/physics/solver_fixed_position.go | 2 +- game/physics/solver_fixed_rotation.go | 87 ++++++++++++++++++++++ 3 files changed, 88 insertions(+), 45 deletions(-) delete mode 100644 game/physics/constraint/static_rotation.go create mode 100644 game/physics/solver_fixed_rotation.go diff --git a/game/physics/constraint/static_rotation.go b/game/physics/constraint/static_rotation.go deleted file mode 100644 index 4d10ecf0..00000000 --- a/game/physics/constraint/static_rotation.go +++ /dev/null @@ -1,44 +0,0 @@ -package constraint - -// import ( -// "github.com/mokiat/gomath/dprec" -// "github.com/mokiat/lacking/game/physics/solver" -// ) - -// // NewStaticRotation creates a new StaticRotation constraint solver. -// func NewStaticRotation() *StaticRotation { -// return &StaticRotation{ -// rotation: dprec.IdentityQuat(), -// } -// } - -// var _ solver.Constraint = (*StaticRotation)(nil) - -// // StaticRotation represents the solution for a constraint -// // that keeps a body positioned at the specified fixture location. -// // -// // This solver is immediate - it converges in a single step. -// type StaticRotation struct { -// rotation dprec.Quat -// } - -// // Rotation returns the orientation to which the body will be constrained. -// func (t *StaticRotation) Rotation() dprec.Quat { -// return t.rotation -// } - -// // SetRotation changes the orientation to which the body will be constrained. -// func (t *StaticRotation) SetRotation(rotation dprec.Quat) *StaticRotation { -// t.rotation = rotation -// return t -// } - -// func (s *StaticRotation) Reset(ctx solver.Context) {} - -// func (s *StaticRotation) ApplyImpulses(ctx solver.Context) { -// ctx.Target.SetAngularVelocity(dprec.ZeroVec3()) -// } - -// func (s *StaticRotation) ApplyNudges(ctx solver.Context) { -// ctx.Target.SetRotation(s.rotation) -// } diff --git a/game/physics/solver_fixed_position.go b/game/physics/solver_fixed_position.go index 712cb4c3..1e3c1998 100644 --- a/game/physics/solver_fixed_position.go +++ b/game/physics/solver_fixed_position.go @@ -21,7 +21,7 @@ type FixedPositionSolverConfig struct { // unconditionally zeroes the target's linear velocity through // [FixedPositionSolver.ApplyImpulses] and snaps it to Position through // [FixedPositionSolver.ApplyNudges] on every step. It does not affect -// angular velocity or orientation. +// angular velocity or rotation - see [FixedRotationSolver] for that. // // A FixedPositionSolver must be configured, either through // [NewFixedPositionSolver] or [FixedPositionSolver.Configure], before diff --git a/game/physics/solver_fixed_rotation.go b/game/physics/solver_fixed_rotation.go new file mode 100644 index 00000000..5017cb40 --- /dev/null +++ b/game/physics/solver_fixed_rotation.go @@ -0,0 +1,87 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +// FixedRotationSolverConfig holds the parameters with which a +// [FixedRotationSolver] is configured, either through +// [NewFixedRotationSolver] or [FixedRotationSolver.Configure]. +type FixedRotationSolverConfig struct { + + // Rotation is the world-space orientation at which the target is to + // be held fixed. + Rotation dprec.Quat +} + +// FixedRotationSolver is a [SoloConstraintSolver] that pins a target to a +// fixed rotation in world space, regardless of any forces or torques +// acting on it. +// +// Unlike [SoloCollisionSolver], which only nudges its target enough to +// resolve penetration, FixedRotationSolver is a kinematic constraint - it +// unconditionally zeroes the target's angular velocity through +// [FixedRotationSolver.ApplyImpulses] and snaps it to Rotation through +// [FixedRotationSolver.ApplyNudges] on every step. It does not affect +// linear velocity or position - see [FixedPositionSolver] for that. +// +// A FixedRotationSolver must be configured, either through +// [NewFixedRotationSolver] or [FixedRotationSolver.Configure], before +// being registered with a [Scene] through [SoloConstraintView.Create]. +type FixedRotationSolver struct { + rotation dprec.Quat +} + +var _ SoloConstraintSolver = (*FixedRotationSolver)(nil) + +// NewFixedRotationSolver creates a new [FixedRotationSolver] configured +// according to config. +func NewFixedRotationSolver(config FixedRotationSolverConfig) *FixedRotationSolver { + result := &FixedRotationSolver{} + result.Configure(config) + return result +} + +// Configure configures this solver according to config. +// +// Configure must be called before this solver is registered with a +// [Scene] through [SoloConstraintView.Create]. Unlike +// [NewFixedRotationSolver], it can be called on an already-allocated +// solver, which allows solvers to be cached (e.g. in a slice) and +// configured on demand. +func (s *FixedRotationSolver) Configure(config FixedRotationSolverConfig) { + s.rotation = config.Rotation +} + +// Rotation returns the world-space orientation at which the target is +// held fixed. +func (s *FixedRotationSolver) Rotation() dprec.Quat { + return s.rotation +} + +// SetRotation changes the world-space orientation at which the target is +// held fixed. +// +// It returns the solver itself, so that calls can be chained. +func (s *FixedRotationSolver) SetRotation(rotation dprec.Quat) *FixedRotationSolver { + s.rotation = rotation + return s +} + +// Reset implements [SoloConstraintSolver.Reset]. +// +// It is a no-op, since this solver holds no per-step state that needs to +// be derived from the target's current position or velocity. +func (s *FixedRotationSolver) Reset(ctx SoloConstraintContext) {} + +// ApplyImpulses implements [SoloConstraintSolver.ApplyImpulses]. +// +// It unconditionally zeroes the target's angular velocity. +func (s *FixedRotationSolver) ApplyImpulses(ctx SoloConstraintContext) { + ctx.Target.SetAngularVelocity(dprec.ZeroVec3()) +} + +// ApplyNudges implements [SoloConstraintSolver.ApplyNudges]. +// +// It unconditionally sets the target's rotation to Rotation. +func (s *FixedRotationSolver) ApplyNudges(ctx SoloConstraintContext) { + ctx.Target.SetRotation(s.rotation) +} From 6deee575a24845475964aa79ceef1507a9f22310 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Wed, 5 Aug 2026 21:53:46 +0300 Subject: [PATCH 45/85] Add fixed distance constraint solver --- game/physics/solver_fixed_distance.go | 159 ++++++++++++++++++++++++++ 1 file changed, 159 insertions(+) create mode 100644 game/physics/solver_fixed_distance.go diff --git a/game/physics/solver_fixed_distance.go b/game/physics/solver_fixed_distance.go new file mode 100644 index 00000000..17650376 --- /dev/null +++ b/game/physics/solver_fixed_distance.go @@ -0,0 +1,159 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +// FixedDistanceSolverConfig holds the parameters with which a +// [FixedDistanceSolver] is configured, either through +// [NewFixedDistanceSolver] or [FixedDistanceSolver.Configure]. +type FixedDistanceSolverConfig struct { + + // FixedPoint is the world-space position at which the target is + // anchored. + FixedPoint dprec.Vec3 + + // BodyAnchorOffset is the body-local-space offset, relative to the + // target's center of mass, of the point that is held at Distance + // from FixedPoint. + BodyAnchorOffset dprec.Vec3 + + // Distance is the distance at which the target is held from + // FixedPoint. + Distance float64 +} + +// FixedDistanceSolver is a [SoloConstraintSolver] that holds a point on a +// target at a fixed distance from a fixed point in world space, acting +// like a rigid rod between the two - it resists the target moving both +// closer to and farther from FixedPoint. +// +// A FixedDistanceSolver must be configured, either through +// [NewFixedDistanceSolver] or [FixedDistanceSolver.Configure], before +// being registered with a [Scene] through [SoloConstraintView.Create]. +type FixedDistanceSolver struct { + fixedPoint dprec.Vec3 + bodyAnchorOffset dprec.Vec3 + distance float64 + + jacobian Jacobian + drift float64 +} + +var _ SoloConstraintSolver = (*FixedDistanceSolver)(nil) + +// NewFixedDistanceSolver creates a new [FixedDistanceSolver] configured +// according to config. +func NewFixedDistanceSolver(config FixedDistanceSolverConfig) *FixedDistanceSolver { + result := &FixedDistanceSolver{} + result.Configure(config) + return result +} + +// Configure configures this solver according to config. +// +// Configure must be called before this solver is registered with a +// [Scene] through [SoloConstraintView.Create]. Unlike +// [NewFixedDistanceSolver], it can be called on an already-allocated +// solver, which allows solvers to be cached (e.g. in a slice) and +// configured on demand. +func (s *FixedDistanceSolver) Configure(config FixedDistanceSolverConfig) { + s.fixedPoint = config.FixedPoint + s.bodyAnchorOffset = config.BodyAnchorOffset + s.distance = config.Distance +} + +// FixedPoint returns the world-space position at which the target is +// anchored. +func (s *FixedDistanceSolver) FixedPoint() dprec.Vec3 { + return s.fixedPoint +} + +// SetFixedPoint changes the world-space position at which the target is +// anchored. +// +// It returns the solver itself, so that calls can be chained. +func (s *FixedDistanceSolver) SetFixedPoint(fixedPoint dprec.Vec3) *FixedDistanceSolver { + s.fixedPoint = fixedPoint + return s +} + +// BodyAnchorOffset returns the body-local-space offset, relative to the +// target's center of mass, of the point that is held at Distance from +// FixedPoint. +func (s *FixedDistanceSolver) BodyAnchorOffset() dprec.Vec3 { + return s.bodyAnchorOffset +} + +// SetBodyAnchorOffset changes the body-local-space offset, relative to +// the target's center of mass, of the point that is held at Distance +// from FixedPoint. +// +// It returns the solver itself, so that calls can be chained. +func (s *FixedDistanceSolver) SetBodyAnchorOffset(offset dprec.Vec3) *FixedDistanceSolver { + s.bodyAnchorOffset = offset + return s +} + +// Distance returns the distance at which the target is held from +// FixedPoint. +func (s *FixedDistanceSolver) Distance() float64 { + return s.distance +} + +// SetDistance changes the distance at which the target is held from +// FixedPoint. +// +// It returns the solver itself, so that calls can be chained. +func (s *FixedDistanceSolver) SetDistance(distance float64) *FixedDistanceSolver { + s.distance = distance + return s +} + +// Reset implements [SoloConstraintSolver.Reset]. +// +// It recomputes the constraint's [Jacobian], along with the world-space +// offset from the target's center of mass to its anchor point (derived +// from BodyAnchorOffset and the target's current rotation), and the +// current distance error (drift) between that anchor point and +// FixedPoint, based on the target's current position and rotation. +// +// If the anchor point currently coincides with FixedPoint, the +// constraint direction is undefined; an arbitrary axis is used as a +// fallback in that degenerate case. +func (s *FixedDistanceSolver) Reset(ctx SoloConstraintContext) { + anchorOffsetWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.bodyAnchorOffset) + anchorWS := dprec.Vec3Sum(ctx.Target.Position(), anchorOffsetWS) + delta := dprec.Vec3Diff(anchorWS, s.fixedPoint) + + normal := dprec.BasisXVec3() + actualDistance := delta.Length() + if actualDistance > Epsilon { + normal = dprec.UnitVec3(delta) + } + + s.jacobian = Jacobian{ + LinearSlope: normal, + AngularSlope: dprec.Vec3Cross(anchorOffsetWS, normal), + } + s.drift = s.distance - actualDistance +} + +// ApplyImpulses implements [SoloConstraintSolver.ApplyImpulses]. +// +// It resolves an impulse, without restitution, that drives the target's +// velocity at the anchor point toward closing the distance error +// (drift) computed by [FixedDistanceSolver.Reset], pushing the target +// away from FixedPoint when it is too close and pulling it back when it +// is too far. +func (s *FixedDistanceSolver) ApplyImpulses(ctx SoloConstraintContext) { + impulse := ctx.ImpulseSolution(s.jacobian, s.drift, 0.0) + ctx.Target.ApplyImpulse(impulse) +} + +// ApplyNudges implements [SoloConstraintSolver.ApplyNudges]. +// +// It nudges the target's position and rotation to reduce any remaining +// distance error (drift) between its anchor point and FixedPoint. +func (s *FixedDistanceSolver) ApplyNudges(ctx SoloConstraintContext) { + nudge := ctx.NudgeSolution(s.jacobian, s.drift) + ctx.Target.ApplyNudge(nudge) +} From f924e6437d015aa16a34cfc14097d724dbe922f9 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Wed, 5 Aug 2026 22:00:20 +0300 Subject: [PATCH 46/85] Add copy position constraint solver --- game/physics/constraint/chandelier.go | 98 ------------------------ game/physics/constraint/copy_position.go | 26 ------- game/physics/solver_copy_position.go | 51 ++++++++++++ 3 files changed, 51 insertions(+), 124 deletions(-) delete mode 100644 game/physics/constraint/chandelier.go delete mode 100644 game/physics/constraint/copy_position.go create mode 100644 game/physics/solver_copy_position.go diff --git a/game/physics/constraint/chandelier.go b/game/physics/constraint/chandelier.go deleted file mode 100644 index 3d3d2884..00000000 --- a/game/physics/constraint/chandelier.go +++ /dev/null @@ -1,98 +0,0 @@ -package constraint - -// import ( -// "github.com/mokiat/gomath/dprec" -// "github.com/mokiat/lacking/game/physics/solver" -// ) - -// // NewChandelier creates a new Chandelier constraint solver. -// func NewChandelier() *Chandelier { -// return &Chandelier{ -// fixture: dprec.ZeroVec3(), -// radius: dprec.ZeroVec3(), -// length: 1.0, -// } -// } - -// var _ solver.Constraint = (*Chandelier)(nil) - -// // Chandelier represents the solution for a constraint -// // that keeps a body hanging off of a fixture location similar -// // to a chandelier. -// type Chandelier struct { -// fixture dprec.Vec3 -// radius dprec.Vec3 -// length float64 - -// jacobian solver.Jacobian -// drift float64 -// } - -// // Fixture returns the fixture location for the chandelier hook. -// func (s *Chandelier) Fixture() dprec.Vec3 { -// return s.fixture -// } - -// // SetFixture changes the fixture location for the chandelier hook. -// func (s *Chandelier) SetFixture(fixture dprec.Vec3) *Chandelier { -// s.fixture = fixture -// return s -// } - -// // Radius returns the radius vector of the contact point on the object. -// // -// // The vector is in the object's local space. -// func (s *Chandelier) Radius() dprec.Vec3 { -// return s.radius -// } - -// // SetRadius changes the radius vector of the contact point on the object. -// // -// // The vector is in the object's local space. -// func (s *Chandelier) SetRadius(radius dprec.Vec3) *Chandelier { -// s.radius = radius -// return s -// } - -// // Length returns the chandelier length. -// func (s *Chandelier) Length() float64 { -// return s.length -// } - -// // SetLength changes the chandelier length. -// func (s *Chandelier) SetLength(length float64) *Chandelier { -// s.length = length -// return s -// } - -// // Reset re-evaluates the constraint. -// func (s *Chandelier) Reset(ctx solver.Context) { -// radiusWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.radius) -// pointWS := dprec.Vec3Sum(ctx.Target.Position(), radiusWS) -// deltaPositionWS := dprec.Vec3Diff(pointWS, s.fixture) -// if distance := deltaPositionWS.Length(); distance > solver.Epsilon { -// normalWS := dprec.Vec3Quot(deltaPositionWS, distance) -// s.jacobian = solver.Jacobian{ -// LinearSlope: normalWS, -// AngularSlope: dprec.Vec3Cross(radiusWS, normalWS), -// } -// s.drift = distance - s.length -// } else { -// s.jacobian = solver.Jacobian{} -// s.drift = -s.length -// } -// } - -// // ApplyImpulses applies impulses in order to keep the velocity part of -// // the constraint satisfied. -// func (s *Chandelier) ApplyImpulses(ctx solver.Context) { -// solution := ctx.JacobianImpulseSolution(s.jacobian, s.drift, 0.0) -// ctx.Target.ApplyImpulse(solution) -// } - -// // ApplyNudges applies nudges in order to keep the positional part of the -// // constraint satisfied. -// func (s *Chandelier) ApplyNudges(ctx solver.Context) { -// solution := ctx.JacobianNudgeSolution(s.jacobian, s.drift) -// ctx.Target.ApplyNudge(solution) -// } diff --git a/game/physics/constraint/copy_position.go b/game/physics/constraint/copy_position.go deleted file mode 100644 index b5419482..00000000 --- a/game/physics/constraint/copy_position.go +++ /dev/null @@ -1,26 +0,0 @@ -package constraint - -// import "github.com/mokiat/lacking/game/physics/solver" - -// // NewCopyPosition creates a new CopyPosition constraint solver. -// func NewCopyPosition() *CopyPosition { -// return &CopyPosition{} -// } - -// var _ solver.PairConstraint = (*CopyPosition)(nil) - -// // CopyPosition ensures that the target object has the same position as -// // the source one. -// // -// // This solver is immediate - it converges in a single step. -// type CopyPosition struct{} - -// func (s *CopyPosition) Reset(ctx solver.PairContext) {} - -// func (s *CopyPosition) ApplyImpulses(ctx solver.PairContext) { -// ctx.Target.SetLinearVelocity(ctx.Source.LinearVelocity()) -// } - -// func (s *CopyPosition) ApplyNudges(ctx solver.PairContext) { -// ctx.Target.SetPosition(ctx.Source.Position()) -// } diff --git a/game/physics/solver_copy_position.go b/game/physics/solver_copy_position.go new file mode 100644 index 00000000..ef4d190d --- /dev/null +++ b/game/physics/solver_copy_position.go @@ -0,0 +1,51 @@ +package physics + +// CopyPositionSolver is a [PairConstraintSolver] that makes the primary +// target follow the secondary target's position, regardless of any +// forces or impulses acting on the primary target. +// +// It is a kinematic constraint, in the same vein as +// [FixedPositionSolver], except that it tracks a moving secondary target +// instead of a constant world-space position. Through +// [CopyPositionSolver.ApplyImpulses] it unconditionally overwrites the +// primary target's linear velocity with the secondary target's, and +// through [CopyPositionSolver.ApplyNudges] it unconditionally overwrites +// the primary target's position with the secondary target's, on every +// step. The secondary target itself is never modified. Only linear +// motion is copied - the primary target's rotation and angular velocity +// are left to evolve on their own. +// +// CopyPositionSolver holds no configurable state, so unlike most other +// solvers in this package, it has no Config type or Configure method; +// [NewCopyPositionSolver] is the only way to obtain one, and a single +// instance can safely back any number of constraints. +type CopyPositionSolver struct{} + +var _ PairConstraintSolver = (*CopyPositionSolver)(nil) + +// NewCopyPositionSolver creates a new [CopyPositionSolver]. +func NewCopyPositionSolver() *CopyPositionSolver { + return &CopyPositionSolver{} +} + +// Reset implements [PairConstraintSolver.Reset]. +// +// It is a no-op, since this solver holds no per-step state that needs to +// be derived from the targets' current positions or velocities. +func (s *CopyPositionSolver) Reset(ctx PairConstraintContext) {} + +// ApplyImpulses implements [PairConstraintSolver.ApplyImpulses]. +// +// It unconditionally overwrites the primary target's linear velocity +// with the secondary target's. +func (s *CopyPositionSolver) ApplyImpulses(ctx PairConstraintContext) { + ctx.PrimaryTarget.SetLinearVelocity(ctx.SecondaryTarget.LinearVelocity()) +} + +// ApplyNudges implements [PairConstraintSolver.ApplyNudges]. +// +// It unconditionally overwrites the primary target's position with the +// secondary target's. +func (s *CopyPositionSolver) ApplyNudges(ctx PairConstraintContext) { + ctx.PrimaryTarget.SetPosition(ctx.SecondaryTarget.Position()) +} From 28eebdfd57b23e2867a05b20bb7ef5a74a8c6eb0 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Wed, 5 Aug 2026 22:07:39 +0300 Subject: [PATCH 47/85] Add copy rotation constraint solver --- game/physics/constraint/copy_rotation.go | 26 ------------ game/physics/solver_copy_position.go | 3 +- game/physics/solver_copy_rotation.go | 51 ++++++++++++++++++++++++ 3 files changed, 53 insertions(+), 27 deletions(-) delete mode 100644 game/physics/constraint/copy_rotation.go create mode 100644 game/physics/solver_copy_rotation.go diff --git a/game/physics/constraint/copy_rotation.go b/game/physics/constraint/copy_rotation.go deleted file mode 100644 index 75cca887..00000000 --- a/game/physics/constraint/copy_rotation.go +++ /dev/null @@ -1,26 +0,0 @@ -package constraint - -// import "github.com/mokiat/lacking/game/physics/solver" - -// // NewCopyRotation creates a new CopyRotation constraint solver. -// func NewCopyRotation() *CopyRotation { -// return &CopyRotation{} -// } - -// var _ solver.PairConstraint = (*CopyRotation)(nil) - -// // CopyRotation ensures that the target body has exactly the same rotation -// // as the source one. -// // -// // This solver is immediate - it converges in a single step. -// type CopyRotation struct{} - -// func (s *CopyRotation) Reset(ctx solver.PairContext) {} - -// func (s *CopyRotation) ApplyImpulses(ctx solver.PairContext) { -// ctx.Target.SetAngularVelocity(ctx.Source.AngularVelocity()) -// } - -// func (s *CopyRotation) ApplyNudges(ctx solver.PairContext) { -// ctx.Target.SetRotation(ctx.Source.Rotation()) -// } diff --git a/game/physics/solver_copy_position.go b/game/physics/solver_copy_position.go index ef4d190d..a51d9e57 100644 --- a/game/physics/solver_copy_position.go +++ b/game/physics/solver_copy_position.go @@ -13,7 +13,8 @@ package physics // the primary target's position with the secondary target's, on every // step. The secondary target itself is never modified. Only linear // motion is copied - the primary target's rotation and angular velocity -// are left to evolve on their own. +// are left to evolve on their own; see [CopyRotationSolver] for the +// rotational counterpart. // // CopyPositionSolver holds no configurable state, so unlike most other // solvers in this package, it has no Config type or Configure method; diff --git a/game/physics/solver_copy_rotation.go b/game/physics/solver_copy_rotation.go new file mode 100644 index 00000000..c50a10b8 --- /dev/null +++ b/game/physics/solver_copy_rotation.go @@ -0,0 +1,51 @@ +package physics + +// CopyRotationSolver is a [PairConstraintSolver] that makes the primary +// target follow the secondary target's rotation, regardless of any +// forces or torques acting on the primary target. +// +// It is a kinematic constraint, in the same vein as +// [FixedRotationSolver], except that it tracks a moving secondary target +// instead of a constant world-space rotation. Through +// [CopyRotationSolver.ApplyImpulses] it unconditionally overwrites the +// primary target's angular velocity with the secondary target's, and +// through [CopyRotationSolver.ApplyNudges] it unconditionally overwrites +// the primary target's rotation with the secondary target's, on every +// step. The secondary target itself is never modified. Only rotational +// motion is copied - see [CopyPositionSolver] for the positional +// counterpart. +// +// CopyRotationSolver holds no configurable state, so unlike most other +// solvers in this package, it has no Config type or Configure method; +// [NewCopyRotationSolver] is the only way to obtain one, and a single +// instance can safely back any number of constraints. +type CopyRotationSolver struct{} + +var _ PairConstraintSolver = (*CopyRotationSolver)(nil) + +// NewCopyRotationSolver creates a new [CopyRotationSolver]. +func NewCopyRotationSolver() *CopyRotationSolver { + return &CopyRotationSolver{} +} + +// Reset implements [PairConstraintSolver.Reset]. +// +// It is a no-op, since this solver holds no per-step state that needs to +// be derived from the targets' current rotations or velocities. +func (s *CopyRotationSolver) Reset(ctx PairConstraintContext) {} + +// ApplyImpulses implements [PairConstraintSolver.ApplyImpulses]. +// +// It unconditionally overwrites the primary target's angular velocity +// with the secondary target's. +func (s *CopyRotationSolver) ApplyImpulses(ctx PairConstraintContext) { + ctx.PrimaryTarget.SetAngularVelocity(ctx.SecondaryTarget.AngularVelocity()) +} + +// ApplyNudges implements [PairConstraintSolver.ApplyNudges]. +// +// It unconditionally overwrites the primary target's rotation with the +// secondary target's. +func (s *CopyRotationSolver) ApplyNudges(ctx PairConstraintContext) { + ctx.PrimaryTarget.SetRotation(ctx.SecondaryTarget.Rotation()) +} From af41899a661a53881471b1cbdc69cb1bb963f1d7 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Wed, 5 Aug 2026 22:29:30 +0300 Subject: [PATCH 48/85] Add composite constraint solvers --- game/physics/solver_composite_pair.go | 77 +++++++++++++++++++++++++++ game/physics/solver_composite_solo.go | 77 +++++++++++++++++++++++++++ 2 files changed, 154 insertions(+) create mode 100644 game/physics/solver_composite_pair.go create mode 100644 game/physics/solver_composite_solo.go diff --git a/game/physics/solver_composite_pair.go b/game/physics/solver_composite_pair.go new file mode 100644 index 00000000..edde841c --- /dev/null +++ b/game/physics/solver_composite_pair.go @@ -0,0 +1,77 @@ +package physics + +// CompositePairConstraintSolver is a [PairConstraintSolver] that combines +// several other pair constraint solvers into one, so that they can be +// registered as a single constraint through [PairConstraintView.Create] +// - sharing one [PairConstraintID], and consequently one enable/disable +// toggle - instead of one each. +// +// The composed solvers are driven in the order in which they were +// supplied to [NewCompositePairConstraintSolver]. Since they act on the +// same targets one after another, rather than simultaneously, a solver +// later in the list observes any changes that an earlier one already +// made to the targets during the same call. +// +// CompositePairConstraintSolver holds no configurable state of its own, +// so unlike most other solvers in this package, it has no Config type or +// Configure method; [NewCompositePairConstraintSolver] is the only way +// to obtain one. +type CompositePairConstraintSolver struct { + solvers []PairConstraintSolver +} + +var _ PairConstraintSolver = (*CompositePairConstraintSolver)(nil) + +// NewCompositePairConstraintSolver creates a new +// [CompositePairConstraintSolver] that combines solvers, in the given +// order. +func NewCompositePairConstraintSolver(solvers ...PairConstraintSolver) *CompositePairConstraintSolver { + return &CompositePairConstraintSolver{ + solvers: solvers, + } +} + +// Solvers returns the pair constraint solvers that this solver combines, +// in the order in which they are driven. +func (s *CompositePairConstraintSolver) Solvers() []PairConstraintSolver { + return s.solvers +} + +// Reset implements [PairConstraintSolver.Reset]. +// +// It calls [PairConstraintSolver.Reset] on each combined solver, in +// order. +func (s *CompositePairConstraintSolver) Reset(ctx PairConstraintContext) { + for _, solver := range s.solvers { + solver.Reset(ctx) + } +} + +// ApplyImpulses implements [PairConstraintSolver.ApplyImpulses]. +// +// It calls [PairConstraintSolver.ApplyImpulses] on each combined solver, +// in order. This is safe without an intervening Reset, since impulses do +// not reposition the targets and therefore cannot invalidate any +// position-derived state a combined solver cached during Reset. +func (s *CompositePairConstraintSolver) ApplyImpulses(ctx PairConstraintContext) { + for _, solver := range s.solvers { + solver.ApplyImpulses(ctx) + } +} + +// ApplyNudges implements [PairConstraintSolver.ApplyNudges]. +// +// It calls [PairConstraintSolver.Reset] followed by +// [PairConstraintSolver.ApplyNudges] on each combined solver in turn, +// rather than calling ApplyNudges on all of them in a single pass. This +// preserves the guarantee, documented on [PairConstraintSolver.Reset], +// that Reset always immediately precedes ApplyNudges for a given solver +// - since a combined solver earlier in the list may reposition either +// target, which would otherwise leave a later solver's cached, +// position-derived state stale for the remainder of this call. +func (s *CompositePairConstraintSolver) ApplyNudges(ctx PairConstraintContext) { + for _, solver := range s.solvers { + solver.Reset(ctx) // preserve engine reset behavior + solver.ApplyNudges(ctx) + } +} diff --git a/game/physics/solver_composite_solo.go b/game/physics/solver_composite_solo.go new file mode 100644 index 00000000..eadb08e9 --- /dev/null +++ b/game/physics/solver_composite_solo.go @@ -0,0 +1,77 @@ +package physics + +// CompositeSoloConstraintSolver is a [SoloConstraintSolver] that combines +// several other solo constraint solvers into one, so that they can be +// registered as a single constraint through [SoloConstraintView.Create] +// - sharing one [SoloConstraintID], and consequently one enable/disable +// toggle - instead of one each. +// +// The composed solvers are driven in the order in which they were +// supplied to [NewCompositeSoloConstraintSolver]. Since they act on the +// same target one after another, rather than simultaneously, a solver +// later in the list observes any changes that an earlier one already +// made to the target during the same call. +// +// CompositeSoloConstraintSolver holds no configurable state of its own, +// so unlike most other solvers in this package, it has no Config type or +// Configure method; [NewCompositeSoloConstraintSolver] is the only way +// to obtain one. +type CompositeSoloConstraintSolver struct { + solvers []SoloConstraintSolver +} + +var _ SoloConstraintSolver = (*CompositeSoloConstraintSolver)(nil) + +// NewCompositeSoloConstraintSolver creates a new +// [CompositeSoloConstraintSolver] that combines solvers, in the given +// order. +func NewCompositeSoloConstraintSolver(solvers ...SoloConstraintSolver) *CompositeSoloConstraintSolver { + return &CompositeSoloConstraintSolver{ + solvers: solvers, + } +} + +// Solvers returns the solo constraint solvers that this solver combines, +// in the order in which they are driven. +func (s *CompositeSoloConstraintSolver) Solvers() []SoloConstraintSolver { + return s.solvers +} + +// Reset implements [SoloConstraintSolver.Reset]. +// +// It calls [SoloConstraintSolver.Reset] on each combined solver, in +// order. +func (s *CompositeSoloConstraintSolver) Reset(ctx SoloConstraintContext) { + for _, solver := range s.solvers { + solver.Reset(ctx) + } +} + +// ApplyImpulses implements [SoloConstraintSolver.ApplyImpulses]. +// +// It calls [SoloConstraintSolver.ApplyImpulses] on each combined solver, +// in order. This is safe without an intervening Reset, since impulses do +// not reposition the target and therefore cannot invalidate any +// position-derived state a combined solver cached during Reset. +func (s *CompositeSoloConstraintSolver) ApplyImpulses(ctx SoloConstraintContext) { + for _, solver := range s.solvers { + solver.ApplyImpulses(ctx) + } +} + +// ApplyNudges implements [SoloConstraintSolver.ApplyNudges]. +// +// It calls [SoloConstraintSolver.Reset] followed by +// [SoloConstraintSolver.ApplyNudges] on each combined solver in turn, +// rather than calling ApplyNudges on all of them in a single pass. This +// preserves the guarantee, documented on [SoloConstraintSolver.Reset], +// that Reset always immediately precedes ApplyNudges for a given solver +// - since a combined solver earlier in the list may reposition the +// target, which would otherwise leave a later solver's cached, +// position-derived state stale for the remainder of this call. +func (s *CompositeSoloConstraintSolver) ApplyNudges(ctx SoloConstraintContext) { + for _, solver := range s.solvers { + solver.Reset(ctx) // preserve engine reset behavior + solver.ApplyNudges(ctx) + } +} From b5045ad107521b57910ae3708512c0365e7ae0fe Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Wed, 5 Aug 2026 22:41:01 +0300 Subject: [PATCH 49/85] Remove sold old files --- game/physics/constraint/combined.go | 71 ------------------------- game/physics/constraint/differential.go | 57 -------------------- 2 files changed, 128 deletions(-) delete mode 100644 game/physics/constraint/combined.go delete mode 100644 game/physics/constraint/differential.go diff --git a/game/physics/constraint/combined.go b/game/physics/constraint/combined.go deleted file mode 100644 index c10fd7cb..00000000 --- a/game/physics/constraint/combined.go +++ /dev/null @@ -1,71 +0,0 @@ -package constraint - -// import "github.com/mokiat/lacking/game/physics/solver" - -// // NewCombined creates a new Combined solver based on the specified -// // sub-solvers. -// func NewCombined(delegates ...solver.Constraint) *Combined { -// return &Combined{ -// delegates: delegates, -// } -// } - -// var _ solver.Constraint = (*Combined)(nil) - -// // Combined is a single-object solver that delegates its logic to a -// // number of sub-solvers. -// type Combined struct { -// delegates []solver.Constraint -// } - -// func (s *Combined) Reset(ctx solver.Context) { -// for _, delegate := range s.delegates { -// delegate.Reset(ctx) -// } -// } - -// func (s *Combined) ApplyImpulses(ctx solver.Context) { -// for _, delegate := range s.delegates { -// delegate.ApplyImpulses(ctx) -// } -// } - -// func (s *Combined) ApplyNudges(ctx solver.Context) { -// for _, delegate := range s.delegates { -// delegate.ApplyNudges(ctx) -// } -// } - -// // NewPairCombined creates a new PairCombined solver based on the specified -// // sub-solvers. -// func NewPairCombined(delegates ...solver.PairConstraint) *PairCombined { -// return &PairCombined{ -// delegates: delegates, -// } -// } - -// var _ solver.PairConstraint = (*PairCombined)(nil) - -// // PairCombined is a double-object solver that delegates its logic to a -// // number of sub-solvers. -// type PairCombined struct { -// delegates []solver.PairConstraint -// } - -// func (s *PairCombined) Reset(ctx solver.PairContext) { -// for _, delegate := range s.delegates { -// delegate.Reset(ctx) -// } -// } - -// func (s *PairCombined) ApplyImpulses(ctx solver.PairContext) { -// for _, delegate := range s.delegates { -// delegate.ApplyImpulses(ctx) -// } -// } - -// func (s *PairCombined) ApplyNudges(ctx solver.PairContext) { -// for _, delegate := range s.delegates { -// delegate.ApplyNudges(ctx) -// } -// } diff --git a/game/physics/constraint/differential.go b/game/physics/constraint/differential.go deleted file mode 100644 index 7ced8b3b..00000000 --- a/game/physics/constraint/differential.go +++ /dev/null @@ -1,57 +0,0 @@ -package constraint - -// import ( -// "github.com/mokiat/gomath/dprec" -// "github.com/mokiat/lacking/game/physics/solver" -// ) - -// // NewDifferential creates a new Differential constraint solver. -// func NewDifferential() *Differential { -// return &Differential{ -// maxDelta: 20.0, -// } -// } - -// var _ solver.PairConstraint = (*Differential)(nil) - -// // Differential represents the solution for a constraint that keeps two -// // objects from rotating too much relative to one another over the local X -// // axis. -// type Differential struct { -// maxDelta float64 -// } - -// // MaxDelta returns the maximum difference in velocity that is allowed. -// func (d *Differential) MaxDelta() float64 { -// return d.maxDelta -// } - -// // SetMaxDelta changes the maximum difference in velocity that is allowed. -// func (d *Differential) SetMaxDelta(maxDelta float64) *Differential { -// d.maxDelta = maxDelta -// return d -// } - -// func (d *Differential) Reset(ctx solver.PairContext) {} - -// func (d *Differential) ApplyImpulses(ctx solver.PairContext) { -// targetAxisX := ctx.Target.Rotation().OrientationX() -// targetVelocity := dprec.Vec3Dot(targetAxisX, ctx.Target.AngularVelocity()) -// sourceAxisX := ctx.Source.Rotation().OrientationX() -// sourceVelocity := dprec.Vec3Dot(sourceAxisX, ctx.Source.AngularVelocity()) - -// var targetCorrection dprec.Vec3 -// var sourceCorrection dprec.Vec3 -// if delta := targetVelocity - sourceVelocity; delta > d.maxDelta { -// targetCorrection = dprec.Vec3Prod(targetAxisX, (d.maxDelta-delta)/2.0) -// sourceCorrection = dprec.Vec3Prod(sourceAxisX, (delta-d.maxDelta)/2.0) -// } -// if delta := sourceVelocity - targetVelocity; delta > d.maxDelta { -// sourceCorrection = dprec.Vec3Prod(sourceAxisX, (d.maxDelta-delta)/2.0) -// targetCorrection = dprec.Vec3Prod(targetAxisX, (delta-d.maxDelta)/2.0) -// } -// ctx.Target.SetAngularVelocity(dprec.Vec3Sum(ctx.Target.AngularVelocity(), targetCorrection)) -// ctx.Source.SetAngularVelocity(dprec.Vec3Sum(ctx.Source.AngularVelocity(), sourceCorrection)) -// } - -// func (d *Differential) ApplyNudges(ctx solver.PairContext) {} From 689641fe3e5414984c89d6f2427fbafaf0351efa Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Thu, 6 Aug 2026 23:44:59 +0300 Subject: [PATCH 50/85] Add helper CreateHandle methods --- game/physics/accelerator_body.go | 10 ++++++++++ game/physics/accelerator_global.go | 8 ++++++++ game/physics/constraint_pair.go | 12 ++++++++++++ game/physics/constraint_solo.go | 10 ++++++++++ game/physics/terrain.go | 8 ++++++++ 5 files changed, 48 insertions(+) diff --git a/game/physics/accelerator_body.go b/game/physics/accelerator_body.go index e9f130b1..8326990a 100644 --- a/game/physics/accelerator_body.go +++ b/game/physics/accelerator_body.go @@ -53,6 +53,16 @@ func (v BodyAcceleratorView) Create(bodyID BodyID, solver AccelerationSolver) Bo } } +// CreateHandle behaves like [BodyAcceleratorView.Create] but wraps the +// resulting ID in a [BodyAcceleratorHandle], as returned by +// [BodyAcceleratorView.Handle], for callers that want to keep acting on +// the new body accelerator without holding onto its ID separately. +// +// CreateHandle panics if bodyID does not reference a valid body. +func (v BodyAcceleratorView) CreateHandle(bodyID BodyID, solver AccelerationSolver) BodyAcceleratorHandle { + return v.Handle(v.Create(bodyID, solver)) +} + // Delete removes the body accelerator with the specified ID, unlinking it // from its target body and releasing the underlying storage for reuse. // diff --git a/game/physics/accelerator_global.go b/game/physics/accelerator_global.go index 7a6b8199..e0820abe 100644 --- a/game/physics/accelerator_global.go +++ b/game/physics/accelerator_global.go @@ -44,6 +44,14 @@ func (v GlobalAcceleratorView) Create(solver AccelerationSolver) GlobalAccelerat } } +// CreateHandle behaves like [GlobalAcceleratorView.Create] but wraps the +// resulting ID in a [GlobalAcceleratorHandle], as returned by +// [GlobalAcceleratorView.Handle], for callers that want to keep acting on +// the new global accelerator without holding onto its ID separately. +func (v GlobalAcceleratorView) CreateHandle(solver AccelerationSolver) GlobalAcceleratorHandle { + return v.Handle(v.Create(solver)) +} + // Delete removes the global accelerator with the specified ID. // // It panics if the ID does not reference a valid global accelerator, be it diff --git a/game/physics/constraint_pair.go b/game/physics/constraint_pair.go index b58aa94b..56106eab 100644 --- a/game/physics/constraint_pair.go +++ b/game/physics/constraint_pair.go @@ -231,6 +231,18 @@ func (v PairConstraintView) Create(primaryID, secondaryID BodyID, solver PairCon } } +// CreateHandle behaves like [PairConstraintView.Create] but wraps the +// resulting ID in a [PairConstraintHandle], as returned by +// [PairConstraintView.Handle], for callers that want to keep acting on +// the new pair constraint without holding onto its ID separately. +// +// CreateHandle panics if primaryID or secondaryID does not reference a +// valid body, or if they both reference the same body, since a pair +// constraint cannot act on a single body twice. +func (v PairConstraintView) CreateHandle(primaryID, secondaryID BodyID, solver PairConstraintSolver) PairConstraintHandle { + return v.Handle(v.Create(primaryID, secondaryID, solver)) +} + // Delete removes the pair constraint identified by id, unlinking it from // both of its target bodies and releasing the underlying storage for // reuse. diff --git a/game/physics/constraint_solo.go b/game/physics/constraint_solo.go index 2fd6ca99..be714c77 100644 --- a/game/physics/constraint_solo.go +++ b/game/physics/constraint_solo.go @@ -192,6 +192,16 @@ func (v SoloConstraintView) Create(bodyID BodyID, solver SoloConstraintSolver) S } } +// CreateHandle behaves like [SoloConstraintView.Create] but wraps the +// resulting ID in a [SoloConstraintHandle], as returned by +// [SoloConstraintView.Handle], for callers that want to keep acting on +// the new solo constraint without holding onto its ID separately. +// +// CreateHandle panics if bodyID does not reference a valid body. +func (v SoloConstraintView) CreateHandle(bodyID BodyID, solver SoloConstraintSolver) SoloConstraintHandle { + return v.Handle(v.Create(bodyID, solver)) +} + // Delete removes the solo constraint identified by id, unlinking it from // its target body and releasing the underlying storage for reuse. // diff --git a/game/physics/terrain.go b/game/physics/terrain.go index 6e464150..645aa46a 100644 --- a/game/physics/terrain.go +++ b/game/physics/terrain.go @@ -49,6 +49,14 @@ func (v TerrainView) Create(position dprec.Vec3, rotation dprec.Quat, mesh Colli } } +// CreateHandle behaves like [TerrainView.Create] but wraps the resulting +// ID in a [TerrainHandle], as returned by [TerrainView.Handle], for +// callers that want to keep acting on the new terrain without holding +// onto its ID separately. +func (v TerrainView) CreateHandle(position dprec.Vec3, rotation dprec.Quat, mesh CollisionMesh) TerrainHandle { + return v.Handle(v.Create(position, rotation, mesh)) +} + func (v TerrainView) Delete(id TerrainID) { terrain := v.resolve(id, true) From d76c23a8216062cb407bbcbaa56adde096c63f8a Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Thu, 6 Aug 2026 23:53:16 +0300 Subject: [PATCH 51/85] Add additional godoc to Handle types --- game/physics/accelerator_body.go | 8 ++++++++ game/physics/accelerator_global.go | 8 ++++++++ game/physics/body.go | 12 ++++++++++++ game/physics/constraint_pair.go | 8 ++++++++ game/physics/constraint_solo.go | 8 ++++++++ game/physics/terrain.go | 12 ++++++++++++ 6 files changed, 56 insertions(+) diff --git a/game/physics/accelerator_body.go b/game/physics/accelerator_body.go index 8326990a..582b459e 100644 --- a/game/physics/accelerator_body.go +++ b/game/physics/accelerator_body.go @@ -215,6 +215,14 @@ func (v BodyAcceleratorView) resolve(id BodyAcceleratorID, required bool) *bodyA // have to keep passing its ID around. // // It is created through [BodyAcceleratorView.Handle]. +// +// Wrapping an ID this way needs no allocation of its own, but unlike a +// plain [BodyAcceleratorID] (which holds no pointers), a Handle keeps an +// internal reference to the owning [Scene]. This keeps that Scene +// reachable for as long as the handle is retained, and adds a pointer +// that the garbage collector must trace wherever the handle is stored. +// Prefer storing IDs over handles in long-lived collections unless the +// convenience is worth that cost. type BodyAcceleratorHandle struct { view BodyAcceleratorView id BodyAcceleratorID diff --git a/game/physics/accelerator_global.go b/game/physics/accelerator_global.go index e0820abe..6fbc9e56 100644 --- a/game/physics/accelerator_global.go +++ b/game/physics/accelerator_global.go @@ -160,6 +160,14 @@ func (v GlobalAcceleratorView) resolve(id GlobalAcceleratorID, required bool) *g // do not have to keep passing its ID around. // // It is created through [GlobalAcceleratorView.Handle]. +// +// Wrapping an ID this way needs no allocation of its own, but unlike a +// plain [GlobalAcceleratorID] (which holds no pointers), a Handle keeps an +// internal reference to the owning [Scene]. This keeps that Scene +// reachable for as long as the handle is retained, and adds a pointer +// that the garbage collector must trace wherever the handle is stored. +// Prefer storing IDs over handles in long-lived collections unless the +// convenience is worth that cost. type GlobalAcceleratorHandle struct { view GlobalAcceleratorView id GlobalAcceleratorID diff --git a/game/physics/body.go b/game/physics/body.go index 446ef0b2..40a1cb9a 100644 --- a/game/physics/body.go +++ b/game/physics/body.go @@ -247,6 +247,18 @@ func (v BodyView) resolve(id BodyID, required bool) *bodyState { return body } +// BodyHandle is an object-oriented alternative to [BodyView] that is +// bound to a specific [BodyID]. +// +// It is obtained through [BodyView.Handle]. +// +// Wrapping an ID this way needs no allocation of its own, but unlike a +// plain [BodyID] (which holds no pointers), a Handle keeps an internal +// reference to the owning [Scene]. This keeps that Scene reachable for as +// long as the handle is retained, and adds a pointer that the garbage +// collector must trace wherever the handle is stored. Prefer storing IDs +// over handles in long-lived collections unless the convenience is worth +// that cost. type BodyHandle struct { view BodyView id BodyID diff --git a/game/physics/constraint_pair.go b/game/physics/constraint_pair.go index 56106eab..20d45fde 100644 --- a/game/physics/constraint_pair.go +++ b/game/physics/constraint_pair.go @@ -451,6 +451,14 @@ func (v PairConstraintView) resolve(id PairConstraintID, required bool) *pairCon // [PairConstraintView] that is bound to a specific [PairConstraintID]. // // It is obtained through [PairConstraintView.Handle]. +// +// Wrapping an ID this way needs no allocation of its own, but unlike a +// plain [PairConstraintID] (which holds no pointers), a Handle keeps an +// internal reference to the owning [Scene]. This keeps that Scene +// reachable for as long as the handle is retained, and adds a pointer +// that the garbage collector must trace wherever the handle is stored. +// Prefer storing IDs over handles in long-lived collections unless the +// convenience is worth that cost. type PairConstraintHandle struct { view PairConstraintView id PairConstraintID diff --git a/game/physics/constraint_solo.go b/game/physics/constraint_solo.go index be714c77..5b29871e 100644 --- a/game/physics/constraint_solo.go +++ b/game/physics/constraint_solo.go @@ -349,6 +349,14 @@ func (v SoloConstraintView) resolve(id SoloConstraintID, required bool) *soloCon // [SoloConstraintView] that is bound to a specific [SoloConstraintID]. // // It is obtained through [SoloConstraintView.Handle]. +// +// Wrapping an ID this way needs no allocation of its own, but unlike a +// plain [SoloConstraintID] (which holds no pointers), a Handle keeps an +// internal reference to the owning [Scene]. This keeps that Scene +// reachable for as long as the handle is retained, and adds a pointer +// that the garbage collector must trace wherever the handle is stored. +// Prefer storing IDs over handles in long-lived collections unless the +// convenience is worth that cost. type SoloConstraintHandle struct { view SoloConstraintView id SoloConstraintID diff --git a/game/physics/terrain.go b/game/physics/terrain.go index 645aa46a..2a57b986 100644 --- a/game/physics/terrain.go +++ b/game/physics/terrain.go @@ -118,6 +118,18 @@ func (v TerrainView) resolve(id TerrainID, required bool) *terrainState { return terrain } +// TerrainHandle is an object-oriented alternative to [TerrainView] that +// is bound to a specific [TerrainID]. +// +// It is obtained through [TerrainView.Handle]. +// +// Wrapping an ID this way needs no allocation of its own, but unlike a +// plain [TerrainID] (which holds no pointers), a Handle keeps an internal +// reference to the owning [Scene]. This keeps that Scene reachable for as +// long as the handle is retained, and adds a pointer that the garbage +// collector must trace wherever the handle is stored. Prefer storing IDs +// over handles in long-lived collections unless the convenience is worth +// that cost. type TerrainHandle struct { view TerrainView id TerrainID From 97494474b1ba9bf34cc503eee38be05afbc244b7 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Fri, 7 Aug 2026 00:45:52 +0300 Subject: [PATCH 52/85] Add coilover constraint solver --- game/physics/constraint/coilover.go | 139 -------------- game/physics/solver_coilover.go | 273 ++++++++++++++++++++++++++++ 2 files changed, 273 insertions(+), 139 deletions(-) delete mode 100644 game/physics/constraint/coilover.go create mode 100644 game/physics/solver_coilover.go diff --git a/game/physics/constraint/coilover.go b/game/physics/constraint/coilover.go deleted file mode 100644 index 91e4872a..00000000 --- a/game/physics/constraint/coilover.go +++ /dev/null @@ -1,139 +0,0 @@ -package constraint - -// import ( -// "github.com/mokiat/gomath/dprec" -// "github.com/mokiat/lacking/game/physics/solver" -// ) - -// // NewCoilover creates a new Coilover constraint solver. -// func NewCoilover() *Coilover { -// return &Coilover{ -// primaryRadius: dprec.ZeroVec3(), -// secondaryRadius: dprec.ZeroVec3(), -// frequency: 1.0, -// damping: 0.5, -// } -// } - -// var _ solver.PairConstraint = (*Coilover)(nil) - -// // Coilover represents the solution for a constraint that immitates -// // a car coilover through a damped harmonic oscillator. -// type Coilover struct { -// primaryRadius dprec.Vec3 -// secondaryRadius dprec.Vec3 -// frequency float64 -// damping float64 - -// appliedLambda float64 -// jacobian solver.PairJacobian -// drift float64 -// } - -// // PrimaryRadius returns the radius vector of the contact point -// // on the primary object. -// // -// // The vector is in the object's local space. -// func (s *Coilover) PrimaryRadius() dprec.Vec3 { -// return s.primaryRadius -// } - -// // SetPrimaryRadius changes the radius vector of the contact point -// // on the primary object. -// // -// // The vector is in the object's local space. -// func (s *Coilover) SetPrimaryRadius(radius dprec.Vec3) *Coilover { -// s.primaryRadius = radius -// return s -// } - -// // SecondaryRadius returns the radius vector of the contact point -// // on the secondary object. -// // -// // The vector is in the object's local space. -// func (s *Coilover) SecondaryRadius() dprec.Vec3 { -// return s.secondaryRadius -// } - -// // SetSecondaryRadius changes the radius vector of the contact point -// // on the secondary object. -// // -// // The vector is in the object's local space. -// func (s *Coilover) SetSecondaryRadius(radius dprec.Vec3) *Coilover { -// s.secondaryRadius = radius -// return s -// } - -// // Frequency returns the frequency (in Hz) of the damped harmonic -// // oscillator that represents this coilover. -// func (s *Coilover) Frequency() float64 { -// return s.frequency -// } - -// // SetFrequency changes the frequency (in Hz) of the damped harmonic -// // oscillator that represents this coilover. -// func (s *Coilover) SetFrequency(frequency float64) *Coilover { -// s.frequency = frequency -// return s -// } - -// // Damping returns the damping ratio of the damped harmonic oscillator -// // that represents this coilover. -// func (s *Coilover) Damping() float64 { -// return s.damping -// } - -// // SetDamping changes the damping ratio of the damped harmonic oscillator -// // that represents this coilover. -// func (s *Coilover) SetDamping(damping float64) *Coilover { -// s.damping = damping -// return s -// } - -// func (s *Coilover) Reset(ctx solver.PairContext) { -// s.appliedLambda = 0.0 - -// primaryRadiusWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.primaryRadius) -// primaryPointWS := dprec.Vec3Sum(ctx.Target.Position(), primaryRadiusWS) -// secondaryRadiusWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.secondaryRadius) -// secondaryPointWS := dprec.Vec3Sum(ctx.Source.Position(), secondaryRadiusWS) - -// deltaPosition := dprec.Vec3Diff(secondaryPointWS, primaryPointWS) -// s.drift = deltaPosition.Length() -// normal := dprec.BasisYVec3() -// if s.drift > solver.Epsilon { -// normal = dprec.UnitVec3(deltaPosition) -// } -// s.jacobian = solver.PairJacobian{ -// Target: solver.Jacobian{ -// LinearSlope: dprec.InverseVec3(normal), -// AngularSlope: dprec.Vec3Cross(normal, primaryRadiusWS), -// }, -// Source: solver.Jacobian{ -// LinearSlope: normal, -// AngularSlope: dprec.Vec3Cross(secondaryRadiusWS, normal), -// }, -// } -// } - -// func (s *Coilover) ApplyImpulses(ctx solver.PairContext) { -// if s.drift < solver.Epsilon { -// return -// } -// invertedEffectiveMass := s.jacobian.InverseEffectiveMass(ctx.Target, ctx.Source) -// w := 2.0 * dprec.Pi * s.frequency -// dc := 2.0 * s.damping * w / invertedEffectiveMass -// k := w * w / invertedEffectiveMass - -// gamma := 1.0 / (ctx.DeltaTime * (dc + ctx.DeltaTime*k)) -// beta := ctx.DeltaTime * k * gamma - -// effectiveVelocity := s.jacobian.EffectiveVelocity(ctx.Target, ctx.Source) -// lambda := -(effectiveVelocity + beta*s.drift + gamma*s.appliedLambda) / (invertedEffectiveMass + gamma) -// solution := s.jacobian.Impulse(lambda) -// ctx.Target.ApplyImpulse(solution.Target) -// ctx.Source.ApplyImpulse(solution.Source) -// s.appliedLambda += lambda -// } - -// func (s *Coilover) ApplyNudges(ctx solver.PairContext) {} diff --git a/game/physics/solver_coilover.go b/game/physics/solver_coilover.go new file mode 100644 index 00000000..9d82cba0 --- /dev/null +++ b/game/physics/solver_coilover.go @@ -0,0 +1,273 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +// CoiloverSolverConfig holds the parameters with which a [CoiloverSolver] +// is configured, either through [NewCoiloverSolver] or +// [CoiloverSolver.Configure]. +type CoiloverSolverConfig struct { + + // PrimaryBodyAnchorOffset is the body-local-space offset, relative to + // the primary target's center of mass, of the point at which the + // coilover is anchored on the primary body. + PrimaryBodyAnchorOffset dprec.Vec3 + + // SecondaryBodyAnchorOffset is the body-local-space offset, relative + // to the secondary target's center of mass, of the point at which + // the coilover is anchored on the secondary body. + SecondaryBodyAnchorOffset dprec.Vec3 + + // Frequency is the natural frequency, in Hz, of the spring-damper + // response that drives the two anchor points toward being + // RelaxedLength apart. + // + // A zero (or otherwise degenerate) Frequency disables the spring and + // damping forces entirely; [CoiloverSolver.ApplyImpulses] then falls + // back to only cancelling relative velocity along the constraint's + // axis, without pulling the anchors back toward RelaxedLength. + Frequency float64 + + // Damping is the damping ratio of the spring-damper response, in the + // [0.0, 1.0] range, where 0.0 leaves it undamped (it oscillates + // indefinitely around RelaxedLength) and 1.0 is critically damped + // (it settles as fast as possible without oscillating). + Damping float64 + + // RelaxedLength is the distance between the two anchor points at + // which the coilover exerts no force. + RelaxedLength float64 +} + +// CoiloverSolver is a [PairConstraintSolver] that models a coilover - the +// spring-damper suspension unit found on vehicles - acting between an +// anchor point on each of its two target bodies. +// +// Unlike a rigid constraint such as [FixedDistanceSolver], it does not +// hold its two anchor points at a fixed distance apart. Instead, it +// drives them toward CoiloverSolverConfig.RelaxedLength through a soft, +// tunable spring-damper response (see CoiloverSolverConfig.Frequency and +// CoiloverSolverConfig.Damping) applied entirely through +// [CoiloverSolver.ApplyImpulses]; [CoiloverSolver.ApplyNudges] is a +// no-op, since the softness of that response already accounts for any +// positional drift over successive steps. +// +// A CoiloverSolver must be configured, either through +// [NewCoiloverSolver] or [CoiloverSolver.Configure], before being +// registered with a [Scene] through [PairConstraintView.Create]. +type CoiloverSolver struct { + primaryBodyAnchorOffset dprec.Vec3 + secondaryBodyAnchorOffset dprec.Vec3 + frequency float64 + damping float64 + relaxedLength float64 + + appliedLambda float64 + primaryJacobian Jacobian + secondaryJacobian Jacobian + drift float64 +} + +var _ PairConstraintSolver = (*CoiloverSolver)(nil) + +// NewCoiloverSolver creates a new [CoiloverSolver] configured according +// to config. +func NewCoiloverSolver(config CoiloverSolverConfig) *CoiloverSolver { + result := &CoiloverSolver{} + result.Configure(config) + return result +} + +// Configure configures this solver according to config. +// +// Configure must be called before this solver is registered with a +// [Scene] through [PairConstraintView.Create]. Unlike +// [NewCoiloverSolver], it can be called on an already-allocated solver, +// which allows solvers to be cached (e.g. in a slice) and configured on +// demand. +func (s *CoiloverSolver) Configure(config CoiloverSolverConfig) { + s.primaryBodyAnchorOffset = config.PrimaryBodyAnchorOffset + s.secondaryBodyAnchorOffset = config.SecondaryBodyAnchorOffset + s.frequency = config.Frequency + s.damping = config.Damping + s.relaxedLength = config.RelaxedLength +} + +// PrimaryBodyAnchorOffset returns the body-local-space offset, relative +// to the primary target's center of mass, of the point at which the +// coilover is anchored on the primary body. +func (s *CoiloverSolver) PrimaryBodyAnchorOffset() dprec.Vec3 { + return s.primaryBodyAnchorOffset +} + +// SetPrimaryBodyAnchorOffset changes the body-local-space offset, +// relative to the primary target's center of mass, of the point at +// which the coilover is anchored on the primary body. +// +// It returns the solver itself, so that calls can be chained. +func (s *CoiloverSolver) SetPrimaryBodyAnchorOffset(offset dprec.Vec3) *CoiloverSolver { + s.primaryBodyAnchorOffset = offset + return s +} + +// SecondaryBodyAnchorOffset returns the body-local-space offset, +// relative to the secondary target's center of mass, of the point at +// which the coilover is anchored on the secondary body. +func (s *CoiloverSolver) SecondaryBodyAnchorOffset() dprec.Vec3 { + return s.secondaryBodyAnchorOffset +} + +// SetSecondaryBodyAnchorOffset changes the body-local-space offset, +// relative to the secondary target's center of mass, of the point at +// which the coilover is anchored on the secondary body. +// +// It returns the solver itself, so that calls can be chained. +func (s *CoiloverSolver) SetSecondaryBodyAnchorOffset(offset dprec.Vec3) *CoiloverSolver { + s.secondaryBodyAnchorOffset = offset + return s +} + +// Frequency returns the natural frequency, in Hz, of the spring-damper +// response that drives the two anchor points toward being RelaxedLength +// apart. +func (s *CoiloverSolver) Frequency() float64 { + return s.frequency +} + +// SetFrequency changes the natural frequency, in Hz, of the +// spring-damper response that drives the two anchor points toward being +// RelaxedLength apart. +// +// It returns the solver itself, so that calls can be chained. +func (s *CoiloverSolver) SetFrequency(frequency float64) *CoiloverSolver { + s.frequency = frequency + return s +} + +// Damping returns the damping ratio of the spring-damper response, in +// the [0.0, 1.0] range. +func (s *CoiloverSolver) Damping() float64 { + return s.damping +} + +// SetDamping changes the damping ratio of the spring-damper response, in +// the [0.0, 1.0] range. +// +// It returns the solver itself, so that calls can be chained. +func (s *CoiloverSolver) SetDamping(damping float64) *CoiloverSolver { + s.damping = damping + return s +} + +// RelaxedLength returns the distance between the two anchor points at +// which the coilover exerts no force. +func (s *CoiloverSolver) RelaxedLength() float64 { + return s.relaxedLength +} + +// SetRelaxedLength changes the distance between the two anchor points at +// which the coilover exerts no force. +// +// It returns the solver itself, so that calls can be chained. +func (s *CoiloverSolver) SetRelaxedLength(length float64) *CoiloverSolver { + s.relaxedLength = length + return s +} + +// Reset implements [PairConstraintSolver.Reset]. +// +// It recomputes the constraint's primary and secondary [Jacobian]s, +// along with the world-space offset from each target's center of mass to +// its respective anchor point (derived from +// CoiloverSolverConfig.PrimaryBodyAnchorOffset and +// CoiloverSolverConfig.SecondaryBodyAnchorOffset combined with each +// target's current rotation), and the current length error (drift) +// between the distance separating the two anchor points and +// CoiloverSolverConfig.RelaxedLength, based on the targets' current +// positions and rotations. It also clears any impulse accumulated by a +// prior [CoiloverSolver.ApplyImpulses] call, so that the spring-damper +// response starts fresh for the new step. +// +// If the two anchor points currently coincide, the constraint's axis is +// undefined; the world Y axis is used as a fallback in that degenerate +// case. +func (s *CoiloverSolver) Reset(ctx PairConstraintContext) { + primaryAnchorOffsetWS := dprec.QuatVec3Rotation(ctx.PrimaryTarget.Rotation(), s.primaryBodyAnchorOffset) + primaryAnchorWS := dprec.Vec3Sum(ctx.PrimaryTarget.Position(), primaryAnchorOffsetWS) + + secondaryAnchorOffsetWS := dprec.QuatVec3Rotation(ctx.SecondaryTarget.Rotation(), s.secondaryBodyAnchorOffset) + secondaryAnchorWS := dprec.Vec3Sum(ctx.SecondaryTarget.Position(), secondaryAnchorOffsetWS) + + delta := dprec.Vec3Diff(secondaryAnchorWS, primaryAnchorWS) + actualDistance := delta.Length() + + normal := dprec.BasisYVec3() + if actualDistance > Epsilon { + normal = dprec.Vec3Quot(delta, actualDistance) + } + + s.appliedLambda = 0.0 // reset applied impulse + s.primaryJacobian = Jacobian{ + LinearSlope: dprec.InverseVec3(normal), + AngularSlope: dprec.Vec3Cross(normal, primaryAnchorOffsetWS), + } + s.secondaryJacobian = Jacobian{ + LinearSlope: normal, + AngularSlope: dprec.Vec3Cross(secondaryAnchorOffsetWS, normal), + } + s.drift = actualDistance - s.relaxedLength +} + +// ApplyImpulses implements [PairConstraintSolver.ApplyImpulses]. +// +// It resolves a pair of impulses that drive the anchor points' relative +// velocity along the constraint's axis according to a soft +// spring-damper response tuned by CoiloverSolverConfig.Frequency and +// CoiloverSolverConfig.Damping, pulling the two anchors back toward +// being CoiloverSolverConfig.RelaxedLength apart. The impulse is +// accumulated internally across successive calls within the same step +// and fed back into the response, as is standard practice for soft +// constraints. +// +// If Frequency is zero (or otherwise produces a degenerate spring +// constant), this falls back to only cancelling relative velocity along +// the constraint's axis, without pulling the anchors toward +// RelaxedLength. +// +// ApplyImpulses does nothing if both targets have infinite effective +// mass along the constraint's axis (e.g. both are static, or otherwise +// immovable along it). +func (s *CoiloverSolver) ApplyImpulses(ctx PairConstraintContext) { + invEffectiveMass := s.primaryJacobian.InverseEffectiveMass(ctx.PrimaryTarget) + s.secondaryJacobian.InverseEffectiveMass(ctx.SecondaryTarget) + if invEffectiveMass < Epsilon { + return // infinite effective mass + } + + w := 2.0 * dprec.Pi * s.frequency + dc := 2.0 * s.damping * w / invEffectiveMass + k := w * w / invEffectiveMass + + var gamma, beta float64 + if k > Epsilon { + gamma = 1.0 / (ctx.DeltaSeconds * (dc + ctx.DeltaSeconds*k)) + beta = ctx.DeltaSeconds * k * gamma + } + + effVelocity := s.primaryJacobian.EffectiveVelocity(ctx.PrimaryTarget) + s.secondaryJacobian.EffectiveVelocity(ctx.SecondaryTarget) + lambda := -(effVelocity + beta*s.drift + gamma*s.appliedLambda) / (invEffectiveMass + gamma) + s.appliedLambda += lambda + + primaryImpulse := s.primaryJacobian.Impulse(lambda) + ctx.PrimaryTarget.ApplyImpulse(primaryImpulse) + + secondaryImpulse := s.secondaryJacobian.Impulse(lambda) + ctx.SecondaryTarget.ApplyImpulse(secondaryImpulse) +} + +// ApplyNudges implements [PairConstraintSolver.ApplyNudges]. +// +// It does nothing. Unlike a rigid constraint, a coilover's softness (see +// CoiloverSolverConfig.Frequency and CoiloverSolverConfig.Damping) +// already accounts for positional drift through +// [CoiloverSolver.ApplyImpulses], so no separate position-level +// correction is needed. +func (s *CoiloverSolver) ApplyNudges(ctx PairConstraintContext) {} From 2cc458a2816c39e3915249c6c6ffdd85775c1957 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Fri, 7 Aug 2026 01:06:46 +0300 Subject: [PATCH 53/85] Rework of Constraint contract --- game/physics/constraint_pair.go | 25 ++++++++--- game/physics/constraint_solo.go | 25 ++++++++--- game/physics/scene.go | 2 - game/physics/solver_collision_pair.go | 64 +++++++++++++++----------- game/physics/solver_collision_solo.go | 37 ++++++++++----- game/physics/solver_composite_pair.go | 15 +++---- game/physics/solver_composite_solo.go | 15 +++---- game/physics/solver_fixed_distance.go | 65 ++++++++++++++++----------- 8 files changed, 150 insertions(+), 98 deletions(-) diff --git a/game/physics/constraint_pair.go b/game/physics/constraint_pair.go index 20d45fde..1a1dbbe0 100644 --- a/game/physics/constraint_pair.go +++ b/game/physics/constraint_pair.go @@ -142,12 +142,12 @@ type PairConstraintSolver interface { // This is called once before the first // [PairConstraintSolver.ApplyImpulses] iteration of a step, since the // target bodies' positions and orientations remain unchanged - // throughout that loop. + // throughout that loop. This amortizes that recomputation across + // every iteration of the loop, rather than repeating it on each one. // - // This is also called before every single - // [PairConstraintSolver.ApplyNudges] invocation, since nudges - // reposition the target bodies and would otherwise leave the solver's - // cached, position-derived data stale for subsequent iterations. + // Reset is not called again before [PairConstraintSolver.ApplyNudges] + // - see that method for how position-derived state is kept fresh once + // nudges start repositioning the target bodies. Reset(ctx PairConstraintContext) // ApplyImpulses is called by the physics engine to instruct the @@ -156,7 +156,10 @@ type PairConstraintSolver interface { // satisfied. // // This is called multiple times per step, once for each impulse - // resolution iteration. + // resolution iteration, always preceded by exactly one call to + // [PairConstraintSolver.Reset] for the step. Since impulses only + // change velocity, not position, any position-derived state Reset + // computed remains valid for every iteration. ApplyImpulses(ctx PairConstraintContext) // ApplyNudges is called by the physics engine to instruct the solver @@ -164,7 +167,15 @@ type PairConstraintSolver interface { // correct their positions so that the constraint is satisfied. // // This is called multiple times per step, once for each nudge - // resolution iteration. + // resolution iteration, with no preceding call to + // [PairConstraintSolver.Reset]. Unlike + // [PairConstraintSolver.ApplyImpulses], each call may follow a change + // in either target body's position or orientation - caused by this + // solver's own previous iteration, or by another constraint acting on + // either target - so an implementation that caches position-derived + // state (e.g. Jacobians) is responsible for recomputing it itself at + // the start of every call, rather than relying on stale state left + // over from Reset. ApplyNudges(ctx PairConstraintContext) } diff --git a/game/physics/constraint_solo.go b/game/physics/constraint_solo.go index 5b29871e..dd409389 100644 --- a/game/physics/constraint_solo.go +++ b/game/physics/constraint_solo.go @@ -114,12 +114,12 @@ type SoloConstraintSolver interface { // This is called once before the first // [SoloConstraintSolver.ApplyImpulses] iteration of a step, since the // target body's position and orientation remain unchanged throughout - // that loop. + // that loop. This amortizes that recomputation across every + // iteration of the loop, rather than repeating it on each one. // - // This is also called before every single - // [SoloConstraintSolver.ApplyNudges] invocation, since nudges - // reposition the target body and would otherwise leave the solver's - // cached, position-derived data stale for subsequent iterations. + // Reset is not called again before [SoloConstraintSolver.ApplyNudges] + // - see that method for how position-derived state is kept fresh once + // nudges start repositioning the target body. Reset(ctx SoloConstraintContext) // ApplyImpulses is called by the physics engine to instruct the solver @@ -127,7 +127,10 @@ type SoloConstraintSolver interface { // correct its velocity so that the constraint is satisfied. // // This is called multiple times per step, once for each impulse - // resolution iteration. + // resolution iteration, always preceded by exactly one call to + // [SoloConstraintSolver.Reset] for the step. Since impulses only + // change velocity, not position, any position-derived state Reset + // computed remains valid for every iteration. ApplyImpulses(ctx SoloConstraintContext) // ApplyNudges is called by the physics engine to instruct the solver @@ -135,7 +138,15 @@ type SoloConstraintSolver interface { // correct its position so that the constraint is satisfied. // // This is called multiple times per step, once for each nudge - // resolution iteration. + // resolution iteration, with no preceding call to + // [SoloConstraintSolver.Reset]. Unlike + // [SoloConstraintSolver.ApplyImpulses], each call may follow a change + // in the target body's position or orientation - caused by this + // solver's own previous iteration, or by another constraint acting on + // the same target - so an implementation that caches position-derived + // state (e.g. Jacobians) is responsible for recomputing it itself at + // the start of every call, rather than relying on stale state left + // over from Reset. ApplyNudges(ctx SoloConstraintContext) } diff --git a/game/physics/scene.go b/game/physics/scene.go index 06100005..9985a7f1 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -715,7 +715,6 @@ func (s *Scene) applyNudges(elapsedSeconds float64) { SecondaryTarget: newConstraintTarget(secondaryBody), } - constraint.solver.Reset(ctx) constraint.solver.ApplyNudges(ctx) }) @@ -729,7 +728,6 @@ func (s *Scene) applyNudges(elapsedSeconds float64) { Target: newConstraintTarget(body), } - constraint.solver.Reset(ctx) constraint.solver.ApplyNudges(ctx) }) diff --git a/game/physics/solver_collision_pair.go b/game/physics/solver_collision_pair.go index db226478..a752f5d7 100644 --- a/game/physics/solver_collision_pair.go +++ b/game/physics/solver_collision_pair.go @@ -123,32 +123,10 @@ func (s *PairCollisionSolver) Configure(config PairCollisionSolverConfig) { // Reset implements [PairConstraintSolver.Reset]. // -// It recomputes the contact's primary and secondary [Jacobian]s, along -// with the world-space offsets from each target's center of mass to its -// respective contact point that they are derived from, based on the -// targets' current positions. -// -// Each jacobian's linear slope is built from the other body's contact -// normal (e.g. the primary jacobian uses SecondaryContactNormal, not -// PrimaryContactNormal) rather than from an explicit negation, relying -// on the two normals being approximately antiparallel. This is what -// allows a single lambda, as computed by [PairConstraintContext], to be -// applied to both targets - see -// [PairConstraintContext.ImpulseLambda] for that requirement. +// It recomputes the contact's primary and secondary [Jacobian]s, the +// same way [PairCollisionSolver.recompute] does. func (s *PairCollisionSolver) Reset(ctx PairConstraintContext) { - s.primaryPointOffsetWS = dprec.Vec3Diff(s.primaryContactPoint, ctx.PrimaryTarget.Position()) - s.secondaryPointOffsetWS = dprec.Vec3Diff(s.secondaryContactPoint, ctx.SecondaryTarget.Position()) - - s.primaryJacobian = Jacobian{ - LinearSlope: s.secondaryContactNormal, - AngularSlope: dprec.Vec3Cross(s.primaryPointOffsetWS, s.secondaryContactNormal), - } - s.secondaryJacobian = Jacobian{ - LinearSlope: s.primaryContactNormal, - AngularSlope: dprec.Vec3Cross(s.secondaryPointOffsetWS, s.primaryContactNormal), - } - - s.drift = s.contactDepth + s.recompute(ctx) } // ApplyImpulses implements [PairConstraintSolver.ApplyImpulses]. @@ -203,12 +181,46 @@ func (s *PairCollisionSolver) ApplyImpulses(ctx PairConstraintContext) { // ApplyNudges implements [PairConstraintSolver.ApplyNudges]. // -// If the contact is still penetrating, it nudges the two targets apart +// It first recomputes the contact's primary and secondary [Jacobian]s, +// the same way [PairCollisionSolver.recompute] does, since a preceding +// nudge - by this solver's own previous iteration, or by another +// constraint acting on either target - may have moved a target since +// [PairCollisionSolver.Reset] or the last call to this method. If the +// contact is still penetrating, it then nudges the two targets apart // along their contact normals to reduce the penetration. func (s *PairCollisionSolver) ApplyNudges(ctx PairConstraintContext) { + s.recompute(ctx) if s.drift > 0.0 { primaryNudge, secondaryNudge := ctx.NudgeSolution(s.primaryJacobian, s.secondaryJacobian, s.drift) ctx.PrimaryTarget.ApplyNudge(primaryNudge) ctx.SecondaryTarget.ApplyNudge(secondaryNudge) } } + +// recompute recalculates the contact's primary and secondary +// [Jacobian]s, along with the world-space offsets from each target's +// center of mass to its respective contact point that they are derived +// from, based on the targets' current positions. +// +// Each jacobian's linear slope is built from the other body's contact +// normal (e.g. the primary jacobian uses SecondaryContactNormal, not +// PrimaryContactNormal) rather than from an explicit negation, relying +// on the two normals being approximately antiparallel. This is what +// allows a single lambda, as computed by [PairConstraintContext], to be +// applied to both targets - see [PairConstraintContext.ImpulseLambda] +// for that requirement. +func (s *PairCollisionSolver) recompute(ctx PairConstraintContext) { + s.primaryPointOffsetWS = dprec.Vec3Diff(s.primaryContactPoint, ctx.PrimaryTarget.Position()) + s.secondaryPointOffsetWS = dprec.Vec3Diff(s.secondaryContactPoint, ctx.SecondaryTarget.Position()) + + s.primaryJacobian = Jacobian{ + LinearSlope: s.secondaryContactNormal, + AngularSlope: dprec.Vec3Cross(s.primaryPointOffsetWS, s.secondaryContactNormal), + } + s.secondaryJacobian = Jacobian{ + LinearSlope: s.primaryContactNormal, + AngularSlope: dprec.Vec3Cross(s.secondaryPointOffsetWS, s.primaryContactNormal), + } + + s.drift = s.contactDepth +} diff --git a/game/physics/solver_collision_solo.go b/game/physics/solver_collision_solo.go index 686517d8..287695ea 100644 --- a/game/physics/solver_collision_solo.go +++ b/game/physics/solver_collision_solo.go @@ -102,18 +102,10 @@ func (s *SoloCollisionSolver) Configure(config SoloCollisionSolverConfig) { // Reset implements [SoloConstraintSolver.Reset]. // -// It recomputes the contact's [Jacobian], along with the world-space -// offset from the target's center of mass to the contact point that it -// is derived from, based on the target's current position. +// It recomputes the contact's [Jacobian], the same way +// [SoloCollisionSolver.recompute] does. func (s *SoloCollisionSolver) Reset(ctx SoloConstraintContext) { - s.pointOffsetWS = dprec.Vec3Diff(s.bodyContactPoint, ctx.Target.Position()) - - s.jacobian = Jacobian{ - LinearSlope: s.terrainContactNormal, - AngularSlope: dprec.Vec3Cross(s.pointOffsetWS, s.terrainContactNormal), - } - - s.drift = s.contactDepth + s.recompute(ctx) } // ApplyImpulses implements [SoloConstraintSolver.ApplyImpulses]. @@ -158,11 +150,32 @@ func (s *SoloCollisionSolver) ApplyImpulses(ctx SoloConstraintContext) { // ApplyNudges implements [SoloConstraintSolver.ApplyNudges]. // -// If the contact is still penetrating, it nudges the target along the +// It first recomputes the contact's [Jacobian], the same way +// [SoloCollisionSolver.recompute] does, since a preceding nudge - by +// this solver's own previous iteration, or by another constraint acting +// on the same target - may have moved the target since +// [SoloCollisionSolver.Reset] or the last call to this method. If the +// contact is still penetrating, it then nudges the target along the // terrain's contact normal to reduce the penetration. func (s *SoloCollisionSolver) ApplyNudges(ctx SoloConstraintContext) { + s.recompute(ctx) if s.drift > 0.0 { nudge := ctx.NudgeSolution(s.jacobian, s.drift) ctx.Target.ApplyNudge(nudge) } } + +// recompute recalculates the contact's [Jacobian], along with the +// world-space offset from the target's center of mass to the contact +// point that it is derived from, based on the target's current +// position. +func (s *SoloCollisionSolver) recompute(ctx SoloConstraintContext) { + s.pointOffsetWS = dprec.Vec3Diff(s.bodyContactPoint, ctx.Target.Position()) + + s.jacobian = Jacobian{ + LinearSlope: s.terrainContactNormal, + AngularSlope: dprec.Vec3Cross(s.pointOffsetWS, s.terrainContactNormal), + } + + s.drift = s.contactDepth +} diff --git a/game/physics/solver_composite_pair.go b/game/physics/solver_composite_pair.go index edde841c..cee55d0a 100644 --- a/game/physics/solver_composite_pair.go +++ b/game/physics/solver_composite_pair.go @@ -61,17 +61,14 @@ func (s *CompositePairConstraintSolver) ApplyImpulses(ctx PairConstraintContext) // ApplyNudges implements [PairConstraintSolver.ApplyNudges]. // -// It calls [PairConstraintSolver.Reset] followed by -// [PairConstraintSolver.ApplyNudges] on each combined solver in turn, -// rather than calling ApplyNudges on all of them in a single pass. This -// preserves the guarantee, documented on [PairConstraintSolver.Reset], -// that Reset always immediately precedes ApplyNudges for a given solver -// - since a combined solver earlier in the list may reposition either -// target, which would otherwise leave a later solver's cached, -// position-derived state stale for the remainder of this call. +// It calls [PairConstraintSolver.ApplyNudges] on each combined solver, in +// order. A combined solver earlier in the list may reposition either +// target, but this requires no special handling here - per +// [PairConstraintSolver.ApplyNudges], each combined solver is already +// responsible for recomputing any position-derived state it needs at the +// start of its own call. func (s *CompositePairConstraintSolver) ApplyNudges(ctx PairConstraintContext) { for _, solver := range s.solvers { - solver.Reset(ctx) // preserve engine reset behavior solver.ApplyNudges(ctx) } } diff --git a/game/physics/solver_composite_solo.go b/game/physics/solver_composite_solo.go index eadb08e9..6e77a071 100644 --- a/game/physics/solver_composite_solo.go +++ b/game/physics/solver_composite_solo.go @@ -61,17 +61,14 @@ func (s *CompositeSoloConstraintSolver) ApplyImpulses(ctx SoloConstraintContext) // ApplyNudges implements [SoloConstraintSolver.ApplyNudges]. // -// It calls [SoloConstraintSolver.Reset] followed by -// [SoloConstraintSolver.ApplyNudges] on each combined solver in turn, -// rather than calling ApplyNudges on all of them in a single pass. This -// preserves the guarantee, documented on [SoloConstraintSolver.Reset], -// that Reset always immediately precedes ApplyNudges for a given solver -// - since a combined solver earlier in the list may reposition the -// target, which would otherwise leave a later solver's cached, -// position-derived state stale for the remainder of this call. +// It calls [SoloConstraintSolver.ApplyNudges] on each combined solver, in +// order. A combined solver earlier in the list may reposition the +// target, but this requires no special handling here - per +// [SoloConstraintSolver.ApplyNudges], each combined solver is already +// responsible for recomputing any position-derived state it needs at the +// start of its own call. func (s *CompositeSoloConstraintSolver) ApplyNudges(ctx SoloConstraintContext) { for _, solver := range s.solvers { - solver.Reset(ctx) // preserve engine reset behavior solver.ApplyNudges(ctx) } } diff --git a/game/physics/solver_fixed_distance.go b/game/physics/solver_fixed_distance.go index 17650376..29614db6 100644 --- a/game/physics/solver_fixed_distance.go +++ b/game/physics/solver_fixed_distance.go @@ -110,31 +110,10 @@ func (s *FixedDistanceSolver) SetDistance(distance float64) *FixedDistanceSolver // Reset implements [SoloConstraintSolver.Reset]. // -// It recomputes the constraint's [Jacobian], along with the world-space -// offset from the target's center of mass to its anchor point (derived -// from BodyAnchorOffset and the target's current rotation), and the -// current distance error (drift) between that anchor point and -// FixedPoint, based on the target's current position and rotation. -// -// If the anchor point currently coincides with FixedPoint, the -// constraint direction is undefined; an arbitrary axis is used as a -// fallback in that degenerate case. +// It recomputes the constraint's [Jacobian] and current distance error +// (drift), the same way [FixedDistanceSolver.recompute] does. func (s *FixedDistanceSolver) Reset(ctx SoloConstraintContext) { - anchorOffsetWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.bodyAnchorOffset) - anchorWS := dprec.Vec3Sum(ctx.Target.Position(), anchorOffsetWS) - delta := dprec.Vec3Diff(anchorWS, s.fixedPoint) - - normal := dprec.BasisXVec3() - actualDistance := delta.Length() - if actualDistance > Epsilon { - normal = dprec.UnitVec3(delta) - } - - s.jacobian = Jacobian{ - LinearSlope: normal, - AngularSlope: dprec.Vec3Cross(anchorOffsetWS, normal), - } - s.drift = s.distance - actualDistance + s.recompute(ctx) } // ApplyImpulses implements [SoloConstraintSolver.ApplyImpulses]. @@ -151,9 +130,43 @@ func (s *FixedDistanceSolver) ApplyImpulses(ctx SoloConstraintContext) { // ApplyNudges implements [SoloConstraintSolver.ApplyNudges]. // -// It nudges the target's position and rotation to reduce any remaining -// distance error (drift) between its anchor point and FixedPoint. +// It first recomputes the constraint's [Jacobian] and current distance +// error (drift), the same way [FixedDistanceSolver.recompute] does, +// since a preceding nudge - by this solver's own previous iteration, or +// by another constraint acting on the same target - may have moved the +// target since [FixedDistanceSolver.Reset] or the last call to this +// method. It then nudges the target's position and rotation to reduce +// any remaining distance error between its anchor point and FixedPoint. func (s *FixedDistanceSolver) ApplyNudges(ctx SoloConstraintContext) { + s.recompute(ctx) nudge := ctx.NudgeSolution(s.jacobian, s.drift) ctx.Target.ApplyNudge(nudge) } + +// recompute recalculates the constraint's [Jacobian], along with the +// world-space offset from the target's center of mass to its anchor +// point (derived from BodyAnchorOffset and the target's current +// rotation), and the current distance error (drift) between that anchor +// point and FixedPoint, based on the target's current position and +// rotation. +// +// If the anchor point currently coincides with FixedPoint, the +// constraint direction is undefined; an arbitrary axis is used as a +// fallback in that degenerate case. +func (s *FixedDistanceSolver) recompute(ctx SoloConstraintContext) { + anchorOffsetWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.bodyAnchorOffset) + anchorWS := dprec.Vec3Sum(ctx.Target.Position(), anchorOffsetWS) + delta := dprec.Vec3Diff(anchorWS, s.fixedPoint) + + normal := dprec.BasisXVec3() + actualDistance := delta.Length() + if actualDistance > Epsilon { + normal = dprec.UnitVec3(delta) + } + + s.jacobian = Jacobian{ + LinearSlope: normal, + AngularSlope: dprec.Vec3Cross(anchorOffsetWS, normal), + } + s.drift = s.distance - actualDistance +} From e045139a54f116d7f2ae85e721696541433f6fe0 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sat, 8 Aug 2026 14:42:50 +0300 Subject: [PATCH 54/85] Minor improvement to Coilover solveer --- game/physics/solver_coilover.go | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/game/physics/solver_coilover.go b/game/physics/solver_coilover.go index 9d82cba0..e3e1cb4f 100644 --- a/game/physics/solver_coilover.go +++ b/game/physics/solver_coilover.go @@ -214,7 +214,7 @@ func (s *CoiloverSolver) Reset(ctx PairConstraintContext) { LinearSlope: normal, AngularSlope: dprec.Vec3Cross(secondaryAnchorOffsetWS, normal), } - s.drift = actualDistance - s.relaxedLength + s.drift = s.relaxedLength - actualDistance } // ApplyImpulses implements [PairConstraintSolver.ApplyImpulses]. @@ -253,7 +253,7 @@ func (s *CoiloverSolver) ApplyImpulses(ctx PairConstraintContext) { } effVelocity := s.primaryJacobian.EffectiveVelocity(ctx.PrimaryTarget) + s.secondaryJacobian.EffectiveVelocity(ctx.SecondaryTarget) - lambda := -(effVelocity + beta*s.drift + gamma*s.appliedLambda) / (invEffectiveMass + gamma) + lambda := -(effVelocity - beta*s.drift + gamma*s.appliedLambda) / (invEffectiveMass + gamma) s.appliedLambda += lambda primaryImpulse := s.primaryJacobian.Impulse(lambda) From fab9d5a987747ebcaa5512d6717b86db7fe6ce6d Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sat, 8 Aug 2026 14:45:51 +0300 Subject: [PATCH 55/85] Add distance constraint solver --- game/physics/solver_distance.go | 198 ++++++++++++++++++++++++++++++++ 1 file changed, 198 insertions(+) create mode 100644 game/physics/solver_distance.go diff --git a/game/physics/solver_distance.go b/game/physics/solver_distance.go new file mode 100644 index 00000000..f855d8e5 --- /dev/null +++ b/game/physics/solver_distance.go @@ -0,0 +1,198 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +// DistanceSolverConfig holds the parameters with which a [DistanceSolver] +// is configured, either through [NewDistanceSolver] or +// [DistanceSolver.Configure]. +type DistanceSolverConfig struct { + + // PrimaryBodyAnchorOffset is the body-local-space offset, relative to + // the primary target's center of mass, of the point at which the + // distance constraint is anchored on the primary body. + PrimaryBodyAnchorOffset dprec.Vec3 + + // SecondaryBodyAnchorOffset is the body-local-space offset, relative + // to the secondary target's center of mass, of the point at which + // the distance constraint is anchored on the secondary body. + SecondaryBodyAnchorOffset dprec.Vec3 + + // Distance is the distance at which the two anchor points are held + // apart. + Distance float64 +} + +// DistanceSolver is a [PairConstraintSolver] that holds an anchor point on +// each of its two target bodies at a fixed distance apart, acting like a +// rigid rod between the two - it resists the anchor points moving both +// closer together and farther apart. +// +// Unlike [FixedDistanceSolver], which anchors a single target to a fixed +// point in world space, DistanceSolver acts between two independently +// moving targets. +// +// A DistanceSolver must be configured, either through [NewDistanceSolver] +// or [DistanceSolver.Configure], before being registered with a [Scene] +// through [PairConstraintView.Create]. +type DistanceSolver struct { + primaryBodyAnchorOffset dprec.Vec3 + secondaryBodyAnchorOffset dprec.Vec3 + distance float64 + + primaryJacobian Jacobian + secondaryJacobian Jacobian + drift float64 +} + +var _ PairConstraintSolver = (*DistanceSolver)(nil) + +// NewDistanceSolver creates a new [DistanceSolver] configured according to +// config. +func NewDistanceSolver(config DistanceSolverConfig) *DistanceSolver { + result := &DistanceSolver{} + result.Configure(config) + return result +} + +// Configure configures this solver according to config. Negative +// Distance values are clamped to zero, as with [DistanceSolver.SetDistance]. +// +// Configure must be called before this solver is registered with a +// [Scene] through [PairConstraintView.Create]. Unlike [NewDistanceSolver], +// it can be called on an already-allocated solver, which allows solvers to +// be cached (e.g. in a slice) and configured on demand. +func (s *DistanceSolver) Configure(config DistanceSolverConfig) { + s.primaryBodyAnchorOffset = config.PrimaryBodyAnchorOffset + s.secondaryBodyAnchorOffset = config.SecondaryBodyAnchorOffset + s.distance = max(0.0, config.Distance) +} + +// PrimaryBodyAnchorOffset returns the body-local-space offset, relative +// to the primary target's center of mass, of the point at which the +// distance constraint is anchored on the primary body. +func (s *DistanceSolver) PrimaryBodyAnchorOffset() dprec.Vec3 { + return s.primaryBodyAnchorOffset +} + +// SetPrimaryBodyAnchorOffset changes the body-local-space offset, +// relative to the primary target's center of mass, of the point at +// which the distance constraint is anchored on the primary body. +// +// It returns the solver itself, so that calls can be chained. +func (s *DistanceSolver) SetPrimaryBodyAnchorOffset(offset dprec.Vec3) *DistanceSolver { + s.primaryBodyAnchorOffset = offset + return s +} + +// SecondaryBodyAnchorOffset returns the body-local-space offset, +// relative to the secondary target's center of mass, of the point at +// which the distance constraint is anchored on the secondary body. +func (s *DistanceSolver) SecondaryBodyAnchorOffset() dprec.Vec3 { + return s.secondaryBodyAnchorOffset +} + +// SetSecondaryBodyAnchorOffset changes the body-local-space offset, +// relative to the secondary target's center of mass, of the point at +// which the distance constraint is anchored on the secondary body. +// +// It returns the solver itself, so that calls can be chained. +func (s *DistanceSolver) SetSecondaryBodyAnchorOffset(offset dprec.Vec3) *DistanceSolver { + s.secondaryBodyAnchorOffset = offset + return s +} + +// Distance returns the distance at which the two anchor points are held +// apart. +func (s *DistanceSolver) Distance() float64 { + return s.distance +} + +// SetDistance changes the distance at which the two anchor points are +// held apart. Negative values are clamped to zero. +// +// It returns the solver itself, so that calls can be chained. +func (s *DistanceSolver) SetDistance(distance float64) *DistanceSolver { + s.distance = max(0.0, distance) + return s +} + +// Reset implements [PairConstraintSolver.Reset]. +// +// It recomputes the constraint's primary and secondary [Jacobian]s and +// current distance error (drift), the same way +// [DistanceSolver.recompute] does. +func (s *DistanceSolver) Reset(ctx PairConstraintContext) { + s.recompute(ctx) +} + +// ApplyImpulses implements [PairConstraintSolver.ApplyImpulses]. +// +// It resolves a pair of impulses, without restitution, that drive the +// anchor points' relative velocity toward closing the distance error +// (drift) computed by [DistanceSolver.Reset], pulling the anchor points +// together when they are too far apart and pushing them apart when they +// are too close. +func (s *DistanceSolver) ApplyImpulses(ctx PairConstraintContext) { + primaryImpulse, secondaryImpulse := ctx.ImpulseSolution( + s.primaryJacobian, s.secondaryJacobian, s.drift, 0.0, + ) + ctx.PrimaryTarget.ApplyImpulse(primaryImpulse) + ctx.SecondaryTarget.ApplyImpulse(secondaryImpulse) +} + +// ApplyNudges implements [PairConstraintSolver.ApplyNudges]. +// +// It first recomputes the constraint's primary and secondary [Jacobian]s +// and current distance error (drift), the same way +// [DistanceSolver.recompute] does, since a preceding nudge - by this +// solver's own previous iteration, or by another constraint acting on +// either target - may have moved either target since +// [DistanceSolver.Reset] or the last call to this method. It then nudges +// both targets' positions and rotations to reduce any remaining distance +// error between their anchor points. +func (s *DistanceSolver) ApplyNudges(ctx PairConstraintContext) { + s.recompute(ctx) + + primaryNudge, secondaryNudge := ctx.NudgeSolution( + s.primaryJacobian, s.secondaryJacobian, s.drift, + ) + ctx.PrimaryTarget.ApplyNudge(primaryNudge) + ctx.SecondaryTarget.ApplyNudge(secondaryNudge) +} + +// recompute recalculates the constraint's primary and secondary +// [Jacobian]s, along with the world-space offset from each target's +// center of mass to its respective anchor point (derived from +// PrimaryBodyAnchorOffset and SecondaryBodyAnchorOffset combined with +// each target's current rotation), and the current distance error +// (drift) between the distance separating the two anchor points and +// Distance, based on the targets' current positions and rotations. +// +// If the two anchor points currently coincide, the constraint's axis is +// undefined; the world Y axis is used as a fallback in that degenerate +// case. +func (s *DistanceSolver) recompute(ctx PairConstraintContext) { + primaryAnchorOffsetWS := dprec.QuatVec3Rotation(ctx.PrimaryTarget.Rotation(), s.primaryBodyAnchorOffset) + primaryAnchorWS := dprec.Vec3Sum(ctx.PrimaryTarget.Position(), primaryAnchorOffsetWS) + + secondaryAnchorOffsetWS := dprec.QuatVec3Rotation(ctx.SecondaryTarget.Rotation(), s.secondaryBodyAnchorOffset) + secondaryAnchorWS := dprec.Vec3Sum(ctx.SecondaryTarget.Position(), secondaryAnchorOffsetWS) + + delta := dprec.Vec3Diff(secondaryAnchorWS, primaryAnchorWS) + actualDistance := delta.Length() + + normal := dprec.BasisYVec3() + if actualDistance > Epsilon { + normal = dprec.UnitVec3(delta) + } + + s.primaryJacobian = Jacobian{ + LinearSlope: dprec.InverseVec3(normal), + AngularSlope: dprec.Vec3Cross(normal, primaryAnchorOffsetWS), + } + s.secondaryJacobian = Jacobian{ + LinearSlope: normal, + AngularSlope: dprec.Vec3Cross(secondaryAnchorOffsetWS, normal), + } + s.drift = s.distance - actualDistance +} From b76fc0311e67c267c6da2c2cdaf747209e1a08d3 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sat, 8 Aug 2026 15:14:48 +0300 Subject: [PATCH 56/85] Changes to acceleration solver API --- game/physics/acceleration.go | 28 ++++--- game/physics/constraint/hinged_rod.go | 113 -------------------------- game/physics/scene.go | 6 +- game/physics/solver_gravity.go | 4 +- 4 files changed, 22 insertions(+), 129 deletions(-) delete mode 100644 game/physics/constraint/hinged_rod.go diff --git a/game/physics/acceleration.go b/game/physics/acceleration.go index b704fa6c..aa92aa14 100644 --- a/game/physics/acceleration.go +++ b/game/physics/acceleration.go @@ -132,12 +132,13 @@ func (t AccelerationTarget) ApplyOffsetForce(offset, force dprec.Vec3) { t.body.applyOffsetForce(offset, force) } -// AccelerationContext describes the surrounding medium at the location of -// the target that is being accelerated. +// AccelerationContext carries everything an [AccelerationSolver] needs in +// order to accelerate a single target during a physics simulation step. // -// It carries the parts of the environment that are shared by all -// contributors, so that each of them does not have to sample the medium on -// its own. +// Alongside the target itself, it carries the parts of the surrounding +// medium at the target's location that are shared by every contributor +// evaluated against that target this step, so that each of them does not +// have to sample the medium on its own. type AccelerationContext struct { // DeltaSeconds is the time step of the simulation, in seconds. Use this @@ -149,6 +150,11 @@ type AccelerationContext struct { // MediumDensity is the density of the medium in kg/m^3. MediumDensity float64 + + // Target is the [AccelerationTarget] being accelerated, through which + // the solver reads its motion state and accumulates the acceleration + // it contributes. + Target AccelerationTarget } // AccelerationSolver applies an acceleration effect to a target. @@ -157,11 +163,11 @@ type AccelerationContext struct { // and are evaluated once per body per simulation step. type AccelerationSolver interface { - // ApplyAcceleration accumulates on the target the acceleration that this - // effect produces on it under the specified context. + // ApplyAcceleration accumulates on ctx.Target the acceleration that this + // effect produces on it, under the rest of ctx. // - // Implementations must not retain the target, since it is only valid for - // the duration of the call, and must not mutate any state that other - // contributors observe, since the evaluation order is unspecified. - ApplyAcceleration(ctx AccelerationContext, target AccelerationTarget) + // Implementations must not retain ctx or ctx.Target, since both are only + // valid for the duration of the call, and must not mutate any state that + // other contributors observe, since the evaluation order is unspecified. + ApplyAcceleration(ctx AccelerationContext) } diff --git a/game/physics/constraint/hinged_rod.go b/game/physics/constraint/hinged_rod.go deleted file mode 100644 index 6a9f0ccc..00000000 --- a/game/physics/constraint/hinged_rod.go +++ /dev/null @@ -1,113 +0,0 @@ -package constraint - -// import ( -// "github.com/mokiat/gomath/dprec" -// "github.com/mokiat/lacking/game/physics/solver" -// ) - -// // NewHingedRod creates a new HingedRod constraint solver. -// func NewHingedRod() *HingedRod { -// return &HingedRod{ -// primaryRadius: dprec.ZeroVec3(), -// secondaryRadius: dprec.ZeroVec3(), -// length: 1.0, -// } -// } - -// var _ solver.PairConstraint = (*HingedRod)(nil) - -// // HingedRod represents the solution for a constraint that keeps two bodies -// // tied together with a hard link of specific length. -// type HingedRod struct { -// primaryRadius dprec.Vec3 -// secondaryRadius dprec.Vec3 -// length float64 - -// jacobian solver.PairJacobian -// drift float64 -// } - -// // PrimaryRadius returns the radius vector of the contact point -// // on the primary object. -// // -// // The vector is in the object's local space. -// func (s *HingedRod) PrimaryRadius() dprec.Vec3 { -// return s.primaryRadius -// } - -// // SetPrimaryRadius changes the attachment point of the link -// // on the primary body. -// func (s *HingedRod) SetPrimaryRadius(radius dprec.Vec3) *HingedRod { -// s.primaryRadius = radius -// return s -// } - -// // SecondaryRadius returns the radius vector of the contact point -// // on the secondary object. -// // -// // The vector is in the object's local space. -// func (s *HingedRod) SecondaryRadius() dprec.Vec3 { -// return s.secondaryRadius -// } - -// // SetSecondaryRadius changes the radius vector of the contact point -// // on the secondary object. -// // -// // The vector is in the object's local space. -// func (s *HingedRod) SetSecondaryRadius(radius dprec.Vec3) *HingedRod { -// s.secondaryRadius = radius -// return s -// } - -// // Length returns the link length. -// func (s *HingedRod) Length() float64 { -// return s.length -// } - -// // SetLength changes the link length. -// func (s *HingedRod) SetLength(length float64) *HingedRod { -// s.length = length -// return s -// } - -// // Reset re-evaluates the constraint. -// func (s *HingedRod) Reset(ctx solver.PairContext) { -// primaryRadiusWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.primaryRadius) -// primaryAnchorWS := dprec.Vec3Sum(ctx.Target.Position(), primaryRadiusWS) -// secondaryRadiusWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.secondaryRadius) -// secondaryAnchorWS := dprec.Vec3Sum(ctx.Source.Position(), secondaryRadiusWS) -// deltaPosition := dprec.Vec3Diff(secondaryAnchorWS, primaryAnchorWS) -// if lng := deltaPosition.Length(); lng > solver.Epsilon { -// normal := dprec.Vec3Quot(deltaPosition, lng) -// s.jacobian = solver.PairJacobian{ -// Target: solver.Jacobian{ -// LinearSlope: dprec.InverseVec3(normal), -// AngularSlope: dprec.Vec3Cross(normal, primaryRadiusWS), -// }, -// Source: solver.Jacobian{ -// LinearSlope: normal, -// AngularSlope: dprec.Vec3Cross(secondaryRadiusWS, normal), -// }, -// } -// s.drift = lng - s.length -// } else { -// s.jacobian = solver.PairJacobian{} -// s.drift = 0.0 -// } -// } - -// // ApplyImpulses applies impulses in order to keep the velocity part of -// // the constraint satisfied. -// func (s *HingedRod) ApplyImpulses(ctx solver.PairContext) { -// solution := ctx.JacobianImpulseSolution(s.jacobian, s.drift, 0.0) -// ctx.Target.ApplyImpulse(solution.Target) -// ctx.Source.ApplyImpulse(solution.Source) -// } - -// // ApplyNudges applies nudges in order to keep the positional part of the -// // constraint satisfied. -// func (s *HingedRod) ApplyNudges(ctx solver.PairContext) { -// solution := ctx.JacobianNudgeSolution(s.jacobian, s.drift) -// ctx.Target.ApplyNudge(solution.Target) -// ctx.Source.ApplyNudge(solution.Source) -// } diff --git a/game/physics/scene.go b/game/physics/scene.go index 9985a7f1..600f2b34 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -593,8 +593,8 @@ func (s *Scene) applyAcceleration(elapsedSeconds float64) { DeltaSeconds: elapsedSeconds, MediumVelocity: s.mediumSolver.Velocity(body.position), MediumDensity: s.mediumSolver.Density(body.position), + Target: newAccelerationTarget(body), } - target := newAccelerationTarget(body) // Reset accumulated accelerations. body.linearAcceleration = dprec.ZeroVec3() @@ -602,12 +602,12 @@ func (s *Scene) applyAcceleration(elapsedSeconds float64) { // Apply global accelerators. s.eachEnabledGlobalAccelerator(func(_ int, accelerator *globalAcceleratorState) { - accelerator.solver.ApplyAcceleration(ctx, target) + accelerator.solver.ApplyAcceleration(ctx) }) // Apply body accelerators. s.eachEnabledBodyAccelerator(body, func(_ int, accelerator *bodyAcceleratorState) { - accelerator.solver.ApplyAcceleration(ctx, target) + accelerator.solver.ApplyAcceleration(ctx) }) // Constrain the accumulated accelerations to the maximum allowed values. diff --git a/game/physics/solver_gravity.go b/game/physics/solver_gravity.go index 0277d8cd..4dd13b19 100644 --- a/game/physics/solver_gravity.go +++ b/game/physics/solver_gravity.go @@ -62,8 +62,8 @@ func (s *GravitySolver) SetMagnitude(magnitude float64) *GravitySolver { // ApplyAcceleration accumulates the gravitational acceleration on the // target. The medium of the context is not taken into account, meaning that // buoyancy is not modeled. -func (s *GravitySolver) ApplyAcceleration(ctx AccelerationContext, target AccelerationTarget) { - target.AddLinearAcceleration(s.acceleration) +func (s *GravitySolver) ApplyAcceleration(ctx AccelerationContext) { + ctx.Target.AddLinearAcceleration(s.acceleration) } func (s *GravitySolver) refreshAcceleration() { From a54f8d6909d98f50a2b0e724464bd034f4ee8489 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sat, 8 Aug 2026 15:24:02 +0300 Subject: [PATCH 57/85] Add axis-force acceleration solver --- game/physics/solver_axis_force.go | 125 ++++++++++++++++++++++++++++++ 1 file changed, 125 insertions(+) create mode 100644 game/physics/solver_axis_force.go diff --git a/game/physics/solver_axis_force.go b/game/physics/solver_axis_force.go new file mode 100644 index 00000000..58a52828 --- /dev/null +++ b/game/physics/solver_axis_force.go @@ -0,0 +1,125 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +// AxisForceSolverConfig holds the parameters with which an +// [AxisForceSolver] is configured, either through [NewAxisForceSolver] or +// [AxisForceSolver.Configure]. +type AxisForceSolverConfig struct { + + // BodyAnchorOffset is the body-local-space offset, relative to the + // target's center of mass, of the point at which the force is applied. + // + // An offset away from the center of mass causes the force to also + // induce torque on the target, in addition to accelerating it linearly. + BodyAnchorOffset dprec.Vec3 + + // Axis is the body-local-space direction along which the force acts. + // It need not be normalized; it is normalized internally, so that + // Magnitude alone controls the force's magnitude. + Axis dprec.Vec3 + + // Magnitude is the magnitude, in Newtons, of the force applied along + // Axis. + Magnitude float64 +} + +// AxisForceSolver is an [AccelerationSolver] that applies a constant force +// along a body-fixed axis, at a fixed offset from the target's center of +// mass - modeling effects such as a thruster or engine mounted on the +// target. +// +// Since Axis and BodyAnchorOffset are both expressed in the target's +// local space, the force rotates together with the target, unlike +// [GravitySolver], which pulls along a fixed direction in world space. +// +// An AxisForceSolver must be configured, either through +// [NewAxisForceSolver] or [AxisForceSolver.Configure], before being +// registered with a [Scene] through [BodyAcceleratorView.Create] or +// [GlobalAcceleratorView.Create]. +type AxisForceSolver struct { + bodyAnchorOffset dprec.Vec3 + axis dprec.Vec3 + magnitude float64 +} + +var _ AccelerationSolver = (*AxisForceSolver)(nil) + +// NewAxisForceSolver creates a new [AxisForceSolver] configured according +// to config. +func NewAxisForceSolver(config AxisForceSolverConfig) *AxisForceSolver { + result := &AxisForceSolver{} + result.Configure(config) + return result +} + +// Configure configures this solver according to config. +// +// Configure must be called before this solver is registered with a +// [Scene] through [BodyAcceleratorView.Create] or +// [GlobalAcceleratorView.Create]. Unlike [NewAxisForceSolver], it can be +// called on an already-allocated solver, which allows solvers to be +// cached (e.g. in a slice) and configured on demand. +func (s *AxisForceSolver) Configure(config AxisForceSolverConfig) { + s.bodyAnchorOffset = config.BodyAnchorOffset + s.axis = dprec.UnitVec3(config.Axis) + s.magnitude = config.Magnitude +} + +// BodyAnchorOffset returns the body-local-space offset, relative to the +// target's center of mass, of the point at which the force is applied. +func (s *AxisForceSolver) BodyAnchorOffset() dprec.Vec3 { + return s.bodyAnchorOffset +} + +// SetBodyAnchorOffset changes the body-local-space offset, relative to +// the target's center of mass, of the point at which the force is +// applied. +// +// It returns the solver itself, so that calls can be chained. +func (s *AxisForceSolver) SetBodyAnchorOffset(offset dprec.Vec3) *AxisForceSolver { + s.bodyAnchorOffset = offset + return s +} + +// Axis returns the body-local-space direction along which the force +// acts, as a unit vector. +func (s *AxisForceSolver) Axis() dprec.Vec3 { + return s.axis +} + +// SetAxis changes the body-local-space direction along which the force +// acts. The specified direction need not be normalized. +// +// It returns the solver itself, so that calls can be chained. +func (s *AxisForceSolver) SetAxis(axis dprec.Vec3) *AxisForceSolver { + s.axis = dprec.UnitVec3(axis) + return s +} + +// Magnitude returns the magnitude, in Newtons, of the force applied along +// Axis. +func (s *AxisForceSolver) Magnitude() float64 { + return s.magnitude +} + +// SetMagnitude changes the magnitude, in Newtons, of the force applied +// along Axis. +// +// It returns the solver itself, so that calls can be chained. +func (s *AxisForceSolver) SetMagnitude(magnitude float64) *AxisForceSolver { + s.magnitude = magnitude + return s +} + +// ApplyAcceleration accumulates on ctx.Target the linear and angular +// acceleration that result from a force of Magnitude, acting along Axis, +// applied at BodyAnchorOffset - all three rotated from the target's local +// space into world space using its current orientation. +func (s *AxisForceSolver) ApplyAcceleration(ctx AccelerationContext) { + anchorOffsetWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.bodyAnchorOffset) + + force := dprec.Vec3Prod(s.axis, s.magnitude) + forceWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), force) + ctx.Target.ApplyOffsetForce(anchorOffsetWS, forceWS) +} From 9373fdb7a86808574cdcd0f0fa84cfb7a55b1f94 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sat, 8 Aug 2026 15:32:28 +0300 Subject: [PATCH 58/85] Add new direction-force acceleration solver --- game/physics/solver_direction_force.go | 131 +++++++++++++++++++++++++ 1 file changed, 131 insertions(+) create mode 100644 game/physics/solver_direction_force.go diff --git a/game/physics/solver_direction_force.go b/game/physics/solver_direction_force.go new file mode 100644 index 00000000..77ed2f66 --- /dev/null +++ b/game/physics/solver_direction_force.go @@ -0,0 +1,131 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +// DirectionForceSolverConfig holds the parameters with which a +// [DirectionForceSolver] is configured, either through +// [NewDirectionForceSolver] or [DirectionForceSolver.Configure]. +type DirectionForceSolverConfig struct { + + // BodyAnchorOffset is the body-local-space offset, relative to the + // target's center of mass, of the point at which the force is applied. + // + // An offset away from the center of mass causes the force to also + // induce torque on the target, in addition to accelerating it linearly. + BodyAnchorOffset dprec.Vec3 + + // Direction is the world-space direction along which the force acts. + // It need not be normalized; it is normalized internally, so that + // Magnitude alone controls the force's magnitude. + Direction dprec.Vec3 + + // Magnitude is the magnitude, in Newtons, of the force applied along + // Direction. + Magnitude float64 +} + +// DirectionForceSolver is an [AccelerationSolver] that applies a constant +// force along a fixed direction in world space, at a fixed offset from the +// target's center of mass - modeling effects such as wind or an external +// jet blowing on the target from one side. +// +// Unlike [AxisForceSolver], whose axis is expressed in the target's local +// space and so rotates together with it, DirectionForceSolver's Direction +// is expressed in world space and stays constant regardless of the +// target's orientation. BodyAnchorOffset, however, is still expressed in +// the target's local space, same as with AxisForceSolver, so the point at +// which the force is applied still moves and rotates together with the +// target, meaning the torque induced by the force still varies as the +// target rotates even though the force's direction does not. +// +// A DirectionForceSolver must be configured, either through +// [NewDirectionForceSolver] or [DirectionForceSolver.Configure], before +// being registered with a [Scene] through [BodyAcceleratorView.Create] or +// [GlobalAcceleratorView.Create]. +type DirectionForceSolver struct { + bodyAnchorOffset dprec.Vec3 + direction dprec.Vec3 + magnitude float64 +} + +var _ AccelerationSolver = (*DirectionForceSolver)(nil) + +// NewDirectionForceSolver creates a new [DirectionForceSolver] configured +// according to config. +func NewDirectionForceSolver(config DirectionForceSolverConfig) *DirectionForceSolver { + result := &DirectionForceSolver{} + result.Configure(config) + return result +} + +// Configure configures this solver according to config. +// +// Configure must be called before this solver is registered with a +// [Scene] through [BodyAcceleratorView.Create] or +// [GlobalAcceleratorView.Create]. Unlike [NewDirectionForceSolver], it can +// be called on an already-allocated solver, which allows solvers to be +// cached (e.g. in a slice) and configured on demand. +func (s *DirectionForceSolver) Configure(config DirectionForceSolverConfig) { + s.bodyAnchorOffset = config.BodyAnchorOffset + s.direction = dprec.UnitVec3(config.Direction) + s.magnitude = config.Magnitude +} + +// BodyAnchorOffset returns the body-local-space offset, relative to the +// target's center of mass, of the point at which the force is applied. +func (s *DirectionForceSolver) BodyAnchorOffset() dprec.Vec3 { + return s.bodyAnchorOffset +} + +// SetBodyAnchorOffset changes the body-local-space offset, relative to +// the target's center of mass, of the point at which the force is +// applied. +// +// It returns the solver itself, so that calls can be chained. +func (s *DirectionForceSolver) SetBodyAnchorOffset(offset dprec.Vec3) *DirectionForceSolver { + s.bodyAnchorOffset = offset + return s +} + +// Direction returns the world-space direction along which the force acts, +// as a unit vector. +func (s *DirectionForceSolver) Direction() dprec.Vec3 { + return s.direction +} + +// SetDirection changes the world-space direction along which the force +// acts. The specified direction need not be normalized. +// +// It returns the solver itself, so that calls can be chained. +func (s *DirectionForceSolver) SetDirection(direction dprec.Vec3) *DirectionForceSolver { + s.direction = dprec.UnitVec3(direction) + return s +} + +// Magnitude returns the magnitude, in Newtons, of the force applied along +// Direction. +func (s *DirectionForceSolver) Magnitude() float64 { + return s.magnitude +} + +// SetMagnitude changes the magnitude, in Newtons, of the force applied +// along Direction. +// +// It returns the solver itself, so that calls can be chained. +func (s *DirectionForceSolver) SetMagnitude(magnitude float64) *DirectionForceSolver { + s.magnitude = magnitude + return s +} + +// ApplyAcceleration accumulates on ctx.Target the linear and angular +// acceleration that result from a force of Magnitude, acting along +// Direction, applied at BodyAnchorOffset - with BodyAnchorOffset rotated +// from the target's local space into world space using its current +// orientation, and Direction used as-is, since it is already expressed in +// world space. +func (s *DirectionForceSolver) ApplyAcceleration(ctx AccelerationContext) { + anchorOffsetWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.bodyAnchorOffset) + + forceWS := dprec.Vec3Prod(s.direction, s.magnitude) + ctx.Target.ApplyOffsetForce(anchorOffsetWS, forceWS) +} From 4e51e0e306bd1c5c627c75f0e0aa4a1a4b07a79b Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sat, 8 Aug 2026 15:51:20 +0300 Subject: [PATCH 59/85] Add helper functions for friction and restitution coefficients --- game/physics/solver_collision_pair.go | 10 ++++++++-- game/physics/solver_collision_solo.go | 10 ++++++++-- game/physics/util.go | 23 +++++++++++++++++++++++ 3 files changed, 39 insertions(+), 4 deletions(-) diff --git a/game/physics/solver_collision_pair.go b/game/physics/solver_collision_pair.go index a752f5d7..a9b569e2 100644 --- a/game/physics/solver_collision_pair.go +++ b/game/physics/solver_collision_pair.go @@ -117,8 +117,14 @@ func (s *PairCollisionSolver) Configure(config PairCollisionSolverConfig) { s.secondaryContactPoint = config.SecondaryContactPoint s.contactDepth = config.ContactDepth - s.frictionCoefficient = dprec.Sqrt(config.PrimaryFrictionCoefficient * config.SecondaryFrictionCoefficient) - s.restitutionCoefficient = max(config.PrimaryRestitutionCoefficient, config.SecondaryRestitutionCoefficient) + s.frictionCoefficient = CombinedFrictionCoefficient( + config.PrimaryFrictionCoefficient, + config.SecondaryFrictionCoefficient, + ) + s.restitutionCoefficient = CombinedRestitutionCoefficient( + config.PrimaryRestitutionCoefficient, + config.SecondaryRestitutionCoefficient, + ) } // Reset implements [PairConstraintSolver.Reset]. diff --git a/game/physics/solver_collision_solo.go b/game/physics/solver_collision_solo.go index 287695ea..f230db59 100644 --- a/game/physics/solver_collision_solo.go +++ b/game/physics/solver_collision_solo.go @@ -96,8 +96,14 @@ func (s *SoloCollisionSolver) Configure(config SoloCollisionSolverConfig) { s.bodyContactPoint = config.BodyContactPoint s.contactDepth = config.ContactDepth - s.frictionCoefficient = dprec.Sqrt(config.BodyFrictionCoefficient * config.TerrainFrictionCoefficient) - s.restitutionCoefficient = max(config.BodyRestitutionCoefficient, config.TerrainRestitutionCoefficient) + s.frictionCoefficient = CombinedFrictionCoefficient( + config.BodyFrictionCoefficient, + config.TerrainFrictionCoefficient, + ) + s.restitutionCoefficient = CombinedRestitutionCoefficient( + config.BodyRestitutionCoefficient, + config.TerrainRestitutionCoefficient, + ) } // Reset implements [SoloConstraintSolver.Reset]. diff --git a/game/physics/util.go b/game/physics/util.go index 7b4896f8..0f14ecca 100644 --- a/game/physics/util.go +++ b/game/physics/util.go @@ -26,6 +26,29 @@ func QuatFromVector(vector dprec.Vec3) dprec.Quat { return dprec.RotationQuat(dprec.Radians(radians), vector) } +// CombinedFrictionCoefficient returns the friction coefficient to use for a +// contact between two materials whose individual friction coefficients are +// first and second, combining them as their geometric mean. +// +// This is independent of which kind of solver the contact belongs to; it +// applies equally to a body-body contact (e.g. [PairCollisionSolver]) and +// a body-terrain contact (e.g. [SoloCollisionSolver]). +func CombinedFrictionCoefficient(first, second float64) float64 { + return dprec.Sqrt(first * second) +} + +// CombinedRestitutionCoefficient returns the restitution coefficient to use +// for a contact between two materials whose individual restitution +// coefficients are first and second, combining them by taking the larger +// of the two. +// +// This is independent of which kind of solver the contact belongs to; it +// applies equally to a body-body contact (e.g. [PairCollisionSolver]) and +// a body-terrain contact (e.g. [SoloCollisionSolver]). +func CombinedRestitutionCoefficient(first, second float64) float64 { + return max(first, second) +} + // RestitutionClamp specifies a ratio that describes how much the restitution // coefficient should be allowed to apply. // From ac41325c65255905c416e73b5f5dbe247c9e967c Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sat, 8 Aug 2026 17:33:45 +0300 Subject: [PATCH 60/85] Add AABB type to shape3d package --- core/spatial/shape3d/aabb.go | 74 ++++++++++++++++++ core/spatial/shape3d/aabb_test.go | 121 ++++++++++++++++++++++++++++++ 2 files changed, 195 insertions(+) create mode 100644 core/spatial/shape3d/aabb.go create mode 100644 core/spatial/shape3d/aabb_test.go diff --git a/core/spatial/shape3d/aabb.go b/core/spatial/shape3d/aabb.go new file mode 100644 index 00000000..1146e464 --- /dev/null +++ b/core/spatial/shape3d/aabb.go @@ -0,0 +1,74 @@ +package shape3d + +import "github.com/mokiat/gomath/dprec" + +// AABB represents an axis-aligned bounding box in 3D space. +type AABB struct { + // MinX specifies the minimum X coordinate of the box. + MinX float64 + // MinY specifies the minimum Y coordinate of the box. + MinY float64 + // MinZ specifies the minimum Z coordinate of the box. + MinZ float64 + // MaxX specifies the maximum X coordinate of the box. + MaxX float64 + // MaxY specifies the maximum Y coordinate of the box. + MaxY float64 + // MaxZ specifies the maximum Z coordinate of the box. + MaxZ float64 +} + +// NewAABB creates an [AABB] with the given minimum and maximum coordinates. +func NewAABB(minX, minY, minZ, maxX, maxY, maxZ float64) AABB { + return AABB{ + MinX: minX, + MinY: minY, + MinZ: minZ, + MaxX: maxX, + MaxY: maxY, + MaxZ: maxZ, + } +} + +// AABBFromSphere returns the smallest [AABB] that fully encompasses the +// given sphere. +func AABBFromSphere(sphere Sphere) AABB { + return AABB{ + MinX: sphere.Center.X - sphere.Radius, + MinY: sphere.Center.Y - sphere.Radius, + MinZ: sphere.Center.Z - sphere.Radius, + MaxX: sphere.Center.X + sphere.Radius, + MaxY: sphere.Center.Y + sphere.Radius, + MaxZ: sphere.Center.Z + sphere.Radius, + } +} + +// AABBFromBox returns the smallest [AABB] that fully encompasses the given +// box, taking the box's orientation into account. +func AABBFromBox(box Box) AABB { + rotation := box.Rotation + halfExtentX := dprec.Abs(rotation.BasisX.X)*box.HalfWidth + + dprec.Abs(rotation.BasisY.X)*box.HalfHeight + + dprec.Abs(rotation.BasisZ.X)*box.HalfLength + halfExtentY := dprec.Abs(rotation.BasisX.Y)*box.HalfWidth + + dprec.Abs(rotation.BasisY.Y)*box.HalfHeight + + dprec.Abs(rotation.BasisZ.Y)*box.HalfLength + halfExtentZ := dprec.Abs(rotation.BasisX.Z)*box.HalfWidth + + dprec.Abs(rotation.BasisY.Z)*box.HalfHeight + + dprec.Abs(rotation.BasisZ.Z)*box.HalfLength + + return AABB{ + MinX: box.Center.X - halfExtentX, + MinY: box.Center.Y - halfExtentY, + MinZ: box.Center.Z - halfExtentZ, + MaxX: box.Center.X + halfExtentX, + MaxY: box.Center.Y + halfExtentY, + MaxZ: box.Center.Z + halfExtentZ, + } +} + +// IsEmpty returns whether this AABB has no volume, which is the case when +// its minimum coordinate exceeds its maximum coordinate along some axis. +func (aabb AABB) IsEmpty() bool { + return (aabb.MinX > aabb.MaxX) || (aabb.MinY > aabb.MaxY) || (aabb.MinZ > aabb.MaxZ) +} diff --git a/core/spatial/shape3d/aabb_test.go b/core/spatial/shape3d/aabb_test.go new file mode 100644 index 00000000..6747ba30 --- /dev/null +++ b/core/spatial/shape3d/aabb_test.go @@ -0,0 +1,121 @@ +package shape3d_test + +import ( + . "github.com/onsi/ginkgo/v2" + . "github.com/onsi/gomega" + + "github.com/mokiat/gomath/dprec" + "github.com/mokiat/lacking/core/spatial/shape3d" +) + +var _ = Describe("AABB", func() { + + Describe("NewAABB", func() { + It("assigns the min and max components to the matching fields", func() { + aabb := shape3d.NewAABB(1.0, 2.0, 3.0, 4.0, 5.0, 6.0) + Expect(aabb.MinX).To(BeNumerically("~", 1.0, 1e-6)) + Expect(aabb.MinY).To(BeNumerically("~", 2.0, 1e-6)) + Expect(aabb.MinZ).To(BeNumerically("~", 3.0, 1e-6)) + Expect(aabb.MaxX).To(BeNumerically("~", 4.0, 1e-6)) + Expect(aabb.MaxY).To(BeNumerically("~", 5.0, 1e-6)) + Expect(aabb.MaxZ).To(BeNumerically("~", 6.0, 1e-6)) + }) + }) + + Describe("AABBFromSphere", func() { + It("encloses the sphere tightly", func() { + sphere := shape3d.NewSphere(dprec.NewVec3(3.0, 4.0, 5.0), 2.0) + aabb := shape3d.AABBFromSphere(sphere) + Expect(aabb.MinX).To(BeNumerically("~", 1.0, 1e-6)) + Expect(aabb.MinY).To(BeNumerically("~", 2.0, 1e-6)) + Expect(aabb.MinZ).To(BeNumerically("~", 3.0, 1e-6)) + Expect(aabb.MaxX).To(BeNumerically("~", 5.0, 1e-6)) + Expect(aabb.MaxY).To(BeNumerically("~", 6.0, 1e-6)) + Expect(aabb.MaxZ).To(BeNumerically("~", 7.0, 1e-6)) + }) + }) + + Describe("AABBFromBox", func() { + It("encloses an axis-aligned box tightly", func() { + box := shape3d.NewBox( + dprec.NewVec3(3.0, 4.0, 5.0), + shape3d.IdentityRotation(), + dprec.NewVec3(3.0, 4.0, 2.0), + ) + aabb := shape3d.AABBFromBox(box) + Expect(aabb.MinX).To(BeNumerically("~", 0.0, 1e-6)) + Expect(aabb.MinY).To(BeNumerically("~", 0.0, 1e-6)) + Expect(aabb.MinZ).To(BeNumerically("~", 3.0, 1e-6)) + Expect(aabb.MaxX).To(BeNumerically("~", 6.0, 1e-6)) + Expect(aabb.MaxY).To(BeNumerically("~", 8.0, 1e-6)) + Expect(aabb.MaxZ).To(BeNumerically("~", 7.0, 1e-6)) + }) + + It("accounts for the box orientation", func() { + // A 90deg rotation about Z swaps the contribution of the width and + // height half-extents between the world X and Y axes: local X maps + // to world Y, and local Y maps to world -X. See the equivalent case + // in box_test.go's ContainsPoint tests. + box := shape3d.NewBox( + dprec.NewVec3(3.0, 4.0, 5.0), + shape3d.RotationFromQuat(dprec.RotationQuat(dprec.Degrees(90.0), dprec.BasisZVec3())), + dprec.NewVec3(3.0, 4.0, 2.0), + ) + aabb := shape3d.AABBFromBox(box) + Expect(aabb.MinX).To(BeNumerically("~", -1.0, 1e-6)) + Expect(aabb.MinY).To(BeNumerically("~", 1.0, 1e-6)) + Expect(aabb.MinZ).To(BeNumerically("~", 3.0, 1e-6)) + Expect(aabb.MaxX).To(BeNumerically("~", 7.0, 1e-6)) + Expect(aabb.MaxY).To(BeNumerically("~", 7.0, 1e-6)) + Expect(aabb.MaxZ).To(BeNumerically("~", 7.0, 1e-6)) + }) + + It("degenerates to the center point when all dimensions are zero", func() { + box := shape3d.NewBox( + dprec.NewVec3(1.0, 2.0, 3.0), + shape3d.IdentityRotation(), + dprec.NewVec3(0.0, 0.0, 0.0), + ) + aabb := shape3d.AABBFromBox(box) + Expect(aabb.MinX).To(BeNumerically("~", 1.0, 1e-6)) + Expect(aabb.MinY).To(BeNumerically("~", 2.0, 1e-6)) + Expect(aabb.MinZ).To(BeNumerically("~", 3.0, 1e-6)) + Expect(aabb.MaxX).To(BeNumerically("~", 1.0, 1e-6)) + Expect(aabb.MaxY).To(BeNumerically("~", 2.0, 1e-6)) + Expect(aabb.MaxZ).To(BeNumerically("~", 3.0, 1e-6)) + }) + }) + + Describe("IsEmpty", func() { + It("returns false for a regular box", func() { + aabb := shape3d.NewAABB(0.0, 0.0, 0.0, 1.0, 1.0, 1.0) + Expect(aabb.IsEmpty()).To(BeFalse()) + }) + + It("returns false for a single point", func() { + aabb := shape3d.NewAABB(1.0, 2.0, 3.0, 1.0, 2.0, 3.0) + Expect(aabb.IsEmpty()).To(BeFalse()) + }) + + It("returns true when the minimum exceeds the maximum in X", func() { + aabb := shape3d.NewAABB(1.0, 0.0, 0.0, 0.0, 1.0, 1.0) + Expect(aabb.IsEmpty()).To(BeTrue()) + }) + + It("returns true when the minimum exceeds the maximum in Y", func() { + aabb := shape3d.NewAABB(0.0, 1.0, 0.0, 1.0, 0.0, 1.0) + Expect(aabb.IsEmpty()).To(BeTrue()) + }) + + It("returns true when the minimum exceeds the maximum in Z", func() { + aabb := shape3d.NewAABB(0.0, 0.0, 1.0, 1.0, 1.0, 0.0) + Expect(aabb.IsEmpty()).To(BeTrue()) + }) + + It("can be called directly on a returned value", func() { + sphere := shape3d.NewSphere(dprec.NewVec3(0.0, 0.0, 0.0), 1.0) + Expect(shape3d.AABBFromSphere(sphere).IsEmpty()).To(BeFalse()) + }) + }) + +}) From 441cdb98250753960e92244035fa204ba72cf277 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sat, 8 Aug 2026 17:40:42 +0300 Subject: [PATCH 61/85] Add AABB type to shape2d package --- core/spatial/shape2d/aabb.go | 59 +++++++++++++++++ core/spatial/shape2d/aabb_test.go | 105 ++++++++++++++++++++++++++++++ 2 files changed, 164 insertions(+) create mode 100644 core/spatial/shape2d/aabb.go create mode 100644 core/spatial/shape2d/aabb_test.go diff --git a/core/spatial/shape2d/aabb.go b/core/spatial/shape2d/aabb.go new file mode 100644 index 00000000..a496daa9 --- /dev/null +++ b/core/spatial/shape2d/aabb.go @@ -0,0 +1,59 @@ +package shape2d + +import "github.com/mokiat/gomath/dprec" + +// AABB represents an axis-aligned bounding box in 2D space. +type AABB struct { + // MinX specifies the minimum X coordinate of the box. + MinX float64 + // MinY specifies the minimum Y coordinate of the box. + MinY float64 + // MaxX specifies the maximum X coordinate of the box. + MaxX float64 + // MaxY specifies the maximum Y coordinate of the box. + MaxY float64 +} + +// NewAABB creates an [AABB] with the given minimum and maximum coordinates. +func NewAABB(minX, minY, maxX, maxY float64) AABB { + return AABB{ + MinX: minX, + MinY: minY, + MaxX: maxX, + MaxY: maxY, + } +} + +// AABBFromCircle returns the smallest [AABB] that fully encompasses the +// given circle. +func AABBFromCircle(circle Circle) AABB { + return AABB{ + MinX: circle.Center.X - circle.Radius, + MinY: circle.Center.Y - circle.Radius, + MaxX: circle.Center.X + circle.Radius, + MaxY: circle.Center.Y + circle.Radius, + } +} + +// AABBFromRectangle returns the smallest [AABB] that fully encompasses the +// given rectangle, taking the rectangle's orientation into account. +func AABBFromRectangle(rect Rectangle) AABB { + rotation := rect.Rotation + halfExtentX := dprec.Abs(rotation.BasisX.X)*rect.HalfWidth + + dprec.Abs(rotation.BasisY.X)*rect.HalfHeight + halfExtentY := dprec.Abs(rotation.BasisX.Y)*rect.HalfWidth + + dprec.Abs(rotation.BasisY.Y)*rect.HalfHeight + + return AABB{ + MinX: rect.Center.X - halfExtentX, + MinY: rect.Center.Y - halfExtentY, + MaxX: rect.Center.X + halfExtentX, + MaxY: rect.Center.Y + halfExtentY, + } +} + +// IsEmpty returns whether this AABB has no area, which is the case when its +// minimum coordinate exceeds its maximum coordinate along some axis. +func (aabb AABB) IsEmpty() bool { + return (aabb.MinX > aabb.MaxX) || (aabb.MinY > aabb.MaxY) +} diff --git a/core/spatial/shape2d/aabb_test.go b/core/spatial/shape2d/aabb_test.go new file mode 100644 index 00000000..c56a9571 --- /dev/null +++ b/core/spatial/shape2d/aabb_test.go @@ -0,0 +1,105 @@ +package shape2d_test + +import ( + . "github.com/onsi/ginkgo/v2" + . "github.com/onsi/gomega" + + "github.com/mokiat/gomath/dprec" + "github.com/mokiat/lacking/core/spatial/shape2d" +) + +var _ = Describe("AABB", func() { + + Describe("NewAABB", func() { + It("assigns the min and max components to the matching fields", func() { + aabb := shape2d.NewAABB(1.0, 2.0, 3.0, 4.0) + Expect(aabb.MinX).To(BeNumerically("~", 1.0, 1e-6)) + Expect(aabb.MinY).To(BeNumerically("~", 2.0, 1e-6)) + Expect(aabb.MaxX).To(BeNumerically("~", 3.0, 1e-6)) + Expect(aabb.MaxY).To(BeNumerically("~", 4.0, 1e-6)) + }) + }) + + Describe("AABBFromCircle", func() { + It("encloses the circle tightly", func() { + circle := shape2d.NewCircle(dprec.NewVec2(3.0, 4.0), 2.0) + aabb := shape2d.AABBFromCircle(circle) + Expect(aabb.MinX).To(BeNumerically("~", 1.0, 1e-6)) + Expect(aabb.MinY).To(BeNumerically("~", 2.0, 1e-6)) + Expect(aabb.MaxX).To(BeNumerically("~", 5.0, 1e-6)) + Expect(aabb.MaxY).To(BeNumerically("~", 6.0, 1e-6)) + }) + }) + + Describe("AABBFromRectangle", func() { + It("encloses an axis-aligned rectangle tightly", func() { + rect := shape2d.NewRectangle( + dprec.NewVec2(3.0, 4.0), + shape2d.IdentityRotation(), + dprec.NewVec2(3.0, 4.0), + ) + aabb := shape2d.AABBFromRectangle(rect) + Expect(aabb.MinX).To(BeNumerically("~", 0.0, 1e-6)) + Expect(aabb.MinY).To(BeNumerically("~", 0.0, 1e-6)) + Expect(aabb.MaxX).To(BeNumerically("~", 6.0, 1e-6)) + Expect(aabb.MaxY).To(BeNumerically("~", 8.0, 1e-6)) + }) + + It("accounts for the rectangle orientation", func() { + // A 90deg CCW rotation swaps the contribution of the width and + // height half-extents between the world X and Y axes. See the + // equivalent case in rectangle_test.go's ContainsPoint tests. + rect := shape2d.NewRectangle( + dprec.NewVec2(3.0, 4.0), + shape2d.RotationFromCosSin(0.0, 1.0), + dprec.NewVec2(3.0, 4.0), + ) + aabb := shape2d.AABBFromRectangle(rect) + Expect(aabb.MinX).To(BeNumerically("~", -1.0, 1e-6)) + Expect(aabb.MinY).To(BeNumerically("~", 1.0, 1e-6)) + Expect(aabb.MaxX).To(BeNumerically("~", 7.0, 1e-6)) + Expect(aabb.MaxY).To(BeNumerically("~", 7.0, 1e-6)) + }) + + It("degenerates to the center point when both dimensions are zero", func() { + rect := shape2d.NewRectangle( + dprec.NewVec2(1.0, 2.0), + shape2d.IdentityRotation(), + dprec.NewVec2(0.0, 0.0), + ) + aabb := shape2d.AABBFromRectangle(rect) + Expect(aabb.MinX).To(BeNumerically("~", 1.0, 1e-6)) + Expect(aabb.MinY).To(BeNumerically("~", 2.0, 1e-6)) + Expect(aabb.MaxX).To(BeNumerically("~", 1.0, 1e-6)) + Expect(aabb.MaxY).To(BeNumerically("~", 2.0, 1e-6)) + }) + }) + + Describe("IsEmpty", func() { + It("returns false for a regular box", func() { + aabb := shape2d.NewAABB(0.0, 0.0, 1.0, 1.0) + Expect(aabb.IsEmpty()).To(BeFalse()) + }) + + It("returns false for a single point", func() { + aabb := shape2d.NewAABB(1.0, 2.0, 1.0, 2.0) + Expect(aabb.IsEmpty()).To(BeFalse()) + }) + + It("returns true when the minimum exceeds the maximum in X", func() { + aabb := shape2d.NewAABB(1.0, 0.0, 0.0, 1.0) + Expect(aabb.IsEmpty()).To(BeTrue()) + }) + + It("returns true when the minimum exceeds the maximum in Y", func() { + aabb := shape2d.NewAABB(0.0, 1.0, 1.0, 0.0) + Expect(aabb.IsEmpty()).To(BeTrue()) + }) + + It("can be called directly on a returned value", func() { + circle := shape2d.NewCircle(dprec.NewVec2(0.0, 0.0), 1.0) + Expect(shape2d.AABBFromCircle(circle).IsEmpty()).To(BeFalse()) + }) + }) + +}) From 2fd4e5f3d2304ca6719cd531f22779b37343579e Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sat, 8 Aug 2026 18:02:10 +0300 Subject: [PATCH 62/85] Improve Octree API in query3d package --- core/spatial/placement3d/scene.go | 18 ++- core/spatial/query3d/aabb.go | 51 ------- core/spatial/query3d/area.go | 32 ----- core/spatial/query3d/doc.go | 13 +- core/spatial/query3d/octree.go | 201 +++++++++++++++++----------- core/spatial/query3d/octree_test.go | 96 +++++++++---- core/spatial/query3d/segment.go | 18 --- 7 files changed, 209 insertions(+), 220 deletions(-) delete mode 100644 core/spatial/query3d/aabb.go delete mode 100644 core/spatial/query3d/area.go delete mode 100644 core/spatial/query3d/segment.go diff --git a/core/spatial/placement3d/scene.go b/core/spatial/placement3d/scene.go index c4c6175c..362034e5 100644 --- a/core/spatial/placement3d/scene.go +++ b/core/spatial/placement3d/scene.go @@ -146,7 +146,7 @@ func (s *Scene[O, S, M]) SetObjectTransform(objID ObjectID, transform shape3d.Tr s.eachObjectShape(object, func(_ int32, shape *shape[S]) { shape.update(transform) bs := shape.boundingSphere() - s.shapeTree.Update(shape.spatialID, query3d.AreaFromSphere(bs)) + s.shapeTree.Update(shape.spatialID, shape3d.AABBFromSphere(bs)) }) } @@ -310,7 +310,7 @@ func (s *Scene[O, S, M]) CreateMesh(info MeshInfo[M]) MeshID { ), } representation := newMeshRepresentation(shape3d.TransformedMesh(info.Mesh, transform)) - area := query3d.AreaFromSphere(representation.boundingSphere()) + area := shape3d.AABBFromSphere(representation.boundingSphere()) index := s.allocateMesh() s.meshes[index] = meshShape[M]{ @@ -348,11 +348,9 @@ func (s *Scene[O, S, M]) SetMeshUserData(meshID MeshID, userData M) { // CollectSegmentIntersections collects all intersections of the segment // with objects in the scene. func (s *Scene[O, S, M]) CollectSegmentIntersections(segment shape3d.Segment, filter Filter, yield ContactCallback) { - querySegment := query3d.NewSegment(segment.A, segment.B) - if !filter.SkipDynamic { s.shapeCandidates = s.shapeCandidates[:0] - s.shapeTree.QuerySegment(querySegment, func(index int32) bool { + s.shapeTree.QuerySegment(segment, func(index int32) bool { s.shapeCandidates = append(s.shapeCandidates, index) return true }) @@ -361,7 +359,7 @@ func (s *Scene[O, S, M]) CollectSegmentIntersections(segment shape3d.Segment, fi if !filter.SkipStatic { s.meshCandidates = s.meshCandidates[:0] - s.meshTree.QuerySegment(querySegment, func(index int32) bool { + s.meshTree.QuerySegment(segment, func(index int32) bool { s.meshCandidates = append(s.meshCandidates, index) return true }) @@ -380,7 +378,7 @@ func (s *Scene[O, S, M]) CheckSegmentIntersection(segment shape3d.Segment, filte // CollectSphereIntersections collects all intersections of the sphere // with objects in the scene. func (s *Scene[O, S, M]) CollectSphereIntersections(sphere shape3d.Sphere, filter Filter, yield ContactCallback) { - queryAABB := query3d.AABBFromSphere(sphere) + queryAABB := shape3d.AABBFromSphere(sphere) if !filter.SkipDynamic { s.shapeCandidates = s.shapeCandidates[:0] @@ -412,7 +410,7 @@ func (s *Scene[O, S, M]) CheckSphereIntersection(sphere shape3d.Sphere, filter F // CollectBoxIntersections collects all intersections of the box // with objects in the scene. func (s *Scene[O, S, M]) CollectBoxIntersections(box shape3d.Box, filter Filter, yield ContactCallback) { - queryAABB := query3d.AABBFromBox(box) + queryAABB := shape3d.AABBFromBox(box) if !filter.SkipDynamic { s.shapeCandidates = s.shapeCandidates[:0] @@ -450,7 +448,7 @@ func (s *Scene[O, S, M]) CollectIntersections(yield ContactCallback) { continue } - queryAABB := query3d.AABBFromSphere(srcShape.boundingSphere()) + queryAABB := shape3d.AABBFromSphere(srcShape.boundingSphere()) s.shapeCandidates = s.shapeCandidates[:0] s.shapeTree.QueryAABB(queryAABB, func(tgtIndex int32) bool { @@ -519,7 +517,7 @@ func (s *Scene[O, S, M]) attachShape( index := s.allocateShape() representation.update(object.transform) - area := query3d.AreaFromSphere(representation.boundingSphere()) + area := shape3d.AABBFromSphere(representation.boundingSphere()) s.shapes[index] = shape[S]{ objectIndex: objectIndex, diff --git a/core/spatial/query3d/aabb.go b/core/spatial/query3d/aabb.go deleted file mode 100644 index 52beb68b..00000000 --- a/core/spatial/query3d/aabb.go +++ /dev/null @@ -1,51 +0,0 @@ -package query3d - -import "github.com/mokiat/lacking/core/spatial/shape3d" - -// AABB is an axis-aligned bounding box that can be used for spatial queries. -type AABB struct { - minX float64 - minY float64 - minZ float64 - maxX float64 - maxY float64 - maxZ float64 -} - -// NewAABB creates a new [AABB] with the given minimum and maximum coordinates. -func NewAABB(minX, minY, minZ, maxX, maxY, maxZ float64) AABB { - return AABB{ - minX: minX, - minY: minY, - minZ: minZ, - maxX: maxX, - maxY: maxY, - maxZ: maxZ, - } -} - -// AABBFromSphere creates an [AABB] that fully contains the given sphere. -func AABBFromSphere(sphere shape3d.Sphere) AABB { - return AABB{ - minX: sphere.Center.X - sphere.Radius, - minY: sphere.Center.Y - sphere.Radius, - minZ: sphere.Center.Z - sphere.Radius, - maxX: sphere.Center.X + sphere.Radius, - maxY: sphere.Center.Y + sphere.Radius, - maxZ: sphere.Center.Z + sphere.Radius, - } -} - -// AABBFromBox creates an [AABB] from the given box's center and half-extents. -// The box orientation is ignored, so the result encloses the box only when it -// is axis-aligned. -func AABBFromBox(box shape3d.Box) AABB { - return AABB{ - minX: box.Center.X - box.HalfWidth, - minY: box.Center.Y - box.HalfHeight, - minZ: box.Center.Z - box.HalfLength, - maxX: box.Center.X + box.HalfWidth, - maxY: box.Center.Y + box.HalfHeight, - maxZ: box.Center.Z + box.HalfLength, - } -} diff --git a/core/spatial/query3d/area.go b/core/spatial/query3d/area.go deleted file mode 100644 index 50008472..00000000 --- a/core/spatial/query3d/area.go +++ /dev/null @@ -1,32 +0,0 @@ -package query3d - -import "github.com/mokiat/lacking/core/spatial/shape3d" - -// Area represents the spatial area of an object in the 3D space. -type Area struct { - x float64 - y float64 - z float64 - r float64 -} - -// AreaFromSphere creates an [Area] from the given sphere's center and radius. -func AreaFromSphere(sphere shape3d.Sphere) Area { - return Area{ - x: sphere.Center.X, - y: sphere.Center.Y, - z: sphere.Center.Z, - r: sphere.Radius, - } -} - -// AreaFromBox creates an [Area] from the given box, using its largest -// half-extent as the area radius. -func AreaFromBox(box shape3d.Box) Area { - return Area{ - x: box.Center.X, - y: box.Center.Y, - z: box.Center.Z, - r: max(box.HalfWidth, box.HalfHeight, box.HalfLength), - } -} diff --git a/core/spatial/query3d/doc.go b/core/spatial/query3d/doc.go index 536efe7e..d035cc19 100644 --- a/core/spatial/query3d/doc.go +++ b/core/spatial/query3d/doc.go @@ -1,15 +1,16 @@ // Package query3d provides a 3D spatial query interface. // -// The package is built around an [Octree], a loose octree that indexes items by -// their spatial [Area] and allows them to be searched through [Octree.QueryAABB] -// and [Octree.QuerySegment]. +// The package is built around an [Octree], a loose octree that indexes items +// by their axis-aligned bounding box ([shape3d.AABB]) and allows them to be +// searched through [Octree.QueryAABB] and [Octree.QuerySegment]. // // It is intended as a broad-phase (high-level) pass: queries are conservative // and may yield false positives, so callers are expected to run their own // narrow-phase tests on the returned items. It will never omit an item that // truly matches the query. // -// Every item is reduced to a center and a half-extent (an axis-aligned -// bounding box). As a result, non-cubic shapes are indexed by their bounding -// cube, which is a deliberate trade-off in favor of speed and simplicity. +// Every item is reduced to an axis-aligned bounding box, which means that +// orientation and concavity are not taken into account. Callers get the +// tightest results by passing the smallest box that still fully encompasses +// the item. package query3d diff --git a/core/spatial/query3d/octree.go b/core/spatial/query3d/octree.go index d66bf365..f9fcdb7d 100644 --- a/core/spatial/query3d/octree.go +++ b/core/spatial/query3d/octree.go @@ -6,6 +6,7 @@ import ( "github.com/mokiat/gog/ds" "github.com/mokiat/gog/opt" "github.com/mokiat/gomath/dprec" + "github.com/mokiat/lacking/core/spatial/shape3d" ) // OctreeSettings contains the settings for an [Octree]. @@ -77,12 +78,12 @@ func NewOctree[T any](settings OctreeSettings) *Octree[T] { itemOffset: 0, placeOffset: 0, looseArea: octreeCube{ - x: 0.0, - y: 0.0, - z: 0.0, - r: size, // using size here since a loose area has twice the size + x: 0.0, + y: 0.0, + z: 0.0, + halfSize: size, // using size here since a loose area has twice the size }, - box: emptyOctreeAABB(), + tightArea: emptyOctreeAABB(), }) return &Octree[T]{ @@ -129,45 +130,62 @@ func (t *Octree[T]) VisitStats() TreeVisitStats { } } -// Insert adds an item, which occupies the specified area, to this -// tree. -func (t *Octree[T]) Insert(area Area, value T) TreeItemID { - nodeIndex := t.pickNodeForItem(area) - box := newOctreeAABBFromArea(area) +// Insert adds an item, which occupies the specified axis-aligned bounding +// box, to this tree. +// +// The box must not be empty (as per [shape3d.AABB.IsEmpty]), otherwise this +// function panics. The box should be contained within the bounds of the tree, +// otherwise the placement of the item is undefined. +func (t *Octree[T]) Insert(aabb shape3d.AABB, value T) TreeItemID { + if aabb.IsEmpty() { + panic("cannot insert item with empty area") + } + + looseArea := newOctreeCubeFromAABB(aabb) + nodeIndex := t.pickNodeForItem(looseArea) + tightArea := newOctreeAABBFromAABB(aabb) t.increaseNodeItems(nodeIndex) if t.freeItemIDs.IsEmpty() { id := TreeItemID(len(t.items)) t.idMappings = append(t.idMappings, int32(id)) t.items = append(t.items, octreeItem[T]{ - id: id, - node: nodeIndex, - box: box, - value: value, + id: id, + node: nodeIndex, + tightArea: tightArea, + value: value, }) return id } else { id := t.freeItemIDs.Pop() itemIndex := t.idMappings[id] item := &t.items[itemIndex] - item.box = box + item.tightArea = tightArea item.value = value item.node = nodeIndex return item.id } } -// Update repositions the item with the specified id to the new area. -func (t *Octree[T]) Update(id TreeItemID, area Area) { +// Update repositions and resizes the item with the specified id to the new +// axis-aligned bounding box. +// +// As with [Octree.Insert], the box must not be empty, otherwise this function +// panics. Updating an item that has already been removed panics as well. +func (t *Octree[T]) Update(id TreeItemID, aabb shape3d.AABB) { + if aabb.IsEmpty() { + panic("cannot update item to empty area") + } + itemIndex := t.idMappings[id] item := &t.items[itemIndex] if item.node == nullOctreeIndex { panic("cannot update removed item") } - item.box = newOctreeAABBFromArea(area) + item.tightArea = newOctreeAABBFromAABB(aabb) oldNodeIndex := item.node t.decreaseNodeItems(item.node) // previous node - item.node = t.pickNodeForItem(area) + item.node = t.pickNodeForItem(newOctreeCubeFromAABB(aabb)) t.increaseNodeItems(item.node) // new node t.gcNode(oldNodeIndex) } @@ -189,7 +207,7 @@ func (t *Octree[T]) Remove(id TreeItemID) { // QuerySegment finds all items that intersect the specified segment. Each // found item is passed to the specified yield function. The order in which // items are passed is undefined and might change between invocations. -func (t *Octree[T]) QuerySegment(segment Segment, yield VisitorFunc[T]) { +func (t *Octree[T]) QuerySegment(segment shape3d.Segment, yield VisitorFunc[T]) { t.resetVisitStats() t.refresh() t.visitNodeInSegment(0, &segment, yield) @@ -199,7 +217,7 @@ func (t *Octree[T]) QuerySegment(segment Segment, yield VisitorFunc[T]) { // axis-aligned bounding box. Each found item is passed to the specified yield // function. The order in which items are passed is undefined and might change // between invocations. -func (t *Octree[T]) QueryAABB(aabb AABB, yield VisitorFunc[T]) { +func (t *Octree[T]) QueryAABB(aabb shape3d.AABB, yield VisitorFunc[T]) { t.resetVisitStats() t.refresh() t.visitNodeInAABB(0, &aabb, yield) @@ -249,7 +267,7 @@ func (t *Octree[T]) itemsAtDepth(nodeIndex int32, currentDepth, depth uint32) ui return result } -func (t *Octree[T]) pickNodeForItem(area Area) int32 { +func (t *Octree[T]) pickNodeForItem(area octreeCube) int32 { bestNodeIndex := nullOctreeIndex currentNodeIndex := int32(0) var depth uint32 @@ -264,25 +282,25 @@ func (t *Octree[T]) pickNodeForItem(area Area) int32 { return bestNodeIndex } -func (t *Octree[T]) pickChildNode(parentNodeIndex int32, area Area) int32 { +func (t *Octree[T]) pickChildNode(parentNodeIndex int32, area octreeCube) int32 { parentNode := &t.nodes[parentNodeIndex] parentLooseArea := parentNode.looseArea // Make sure that it can fit inside a child. The requirement is that - // the radius must be smaller than the loose margin of the child. - childLooseRadius := parentLooseArea.r / 2.0 - if area.r > (childLooseRadius / 2.0) { // div by 2 to convert to margin + // the half-size must be smaller than the loose margin of the child. + childLooseHalfSize := parentLooseArea.halfSize / 2.0 + if area.halfSize > (childLooseHalfSize / 2.0) { // div by 2 to convert to margin return nullOctreeIndex } - // It has to be inside one of the four children. + // It has to be inside one of the eight children. var ( childIndex = 0 childX = parentLooseArea.x childY = parentLooseArea.y childZ = parentLooseArea.z ) - childOffset := parentLooseArea.r / 4.0 + childOffset := parentLooseArea.halfSize / 4.0 if area.x < parentLooseArea.x { childX -= childOffset } else { @@ -307,10 +325,10 @@ func (t *Octree[T]) pickChildNode(parentNodeIndex int32, area Area) int32 { } childLooseArea := octreeCube{ - x: childX, - y: childY, - z: childZ, - r: childLooseRadius, + x: childX, + y: childY, + z: childZ, + halfSize: childLooseHalfSize, } if t.freeNodeIndices.IsEmpty() { childNodeIndex := int32(len(t.nodes)) // predict next node index @@ -438,29 +456,29 @@ func (t *Octree[T]) updateAABB(nodeIndex int32) bool { for _, childIndex := range node.children { if childIndex != nullOctreeIndex { child := &t.nodes[childIndex] - result = mergeOctreeAABBs(result, child.box) + result = mergeOctreeAABBs(result, child.tightArea) } } itemIndex := node.itemOffset for range node.itemCount { item := &t.items[itemIndex] - result = mergeOctreeAABBs(result, item.box) + result = mergeOctreeAABBs(result, item.tightArea) itemIndex++ } - node.box = result + node.tightArea = result node.isDirty = false return true } -func (t *Octree[T]) visitNodeInSegment(nodeIndex int32, querySegment *Segment, yield VisitorFunc[T]) bool { +func (t *Octree[T]) visitNodeInSegment(nodeIndex int32, querySegment *shape3d.Segment, yield VisitorFunc[T]) bool { node := &t.nodes[nodeIndex] - if node.box.intersectsSegment(querySegment) { + if node.tightArea.intersectsSegment(querySegment) { t.nodeCountAccepted++ itemIndex := node.itemOffset for range node.itemCount { item := &t.items[itemIndex] - if item.box.intersectsSegment(querySegment) { + if item.tightArea.intersectsSegment(querySegment) { t.itemCountAccepted++ if !yield(item.value) { return false @@ -483,14 +501,14 @@ func (t *Octree[T]) visitNodeInSegment(nodeIndex int32, querySegment *Segment, y return true } -func (t *Octree[T]) visitNodeInAABB(nodeIndex int32, queryAABB *AABB, yield VisitorFunc[T]) bool { +func (t *Octree[T]) visitNodeInAABB(nodeIndex int32, queryAABB *shape3d.AABB, yield VisitorFunc[T]) bool { node := &t.nodes[nodeIndex] - if node.box.intersectsAABB(queryAABB) { + if node.tightArea.intersectsAABB(queryAABB) { t.nodeCountAccepted++ itemIndex := node.itemOffset for range node.itemCount { item := &t.items[itemIndex] - if item.box.intersectsAABB(queryAABB) { + if item.tightArea.intersectsAABB(queryAABB) { t.itemCountAccepted++ if !yield(item.value) { return false @@ -524,10 +542,17 @@ var emptyOctreeNodeChildren = [8]int32{ } type octreeNode struct { - parent int32 - children [8]int32 - looseArea octreeCube - box octreeAABB + parent int32 + children [8]int32 + + // looseArea is the fixed cube that determines which items can be placed + // in this node. It is twice the size of the node's share of the tree. + looseArea octreeCube + + // tightArea is the cached bounding box of everything actually stored in + // this node and its descendants. It is what queries are tested against. + tightArea octreeAABB + itemCount uint32 itemOffset uint32 placeOffset uint32 @@ -539,17 +564,37 @@ func (n *octreeNode) isEmpty() bool { } type octreeItem[T any] struct { - id TreeItemID - node int32 - box octreeAABB - value T + id TreeItemID + node int32 + tightArea octreeAABB + value T } +// octreeCube is a cube, described through its center and half-size, that is +// used to determine the node in which an item should be placed. type octreeCube struct { - x float64 - y float64 - z float64 - r float64 + x float64 + y float64 + z float64 + halfSize float64 +} + +// newOctreeCubeFromAABB returns the smallest cube, centered at the center of +// the given box, that fully contains it. As node placement is based on cubes, +// an elongated box is placed as though it were as large along every axis as it +// is along its longest one. +func newOctreeCubeFromAABB(aabb shape3d.AABB) octreeCube { + const half = 1.0 / 2.0 + return octreeCube{ + x: (aabb.MinX + aabb.MaxX) * half, + y: (aabb.MinY + aabb.MaxY) * half, + z: (aabb.MinZ + aabb.MaxZ) * half, + halfSize: max( + (aabb.MaxX-aabb.MinX), + (aabb.MaxY-aabb.MinY), + (aabb.MaxZ-aabb.MinZ), + ) * half, + } } type octreeAABB struct { @@ -572,14 +617,14 @@ func emptyOctreeAABB() octreeAABB { } } -func newOctreeAABBFromArea(area Area) octreeAABB { +func newOctreeAABBFromAABB(aabb shape3d.AABB) octreeAABB { return octreeAABB{ - minX: area.x - area.r, - minY: area.y - area.r, - minZ: area.z - area.r, - maxX: area.x + area.r, - maxY: area.y + area.r, - maxZ: area.z + area.r, + minX: aabb.MinX, + minY: aabb.MinY, + minZ: aabb.MinZ, + maxX: aabb.MaxX, + maxY: aabb.MaxY, + maxZ: aabb.MaxZ, } } @@ -598,51 +643,51 @@ func (aabb *octreeAABB) isEmpty() bool { return (aabb.minX > aabb.maxX) || (aabb.minY > aabb.maxY) || (aabb.minZ > aabb.maxZ) } -func (aabb *octreeAABB) intersectsSegment(segment *Segment) bool { +func (aabb *octreeAABB) intersectsSegment(segment *shape3d.Segment) bool { if aabb.isEmpty() { return false } - delta := dprec.Vec3Diff(segment.b, segment.a) + delta := dprec.Vec3Diff(segment.B, segment.A) var tCloseX, tFarX float64 if delta.X == 0.0 { - if (segment.a.X < aabb.minX) || (segment.a.X > aabb.maxX) { + if (segment.A.X < aabb.minX) || (segment.A.X > aabb.maxX) { return false // both points are outside the box on the left or right } tCloseX = -math.MaxFloat64 tFarX = math.MaxFloat64 } else { - tLowX := (aabb.minX - segment.a.X) / delta.X - tHighX := (aabb.maxX - segment.a.X) / delta.X + tLowX := (aabb.minX - segment.A.X) / delta.X + tHighX := (aabb.maxX - segment.A.X) / delta.X tCloseX = min(tLowX, tHighX) tFarX = max(tLowX, tHighX) } var tCloseY, tFarY float64 if delta.Y == 0.0 { - if (segment.a.Y < aabb.minY) || (segment.a.Y > aabb.maxY) { + if (segment.A.Y < aabb.minY) || (segment.A.Y > aabb.maxY) { return false // both points are outside the box on the top or bottom } tCloseY = -math.MaxFloat64 tFarY = math.MaxFloat64 } else { - tLowY := (aabb.minY - segment.a.Y) / delta.Y - tHighY := (aabb.maxY - segment.a.Y) / delta.Y + tLowY := (aabb.minY - segment.A.Y) / delta.Y + tHighY := (aabb.maxY - segment.A.Y) / delta.Y tCloseY = min(tLowY, tHighY) tFarY = max(tLowY, tHighY) } var tCloseZ, tFarZ float64 if delta.Z == 0.0 { - if (segment.a.Z < aabb.minZ) || (segment.a.Z > aabb.maxZ) { + if (segment.A.Z < aabb.minZ) || (segment.A.Z > aabb.maxZ) { return false // both points are outside the box on the front or back } tCloseZ = -math.MaxFloat64 tFarZ = math.MaxFloat64 } else { - tLowZ := (aabb.minZ - segment.a.Z) / delta.Z - tHighZ := (aabb.maxZ - segment.a.Z) / delta.Z + tLowZ := (aabb.minZ - segment.A.Z) / delta.Z + tHighZ := (aabb.maxZ - segment.A.Z) / delta.Z tCloseZ = min(tLowZ, tHighZ) tFarZ = max(tLowZ, tHighZ) } @@ -653,14 +698,14 @@ func (aabb *octreeAABB) intersectsSegment(segment *Segment) bool { return tClose <= tFar && tClose <= 1.0 && tFar >= 0.0 } -func (aabb *octreeAABB) intersectsAABB(other *AABB) bool { +func (aabb *octreeAABB) intersectsAABB(other *shape3d.AABB) bool { if aabb.isEmpty() { return false } - return (aabb.minX <= other.maxX) && - (aabb.minY <= other.maxY) && - (aabb.maxX >= other.minX) && - (aabb.maxY >= other.minY) && - (aabb.minZ <= other.maxZ) && - (aabb.maxZ >= other.minZ) + return (aabb.minX <= other.MaxX) && + (aabb.minY <= other.MaxY) && + (aabb.maxX >= other.MinX) && + (aabb.maxY >= other.MinY) && + (aabb.minZ <= other.MaxZ) && + (aabb.maxZ >= other.MinZ) } diff --git a/core/spatial/query3d/octree_test.go b/core/spatial/query3d/octree_test.go index e068609b..769941ba 100644 --- a/core/spatial/query3d/octree_test.go +++ b/core/spatial/query3d/octree_test.go @@ -12,18 +12,10 @@ import ( "github.com/mokiat/lacking/core/spatial/shape3d" ) -// areaFromSphere builds an Area from sphere center coordinates and radius. -func areaFromSphere(x, y, z, radius float64) query3d.Area { - return query3d.AreaFromSphere(shape3d.Sphere{ - Center: dprec.NewVec3(x, y, z), - Radius: radius, - }) -} - // aabbFromSphere builds an AABB enclosing a sphere with the given center // coordinates and radius. -func aabbFromSphere(x, y, z, radius float64) query3d.AABB { - return query3d.AABBFromSphere(shape3d.Sphere{ +func aabbFromSphere(x, y, z, radius float64) shape3d.AABB { + return shape3d.AABBFromSphere(shape3d.Sphere{ Center: dprec.NewVec3(x, y, z), Radius: radius, }) @@ -47,6 +39,60 @@ var _ = Describe("Octree", func() { Expect(state.ItemCount).To(Equal(uint32(0))) }) + It("panics when an item with an empty box is inserted", func() { + emptyAABB := shape3d.NewAABB(1.0, 1.0, 1.0, -1.0, -1.0, -1.0) + Expect(func() { tree.Insert(emptyAABB, "Empty") }).To(Panic()) + }) + + It("panics when an item is updated to an empty box", func() { + itemID := tree.Insert(aabbFromSphere(0.0, 0.0, 0.0, 1.0), "Item") + emptyAABB := shape3d.NewAABB(1.0, 1.0, 1.0, -1.0, -1.0, -1.0) + Expect(func() { tree.Update(itemID, emptyAABB) }).To(Panic()) + }) + + When("an item has a non-cubic box", func() { + BeforeEach(func() { + // A rod stretching along the X axis. Its bounding cube would span + // 40 units in every direction, whereas the box itself is only two + // units thick along Y and Z. + tree.Insert( + shape3d.NewAABB(-40.0, -2.0, -2.0, 40.0, 2.0, 2.0), + "Rod", + ) + }) + + It("is found through a query that overlaps the box", func() { + var found []string + tree.QueryAABB(aabbFromSphere(30.0, 0.0, 0.0, 2.0), func(item string) bool { + found = append(found, item) + return true + }) + Expect(found).To(ConsistOf("Rod")) + }) + + It("is not found through a query that only overlaps its bounding cube", func() { + var found []string + tree.QueryAABB(aabbFromSphere(30.0, 20.0, 0.0, 2.0), func(item string) bool { + found = append(found, item) + return true + }) + Expect(found).To(BeEmpty()) + }) + + It("is not found through a segment that only crosses its bounding cube", func() { + segment := shape3d.NewSegment( + dprec.NewVec3(20.0, 10.0, -30.0), + dprec.NewVec3(20.0, 10.0, 30.0), + ) + var found []string + tree.QuerySegment(segment, func(item string) bool { + found = append(found, item) + return true + }) + Expect(found).To(BeEmpty()) + }) + }) + When("items are inserted", func() { var ( firstItemID query3d.TreeItemID @@ -56,15 +102,15 @@ var _ = Describe("Octree", func() { BeforeEach(func() { firstItemID = tree.Insert( - areaFromSphere(16.0, 16.0, 16.0, 2.0), + aabbFromSphere(16.0, 16.0, 16.0, 2.0), "First", ) secondItemID = tree.Insert( - areaFromSphere(48.0, 48.0, 48.0, 2.0), + aabbFromSphere(48.0, 48.0, 48.0, 2.0), "Second", ) thirdItemID = tree.Insert( - areaFromSphere(-16.0, -48.0, -16.0, 32.0), + aabbFromSphere(-16.0, -48.0, -16.0, 32.0), "Third", ) }) @@ -87,7 +133,7 @@ var _ = Describe("Octree", func() { It("is possible to segment-search for items", func() { from := dprec.NewVec3(1.0, 1.0, 1.0) to := dprec.NewVec3(127.0, 127.0, 127.0) - segment := query3d.NewSegment(from, to) + segment := shape3d.NewSegment(from, to) var found []string tree.QuerySegment(segment, func(item string) bool { found = append(found, item) @@ -99,7 +145,7 @@ var _ = Describe("Octree", func() { It("stops QuerySegment after the visitor returns false", func() { from := dprec.NewVec3(1.0, 1.0, 1.0) to := dprec.NewVec3(127.0, 127.0, 127.0) - segment := query3d.NewSegment(from, to) + segment := shape3d.NewSegment(from, to) count := 0 tree.QuerySegment(segment, func(item string) bool { count++ @@ -150,7 +196,7 @@ var _ = Describe("Octree", func() { When("an item is updated", func() { BeforeEach(func() { tree.Update(secondItemID, - areaFromSphere(-48.0, 48.0, -48.0, 2.0), + aabbFromSphere(-48.0, 48.0, -48.0, 2.0), ) }) @@ -166,7 +212,7 @@ var _ = Describe("Octree", func() { It("is reflected in segment-search for items", func() { from := dprec.NewVec3(1.0, 1.0, 1.0) to := dprec.NewVec3(127.0, 127.0, 127.0) - segment := query3d.NewSegment(from, to) + segment := shape3d.NewSegment(from, to) var found []string tree.QuerySegment(segment, func(item string) bool { found = append(found, item) @@ -207,7 +253,7 @@ var _ = Describe("Octree", func() { It("does not return an active item id on new insert", func() { tree.Stats() // forces internal reordering of items (white box testing) secondItemID = tree.Insert( - areaFromSphere(48.0, 48.0, 48.0, 2.0), + aabbFromSphere(48.0, 48.0, 48.0, 2.0), "Second", ) Expect(secondItemID).ToNot(Equal(firstItemID)) @@ -217,7 +263,7 @@ var _ = Describe("Octree", func() { It("is reflected in segment-search for items", func() { from := dprec.NewVec3(1.0, 1.0, 1.0) to := dprec.NewVec3(127.0, 127.0, 127.0) - segment := query3d.NewSegment(from, to) + segment := shape3d.NewSegment(from, to) var found []string tree.QuerySegment(segment, func(item string) bool { found = append(found, item) @@ -245,7 +291,7 @@ var _ = Describe("Octree", func() { // A tiny item placed off-center descends to the deepest allowed // node, allocating one node per depth level along the way. deepItemID = tree.Insert( - areaFromSphere(60.0, 60.0, 60.0, 1.0), + aabbFromSphere(60.0, 60.0, 60.0, 1.0), "Deep", ) }) @@ -273,7 +319,7 @@ var _ = Describe("Octree", func() { // A large item can no longer fit in any child, so it lands on // the root and the vacated branch must collapse. tree.Update(deepItemID, - areaFromSphere(0.0, 0.0, 0.0, 60.0), + aabbFromSphere(0.0, 0.0, 0.0, 60.0), ) }) @@ -293,11 +339,11 @@ var _ = Describe("Octree", func() { // leaves. Removing the far item must collapse its leaf and shrink // the cached bounding boxes of the surviving ancestors. tree.Insert( - areaFromSphere(16.0, 16.0, 16.0, 2.0), + aabbFromSphere(16.0, 16.0, 16.0, 2.0), "Near", ) farItemID = tree.Insert( - areaFromSphere(60.0, 60.0, 60.0, 1.0), + aabbFromSphere(60.0, 60.0, 60.0, 1.0), "Far", ) // Settle the tree so every cached box is clean. Only the collapse @@ -344,11 +390,11 @@ var _ = Describe("Octree", func() { ids := make([]query3d.TreeItemID, count) expected := make(map[query3d.TreeItemID]string, count) - positionFor := func(i int) query3d.Area { + positionFor := func(i int) shape3d.AABB { x := float64(-60 + (i*7)%120) y := float64(-60 + (i*13)%120) z := float64(-60 + (i*5)%120) - return areaFromSphere(x, y, z, 1.0) + return aabbFromSphere(x, y, z, 1.0) } // Populate the tree. diff --git a/core/spatial/query3d/segment.go b/core/spatial/query3d/segment.go deleted file mode 100644 index 719c2850..00000000 --- a/core/spatial/query3d/segment.go +++ /dev/null @@ -1,18 +0,0 @@ -package query3d - -import "github.com/mokiat/gomath/dprec" - -// Segment represents a line segment in 3D space that can be used for spatial -// queries. -type Segment struct { - a dprec.Vec3 - b dprec.Vec3 -} - -// NewSegment creates a new [Segment] with the given endpoints. -func NewSegment(a, b dprec.Vec3) Segment { - return Segment{ - a: a, - b: b, - } -} From b4cbe44c9d8c29f7e53c192367d30be011d8ff93 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sat, 8 Aug 2026 18:12:12 +0300 Subject: [PATCH 63/85] Improve Quadtree API in query2d package --- core/spatial/placement2d/scene.go | 18 ++- core/spatial/query2d/aabb.go | 43 ------- core/spatial/query2d/area.go | 33 ----- core/spatial/query2d/doc.go | 13 +- core/spatial/query2d/quadtree.go | 178 ++++++++++++++++---------- core/spatial/query2d/quadtree_test.go | 94 ++++++++++---- core/spatial/query2d/segment.go | 18 --- 7 files changed, 197 insertions(+), 200 deletions(-) delete mode 100644 core/spatial/query2d/aabb.go delete mode 100644 core/spatial/query2d/area.go delete mode 100644 core/spatial/query2d/segment.go diff --git a/core/spatial/placement2d/scene.go b/core/spatial/placement2d/scene.go index 4af71e52..aaccec69 100644 --- a/core/spatial/placement2d/scene.go +++ b/core/spatial/placement2d/scene.go @@ -146,7 +146,7 @@ func (s *Scene[O, S, M]) SetObjectTransform(objID ObjectID, transform shape2d.Tr s.eachObjectShape(object, func(_ int32, shape *shape[S]) { shape.update(transform) bc := shape.boundingCircle() - s.shapeTree.Update(shape.spatialID, query2d.AreaFromCircle(bc)) + s.shapeTree.Update(shape.spatialID, shape2d.AABBFromCircle(bc)) }) } @@ -305,7 +305,7 @@ func (s *Scene[O, S, M]) CreateMesh(info MeshInfo[M]) MeshID { ), } representation := newMeshRepresentation(shape2d.TransformedMesh(info.Mesh, transform)) - area := query2d.AreaFromCircle(representation.boundingCircle()) + area := shape2d.AABBFromCircle(representation.boundingCircle()) index := s.allocateMesh() s.meshes[index] = meshShape[M]{ @@ -343,11 +343,9 @@ func (s *Scene[O, S, M]) SetMeshUserData(meshID MeshID, userData M) { // CollectSegmentIntersections collects all intersections of the segment // with objects in the scene. func (s *Scene[O, S, M]) CollectSegmentIntersections(segment shape2d.Segment, filter Filter, yield ContactCallback) { - querySegment := query2d.NewSegment(segment.A, segment.B) - if !filter.SkipDynamic { s.shapeCandidates = s.shapeCandidates[:0] - s.shapeTree.QuerySegment(querySegment, func(index int32) bool { + s.shapeTree.QuerySegment(segment, func(index int32) bool { s.shapeCandidates = append(s.shapeCandidates, index) return true }) @@ -356,7 +354,7 @@ func (s *Scene[O, S, M]) CollectSegmentIntersections(segment shape2d.Segment, fi if !filter.SkipStatic { s.meshCandidates = s.meshCandidates[:0] - s.meshTree.QuerySegment(querySegment, func(index int32) bool { + s.meshTree.QuerySegment(segment, func(index int32) bool { s.meshCandidates = append(s.meshCandidates, index) return true }) @@ -375,7 +373,7 @@ func (s *Scene[O, S, M]) CheckSegmentIntersection(segment shape2d.Segment, filte // CollectCircleIntersections collects all intersections of the circle // with objects in the scene. func (s *Scene[O, S, M]) CollectCircleIntersections(circle shape2d.Circle, filter Filter, yield ContactCallback) { - queryAABB := query2d.AABBFromCircle(circle) + queryAABB := shape2d.AABBFromCircle(circle) if !filter.SkipDynamic { s.shapeCandidates = s.shapeCandidates[:0] @@ -407,7 +405,7 @@ func (s *Scene[O, S, M]) CheckCircleIntersection(circle shape2d.Circle, filter F // CollectRectangleIntersections collects all intersections of the rectangle // with objects in the scene. func (s *Scene[O, S, M]) CollectRectangleIntersections(rectangle shape2d.Rectangle, filter Filter, yield ContactCallback) { - queryAABB := query2d.AABBFromRectangle(rectangle) + queryAABB := shape2d.AABBFromRectangle(rectangle) if !filter.SkipDynamic { s.shapeCandidates = s.shapeCandidates[:0] @@ -445,7 +443,7 @@ func (s *Scene[O, S, M]) CollectIntersections(yield ContactCallback) { continue } - queryAABB := query2d.AABBFromCircle(srcShape.boundingCircle()) + queryAABB := shape2d.AABBFromCircle(srcShape.boundingCircle()) s.shapeCandidates = s.shapeCandidates[:0] s.shapeTree.QueryAABB(queryAABB, func(tgtIndex int32) bool { @@ -514,7 +512,7 @@ func (s *Scene[O, S, M]) attachShape( index := s.allocateShape() representation.update(object.transform) - area := query2d.AreaFromCircle(representation.boundingCircle()) + area := shape2d.AABBFromCircle(representation.boundingCircle()) s.shapes[index] = shape[S]{ objectIndex: objectIndex, diff --git a/core/spatial/query2d/aabb.go b/core/spatial/query2d/aabb.go deleted file mode 100644 index fbdbbb1d..00000000 --- a/core/spatial/query2d/aabb.go +++ /dev/null @@ -1,43 +0,0 @@ -package query2d - -import "github.com/mokiat/lacking/core/spatial/shape2d" - -// AABB is an axis-aligned bounding box that can be used for spatial queries. -type AABB struct { - minX float64 - minY float64 - maxX float64 - maxY float64 -} - -// NewAABB creates a new [AABB] with the given minimum and maximum coordinates. -func NewAABB(minX, minY, maxX, maxY float64) AABB { - return AABB{ - minX: minX, - minY: minY, - maxX: maxX, - maxY: maxY, - } -} - -// AABBFromCircle creates an [AABB] that fully contains the given circle. -func AABBFromCircle(circle shape2d.Circle) AABB { - return AABB{ - minX: circle.Center.X - circle.Radius, - minY: circle.Center.Y - circle.Radius, - maxX: circle.Center.X + circle.Radius, - maxY: circle.Center.Y + circle.Radius, - } -} - -// AABBFromRectangle creates an [AABB] from the given rectangle's center and -// half-extents. The rectangle orientation is ignored, so the result encloses -// the rectangle only when it is axis-aligned. -func AABBFromRectangle(rect shape2d.Rectangle) AABB { - return AABB{ - minX: rect.Center.X - rect.HalfWidth, - minY: rect.Center.Y - rect.HalfHeight, - maxX: rect.Center.X + rect.HalfWidth, - maxY: rect.Center.Y + rect.HalfHeight, - } -} diff --git a/core/spatial/query2d/area.go b/core/spatial/query2d/area.go deleted file mode 100644 index a2ed416b..00000000 --- a/core/spatial/query2d/area.go +++ /dev/null @@ -1,33 +0,0 @@ -package query2d - -import "github.com/mokiat/lacking/core/spatial/shape2d" - -// Area represents the spatial area of an object in the 2D space. -type Area struct { - x float64 - y float64 - r float64 -} - -// AreaFromCircle creates an [Area] that covers the given circle. -func AreaFromCircle(circle shape2d.Circle) Area { - return Area{ - x: circle.Center.X, - y: circle.Center.Y, - r: circle.Radius, - } -} - -// AreaFromRectangle creates an [Area] that covers the given rectangle. -// -// The area is a circle centered on the rectangle, with a radius equal to the -// larger of the rectangle's half-width and half-height. This covers the -// rectangle along its shorter axis but not necessarily its corners; it is a -// conservative bound for broad-phase queries rather than an exact fit. -func AreaFromRectangle(rect shape2d.Rectangle) Area { - return Area{ - x: rect.Center.X, - y: rect.Center.Y, - r: max(rect.HalfWidth, rect.HalfHeight), - } -} diff --git a/core/spatial/query2d/doc.go b/core/spatial/query2d/doc.go index d8a23c63..bdf573a4 100644 --- a/core/spatial/query2d/doc.go +++ b/core/spatial/query2d/doc.go @@ -1,15 +1,16 @@ // Package query2d provides a 2D spatial query interface. // -// The package is built around a [Quadtree], a loose quadtree that indexes items -// by their spatial [Area] and allows them to be searched through -// [Quadtree.QueryAABB] and [Quadtree.QuerySegment]. +// The package is built around a [Quadtree], a loose quadtree that indexes +// items by their axis-aligned bounding box ([shape2d.AABB]) and allows them to +// be searched through [Quadtree.QueryAABB] and [Quadtree.QuerySegment]. // // It is intended as a broad-phase (high-level) pass: queries are conservative // and may yield false positives, so callers are expected to run their own // narrow-phase tests on the returned items. It will never omit an item that // truly matches the query. // -// Every item is reduced to a center and a half-extent (an axis-aligned -// bounding box). As a result, non-square shapes are indexed by their bounding -// square, which is a deliberate trade-off in favor of speed and simplicity. +// Every item is reduced to an axis-aligned bounding box, which means that +// orientation and concavity are not taken into account. Callers get the +// tightest results by passing the smallest box that still fully encompasses +// the item. package query2d diff --git a/core/spatial/query2d/quadtree.go b/core/spatial/query2d/quadtree.go index 1584dd73..bc6ad50a 100644 --- a/core/spatial/query2d/quadtree.go +++ b/core/spatial/query2d/quadtree.go @@ -6,6 +6,7 @@ import ( "github.com/mokiat/gog/ds" "github.com/mokiat/gog/opt" "github.com/mokiat/gomath/dprec" + "github.com/mokiat/lacking/core/spatial/shape2d" ) // QuadtreeSettings contains the settings for a [Quadtree]. @@ -77,11 +78,11 @@ func NewQuadtree[T any](settings QuadtreeSettings) *Quadtree[T] { itemOffset: 0, placeOffset: 0, looseArea: quadtreeQuad{ - x: 0.0, - y: 0.0, - r: size, // using size here since a loose area has twice the size + x: 0.0, + y: 0.0, + halfSize: size, // using size here since a loose area has twice the size }, - box: emptyQuadtreeAABB(), + tightArea: emptyQuadtreeAABB(), }) return &Quadtree[T]{ @@ -128,45 +129,63 @@ func (t *Quadtree[T]) VisitStats() TreeVisitStats { } } -// Insert adds an item, which occupies the specified area, to this -// tree. -func (t *Quadtree[T]) Insert(area Area, value T) TreeItemID { - nodeIndex := t.pickNodeForItem(area) - box := newQuadtreeAABBFromArea(area) +// Insert adds an item, which occupies the specified axis-aligned bounding +// box, to this tree. +// +// The box must not be empty (as per [shape2d.AABB.IsEmpty]), otherwise this +// function panics. The box should be contained within the bounds of the tree, +// otherwise the placement of the item is undefined. +func (t *Quadtree[T]) Insert(aabb shape2d.AABB, value T) TreeItemID { + if aabb.IsEmpty() { + panic("cannot insert item with empty area") + } + + looseArea := newQuadtreeQuadFromAABB(aabb) + nodeIndex := t.pickNodeForItem(looseArea) + tightArea := newQuadtreeAABBFromAABB(aabb) t.increaseNodeItems(nodeIndex) if t.freeItemIDs.IsEmpty() { id := TreeItemID(len(t.items)) t.idMappings = append(t.idMappings, int32(id)) t.items = append(t.items, quadtreeItem[T]{ - id: id, - node: nodeIndex, - box: box, - value: value, + id: id, + node: nodeIndex, + tightArea: tightArea, + value: value, }) return id } else { id := t.freeItemIDs.Pop() itemIndex := t.idMappings[id] item := &t.items[itemIndex] - item.box = box + item.tightArea = tightArea item.value = value item.node = nodeIndex return item.id } } -// Update repositions the item with the specified id to the new area. -func (t *Quadtree[T]) Update(id TreeItemID, area Area) { +// Update repositions and resizes the item with the specified id to the new +// axis-aligned bounding box. +// +// As with [Quadtree.Insert], the box must not be empty, otherwise this +// function panics. Updating an item that has already been removed panics as +// well. +func (t *Quadtree[T]) Update(id TreeItemID, aabb shape2d.AABB) { + if aabb.IsEmpty() { + panic("cannot update item to empty area") + } + itemIndex := t.idMappings[id] item := &t.items[itemIndex] if item.node == nullQuadtreeIndex { panic("cannot update removed item") } - item.box = newQuadtreeAABBFromArea(area) + item.tightArea = newQuadtreeAABBFromAABB(aabb) oldNodeIndex := item.node t.decreaseNodeItems(item.node) // previous node - item.node = t.pickNodeForItem(area) + item.node = t.pickNodeForItem(newQuadtreeQuadFromAABB(aabb)) t.increaseNodeItems(item.node) // new node t.gcNode(oldNodeIndex) } @@ -188,7 +207,7 @@ func (t *Quadtree[T]) Remove(id TreeItemID) { // QuerySegment finds all items that intersect the specified segment. Each // found item is passed to the specified yield function. The order in which // items are passed is undefined and might change between invocations. -func (t *Quadtree[T]) QuerySegment(segment Segment, yield VisitorFunc[T]) { +func (t *Quadtree[T]) QuerySegment(segment shape2d.Segment, yield VisitorFunc[T]) { t.resetVisitStats() t.refresh() t.visitNodeInSegment(0, &segment, yield) @@ -198,7 +217,7 @@ func (t *Quadtree[T]) QuerySegment(segment Segment, yield VisitorFunc[T]) { // axis-aligned bounding box. Each found item is passed to the specified yield // function. The order in which items are passed is undefined and might change // between invocations. -func (t *Quadtree[T]) QueryAABB(aabb AABB, yield VisitorFunc[T]) { +func (t *Quadtree[T]) QueryAABB(aabb shape2d.AABB, yield VisitorFunc[T]) { t.resetVisitStats() t.refresh() t.visitNodeInAABB(0, &aabb, yield) @@ -248,7 +267,7 @@ func (t *Quadtree[T]) itemsAtDepth(nodeIndex int32, currentDepth, depth uint32) return result } -func (t *Quadtree[T]) pickNodeForItem(area Area) int32 { +func (t *Quadtree[T]) pickNodeForItem(area quadtreeQuad) int32 { bestNodeIndex := nullQuadtreeIndex currentNodeIndex := int32(0) var depth uint32 @@ -263,14 +282,14 @@ func (t *Quadtree[T]) pickNodeForItem(area Area) int32 { return bestNodeIndex } -func (t *Quadtree[T]) pickChildNode(parentNodeIndex int32, area Area) int32 { +func (t *Quadtree[T]) pickChildNode(parentNodeIndex int32, area quadtreeQuad) int32 { parentNode := &t.nodes[parentNodeIndex] parentLooseArea := parentNode.looseArea // Make sure that it can fit inside a child. The requirement is that - // the radius must be smaller than the loose margin of the child. - childLooseRadius := parentLooseArea.r / 2.0 - if area.r > (childLooseRadius / 2.0) { // div by 2 to convert to margin + // the half-size must be smaller than the loose margin of the child. + childLooseHalfSize := parentLooseArea.halfSize / 2.0 + if area.halfSize > (childLooseHalfSize / 2.0) { // div by 2 to convert to margin return nullQuadtreeIndex } @@ -280,7 +299,7 @@ func (t *Quadtree[T]) pickChildNode(parentNodeIndex int32, area Area) int32 { childX = parentLooseArea.x childY = parentLooseArea.y ) - childOffset := parentLooseArea.r / 4.0 + childOffset := parentLooseArea.halfSize / 4.0 if area.x < parentLooseArea.x { childX -= childOffset } else { @@ -299,9 +318,9 @@ func (t *Quadtree[T]) pickChildNode(parentNodeIndex int32, area Area) int32 { } childLooseArea := quadtreeQuad{ - x: childX, - y: childY, - r: childLooseRadius, + x: childX, + y: childY, + halfSize: childLooseHalfSize, } if t.freeNodeIndices.IsEmpty() { childNodeIndex := int32(len(t.nodes)) // predict next node index @@ -429,29 +448,29 @@ func (t *Quadtree[T]) updateAABB(nodeIndex int32) bool { for _, childIndex := range node.children { if childIndex != nullQuadtreeIndex { child := &t.nodes[childIndex] - result = mergeQuadtreeAABBs(result, child.box) + result = mergeQuadtreeAABBs(result, child.tightArea) } } itemIndex := node.itemOffset for range node.itemCount { item := &t.items[itemIndex] - result = mergeQuadtreeAABBs(result, item.box) + result = mergeQuadtreeAABBs(result, item.tightArea) itemIndex++ } - node.box = result + node.tightArea = result node.isDirty = false return true } -func (t *Quadtree[T]) visitNodeInSegment(nodeIndex int32, querySegment *Segment, yield VisitorFunc[T]) bool { +func (t *Quadtree[T]) visitNodeInSegment(nodeIndex int32, querySegment *shape2d.Segment, yield VisitorFunc[T]) bool { node := &t.nodes[nodeIndex] - if node.box.intersectsSegment(querySegment) { + if node.tightArea.intersectsSegment(querySegment) { t.nodeCountAccepted++ itemIndex := node.itemOffset for range node.itemCount { item := &t.items[itemIndex] - if item.box.intersectsSegment(querySegment) { + if item.tightArea.intersectsSegment(querySegment) { t.itemCountAccepted++ if !yield(item.value) { return false @@ -474,14 +493,14 @@ func (t *Quadtree[T]) visitNodeInSegment(nodeIndex int32, querySegment *Segment, return true } -func (t *Quadtree[T]) visitNodeInAABB(nodeIndex int32, queryAABB *AABB, yield VisitorFunc[T]) bool { +func (t *Quadtree[T]) visitNodeInAABB(nodeIndex int32, queryAABB *shape2d.AABB, yield VisitorFunc[T]) bool { node := &t.nodes[nodeIndex] - if node.box.intersectsAABB(queryAABB) { + if node.tightArea.intersectsAABB(queryAABB) { t.nodeCountAccepted++ itemIndex := node.itemOffset for range node.itemCount { item := &t.items[itemIndex] - if item.box.intersectsAABB(queryAABB) { + if item.tightArea.intersectsAABB(queryAABB) { t.itemCountAccepted++ if !yield(item.value) { return false @@ -512,10 +531,17 @@ var emptyQuadtreeNodeChildren = [4]int32{ } type quadtreeNode struct { - parent int32 - children [4]int32 - looseArea quadtreeQuad - box quadtreeAABB + parent int32 + children [4]int32 + + // looseArea is the fixed square that determines which items can be placed + // in this node. It is twice the size of the node's share of the tree. + looseArea quadtreeQuad + + // tightArea is the cached bounding box of everything actually stored in + // this node and its descendants. It is what queries are tested against. + tightArea quadtreeAABB + itemCount uint32 itemOffset uint32 placeOffset uint32 @@ -527,16 +553,34 @@ func (n *quadtreeNode) isEmpty() bool { } type quadtreeItem[T any] struct { - id TreeItemID - node int32 - box quadtreeAABB - value T + id TreeItemID + node int32 + tightArea quadtreeAABB + value T } +// quadtreeQuad is a square, described through its center and half-size, that +// is used to determine the node in which an item should be placed. type quadtreeQuad struct { - x float64 - y float64 - r float64 + x float64 + y float64 + halfSize float64 +} + +// newQuadtreeQuadFromAABB returns the smallest square, centered at the center +// of the given box, that fully contains it. As node placement is based on +// squares, an elongated box is placed as though it were as large along both +// axes as it is along its longest one. +func newQuadtreeQuadFromAABB(aabb shape2d.AABB) quadtreeQuad { + const half = 1.0 / 2.0 + return quadtreeQuad{ + x: (aabb.MinX + aabb.MaxX) * half, + y: (aabb.MinY + aabb.MaxY) * half, + halfSize: max( + (aabb.MaxX-aabb.MinX), + (aabb.MaxY-aabb.MinY), + ) * half, + } } type quadtreeAABB struct { @@ -555,12 +599,12 @@ func emptyQuadtreeAABB() quadtreeAABB { } } -func newQuadtreeAABBFromArea(area Area) quadtreeAABB { +func newQuadtreeAABBFromAABB(aabb shape2d.AABB) quadtreeAABB { return quadtreeAABB{ - minX: area.x - area.r, - minY: area.y - area.r, - maxX: area.x + area.r, - maxY: area.y + area.r, + minX: aabb.MinX, + minY: aabb.MinY, + maxX: aabb.MaxX, + maxY: aabb.MaxY, } } @@ -577,37 +621,37 @@ func (aabb *quadtreeAABB) isEmpty() bool { return (aabb.minX > aabb.maxX) || (aabb.minY > aabb.maxY) } -func (aabb *quadtreeAABB) intersectsSegment(segment *Segment) bool { +func (aabb *quadtreeAABB) intersectsSegment(segment *shape2d.Segment) bool { if aabb.isEmpty() { return false } - delta := dprec.Vec2Diff(segment.b, segment.a) + delta := dprec.Vec2Diff(segment.B, segment.A) var tCloseX, tFarX float64 if delta.X == 0.0 { - if (segment.a.X < aabb.minX) || (segment.a.X > aabb.maxX) { + if (segment.A.X < aabb.minX) || (segment.A.X > aabb.maxX) { return false // both points are outside the box on the left or right } tCloseX = -math.MaxFloat64 tFarX = math.MaxFloat64 } else { - tLowX := (aabb.minX - segment.a.X) / delta.X - tHighX := (aabb.maxX - segment.a.X) / delta.X + tLowX := (aabb.minX - segment.A.X) / delta.X + tHighX := (aabb.maxX - segment.A.X) / delta.X tCloseX = min(tLowX, tHighX) tFarX = max(tLowX, tHighX) } var tCloseY, tFarY float64 if delta.Y == 0.0 { - if (segment.a.Y < aabb.minY) || (segment.a.Y > aabb.maxY) { + if (segment.A.Y < aabb.minY) || (segment.A.Y > aabb.maxY) { return false // both points are outside the box on the top or bottom } tCloseY = -math.MaxFloat64 tFarY = math.MaxFloat64 } else { - tLowY := (aabb.minY - segment.a.Y) / delta.Y - tHighY := (aabb.maxY - segment.a.Y) / delta.Y + tLowY := (aabb.minY - segment.A.Y) / delta.Y + tHighY := (aabb.maxY - segment.A.Y) / delta.Y tCloseY = min(tLowY, tHighY) tFarY = max(tLowY, tHighY) } @@ -618,12 +662,12 @@ func (aabb *quadtreeAABB) intersectsSegment(segment *Segment) bool { return tClose <= tFar && tClose <= 1.0 && tFar >= 0.0 } -func (aabb *quadtreeAABB) intersectsAABB(other *AABB) bool { +func (aabb *quadtreeAABB) intersectsAABB(other *shape2d.AABB) bool { if aabb.isEmpty() { return false } - return (aabb.minX <= other.maxX) && - (aabb.minY <= other.maxY) && - (aabb.maxX >= other.minX) && - (aabb.maxY >= other.minY) + return (aabb.minX <= other.MaxX) && + (aabb.minY <= other.MaxY) && + (aabb.maxX >= other.MinX) && + (aabb.maxY >= other.MinY) } diff --git a/core/spatial/query2d/quadtree_test.go b/core/spatial/query2d/quadtree_test.go index e3c42cb5..0e746430 100644 --- a/core/spatial/query2d/quadtree_test.go +++ b/core/spatial/query2d/quadtree_test.go @@ -14,19 +14,13 @@ import ( // aabbFromCircle builds an AABB enclosing a circle with the given center // coordinates and radius. -func aabbFromCircle(x, y, radius float64) query2d.AABB { - return query2d.AABBFromCircle(shape2d.Circle{ +func aabbFromCircle(x, y, radius float64) shape2d.AABB { + return shape2d.AABBFromCircle(shape2d.Circle{ Center: dprec.NewVec2(x, y), Radius: radius, }) } -// areaFromCircle builds an Area covering a circle with the given center -// coordinates and radius. -func areaFromCircle(x, y, radius float64) query2d.Area { - return query2d.AreaFromCircle(shape2d.NewCircle(dprec.NewVec2(x, y), radius)) -} - var _ = Describe("Quadtree", func() { var ( tree *query2d.Quadtree[string] @@ -45,6 +39,60 @@ var _ = Describe("Quadtree", func() { Expect(state.ItemCount).To(Equal(uint32(0))) }) + It("panics when an item with an empty box is inserted", func() { + emptyAABB := shape2d.NewAABB(1.0, 1.0, -1.0, -1.0) + Expect(func() { tree.Insert(emptyAABB, "Empty") }).To(Panic()) + }) + + It("panics when an item is updated to an empty box", func() { + itemID := tree.Insert(aabbFromCircle(0.0, 0.0, 1.0), "Item") + emptyAABB := shape2d.NewAABB(1.0, 1.0, -1.0, -1.0) + Expect(func() { tree.Update(itemID, emptyAABB) }).To(Panic()) + }) + + When("an item has a non-square box", func() { + BeforeEach(func() { + // A bar stretching along the X axis. Its bounding square would span + // 40 units in every direction, whereas the box itself is only two + // units thick along Y. + tree.Insert( + shape2d.NewAABB(-40.0, -2.0, 40.0, 2.0), + "Bar", + ) + }) + + It("is found through a query that overlaps the box", func() { + var found []string + tree.QueryAABB(aabbFromCircle(30.0, 0.0, 2.0), func(item string) bool { + found = append(found, item) + return true + }) + Expect(found).To(ConsistOf("Bar")) + }) + + It("is not found through a query that only overlaps its bounding square", func() { + var found []string + tree.QueryAABB(aabbFromCircle(30.0, 20.0, 2.0), func(item string) bool { + found = append(found, item) + return true + }) + Expect(found).To(BeEmpty()) + }) + + It("is not found through a segment that only crosses its bounding square", func() { + segment := shape2d.NewSegment( + dprec.NewVec2(-30.0, 10.0), + dprec.NewVec2(30.0, 10.0), + ) + var found []string + tree.QuerySegment(segment, func(item string) bool { + found = append(found, item) + return true + }) + Expect(found).To(BeEmpty()) + }) + }) + When("items are inserted", func() { var ( firstItemID query2d.TreeItemID @@ -54,15 +102,15 @@ var _ = Describe("Quadtree", func() { BeforeEach(func() { firstItemID = tree.Insert( - areaFromCircle(16.0, 16.0, 2.0), + aabbFromCircle(16.0, 16.0, 2.0), "First", ) secondItemID = tree.Insert( - areaFromCircle(48.0, 48.0, 2.0), + aabbFromCircle(48.0, 48.0, 2.0), "Second", ) thirdItemID = tree.Insert( - areaFromCircle(-16.0, -48.0, 32.0), + aabbFromCircle(-16.0, -48.0, 32.0), "Third", ) }) @@ -85,7 +133,7 @@ var _ = Describe("Quadtree", func() { It("is possible to segment-search for items", func() { from := dprec.NewVec2(1.0, 1.0) to := dprec.NewVec2(127.0, 127.0) - segment := query2d.NewSegment(from, to) + segment := shape2d.NewSegment(from, to) var found []string tree.QuerySegment(segment, func(item string) bool { found = append(found, item) @@ -97,7 +145,7 @@ var _ = Describe("Quadtree", func() { It("stops QuerySegment after the visitor returns false", func() { from := dprec.NewVec2(1.0, 1.0) to := dprec.NewVec2(127.0, 127.0) - segment := query2d.NewSegment(from, to) + segment := shape2d.NewSegment(from, to) count := 0 tree.QuerySegment(segment, func(item string) bool { count++ @@ -148,7 +196,7 @@ var _ = Describe("Quadtree", func() { When("an item is updated", func() { BeforeEach(func() { tree.Update(secondItemID, - areaFromCircle(-48.0, 48.0, 2.0), + aabbFromCircle(-48.0, 48.0, 2.0), ) }) @@ -164,7 +212,7 @@ var _ = Describe("Quadtree", func() { It("is reflected in segment-search for items", func() { from := dprec.NewVec2(1.0, 1.0) to := dprec.NewVec2(127.0, 127.0) - segment := query2d.NewSegment(from, to) + segment := shape2d.NewSegment(from, to) var found []string tree.QuerySegment(segment, func(item string) bool { found = append(found, item) @@ -205,7 +253,7 @@ var _ = Describe("Quadtree", func() { It("does not return an active item id on new insert", func() { tree.Stats() // forces internal reordering of items (white box testing) secondItemID = tree.Insert( - areaFromCircle(48.0, 48.0, 2.0), + aabbFromCircle(48.0, 48.0, 2.0), "Second", ) Expect(secondItemID).ToNot(Equal(firstItemID)) @@ -215,7 +263,7 @@ var _ = Describe("Quadtree", func() { It("is reflected in segment-search for items", func() { from := dprec.NewVec2(1.0, 1.0) to := dprec.NewVec2(127.0, 127.0) - segment := query2d.NewSegment(from, to) + segment := shape2d.NewSegment(from, to) var found []string tree.QuerySegment(segment, func(item string) bool { found = append(found, item) @@ -243,7 +291,7 @@ var _ = Describe("Quadtree", func() { // A tiny item placed off-center descends to the deepest allowed // node, allocating one node per depth level along the way. deepItemID = tree.Insert( - areaFromCircle(60.0, 60.0, 1.0), + aabbFromCircle(60.0, 60.0, 1.0), "Deep", ) }) @@ -271,7 +319,7 @@ var _ = Describe("Quadtree", func() { // A large item can no longer fit in any child, so it lands on // the root and the vacated branch must collapse. tree.Update(deepItemID, - areaFromCircle(0.0, 0.0, 60.0), + aabbFromCircle(0.0, 0.0, 60.0), ) }) @@ -291,11 +339,11 @@ var _ = Describe("Quadtree", func() { // leaves. Removing the far item must collapse its leaf and shrink // the cached bounding boxes of the surviving ancestors. tree.Insert( - areaFromCircle(16.0, 16.0, 2.0), + aabbFromCircle(16.0, 16.0, 2.0), "Near", ) farItemID = tree.Insert( - areaFromCircle(60.0, 60.0, 1.0), + aabbFromCircle(60.0, 60.0, 1.0), "Far", ) // Settle the tree so every cached box is clean. Only the collapse @@ -342,10 +390,10 @@ var _ = Describe("Quadtree", func() { ids := make([]query2d.TreeItemID, count) expected := make(map[query2d.TreeItemID]string, count) - positionFor := func(i int) query2d.Area { + positionFor := func(i int) shape2d.AABB { x := float64(-60 + (i*7)%120) y := float64(-60 + (i*13)%120) - return areaFromCircle(x, y, 1.0) + return aabbFromCircle(x, y, 1.0) } // Populate the tree. diff --git a/core/spatial/query2d/segment.go b/core/spatial/query2d/segment.go deleted file mode 100644 index 5330ffe7..00000000 --- a/core/spatial/query2d/segment.go +++ /dev/null @@ -1,18 +0,0 @@ -package query2d - -import "github.com/mokiat/gomath/dprec" - -// Segment represents a line segment in 2D space that can be used for spatial -// queries. -type Segment struct { - a dprec.Vec2 - b dprec.Vec2 -} - -// NewSegment creates a new [Segment] with the given endpoints. -func NewSegment(a, b dprec.Vec2) Segment { - return Segment{ - a: a, - b: b, - } -} From 58842ddcb4e58bd0a4ab9204b5a613db7a22d9cb Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sat, 8 Aug 2026 19:05:07 +0300 Subject: [PATCH 64/85] Handle non-symmetric shapes better in query trees --- core/spatial/query2d/quadtree.go | 76 ++++++++++++------------ core/spatial/query2d/quadtree_test.go | 57 ++++++++++++++++-- core/spatial/query3d/octree.go | 83 ++++++++++++++------------- core/spatial/query3d/octree_test.go | 57 ++++++++++++++++-- 4 files changed, 182 insertions(+), 91 deletions(-) diff --git a/core/spatial/query2d/quadtree.go b/core/spatial/query2d/quadtree.go index bc6ad50a..f6cac3a3 100644 --- a/core/spatial/query2d/quadtree.go +++ b/core/spatial/query2d/quadtree.go @@ -16,7 +16,9 @@ type QuadtreeSettings struct { // // If not specified, a default size of 4096 is used. // - // Inserting an item outside these bounds has undefined behavior. + // Items that lie outside these bounds are still found by queries, but they + // settle in nodes close to the root and therefore degrade query + // performance. Size opt.T[float64] // MaxDepth controls the maximum depth that the tree can reach. @@ -132,17 +134,17 @@ func (t *Quadtree[T]) VisitStats() TreeVisitStats { // Insert adds an item, which occupies the specified axis-aligned bounding // box, to this tree. // -// The box must not be empty (as per [shape2d.AABB.IsEmpty]), otherwise this -// function panics. The box should be contained within the bounds of the tree, -// otherwise the placement of the item is undefined. +// The item is placed in the deepest node that can fully contain the box, so +// the smaller and the better centered the box is, the less work queries have +// to do. The box must not be empty (as per [shape2d.AABB.IsEmpty]), otherwise +// this function panics. func (t *Quadtree[T]) Insert(aabb shape2d.AABB, value T) TreeItemID { if aabb.IsEmpty() { panic("cannot insert item with empty area") } - looseArea := newQuadtreeQuadFromAABB(aabb) - nodeIndex := t.pickNodeForItem(looseArea) tightArea := newQuadtreeAABBFromAABB(aabb) + nodeIndex := t.pickNodeForItem(tightArea) t.increaseNodeItems(nodeIndex) if t.freeItemIDs.IsEmpty() { @@ -182,10 +184,11 @@ func (t *Quadtree[T]) Update(id TreeItemID, aabb shape2d.AABB) { if item.node == nullQuadtreeIndex { panic("cannot update removed item") } - item.tightArea = newQuadtreeAABBFromAABB(aabb) + tightArea := newQuadtreeAABBFromAABB(aabb) + item.tightArea = tightArea oldNodeIndex := item.node t.decreaseNodeItems(item.node) // previous node - item.node = t.pickNodeForItem(newQuadtreeQuadFromAABB(aabb)) + item.node = t.pickNodeForItem(tightArea) t.increaseNodeItems(item.node) // new node t.gcNode(oldNodeIndex) } @@ -267,7 +270,9 @@ func (t *Quadtree[T]) itemsAtDepth(nodeIndex int32, currentDepth, depth uint32) return result } -func (t *Quadtree[T]) pickNodeForItem(area quadtreeQuad) int32 { +// pickNodeForItem returns the deepest node whose loose area still fully +// contains the specified area. +func (t *Quadtree[T]) pickNodeForItem(area quadtreeAABB) int32 { bestNodeIndex := nullQuadtreeIndex currentNodeIndex := int32(0) var depth uint32 @@ -282,37 +287,47 @@ func (t *Quadtree[T]) pickNodeForItem(area quadtreeQuad) int32 { return bestNodeIndex } -func (t *Quadtree[T]) pickChildNode(parentNodeIndex int32, area quadtreeQuad) int32 { +// pickChildNode returns the child of the specified node whose loose area fully +// contains the specified area, allocating that child if it does not exist yet. +// It returns nullQuadtreeIndex if the area does not fit in any child. +func (t *Quadtree[T]) pickChildNode(parentNodeIndex int32, area quadtreeAABB) int32 { parentNode := &t.nodes[parentNodeIndex] parentLooseArea := parentNode.looseArea - // Make sure that it can fit inside a child. The requirement is that - // the half-size must be smaller than the loose margin of the child. - childLooseHalfSize := parentLooseArea.halfSize / 2.0 - if area.halfSize > (childLooseHalfSize / 2.0) { // div by 2 to convert to margin - return nullQuadtreeIndex - } - - // It has to be inside one of the four children. + // The candidate child is the one whose own (tight) quadrant holds the + // center of the area. + const half = 1.0 / 2.0 var ( childIndex = 0 childX = parentLooseArea.x childY = parentLooseArea.y ) childOffset := parentLooseArea.halfSize / 4.0 - if area.x < parentLooseArea.x { + if (area.minX+area.maxX)*half < parentLooseArea.x { childX -= childOffset } else { childIndex += 1 childX += childOffset } - if area.y < parentLooseArea.y { + if (area.minY+area.maxY)*half < parentLooseArea.y { childY -= childOffset } else { childIndex += 2 childY += childOffset } + // The area has to fit within the loose area of that child. Each axis is + // checked against its own extent, so an elongated or a well-centered item + // is no longer held back by its largest dimension and can descend deeper + // than a bounding-square test would allow. + childLooseHalfSize := parentLooseArea.halfSize * half + if (area.minX < childX-childLooseHalfSize) || (area.maxX > childX+childLooseHalfSize) { + return nullQuadtreeIndex + } + if (area.minY < childY-childLooseHalfSize) || (area.maxY > childY+childLooseHalfSize) { + return nullQuadtreeIndex + } + if parentNode.children[childIndex] != nullQuadtreeIndex { return parentNode.children[childIndex] } @@ -559,30 +574,15 @@ type quadtreeItem[T any] struct { value T } -// quadtreeQuad is a square, described through its center and half-size, that -// is used to determine the node in which an item should be placed. +// quadtreeQuad is a square, described through its center and half-size. It +// describes the loose area of a node, which is what an item has to fit into in +// order to be placed there. type quadtreeQuad struct { x float64 y float64 halfSize float64 } -// newQuadtreeQuadFromAABB returns the smallest square, centered at the center -// of the given box, that fully contains it. As node placement is based on -// squares, an elongated box is placed as though it were as large along both -// axes as it is along its longest one. -func newQuadtreeQuadFromAABB(aabb shape2d.AABB) quadtreeQuad { - const half = 1.0 / 2.0 - return quadtreeQuad{ - x: (aabb.MinX + aabb.MaxX) * half, - y: (aabb.MinY + aabb.MaxY) * half, - halfSize: max( - (aabb.MaxX-aabb.MinX), - (aabb.MaxY-aabb.MinY), - ) * half, - } -} - type quadtreeAABB struct { minX float64 minY float64 diff --git a/core/spatial/query2d/quadtree_test.go b/core/spatial/query2d/quadtree_test.go index 0e746430..aa651466 100644 --- a/core/spatial/query2d/quadtree_test.go +++ b/core/spatial/query2d/quadtree_test.go @@ -123,10 +123,13 @@ var _ = Describe("Quadtree", func() { It("has the correct state", func() { state := tree.Stats() - Expect(state.NodeCount).To(Equal(uint32(5))) + Expect(state.NodeCount).To(Equal(uint32(6))) Expect(state.ItemCount).To(Equal(uint32(3))) + // The third item is as wide as a whole child node, but it is + // positioned so that it still fits within the loose area of a + // grandchild along both axes. Expect(state.ItemCountPerDepth).To(Equal([]uint32{ - 0, 1, 2, + 0, 0, 3, })) }) @@ -202,10 +205,10 @@ var _ = Describe("Quadtree", func() { It("has the correct state", func() { state := tree.Stats() - Expect(state.NodeCount).To(Equal(uint32(6))) + Expect(state.NodeCount).To(Equal(uint32(7))) Expect(state.ItemCount).To(Equal(uint32(3))) Expect(state.ItemCountPerDepth).To(Equal([]uint32{ - 0, 1, 2, + 0, 0, 3, })) }) @@ -243,10 +246,10 @@ var _ = Describe("Quadtree", func() { It("has the correct state", func() { state := tree.Stats() - Expect(state.NodeCount).To(Equal(uint32(4))) + Expect(state.NodeCount).To(Equal(uint32(5))) Expect(state.ItemCount).To(Equal(uint32(2))) Expect(state.ItemCountPerDepth).To(Equal([]uint32{ - 0, 1, 1, + 0, 0, 2, })) }) @@ -284,6 +287,48 @@ var _ = Describe("Quadtree", func() { }) }) + When("an item is thin along one axis", func() { + // Both boxes have the same center and the same largest extent, and + // both descend into the same child node. They differ only along Y, + // where the node they would descend into next has its center 15 units + // away. The slab is thin enough along Y to still fit; the block, being + // as tall as it is wide, is not. + var ( + slabAABB = shape2d.NewAABB(-14.0, 32.0, 46.0, 34.0) + blockAABB = shape2d.NewAABB(-14.0, 3.0, 46.0, 63.0) + ) + + It("descends deeper than a square item of the same largest extent", func() { + tree.Insert(slabAABB, "Slab") + state := tree.Stats() + Expect(state.NodeCount).To(Equal(uint32(3))) // root + child + grandchild + Expect(state.ItemCountPerDepth).To(Equal([]uint32{ + 0, 0, 1, + })) + }) + + It("keeps a square item at the depth its largest extent allows", func() { + tree.Insert(blockAABB, "Block") + state := tree.Stats() + Expect(state.NodeCount).To(Equal(uint32(2))) // root + child + Expect(state.ItemCountPerDepth).To(Equal([]uint32{ + 0, 1, 0, + })) + }) + + It("finds both items regardless of the depth they settle at", func() { + tree.Insert(slabAABB, "Slab") + tree.Insert(blockAABB, "Block") + var found []string + tree.QueryAABB(shape2d.NewAABB(15.0, 33.0, 17.0, 33.0), + func(item string) bool { + found = append(found, item) + return true + }) + Expect(found).To(ConsistOf("Slab", "Block")) + }) + }) + When("an item creates a deeply nested branch", func() { var deepItemID query2d.TreeItemID diff --git a/core/spatial/query3d/octree.go b/core/spatial/query3d/octree.go index f9fcdb7d..0f1272a7 100644 --- a/core/spatial/query3d/octree.go +++ b/core/spatial/query3d/octree.go @@ -16,7 +16,9 @@ type OctreeSettings struct { // // If not specified, a default size of 4096 is used. // - // Inserting an item outside these bounds has undefined behavior. + // Items that lie outside these bounds are still found by queries, but they + // settle in nodes close to the root and therefore degrade query + // performance. Size opt.T[float64] // MaxDepth controls the maximum depth that the tree can reach. @@ -133,17 +135,17 @@ func (t *Octree[T]) VisitStats() TreeVisitStats { // Insert adds an item, which occupies the specified axis-aligned bounding // box, to this tree. // -// The box must not be empty (as per [shape3d.AABB.IsEmpty]), otherwise this -// function panics. The box should be contained within the bounds of the tree, -// otherwise the placement of the item is undefined. +// The item is placed in the deepest node that can fully contain the box, so +// the smaller and the better centered the box is, the less work queries have +// to do. The box must not be empty (as per [shape3d.AABB.IsEmpty]), otherwise +// this function panics. func (t *Octree[T]) Insert(aabb shape3d.AABB, value T) TreeItemID { if aabb.IsEmpty() { panic("cannot insert item with empty area") } - looseArea := newOctreeCubeFromAABB(aabb) - nodeIndex := t.pickNodeForItem(looseArea) tightArea := newOctreeAABBFromAABB(aabb) + nodeIndex := t.pickNodeForItem(tightArea) t.increaseNodeItems(nodeIndex) if t.freeItemIDs.IsEmpty() { @@ -182,10 +184,11 @@ func (t *Octree[T]) Update(id TreeItemID, aabb shape3d.AABB) { if item.node == nullOctreeIndex { panic("cannot update removed item") } - item.tightArea = newOctreeAABBFromAABB(aabb) + tightArea := newOctreeAABBFromAABB(aabb) + item.tightArea = tightArea oldNodeIndex := item.node t.decreaseNodeItems(item.node) // previous node - item.node = t.pickNodeForItem(newOctreeCubeFromAABB(aabb)) + item.node = t.pickNodeForItem(tightArea) t.increaseNodeItems(item.node) // new node t.gcNode(oldNodeIndex) } @@ -267,7 +270,9 @@ func (t *Octree[T]) itemsAtDepth(nodeIndex int32, currentDepth, depth uint32) ui return result } -func (t *Octree[T]) pickNodeForItem(area octreeCube) int32 { +// pickNodeForItem returns the deepest node whose loose area still fully +// contains the specified area. +func (t *Octree[T]) pickNodeForItem(area octreeAABB) int32 { bestNodeIndex := nullOctreeIndex currentNodeIndex := int32(0) var depth uint32 @@ -282,18 +287,16 @@ func (t *Octree[T]) pickNodeForItem(area octreeCube) int32 { return bestNodeIndex } -func (t *Octree[T]) pickChildNode(parentNodeIndex int32, area octreeCube) int32 { +// pickChildNode returns the child of the specified node whose loose area fully +// contains the specified area, allocating that child if it does not exist yet. +// It returns nullOctreeIndex if the area does not fit in any child. +func (t *Octree[T]) pickChildNode(parentNodeIndex int32, area octreeAABB) int32 { parentNode := &t.nodes[parentNodeIndex] parentLooseArea := parentNode.looseArea - // Make sure that it can fit inside a child. The requirement is that - // the half-size must be smaller than the loose margin of the child. - childLooseHalfSize := parentLooseArea.halfSize / 2.0 - if area.halfSize > (childLooseHalfSize / 2.0) { // div by 2 to convert to margin - return nullOctreeIndex - } - - // It has to be inside one of the eight children. + // The candidate child is the one whose own (tight) octant holds the center + // of the area. + const half = 1.0 / 2.0 var ( childIndex = 0 childX = parentLooseArea.x @@ -301,25 +304,40 @@ func (t *Octree[T]) pickChildNode(parentNodeIndex int32, area octreeCube) int32 childZ = parentLooseArea.z ) childOffset := parentLooseArea.halfSize / 4.0 - if area.x < parentLooseArea.x { + if (area.minX+area.maxX)*half < parentLooseArea.x { childX -= childOffset } else { childIndex += 1 childX += childOffset } - if area.z < parentLooseArea.z { + if (area.minZ+area.maxZ)*half < parentLooseArea.z { childZ -= childOffset } else { childIndex += 2 childZ += childOffset } - if area.y < parentLooseArea.y { + if (area.minY+area.maxY)*half < parentLooseArea.y { childY -= childOffset } else { childIndex += 4 childY += childOffset } + // The area has to fit within the loose area of that child. Each axis is + // checked against its own extent, so an elongated or a well-centered item + // is no longer held back by its largest dimension and can descend deeper + // than a bounding-cube test would allow. + childLooseHalfSize := parentLooseArea.halfSize * half + if (area.minX < childX-childLooseHalfSize) || (area.maxX > childX+childLooseHalfSize) { + return nullOctreeIndex + } + if (area.minY < childY-childLooseHalfSize) || (area.maxY > childY+childLooseHalfSize) { + return nullOctreeIndex + } + if (area.minZ < childZ-childLooseHalfSize) || (area.maxZ > childZ+childLooseHalfSize) { + return nullOctreeIndex + } + if parentNode.children[childIndex] != nullOctreeIndex { return parentNode.children[childIndex] } @@ -570,8 +588,9 @@ type octreeItem[T any] struct { value T } -// octreeCube is a cube, described through its center and half-size, that is -// used to determine the node in which an item should be placed. +// octreeCube is a cube, described through its center and half-size. It +// describes the loose area of a node, which is what an item has to fit into in +// order to be placed there. type octreeCube struct { x float64 y float64 @@ -579,24 +598,6 @@ type octreeCube struct { halfSize float64 } -// newOctreeCubeFromAABB returns the smallest cube, centered at the center of -// the given box, that fully contains it. As node placement is based on cubes, -// an elongated box is placed as though it were as large along every axis as it -// is along its longest one. -func newOctreeCubeFromAABB(aabb shape3d.AABB) octreeCube { - const half = 1.0 / 2.0 - return octreeCube{ - x: (aabb.MinX + aabb.MaxX) * half, - y: (aabb.MinY + aabb.MaxY) * half, - z: (aabb.MinZ + aabb.MaxZ) * half, - halfSize: max( - (aabb.MaxX-aabb.MinX), - (aabb.MaxY-aabb.MinY), - (aabb.MaxZ-aabb.MinZ), - ) * half, - } -} - type octreeAABB struct { minX float64 minY float64 diff --git a/core/spatial/query3d/octree_test.go b/core/spatial/query3d/octree_test.go index 769941ba..955c2d6a 100644 --- a/core/spatial/query3d/octree_test.go +++ b/core/spatial/query3d/octree_test.go @@ -123,10 +123,13 @@ var _ = Describe("Octree", func() { It("has the correct state", func() { state := tree.Stats() - Expect(state.NodeCount).To(Equal(uint32(5))) + Expect(state.NodeCount).To(Equal(uint32(6))) Expect(state.ItemCount).To(Equal(uint32(3))) + // The third item is as wide as a whole child node, but it is + // positioned so that it still fits within the loose area of a + // grandchild along every axis. Expect(state.ItemCountPerDepth).To(Equal([]uint32{ - 0, 1, 2, + 0, 0, 3, })) }) @@ -202,10 +205,10 @@ var _ = Describe("Octree", func() { It("has the correct state", func() { state := tree.Stats() - Expect(state.NodeCount).To(Equal(uint32(6))) + Expect(state.NodeCount).To(Equal(uint32(7))) Expect(state.ItemCount).To(Equal(uint32(3))) Expect(state.ItemCountPerDepth).To(Equal([]uint32{ - 0, 1, 2, + 0, 0, 3, })) }) @@ -243,10 +246,10 @@ var _ = Describe("Octree", func() { It("has the correct state", func() { state := tree.Stats() - Expect(state.NodeCount).To(Equal(uint32(4))) + Expect(state.NodeCount).To(Equal(uint32(5))) Expect(state.ItemCount).To(Equal(uint32(2))) Expect(state.ItemCountPerDepth).To(Equal([]uint32{ - 0, 1, 1, + 0, 0, 2, })) }) @@ -284,6 +287,48 @@ var _ = Describe("Octree", func() { }) }) + When("an item is thin along one axis", func() { + // Both boxes have the same center and the same largest extent, and + // both descend into the same child node. They differ only along Y, + // where the node they would descend into next has its center 15 units + // away. The slab is thin enough along Y to still fit; the block, being + // as tall as it is wide, is not. + var ( + slabAABB = shape3d.NewAABB(-14.0, 32.0, 15.0, 46.0, 34.0, 17.0) + blockAABB = shape3d.NewAABB(-14.0, 3.0, -14.0, 46.0, 63.0, 46.0) + ) + + It("descends deeper than a cubic item of the same largest extent", func() { + tree.Insert(slabAABB, "Slab") + state := tree.Stats() + Expect(state.NodeCount).To(Equal(uint32(3))) // root + child + grandchild + Expect(state.ItemCountPerDepth).To(Equal([]uint32{ + 0, 0, 1, + })) + }) + + It("keeps a cubic item at the depth its largest extent allows", func() { + tree.Insert(blockAABB, "Block") + state := tree.Stats() + Expect(state.NodeCount).To(Equal(uint32(2))) // root + child + Expect(state.ItemCountPerDepth).To(Equal([]uint32{ + 0, 1, 0, + })) + }) + + It("finds both items regardless of the depth they settle at", func() { + tree.Insert(slabAABB, "Slab") + tree.Insert(blockAABB, "Block") + var found []string + tree.QueryAABB(shape3d.NewAABB(15.0, 33.0, 16.0, 17.0, 33.0, 16.0), + func(item string) bool { + found = append(found, item) + return true + }) + Expect(found).To(ConsistOf("Slab", "Block")) + }) + }) + When("an item creates a deeply nested branch", func() { var deepItemID query3d.TreeItemID From 1c6bfb415375e9fc34a07fcebd8c0af86fffbcb5 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 9 Aug 2026 11:32:35 +0300 Subject: [PATCH 65/85] Improvements to shape packages related to AABB --- core/spatial/shape2d/aabb.go | 18 ++++++++- core/spatial/shape2d/aabb_test.go | 21 +++++++++++ core/spatial/shape2d/mesh.go | 23 ++++++++++++ core/spatial/shape2d/mesh_test.go | 42 +++++++++++++++++++++ core/spatial/shape3d/aabb.go | 20 +++++++++- core/spatial/shape3d/aabb_test.go | 25 +++++++++++++ core/spatial/shape3d/mesh.go | 35 +++++++++++++++++ core/spatial/shape3d/mesh_test.go | 62 +++++++++++++++++++++++++++++++ 8 files changed, 244 insertions(+), 2 deletions(-) diff --git a/core/spatial/shape2d/aabb.go b/core/spatial/shape2d/aabb.go index a496daa9..3775e105 100644 --- a/core/spatial/shape2d/aabb.go +++ b/core/spatial/shape2d/aabb.go @@ -1,6 +1,10 @@ package shape2d -import "github.com/mokiat/gomath/dprec" +import ( + "math" + + "github.com/mokiat/gomath/dprec" +) // AABB represents an axis-aligned bounding box in 2D space. type AABB struct { @@ -24,6 +28,18 @@ func NewAABB(minX, minY, maxX, maxY float64) AABB { } } +// EmptyAABB returns an [AABB] that represents an empty area, suitable as the +// starting point for incrementally growing a bounding box (e.g. via repeated +// min/max updates as points are encountered). +func EmptyAABB() AABB { + return AABB{ + MinX: math.MaxFloat64, + MinY: math.MaxFloat64, + MaxX: -math.MaxFloat64, + MaxY: -math.MaxFloat64, + } +} + // AABBFromCircle returns the smallest [AABB] that fully encompasses the // given circle. func AABBFromCircle(circle Circle) AABB { diff --git a/core/spatial/shape2d/aabb_test.go b/core/spatial/shape2d/aabb_test.go index c56a9571..8b1f365d 100644 --- a/core/spatial/shape2d/aabb_test.go +++ b/core/spatial/shape2d/aabb_test.go @@ -20,6 +20,27 @@ var _ = Describe("AABB", func() { }) }) + Describe("EmptyAABB", func() { + It("is empty", func() { + Expect(shape2d.EmptyAABB().IsEmpty()).To(BeTrue()) + }) + + It("expands to exactly enclose a single point when grown with min/max", func() { + aabb := shape2d.EmptyAABB() + point := dprec.NewVec2(3.0, -2.0) + aabb.MinX = min(aabb.MinX, point.X) + aabb.MinY = min(aabb.MinY, point.Y) + aabb.MaxX = max(aabb.MaxX, point.X) + aabb.MaxY = max(aabb.MaxY, point.Y) + + Expect(aabb.MinX).To(BeNumerically("~", 3.0, 1e-6)) + Expect(aabb.MinY).To(BeNumerically("~", -2.0, 1e-6)) + Expect(aabb.MaxX).To(BeNumerically("~", 3.0, 1e-6)) + Expect(aabb.MaxY).To(BeNumerically("~", -2.0, 1e-6)) + Expect(aabb.IsEmpty()).To(BeFalse()) + }) + }) + Describe("AABBFromCircle", func() { It("encloses the circle tightly", func() { circle := shape2d.NewCircle(dprec.NewVec2(3.0, 4.0), 2.0) diff --git a/core/spatial/shape2d/mesh.go b/core/spatial/shape2d/mesh.go index b0769c10..be347168 100644 --- a/core/spatial/shape2d/mesh.go +++ b/core/spatial/shape2d/mesh.go @@ -67,3 +67,26 @@ func (m Mesh) BoundingCircle() Circle { Radius: radius, } } + +// BoundingAABB returns the smallest [AABB] that fully encompasses the mesh. +// +// An empty mesh yields an empty [AABB] (see [EmptyAABB]). +func (m Mesh) BoundingAABB() AABB { + result := EmptyAABB() + + for _, edge := range m.Edges { + result.MinX = min(result.MinX, edge.A.X) + result.MinX = min(result.MinX, edge.B.X) + + result.MinY = min(result.MinY, edge.A.Y) + result.MinY = min(result.MinY, edge.B.Y) + + result.MaxX = max(result.MaxX, edge.A.X) + result.MaxX = max(result.MaxX, edge.B.X) + + result.MaxY = max(result.MaxY, edge.A.Y) + result.MaxY = max(result.MaxY, edge.B.Y) + } + + return result +} diff --git a/core/spatial/shape2d/mesh_test.go b/core/spatial/shape2d/mesh_test.go index dc10b2f9..effb1fc1 100644 --- a/core/spatial/shape2d/mesh_test.go +++ b/core/spatial/shape2d/mesh_test.go @@ -99,4 +99,46 @@ var _ = Describe("Mesh", func() { Expect(bc.Radius).To(Equal(0.0)) }) }) + + Describe("BoundingAABB", func() { + It("encloses all edge endpoints tightly", func() { + aabb := mesh.BoundingAABB() + Expect(aabb.MinX).To(BeNumerically("~", 0.0, 1e-6)) + Expect(aabb.MinY).To(BeNumerically("~", 0.0, 1e-6)) + Expect(aabb.MaxX).To(BeNumerically("~", 2.0, 1e-6)) + Expect(aabb.MaxY).To(BeNumerically("~", 2.0, 1e-6)) + }) + + It("contains every endpoint of every edge", func() { + aabb := mesh.BoundingAABB() + for _, edge := range mesh.Edges { + Expect(aabb.MinX).To(BeNumerically("<=", edge.A.X)) + Expect(aabb.MinX).To(BeNumerically("<=", edge.B.X)) + Expect(aabb.MinY).To(BeNumerically("<=", edge.A.Y)) + Expect(aabb.MinY).To(BeNumerically("<=", edge.B.Y)) + Expect(aabb.MaxX).To(BeNumerically(">=", edge.A.X)) + Expect(aabb.MaxX).To(BeNumerically(">=", edge.B.X)) + Expect(aabb.MaxY).To(BeNumerically(">=", edge.A.Y)) + Expect(aabb.MaxY).To(BeNumerically(">=", edge.B.Y)) + } + }) + + It("keeps the Y axis independent of X", func() { + // Regression check: an edge whose endpoints vary along Y but are + // constant along X must still report a tight Y bound instead of + // collapsing to the X extent. + single := shape2d.NewMesh([]shape2d.Edge{ + shape2d.NewEdge(dprec.NewVec2(5.0, -2.0), dprec.NewVec2(5.0, 4.0)), + }) + aabb := single.BoundingAABB() + Expect(aabb.MinX).To(BeNumerically("~", 5.0, 1e-6)) + Expect(aabb.MaxX).To(BeNumerically("~", 5.0, 1e-6)) + Expect(aabb.MinY).To(BeNumerically("~", -2.0, 1e-6)) + Expect(aabb.MaxY).To(BeNumerically("~", 4.0, 1e-6)) + }) + + It("returns an empty AABB for an empty mesh", func() { + Expect(shape2d.Mesh{}.BoundingAABB().IsEmpty()).To(BeTrue()) + }) + }) }) diff --git a/core/spatial/shape3d/aabb.go b/core/spatial/shape3d/aabb.go index 1146e464..6e27ffa7 100644 --- a/core/spatial/shape3d/aabb.go +++ b/core/spatial/shape3d/aabb.go @@ -1,6 +1,10 @@ package shape3d -import "github.com/mokiat/gomath/dprec" +import ( + "math" + + "github.com/mokiat/gomath/dprec" +) // AABB represents an axis-aligned bounding box in 3D space. type AABB struct { @@ -30,6 +34,20 @@ func NewAABB(minX, minY, minZ, maxX, maxY, maxZ float64) AABB { } } +// EmptyAABB returns an [AABB] that represents an empty volume, suitable as +// the starting point for incrementally growing a bounding box (e.g. via +// repeated min/max updates as points are encountered). +func EmptyAABB() AABB { + return AABB{ + MinX: math.MaxFloat64, + MinY: math.MaxFloat64, + MinZ: math.MaxFloat64, + MaxX: -math.MaxFloat64, + MaxY: -math.MaxFloat64, + MaxZ: -math.MaxFloat64, + } +} + // AABBFromSphere returns the smallest [AABB] that fully encompasses the // given sphere. func AABBFromSphere(sphere Sphere) AABB { diff --git a/core/spatial/shape3d/aabb_test.go b/core/spatial/shape3d/aabb_test.go index 6747ba30..c6236b36 100644 --- a/core/spatial/shape3d/aabb_test.go +++ b/core/spatial/shape3d/aabb_test.go @@ -22,6 +22,31 @@ var _ = Describe("AABB", func() { }) }) + Describe("EmptyAABB", func() { + It("is empty", func() { + Expect(shape3d.EmptyAABB().IsEmpty()).To(BeTrue()) + }) + + It("expands to exactly enclose a single point when grown with min/max", func() { + aabb := shape3d.EmptyAABB() + point := dprec.NewVec3(3.0, -2.0, 5.0) + aabb.MinX = min(aabb.MinX, point.X) + aabb.MinY = min(aabb.MinY, point.Y) + aabb.MinZ = min(aabb.MinZ, point.Z) + aabb.MaxX = max(aabb.MaxX, point.X) + aabb.MaxY = max(aabb.MaxY, point.Y) + aabb.MaxZ = max(aabb.MaxZ, point.Z) + + Expect(aabb.MinX).To(BeNumerically("~", 3.0, 1e-6)) + Expect(aabb.MinY).To(BeNumerically("~", -2.0, 1e-6)) + Expect(aabb.MinZ).To(BeNumerically("~", 5.0, 1e-6)) + Expect(aabb.MaxX).To(BeNumerically("~", 3.0, 1e-6)) + Expect(aabb.MaxY).To(BeNumerically("~", -2.0, 1e-6)) + Expect(aabb.MaxZ).To(BeNumerically("~", 5.0, 1e-6)) + Expect(aabb.IsEmpty()).To(BeFalse()) + }) + }) + Describe("AABBFromSphere", func() { It("encloses the sphere tightly", func() { sphere := shape3d.NewSphere(dprec.NewVec3(3.0, 4.0, 5.0), 2.0) diff --git a/core/spatial/shape3d/mesh.go b/core/spatial/shape3d/mesh.go index d85b0974..e2973006 100644 --- a/core/spatial/shape3d/mesh.go +++ b/core/spatial/shape3d/mesh.go @@ -69,3 +69,38 @@ func (m Mesh) BoundingSphere() Sphere { Radius: radius, } } + +// BoundingAABB returns the smallest [AABB] that fully encompasses the mesh. +// +// An empty mesh yields an empty [AABB] (see [EmptyAABB]). +func (m Mesh) BoundingAABB() AABB { + result := EmptyAABB() + + for _, triangle := range m.Triangles { + result.MinX = min(result.MinX, triangle.A.X) + result.MinX = min(result.MinX, triangle.B.X) + result.MinX = min(result.MinX, triangle.C.X) + + result.MinY = min(result.MinY, triangle.A.Y) + result.MinY = min(result.MinY, triangle.B.Y) + result.MinY = min(result.MinY, triangle.C.Y) + + result.MinZ = min(result.MinZ, triangle.A.Z) + result.MinZ = min(result.MinZ, triangle.B.Z) + result.MinZ = min(result.MinZ, triangle.C.Z) + + result.MaxX = max(result.MaxX, triangle.A.X) + result.MaxX = max(result.MaxX, triangle.B.X) + result.MaxX = max(result.MaxX, triangle.C.X) + + result.MaxY = max(result.MaxY, triangle.A.Y) + result.MaxY = max(result.MaxY, triangle.B.Y) + result.MaxY = max(result.MaxY, triangle.C.Y) + + result.MaxZ = max(result.MaxZ, triangle.A.Z) + result.MaxZ = max(result.MaxZ, triangle.B.Z) + result.MaxZ = max(result.MaxZ, triangle.C.Z) + } + + return result +} diff --git a/core/spatial/shape3d/mesh_test.go b/core/spatial/shape3d/mesh_test.go index 8c47e8ba..e46d473f 100644 --- a/core/spatial/shape3d/mesh_test.go +++ b/core/spatial/shape3d/mesh_test.go @@ -101,4 +101,66 @@ var _ = Describe("Mesh", func() { Expect(bs.Radius).To(Equal(0.0)) }) }) + + Describe("BoundingAABB", func() { + It("encloses all triangle vertices tightly", func() { + aabb := mesh.BoundingAABB() + Expect(aabb.MinX).To(BeNumerically("~", 0.0, 1e-6)) + Expect(aabb.MinY).To(BeNumerically("~", 0.0, 1e-6)) + Expect(aabb.MinZ).To(BeNumerically("~", 0.0, 1e-6)) + Expect(aabb.MaxX).To(BeNumerically("~", 6.0, 1e-6)) + Expect(aabb.MaxY).To(BeNumerically("~", 6.0, 1e-6)) + Expect(aabb.MaxZ).To(BeNumerically("~", 6.0, 1e-6)) + }) + + It("contains every vertex of every triangle", func() { + aabb := mesh.BoundingAABB() + for _, triangle := range mesh.Triangles { + Expect(aabb.MinX).To(BeNumerically("<=", triangle.A.X)) + Expect(aabb.MinX).To(BeNumerically("<=", triangle.B.X)) + Expect(aabb.MinX).To(BeNumerically("<=", triangle.C.X)) + Expect(aabb.MinY).To(BeNumerically("<=", triangle.A.Y)) + Expect(aabb.MinY).To(BeNumerically("<=", triangle.B.Y)) + Expect(aabb.MinY).To(BeNumerically("<=", triangle.C.Y)) + Expect(aabb.MinZ).To(BeNumerically("<=", triangle.A.Z)) + Expect(aabb.MinZ).To(BeNumerically("<=", triangle.B.Z)) + Expect(aabb.MinZ).To(BeNumerically("<=", triangle.C.Z)) + Expect(aabb.MaxX).To(BeNumerically(">=", triangle.A.X)) + Expect(aabb.MaxX).To(BeNumerically(">=", triangle.B.X)) + Expect(aabb.MaxX).To(BeNumerically(">=", triangle.C.X)) + Expect(aabb.MaxY).To(BeNumerically(">=", triangle.A.Y)) + Expect(aabb.MaxY).To(BeNumerically(">=", triangle.B.Y)) + Expect(aabb.MaxY).To(BeNumerically(">=", triangle.C.Y)) + Expect(aabb.MaxZ).To(BeNumerically(">=", triangle.A.Z)) + Expect(aabb.MaxZ).To(BeNumerically(">=", triangle.B.Z)) + Expect(aabb.MaxZ).To(BeNumerically(">=", triangle.C.Z)) + } + }) + + It("keeps the Y and Z axes independent of X", func() { + // Regression check: a mesh whose vertices vary along Y and Z but + // are constant along X must still report tight Y/Z bounds instead + // of collapsing to the X extent. + single := shape3d.Mesh{ + Triangles: []shape3d.Triangle{ + { + A: dprec.NewVec3(5.0, -2.0, -3.0), + B: dprec.NewVec3(5.0, 4.0, 1.0), + C: dprec.NewVec3(5.0, 0.0, 7.0), + }, + }, + } + aabb := single.BoundingAABB() + Expect(aabb.MinX).To(BeNumerically("~", 5.0, 1e-6)) + Expect(aabb.MaxX).To(BeNumerically("~", 5.0, 1e-6)) + Expect(aabb.MinY).To(BeNumerically("~", -2.0, 1e-6)) + Expect(aabb.MaxY).To(BeNumerically("~", 4.0, 1e-6)) + Expect(aabb.MinZ).To(BeNumerically("~", -3.0, 1e-6)) + Expect(aabb.MaxZ).To(BeNumerically("~", 7.0, 1e-6)) + }) + + It("returns an empty AABB for an empty mesh", func() { + Expect(shape3d.Mesh{}.BoundingAABB().IsEmpty()).To(BeTrue()) + }) + }) }) From 9037b94e16f01f0ba95420c9751336f1ff9d4747 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 9 Aug 2026 11:46:58 +0300 Subject: [PATCH 66/85] Use tighter fit for meshes in placement scenes --- core/spatial/placement2d/mesh.go | 21 +++++++++++---------- core/spatial/placement2d/scene.go | 13 ++++++++----- core/spatial/placement2d/shape.go | 4 ---- core/spatial/placement3d/mesh.go | 21 +++++++++++---------- core/spatial/placement3d/scene.go | 13 ++++++++----- core/spatial/placement3d/shape.go | 4 ---- 6 files changed, 38 insertions(+), 38 deletions(-) diff --git a/core/spatial/placement2d/mesh.go b/core/spatial/placement2d/mesh.go index c07d81ab..784b8208 100644 --- a/core/spatial/placement2d/mesh.go +++ b/core/spatial/placement2d/mesh.go @@ -33,6 +33,9 @@ type MeshInfo[M any] struct { UserData M // Mesh contains the mesh information. + // + // The mesh must have at least one edge. An empty mesh has no area to be + // placed in the scene and is considered invalid. Mesh shape2d.Mesh } @@ -47,23 +50,21 @@ func shapeMeshCanIntersect[S, M any](shape *shape[S], mesh *meshShape[M]) bool { return shape.canInteractWith(&mesh.filterRepresentation) } +// TODO: Consider using a different storage mechanism. For example a +// Quadtree or BVH structure. +// Alternatively experiment with placing each mesh edge in the existing +// mesh tree, through this will likely destroy the mesh tree performance. + type meshRepresentation struct { wsBCircle shape2d.Circle - - // TODO: Consider using a different storage mechanism. For example a - // Quadtree or BVH structure. - // Alternatively experiment with placing each mesh edge in the existing - // mesh tree, through this will likely destroy the mesh tree performance. - wsEdges []shape2d.Edge + wsAABB shape2d.AABB + wsEdges []shape2d.Edge } func newMeshRepresentation(mesh shape2d.Mesh) meshRepresentation { return meshRepresentation{ wsBCircle: mesh.BoundingCircle(), + wsAABB: mesh.BoundingAABB(), wsEdges: mesh.Edges, } } - -func (s *meshRepresentation) boundingCircle() shape2d.Circle { - return s.wsBCircle -} diff --git a/core/spatial/placement2d/scene.go b/core/spatial/placement2d/scene.go index aaccec69..ce691acb 100644 --- a/core/spatial/placement2d/scene.go +++ b/core/spatial/placement2d/scene.go @@ -145,8 +145,8 @@ func (s *Scene[O, S, M]) SetObjectTransform(objID ObjectID, transform shape2d.Tr s.eachObjectShape(object, func(_ int32, shape *shape[S]) { shape.update(transform) - bc := shape.boundingCircle() - s.shapeTree.Update(shape.spatialID, shape2d.AABBFromCircle(bc)) + area := shape2d.AABBFromCircle(shape.wsBCircle) + s.shapeTree.Update(shape.spatialID, area) }) } @@ -297,6 +297,9 @@ func (s *Scene[O, S, M]) RectangleIter(filter Filter) iter.Seq[shape2d.Rectangle // directly through the [MeshInfo.Position] and [MeshInfo.Rotation] fields and // is intended for static geometry that participates in intersection tests as a // collection of edges. +// +// The mesh specified through [MeshInfo.Mesh] must not be empty, otherwise this +// function panics. func (s *Scene[O, S, M]) CreateMesh(info MeshInfo[M]) MeshID { transform := shape2d.Transform{ Translation: info.Position.ValueOrDefault(dprec.ZeroVec2()), @@ -305,7 +308,7 @@ func (s *Scene[O, S, M]) CreateMesh(info MeshInfo[M]) MeshID { ), } representation := newMeshRepresentation(shape2d.TransformedMesh(info.Mesh, transform)) - area := shape2d.AABBFromCircle(representation.boundingCircle()) + area := representation.wsAABB index := s.allocateMesh() s.meshes[index] = meshShape[M]{ @@ -443,7 +446,7 @@ func (s *Scene[O, S, M]) CollectIntersections(yield ContactCallback) { continue } - queryAABB := shape2d.AABBFromCircle(srcShape.boundingCircle()) + queryAABB := shape2d.AABBFromCircle(srcShape.wsBCircle) s.shapeCandidates = s.shapeCandidates[:0] s.shapeTree.QueryAABB(queryAABB, func(tgtIndex int32) bool { @@ -512,7 +515,7 @@ func (s *Scene[O, S, M]) attachShape( index := s.allocateShape() representation.update(object.transform) - area := shape2d.AABBFromCircle(representation.boundingCircle()) + area := shape2d.AABBFromCircle(representation.wsBCircle) s.shapes[index] = shape[S]{ objectIndex: objectIndex, diff --git a/core/spatial/placement2d/shape.go b/core/spatial/placement2d/shape.go index 83ff37db..01576cc4 100644 --- a/core/spatial/placement2d/shape.go +++ b/core/spatial/placement2d/shape.go @@ -77,10 +77,6 @@ func (s *shapeRepresentation) update(parentTransform shape2d.Transform) { ) } -func (s *shapeRepresentation) boundingCircle() shape2d.Circle { - return s.wsBCircle -} - func (s *shapeRepresentation) gjkShape() gjk2d.Shape { return gjk2d.Shape{ Position: s.wsTransform.Translation, diff --git a/core/spatial/placement3d/mesh.go b/core/spatial/placement3d/mesh.go index e89538dc..6b48a5c6 100644 --- a/core/spatial/placement3d/mesh.go +++ b/core/spatial/placement3d/mesh.go @@ -33,6 +33,9 @@ type MeshInfo[M any] struct { UserData M // Mesh contains the mesh information. + // + // The mesh must have at least one triangle. An empty mesh has no area to + // be placed in the scene and is considered invalid. Mesh shape3d.Mesh } @@ -47,23 +50,21 @@ func shapeMeshCanIntersect[S, M any](shape *shape[S], mesh *meshShape[M]) bool { return shape.canInteractWith(&mesh.filterRepresentation) } -type meshRepresentation struct { - wsBSphere shape3d.Sphere +// TODO: Consider using a different storage mechanism. For example an +// Octree or BVH structure. +// Alternatively experiment with placing each mesh triangle in the existing +// mesh tree, through this will likely destroy the mesh tree performance. - // TODO: Consider using a different storage mechanism. For example an - // Octree or BVH structure. - // Alternatively experiment with placing each mesh triangle in the existing - // mesh tree, through this will likely destroy the mesh tree performance. +type meshRepresentation struct { + wsBSphere shape3d.Sphere + wsAABB shape3d.AABB wsTriangles []shape3d.Triangle } func newMeshRepresentation(mesh shape3d.Mesh) meshRepresentation { return meshRepresentation{ wsBSphere: mesh.BoundingSphere(), + wsAABB: mesh.BoundingAABB(), wsTriangles: mesh.Triangles, } } - -func (s *meshRepresentation) boundingSphere() shape3d.Sphere { - return s.wsBSphere -} diff --git a/core/spatial/placement3d/scene.go b/core/spatial/placement3d/scene.go index 362034e5..65ef5265 100644 --- a/core/spatial/placement3d/scene.go +++ b/core/spatial/placement3d/scene.go @@ -145,8 +145,8 @@ func (s *Scene[O, S, M]) SetObjectTransform(objID ObjectID, transform shape3d.Tr s.eachObjectShape(object, func(_ int32, shape *shape[S]) { shape.update(transform) - bs := shape.boundingSphere() - s.shapeTree.Update(shape.spatialID, shape3d.AABBFromSphere(bs)) + area := shape3d.AABBFromSphere(shape.wsBSphere) + s.shapeTree.Update(shape.spatialID, area) }) } @@ -302,6 +302,9 @@ func (s *Scene[O, S, M]) BoxIter(filter Filter) iter.Seq[shape3d.Box] { // directly through the [MeshInfo.Position] and [MeshInfo.Rotation] fields and // is intended for static geometry that participates in intersection tests as a // collection of triangles. +// +// The mesh specified through [MeshInfo.Mesh] must not be empty, otherwise this +// function panics. func (s *Scene[O, S, M]) CreateMesh(info MeshInfo[M]) MeshID { transform := shape3d.Transform{ Translation: info.Position.ValueOrDefault(dprec.ZeroVec3()), @@ -310,7 +313,7 @@ func (s *Scene[O, S, M]) CreateMesh(info MeshInfo[M]) MeshID { ), } representation := newMeshRepresentation(shape3d.TransformedMesh(info.Mesh, transform)) - area := shape3d.AABBFromSphere(representation.boundingSphere()) + area := representation.wsAABB index := s.allocateMesh() s.meshes[index] = meshShape[M]{ @@ -448,7 +451,7 @@ func (s *Scene[O, S, M]) CollectIntersections(yield ContactCallback) { continue } - queryAABB := shape3d.AABBFromSphere(srcShape.boundingSphere()) + queryAABB := shape3d.AABBFromSphere(srcShape.wsBSphere) s.shapeCandidates = s.shapeCandidates[:0] s.shapeTree.QueryAABB(queryAABB, func(tgtIndex int32) bool { @@ -517,7 +520,7 @@ func (s *Scene[O, S, M]) attachShape( index := s.allocateShape() representation.update(object.transform) - area := shape3d.AABBFromSphere(representation.boundingSphere()) + area := shape3d.AABBFromSphere(representation.wsBSphere) s.shapes[index] = shape[S]{ objectIndex: objectIndex, diff --git a/core/spatial/placement3d/shape.go b/core/spatial/placement3d/shape.go index 535f2eb8..61b77e97 100644 --- a/core/spatial/placement3d/shape.go +++ b/core/spatial/placement3d/shape.go @@ -77,10 +77,6 @@ func (s *shapeRepresentation) update(parentTransform shape3d.Transform) { ) } -func (s *shapeRepresentation) boundingSphere() shape3d.Sphere { - return s.wsBSphere -} - func (s *shapeRepresentation) gjkShape() gjk3d.Shape { return gjk3d.Shape{ Position: s.wsTransform.Translation, From 813e1086790b7fb8195ad6887e082cb3ab3469d8 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 9 Aug 2026 16:22:57 +0300 Subject: [PATCH 67/85] Rework placement3d API --- core/spatial/placement3d/contact.go | 153 --- core/spatial/placement3d/contact_object.go | 156 +++ core/spatial/placement3d/contact_terrain.go | 160 +++ core/spatial/placement3d/doc.go | 34 +- core/spatial/placement3d/filter.go | 55 +- core/spatial/placement3d/mesh.go | 70 -- core/spatial/placement3d/object.go | 9 +- .../placement3d/{shape.go => object_shape.go} | 47 +- core/spatial/placement3d/scene.go | 909 +++++++++++------- core/spatial/placement3d/scene_test.go | 677 +++++++++---- core/spatial/placement3d/terrain.go | 23 + core/spatial/placement3d/terrain_shape.go | 69 ++ 12 files changed, 1546 insertions(+), 816 deletions(-) delete mode 100644 core/spatial/placement3d/contact.go create mode 100644 core/spatial/placement3d/contact_object.go create mode 100644 core/spatial/placement3d/contact_terrain.go delete mode 100644 core/spatial/placement3d/mesh.go rename core/spatial/placement3d/{shape.go => object_shape.go} (64%) create mode 100644 core/spatial/placement3d/terrain.go create mode 100644 core/spatial/placement3d/terrain_shape.go diff --git a/core/spatial/placement3d/contact.go b/core/spatial/placement3d/contact.go deleted file mode 100644 index 47ba7c65..00000000 --- a/core/spatial/placement3d/contact.go +++ /dev/null @@ -1,153 +0,0 @@ -package placement3d - -import "github.com/mokiat/lacking/core/spatial/shape3d" - -// Contact describes the intersection of a source shape with a target shape. -// -// Its fields are expressed relative to the target shape. The equivalent values -// for the source shape can be derived via [shape3d.Contact.EvalSourcePoint] and -// [shape3d.Contact.EvalSourceNormal]. -type Contact struct { - - // SourceShapeID contains the ID of the shape from the first involved object. - // - // This ID is equal to [InvalidShapeID] if the check was not performed with - // a scene object. - SourceShapeID ShapeID - - // TargetShapeID contains the ID of the shape from the second involved object. - // - // This ID is equal to [InvalidShapeID] when the target of the intersection - // was a mesh, in which case [Contact.TargetMeshID] identifies it instead. - TargetShapeID ShapeID - - // TargetMeshID contains the ID of the mesh that was intersected. - // - // This ID is equal to [InvalidMeshID] when the target of the intersection - // was a shape rather than a mesh. - TargetMeshID MeshID - - // Contact holds the underlying raw shape intersection. - shape3d.Contact -} - -// ContactCallback is invoked for each [Contact] discovered while testing shapes -// for intersection. -type ContactCallback func(contact Contact) - -// LastContact is a contact sink that retains the most recently added [Contact]. -// -// Its AddContact method satisfies [ContactCallback] and can be passed directly to -// intersection routines. -type LastContact struct { - contact Contact - hasContact bool -} - -// Reset clears any retained contact. -func (c *LastContact) Reset() { - c.hasContact = false -} - -// AddContact retains the given contact, replacing any previously retained one. -func (c *LastContact) AddContact(contact Contact) { - c.contact = contact - c.hasContact = true -} - -// Contact returns the retained contact and whether one was added since the last -// Reset. -func (c *LastContact) Contact() (Contact, bool) { - return c.contact, c.hasContact -} - -// DeepestContact is a contact sink that retains the added [Contact] with the -// greatest Depth. -// -// Its AddContact method satisfies [ContactCallback] and can be passed directly to -// intersection routines. -type DeepestContact struct { - contact Contact - hasContact bool -} - -// Reset clears any retained contact. -func (c *DeepestContact) Reset() { - c.hasContact = false -} - -// AddContact retains the given contact if it is deeper than any previously -// retained one. -func (c *DeepestContact) AddContact(contact Contact) { - if !c.hasContact || contact.Depth > c.contact.Depth { - c.contact = contact - c.hasContact = true - } -} - -// Contact returns the deepest retained contact and whether one was added since -// the last Reset. -func (c *DeepestContact) Contact() (Contact, bool) { - return c.contact, c.hasContact -} - -// ShallowestContact is a contact sink that retains the added [Contact] with the -// smallest Depth. -// -// Its AddContact method satisfies [ContactCallback] and can be passed directly to -// intersection routines. -type ShallowestContact struct { - contact Contact - hasContact bool -} - -// Reset clears any retained contact. -func (c *ShallowestContact) Reset() { - c.hasContact = false -} - -// AddContact retains the given contact if it is shallower than any previously -// retained one. -func (c *ShallowestContact) AddContact(contact Contact) { - if !c.hasContact || contact.Depth < c.contact.Depth { - c.contact = contact - c.hasContact = true - } -} - -// Contact returns the shallowest retained contact and whether one was added -// since the last Reset. -func (c *ShallowestContact) Contact() (Contact, bool) { - return c.contact, c.hasContact -} - -// ContactList is a contact sink that retains every added [Contact] in the order -// it was added. -// -// Its AddContact method satisfies [ContactCallback] and can be passed directly to -// intersection routines. As it is itself a slice, the retained contacts can be -// ranged over directly. -// -// Use make(ContactList, 0, n) to pre-size it and avoid reallocations as -// contacts are added. With a constant n that does not escape, the compiler can -// keep the backing array on the stack. -type ContactList []Contact - -// Reset clears the retained contacts while preserving the underlying capacity -// so it can be reused without reallocating. -func (l *ContactList) Reset() { - *l = (*l)[:0] -} - -// AddContact appends the given contact to the list. -func (l *ContactList) AddContact(contact Contact) { - *l = append(*l, contact) -} - -// Contacts returns the retained contacts in the order they were added. -// -// The result aliases the internal storage and remains valid until the next -// AddContact or Reset call. -func (l ContactList) Contacts() []Contact { - return l -} diff --git a/core/spatial/placement3d/contact_object.go b/core/spatial/placement3d/contact_object.go new file mode 100644 index 00000000..98e28753 --- /dev/null +++ b/core/spatial/placement3d/contact_object.go @@ -0,0 +1,156 @@ +package placement3d + +import "github.com/mokiat/lacking/core/spatial/shape3d" + +// ObjectContact describes the intersection of a source shape with an object +// shape. +// +// Its fields are expressed relative to the target shape. The equivalent values +// for the source shape can be derived via [shape3d.Contact.EvalSourcePoint] and +// [shape3d.Contact.EvalSourceNormal]. +type ObjectContact struct { + + // SourceObjectID contains the ID of the object that owns the source shape. + // + // This ID is equal to [NilObjectID] when the intersection was produced by a + // query primitive rather than by a shape in the scene. + SourceObjectID ObjectID + + // SourceShapeID contains the ID of the shape that acted as the source of + // the intersection. + // + // This ID is equal to [NilObjectShapeID] when the intersection was produced + // by a query primitive rather than by a shape in the scene. + SourceShapeID ObjectShapeID + + // TargetObjectID contains the ID of the object that owns the target shape. + TargetObjectID ObjectID + + // TargetShapeID contains the ID of the shape that was intersected. + TargetShapeID ObjectShapeID + + // Contact holds the underlying raw shape intersection. + shape3d.Contact +} + +// ObjectContactCallback is invoked for each [ObjectContact] discovered while +// testing shapes for intersection. +type ObjectContactCallback func(contact ObjectContact) + +// DeepestObjectContact is a contact sink that retains the added +// [ObjectContact] with the greatest Depth. +// +// Its AddContact method satisfies [ObjectContactCallback] and can be passed +// directly to intersection routines. +type DeepestObjectContact struct { + contact ObjectContact + hasContact bool +} + +// Reset clears any retained contact. +func (c *DeepestObjectContact) Reset() { + c.hasContact = false +} + +// AddContact retains the given contact if it is deeper than any previously +// retained one. +func (c *DeepestObjectContact) AddContact(contact ObjectContact) { + if !c.hasContact || contact.Depth > c.contact.Depth { + c.contact = contact + c.hasContact = true + } +} + +// Contact returns the deepest retained contact and whether one was added since +// the last Reset. +func (c *DeepestObjectContact) Contact() (ObjectContact, bool) { + return c.contact, c.hasContact +} + +// ShallowestObjectContact is a contact sink that retains the added +// [ObjectContact] with the smallest Depth. +// +// Its AddContact method satisfies [ObjectContactCallback] and can be passed +// directly to intersection routines. +type ShallowestObjectContact struct { + contact ObjectContact + hasContact bool +} + +// Reset clears any retained contact. +func (c *ShallowestObjectContact) Reset() { + c.hasContact = false +} + +// AddContact retains the given contact if it is shallower than any previously +// retained one. +func (c *ShallowestObjectContact) AddContact(contact ObjectContact) { + if !c.hasContact || contact.Depth < c.contact.Depth { + c.contact = contact + c.hasContact = true + } +} + +// Contact returns the shallowest retained contact and whether one was added +// since the last Reset. +func (c *ShallowestObjectContact) Contact() (ObjectContact, bool) { + return c.contact, c.hasContact +} + +// ObjectContactList is a contact sink that retains every added [ObjectContact] +// in the order it was added. +// +// Its AddContact method satisfies [ObjectContactCallback] and can be passed +// directly to intersection routines. As it is itself a slice, the retained +// contacts can be ranged over directly. +// +// Use make(ObjectContactList, 0, n) to pre-size it and avoid reallocations as +// contacts are added. With a constant n that does not escape, the compiler can +// keep the backing array on the stack. +type ObjectContactList []ObjectContact + +// Reset clears the retained contacts while preserving the underlying capacity +// so it can be reused without reallocating. +func (l *ObjectContactList) Reset() { + *l = (*l)[:0] +} + +// AddContact appends the given contact to the list. +func (l *ObjectContactList) AddContact(contact ObjectContact) { + *l = append(*l, contact) +} + +// Contacts returns the retained contacts in the order they were added. +// +// The result aliases the internal storage and remains valid until the next +// AddContact or Reset call. +func (l ObjectContactList) Contacts() []ObjectContact { + return l +} + +// LastObjectContact is a contact sink that retains the most recently added +// [ObjectContact]. +// +// Its AddContact method satisfies [ObjectContactCallback] and can be passed +// directly to intersection routines. +type LastObjectContact struct { + contact ObjectContact + hasContact bool +} + +// Reset clears any retained contact. +func (c *LastObjectContact) Reset() { + c.hasContact = false +} + +// AddContact retains the given contact, replacing any previously retained one. +func (c *LastObjectContact) AddContact(contact ObjectContact) { + c.contact = contact + c.hasContact = true +} + +// Contact returns the retained contact and whether one was added since the +// last Reset. +func (c *LastObjectContact) Contact() (ObjectContact, bool) { + return c.contact, c.hasContact +} diff --git a/core/spatial/placement3d/contact_terrain.go b/core/spatial/placement3d/contact_terrain.go new file mode 100644 index 00000000..07031b3b --- /dev/null +++ b/core/spatial/placement3d/contact_terrain.go @@ -0,0 +1,160 @@ +package placement3d + +import "github.com/mokiat/lacking/core/spatial/shape3d" + +// TerrainContact describes the intersection of a source shape with a terrain +// shape. +// +// Its fields are expressed relative to the target shape. The equivalent values +// for the source shape can be derived via [shape3d.Contact.EvalSourcePoint] and +// [shape3d.Contact.EvalSourceNormal]. +type TerrainContact struct { + + // SourceObjectID contains the ID of the object that owns the source shape. + // + // This ID is equal to [NilObjectID] when the intersection was produced by a + // query primitive rather than by a shape in the scene. + SourceObjectID ObjectID + + // SourceShapeID contains the ID of the shape that acted as the source of + // the intersection. + // + // This ID is equal to [NilObjectShapeID] when the intersection was produced + // by a query primitive rather than by a shape in the scene. + // + // The source of a terrain contact is always an object shape, since terrain + // shapes are never tested against one another. + SourceShapeID ObjectShapeID + + // TargetTerrainID contains the ID of the terrain that owns the target + // shape. + TargetTerrainID TerrainID + + // TargetShapeID contains the ID of the shape that was intersected. + TargetShapeID TerrainShapeID + + // Contact holds the underlying raw shape intersection. + shape3d.Contact +} + +// TerrainContactCallback is invoked for each [TerrainContact] discovered while +// testing shapes for intersection. +type TerrainContactCallback func(contact TerrainContact) + +// DeepestTerrainContact is a contact sink that retains the added +// [TerrainContact] with the greatest Depth. +// +// Its AddContact method satisfies [TerrainContactCallback] and can be passed +// directly to intersection routines. +type DeepestTerrainContact struct { + contact TerrainContact + hasContact bool +} + +// Reset clears any retained contact. +func (c *DeepestTerrainContact) Reset() { + c.hasContact = false +} + +// AddContact retains the given contact if it is deeper than any previously +// retained one. +func (c *DeepestTerrainContact) AddContact(contact TerrainContact) { + if !c.hasContact || contact.Depth > c.contact.Depth { + c.contact = contact + c.hasContact = true + } +} + +// Contact returns the deepest retained contact and whether one was added since +// the last Reset. +func (c *DeepestTerrainContact) Contact() (TerrainContact, bool) { + return c.contact, c.hasContact +} + +// ShallowestTerrainContact is a contact sink that retains the added +// [TerrainContact] with the smallest Depth. +// +// Its AddContact method satisfies [TerrainContactCallback] and can be passed +// directly to intersection routines. +type ShallowestTerrainContact struct { + contact TerrainContact + hasContact bool +} + +// Reset clears any retained contact. +func (c *ShallowestTerrainContact) Reset() { + c.hasContact = false +} + +// AddContact retains the given contact if it is shallower than any previously +// retained one. +func (c *ShallowestTerrainContact) AddContact(contact TerrainContact) { + if !c.hasContact || contact.Depth < c.contact.Depth { + c.contact = contact + c.hasContact = true + } +} + +// Contact returns the shallowest retained contact and whether one was added +// since the last Reset. +func (c *ShallowestTerrainContact) Contact() (TerrainContact, bool) { + return c.contact, c.hasContact +} + +// TerrainContactList is a contact sink that retains every added +// [TerrainContact] in the order it was added. +// +// Its AddContact method satisfies [TerrainContactCallback] and can be passed +// directly to intersection routines. As it is itself a slice, the retained +// contacts can be ranged over directly. +// +// Use make(TerrainContactList, 0, n) to pre-size it and avoid reallocations as +// contacts are added. With a constant n that does not escape, the compiler can +// keep the backing array on the stack. +type TerrainContactList []TerrainContact + +// Reset clears the retained contacts while preserving the underlying capacity +// so it can be reused without reallocating. +func (l *TerrainContactList) Reset() { + *l = (*l)[:0] +} + +// AddContact appends the given contact to the list. +func (l *TerrainContactList) AddContact(contact TerrainContact) { + *l = append(*l, contact) +} + +// Contacts returns the retained contacts in the order they were added. +// +// The result aliases the internal storage and remains valid until the next +// AddContact or Reset call. +func (l TerrainContactList) Contacts() []TerrainContact { + return l +} + +// LastTerrainContact is a contact sink that retains the most recently added +// [TerrainContact]. +// +// Its AddContact method satisfies [TerrainContactCallback] and can be passed +// directly to intersection routines. +type LastTerrainContact struct { + contact TerrainContact + hasContact bool +} + +// Reset clears any retained contact. +func (c *LastTerrainContact) Reset() { + c.hasContact = false +} + +// AddContact retains the given contact, replacing any previously retained one. +func (c *LastTerrainContact) AddContact(contact TerrainContact) { + c.contact = contact + c.hasContact = true +} + +// Contact returns the retained contact and whether one was added since the +// last Reset. +func (c *LastTerrainContact) Contact() (TerrainContact, bool) { + return c.contact, c.hasContact +} diff --git a/core/spatial/placement3d/doc.go b/core/spatial/placement3d/doc.go index 980ebd09..faa50bac 100644 --- a/core/spatial/placement3d/doc.go +++ b/core/spatial/placement3d/doc.go @@ -1,12 +1,28 @@ -// Package placement3d provides a 3D scene in which objects, built from convex -// shapes, and static meshes can be placed and tested for intersection. +// Package placement3d provides a 3D scene in which objects and terrains can be +// placed and tested for intersection. // -// Objects are dynamic entities that own one or more convex shapes (spheres and -// boxes). Meshes are static entities made of triangles. Both are indexed in -// separate octrees for efficient broad-phase queries, and narrow-phase -// intersection is resolved via GJK/EPA (see [github.com/mokiat/lacking/core/spatial/gjk3d]). +// An object is a movable entity that owns one or more convex shapes (see +// [Scene.AttachSphere] and [Scene.AttachBox]). Moving the object through +// [Scene.SetObjectTransform] moves all of its shapes along with it. // -// Intersections are reported as [Contact] values through a [ContactCallback]. -// A number of contact sinks (for example [DeepestContact] and [ContactList]) -// are provided for common accumulation strategies. +// A terrain is an immovable entity that owns one or more concave shapes (see +// [Scene.AttachMesh]). Terrain shapes are specified directly in world space and +// cannot be relocated once attached. +// +// Object shapes and terrain shapes are indexed in separate octrees for +// efficient broad-phase queries. Narrow-phase intersection is resolved via +// GJK/EPA (see [github.com/mokiat/lacking/core/spatial/gjk3d]), with concave +// shapes being decomposed into convex pieces beforehand. +// +// Intersections come in two flavors, which are reported through separate +// methods so that callers never need to branch on the kind of the target. An +// intersection with an object shape is reported as an [ObjectContact] through +// an [ObjectContactCallback], whereas an intersection with a terrain shape is +// reported as a [TerrainContact] through a [TerrainContactCallback]. A number +// of contact sinks (for example [DeepestObjectContact] and +// [TerrainContactList]) are provided for common accumulation strategies. +// +// Since terrains cannot move, terrain shapes are never tested against one +// another. The source of a [TerrainContact] is always either an object shape +// or a query primitive. package placement3d diff --git a/core/spatial/placement3d/filter.go b/core/spatial/placement3d/filter.go index eabf07ca..ca08d2ba 100644 --- a/core/spatial/placement3d/filter.go +++ b/core/spatial/placement3d/filter.go @@ -2,38 +2,40 @@ package placement3d import "github.com/mokiat/gog/opt" -// Filter represents a set of criteria to filter 3D shapes in a scene. -type Filter struct { - - // Mask is a bitmask used to filter shapes based on their assigned layers. - Mask opt.T[uint32] - - // SkipDynamic indicates whether dynamic shapes should be excluded from the - // results. - SkipDynamic bool +// Mask is a bitmask over the layers that a shape can occupy. Queries use it to +// narrow down the shapes that they consider. +// +// A shape is considered by a query when at least one bit is set in both the +// mask of the query and the [FilterInfo.SourceMask] of the shape. Note that +// this means that the zero value matches no shape at all. Use [FullMask] to +// consider every shape in the scene. +type Mask = uint32 - // SkipStatic indicates whether static shapes should be excluded from the - // results. - SkipStatic bool -} +// FullMask is a [Mask] with all layer bits set. A query that uses it considers +// every shape in the scene, regardless of the layers that the shape occupies. +const FullMask Mask = 0xFFFFFFFF -// FilterInfo holds the collision-filtering metadata common to every entity -// that can be placed in a scene, whether a shape (see [SphereInfo] and -// [BoxInfo]) or a mesh (see [MeshInfo]). +// FilterInfo holds the collision-filtering metadata common to every shape that +// can be placed in a scene, whether an object shape (see [SphereInfo] and +// [BoxInfo]) or a terrain shape (see [MeshInfo]). // -// Its fields determine which entities are tested against one another during +// Its fields determine which shapes are tested against one another during // intersection queries. type FilterInfo struct { // RejectGroup becomes active if a value larger than zero is specified. - // Entities that share the same reject group are not checked for + // Shapes that share the same reject group are not checked for // intersection. RejectGroup uint32 - // SourceMask specifies the layers in which this entity is positioned. + // SourceMask specifies the layers in which this shape is positioned. + // + // Defaults to the first layer only. SourceMask opt.T[uint32] - // TargetMask specifies the layers with which this entity can intersect. + // TargetMask specifies the layers with which this shape can intersect. + // + // Defaults to the first layer only. TargetMask opt.T[uint32] } @@ -51,15 +53,14 @@ func newFilterRepresentation(info FilterInfo) filterRepresentation { } } -func (s *filterRepresentation) matchesFilter(filter Filter) bool { - if mask, ok := filter.Mask.Unwrap(); ok { - if (s.sourceMask & mask) == 0 { - return false - } - } - return true +// satisfiesMask reports whether this shape occupies at least one of the layers +// covered by the specified query mask. +func (s *filterRepresentation) satisfiesMask(mask Mask) bool { + return (s.sourceMask & mask) != 0 } +// canInteractWith reports whether this shape and the specified one are allowed +// to be checked for intersection. func (s *filterRepresentation) canInteractWith(other *filterRepresentation) bool { if s.rejectGroup != 0 && (s.rejectGroup == other.rejectGroup) { return false diff --git a/core/spatial/placement3d/mesh.go b/core/spatial/placement3d/mesh.go deleted file mode 100644 index 6b48a5c6..00000000 --- a/core/spatial/placement3d/mesh.go +++ /dev/null @@ -1,70 +0,0 @@ -package placement3d - -import ( - "github.com/mokiat/gog/opt" - "github.com/mokiat/gomath/dprec" - "github.com/mokiat/lacking/core/spatial/query3d" - "github.com/mokiat/lacking/core/spatial/shape3d" -) - -// InvalidMeshID indicates a mesh that can never be part of the scene. -const InvalidMeshID = MeshID(nilIndex) - -// MeshID is a reference to a mesh in the scene. -type MeshID int32 - -// MeshInfo contains the information needed to create a mesh shape. -type MeshInfo[M any] struct { - - // Position optionally specifies a position where the mesh should be placed. - // - // Defaults to the origin. - Position opt.T[dprec.Vec3] - - // Rotation optionally specifies a rotation of the mesh. - // - // Defaults to the identity rotation. - Rotation opt.T[dprec.Quat] - - // Filtering holds the collision-filtering metadata for the mesh. - Filtering FilterInfo - - // UserData allows one to attach custom user data to the mesh. - UserData M - - // Mesh contains the mesh information. - // - // The mesh must have at least one triangle. An empty mesh has no area to - // be placed in the scene and is considered invalid. - Mesh shape3d.Mesh -} - -type meshShape[M any] struct { - spatialID query3d.TreeItemID - filterRepresentation - meshRepresentation - userData M -} - -func shapeMeshCanIntersect[S, M any](shape *shape[S], mesh *meshShape[M]) bool { - return shape.canInteractWith(&mesh.filterRepresentation) -} - -// TODO: Consider using a different storage mechanism. For example an -// Octree or BVH structure. -// Alternatively experiment with placing each mesh triangle in the existing -// mesh tree, through this will likely destroy the mesh tree performance. - -type meshRepresentation struct { - wsBSphere shape3d.Sphere - wsAABB shape3d.AABB - wsTriangles []shape3d.Triangle -} - -func newMeshRepresentation(mesh shape3d.Mesh) meshRepresentation { - return meshRepresentation{ - wsBSphere: mesh.BoundingSphere(), - wsAABB: mesh.BoundingAABB(), - wsTriangles: mesh.Triangles, - } -} diff --git a/core/spatial/placement3d/object.go b/core/spatial/placement3d/object.go index 3fb050e3..96e4cfc4 100644 --- a/core/spatial/placement3d/object.go +++ b/core/spatial/placement3d/object.go @@ -6,8 +6,11 @@ import ( "github.com/mokiat/lacking/core/spatial/shape3d" ) -// InvalidObjectID indicates an object that can never be part of the scene. -const InvalidObjectID = ObjectID(nilIndex) +// NilObjectID indicates an object that can never be part of the scene. +// +// It is also used to denote the absence of a source object in contacts that +// were produced by a query primitive rather than by a scene shape. +const NilObjectID = ObjectID(nilIndex) // ObjectID is a reference to an object in the scene. type ObjectID int32 @@ -30,7 +33,7 @@ type ObjectInfo[O any] struct { UserData O } -type sceneObject[O any] struct { +type objectState[O any] struct { transform shape3d.Transform firstShapeIndex int32 lastShapeIndex int32 diff --git a/core/spatial/placement3d/shape.go b/core/spatial/placement3d/object_shape.go similarity index 64% rename from core/spatial/placement3d/shape.go rename to core/spatial/placement3d/object_shape.go index 61b77e97..c4c71ace 100644 --- a/core/spatial/placement3d/shape.go +++ b/core/spatial/placement3d/object_shape.go @@ -7,11 +7,16 @@ import ( "github.com/mokiat/lacking/core/spatial/shape3d" ) -// InvalidShapeID indicates a shape that can never be part of the scene. -const InvalidShapeID = ShapeID(nilIndex) +// NilObjectShapeID indicates an object shape that can never be part of the +// scene. +// +// It is also used to denote the absence of a source shape in contacts that +// were produced by a query primitive rather than by a scene shape. +const NilObjectShapeID = ObjectShapeID(nilIndex) -// ShapeID is a reference to a shape in the scene. -type ShapeID int32 +// ObjectShapeID is a reference to a convex shape that is attached to an object +// in the scene. +type ObjectShapeID int32 // SphereInfo contains the information needed to create a sphere shape. type SphereInfo[S any] struct { @@ -23,6 +28,9 @@ type SphereInfo[S any] struct { UserData S // Sphere contains the sphere information. + // + // It is specified in the local space of the object that the shape is + // attached to. Sphere shape3d.Sphere } @@ -36,39 +44,42 @@ type BoxInfo[S any] struct { UserData S // Box contains the box information. + // + // It is specified in the local space of the object that the shape is + // attached to. Box shape3d.Box } -type shape[S any] struct { +type objectShapeState[S any] struct { objectIndex int32 nextShapeIndex int32 prevShapeIndex int32 spatialID query3d.TreeItemID filterRepresentation - shapeRepresentation + objectShapeRepresentation userData S } -func shapesCanIntersect[S any](a, b *shape[S]) bool { +func objectShapesCanIntersect[S any](a, b *objectShapeState[S]) bool { if a.objectIndex >= b.objectIndex { return false // prevent self-intersection and repeated checks } return a.filterRepresentation.canInteractWith(&b.filterRepresentation) } -type shapeRepresentation struct { +type objectShapeRepresentation struct { lsBSphere shape3d.Sphere wsBSphere shape3d.Sphere lsTransform shape3d.Transform wsTransform shape3d.Transform - kind shapeKind + kind objectShapeKind points []dprec.Vec3 skinRadius float64 } -func (s *shapeRepresentation) update(parentTransform shape3d.Transform) { +func (s *objectShapeRepresentation) update(parentTransform shape3d.Transform) { s.wsBSphere = shape3d.TransformedSphere(s.lsBSphere, parentTransform) s.wsTransform = shape3d.ChainedTransform( @@ -77,7 +88,7 @@ func (s *shapeRepresentation) update(parentTransform shape3d.Transform) { ) } -func (s *shapeRepresentation) gjkShape() gjk3d.Shape { +func (s *objectShapeRepresentation) gjkShape() gjk3d.Shape { return gjk3d.Shape{ Position: s.wsTransform.Translation, Rotation: s.wsTransform.Rotation, @@ -86,14 +97,14 @@ func (s *shapeRepresentation) gjkShape() gjk3d.Shape { } } -func (s *shapeRepresentation) toSphere() shape3d.Sphere { +func (s *objectShapeRepresentation) toSphere() shape3d.Sphere { return shape3d.Sphere{ Center: s.wsTransform.Translation, Radius: s.skinRadius, } } -func (s *shapeRepresentation) toBox() shape3d.Box { +func (s *objectShapeRepresentation) toBox() shape3d.Box { var halfWidth, halfHeight, halfLength float64 for _, point := range s.points { halfWidth = max(halfWidth, point.X) @@ -109,11 +120,11 @@ func (s *shapeRepresentation) toBox() shape3d.Box { } } -type shapeKind uint32 +type objectShapeKind uint32 const ( - shapeKindSphere shapeKind = iota - shapeKindBox - shapeKindCapsule - shapeKindConvexHull + objectShapeKindSphere objectShapeKind = iota + objectShapeKindBox + objectShapeKindCapsule + objectShapeKindConvexHull ) diff --git a/core/spatial/placement3d/scene.go b/core/spatial/placement3d/scene.go index 65ef5265..78e3d994 100644 --- a/core/spatial/placement3d/scene.go +++ b/core/spatial/placement3d/scene.go @@ -34,52 +34,61 @@ type SceneSettings struct { InitialItemCapacity opt.T[uint32] } -// Scene represents a 3D scene into which dynamic objects (built from convex -// shapes) and static meshes can be placed and tested for intersection. +// Scene represents a 3D scene into which movable objects (built from convex +// shapes) and immovable terrains (built from concave shapes) can be placed and +// tested for intersection. // // The type parameters specify the user data attached to each kind of entity: -// O for objects, S for shapes, and M for meshes. -type Scene[O, S, M any] struct { - shapeTree *query3d.Octree[int32] - meshTree *query3d.Octree[int32] - +// O for objects, T for terrains, and S for the shapes of both. +// +// A scene is not safe for concurrent use. Furthermore, the intersection +// queries share internal scratch buffers, so a query must not be started from +// within the callback of another query. +type Scene[O, T, S any] struct { solver *gjk3d.Solver - freeObjectIndices *ds.Stack[int32] - freeShapeIndices *ds.Stack[int32] - freeMeshIndices *ds.Stack[int32] + objectShapeTree *query3d.Octree[int32] + terrainShapeTree *query3d.Octree[int32] + + freeObjectIndices *ds.Stack[int32] + freeObjectShapeIndices *ds.Stack[int32] + freeTerrainIndices *ds.Stack[int32] + freeTerrainShapeIndices *ds.Stack[int32] - objects []sceneObject[O] - shapes []shape[S] - meshes []meshShape[M] + objects []objectState[O] + objectShapes []objectShapeState[S] + terrains []terrainState[T] + terrainShapes []terrainShapeState[S] - shapeCandidates []int32 - meshCandidates []int32 + objectShapeCandidates []int32 + terrainShapeCandidates []int32 tempGJKSource gjk3d.Shape tempGJKTarget gjk3d.Shape } // NewScene creates a new scene. -func NewScene[O, S, M any](settings SceneSettings) *Scene[O, S, M] { +func NewScene[O, T, S any](settings SceneSettings) *Scene[O, T, S] { treeSettings := query3d.OctreeSettings(settings) - return &Scene[O, S, M]{ - shapeTree: query3d.NewOctree[int32](treeSettings), - meshTree: query3d.NewOctree[int32](treeSettings), - + return &Scene[O, T, S]{ solver: gjk3d.NewSolver(), - freeObjectIndices: ds.EmptyStack[int32](), - freeShapeIndices: ds.EmptyStack[int32](), - freeMeshIndices: ds.EmptyStack[int32](), + objectShapeTree: query3d.NewOctree[int32](treeSettings), + terrainShapeTree: query3d.NewOctree[int32](treeSettings), + + freeObjectIndices: ds.EmptyStack[int32](), + freeObjectShapeIndices: ds.EmptyStack[int32](), + freeTerrainIndices: ds.EmptyStack[int32](), + freeTerrainShapeIndices: ds.EmptyStack[int32](), - objects: make([]sceneObject[O], 0), - shapes: make([]shape[S], 0), - meshes: make([]meshShape[M], 0), + objects: make([]objectState[O], 0), + objectShapes: make([]objectShapeState[S], 0), + terrains: make([]terrainState[T], 0), + terrainShapes: make([]terrainShapeState[S], 0), - shapeCandidates: make([]int32, 0), - meshCandidates: make([]int32, 0), + objectShapeCandidates: make([]int32, 0), + terrainShapeCandidates: make([]int32, 0), tempGJKSource: gjk3d.Shape{ Points: make([]dprec.Vec3, 0, 8), @@ -91,7 +100,7 @@ func NewScene[O, S, M any](settings SceneSettings) *Scene[O, S, M] { } // CreateObject creates a new object. -func (s *Scene[O, S, M]) CreateObject(info ObjectInfo[O]) ObjectID { +func (s *Scene[O, T, S]) CreateObject(info ObjectInfo[O]) ObjectID { transform := shape3d.Transform{ Translation: info.Position.ValueOrDefault(dprec.ZeroVec3()), Rotation: shape3d.RotationFromQuat( @@ -100,7 +109,7 @@ func (s *Scene[O, S, M]) CreateObject(info ObjectInfo[O]) ObjectID { } index := s.allocateObject() - s.objects[index] = sceneObject[O]{ + s.objects[index] = objectState[O]{ transform: transform, firstShapeIndex: nilIndex, lastShapeIndex: nilIndex, @@ -110,69 +119,78 @@ func (s *Scene[O, S, M]) CreateObject(info ObjectInfo[O]) ObjectID { } // DeleteObject deletes an object. -func (s *Scene[O, S, M]) DeleteObject(objID ObjectID) { +func (s *Scene[O, T, S]) DeleteObject(objID ObjectID) { index := int32(objID) object := &s.objects[index] object.userData = gog.Zero[O]() // in case of pointer - s.eachObjectShape(object, func(shapeIndex int32, _ *shape[S]) { - s.detachShape(shapeIndex) + s.eachObjectShape(object, func(shapeIndex int32, _ *objectShapeState[S]) { + s.detachObjectShape(shapeIndex) }) s.releaseObject(index) } // GetObjectUserData returns the user data associated with the given object. -func (s *Scene[O, S, M]) GetObjectUserData(objID ObjectID) O { - object := &s.objects[objID] +func (s *Scene[O, T, S]) GetObjectUserData(objID ObjectID) O { + index := int32(objID) + object := &s.objects[index] return object.userData } // SetObjectUserData assigns the specified user data to the object. -func (s *Scene[O, S, M]) SetObjectUserData(objID ObjectID, userData O) { - object := &s.objects[objID] +func (s *Scene[O, T, S]) SetObjectUserData(objID ObjectID, userData O) { + index := int32(objID) + object := &s.objects[index] object.userData = userData } // GetObjectTransform returns the given object's transform. -func (s *Scene[O, S, M]) GetObjectTransform(objID ObjectID) shape3d.Transform { - object := &s.objects[objID] +func (s *Scene[O, T, S]) GetObjectTransform(objID ObjectID) shape3d.Transform { + index := int32(objID) + object := &s.objects[index] return object.transform } // SetObjectTransform relocates the given object. -func (s *Scene[O, S, M]) SetObjectTransform(objID ObjectID, transform shape3d.Transform) { - object := &s.objects[objID] +func (s *Scene[O, T, S]) SetObjectTransform(objID ObjectID, transform shape3d.Transform) { + index := int32(objID) + object := &s.objects[index] object.transform = transform - s.eachObjectShape(object, func(_ int32, shape *shape[S]) { + s.eachObjectShape(object, func(_ int32, shape *objectShapeState[S]) { shape.update(transform) area := shape3d.AABBFromSphere(shape.wsBSphere) - s.shapeTree.Update(shape.spatialID, area) + s.objectShapeTree.Update(shape.spatialID, area) }) } -// GetShapeObject returns the ID of the object that the given shape is +// GetObjectForShape returns the ID of the object that the given shape is // attached to. -func (s *Scene[O, S, M]) GetShapeObject(shapeID ShapeID) ObjectID { +func (s *Scene[O, T, S]) GetObjectForShape(shapeID ObjectShapeID) ObjectID { index := int32(shapeID) - shape := &s.shapes[index] + shape := &s.objectShapes[index] return ObjectID(shape.objectIndex) } // AttachSphere creates a sphere shape and attaches it to the object to be // used for intersection tests. -func (s *Scene[O, S, M]) AttachSphere(objID ObjectID, info SphereInfo[S]) ShapeID { +// +// The sphere is specified in the local space of the object and moves along +// with it. +func (s *Scene[O, T, S]) AttachSphere(objID ObjectID, info SphereInfo[S]) ObjectShapeID { + index := int32(objID) + sphere := info.Sphere transform := shape3d.Transform{ Translation: sphere.Center, Rotation: shape3d.IdentityRotation(), } - return s.attachShape(int32(objID), info.Filtering, shapeRepresentation{ + return s.attachObjectShape(index, info.Filtering, objectShapeRepresentation{ lsBSphere: sphere, wsBSphere: sphere, lsTransform: transform, wsTransform: transform, - kind: shapeKindSphere, + kind: objectShapeKindSphere, points: []dprec.Vec3{ // TODO: Consider reusing from a buffer. dprec.ZeroVec3(), }, @@ -182,7 +200,12 @@ func (s *Scene[O, S, M]) AttachSphere(objID ObjectID, info SphereInfo[S]) ShapeI // AttachBox creates a box shape and attaches it to the object to be used for // intersection tests. -func (s *Scene[O, S, M]) AttachBox(objID ObjectID, info BoxInfo[S]) ShapeID { +// +// The box is specified in the local space of the object and moves along with +// it. +func (s *Scene[O, T, S]) AttachBox(objID ObjectID, info BoxInfo[S]) ObjectShapeID { + index := int32(objID) + box := info.Box transform := shape3d.Transform{ Translation: info.Box.Center, @@ -193,12 +216,12 @@ func (s *Scene[O, S, M]) AttachBox(objID ObjectID, info BoxInfo[S]) ShapeID { halfHeight := box.HalfHeight halfLength := box.HalfLength - return s.attachShape(int32(objID), info.Filtering, shapeRepresentation{ + return s.attachObjectShape(index, info.Filtering, objectShapeRepresentation{ lsBSphere: bSphere, wsBSphere: bSphere, lsTransform: transform, wsTransform: transform, - kind: shapeKindBox, + kind: objectShapeKindBox, points: []dprec.Vec3{ // TODO: Consider reusing from a buffer. dprec.NewVec3(-halfWidth, -halfHeight, -halfLength), dprec.NewVec3(halfWidth, -halfHeight, -halfLength), @@ -213,42 +236,44 @@ func (s *Scene[O, S, M]) AttachBox(objID ObjectID, info BoxInfo[S]) ShapeID { }, info.UserData) } -// DeleteShape deletes a shape from an object. The object is not +// DeleteObjectShape deletes a shape from an object. The object is not // deleted and continues to exist in the scene. -func (s *Scene[O, S, M]) DeleteShape(shapeID ShapeID) { +func (s *Scene[O, T, S]) DeleteObjectShape(shapeID ObjectShapeID) { index := int32(shapeID) - s.detachShape(index) + s.detachObjectShape(index) } -// GetShapeUserData returns the user data associated with the given shape. -func (s *Scene[O, S, M]) GetShapeUserData(shapeID ShapeID) S { +// GetObjectShapeUserData returns the user data associated with the given +// object shape. +func (s *Scene[O, T, S]) GetObjectShapeUserData(shapeID ObjectShapeID) S { index := int32(shapeID) - shape := &s.shapes[index] + shape := &s.objectShapes[index] return shape.userData } -// SetShapeUserData assigns the specified user data to the shape. -func (s *Scene[O, S, M]) SetShapeUserData(shapeID ShapeID, userData S) { +// SetObjectShapeUserData assigns the specified user data to the object shape. +func (s *Scene[O, T, S]) SetObjectShapeUserData(shapeID ObjectShapeID, userData S) { index := int32(shapeID) - shape := &s.shapes[index] + shape := &s.objectShapes[index] shape.userData = userData } -// EachSphere iterates over all sphere shapes in the scene that match the -// filter and yields them to the provided callback. -func (s *Scene[O, S, M]) EachSphere(filter Filter, yield func(shape3d.Sphere) bool) { - if filter.SkipDynamic { - return - } - for index := range s.shapes { - shape := &s.shapes[index] +// EachSphere iterates over all sphere shapes in the scene that match the mask +// and yields them, in world space, to the provided callback. Iteration stops +// early if the callback returns false. +// +// Note that a zero mask matches no shape at all. Use [FullMask] to iterate +// over every sphere in the scene. +func (s *Scene[O, T, S]) EachSphere(mask Mask, yield func(shape3d.Sphere) bool) { + for index := range s.objectShapes { + shape := &s.objectShapes[index] if shape.spatialID == query3d.InvalidTreeItemID { continue } - if shape.kind != shapeKindSphere { + if shape.kind != objectShapeKindSphere { continue } - if !shape.matchesFilter(filter) { + if !shape.satisfiesMask(mask) { continue } if !yield(shape.toSphere()) { @@ -257,29 +282,30 @@ func (s *Scene[O, S, M]) EachSphere(filter Filter, yield func(shape3d.Sphere) bo } } -// SphereIter returns an iterator over all sphere shapes in the scene that match -// the filter. -func (s *Scene[O, S, M]) SphereIter(filter Filter) iter.Seq[shape3d.Sphere] { +// SphereIter returns an iterator over all sphere shapes in the scene that +// match the mask, as described by [Scene.EachSphere]. +func (s *Scene[O, T, S]) SphereIter(mask Mask) iter.Seq[shape3d.Sphere] { return func(yield func(shape3d.Sphere) bool) { - s.EachSphere(filter, yield) + s.EachSphere(mask, yield) } } -// EachBox iterates over all box shapes in the scene that match the -// filter and yields them to the provided callback. -func (s *Scene[O, S, M]) EachBox(filter Filter, yield func(shape3d.Box) bool) { - if filter.SkipDynamic { - return - } - for index := range s.shapes { - shape := &s.shapes[index] +// EachBox iterates over all box shapes in the scene that match the mask and +// yields them, in world space, to the provided callback. Iteration stops early +// if the callback returns false. +// +// Note that a zero mask matches no shape at all. Use [FullMask] to iterate +// over every box in the scene. +func (s *Scene[O, T, S]) EachBox(mask Mask, yield func(shape3d.Box) bool) { + for index := range s.objectShapes { + shape := &s.objectShapes[index] if shape.spatialID == query3d.InvalidTreeItemID { continue } - if shape.kind != shapeKindBox { + if shape.kind != objectShapeKindBox { continue } - if !shape.matchesFilter(filter) { + if !shape.satisfiesMask(mask) { continue } if !yield(shape.toBox()) { @@ -288,263 +314,379 @@ func (s *Scene[O, S, M]) EachBox(filter Filter, yield func(shape3d.Box) bool) { } } -// BoxIter returns an iterator over all box shapes in the scene that match -// the filter. -func (s *Scene[O, S, M]) BoxIter(filter Filter) iter.Seq[shape3d.Box] { +// BoxIter returns an iterator over all box shapes in the scene that match the +// mask, as described by [Scene.EachBox]. +func (s *Scene[O, T, S]) BoxIter(mask Mask) iter.Seq[shape3d.Box] { return func(yield func(shape3d.Box) bool) { - s.EachBox(filter, yield) + s.EachBox(mask, yield) + } +} + +// CreateTerrain creates a new terrain. +// +// A terrain has no transform of its own. It merely groups the concave shapes +// that are attached to it, which are specified in world space. +func (s *Scene[O, T, S]) CreateTerrain(info TerrainInfo[T]) TerrainID { + index := s.allocateTerrain() + s.terrains[index] = terrainState[T]{ + firstShapeIndex: nilIndex, + lastShapeIndex: nilIndex, + userData: info.UserData, } + return TerrainID(index) +} + +// DeleteTerrain deletes a terrain, along with all of the shapes that are +// attached to it. +func (s *Scene[O, T, S]) DeleteTerrain(terrainID TerrainID) { + index := int32(terrainID) + terrain := &s.terrains[index] + terrain.userData = gog.Zero[T]() // in case of pointer + s.eachTerrainShape(terrain, func(shapeIndex int32, _ *terrainShapeState[S]) { + s.detachTerrainShape(shapeIndex) + }) + s.releaseTerrain(index) +} + +// GetTerrainUserData returns the user data associated with the given terrain. +func (s *Scene[O, T, S]) GetTerrainUserData(terrainID TerrainID) T { + index := int32(terrainID) + terrain := &s.terrains[index] + return terrain.userData +} + +// SetTerrainUserData assigns the specified user data to the terrain. +func (s *Scene[O, T, S]) SetTerrainUserData(terrainID TerrainID, userData T) { + index := int32(terrainID) + terrain := &s.terrains[index] + terrain.userData = userData +} + +// GetTerrainForShape returns the ID of the terrain that the given shape is +// attached to. +func (s *Scene[O, T, S]) GetTerrainForShape(shapeID TerrainShapeID) TerrainID { + index := int32(shapeID) + shape := &s.terrainShapes[index] + return TerrainID(shape.terrainIndex) } -// CreateMesh creates a new static mesh in the scene. +// AttachMesh creates a mesh shape and attaches it to the terrain to be used +// for intersection tests. // -// Unlike shapes, a mesh is not attached to an object. It is positioned -// directly through the [MeshInfo.Position] and [MeshInfo.Rotation] fields and -// is intended for static geometry that participates in intersection tests as a -// collection of triangles. +// The mesh is specified in world space, as terrains have no transform of their +// own, and cannot be relocated afterwards. // // The mesh specified through [MeshInfo.Mesh] must not be empty, otherwise this // function panics. -func (s *Scene[O, S, M]) CreateMesh(info MeshInfo[M]) MeshID { - transform := shape3d.Transform{ - Translation: info.Position.ValueOrDefault(dprec.ZeroVec3()), - Rotation: shape3d.RotationFromQuat( - info.Rotation.ValueOrDefault(dprec.IdentityQuat()), - ), - } - representation := newMeshRepresentation(shape3d.TransformedMesh(info.Mesh, transform)) - area := representation.wsAABB +func (s *Scene[O, T, S]) AttachMesh(terrainID TerrainID, info MeshInfo[S]) TerrainShapeID { + index := int32(terrainID) - index := s.allocateMesh() - s.meshes[index] = meshShape[M]{ - spatialID: s.meshTree.Insert(area, index), - filterRepresentation: newFilterRepresentation(info.Filtering), - meshRepresentation: representation, - userData: info.UserData, - } + mesh := info.Mesh + bSphere := mesh.BoundingSphere() + aabb := mesh.BoundingAABB() - return MeshID(index) + return s.attachTerrainShape(index, info.Filtering, terrainShapeRepresentation{ + wsBSphere: bSphere, + wsAABB: aabb, + wsTriangles: mesh.Triangles, + }, info.UserData) } -// DeleteMesh removes the given mesh from the scene. -func (s *Scene[O, S, M]) DeleteMesh(meshID MeshID) { - index := int32(meshID) - mesh := &s.meshes[index] - s.meshTree.Remove(mesh.spatialID) - mesh.spatialID = query3d.InvalidTreeItemID - mesh.userData = gog.Zero[M]() // in case of pointer - s.releaseMesh(index) +// DeleteTerrainShape deletes a shape from a terrain. The terrain is not +// deleted and continues to exist in the scene. +func (s *Scene[O, T, S]) DeleteTerrainShape(shapeID TerrainShapeID) { + index := int32(shapeID) + s.detachTerrainShape(index) } -// GetMeshUserData returns the user data associated with the given mesh. -func (s *Scene[O, S, M]) GetMeshUserData(meshID MeshID) M { - mesh := &s.meshes[meshID] - return mesh.userData +// GetTerrainShapeUserData returns the user data associated with the given +// terrain shape. +func (s *Scene[O, T, S]) GetTerrainShapeUserData(shapeID TerrainShapeID) S { + index := int32(shapeID) + shape := &s.terrainShapes[index] + return shape.userData } -// SetMeshUserData assigns the specified user data to the mesh. -func (s *Scene[O, S, M]) SetMeshUserData(meshID MeshID, userData M) { - mesh := &s.meshes[meshID] - mesh.userData = userData +// SetTerrainShapeUserData assigns the specified user data to the terrain +// shape. +func (s *Scene[O, T, S]) SetTerrainShapeUserData(shapeID TerrainShapeID, userData S) { + index := int32(shapeID) + shape := &s.terrainShapes[index] + shape.userData = userData } -// CollectSegmentIntersections collects all intersections of the segment -// with objects in the scene. -func (s *Scene[O, S, M]) CollectSegmentIntersections(segment shape3d.Segment, filter Filter, yield ContactCallback) { - if !filter.SkipDynamic { - s.shapeCandidates = s.shapeCandidates[:0] - s.shapeTree.QuerySegment(segment, func(index int32) bool { - s.shapeCandidates = append(s.shapeCandidates, index) - return true - }) - s.collectSegmentShape(segment, filter, yield) - } +// CollectSegmentObjectIntersections collects all intersections of the segment +// with the object shapes in the scene that match the mask. +// +// The reported contacts have no source, since the segment is not part of the +// scene. Their Depth is the fraction of the segment that lies beyond the +// contact point, as described by [shape3d.Contact]. +func (s *Scene[O, T, S]) CollectSegmentObjectIntersections(segment shape3d.Segment, mask Mask, yield ObjectContactCallback) { + s.objectShapeCandidates = s.objectShapeCandidates[:0] + s.objectShapeTree.QuerySegment(segment, func(index int32) bool { + s.objectShapeCandidates = append(s.objectShapeCandidates, index) + return true + }) + s.collectSegmentObject(segment, mask, yield) +} - if !filter.SkipStatic { - s.meshCandidates = s.meshCandidates[:0] - s.meshTree.QuerySegment(segment, func(index int32) bool { - s.meshCandidates = append(s.meshCandidates, index) - return true - }) - s.collectSegmentMesh(segment, filter, yield) - } +// CheckSegmentObjectIntersection returns the intersection of the segment with +// the object shape that it enters first, if any. +func (s *Scene[O, T, S]) CheckSegmentObjectIntersection(segment shape3d.Segment, mask Mask) (ObjectContact, bool) { + var collection DeepestObjectContact + s.CollectSegmentObjectIntersections(segment, mask, collection.AddContact) + return collection.Contact() +} + +// CollectSegmentTerrainIntersections collects all intersections of the segment +// with the terrain shapes in the scene that match the mask. At most one +// contact is reported per terrain shape. +// +// The reported contacts have no source, since the segment is not part of the +// scene. Their Depth is the fraction of the segment that lies beyond the +// contact point, as described by [shape3d.Contact]. +func (s *Scene[O, T, S]) CollectSegmentTerrainIntersections(segment shape3d.Segment, mask Mask, yield TerrainContactCallback) { + s.terrainShapeCandidates = s.terrainShapeCandidates[:0] + s.terrainShapeTree.QuerySegment(segment, func(index int32) bool { + s.terrainShapeCandidates = append(s.terrainShapeCandidates, index) + return true + }) + s.collectSegmentTerrain(segment, mask, yield) } -// CheckSegmentIntersection returns the deepest intersection of the segment -// with the scene. -func (s *Scene[O, S, M]) CheckSegmentIntersection(segment shape3d.Segment, filter Filter) (Contact, bool) { - var collection DeepestContact - s.CollectSegmentIntersections(segment, filter, collection.AddContact) +// CheckSegmentTerrainIntersection returns the intersection of the segment with +// the terrain shape that it enters first, if any. +func (s *Scene[O, T, S]) CheckSegmentTerrainIntersection(segment shape3d.Segment, mask Mask) (TerrainContact, bool) { + var collection DeepestTerrainContact + s.CollectSegmentTerrainIntersections(segment, mask, collection.AddContact) return collection.Contact() } -// CollectSphereIntersections collects all intersections of the sphere -// with objects in the scene. -func (s *Scene[O, S, M]) CollectSphereIntersections(sphere shape3d.Sphere, filter Filter, yield ContactCallback) { +// CollectSphereObjectIntersections collects all intersections of the sphere +// with the object shapes in the scene that match the mask. +// +// The reported contacts have no source, since the sphere is not part of the +// scene. +func (s *Scene[O, T, S]) CollectSphereObjectIntersections(sphere shape3d.Sphere, mask Mask, yield ObjectContactCallback) { queryAABB := shape3d.AABBFromSphere(sphere) - if !filter.SkipDynamic { - s.shapeCandidates = s.shapeCandidates[:0] - s.shapeTree.QueryAABB(queryAABB, func(index int32) bool { - s.shapeCandidates = append(s.shapeCandidates, index) - return true - }) - s.collectSphereShape(sphere, filter, yield) - } + s.objectShapeCandidates = s.objectShapeCandidates[:0] + s.objectShapeTree.QueryAABB(queryAABB, func(index int32) bool { + s.objectShapeCandidates = append(s.objectShapeCandidates, index) + return true + }) + s.collectSphereObject(sphere, mask, yield) +} - if !filter.SkipStatic { - s.meshCandidates = s.meshCandidates[:0] - s.meshTree.QueryAABB(queryAABB, func(index int32) bool { - s.meshCandidates = append(s.meshCandidates, index) - return true - }) - s.collectSphereMesh(sphere, filter, yield) - } +// CheckSphereObjectIntersection returns the deepest intersection of the sphere +// with an object shape in the scene, if any. +func (s *Scene[O, T, S]) CheckSphereObjectIntersection(sphere shape3d.Sphere, mask Mask) (ObjectContact, bool) { + var collection DeepestObjectContact + s.CollectSphereObjectIntersections(sphere, mask, collection.AddContact) + return collection.Contact() +} + +// CollectSphereTerrainIntersections collects all intersections of the sphere +// with the terrain shapes in the scene that match the mask. At most one +// contact is reported per terrain shape. +// +// The reported contacts have no source, since the sphere is not part of the +// scene. +func (s *Scene[O, T, S]) CollectSphereTerrainIntersections(sphere shape3d.Sphere, mask Mask, yield TerrainContactCallback) { + queryAABB := shape3d.AABBFromSphere(sphere) + + s.terrainShapeCandidates = s.terrainShapeCandidates[:0] + s.terrainShapeTree.QueryAABB(queryAABB, func(index int32) bool { + s.terrainShapeCandidates = append(s.terrainShapeCandidates, index) + return true + }) + s.collectSphereTerrain(sphere, mask, yield) } -// CheckSphereIntersection returns the deepest intersection of the sphere -// with the scene. -func (s *Scene[O, S, M]) CheckSphereIntersection(sphere shape3d.Sphere, filter Filter) (Contact, bool) { - var collection DeepestContact - s.CollectSphereIntersections(sphere, filter, collection.AddContact) +// CheckSphereTerrainIntersection returns the deepest intersection of the +// sphere with a terrain shape in the scene, if any. +func (s *Scene[O, T, S]) CheckSphereTerrainIntersection(sphere shape3d.Sphere, mask Mask) (TerrainContact, bool) { + var collection DeepestTerrainContact + s.CollectSphereTerrainIntersections(sphere, mask, collection.AddContact) return collection.Contact() } -// CollectBoxIntersections collects all intersections of the box -// with objects in the scene. -func (s *Scene[O, S, M]) CollectBoxIntersections(box shape3d.Box, filter Filter, yield ContactCallback) { +// CollectBoxObjectIntersections collects all intersections of the box with the +// object shapes in the scene that match the mask. +// +// The reported contacts have no source, since the box is not part of the +// scene. +func (s *Scene[O, T, S]) CollectBoxObjectIntersections(box shape3d.Box, mask Mask, yield ObjectContactCallback) { queryAABB := shape3d.AABBFromBox(box) - if !filter.SkipDynamic { - s.shapeCandidates = s.shapeCandidates[:0] - s.shapeTree.QueryAABB(queryAABB, func(index int32) bool { - s.shapeCandidates = append(s.shapeCandidates, index) - return true - }) - s.collectBoxShape(box, filter, yield) - } + s.objectShapeCandidates = s.objectShapeCandidates[:0] + s.objectShapeTree.QueryAABB(queryAABB, func(index int32) bool { + s.objectShapeCandidates = append(s.objectShapeCandidates, index) + return true + }) + s.collectBoxObject(box, mask, yield) +} - if !filter.SkipStatic { - s.meshCandidates = s.meshCandidates[:0] - s.meshTree.QueryAABB(queryAABB, func(index int32) bool { - s.meshCandidates = append(s.meshCandidates, index) - return true - }) - s.collectBoxMesh(box, filter, yield) - } +// CheckBoxObjectIntersection returns the deepest intersection of the box with +// an object shape in the scene, if any. +func (s *Scene[O, T, S]) CheckBoxObjectIntersection(box shape3d.Box, mask Mask) (ObjectContact, bool) { + var collection DeepestObjectContact + s.CollectBoxObjectIntersections(box, mask, collection.AddContact) + return collection.Contact() +} + +// CollectBoxTerrainIntersections collects all intersections of the box with +// the terrain shapes in the scene that match the mask. At most one contact is +// reported per terrain shape. +// +// The reported contacts have no source, since the box is not part of the +// scene. +func (s *Scene[O, T, S]) CollectBoxTerrainIntersections(box shape3d.Box, mask Mask, yield TerrainContactCallback) { + queryAABB := shape3d.AABBFromBox(box) + + s.terrainShapeCandidates = s.terrainShapeCandidates[:0] + s.terrainShapeTree.QueryAABB(queryAABB, func(index int32) bool { + s.terrainShapeCandidates = append(s.terrainShapeCandidates, index) + return true + }) + s.collectBoxTerrain(box, mask, yield) } -// CheckBoxIntersection returns the deepest intersection of the box -// with the scene. -func (s *Scene[O, S, M]) CheckBoxIntersection(box shape3d.Box, filter Filter) (Contact, bool) { - var collection DeepestContact - s.CollectBoxIntersections(box, filter, collection.AddContact) +// CheckBoxTerrainIntersection returns the deepest intersection of the box with +// a terrain shape in the scene, if any. +func (s *Scene[O, T, S]) CheckBoxTerrainIntersection(box shape3d.Box, mask Mask) (TerrainContact, bool) { + var collection DeepestTerrainContact + s.CollectBoxTerrainIntersections(box, mask, collection.AddContact) return collection.Contact() } -// CollectIntersections yields intersections found in this scene. -func (s *Scene[O, S, M]) CollectIntersections(yield ContactCallback) { - for i := range s.shapes { +// CollectObjectIntersections yields the intersections between the object +// shapes in this scene. +// +// Each intersecting pair is reported exactly once, and shapes that belong to +// the same object are never tested against one another. Both the source and +// the target of the reported contacts are object shapes. +func (s *Scene[O, T, S]) CollectObjectIntersections(yield ObjectContactCallback) { + for i := range s.objectShapes { srcIndex := int32(i) - srcShape := &s.shapes[srcIndex] + srcShape := &s.objectShapes[srcIndex] if srcShape.spatialID == query3d.InvalidTreeItemID { continue } queryAABB := shape3d.AABBFromSphere(srcShape.wsBSphere) - s.shapeCandidates = s.shapeCandidates[:0] - s.shapeTree.QueryAABB(queryAABB, func(tgtIndex int32) bool { - s.shapeCandidates = append(s.shapeCandidates, tgtIndex) + s.objectShapeCandidates = s.objectShapeCandidates[:0] + s.objectShapeTree.QueryAABB(queryAABB, func(tgtIndex int32) bool { + s.objectShapeCandidates = append(s.objectShapeCandidates, tgtIndex) return true }) - s.collectShapeShape(srcIndex, srcShape, yield) + s.collectObjectObject(srcIndex, srcShape, yield) + } +} + +// CollectTerrainIntersections yields the intersections between the object +// shapes and the terrain shapes in this scene. At most one contact is reported +// per object shape and terrain shape pair. +// +// Terrain shapes are never tested against one another, since terrains cannot +// move. The source of the reported contacts is therefore always an object +// shape. +func (s *Scene[O, T, S]) CollectTerrainIntersections(yield TerrainContactCallback) { + for i := range s.objectShapes { + srcIndex := int32(i) + srcShape := &s.objectShapes[srcIndex] + if srcShape.spatialID == query3d.InvalidTreeItemID { + continue + } + + queryAABB := shape3d.AABBFromSphere(srcShape.wsBSphere) - s.meshCandidates = s.meshCandidates[:0] - s.meshTree.QueryAABB(queryAABB, func(tgtIndex int32) bool { - s.meshCandidates = append(s.meshCandidates, tgtIndex) + s.terrainShapeCandidates = s.terrainShapeCandidates[:0] + s.terrainShapeTree.QueryAABB(queryAABB, func(tgtIndex int32) bool { + s.terrainShapeCandidates = append(s.terrainShapeCandidates, tgtIndex) return true }) - s.collectShapeMesh(srcIndex, srcShape, yield) + s.collectObjectTerrain(srcIndex, srcShape, yield) } } const nilIndex = -1 -func (s *Scene[O, S, M]) allocateObject() int32 { +func (s *Scene[O, T, S]) allocateObject() int32 { if s.freeObjectIndices.IsEmpty() { index := len(s.objects) - s.objects = append(s.objects, sceneObject[O]{}) + s.objects = append(s.objects, objectState[O]{}) return int32(index) } else { return s.freeObjectIndices.Pop() } } -func (s *Scene[O, S, M]) releaseObject(index int32) { +func (s *Scene[O, T, S]) releaseObject(index int32) { s.freeObjectIndices.Push(index) } -func (s *Scene[O, S, M]) eachObjectShape(object *sceneObject[O], cb func(int32, *shape[S])) { +func (s *Scene[O, T, S]) eachObjectShape(object *objectState[O], cb func(int32, *objectShapeState[S])) { index := object.firstShapeIndex for index >= 0 { - shape := &s.shapes[index] + shape := &s.objectShapes[index] nextIndex := shape.nextShapeIndex cb(index, shape) index = nextIndex } } -func (s *Scene[O, S, M]) allocateShape() int32 { - if s.freeShapeIndices.IsEmpty() { - index := len(s.shapes) - s.shapes = append(s.shapes, shape[S]{}) +func (s *Scene[O, T, S]) allocateObjectShape() int32 { + if s.freeObjectShapeIndices.IsEmpty() { + index := len(s.objectShapes) + s.objectShapes = append(s.objectShapes, objectShapeState[S]{}) return int32(index) } else { - return s.freeShapeIndices.Pop() + return s.freeObjectShapeIndices.Pop() } } -func (s *Scene[O, S, M]) releaseShape(index int32) { - s.freeShapeIndices.Push(index) +func (s *Scene[O, T, S]) releaseObjectShape(index int32) { + s.freeObjectShapeIndices.Push(index) } -func (s *Scene[O, S, M]) attachShape( +func (s *Scene[O, T, S]) attachObjectShape( objectIndex int32, filterInfo FilterInfo, - representation shapeRepresentation, + representation objectShapeRepresentation, userData S, -) ShapeID { +) ObjectShapeID { object := &s.objects[objectIndex] - index := s.allocateShape() + index := s.allocateObjectShape() representation.update(object.transform) area := shape3d.AABBFromSphere(representation.wsBSphere) - s.shapes[index] = shape[S]{ - objectIndex: objectIndex, - nextShapeIndex: nilIndex, - prevShapeIndex: object.lastShapeIndex, - spatialID: s.shapeTree.Insert(area, index), - filterRepresentation: newFilterRepresentation(filterInfo), - shapeRepresentation: representation, - userData: userData, + s.objectShapes[index] = objectShapeState[S]{ + objectIndex: objectIndex, + nextShapeIndex: nilIndex, + prevShapeIndex: object.lastShapeIndex, + spatialID: s.objectShapeTree.Insert(area, index), + filterRepresentation: newFilterRepresentation(filterInfo), + objectShapeRepresentation: representation, + userData: userData, } if object.firstShapeIndex == nilIndex { object.firstShapeIndex = index } else { - s.shapes[object.lastShapeIndex].nextShapeIndex = index + s.objectShapes[object.lastShapeIndex].nextShapeIndex = index } object.lastShapeIndex = index - return ShapeID(index) + return ObjectShapeID(index) } -func (s *Scene[O, S, M]) detachShape(index int32) { - shape := &s.shapes[index] +func (s *Scene[O, T, S]) detachObjectShape(index int32) { + shape := &s.objectShapes[index] - s.shapeTree.Remove(shape.spatialID) + s.objectShapeTree.Remove(shape.spatialID) shape.spatialID = query3d.InvalidTreeItemID object := &s.objects[shape.objectIndex] @@ -555,185 +697,282 @@ func (s *Scene[O, S, M]) detachShape(index int32) { object.lastShapeIndex = shape.prevShapeIndex } if shape.prevShapeIndex != nilIndex { - prevShape := &s.shapes[shape.prevShapeIndex] + prevShape := &s.objectShapes[shape.prevShapeIndex] prevShape.nextShapeIndex = shape.nextShapeIndex } if shape.nextShapeIndex != nilIndex { - nextShape := &s.shapes[shape.nextShapeIndex] + nextShape := &s.objectShapes[shape.nextShapeIndex] nextShape.prevShapeIndex = shape.prevShapeIndex } shape.objectIndex = -1 shape.userData = gog.Zero[S]() // in case of pointer - s.releaseShape(index) + s.releaseObjectShape(index) +} + +func (s *Scene[O, T, S]) allocateTerrain() int32 { + if s.freeTerrainIndices.IsEmpty() { + index := len(s.terrains) + s.terrains = append(s.terrains, terrainState[T]{}) + return int32(index) + } else { + return s.freeTerrainIndices.Pop() + } +} + +func (s *Scene[O, T, S]) releaseTerrain(index int32) { + s.freeTerrainIndices.Push(index) +} + +func (s *Scene[O, T, S]) eachTerrainShape(terrain *terrainState[T], cb func(int32, *terrainShapeState[S])) { + index := terrain.firstShapeIndex + for index >= 0 { + shape := &s.terrainShapes[index] + nextIndex := shape.nextShapeIndex + cb(index, shape) + index = nextIndex + } } -func (s *Scene[O, S, M]) allocateMesh() int32 { - if s.freeMeshIndices.IsEmpty() { - index := len(s.meshes) - s.meshes = append(s.meshes, meshShape[M]{}) +func (s *Scene[O, T, S]) allocateTerrainShape() int32 { + if s.freeTerrainShapeIndices.IsEmpty() { + index := len(s.terrainShapes) + s.terrainShapes = append(s.terrainShapes, terrainShapeState[S]{}) return int32(index) } else { - return s.freeMeshIndices.Pop() + return s.freeTerrainShapeIndices.Pop() + } +} + +func (s *Scene[O, T, S]) releaseTerrainShape(index int32) { + s.freeTerrainShapeIndices.Push(index) +} + +func (s *Scene[O, T, S]) attachTerrainShape( + terrainIndex int32, + filterInfo FilterInfo, + representation terrainShapeRepresentation, + userData S, +) TerrainShapeID { + + terrain := &s.terrains[terrainIndex] + index := s.allocateTerrainShape() + + area := representation.wsAABB + + s.terrainShapes[index] = terrainShapeState[S]{ + terrainIndex: terrainIndex, + nextShapeIndex: nilIndex, + prevShapeIndex: terrain.lastShapeIndex, + spatialID: s.terrainShapeTree.Insert(area, index), + filterRepresentation: newFilterRepresentation(filterInfo), + terrainShapeRepresentation: representation, + userData: userData, + } + if terrain.firstShapeIndex == nilIndex { + terrain.firstShapeIndex = index + } else { + s.terrainShapes[terrain.lastShapeIndex].nextShapeIndex = index } + terrain.lastShapeIndex = index + + return TerrainShapeID(index) } -func (s *Scene[O, S, M]) releaseMesh(index int32) { - s.freeMeshIndices.Push(index) +func (s *Scene[O, T, S]) detachTerrainShape(index int32) { + shape := &s.terrainShapes[index] + + s.terrainShapeTree.Remove(shape.spatialID) + shape.spatialID = query3d.InvalidTreeItemID + + terrain := &s.terrains[shape.terrainIndex] + if terrain.firstShapeIndex == index { + terrain.firstShapeIndex = shape.nextShapeIndex + } + if terrain.lastShapeIndex == index { + terrain.lastShapeIndex = shape.prevShapeIndex + } + if shape.prevShapeIndex != nilIndex { + prevShape := &s.terrainShapes[shape.prevShapeIndex] + prevShape.nextShapeIndex = shape.nextShapeIndex + } + if shape.nextShapeIndex != nilIndex { + nextShape := &s.terrainShapes[shape.nextShapeIndex] + nextShape.prevShapeIndex = shape.prevShapeIndex + } + shape.terrainIndex = -1 + shape.userData = gog.Zero[S]() // in case of pointer + + s.releaseTerrainShape(index) } -func (s *Scene[O, S, M]) collectSegmentShape(segment shape3d.Segment, filter Filter, yield ContactCallback) { - for index, shape := range s.iterCandidateShape(filter) { +func (s *Scene[O, T, S]) collectSegmentObject(segment shape3d.Segment, mask Mask, yield ObjectContactCallback) { + for index, shape := range s.iterCandidateObjectShapes(mask) { if !isec3d.CheckSegmentSphereOverlap(segment, shape.wsBSphere) { continue } onContact := func(contact shape3d.Contact) { - yield(Contact{ - SourceShapeID: InvalidShapeID, - TargetShapeID: ShapeID(index), - TargetMeshID: InvalidMeshID, - Contact: contact, + yield(ObjectContact{ + SourceObjectID: NilObjectID, + SourceShapeID: NilObjectShapeID, + TargetObjectID: ObjectID(shape.objectIndex), + TargetShapeID: ObjectShapeID(index), + Contact: contact, }) } switch shape.kind { - case shapeKindSphere: + case objectShapeKindSphere: sphere := shape.toSphere() isec3d.ResolveSegmentSphere(segment, sphere, onContact) - case shapeKindBox: + case objectShapeKindBox: box := shape.toBox() isec3d.ResolveSegmentBox(segment, box, onContact) } } } -func (s *Scene[O, S, M]) collectSegmentMesh(segment shape3d.Segment, filter Filter, yield ContactCallback) { - for index, mesh := range s.iterCandidateMesh(filter) { - if !isec3d.CheckSegmentSphereOverlap(segment, mesh.wsBSphere) { +func (s *Scene[O, T, S]) collectSegmentTerrain(segment shape3d.Segment, mask Mask, yield TerrainContactCallback) { + for index, shape := range s.iterCandidateTerrainShapes(mask) { + if !isec3d.CheckSegmentSphereOverlap(segment, shape.wsBSphere) { continue } var deepestContact shape3d.DeepestContact - for _, triangle := range mesh.wsTriangles { + for _, triangle := range shape.wsTriangles { isec3d.ResolveSegmentTriangle(segment, triangle, deepestContact.AddContact) } if contact, ok := deepestContact.Contact(); ok { - yield(Contact{ - SourceShapeID: InvalidShapeID, - TargetShapeID: InvalidShapeID, - TargetMeshID: MeshID(index), - Contact: contact, + yield(TerrainContact{ + SourceObjectID: NilObjectID, + SourceShapeID: NilObjectShapeID, + TargetTerrainID: TerrainID(shape.terrainIndex), + TargetShapeID: TerrainShapeID(index), + Contact: contact, }) } } } -func (s *Scene[O, S, M]) collectSphereShape(sphere shape3d.Sphere, filter Filter, yield ContactCallback) { +func (s *Scene[O, T, S]) collectSphereObject(sphere shape3d.Sphere, mask Mask, yield ObjectContactCallback) { initGJKShapeForSphere(sphere, &s.tempGJKSource) - for index, shape := range s.iterCandidateShape(filter) { + for index, shape := range s.iterCandidateObjectShapes(mask) { if !isec3d.CheckSphereSphere(sphere, shape.wsBSphere) { continue } if contact, ok := s.solver.Resolve(s.tempGJKSource, shape.gjkShape()); ok { - yield(Contact{ - SourceShapeID: InvalidShapeID, - TargetShapeID: ShapeID(index), - TargetMeshID: InvalidMeshID, - Contact: contact, + yield(ObjectContact{ + SourceObjectID: NilObjectID, + SourceShapeID: NilObjectShapeID, + TargetObjectID: ObjectID(shape.objectIndex), + TargetShapeID: ObjectShapeID(index), + Contact: contact, }) } } } -func (s *Scene[O, S, M]) collectSphereMesh(sphere shape3d.Sphere, filter Filter, yield ContactCallback) { +func (s *Scene[O, T, S]) collectSphereTerrain(sphere shape3d.Sphere, mask Mask, yield TerrainContactCallback) { initGJKShapeForSphere(sphere, &s.tempGJKSource) - for tgtIndex, tgtMesh := range s.iterCandidateMesh(filter) { - s.resolveGJKMesh(s.tempGJKSource, sphere, tgtMesh, func(contact shape3d.Contact) { - yield(Contact{ - SourceShapeID: InvalidShapeID, - TargetShapeID: InvalidShapeID, - TargetMeshID: MeshID(tgtIndex), - Contact: contact, + for index, shape := range s.iterCandidateTerrainShapes(mask) { + s.resolveTerrainShape(s.tempGJKSource, sphere, shape, func(contact shape3d.Contact) { + yield(TerrainContact{ + SourceObjectID: NilObjectID, + SourceShapeID: NilObjectShapeID, + TargetTerrainID: TerrainID(shape.terrainIndex), + TargetShapeID: TerrainShapeID(index), + Contact: contact, }) }) } } -func (s *Scene[O, S, M]) collectBoxShape(box shape3d.Box, filter Filter, yield ContactCallback) { +func (s *Scene[O, T, S]) collectBoxObject(box shape3d.Box, mask Mask, yield ObjectContactCallback) { initGJKShapeForBox(box, &s.tempGJKSource) - for index, shape := range s.iterCandidateShape(filter) { + for index, shape := range s.iterCandidateObjectShapes(mask) { if !isec3d.CheckSphereSphere(box.BoundingSphere(), shape.wsBSphere) { continue } if contact, ok := s.solver.Resolve(s.tempGJKSource, shape.gjkShape()); ok { - yield(Contact{ - SourceShapeID: InvalidShapeID, - TargetShapeID: ShapeID(index), - TargetMeshID: InvalidMeshID, - Contact: contact, + yield(ObjectContact{ + SourceObjectID: NilObjectID, + SourceShapeID: NilObjectShapeID, + TargetObjectID: ObjectID(shape.objectIndex), + TargetShapeID: ObjectShapeID(index), + Contact: contact, }) } } } -func (s *Scene[O, S, M]) collectBoxMesh(box shape3d.Box, filter Filter, yield ContactCallback) { +func (s *Scene[O, T, S]) collectBoxTerrain(box shape3d.Box, mask Mask, yield TerrainContactCallback) { initGJKShapeForBox(box, &s.tempGJKSource) - for tgtIndex, tgtMesh := range s.iterCandidateMesh(filter) { - s.resolveGJKMesh(s.tempGJKSource, box.BoundingSphere(), tgtMesh, func(contact shape3d.Contact) { - yield(Contact{ - SourceShapeID: InvalidShapeID, - TargetShapeID: InvalidShapeID, - TargetMeshID: MeshID(tgtIndex), - Contact: contact, + for index, shape := range s.iterCandidateTerrainShapes(mask) { + s.resolveTerrainShape(s.tempGJKSource, box.BoundingSphere(), shape, func(contact shape3d.Contact) { + yield(TerrainContact{ + SourceObjectID: NilObjectID, + SourceShapeID: NilObjectShapeID, + TargetTerrainID: TerrainID(shape.terrainIndex), + TargetShapeID: TerrainShapeID(index), + Contact: contact, }) }) } } -func (s *Scene[O, S, M]) collectShapeShape(srcIndex int32, srcShape *shape[S], yield ContactCallback) { +func (s *Scene[O, T, S]) collectObjectObject(srcIndex int32, srcShape *objectShapeState[S], yield ObjectContactCallback) { srcGJKShape := srcShape.gjkShape() - for _, tgtIndex := range s.shapeCandidates { - tgtShape := &s.shapes[tgtIndex] - if !shapesCanIntersect(srcShape, tgtShape) { + for _, tgtIndex := range s.objectShapeCandidates { + tgtShape := &s.objectShapes[tgtIndex] + if !objectShapesCanIntersect(srcShape, tgtShape) { continue } if !isec3d.CheckSphereSphere(srcShape.wsBSphere, tgtShape.wsBSphere) { continue } if contact, ok := s.solver.Resolve(srcGJKShape, tgtShape.gjkShape()); ok { - yield(Contact{ - SourceShapeID: ShapeID(srcIndex), - TargetShapeID: ShapeID(tgtIndex), - TargetMeshID: InvalidMeshID, - Contact: contact, + yield(ObjectContact{ + SourceObjectID: ObjectID(srcShape.objectIndex), + SourceShapeID: ObjectShapeID(srcIndex), + TargetObjectID: ObjectID(tgtShape.objectIndex), + TargetShapeID: ObjectShapeID(tgtIndex), + Contact: contact, }) } } } -func (s *Scene[O, S, M]) collectShapeMesh(srcIndex int32, srcShape *shape[S], yield ContactCallback) { +func (s *Scene[O, T, S]) collectObjectTerrain(srcIndex int32, srcShape *objectShapeState[S], yield TerrainContactCallback) { srcGJKShape := srcShape.gjkShape() - for _, tgtIndex := range s.meshCandidates { - tgtMesh := &s.meshes[tgtIndex] - if !shapeMeshCanIntersect(srcShape, tgtMesh) { + for _, tgtIndex := range s.terrainShapeCandidates { + tgtShape := &s.terrainShapes[tgtIndex] + if !objectTerrainShapesCanIntersect(srcShape, tgtShape) { continue } - s.resolveGJKMesh(srcGJKShape, srcShape.wsBSphere, tgtMesh, func(contact shape3d.Contact) { - yield(Contact{ - SourceShapeID: ShapeID(srcIndex), - TargetShapeID: InvalidShapeID, - TargetMeshID: MeshID(tgtIndex), - Contact: contact, + s.resolveTerrainShape(srcGJKShape, srcShape.wsBSphere, tgtShape, func(contact shape3d.Contact) { + yield(TerrainContact{ + SourceObjectID: ObjectID(srcShape.objectIndex), + SourceShapeID: ObjectShapeID(srcIndex), + TargetTerrainID: TerrainID(tgtShape.terrainIndex), + TargetShapeID: TerrainShapeID(tgtIndex), + Contact: contact, }) }) } } -func (s *Scene[O, S, M]) resolveGJKMesh(srcGJK gjk3d.Shape, srcBS shape3d.Sphere, tgtMesh *meshShape[M], yield shape3d.ContactCallback) { - if !isec3d.CheckSphereSphere(srcBS, tgtMesh.wsBSphere) { +// resolveTerrainShape resolves the intersection of the specified convex source +// shape with a terrain shape, by testing it against each of the triangles that +// make up the terrain shape. +// +// Only the deepest contact is yielded, and at most once, so that a source +// shape that overlaps many triangles of the same terrain shape does not +// produce a pile of nearly identical contacts. +func (s *Scene[O, T, S]) resolveTerrainShape(srcGJK gjk3d.Shape, srcBS shape3d.Sphere, tgtShape *terrainShapeState[S], yield shape3d.ContactCallback) { + if !isec3d.CheckSphereSphere(srcBS, tgtShape.wsBSphere) { return } points := initGJKShapeForMesh(&s.tempGJKTarget) var deepestContact shape3d.DeepestContact - for _, triangle := range tgtMesh.wsTriangles { + for _, triangle := range tgtShape.wsTriangles { tgtBSphere := triangle.BoundingSphere() if !isec3d.CheckSphereSphere(srcBS, tgtBSphere) { continue @@ -753,10 +992,10 @@ func (s *Scene[O, S, M]) resolveGJKMesh(srcGJK gjk3d.Shape, srcBS shape3d.Sphere } } -func (s *Scene[O, S, M]) eachCandidateShape(filter Filter, cb func(int32, *shape[S]) bool) { - for _, index := range s.shapeCandidates { - shape := &s.shapes[index] - if !shape.matchesFilter(filter) { +func (s *Scene[O, T, S]) eachCandidateObjectShape(mask Mask, cb func(int32, *objectShapeState[S]) bool) { + for _, index := range s.objectShapeCandidates { + shape := &s.objectShapes[index] + if !shape.satisfiesMask(mask) { continue } if !cb(index, shape) { @@ -765,27 +1004,27 @@ func (s *Scene[O, S, M]) eachCandidateShape(filter Filter, cb func(int32, *shape } } -func (s *Scene[O, S, M]) iterCandidateShape(filter Filter) iter.Seq2[int32, *shape[S]] { - return func(yield func(int32, *shape[S]) bool) { - s.eachCandidateShape(filter, yield) +func (s *Scene[O, T, S]) iterCandidateObjectShapes(mask Mask) iter.Seq2[int32, *objectShapeState[S]] { + return func(yield func(int32, *objectShapeState[S]) bool) { + s.eachCandidateObjectShape(mask, yield) } } -func (s *Scene[O, S, M]) eachCandidateMesh(filter Filter, cb func(int32, *meshShape[M]) bool) { - for _, index := range s.meshCandidates { - mesh := &s.meshes[index] - if !mesh.matchesFilter(filter) { +func (s *Scene[O, T, S]) eachCandidateTerrainShape(mask Mask, cb func(int32, *terrainShapeState[S]) bool) { + for _, index := range s.terrainShapeCandidates { + shape := &s.terrainShapes[index] + if !shape.satisfiesMask(mask) { continue } - if !cb(index, mesh) { + if !cb(index, shape) { return } } } -func (s *Scene[O, S, M]) iterCandidateMesh(filter Filter) iter.Seq2[int32, *meshShape[M]] { - return func(yield func(int32, *meshShape[M]) bool) { - s.eachCandidateMesh(filter, yield) +func (s *Scene[O, T, S]) iterCandidateTerrainShapes(mask Mask) iter.Seq2[int32, *terrainShapeState[S]] { + return func(yield func(int32, *terrainShapeState[S]) bool) { + s.eachCandidateTerrainShape(mask, yield) } } diff --git a/core/spatial/placement3d/scene_test.go b/core/spatial/placement3d/scene_test.go index 26880a55..ee8fad70 100644 --- a/core/spatial/placement3d/scene_test.go +++ b/core/spatial/placement3d/scene_test.go @@ -30,8 +30,8 @@ func boxAt(x, y, z, half float64) shape3d.Box { } // planeMesh builds a mesh made of two triangles forming a quad in the XZ plane -// (at y == 0), centered at the given point and spanning halfSize in each of -// the X and Z directions. +// (at the given y), centered at the given point and spanning halfSize in each +// of the X and Z directions. func planeMesh(x, y, z, halfSize float64) shape3d.Mesh { a := dprec.NewVec3(x-halfSize, y, z-halfSize) b := dprec.NewVec3(x+halfSize, y, z-halfSize) @@ -56,7 +56,7 @@ var _ = Describe("Scene", func() { Describe("object management", func() { It("creates objects placed at the origin by default", func() { objID := scene.CreateObject(placement3d.ObjectInfo[string]{}) - Expect(objID).NotTo(Equal(placement3d.InvalidObjectID)) + Expect(objID).NotTo(Equal(placement3d.NilObjectID)) transform := scene.GetObjectTransform(objID) Expect(transform.Translation).To(dprectest.HaveVec3Coords(0.0, 0.0, 0.0)) @@ -97,6 +97,59 @@ var _ = Describe("Scene", func() { }) }) + Describe("terrain management", func() { + It("creates terrains", func() { + terrainID := scene.CreateTerrain(placement3d.TerrainInfo[string]{}) + Expect(terrainID).NotTo(Equal(placement3d.NilTerrainID)) + }) + + It("stores and updates user data", func() { + terrainID := scene.CreateTerrain(placement3d.TerrainInfo[string]{ + UserData: "first", + }) + Expect(scene.GetTerrainUserData(terrainID)).To(Equal("first")) + + scene.SetTerrainUserData(terrainID, "second") + Expect(scene.GetTerrainUserData(terrainID)).To(Equal("second")) + }) + + It("stores and updates terrain shape user data", func() { + terrainID := scene.CreateTerrain(placement3d.TerrainInfo[string]{}) + shapeID := scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ + Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), + UserData: "a", + }) + Expect(scene.GetTerrainShapeUserData(shapeID)).To(Equal("a")) + + scene.SetTerrainShapeUserData(shapeID, "b") + Expect(scene.GetTerrainShapeUserData(shapeID)).To(Equal("b")) + }) + + It("maps a terrain shape back to its owning terrain", func() { + terrainID := scene.CreateTerrain(placement3d.TerrainInfo[string]{}) + shapeID := scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ + Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), + }) + Expect(scene.GetTerrainForShape(shapeID)).To(Equal(terrainID)) + }) + + It("reuses the indices of deleted terrains", func() { + first := scene.CreateTerrain(placement3d.TerrainInfo[string]{}) + scene.DeleteTerrain(first) + second := scene.CreateTerrain(placement3d.TerrainInfo[string]{}) + Expect(second).To(Equal(first)) + }) + + It("panics when attaching an empty mesh", func() { + terrainID := scene.CreateTerrain(placement3d.TerrainInfo[string]{}) + Expect(func() { + scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ + Mesh: shape3d.NewMesh(nil), + }) + }).To(Panic()) + }) + }) + Describe("shape iteration", func() { var objID placement3d.ObjectID @@ -113,7 +166,7 @@ var _ = Describe("Scene", func() { }) var found []shape3d.Sphere - scene.EachSphere(placement3d.Filter{}, func(s shape3d.Sphere) bool { + scene.EachSphere(placement3d.FullMask, func(s shape3d.Sphere) bool { found = append(found, s) return true }) @@ -132,7 +185,7 @@ var _ = Describe("Scene", func() { }) count := 0 - scene.EachBox(placement3d.Filter{}, func(shape3d.Box) bool { + scene.EachBox(placement3d.FullMask, func(shape3d.Box) bool { count++ return true }) @@ -145,7 +198,19 @@ var _ = Describe("Scene", func() { }) count := 0 - for range scene.SphereIter(placement3d.Filter{}) { + for range scene.SphereIter(placement3d.FullMask) { + count++ + } + Expect(count).To(Equal(1)) + }) + + It("exposes a box iterator", func() { + scene.AttachBox(objID, placement3d.BoxInfo[string]{ + Box: boxAt(0.0, 0.0, 0.0, 1.0), + }) + + count := 0 + for range scene.BoxIter(placement3d.FullMask) { count++ } Expect(count).To(Equal(1)) @@ -156,27 +221,27 @@ var _ = Describe("Scene", func() { Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), UserData: "a", }) - Expect(scene.GetShapeUserData(shapeID)).To(Equal("a")) + Expect(scene.GetObjectShapeUserData(shapeID)).To(Equal("a")) - scene.SetShapeUserData(shapeID, "b") - Expect(scene.GetShapeUserData(shapeID)).To(Equal("b")) + scene.SetObjectShapeUserData(shapeID, "b") + Expect(scene.GetObjectShapeUserData(shapeID)).To(Equal("b")) }) It("maps a shape back to its owning object", func() { shapeID := scene.AttachSphere(objID, placement3d.SphereInfo[string]{ Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), }) - Expect(scene.GetShapeObject(shapeID)).To(Equal(objID)) + Expect(scene.GetObjectForShape(shapeID)).To(Equal(objID)) }) It("removes a deleted shape from iteration", func() { shapeID := scene.AttachSphere(objID, placement3d.SphereInfo[string]{ Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), }) - scene.DeleteShape(shapeID) + scene.DeleteObjectShape(shapeID) count := 0 - scene.EachSphere(placement3d.Filter{}, func(shape3d.Sphere) bool { + scene.EachSphere(placement3d.FullMask, func(shape3d.Sphere) bool { count++ return true }) @@ -192,7 +257,7 @@ var _ = Describe("Scene", func() { }) count := 0 - scene.EachSphere(placement3d.Filter{}, func(shape3d.Sphere) bool { + scene.EachSphere(placement3d.FullMask, func(shape3d.Sphere) bool { count++ return false }) @@ -211,7 +276,7 @@ var _ = Describe("Scene", func() { )) var centers []dprec.Vec3 - scene.EachSphere(placement3d.Filter{}, func(s shape3d.Sphere) bool { + scene.EachSphere(placement3d.FullMask, func(s shape3d.Sphere) bool { centers = append(centers, s.Center) return true }) @@ -222,33 +287,42 @@ var _ = Describe("Scene", func() { }) }) - Describe("shape iteration filters", func() { - It("filters by layer mask", func() { - objID := scene.CreateObject(placement3d.ObjectInfo[string]{}) + Describe("shape iteration masks", func() { + var objID placement3d.ObjectID + + BeforeEach(func() { + objID = scene.CreateObject(placement3d.ObjectInfo[string]{}) scene.AttachSphere(objID, placement3d.SphereInfo[string]{ Filtering: placement3d.FilterInfo{ SourceMask: opt.V(uint32(0b01)), }, Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), }) + }) - matching := 0 - scene.EachSphere(placement3d.Filter{Mask: opt.V(uint32(0b01))}, func(shape3d.Sphere) bool { - matching++ + countSpheres := func(mask placement3d.Mask) int { + count := 0 + scene.EachSphere(mask, func(shape3d.Sphere) bool { + count++ return true }) - Expect(matching).To(Equal(1)) + return count + } - nonMatching := 0 - scene.EachSphere(placement3d.Filter{Mask: opt.V(uint32(0b10))}, func(shape3d.Sphere) bool { - nonMatching++ - return true - }) - Expect(nonMatching).To(BeZero()) + It("yields shapes that occupy a layer of the mask", func() { + Expect(countSpheres(0b01)).To(Equal(1)) + }) + + It("skips shapes that occupy no layer of the mask", func() { + Expect(countSpheres(0b10)).To(BeZero()) + }) + + It("yields nothing for the zero mask", func() { + Expect(countSpheres(0)).To(BeZero()) }) }) - Describe("CollectIntersections", func() { + Describe("CollectObjectIntersections", func() { // attachOverlappingSpheres places two unit spheres 1.5 apart (so they // overlap) on freshly created objects and returns the object IDs. attachOverlappingSpheres := func() (placement3d.ObjectID, placement3d.ObjectID) { @@ -267,9 +341,9 @@ var _ = Describe("Scene", func() { return first, second } - collect := func() placement3d.ContactList { - var contacts placement3d.ContactList - scene.CollectIntersections(contacts.AddContact) + collect := func() placement3d.ObjectContactList { + var contacts placement3d.ObjectContactList + scene.CollectObjectIntersections(contacts.AddContact) return contacts } @@ -278,11 +352,25 @@ var _ = Describe("Scene", func() { contacts := collect() Expect(contacts).To(HaveLen(1)) Expect([]placement3d.ObjectID{ - scene.GetShapeObject(contacts[0].SourceShapeID), - scene.GetShapeObject(contacts[0].TargetShapeID), + contacts[0].SourceObjectID, + contacts[0].TargetObjectID, }).To(ConsistOf(first, second)) }) + It("reports object IDs that agree with the shape IDs", func() { + attachOverlappingSpheres() + contacts := collect() + Expect(contacts).To(HaveLen(1)) + + contact := contacts[0] + Expect(contact.SourceObjectID).To(Equal( + scene.GetObjectForShape(contact.SourceShapeID), + )) + Expect(contact.TargetObjectID).To(Equal( + scene.GetObjectForShape(contact.TargetShapeID), + )) + }) + It("does not report contacts between shapes of the same object", func() { objID := scene.CreateObject(placement3d.ObjectInfo[string]{}) scene.AttachSphere(objID, placement3d.SphereInfo[string]{ @@ -368,273 +456,453 @@ var _ = Describe("Scene", func() { Expect(collect()).To(HaveLen(1)) }) - It("reports a contact between a shape and an overlapping mesh", func() { + It("reports a contact between two overlapping boxes", func() { + first := scene.CreateObject(placement3d.ObjectInfo[string]{}) + second := scene.CreateObject(placement3d.ObjectInfo[string]{ + Position: opt.V(dprec.NewVec3(1.5, 0.0, 0.0)), + }) + scene.AttachBox(first, placement3d.BoxInfo[string]{ + Box: boxAt(0.0, 0.0, 0.0, 2.0), + }) + scene.AttachBox(second, placement3d.BoxInfo[string]{ + Box: boxAt(0.0, 0.0, 0.0, 2.0), + }) + Expect(collect()).To(HaveLen(1)) + }) + + It("reports a contact between an overlapping sphere and box", func() { + first := scene.CreateObject(placement3d.ObjectInfo[string]{}) + second := scene.CreateObject(placement3d.ObjectInfo[string]{ + Position: opt.V(dprec.NewVec3(1.0, 0.0, 0.0)), + }) + scene.AttachSphere(first, placement3d.SphereInfo[string]{ + Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), + }) + scene.AttachBox(second, placement3d.BoxInfo[string]{ + Box: boxAt(0.0, 0.0, 0.0, 1.0), + }) + Expect(collect()).To(HaveLen(1)) + }) + + It("does not report contacts with terrain shapes", func() { objID := scene.CreateObject(placement3d.ObjectInfo[string]{}) + scene.AttachSphere(objID, placement3d.SphereInfo[string]{ + Sphere: sphereAt(0.0, -0.5, 0.0, 1.0), + }) + terrainID := scene.CreateTerrain(placement3d.TerrainInfo[string]{}) + scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ + Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), + }) + Expect(collect()).To(BeEmpty()) + }) + + It("reports every overlapping pair of two multi-shape objects", func() { + first := scene.CreateObject(placement3d.ObjectInfo[string]{}) + second := scene.CreateObject(placement3d.ObjectInfo[string]{}) + scene.AttachSphere(first, placement3d.SphereInfo[string]{ + Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), + }) + scene.AttachSphere(first, placement3d.SphereInfo[string]{ + Sphere: sphereAt(0.5, 0.0, 0.0, 1.0), + }) + scene.AttachSphere(second, placement3d.SphereInfo[string]{ + Sphere: sphereAt(0.25, 0.0, 0.0, 1.0), + }) + // Both shapes of the first object overlap the single shape of the + // second one, while the two shapes of the first object are not + // tested against each other. + Expect(collect()).To(HaveLen(2)) + }) + + It("does not produce phantom contacts after index reuse", func() { + first, second := attachOverlappingSpheres() + Expect(collect()).To(HaveLen(1)) + + scene.DeleteObject(first) + scene.DeleteObject(second) + Expect(collect()).To(BeEmpty()) + + // Recreate, reusing both the object and the shape indices. + third := scene.CreateObject(placement3d.ObjectInfo[string]{}) + scene.AttachSphere(third, placement3d.SphereInfo[string]{ + Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), + }) + Expect(collect()).To(BeEmpty()) + }) + }) + + Describe("CollectTerrainIntersections", func() { + var objID placement3d.ObjectID + var terrainID placement3d.TerrainID + + BeforeEach(func() { + objID = scene.CreateObject(placement3d.ObjectInfo[string]{}) + terrainID = scene.CreateTerrain(placement3d.TerrainInfo[string]{}) + }) + + collect := func() placement3d.TerrainContactList { + var contacts placement3d.TerrainContactList + scene.CollectTerrainIntersections(contacts.AddContact) + return contacts + } + + It("reports a contact between a shape and an overlapping mesh", func() { // The plane's triangles face -Y, so the sphere is placed just below // the plane (on the front side) where it overlaps and is pushed out. shapeID := scene.AttachSphere(objID, placement3d.SphereInfo[string]{ Sphere: sphereAt(0.0, -0.5, 0.0, 1.0), }) - meshID := scene.CreateMesh(placement3d.MeshInfo[string]{ + meshID := scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), }) contacts := collect() Expect(contacts).To(HaveLen(1)) + Expect(contacts[0].SourceObjectID).To(Equal(objID)) Expect(contacts[0].SourceShapeID).To(Equal(shapeID)) - Expect(contacts[0].TargetShapeID).To(Equal(placement3d.InvalidShapeID)) - Expect(contacts[0].TargetMeshID).To(Equal(meshID)) + Expect(contacts[0].TargetTerrainID).To(Equal(terrainID)) + Expect(contacts[0].TargetShapeID).To(Equal(meshID)) - contact := contacts[0].Contact // The contact normal must push the sphere out the front (-Y) side, // never inward into the mesh. - Expect(contact.TargetNormal.Y).To(BeNumerically("<", 0.0)) + Expect(contacts[0].TargetNormal.Y).To(BeNumerically("<", 0.0)) }) It("does not report a shape overlapping a mesh from behind", func() { // A sphere on the +Y (back) side of the -Y-facing plane would have // to be pushed further inward to separate, which the mesh logic // prevents. - objID := scene.CreateObject(placement3d.ObjectInfo[string]{}) scene.AttachSphere(objID, placement3d.SphereInfo[string]{ Sphere: sphereAt(0.0, 0.5, 0.0, 1.0), }) - scene.CreateMesh(placement3d.MeshInfo[string]{ + scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), }) Expect(collect()).To(BeEmpty()) }) It("does not report a shape disjoint from a mesh", func() { - objID := scene.CreateObject(placement3d.ObjectInfo[string]{}) scene.AttachSphere(objID, placement3d.SphereInfo[string]{ Sphere: sphereAt(0.0, 10.0, 0.0, 1.0), }) - scene.CreateMesh(placement3d.MeshInfo[string]{ + scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), }) Expect(collect()).To(BeEmpty()) }) - }) - Describe("shape-vs-shape with boxes", func() { - It("reports a contact between two overlapping boxes", func() { - first := scene.CreateObject(placement3d.ObjectInfo[string]{}) - second := scene.CreateObject(placement3d.ObjectInfo[string]{ - Position: opt.V(dprec.NewVec3(1.5, 0.0, 0.0)), + It("reports a single contact even when many triangles overlap", func() { + // A large box overlaps both triangles of the plane, yet only the + // deepest contact is reported. + scene.AttachBox(objID, placement3d.BoxInfo[string]{ + Box: boxAt(0.0, -0.5, 0.0, 2.0), }) - scene.AttachBox(first, placement3d.BoxInfo[string]{ - Box: shape3d.NewBox( - dprec.ZeroVec3(), - shape3d.IdentityRotation(), - dprec.NewVec3(2.0, 2.0, 2.0), - ), + scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ + Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), }) - scene.AttachBox(second, placement3d.BoxInfo[string]{ - Box: shape3d.NewBox( - dprec.ZeroVec3(), - shape3d.IdentityRotation(), - dprec.NewVec3(2.0, 2.0, 2.0), - ), + Expect(collect()).To(HaveLen(1)) + }) + + It("reports a contact per terrain shape", func() { + scene.AttachSphere(objID, placement3d.SphereInfo[string]{ + Sphere: sphereAt(0.0, -0.5, 0.0, 1.0), + }) + scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ + Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), }) + scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ + Mesh: planeMesh(0.0, -0.1, 0.0, 5.0), + }) + Expect(collect()).To(HaveLen(2)) + }) - var contacts placement3d.ContactList - scene.CollectIntersections(contacts.AddContact) - Expect(contacts).To(HaveLen(1)) + It("does not report shapes that share a reject group", func() { + scene.AttachSphere(objID, placement3d.SphereInfo[string]{ + Filtering: placement3d.FilterInfo{RejectGroup: 7}, + Sphere: sphereAt(0.0, -0.5, 0.0, 1.0), + }) + scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ + Filtering: placement3d.FilterInfo{RejectGroup: 7}, + Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), + }) + Expect(collect()).To(BeEmpty()) }) - It("reports a contact between an overlapping sphere and box", func() { - first := scene.CreateObject(placement3d.ObjectInfo[string]{}) - second := scene.CreateObject(placement3d.ObjectInfo[string]{ - Position: opt.V(dprec.NewVec3(1.0, 0.0, 0.0)), + It("does not report shapes whose masks do not overlap", func() { + scene.AttachSphere(objID, placement3d.SphereInfo[string]{ + Filtering: placement3d.FilterInfo{ + SourceMask: opt.V(uint32(0b01)), + TargetMask: opt.V(uint32(0b01)), + }, + Sphere: sphereAt(0.0, -0.5, 0.0, 1.0), }) - scene.AttachSphere(first, placement3d.SphereInfo[string]{ - Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), + scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ + Filtering: placement3d.FilterInfo{ + SourceMask: opt.V(uint32(0b10)), + TargetMask: opt.V(uint32(0b10)), + }, + Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), }) - scene.AttachBox(second, placement3d.BoxInfo[string]{ - Box: shape3d.NewBox( - dprec.ZeroVec3(), - shape3d.IdentityRotation(), - dprec.NewVec3(1.0, 1.0, 1.0), - ), + Expect(collect()).To(BeEmpty()) + }) + + It("stops reporting once the terrain is deleted", func() { + scene.AttachSphere(objID, placement3d.SphereInfo[string]{ + Sphere: sphereAt(0.0, -0.5, 0.0, 1.0), + }) + scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ + Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), + }) + Expect(collect()).To(HaveLen(1)) + + scene.DeleteTerrain(terrainID) + Expect(collect()).To(BeEmpty()) + }) + + It("stops reporting once the terrain shape is deleted", func() { + scene.AttachSphere(objID, placement3d.SphereInfo[string]{ + Sphere: sphereAt(0.0, -0.5, 0.0, 1.0), + }) + meshID := scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ + Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), + }) + Expect(collect()).To(HaveLen(1)) + + scene.DeleteTerrainShape(meshID) + Expect(collect()).To(BeEmpty()) + }) + + It("reports a contact per object shape", func() { + scene.AttachSphere(objID, placement3d.SphereInfo[string]{ + Sphere: sphereAt(-1.0, -0.5, 0.0, 1.0), + }) + scene.AttachSphere(objID, placement3d.SphereInfo[string]{ + Sphere: sphereAt(1.0, -0.5, 0.0, 1.0), + }) + scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ + Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), + }) + Expect(collect()).To(HaveLen(2)) + }) + + It("reattaches correctly after terrain shape index reuse", func() { + scene.AttachSphere(objID, placement3d.SphereInfo[string]{ + Sphere: sphereAt(0.0, -0.5, 0.0, 1.0), + }) + meshID := scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ + Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), + }) + scene.DeleteTerrainShape(meshID) + + other := scene.CreateTerrain(placement3d.TerrainInfo[string]{}) + reusedID := scene.AttachMesh(other, placement3d.MeshInfo[string]{ + Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), }) - var contacts placement3d.ContactList - scene.CollectIntersections(contacts.AddContact) + contacts := collect() Expect(contacts).To(HaveLen(1)) + Expect(contacts[0].TargetTerrainID).To(Equal(other)) + Expect(contacts[0].TargetShapeID).To(Equal(reusedID)) + Expect(scene.GetTerrainForShape(reusedID)).To(Equal(other)) + }) + + It("tracks object movement into and out of terrain contact", func() { + scene.AttachSphere(objID, placement3d.SphereInfo[string]{ + Sphere: sphereAt(0.0, -0.5, 0.0, 1.0), + }) + scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ + Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), + }) + Expect(collect()).To(HaveLen(1)) + + scene.SetObjectTransform(objID, shape3d.TranslationTransform( + dprec.NewVec3(0.0, -20.0, 0.0), + )) + Expect(collect()).To(BeEmpty()) + + scene.SetObjectTransform(objID, shape3d.TranslationTransform( + dprec.NewVec3(0.0, 0.0, 0.0), + )) + Expect(collect()).To(HaveLen(1)) }) }) - Describe("CheckSphereIntersection", func() { - It("reports a sphere overlapping a scene shape", func() { + Describe("shape id spaces", func() { + It("keeps object and terrain shape ids independent", func() { objID := scene.CreateObject(placement3d.ObjectInfo[string]{}) - scene.AttachSphere(objID, placement3d.SphereInfo[string]{ + shapeID := scene.AttachSphere(objID, placement3d.SphereInfo[string]{ + Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), + UserData: "object-shape", + }) + terrainID := scene.CreateTerrain(placement3d.TerrainInfo[string]{}) + meshID := scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ + Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), + UserData: "terrain-shape", + }) + + // Both are the first shape of their kind, hence they share the raw + // index while remaining distinct references. + Expect(int32(shapeID)).To(Equal(int32(meshID))) + Expect(scene.GetObjectShapeUserData(shapeID)).To(Equal("object-shape")) + Expect(scene.GetTerrainShapeUserData(meshID)).To(Equal("terrain-shape")) + }) + }) + + Describe("sphere queries", func() { + It("reports a sphere overlapping an object shape", func() { + objID := scene.CreateObject(placement3d.ObjectInfo[string]{}) + shapeID := scene.AttachSphere(objID, placement3d.SphereInfo[string]{ Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), }) - contact, ok := scene.CheckSphereIntersection( + contact, ok := scene.CheckSphereObjectIntersection( sphereAt(1.5, 0.0, 0.0, 1.0), - placement3d.Filter{}, + placement3d.FullMask, ) Expect(ok).To(BeTrue()) - Expect(contact.SourceShapeID).To(Equal(placement3d.InvalidShapeID)) - Expect(scene.GetShapeObject(contact.TargetShapeID)).To(Equal(objID)) - Expect(contact.TargetMeshID).To(Equal(placement3d.InvalidMeshID)) + Expect(contact.SourceObjectID).To(Equal(placement3d.NilObjectID)) + Expect(contact.SourceShapeID).To(Equal(placement3d.NilObjectShapeID)) + Expect(contact.TargetObjectID).To(Equal(objID)) + Expect(contact.TargetShapeID).To(Equal(shapeID)) }) - It("returns false for a sphere disjoint from every shape", func() { + It("returns false for a sphere disjoint from every object shape", func() { objID := scene.CreateObject(placement3d.ObjectInfo[string]{}) scene.AttachSphere(objID, placement3d.SphereInfo[string]{ Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), }) - _, ok := scene.CheckSphereIntersection( + _, ok := scene.CheckSphereObjectIntersection( sphereAt(10.0, 0.0, 0.0, 1.0), - placement3d.Filter{}, + placement3d.FullMask, + ) + Expect(ok).To(BeFalse()) + }) + + It("honors the query mask", func() { + objID := scene.CreateObject(placement3d.ObjectInfo[string]{}) + scene.AttachSphere(objID, placement3d.SphereInfo[string]{ + Filtering: placement3d.FilterInfo{ + SourceMask: opt.V(uint32(0b01)), + }, + Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), + }) + + _, ok := scene.CheckSphereObjectIntersection( + sphereAt(1.5, 0.0, 0.0, 1.0), + 0b10, ) Expect(ok).To(BeFalse()) }) - It("reports a sphere overlapping a mesh from the front", func() { - meshID := scene.CreateMesh(placement3d.MeshInfo[string]{ + It("reports a sphere overlapping a terrain shape from the front", func() { + terrainID := scene.CreateTerrain(placement3d.TerrainInfo[string]{}) + meshID := scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), }) // The plane faces -Y, so approach it from below (the front side). - contact, ok := scene.CheckSphereIntersection( + contact, ok := scene.CheckSphereTerrainIntersection( sphereAt(0.0, -0.5, 0.0, 1.0), - placement3d.Filter{}, + placement3d.FullMask, ) Expect(ok).To(BeTrue()) - Expect(contact.TargetShapeID).To(Equal(placement3d.InvalidShapeID)) - Expect(contact.TargetMeshID).To(Equal(meshID)) + Expect(contact.SourceShapeID).To(Equal(placement3d.NilObjectShapeID)) + Expect(contact.TargetTerrainID).To(Equal(terrainID)) + Expect(contact.TargetShapeID).To(Equal(meshID)) Expect(contact.TargetNormal.Y).To(BeNumerically("<", 0.0)) }) - It("does not report a sphere overlapping a mesh from behind", func() { - scene.CreateMesh(placement3d.MeshInfo[string]{ + It("does not report a sphere overlapping a terrain shape from behind", func() { + terrainID := scene.CreateTerrain(placement3d.TerrainInfo[string]{}) + scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), }) - _, ok := scene.CheckSphereIntersection( + _, ok := scene.CheckSphereTerrainIntersection( sphereAt(0.0, 0.5, 0.0, 1.0), - placement3d.Filter{}, + placement3d.FullMask, ) Expect(ok).To(BeFalse()) }) - It("skips dynamic shapes when SkipDynamic is set", func() { + It("keeps object and terrain queries separate", func() { objID := scene.CreateObject(placement3d.ObjectInfo[string]{}) scene.AttachSphere(objID, placement3d.SphereInfo[string]{ Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), }) - _, ok := scene.CheckSphereIntersection( - sphereAt(1.5, 0.0, 0.0, 1.0), - placement3d.Filter{SkipDynamic: true}, - ) - Expect(ok).To(BeFalse()) - }) - - It("skips static meshes when SkipStatic is set", func() { - scene.CreateMesh(placement3d.MeshInfo[string]{ - Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), - }) - - _, ok := scene.CheckSphereIntersection( - sphereAt(0.0, -0.5, 0.0, 1.0), - placement3d.Filter{SkipStatic: true}, + _, ok := scene.CheckSphereTerrainIntersection( + sphereAt(0.5, 0.0, 0.0, 1.0), + placement3d.FullMask, ) Expect(ok).To(BeFalse()) }) }) - Describe("CheckBoxIntersection", func() { - It("reports a box overlapping a scene shape", func() { + Describe("box queries", func() { + It("reports a box overlapping an object shape", func() { objID := scene.CreateObject(placement3d.ObjectInfo[string]{}) - scene.AttachSphere(objID, placement3d.SphereInfo[string]{ + shapeID := scene.AttachSphere(objID, placement3d.SphereInfo[string]{ Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), }) - contact, ok := scene.CheckBoxIntersection( + contact, ok := scene.CheckBoxObjectIntersection( boxAt(1.5, 0.0, 0.0, 1.0), - placement3d.Filter{}, + placement3d.FullMask, ) Expect(ok).To(BeTrue()) - Expect(contact.SourceShapeID).To(Equal(placement3d.InvalidShapeID)) - Expect(scene.GetShapeObject(contact.TargetShapeID)).To(Equal(objID)) - Expect(contact.TargetMeshID).To(Equal(placement3d.InvalidMeshID)) + Expect(contact.SourceShapeID).To(Equal(placement3d.NilObjectShapeID)) + Expect(contact.TargetObjectID).To(Equal(objID)) + Expect(contact.TargetShapeID).To(Equal(shapeID)) }) - It("returns false for a box disjoint from every shape", func() { + It("returns false for a box disjoint from every object shape", func() { objID := scene.CreateObject(placement3d.ObjectInfo[string]{}) scene.AttachSphere(objID, placement3d.SphereInfo[string]{ Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), }) - _, ok := scene.CheckBoxIntersection( + _, ok := scene.CheckBoxObjectIntersection( boxAt(10.0, 0.0, 0.0, 1.0), - placement3d.Filter{}, + placement3d.FullMask, ) Expect(ok).To(BeFalse()) }) - It("reports a box overlapping a mesh from the front", func() { - meshID := scene.CreateMesh(placement3d.MeshInfo[string]{ + It("reports a box overlapping a terrain shape from the front", func() { + terrainID := scene.CreateTerrain(placement3d.TerrainInfo[string]{}) + meshID := scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), }) // The plane faces -Y, so approach it from below (the front side). - contact, ok := scene.CheckBoxIntersection( + contact, ok := scene.CheckBoxTerrainIntersection( boxAt(0.0, -0.5, 0.0, 1.0), - placement3d.Filter{}, + placement3d.FullMask, ) Expect(ok).To(BeTrue()) - Expect(contact.TargetShapeID).To(Equal(placement3d.InvalidShapeID)) - Expect(contact.TargetMeshID).To(Equal(meshID)) + Expect(contact.TargetTerrainID).To(Equal(terrainID)) + Expect(contact.TargetShapeID).To(Equal(meshID)) Expect(contact.TargetNormal.Y).To(BeNumerically("<", 0.0)) }) - It("does not report a box overlapping a mesh from behind", func() { - scene.CreateMesh(placement3d.MeshInfo[string]{ + It("does not report a box overlapping a terrain shape from behind", func() { + terrainID := scene.CreateTerrain(placement3d.TerrainInfo[string]{}) + scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), }) - _, ok := scene.CheckBoxIntersection( + _, ok := scene.CheckBoxTerrainIntersection( boxAt(0.0, 0.5, 0.0, 1.0), - placement3d.Filter{}, - ) - Expect(ok).To(BeFalse()) - }) - - It("skips dynamic shapes when SkipDynamic is set", func() { - objID := scene.CreateObject(placement3d.ObjectInfo[string]{}) - scene.AttachSphere(objID, placement3d.SphereInfo[string]{ - Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), - }) - - _, ok := scene.CheckBoxIntersection( - boxAt(1.5, 0.0, 0.0, 1.0), - placement3d.Filter{SkipDynamic: true}, - ) - Expect(ok).To(BeFalse()) - }) - - It("skips static meshes when SkipStatic is set", func() { - scene.CreateMesh(placement3d.MeshInfo[string]{ - Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), - }) - - _, ok := scene.CheckBoxIntersection( - boxAt(0.0, -0.5, 0.0, 1.0), - placement3d.Filter{SkipStatic: true}, + placement3d.FullMask, ) Expect(ok).To(BeFalse()) }) }) - Describe("CollectSegmentIntersections", func() { - It("collects every shape a segment passes through", func() { + Describe("segment queries", func() { + It("collects every object shape a segment passes through", func() { near := scene.CreateObject(placement3d.ObjectInfo[string]{}) far := scene.CreateObject(placement3d.ObjectInfo[string]{}) scene.AttachSphere(near, placement3d.SphereInfo[string]{ @@ -644,98 +912,105 @@ var _ = Describe("Scene", func() { Sphere: sphereAt(4.0, 0.0, 0.0, 1.0), }) - var contacts placement3d.ContactList - scene.CollectSegmentIntersections( + var contacts placement3d.ObjectContactList + scene.CollectSegmentObjectIntersections( shape3d.NewSegment( dprec.NewVec3(-5.0, 0.0, 0.0), dprec.NewVec3(9.0, 0.0, 0.0), ), - placement3d.Filter{}, + placement3d.FullMask, contacts.AddContact, ) Expect(contacts).To(HaveLen(2)) }) - }) - Describe("CheckSegmentIntersection", func() { - It("finds a sphere crossed by the segment", func() { + It("finds an object shape crossed by the segment", func() { objID := scene.CreateObject(placement3d.ObjectInfo[string]{}) - scene.AttachSphere(objID, placement3d.SphereInfo[string]{ + shapeID := scene.AttachSphere(objID, placement3d.SphereInfo[string]{ Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), }) - contact, ok := scene.CheckSegmentIntersection( + contact, ok := scene.CheckSegmentObjectIntersection( shape3d.NewSegment( dprec.NewVec3(-5.0, 0.0, 0.0), dprec.NewVec3(5.0, 0.0, 0.0), ), - placement3d.Filter{}, + placement3d.FullMask, ) Expect(ok).To(BeTrue()) - Expect(contact.SourceShapeID).To(Equal(placement3d.InvalidShapeID)) - Expect(scene.GetShapeObject(contact.TargetShapeID)).To(Equal(objID)) + Expect(contact.SourceShapeID).To(Equal(placement3d.NilObjectShapeID)) + Expect(contact.TargetObjectID).To(Equal(objID)) + Expect(contact.TargetShapeID).To(Equal(shapeID)) }) - It("finds a mesh crossed by the segment", func() { - meshID := scene.CreateMesh(placement3d.MeshInfo[string]{ - Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), + It("finds the nearest of two object shapes crossed by the segment", func() { + near := scene.CreateObject(placement3d.ObjectInfo[string]{}) + far := scene.CreateObject(placement3d.ObjectInfo[string]{}) + nearShapeID := scene.AttachSphere(near, placement3d.SphereInfo[string]{ + Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), + }) + scene.AttachSphere(far, placement3d.SphereInfo[string]{ + Sphere: sphereAt(4.0, 0.0, 0.0, 1.0), }) - contact, ok := scene.CheckSegmentIntersection( + contact, ok := scene.CheckSegmentObjectIntersection( shape3d.NewSegment( - dprec.NewVec3(2.0, -5.0, 0.0), - dprec.NewVec3(2.0, 5.0, 0.0), + dprec.NewVec3(-5.0, 0.0, 0.0), + dprec.NewVec3(9.0, 0.0, 0.0), ), - placement3d.Filter{}, + placement3d.FullMask, ) Expect(ok).To(BeTrue()) - Expect(contact.TargetShapeID).To(Equal(placement3d.InvalidShapeID)) - Expect(contact.TargetMeshID).To(Equal(meshID)) + Expect(contact.TargetShapeID).To(Equal(nearShapeID)) }) - It("returns false when the segment misses everything", func() { - objID := scene.CreateObject(placement3d.ObjectInfo[string]{}) - scene.AttachSphere(objID, placement3d.SphereInfo[string]{ - Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), + It("finds a terrain shape crossed by the segment", func() { + terrainID := scene.CreateTerrain(placement3d.TerrainInfo[string]{}) + meshID := scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ + Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), }) - _, ok := scene.CheckSegmentIntersection( + contact, ok := scene.CheckSegmentTerrainIntersection( shape3d.NewSegment( - dprec.NewVec3(-5.0, 5.0, 0.0), - dprec.NewVec3(5.0, 5.0, 0.0), + dprec.NewVec3(2.0, -5.0, 0.0), + dprec.NewVec3(2.0, 5.0, 0.0), ), - placement3d.Filter{}, + placement3d.FullMask, ) - Expect(ok).To(BeFalse()) + Expect(ok).To(BeTrue()) + Expect(contact.SourceShapeID).To(Equal(placement3d.NilObjectShapeID)) + Expect(contact.TargetTerrainID).To(Equal(terrainID)) + Expect(contact.TargetShapeID).To(Equal(meshID)) }) - It("skips dynamic shapes when SkipDynamic is set", func() { + It("returns false when the segment misses everything", func() { objID := scene.CreateObject(placement3d.ObjectInfo[string]{}) scene.AttachSphere(objID, placement3d.SphereInfo[string]{ Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), }) - _, ok := scene.CheckSegmentIntersection( + _, ok := scene.CheckSegmentObjectIntersection( shape3d.NewSegment( - dprec.NewVec3(-5.0, 0.0, 0.0), - dprec.NewVec3(5.0, 0.0, 0.0), + dprec.NewVec3(-5.0, 5.0, 0.0), + dprec.NewVec3(5.0, 5.0, 0.0), ), - placement3d.Filter{SkipDynamic: true}, + placement3d.FullMask, ) Expect(ok).To(BeFalse()) }) - It("skips static meshes when SkipStatic is set", func() { - scene.CreateMesh(placement3d.MeshInfo[string]{ + It("keeps object and terrain queries separate", func() { + terrainID := scene.CreateTerrain(placement3d.TerrainInfo[string]{}) + scene.AttachMesh(terrainID, placement3d.MeshInfo[string]{ Mesh: planeMesh(0.0, 0.0, 0.0, 5.0), }) - _, ok := scene.CheckSegmentIntersection( + _, ok := scene.CheckSegmentObjectIntersection( shape3d.NewSegment( dprec.NewVec3(0.0, 5.0, 0.0), dprec.NewVec3(0.0, -5.0, 0.0), ), - placement3d.Filter{SkipStatic: true}, + placement3d.FullMask, ) Expect(ok).To(BeFalse()) }) diff --git a/core/spatial/placement3d/terrain.go b/core/spatial/placement3d/terrain.go new file mode 100644 index 00000000..f9962b40 --- /dev/null +++ b/core/spatial/placement3d/terrain.go @@ -0,0 +1,23 @@ +package placement3d + +// NilTerrainID indicates a terrain that can never be part of the scene. +const NilTerrainID = TerrainID(nilIndex) + +// TerrainID is a reference to a terrain in the scene. +type TerrainID int32 + +// TerrainInfo contains the information needed to create a terrain in a scene. +// +// Unlike an object, a terrain has no transform of its own. The shapes that are +// attached to it are specified directly in world space. +type TerrainInfo[T any] struct { + + // UserData allows one to attach custom user data to a terrain. + UserData T +} + +type terrainState[T any] struct { + firstShapeIndex int32 + lastShapeIndex int32 + userData T +} diff --git a/core/spatial/placement3d/terrain_shape.go b/core/spatial/placement3d/terrain_shape.go new file mode 100644 index 00000000..caa4fc38 --- /dev/null +++ b/core/spatial/placement3d/terrain_shape.go @@ -0,0 +1,69 @@ +package placement3d + +import ( + "github.com/mokiat/lacking/core/spatial/query3d" + "github.com/mokiat/lacking/core/spatial/shape3d" +) + +// NilTerrainShapeID indicates a terrain shape that can never be part of the +// scene. +const NilTerrainShapeID = TerrainShapeID(nilIndex) + +// TerrainShapeID is a reference to a concave shape that is attached to a +// terrain in the scene. +type TerrainShapeID int32 + +// MeshInfo contains the information needed to create a mesh shape. +type MeshInfo[S any] struct { + + // Filtering holds the collision-filtering metadata for the mesh. + Filtering FilterInfo + + // UserData allows one to attach custom user data to the mesh. + UserData S + + // Mesh contains the mesh information. + // + // The triangles of the mesh are specified in world space, since terrains + // have no transform of their own. Use [shape3d.TransformedMesh] to place a + // mesh that is modeled around the origin. + // + // The triangle slice is retained rather than copied, so it must not be + // modified afterwards. + // + // The mesh must have at least one triangle. An empty mesh has no area to + // be placed in the scene and attaching it panics. + Mesh shape3d.Mesh +} + +type terrainShapeState[S any] struct { + terrainIndex int32 + nextShapeIndex int32 + prevShapeIndex int32 + spatialID query3d.TreeItemID + filterRepresentation + terrainShapeRepresentation + userData S +} + +// objectTerrainShapesCanIntersect reports whether the specified object shape +// and terrain shape are allowed to be checked for intersection. +func objectTerrainShapesCanIntersect[S any](objectShape *objectShapeState[S], terrainShape *terrainShapeState[S]) bool { + return objectShape.canInteractWith(&terrainShape.filterRepresentation) +} + +// TODO: Consider using a different storage mechanism. For example an +// Octree or BVH structure. +// +// TODO: Consider abstracting the triangles through a resolver that can +// find candidate triangles for bounding spheres, allowing for heightmap +// or other implementations (types of shapes). + +// terrainShapeRepresentation holds the world-space geometry of a terrain +// shape. As terrains cannot be relocated, there is no local-space counterpart +// and the representation never needs to be updated after construction. +type terrainShapeRepresentation struct { + wsBSphere shape3d.Sphere + wsAABB shape3d.AABB + wsTriangles []shape3d.Triangle +} From ae1359f789fc683bc55362978db1da2191858a83 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 9 Aug 2026 17:32:48 +0300 Subject: [PATCH 68/85] Align placement2d API to placement3d --- core/spatial/placement2d/contact.go | 153 --- core/spatial/placement2d/contact_object.go | 156 +++ core/spatial/placement2d/contact_terrain.go | 160 +++ core/spatial/placement2d/doc.go | 34 +- core/spatial/placement2d/filter.go | 55 +- core/spatial/placement2d/mesh.go | 70 -- core/spatial/placement2d/object.go | 9 +- .../placement2d/{shape.go => object_shape.go} | 49 +- core/spatial/placement2d/scene.go | 911 +++++++++++------- core/spatial/placement2d/scene_test.go | 690 +++++++++---- core/spatial/placement2d/terrain.go | 23 + core/spatial/placement2d/terrain_shape.go | 69 ++ 12 files changed, 1554 insertions(+), 825 deletions(-) delete mode 100644 core/spatial/placement2d/contact.go create mode 100644 core/spatial/placement2d/contact_object.go create mode 100644 core/spatial/placement2d/contact_terrain.go delete mode 100644 core/spatial/placement2d/mesh.go rename core/spatial/placement2d/{shape.go => object_shape.go} (61%) create mode 100644 core/spatial/placement2d/terrain.go create mode 100644 core/spatial/placement2d/terrain_shape.go diff --git a/core/spatial/placement2d/contact.go b/core/spatial/placement2d/contact.go deleted file mode 100644 index 8de25492..00000000 --- a/core/spatial/placement2d/contact.go +++ /dev/null @@ -1,153 +0,0 @@ -package placement2d - -import "github.com/mokiat/lacking/core/spatial/shape2d" - -// Contact describes the intersection of a source shape with a target shape. -// -// Its fields are expressed relative to the target shape. The equivalent values -// for the source shape can be derived via [shape2d.Contact.EvalSourcePoint] and -// [shape2d.Contact.EvalSourceNormal]. -type Contact struct { - - // SourceShapeID contains the ID of the shape from the first involved object. - // - // This ID is equal to [InvalidShapeID] if the check was not performed with - // a scene object. - SourceShapeID ShapeID - - // TargetShapeID contains the ID of the shape from the second involved object. - // - // This ID is equal to [InvalidShapeID] when the target of the intersection - // was a mesh, in which case [Contact.TargetMeshID] identifies it instead. - TargetShapeID ShapeID - - // TargetMeshID contains the ID of the mesh that was intersected. - // - // This ID is equal to [InvalidMeshID] when the target of the intersection - // was a shape rather than a mesh. - TargetMeshID MeshID - - // Contact holds the underlying raw shape intersection. - shape2d.Contact -} - -// ContactCallback is invoked for each [Contact] discovered while testing shapes -// for intersection. -type ContactCallback func(contact Contact) - -// LastContact is a contact sink that retains the most recently added [Contact]. -// -// Its AddContact method satisfies [ContactCallback] and can be passed directly to -// intersection routines. -type LastContact struct { - contact Contact - hasContact bool -} - -// Reset clears any retained contact. -func (c *LastContact) Reset() { - c.hasContact = false -} - -// AddContact retains the given contact, replacing any previously retained one. -func (c *LastContact) AddContact(contact Contact) { - c.contact = contact - c.hasContact = true -} - -// Contact returns the retained contact and whether one was added since the last -// Reset. -func (c *LastContact) Contact() (Contact, bool) { - return c.contact, c.hasContact -} - -// DeepestContact is a contact sink that retains the added [Contact] with the -// greatest Depth. -// -// Its AddContact method satisfies [ContactCallback] and can be passed directly to -// intersection routines. -type DeepestContact struct { - contact Contact - hasContact bool -} - -// Reset clears any retained contact. -func (c *DeepestContact) Reset() { - c.hasContact = false -} - -// AddContact retains the given contact if it is deeper than any previously -// retained one. -func (c *DeepestContact) AddContact(contact Contact) { - if !c.hasContact || contact.Depth > c.contact.Depth { - c.contact = contact - c.hasContact = true - } -} - -// Contact returns the deepest retained contact and whether one was added since -// the last Reset. -func (c *DeepestContact) Contact() (Contact, bool) { - return c.contact, c.hasContact -} - -// ShallowestContact is a contact sink that retains the added [Contact] with the -// smallest Depth. -// -// Its AddContact method satisfies [ContactCallback] and can be passed directly to -// intersection routines. -type ShallowestContact struct { - contact Contact - hasContact bool -} - -// Reset clears any retained contact. -func (c *ShallowestContact) Reset() { - c.hasContact = false -} - -// AddContact retains the given contact if it is shallower than any previously -// retained one. -func (c *ShallowestContact) AddContact(contact Contact) { - if !c.hasContact || contact.Depth < c.contact.Depth { - c.contact = contact - c.hasContact = true - } -} - -// Contact returns the shallowest retained contact and whether one was added -// since the last Reset. -func (c *ShallowestContact) Contact() (Contact, bool) { - return c.contact, c.hasContact -} - -// ContactList is a contact sink that retains every added [Contact] in the order -// it was added. -// -// Its AddContact method satisfies [ContactCallback] and can be passed directly to -// intersection routines. As it is itself a slice, the retained contacts can be -// ranged over directly. -// -// Use make(ContactList, 0, n) to pre-size it and avoid reallocations as -// contacts are added. With a constant n that does not escape, the compiler can -// keep the backing array on the stack. -type ContactList []Contact - -// Reset clears the retained contacts while preserving the underlying capacity -// so it can be reused without reallocating. -func (l *ContactList) Reset() { - *l = (*l)[:0] -} - -// AddContact appends the given contact to the list. -func (l *ContactList) AddContact(contact Contact) { - *l = append(*l, contact) -} - -// Contacts returns the retained contacts in the order they were added. -// -// The result aliases the internal storage and remains valid until the next -// AddContact or Reset call. -func (l ContactList) Contacts() []Contact { - return l -} diff --git a/core/spatial/placement2d/contact_object.go b/core/spatial/placement2d/contact_object.go new file mode 100644 index 00000000..e95ddf98 --- /dev/null +++ b/core/spatial/placement2d/contact_object.go @@ -0,0 +1,156 @@ +package placement2d + +import "github.com/mokiat/lacking/core/spatial/shape2d" + +// ObjectContact describes the intersection of a source shape with an object +// shape. +// +// Its fields are expressed relative to the target shape. The equivalent values +// for the source shape can be derived via [shape2d.Contact.EvalSourcePoint] and +// [shape2d.Contact.EvalSourceNormal]. +type ObjectContact struct { + + // SourceObjectID contains the ID of the object that owns the source shape. + // + // This ID is equal to [NilObjectID] when the intersection was produced by a + // query primitive rather than by a shape in the scene. + SourceObjectID ObjectID + + // SourceShapeID contains the ID of the shape that acted as the source of + // the intersection. + // + // This ID is equal to [NilObjectShapeID] when the intersection was produced + // by a query primitive rather than by a shape in the scene. + SourceShapeID ObjectShapeID + + // TargetObjectID contains the ID of the object that owns the target shape. + TargetObjectID ObjectID + + // TargetShapeID contains the ID of the shape that was intersected. + TargetShapeID ObjectShapeID + + // Contact holds the underlying raw shape intersection. + shape2d.Contact +} + +// ObjectContactCallback is invoked for each [ObjectContact] discovered while +// testing shapes for intersection. +type ObjectContactCallback func(contact ObjectContact) + +// DeepestObjectContact is a contact sink that retains the added +// [ObjectContact] with the greatest Depth. +// +// Its AddContact method satisfies [ObjectContactCallback] and can be passed +// directly to intersection routines. +type DeepestObjectContact struct { + contact ObjectContact + hasContact bool +} + +// Reset clears any retained contact. +func (c *DeepestObjectContact) Reset() { + c.hasContact = false +} + +// AddContact retains the given contact if it is deeper than any previously +// retained one. +func (c *DeepestObjectContact) AddContact(contact ObjectContact) { + if !c.hasContact || contact.Depth > c.contact.Depth { + c.contact = contact + c.hasContact = true + } +} + +// Contact returns the deepest retained contact and whether one was added since +// the last Reset. +func (c *DeepestObjectContact) Contact() (ObjectContact, bool) { + return c.contact, c.hasContact +} + +// ShallowestObjectContact is a contact sink that retains the added +// [ObjectContact] with the smallest Depth. +// +// Its AddContact method satisfies [ObjectContactCallback] and can be passed +// directly to intersection routines. +type ShallowestObjectContact struct { + contact ObjectContact + hasContact bool +} + +// Reset clears any retained contact. +func (c *ShallowestObjectContact) Reset() { + c.hasContact = false +} + +// AddContact retains the given contact if it is shallower than any previously +// retained one. +func (c *ShallowestObjectContact) AddContact(contact ObjectContact) { + if !c.hasContact || contact.Depth < c.contact.Depth { + c.contact = contact + c.hasContact = true + } +} + +// Contact returns the shallowest retained contact and whether one was added +// since the last Reset. +func (c *ShallowestObjectContact) Contact() (ObjectContact, bool) { + return c.contact, c.hasContact +} + +// ObjectContactList is a contact sink that retains every added [ObjectContact] +// in the order it was added. +// +// Its AddContact method satisfies [ObjectContactCallback] and can be passed +// directly to intersection routines. As it is itself a slice, the retained +// contacts can be ranged over directly. +// +// Use make(ObjectContactList, 0, n) to pre-size it and avoid reallocations as +// contacts are added. With a constant n that does not escape, the compiler can +// keep the backing array on the stack. +type ObjectContactList []ObjectContact + +// Reset clears the retained contacts while preserving the underlying capacity +// so it can be reused without reallocating. +func (l *ObjectContactList) Reset() { + *l = (*l)[:0] +} + +// AddContact appends the given contact to the list. +func (l *ObjectContactList) AddContact(contact ObjectContact) { + *l = append(*l, contact) +} + +// Contacts returns the retained contacts in the order they were added. +// +// The result aliases the internal storage and remains valid until the next +// AddContact or Reset call. +func (l ObjectContactList) Contacts() []ObjectContact { + return l +} + +// LastObjectContact is a contact sink that retains the most recently added +// [ObjectContact]. +// +// Its AddContact method satisfies [ObjectContactCallback] and can be passed +// directly to intersection routines. +type LastObjectContact struct { + contact ObjectContact + hasContact bool +} + +// Reset clears any retained contact. +func (c *LastObjectContact) Reset() { + c.hasContact = false +} + +// AddContact retains the given contact, replacing any previously retained one. +func (c *LastObjectContact) AddContact(contact ObjectContact) { + c.contact = contact + c.hasContact = true +} + +// Contact returns the retained contact and whether one was added since the +// last Reset. +func (c *LastObjectContact) Contact() (ObjectContact, bool) { + return c.contact, c.hasContact +} diff --git a/core/spatial/placement2d/contact_terrain.go b/core/spatial/placement2d/contact_terrain.go new file mode 100644 index 00000000..060bc56d --- /dev/null +++ b/core/spatial/placement2d/contact_terrain.go @@ -0,0 +1,160 @@ +package placement2d + +import "github.com/mokiat/lacking/core/spatial/shape2d" + +// TerrainContact describes the intersection of a source shape with a terrain +// shape. +// +// Its fields are expressed relative to the target shape. The equivalent values +// for the source shape can be derived via [shape2d.Contact.EvalSourcePoint] and +// [shape2d.Contact.EvalSourceNormal]. +type TerrainContact struct { + + // SourceObjectID contains the ID of the object that owns the source shape. + // + // This ID is equal to [NilObjectID] when the intersection was produced by a + // query primitive rather than by a shape in the scene. + SourceObjectID ObjectID + + // SourceShapeID contains the ID of the shape that acted as the source of + // the intersection. + // + // This ID is equal to [NilObjectShapeID] when the intersection was produced + // by a query primitive rather than by a shape in the scene. + // + // The source of a terrain contact is always an object shape, since terrain + // shapes are never tested against one another. + SourceShapeID ObjectShapeID + + // TargetTerrainID contains the ID of the terrain that owns the target + // shape. + TargetTerrainID TerrainID + + // TargetShapeID contains the ID of the shape that was intersected. + TargetShapeID TerrainShapeID + + // Contact holds the underlying raw shape intersection. + shape2d.Contact +} + +// TerrainContactCallback is invoked for each [TerrainContact] discovered while +// testing shapes for intersection. +type TerrainContactCallback func(contact TerrainContact) + +// DeepestTerrainContact is a contact sink that retains the added +// [TerrainContact] with the greatest Depth. +// +// Its AddContact method satisfies [TerrainContactCallback] and can be passed +// directly to intersection routines. +type DeepestTerrainContact struct { + contact TerrainContact + hasContact bool +} + +// Reset clears any retained contact. +func (c *DeepestTerrainContact) Reset() { + c.hasContact = false +} + +// AddContact retains the given contact if it is deeper than any previously +// retained one. +func (c *DeepestTerrainContact) AddContact(contact TerrainContact) { + if !c.hasContact || contact.Depth > c.contact.Depth { + c.contact = contact + c.hasContact = true + } +} + +// Contact returns the deepest retained contact and whether one was added since +// the last Reset. +func (c *DeepestTerrainContact) Contact() (TerrainContact, bool) { + return c.contact, c.hasContact +} + +// ShallowestTerrainContact is a contact sink that retains the added +// [TerrainContact] with the smallest Depth. +// +// Its AddContact method satisfies [TerrainContactCallback] and can be passed +// directly to intersection routines. +type ShallowestTerrainContact struct { + contact TerrainContact + hasContact bool +} + +// Reset clears any retained contact. +func (c *ShallowestTerrainContact) Reset() { + c.hasContact = false +} + +// AddContact retains the given contact if it is shallower than any previously +// retained one. +func (c *ShallowestTerrainContact) AddContact(contact TerrainContact) { + if !c.hasContact || contact.Depth < c.contact.Depth { + c.contact = contact + c.hasContact = true + } +} + +// Contact returns the shallowest retained contact and whether one was added +// since the last Reset. +func (c *ShallowestTerrainContact) Contact() (TerrainContact, bool) { + return c.contact, c.hasContact +} + +// TerrainContactList is a contact sink that retains every added +// [TerrainContact] in the order it was added. +// +// Its AddContact method satisfies [TerrainContactCallback] and can be passed +// directly to intersection routines. As it is itself a slice, the retained +// contacts can be ranged over directly. +// +// Use make(TerrainContactList, 0, n) to pre-size it and avoid reallocations as +// contacts are added. With a constant n that does not escape, the compiler can +// keep the backing array on the stack. +type TerrainContactList []TerrainContact + +// Reset clears the retained contacts while preserving the underlying capacity +// so it can be reused without reallocating. +func (l *TerrainContactList) Reset() { + *l = (*l)[:0] +} + +// AddContact appends the given contact to the list. +func (l *TerrainContactList) AddContact(contact TerrainContact) { + *l = append(*l, contact) +} + +// Contacts returns the retained contacts in the order they were added. +// +// The result aliases the internal storage and remains valid until the next +// AddContact or Reset call. +func (l TerrainContactList) Contacts() []TerrainContact { + return l +} + +// LastTerrainContact is a contact sink that retains the most recently added +// [TerrainContact]. +// +// Its AddContact method satisfies [TerrainContactCallback] and can be passed +// directly to intersection routines. +type LastTerrainContact struct { + contact TerrainContact + hasContact bool +} + +// Reset clears any retained contact. +func (c *LastTerrainContact) Reset() { + c.hasContact = false +} + +// AddContact retains the given contact, replacing any previously retained one. +func (c *LastTerrainContact) AddContact(contact TerrainContact) { + c.contact = contact + c.hasContact = true +} + +// Contact returns the retained contact and whether one was added since the +// last Reset. +func (c *LastTerrainContact) Contact() (TerrainContact, bool) { + return c.contact, c.hasContact +} diff --git a/core/spatial/placement2d/doc.go b/core/spatial/placement2d/doc.go index 8c917046..fc7b3abb 100644 --- a/core/spatial/placement2d/doc.go +++ b/core/spatial/placement2d/doc.go @@ -1,12 +1,28 @@ -// Package placement2d provides a 2D scene in which objects, built from convex -// shapes, and static meshes can be placed and tested for intersection. +// Package placement2d provides a 2D scene in which objects and terrains can be +// placed and tested for intersection. // -// Objects are dynamic entities that own one or more convex shapes (circles and -// rectangles). Meshes are static entities made of edges. Both are indexed in -// separate quadtrees for efficient broad-phase queries, and narrow-phase -// intersection is resolved via GJK/EPA (see [github.com/mokiat/lacking/core/spatial/gjk2d]). +// An object is a movable entity that owns one or more convex shapes (see +// [Scene.AttachCircle] and [Scene.AttachRectangle]). Moving the object through +// [Scene.SetObjectTransform] moves all of its shapes along with it. // -// Intersections are reported as [Contact] values through a [ContactCallback]. -// A number of contact sinks (for example [DeepestContact] and [ContactList]) -// are provided for common accumulation strategies. +// A terrain is an immovable entity that owns one or more concave shapes (see +// [Scene.AttachMesh]). Terrain shapes are specified directly in world space and +// cannot be relocated once attached. +// +// Object shapes and terrain shapes are indexed in separate quadtrees for +// efficient broad-phase queries. Narrow-phase intersection is resolved via +// GJK/EPA (see [github.com/mokiat/lacking/core/spatial/gjk2d]), with concave +// shapes being decomposed into convex pieces beforehand. +// +// Intersections come in two flavors, which are reported through separate +// methods so that callers never need to branch on the kind of the target. An +// intersection with an object shape is reported as an [ObjectContact] through +// an [ObjectContactCallback], whereas an intersection with a terrain shape is +// reported as a [TerrainContact] through a [TerrainContactCallback]. A number +// of contact sinks (for example [DeepestObjectContact] and +// [TerrainContactList]) are provided for common accumulation strategies. +// +// Since terrains cannot move, terrain shapes are never tested against one +// another. The source of a [TerrainContact] is always either an object shape +// or a query primitive. package placement2d diff --git a/core/spatial/placement2d/filter.go b/core/spatial/placement2d/filter.go index 94800c72..9039ae80 100644 --- a/core/spatial/placement2d/filter.go +++ b/core/spatial/placement2d/filter.go @@ -2,38 +2,40 @@ package placement2d import "github.com/mokiat/gog/opt" -// Filter represents a set of criteria to filter 2D shapes in a scene. -type Filter struct { - - // Mask is a bitmask used to filter shapes based on their assigned layers. - Mask opt.T[uint32] - - // SkipDynamic indicates whether dynamic shapes should be excluded from the - // results. - SkipDynamic bool +// Mask is a bitmask over the layers that a shape can occupy. Queries use it to +// narrow down the shapes that they consider. +// +// A shape is considered by a query when at least one bit is set in both the +// mask of the query and the [FilterInfo.SourceMask] of the shape. Note that +// this means that the zero value matches no shape at all. Use [FullMask] to +// consider every shape in the scene. +type Mask = uint32 - // SkipStatic indicates whether static shapes should be excluded from the - // results. - SkipStatic bool -} +// FullMask is a [Mask] with all layer bits set. A query that uses it considers +// every shape in the scene, regardless of the layers that the shape occupies. +const FullMask Mask = 0xFFFFFFFF -// FilterInfo holds the collision-filtering metadata common to every entity -// that can be placed in a scene, whether a shape (see [CircleInfo] and -// [RectangleInfo]) or a mesh (see [MeshInfo]). +// FilterInfo holds the collision-filtering metadata common to every shape that +// can be placed in a scene, whether an object shape (see [CircleInfo] and +// [RectangleInfo]) or a terrain shape (see [MeshInfo]). // -// Its fields determine which entities are tested against one another during +// Its fields determine which shapes are tested against one another during // intersection queries. type FilterInfo struct { // RejectGroup becomes active if a value larger than zero is specified. - // Entities that share the same reject group are not checked for + // Shapes that share the same reject group are not checked for // intersection. RejectGroup uint32 - // SourceMask specifies the layers in which this entity is positioned. + // SourceMask specifies the layers in which this shape is positioned. + // + // Defaults to the first layer only. SourceMask opt.T[uint32] - // TargetMask specifies the layers with which this entity can intersect. + // TargetMask specifies the layers with which this shape can intersect. + // + // Defaults to the first layer only. TargetMask opt.T[uint32] } @@ -51,15 +53,14 @@ func newFilterRepresentation(info FilterInfo) filterRepresentation { } } -func (s *filterRepresentation) matchesFilter(filter Filter) bool { - if mask, ok := filter.Mask.Unwrap(); ok { - if (s.sourceMask & mask) == 0 { - return false - } - } - return true +// satisfiesMask reports whether this shape occupies at least one of the layers +// covered by the specified query mask. +func (s *filterRepresentation) satisfiesMask(mask Mask) bool { + return (s.sourceMask & mask) != 0 } +// canInteractWith reports whether this shape and the specified one are allowed +// to be checked for intersection. func (s *filterRepresentation) canInteractWith(other *filterRepresentation) bool { if s.rejectGroup != 0 && (s.rejectGroup == other.rejectGroup) { return false diff --git a/core/spatial/placement2d/mesh.go b/core/spatial/placement2d/mesh.go deleted file mode 100644 index 784b8208..00000000 --- a/core/spatial/placement2d/mesh.go +++ /dev/null @@ -1,70 +0,0 @@ -package placement2d - -import ( - "github.com/mokiat/gog/opt" - "github.com/mokiat/gomath/dprec" - "github.com/mokiat/lacking/core/spatial/query2d" - "github.com/mokiat/lacking/core/spatial/shape2d" -) - -// InvalidMeshID indicates a mesh that can never be part of the scene. -const InvalidMeshID = MeshID(nilIndex) - -// MeshID is a reference to a mesh in the scene. -type MeshID int32 - -// MeshInfo contains the information needed to create a mesh shape. -type MeshInfo[M any] struct { - - // Position optionally specifies a position where the mesh should be placed. - // - // Defaults to the origin. - Position opt.T[dprec.Vec2] - - // Rotation optionally specifies a rotation of the mesh. - // - // Defaults to the identity rotation. - Rotation opt.T[dprec.Angle] - - // Filtering holds the collision-filtering metadata for the mesh. - Filtering FilterInfo - - // UserData allows one to attach custom user data to the mesh. - UserData M - - // Mesh contains the mesh information. - // - // The mesh must have at least one edge. An empty mesh has no area to be - // placed in the scene and is considered invalid. - Mesh shape2d.Mesh -} - -type meshShape[M any] struct { - spatialID query2d.TreeItemID - filterRepresentation - meshRepresentation - userData M -} - -func shapeMeshCanIntersect[S, M any](shape *shape[S], mesh *meshShape[M]) bool { - return shape.canInteractWith(&mesh.filterRepresentation) -} - -// TODO: Consider using a different storage mechanism. For example a -// Quadtree or BVH structure. -// Alternatively experiment with placing each mesh edge in the existing -// mesh tree, through this will likely destroy the mesh tree performance. - -type meshRepresentation struct { - wsBCircle shape2d.Circle - wsAABB shape2d.AABB - wsEdges []shape2d.Edge -} - -func newMeshRepresentation(mesh shape2d.Mesh) meshRepresentation { - return meshRepresentation{ - wsBCircle: mesh.BoundingCircle(), - wsAABB: mesh.BoundingAABB(), - wsEdges: mesh.Edges, - } -} diff --git a/core/spatial/placement2d/object.go b/core/spatial/placement2d/object.go index 5fcb2ba0..3016517b 100644 --- a/core/spatial/placement2d/object.go +++ b/core/spatial/placement2d/object.go @@ -6,8 +6,11 @@ import ( "github.com/mokiat/lacking/core/spatial/shape2d" ) -// InvalidObjectID indicates an object that can never be part of the scene. -const InvalidObjectID = ObjectID(nilIndex) +// NilObjectID indicates an object that can never be part of the scene. +// +// It is also used to denote the absence of a source object in contacts that +// were produced by a query primitive rather than by a scene shape. +const NilObjectID = ObjectID(nilIndex) // ObjectID is a reference to an object in the scene. type ObjectID int32 @@ -30,7 +33,7 @@ type ObjectInfo[O any] struct { UserData O } -type sceneObject[O any] struct { +type objectState[O any] struct { transform shape2d.Transform firstShapeIndex int32 lastShapeIndex int32 diff --git a/core/spatial/placement2d/shape.go b/core/spatial/placement2d/object_shape.go similarity index 61% rename from core/spatial/placement2d/shape.go rename to core/spatial/placement2d/object_shape.go index 01576cc4..506a54ac 100644 --- a/core/spatial/placement2d/shape.go +++ b/core/spatial/placement2d/object_shape.go @@ -7,11 +7,16 @@ import ( "github.com/mokiat/lacking/core/spatial/shape2d" ) -// InvalidShapeID indicates a shape that can never be part of the scene. -const InvalidShapeID = ShapeID(nilIndex) +// NilObjectShapeID indicates an object shape that can never be part of the +// scene. +// +// It is also used to denote the absence of a source shape in contacts that +// were produced by a query primitive rather than by a scene shape. +const NilObjectShapeID = ObjectShapeID(nilIndex) -// ShapeID is a reference to a shape in the scene. -type ShapeID int32 +// ObjectShapeID is a reference to a convex shape that is attached to an object +// in the scene. +type ObjectShapeID int32 // CircleInfo contains the information needed to create a circle shape. type CircleInfo[S any] struct { @@ -23,6 +28,9 @@ type CircleInfo[S any] struct { UserData S // Circle contains the circle information. + // + // It is specified in the local space of the object that the shape is + // attached to. Circle shape2d.Circle } @@ -36,39 +44,44 @@ type RectangleInfo[S any] struct { UserData S // Rectangle contains the rectangle information. + // + // It is specified in the local space of the object that the shape is + // attached to. Rectangle shape2d.Rectangle } -type shape[S any] struct { +type objectShapeState[S any] struct { objectIndex int32 nextShapeIndex int32 prevShapeIndex int32 spatialID query2d.TreeItemID filterRepresentation - shapeRepresentation + objectShapeRepresentation userData S } -func shapesCanIntersect[S any](a, b *shape[S]) bool { +// objectShapesCanIntersect reports whether the specified two object shapes are +// allowed to be checked for intersection. +func objectShapesCanIntersect[S any](a, b *objectShapeState[S]) bool { if a.objectIndex >= b.objectIndex { return false // prevent self-intersection and repeated checks } return a.filterRepresentation.canInteractWith(&b.filterRepresentation) } -type shapeRepresentation struct { +type objectShapeRepresentation struct { lsBCircle shape2d.Circle wsBCircle shape2d.Circle lsTransform shape2d.Transform wsTransform shape2d.Transform - kind shapeKind + kind objectShapeKind points []dprec.Vec2 skinRadius float64 } -func (s *shapeRepresentation) update(parentTransform shape2d.Transform) { +func (s *objectShapeRepresentation) update(parentTransform shape2d.Transform) { s.wsBCircle = shape2d.TransformedCircle(s.lsBCircle, parentTransform) s.wsTransform = shape2d.ChainedTransform( @@ -77,7 +90,7 @@ func (s *shapeRepresentation) update(parentTransform shape2d.Transform) { ) } -func (s *shapeRepresentation) gjkShape() gjk2d.Shape { +func (s *objectShapeRepresentation) gjkShape() gjk2d.Shape { return gjk2d.Shape{ Position: s.wsTransform.Translation, Rotation: s.wsTransform.Rotation, @@ -86,14 +99,14 @@ func (s *shapeRepresentation) gjkShape() gjk2d.Shape { } } -func (s *shapeRepresentation) toCircle() shape2d.Circle { +func (s *objectShapeRepresentation) toCircle() shape2d.Circle { return shape2d.Circle{ Center: s.wsTransform.Translation, Radius: s.skinRadius, } } -func (s *shapeRepresentation) toRectangle() shape2d.Rectangle { +func (s *objectShapeRepresentation) toRectangle() shape2d.Rectangle { var halfWidth, halfHeight float64 for _, point := range s.points { halfWidth = max(halfWidth, point.X) @@ -107,11 +120,11 @@ func (s *shapeRepresentation) toRectangle() shape2d.Rectangle { } } -type shapeKind uint32 +type objectShapeKind uint32 const ( - shapeKindCircle shapeKind = iota - shapeKindRectangle - shapeKindCapsule - shapeKindConvexHull + objectShapeKindCircle objectShapeKind = iota + objectShapeKindRectangle + objectShapeKindCapsule + objectShapeKindConvexHull ) diff --git a/core/spatial/placement2d/scene.go b/core/spatial/placement2d/scene.go index ce691acb..088c25e4 100644 --- a/core/spatial/placement2d/scene.go +++ b/core/spatial/placement2d/scene.go @@ -34,52 +34,61 @@ type SceneSettings struct { InitialItemCapacity opt.T[uint32] } -// Scene represents a 2D scene into which dynamic objects (built from convex -// shapes) and static meshes can be placed and tested for intersection. +// Scene represents a 2D scene into which movable objects (built from convex +// shapes) and immovable terrains (built from concave shapes) can be placed and +// tested for intersection. // // The type parameters specify the user data attached to each kind of entity: -// O for objects, S for shapes, and M for meshes. -type Scene[O, S, M any] struct { - shapeTree *query2d.Quadtree[int32] - meshTree *query2d.Quadtree[int32] - +// O for objects, T for terrains, and S for the shapes of both. +// +// A scene is not safe for concurrent use. Furthermore, the intersection +// queries share internal scratch buffers, so a query must not be started from +// within the callback of another query. +type Scene[O, T, S any] struct { solver *gjk2d.Solver - freeObjectIndices *ds.Stack[int32] - freeShapeIndices *ds.Stack[int32] - freeMeshIndices *ds.Stack[int32] + objectShapeTree *query2d.Quadtree[int32] + terrainShapeTree *query2d.Quadtree[int32] + + freeObjectIndices *ds.Stack[int32] + freeObjectShapeIndices *ds.Stack[int32] + freeTerrainIndices *ds.Stack[int32] + freeTerrainShapeIndices *ds.Stack[int32] - objects []sceneObject[O] - shapes []shape[S] - meshes []meshShape[M] + objects []objectState[O] + objectShapes []objectShapeState[S] + terrains []terrainState[T] + terrainShapes []terrainShapeState[S] - shapeCandidates []int32 - meshCandidates []int32 + objectShapeCandidates []int32 + terrainShapeCandidates []int32 tempGJKSource gjk2d.Shape tempGJKTarget gjk2d.Shape } // NewScene creates a new scene. -func NewScene[O, S, M any](settings SceneSettings) *Scene[O, S, M] { +func NewScene[O, T, S any](settings SceneSettings) *Scene[O, T, S] { treeSettings := query2d.QuadtreeSettings(settings) - return &Scene[O, S, M]{ - shapeTree: query2d.NewQuadtree[int32](treeSettings), - meshTree: query2d.NewQuadtree[int32](treeSettings), - + return &Scene[O, T, S]{ solver: gjk2d.NewSolver(), - freeObjectIndices: ds.EmptyStack[int32](), - freeShapeIndices: ds.EmptyStack[int32](), - freeMeshIndices: ds.EmptyStack[int32](), + objectShapeTree: query2d.NewQuadtree[int32](treeSettings), + terrainShapeTree: query2d.NewQuadtree[int32](treeSettings), + + freeObjectIndices: ds.EmptyStack[int32](), + freeObjectShapeIndices: ds.EmptyStack[int32](), + freeTerrainIndices: ds.EmptyStack[int32](), + freeTerrainShapeIndices: ds.EmptyStack[int32](), - objects: make([]sceneObject[O], 0), - shapes: make([]shape[S], 0), - meshes: make([]meshShape[M], 0), + objects: make([]objectState[O], 0), + objectShapes: make([]objectShapeState[S], 0), + terrains: make([]terrainState[T], 0), + terrainShapes: make([]terrainShapeState[S], 0), - shapeCandidates: make([]int32, 0), - meshCandidates: make([]int32, 0), + objectShapeCandidates: make([]int32, 0), + terrainShapeCandidates: make([]int32, 0), tempGJKSource: gjk2d.Shape{ Points: make([]dprec.Vec2, 0, 4), @@ -91,7 +100,7 @@ func NewScene[O, S, M any](settings SceneSettings) *Scene[O, S, M] { } // CreateObject creates a new object. -func (s *Scene[O, S, M]) CreateObject(info ObjectInfo[O]) ObjectID { +func (s *Scene[O, T, S]) CreateObject(info ObjectInfo[O]) ObjectID { transform := shape2d.Transform{ Translation: info.Position.ValueOrDefault(dprec.ZeroVec2()), Rotation: shape2d.RotationFromAngle( @@ -100,7 +109,7 @@ func (s *Scene[O, S, M]) CreateObject(info ObjectInfo[O]) ObjectID { } index := s.allocateObject() - s.objects[index] = sceneObject[O]{ + s.objects[index] = objectState[O]{ transform: transform, firstShapeIndex: nilIndex, lastShapeIndex: nilIndex, @@ -110,69 +119,78 @@ func (s *Scene[O, S, M]) CreateObject(info ObjectInfo[O]) ObjectID { } // DeleteObject deletes an object. -func (s *Scene[O, S, M]) DeleteObject(objID ObjectID) { +func (s *Scene[O, T, S]) DeleteObject(objID ObjectID) { index := int32(objID) object := &s.objects[index] object.userData = gog.Zero[O]() // in case of pointer - s.eachObjectShape(object, func(shapeIndex int32, _ *shape[S]) { - s.detachShape(shapeIndex) + s.eachObjectShape(object, func(shapeIndex int32, _ *objectShapeState[S]) { + s.detachObjectShape(shapeIndex) }) s.releaseObject(index) } // GetObjectUserData returns the user data associated with the given object. -func (s *Scene[O, S, M]) GetObjectUserData(objID ObjectID) O { - object := &s.objects[objID] +func (s *Scene[O, T, S]) GetObjectUserData(objID ObjectID) O { + index := int32(objID) + object := &s.objects[index] return object.userData } // SetObjectUserData assigns the specified user data to the object. -func (s *Scene[O, S, M]) SetObjectUserData(objID ObjectID, userData O) { - object := &s.objects[objID] +func (s *Scene[O, T, S]) SetObjectUserData(objID ObjectID, userData O) { + index := int32(objID) + object := &s.objects[index] object.userData = userData } // GetObjectTransform returns the given object's transform. -func (s *Scene[O, S, M]) GetObjectTransform(objID ObjectID) shape2d.Transform { - object := &s.objects[objID] +func (s *Scene[O, T, S]) GetObjectTransform(objID ObjectID) shape2d.Transform { + index := int32(objID) + object := &s.objects[index] return object.transform } // SetObjectTransform relocates the given object. -func (s *Scene[O, S, M]) SetObjectTransform(objID ObjectID, transform shape2d.Transform) { - object := &s.objects[objID] +func (s *Scene[O, T, S]) SetObjectTransform(objID ObjectID, transform shape2d.Transform) { + index := int32(objID) + object := &s.objects[index] object.transform = transform - s.eachObjectShape(object, func(_ int32, shape *shape[S]) { + s.eachObjectShape(object, func(_ int32, shape *objectShapeState[S]) { shape.update(transform) area := shape2d.AABBFromCircle(shape.wsBCircle) - s.shapeTree.Update(shape.spatialID, area) + s.objectShapeTree.Update(shape.spatialID, area) }) } -// GetShapeObject returns the ID of the object that the given shape is +// GetObjectForShape returns the ID of the object that the given shape is // attached to. -func (s *Scene[O, S, M]) GetShapeObject(shapeID ShapeID) ObjectID { +func (s *Scene[O, T, S]) GetObjectForShape(shapeID ObjectShapeID) ObjectID { index := int32(shapeID) - shape := &s.shapes[index] + shape := &s.objectShapes[index] return ObjectID(shape.objectIndex) } // AttachCircle creates a circle shape and attaches it to the object to be // used for intersection tests. -func (s *Scene[O, S, M]) AttachCircle(objID ObjectID, info CircleInfo[S]) ShapeID { +// +// The circle is specified in the local space of the object and moves along +// with it. +func (s *Scene[O, T, S]) AttachCircle(objID ObjectID, info CircleInfo[S]) ObjectShapeID { + index := int32(objID) + circle := info.Circle transform := shape2d.Transform{ Translation: circle.Center, Rotation: shape2d.IdentityRotation(), } - return s.attachShape(int32(objID), info.Filtering, shapeRepresentation{ + return s.attachObjectShape(index, info.Filtering, objectShapeRepresentation{ lsBCircle: circle, wsBCircle: circle, lsTransform: transform, wsTransform: transform, - kind: shapeKindCircle, + kind: objectShapeKindCircle, points: []dprec.Vec2{ // TODO: Consider reusing from a buffer. dprec.ZeroVec2(), }, @@ -180,9 +198,14 @@ func (s *Scene[O, S, M]) AttachCircle(objID ObjectID, info CircleInfo[S]) ShapeI }, info.UserData) } -// AttachRectangle creates a rectangle shape and attaches it to the object to be -// used for intersection tests. -func (s *Scene[O, S, M]) AttachRectangle(objID ObjectID, info RectangleInfo[S]) ShapeID { +// AttachRectangle creates a rectangle shape and attaches it to the object to +// be used for intersection tests. +// +// The rectangle is specified in the local space of the object and moves along +// with it. +func (s *Scene[O, T, S]) AttachRectangle(objID ObjectID, info RectangleInfo[S]) ObjectShapeID { + index := int32(objID) + rectangle := info.Rectangle transform := shape2d.Transform{ Translation: rectangle.Center, @@ -192,12 +215,12 @@ func (s *Scene[O, S, M]) AttachRectangle(objID ObjectID, info RectangleInfo[S]) halfWidth := rectangle.HalfWidth halfHeight := rectangle.HalfHeight - return s.attachShape(int32(objID), info.Filtering, shapeRepresentation{ + return s.attachObjectShape(index, info.Filtering, objectShapeRepresentation{ lsBCircle: bCircle, wsBCircle: bCircle, lsTransform: transform, wsTransform: transform, - kind: shapeKindRectangle, + kind: objectShapeKindRectangle, points: []dprec.Vec2{ // TODO: Consider reusing from a buffer. dprec.NewVec2(-halfWidth, -halfHeight), dprec.NewVec2(halfWidth, -halfHeight), @@ -208,42 +231,44 @@ func (s *Scene[O, S, M]) AttachRectangle(objID ObjectID, info RectangleInfo[S]) }, info.UserData) } -// DeleteShape deletes a shape from an object. The object is not +// DeleteObjectShape deletes a shape from an object. The object is not // deleted and continues to exist in the scene. -func (s *Scene[O, S, M]) DeleteShape(shapeID ShapeID) { +func (s *Scene[O, T, S]) DeleteObjectShape(shapeID ObjectShapeID) { index := int32(shapeID) - s.detachShape(index) + s.detachObjectShape(index) } -// GetShapeUserData returns the user data associated with the given shape. -func (s *Scene[O, S, M]) GetShapeUserData(shapeID ShapeID) S { +// GetObjectShapeUserData returns the user data associated with the given +// object shape. +func (s *Scene[O, T, S]) GetObjectShapeUserData(shapeID ObjectShapeID) S { index := int32(shapeID) - shape := &s.shapes[index] + shape := &s.objectShapes[index] return shape.userData } -// SetShapeUserData assigns the specified user data to the shape. -func (s *Scene[O, S, M]) SetShapeUserData(shapeID ShapeID, userData S) { +// SetObjectShapeUserData assigns the specified user data to the object shape. +func (s *Scene[O, T, S]) SetObjectShapeUserData(shapeID ObjectShapeID, userData S) { index := int32(shapeID) - shape := &s.shapes[index] + shape := &s.objectShapes[index] shape.userData = userData } -// EachCircle iterates over all circle shapes in the scene that match the -// filter and yields them to the provided callback. -func (s *Scene[O, S, M]) EachCircle(filter Filter, yield func(shape2d.Circle) bool) { - if filter.SkipDynamic { - return - } - for index := range s.shapes { - shape := &s.shapes[index] +// EachCircle iterates over all circle shapes in the scene that match the mask +// and yields them, in world space, to the provided callback. Iteration stops +// early if the callback returns false. +// +// Note that a zero mask matches no shape at all. Use [FullMask] to iterate +// over every circle in the scene. +func (s *Scene[O, T, S]) EachCircle(mask Mask, yield func(shape2d.Circle) bool) { + for index := range s.objectShapes { + shape := &s.objectShapes[index] if shape.spatialID == query2d.InvalidTreeItemID { continue } - if shape.kind != shapeKindCircle { + if shape.kind != objectShapeKindCircle { continue } - if !shape.matchesFilter(filter) { + if !shape.satisfiesMask(mask) { continue } if !yield(shape.toCircle()) { @@ -252,29 +277,30 @@ func (s *Scene[O, S, M]) EachCircle(filter Filter, yield func(shape2d.Circle) bo } } -// CircleIter returns an iterator over all circle shapes in the scene that match -// the filter. -func (s *Scene[O, S, M]) CircleIter(filter Filter) iter.Seq[shape2d.Circle] { +// CircleIter returns an iterator over all circle shapes in the scene that +// match the mask, as described by [Scene.EachCircle]. +func (s *Scene[O, T, S]) CircleIter(mask Mask) iter.Seq[shape2d.Circle] { return func(yield func(shape2d.Circle) bool) { - s.EachCircle(filter, yield) + s.EachCircle(mask, yield) } } // EachRectangle iterates over all rectangle shapes in the scene that match the -// filter and yields them to the provided callback. -func (s *Scene[O, S, M]) EachRectangle(filter Filter, yield func(shape2d.Rectangle) bool) { - if filter.SkipDynamic { - return - } - for index := range s.shapes { - shape := &s.shapes[index] +// mask and yields them, in world space, to the provided callback. Iteration +// stops early if the callback returns false. +// +// Note that a zero mask matches no shape at all. Use [FullMask] to iterate +// over every rectangle in the scene. +func (s *Scene[O, T, S]) EachRectangle(mask Mask, yield func(shape2d.Rectangle) bool) { + for index := range s.objectShapes { + shape := &s.objectShapes[index] if shape.spatialID == query2d.InvalidTreeItemID { continue } - if shape.kind != shapeKindRectangle { + if shape.kind != objectShapeKindRectangle { continue } - if !shape.matchesFilter(filter) { + if !shape.satisfiesMask(mask) { continue } if !yield(shape.toRectangle()) { @@ -283,263 +309,379 @@ func (s *Scene[O, S, M]) EachRectangle(filter Filter, yield func(shape2d.Rectang } } -// RectangleIter returns an iterator over all rectangle shapes in the scene that -// match the filter. -func (s *Scene[O, S, M]) RectangleIter(filter Filter) iter.Seq[shape2d.Rectangle] { +// RectangleIter returns an iterator over all rectangle shapes in the scene +// that match the mask, as described by [Scene.EachRectangle]. +func (s *Scene[O, T, S]) RectangleIter(mask Mask) iter.Seq[shape2d.Rectangle] { return func(yield func(shape2d.Rectangle) bool) { - s.EachRectangle(filter, yield) + s.EachRectangle(mask, yield) + } +} + +// CreateTerrain creates a new terrain. +// +// A terrain has no transform of its own. It merely groups the concave shapes +// that are attached to it, which are specified in world space. +func (s *Scene[O, T, S]) CreateTerrain(info TerrainInfo[T]) TerrainID { + index := s.allocateTerrain() + s.terrains[index] = terrainState[T]{ + firstShapeIndex: nilIndex, + lastShapeIndex: nilIndex, + userData: info.UserData, } + return TerrainID(index) +} + +// DeleteTerrain deletes a terrain, along with all of the shapes that are +// attached to it. +func (s *Scene[O, T, S]) DeleteTerrain(terrainID TerrainID) { + index := int32(terrainID) + terrain := &s.terrains[index] + terrain.userData = gog.Zero[T]() // in case of pointer + s.eachTerrainShape(terrain, func(shapeIndex int32, _ *terrainShapeState[S]) { + s.detachTerrainShape(shapeIndex) + }) + s.releaseTerrain(index) +} + +// GetTerrainUserData returns the user data associated with the given terrain. +func (s *Scene[O, T, S]) GetTerrainUserData(terrainID TerrainID) T { + index := int32(terrainID) + terrain := &s.terrains[index] + return terrain.userData +} + +// SetTerrainUserData assigns the specified user data to the terrain. +func (s *Scene[O, T, S]) SetTerrainUserData(terrainID TerrainID, userData T) { + index := int32(terrainID) + terrain := &s.terrains[index] + terrain.userData = userData } -// CreateMesh creates a new static mesh in the scene. +// GetTerrainForShape returns the ID of the terrain that the given shape is +// attached to. +func (s *Scene[O, T, S]) GetTerrainForShape(shapeID TerrainShapeID) TerrainID { + index := int32(shapeID) + shape := &s.terrainShapes[index] + return TerrainID(shape.terrainIndex) +} + +// AttachMesh creates a mesh shape and attaches it to the terrain to be used +// for intersection tests. // -// Unlike shapes, a mesh is not attached to an object. It is positioned -// directly through the [MeshInfo.Position] and [MeshInfo.Rotation] fields and -// is intended for static geometry that participates in intersection tests as a -// collection of edges. +// The mesh is specified in world space, as terrains have no transform of their +// own, and cannot be relocated afterwards. // // The mesh specified through [MeshInfo.Mesh] must not be empty, otherwise this // function panics. -func (s *Scene[O, S, M]) CreateMesh(info MeshInfo[M]) MeshID { - transform := shape2d.Transform{ - Translation: info.Position.ValueOrDefault(dprec.ZeroVec2()), - Rotation: shape2d.RotationFromAngle( - info.Rotation.ValueOrDefault(dprec.Radians(0.0)), - ), - } - representation := newMeshRepresentation(shape2d.TransformedMesh(info.Mesh, transform)) - area := representation.wsAABB +func (s *Scene[O, T, S]) AttachMesh(terrainID TerrainID, info MeshInfo[S]) TerrainShapeID { + index := int32(terrainID) - index := s.allocateMesh() - s.meshes[index] = meshShape[M]{ - spatialID: s.meshTree.Insert(area, index), - filterRepresentation: newFilterRepresentation(info.Filtering), - meshRepresentation: representation, - userData: info.UserData, - } + mesh := info.Mesh + bCircle := mesh.BoundingCircle() + aabb := mesh.BoundingAABB() - return MeshID(index) + return s.attachTerrainShape(index, info.Filtering, terrainShapeRepresentation{ + wsBCircle: bCircle, + wsAABB: aabb, + wsEdges: mesh.Edges, + }, info.UserData) } -// DeleteMesh removes the given mesh from the scene. -func (s *Scene[O, S, M]) DeleteMesh(meshID MeshID) { - index := int32(meshID) - mesh := &s.meshes[index] - s.meshTree.Remove(mesh.spatialID) - mesh.spatialID = query2d.InvalidTreeItemID - mesh.userData = gog.Zero[M]() // in case of pointer - s.releaseMesh(index) +// DeleteTerrainShape deletes a shape from a terrain. The terrain is not +// deleted and continues to exist in the scene. +func (s *Scene[O, T, S]) DeleteTerrainShape(shapeID TerrainShapeID) { + index := int32(shapeID) + s.detachTerrainShape(index) } -// GetMeshUserData returns the user data associated with the given mesh. -func (s *Scene[O, S, M]) GetMeshUserData(meshID MeshID) M { - mesh := &s.meshes[meshID] - return mesh.userData +// GetTerrainShapeUserData returns the user data associated with the given +// terrain shape. +func (s *Scene[O, T, S]) GetTerrainShapeUserData(shapeID TerrainShapeID) S { + index := int32(shapeID) + shape := &s.terrainShapes[index] + return shape.userData } -// SetMeshUserData assigns the specified user data to the mesh. -func (s *Scene[O, S, M]) SetMeshUserData(meshID MeshID, userData M) { - mesh := &s.meshes[meshID] - mesh.userData = userData +// SetTerrainShapeUserData assigns the specified user data to the terrain +// shape. +func (s *Scene[O, T, S]) SetTerrainShapeUserData(shapeID TerrainShapeID, userData S) { + index := int32(shapeID) + shape := &s.terrainShapes[index] + shape.userData = userData } -// CollectSegmentIntersections collects all intersections of the segment -// with objects in the scene. -func (s *Scene[O, S, M]) CollectSegmentIntersections(segment shape2d.Segment, filter Filter, yield ContactCallback) { - if !filter.SkipDynamic { - s.shapeCandidates = s.shapeCandidates[:0] - s.shapeTree.QuerySegment(segment, func(index int32) bool { - s.shapeCandidates = append(s.shapeCandidates, index) - return true - }) - s.collectSegmentShape(segment, filter, yield) - } +// CollectSegmentObjectIntersections collects all intersections of the segment +// with the object shapes in the scene that match the mask. +// +// The reported contacts have no source, since the segment is not part of the +// scene. Their Depth is the fraction of the segment that lies beyond the +// contact point, as described by [shape2d.Contact]. +func (s *Scene[O, T, S]) CollectSegmentObjectIntersections(segment shape2d.Segment, mask Mask, yield ObjectContactCallback) { + s.objectShapeCandidates = s.objectShapeCandidates[:0] + s.objectShapeTree.QuerySegment(segment, func(index int32) bool { + s.objectShapeCandidates = append(s.objectShapeCandidates, index) + return true + }) + s.collectSegmentObject(segment, mask, yield) +} - if !filter.SkipStatic { - s.meshCandidates = s.meshCandidates[:0] - s.meshTree.QuerySegment(segment, func(index int32) bool { - s.meshCandidates = append(s.meshCandidates, index) - return true - }) - s.collectSegmentMesh(segment, filter, yield) - } +// CheckSegmentObjectIntersection returns the intersection of the segment with +// the object shape that it enters first, if any. +func (s *Scene[O, T, S]) CheckSegmentObjectIntersection(segment shape2d.Segment, mask Mask) (ObjectContact, bool) { + var collection DeepestObjectContact + s.CollectSegmentObjectIntersections(segment, mask, collection.AddContact) + return collection.Contact() +} + +// CollectSegmentTerrainIntersections collects all intersections of the segment +// with the terrain shapes in the scene that match the mask. At most one +// contact is reported per terrain shape. +// +// The reported contacts have no source, since the segment is not part of the +// scene. Their Depth is the fraction of the segment that lies beyond the +// contact point, as described by [shape2d.Contact]. +func (s *Scene[O, T, S]) CollectSegmentTerrainIntersections(segment shape2d.Segment, mask Mask, yield TerrainContactCallback) { + s.terrainShapeCandidates = s.terrainShapeCandidates[:0] + s.terrainShapeTree.QuerySegment(segment, func(index int32) bool { + s.terrainShapeCandidates = append(s.terrainShapeCandidates, index) + return true + }) + s.collectSegmentTerrain(segment, mask, yield) } -// CheckSegmentIntersection returns the deepest intersection of the segment -// with the scene. -func (s *Scene[O, S, M]) CheckSegmentIntersection(segment shape2d.Segment, filter Filter) (Contact, bool) { - var collection DeepestContact - s.CollectSegmentIntersections(segment, filter, collection.AddContact) +// CheckSegmentTerrainIntersection returns the intersection of the segment with +// the terrain shape that it enters first, if any. +func (s *Scene[O, T, S]) CheckSegmentTerrainIntersection(segment shape2d.Segment, mask Mask) (TerrainContact, bool) { + var collection DeepestTerrainContact + s.CollectSegmentTerrainIntersections(segment, mask, collection.AddContact) return collection.Contact() } -// CollectCircleIntersections collects all intersections of the circle -// with objects in the scene. -func (s *Scene[O, S, M]) CollectCircleIntersections(circle shape2d.Circle, filter Filter, yield ContactCallback) { +// CollectCircleObjectIntersections collects all intersections of the circle +// with the object shapes in the scene that match the mask. +// +// The reported contacts have no source, since the circle is not part of the +// scene. +func (s *Scene[O, T, S]) CollectCircleObjectIntersections(circle shape2d.Circle, mask Mask, yield ObjectContactCallback) { queryAABB := shape2d.AABBFromCircle(circle) - if !filter.SkipDynamic { - s.shapeCandidates = s.shapeCandidates[:0] - s.shapeTree.QueryAABB(queryAABB, func(index int32) bool { - s.shapeCandidates = append(s.shapeCandidates, index) - return true - }) - s.collectCircleShape(circle, filter, yield) - } + s.objectShapeCandidates = s.objectShapeCandidates[:0] + s.objectShapeTree.QueryAABB(queryAABB, func(index int32) bool { + s.objectShapeCandidates = append(s.objectShapeCandidates, index) + return true + }) + s.collectCircleObject(circle, mask, yield) +} - if !filter.SkipStatic { - s.meshCandidates = s.meshCandidates[:0] - s.meshTree.QueryAABB(queryAABB, func(index int32) bool { - s.meshCandidates = append(s.meshCandidates, index) - return true - }) - s.collectCircleMesh(circle, filter, yield) - } +// CheckCircleObjectIntersection returns the deepest intersection of the circle +// with an object shape in the scene, if any. +func (s *Scene[O, T, S]) CheckCircleObjectIntersection(circle shape2d.Circle, mask Mask) (ObjectContact, bool) { + var collection DeepestObjectContact + s.CollectCircleObjectIntersections(circle, mask, collection.AddContact) + return collection.Contact() +} + +// CollectCircleTerrainIntersections collects all intersections of the circle +// with the terrain shapes in the scene that match the mask. At most one +// contact is reported per terrain shape. +// +// The reported contacts have no source, since the circle is not part of the +// scene. +func (s *Scene[O, T, S]) CollectCircleTerrainIntersections(circle shape2d.Circle, mask Mask, yield TerrainContactCallback) { + queryAABB := shape2d.AABBFromCircle(circle) + + s.terrainShapeCandidates = s.terrainShapeCandidates[:0] + s.terrainShapeTree.QueryAABB(queryAABB, func(index int32) bool { + s.terrainShapeCandidates = append(s.terrainShapeCandidates, index) + return true + }) + s.collectCircleTerrain(circle, mask, yield) } -// CheckCircleIntersection returns the deepest intersection of the circle -// with the scene. -func (s *Scene[O, S, M]) CheckCircleIntersection(circle shape2d.Circle, filter Filter) (Contact, bool) { - var collection DeepestContact - s.CollectCircleIntersections(circle, filter, collection.AddContact) +// CheckCircleTerrainIntersection returns the deepest intersection of the +// circle with a terrain shape in the scene, if any. +func (s *Scene[O, T, S]) CheckCircleTerrainIntersection(circle shape2d.Circle, mask Mask) (TerrainContact, bool) { + var collection DeepestTerrainContact + s.CollectCircleTerrainIntersections(circle, mask, collection.AddContact) return collection.Contact() } -// CollectRectangleIntersections collects all intersections of the rectangle -// with objects in the scene. -func (s *Scene[O, S, M]) CollectRectangleIntersections(rectangle shape2d.Rectangle, filter Filter, yield ContactCallback) { +// CollectRectangleObjectIntersections collects all intersections of the +// rectangle with the object shapes in the scene that match the mask. +// +// The reported contacts have no source, since the rectangle is not part of the +// scene. +func (s *Scene[O, T, S]) CollectRectangleObjectIntersections(rectangle shape2d.Rectangle, mask Mask, yield ObjectContactCallback) { queryAABB := shape2d.AABBFromRectangle(rectangle) - if !filter.SkipDynamic { - s.shapeCandidates = s.shapeCandidates[:0] - s.shapeTree.QueryAABB(queryAABB, func(index int32) bool { - s.shapeCandidates = append(s.shapeCandidates, index) - return true - }) - s.collectRectangleShape(rectangle, filter, yield) - } + s.objectShapeCandidates = s.objectShapeCandidates[:0] + s.objectShapeTree.QueryAABB(queryAABB, func(index int32) bool { + s.objectShapeCandidates = append(s.objectShapeCandidates, index) + return true + }) + s.collectRectangleObject(rectangle, mask, yield) +} - if !filter.SkipStatic { - s.meshCandidates = s.meshCandidates[:0] - s.meshTree.QueryAABB(queryAABB, func(index int32) bool { - s.meshCandidates = append(s.meshCandidates, index) - return true - }) - s.collectRectangleMesh(rectangle, filter, yield) - } +// CheckRectangleObjectIntersection returns the deepest intersection of the +// rectangle with an object shape in the scene, if any. +func (s *Scene[O, T, S]) CheckRectangleObjectIntersection(rectangle shape2d.Rectangle, mask Mask) (ObjectContact, bool) { + var collection DeepestObjectContact + s.CollectRectangleObjectIntersections(rectangle, mask, collection.AddContact) + return collection.Contact() +} + +// CollectRectangleTerrainIntersections collects all intersections of the +// rectangle with the terrain shapes in the scene that match the mask. At most +// one contact is reported per terrain shape. +// +// The reported contacts have no source, since the rectangle is not part of the +// scene. +func (s *Scene[O, T, S]) CollectRectangleTerrainIntersections(rectangle shape2d.Rectangle, mask Mask, yield TerrainContactCallback) { + queryAABB := shape2d.AABBFromRectangle(rectangle) + + s.terrainShapeCandidates = s.terrainShapeCandidates[:0] + s.terrainShapeTree.QueryAABB(queryAABB, func(index int32) bool { + s.terrainShapeCandidates = append(s.terrainShapeCandidates, index) + return true + }) + s.collectRectangleTerrain(rectangle, mask, yield) } -// CheckRectangleIntersection returns the deepest intersection of the rectangle -// with the scene. -func (s *Scene[O, S, M]) CheckRectangleIntersection(rectangle shape2d.Rectangle, filter Filter) (Contact, bool) { - var collection DeepestContact - s.CollectRectangleIntersections(rectangle, filter, collection.AddContact) +// CheckRectangleTerrainIntersection returns the deepest intersection of the +// rectangle with a terrain shape in the scene, if any. +func (s *Scene[O, T, S]) CheckRectangleTerrainIntersection(rectangle shape2d.Rectangle, mask Mask) (TerrainContact, bool) { + var collection DeepestTerrainContact + s.CollectRectangleTerrainIntersections(rectangle, mask, collection.AddContact) return collection.Contact() } -// CollectIntersections yields intersections found in this scene. -func (s *Scene[O, S, M]) CollectIntersections(yield ContactCallback) { - for i := range s.shapes { +// CollectObjectIntersections yields the intersections between the object +// shapes in this scene. +// +// Each intersecting pair is reported exactly once, and shapes that belong to +// the same object are never tested against one another. Both the source and +// the target of the reported contacts are object shapes. +func (s *Scene[O, T, S]) CollectObjectIntersections(yield ObjectContactCallback) { + for i := range s.objectShapes { srcIndex := int32(i) - srcShape := &s.shapes[srcIndex] + srcShape := &s.objectShapes[srcIndex] if srcShape.spatialID == query2d.InvalidTreeItemID { continue } queryAABB := shape2d.AABBFromCircle(srcShape.wsBCircle) - s.shapeCandidates = s.shapeCandidates[:0] - s.shapeTree.QueryAABB(queryAABB, func(tgtIndex int32) bool { - s.shapeCandidates = append(s.shapeCandidates, tgtIndex) + s.objectShapeCandidates = s.objectShapeCandidates[:0] + s.objectShapeTree.QueryAABB(queryAABB, func(tgtIndex int32) bool { + s.objectShapeCandidates = append(s.objectShapeCandidates, tgtIndex) return true }) - s.collectShapeShape(srcIndex, srcShape, yield) + s.collectObjectObject(srcIndex, srcShape, yield) + } +} + +// CollectTerrainIntersections yields the intersections between the object +// shapes and the terrain shapes in this scene. At most one contact is reported +// per object shape and terrain shape pair. +// +// Terrain shapes are never tested against one another, since terrains cannot +// move. The source of the reported contacts is therefore always an object +// shape. +func (s *Scene[O, T, S]) CollectTerrainIntersections(yield TerrainContactCallback) { + for i := range s.objectShapes { + srcIndex := int32(i) + srcShape := &s.objectShapes[srcIndex] + if srcShape.spatialID == query2d.InvalidTreeItemID { + continue + } + + queryAABB := shape2d.AABBFromCircle(srcShape.wsBCircle) - s.meshCandidates = s.meshCandidates[:0] - s.meshTree.QueryAABB(queryAABB, func(tgtIndex int32) bool { - s.meshCandidates = append(s.meshCandidates, tgtIndex) + s.terrainShapeCandidates = s.terrainShapeCandidates[:0] + s.terrainShapeTree.QueryAABB(queryAABB, func(tgtIndex int32) bool { + s.terrainShapeCandidates = append(s.terrainShapeCandidates, tgtIndex) return true }) - s.collectShapeMesh(srcIndex, srcShape, yield) + s.collectObjectTerrain(srcIndex, srcShape, yield) } } const nilIndex = -1 -func (s *Scene[O, S, M]) allocateObject() int32 { +func (s *Scene[O, T, S]) allocateObject() int32 { if s.freeObjectIndices.IsEmpty() { index := len(s.objects) - s.objects = append(s.objects, sceneObject[O]{}) + s.objects = append(s.objects, objectState[O]{}) return int32(index) } else { return s.freeObjectIndices.Pop() } } -func (s *Scene[O, S, M]) releaseObject(index int32) { +func (s *Scene[O, T, S]) releaseObject(index int32) { s.freeObjectIndices.Push(index) } -func (s *Scene[O, S, M]) eachObjectShape(object *sceneObject[O], cb func(int32, *shape[S])) { +func (s *Scene[O, T, S]) eachObjectShape(object *objectState[O], cb func(int32, *objectShapeState[S])) { index := object.firstShapeIndex for index >= 0 { - shape := &s.shapes[index] + shape := &s.objectShapes[index] nextIndex := shape.nextShapeIndex cb(index, shape) index = nextIndex } } -func (s *Scene[O, S, M]) allocateShape() int32 { - if s.freeShapeIndices.IsEmpty() { - index := len(s.shapes) - s.shapes = append(s.shapes, shape[S]{}) +func (s *Scene[O, T, S]) allocateObjectShape() int32 { + if s.freeObjectShapeIndices.IsEmpty() { + index := len(s.objectShapes) + s.objectShapes = append(s.objectShapes, objectShapeState[S]{}) return int32(index) } else { - return s.freeShapeIndices.Pop() + return s.freeObjectShapeIndices.Pop() } } -func (s *Scene[O, S, M]) releaseShape(index int32) { - s.freeShapeIndices.Push(index) +func (s *Scene[O, T, S]) releaseObjectShape(index int32) { + s.freeObjectShapeIndices.Push(index) } -func (s *Scene[O, S, M]) attachShape( +func (s *Scene[O, T, S]) attachObjectShape( objectIndex int32, filterInfo FilterInfo, - representation shapeRepresentation, + representation objectShapeRepresentation, userData S, -) ShapeID { +) ObjectShapeID { object := &s.objects[objectIndex] - index := s.allocateShape() + index := s.allocateObjectShape() representation.update(object.transform) area := shape2d.AABBFromCircle(representation.wsBCircle) - s.shapes[index] = shape[S]{ - objectIndex: objectIndex, - nextShapeIndex: nilIndex, - prevShapeIndex: object.lastShapeIndex, - spatialID: s.shapeTree.Insert(area, index), - filterRepresentation: newFilterRepresentation(filterInfo), - shapeRepresentation: representation, - userData: userData, + s.objectShapes[index] = objectShapeState[S]{ + objectIndex: objectIndex, + nextShapeIndex: nilIndex, + prevShapeIndex: object.lastShapeIndex, + spatialID: s.objectShapeTree.Insert(area, index), + filterRepresentation: newFilterRepresentation(filterInfo), + objectShapeRepresentation: representation, + userData: userData, } if object.firstShapeIndex == nilIndex { object.firstShapeIndex = index } else { - s.shapes[object.lastShapeIndex].nextShapeIndex = index + s.objectShapes[object.lastShapeIndex].nextShapeIndex = index } object.lastShapeIndex = index - return ShapeID(index) + return ObjectShapeID(index) } -func (s *Scene[O, S, M]) detachShape(index int32) { - shape := &s.shapes[index] +func (s *Scene[O, T, S]) detachObjectShape(index int32) { + shape := &s.objectShapes[index] - s.shapeTree.Remove(shape.spatialID) + s.objectShapeTree.Remove(shape.spatialID) shape.spatialID = query2d.InvalidTreeItemID object := &s.objects[shape.objectIndex] @@ -550,185 +692,282 @@ func (s *Scene[O, S, M]) detachShape(index int32) { object.lastShapeIndex = shape.prevShapeIndex } if shape.prevShapeIndex != nilIndex { - prevShape := &s.shapes[shape.prevShapeIndex] + prevShape := &s.objectShapes[shape.prevShapeIndex] prevShape.nextShapeIndex = shape.nextShapeIndex } if shape.nextShapeIndex != nilIndex { - nextShape := &s.shapes[shape.nextShapeIndex] + nextShape := &s.objectShapes[shape.nextShapeIndex] nextShape.prevShapeIndex = shape.prevShapeIndex } shape.objectIndex = -1 shape.userData = gog.Zero[S]() // in case of pointer - s.releaseShape(index) + s.releaseObjectShape(index) +} + +func (s *Scene[O, T, S]) allocateTerrain() int32 { + if s.freeTerrainIndices.IsEmpty() { + index := len(s.terrains) + s.terrains = append(s.terrains, terrainState[T]{}) + return int32(index) + } else { + return s.freeTerrainIndices.Pop() + } +} + +func (s *Scene[O, T, S]) releaseTerrain(index int32) { + s.freeTerrainIndices.Push(index) +} + +func (s *Scene[O, T, S]) eachTerrainShape(terrain *terrainState[T], cb func(int32, *terrainShapeState[S])) { + index := terrain.firstShapeIndex + for index >= 0 { + shape := &s.terrainShapes[index] + nextIndex := shape.nextShapeIndex + cb(index, shape) + index = nextIndex + } } -func (s *Scene[O, S, M]) allocateMesh() int32 { - if s.freeMeshIndices.IsEmpty() { - index := len(s.meshes) - s.meshes = append(s.meshes, meshShape[M]{}) +func (s *Scene[O, T, S]) allocateTerrainShape() int32 { + if s.freeTerrainShapeIndices.IsEmpty() { + index := len(s.terrainShapes) + s.terrainShapes = append(s.terrainShapes, terrainShapeState[S]{}) return int32(index) } else { - return s.freeMeshIndices.Pop() + return s.freeTerrainShapeIndices.Pop() + } +} + +func (s *Scene[O, T, S]) releaseTerrainShape(index int32) { + s.freeTerrainShapeIndices.Push(index) +} + +func (s *Scene[O, T, S]) attachTerrainShape( + terrainIndex int32, + filterInfo FilterInfo, + representation terrainShapeRepresentation, + userData S, +) TerrainShapeID { + + terrain := &s.terrains[terrainIndex] + index := s.allocateTerrainShape() + + area := representation.wsAABB + + s.terrainShapes[index] = terrainShapeState[S]{ + terrainIndex: terrainIndex, + nextShapeIndex: nilIndex, + prevShapeIndex: terrain.lastShapeIndex, + spatialID: s.terrainShapeTree.Insert(area, index), + filterRepresentation: newFilterRepresentation(filterInfo), + terrainShapeRepresentation: representation, + userData: userData, + } + if terrain.firstShapeIndex == nilIndex { + terrain.firstShapeIndex = index + } else { + s.terrainShapes[terrain.lastShapeIndex].nextShapeIndex = index } + terrain.lastShapeIndex = index + + return TerrainShapeID(index) } -func (s *Scene[O, S, M]) releaseMesh(index int32) { - s.freeMeshIndices.Push(index) +func (s *Scene[O, T, S]) detachTerrainShape(index int32) { + shape := &s.terrainShapes[index] + + s.terrainShapeTree.Remove(shape.spatialID) + shape.spatialID = query2d.InvalidTreeItemID + + terrain := &s.terrains[shape.terrainIndex] + if terrain.firstShapeIndex == index { + terrain.firstShapeIndex = shape.nextShapeIndex + } + if terrain.lastShapeIndex == index { + terrain.lastShapeIndex = shape.prevShapeIndex + } + if shape.prevShapeIndex != nilIndex { + prevShape := &s.terrainShapes[shape.prevShapeIndex] + prevShape.nextShapeIndex = shape.nextShapeIndex + } + if shape.nextShapeIndex != nilIndex { + nextShape := &s.terrainShapes[shape.nextShapeIndex] + nextShape.prevShapeIndex = shape.prevShapeIndex + } + shape.terrainIndex = -1 + shape.userData = gog.Zero[S]() // in case of pointer + + s.releaseTerrainShape(index) } -func (s *Scene[O, S, M]) collectSegmentShape(segment shape2d.Segment, filter Filter, yield ContactCallback) { - for index, shape := range s.iterCandidateShape(filter) { +func (s *Scene[O, T, S]) collectSegmentObject(segment shape2d.Segment, mask Mask, yield ObjectContactCallback) { + for index, shape := range s.iterCandidateObjectShapes(mask) { if !isec2d.CheckSegmentCircleOverlap(segment, shape.wsBCircle) { continue } onContact := func(contact shape2d.Contact) { - yield(Contact{ - SourceShapeID: InvalidShapeID, - TargetShapeID: ShapeID(index), - TargetMeshID: InvalidMeshID, - Contact: contact, + yield(ObjectContact{ + SourceObjectID: NilObjectID, + SourceShapeID: NilObjectShapeID, + TargetObjectID: ObjectID(shape.objectIndex), + TargetShapeID: ObjectShapeID(index), + Contact: contact, }) } switch shape.kind { - case shapeKindCircle: + case objectShapeKindCircle: circle := shape.toCircle() isec2d.ResolveSegmentCircle(segment, circle, onContact) - case shapeKindRectangle: + case objectShapeKindRectangle: rectangle := shape.toRectangle() isec2d.ResolveSegmentRectangle(segment, rectangle, onContact) } } } -func (s *Scene[O, S, M]) collectSegmentMesh(segment shape2d.Segment, filter Filter, yield ContactCallback) { - for index, mesh := range s.iterCandidateMesh(filter) { - if !isec2d.CheckSegmentCircleOverlap(segment, mesh.wsBCircle) { +func (s *Scene[O, T, S]) collectSegmentTerrain(segment shape2d.Segment, mask Mask, yield TerrainContactCallback) { + for index, shape := range s.iterCandidateTerrainShapes(mask) { + if !isec2d.CheckSegmentCircleOverlap(segment, shape.wsBCircle) { continue } var deepestContact shape2d.DeepestContact - for _, edge := range mesh.wsEdges { + for _, edge := range shape.wsEdges { isec2d.ResolveSegmentEdge(segment, edge, deepestContact.AddContact) } if contact, ok := deepestContact.Contact(); ok { - yield(Contact{ - SourceShapeID: InvalidShapeID, - TargetShapeID: InvalidShapeID, - TargetMeshID: MeshID(index), - Contact: contact, + yield(TerrainContact{ + SourceObjectID: NilObjectID, + SourceShapeID: NilObjectShapeID, + TargetTerrainID: TerrainID(shape.terrainIndex), + TargetShapeID: TerrainShapeID(index), + Contact: contact, }) } } } -func (s *Scene[O, S, M]) collectCircleShape(circle shape2d.Circle, filter Filter, yield ContactCallback) { +func (s *Scene[O, T, S]) collectCircleObject(circle shape2d.Circle, mask Mask, yield ObjectContactCallback) { initGJKShapeForCircle(circle, &s.tempGJKSource) - for index, shape := range s.iterCandidateShape(filter) { + for index, shape := range s.iterCandidateObjectShapes(mask) { if !isec2d.CheckCircleCircle(circle, shape.wsBCircle) { continue } if contact, ok := s.solver.Resolve(s.tempGJKSource, shape.gjkShape()); ok { - yield(Contact{ - SourceShapeID: InvalidShapeID, - TargetShapeID: ShapeID(index), - TargetMeshID: InvalidMeshID, - Contact: contact, + yield(ObjectContact{ + SourceObjectID: NilObjectID, + SourceShapeID: NilObjectShapeID, + TargetObjectID: ObjectID(shape.objectIndex), + TargetShapeID: ObjectShapeID(index), + Contact: contact, }) } } } -func (s *Scene[O, S, M]) collectCircleMesh(circle shape2d.Circle, filter Filter, yield ContactCallback) { +func (s *Scene[O, T, S]) collectCircleTerrain(circle shape2d.Circle, mask Mask, yield TerrainContactCallback) { initGJKShapeForCircle(circle, &s.tempGJKSource) - for tgtIndex, tgtMesh := range s.iterCandidateMesh(filter) { - s.resolveGJKMesh(s.tempGJKSource, circle, tgtMesh, func(contact shape2d.Contact) { - yield(Contact{ - SourceShapeID: InvalidShapeID, - TargetShapeID: InvalidShapeID, - TargetMeshID: MeshID(tgtIndex), - Contact: contact, + for index, shape := range s.iterCandidateTerrainShapes(mask) { + s.resolveTerrainShape(s.tempGJKSource, circle, shape, func(contact shape2d.Contact) { + yield(TerrainContact{ + SourceObjectID: NilObjectID, + SourceShapeID: NilObjectShapeID, + TargetTerrainID: TerrainID(shape.terrainIndex), + TargetShapeID: TerrainShapeID(index), + Contact: contact, }) }) } } -func (s *Scene[O, S, M]) collectRectangleShape(rectangle shape2d.Rectangle, filter Filter, yield ContactCallback) { +func (s *Scene[O, T, S]) collectRectangleObject(rectangle shape2d.Rectangle, mask Mask, yield ObjectContactCallback) { initGJKShapeForRectangle(rectangle, &s.tempGJKSource) - for index, shape := range s.iterCandidateShape(filter) { + for index, shape := range s.iterCandidateObjectShapes(mask) { if !isec2d.CheckCircleCircle(rectangle.BoundingCircle(), shape.wsBCircle) { continue } if contact, ok := s.solver.Resolve(s.tempGJKSource, shape.gjkShape()); ok { - yield(Contact{ - SourceShapeID: InvalidShapeID, - TargetShapeID: ShapeID(index), - TargetMeshID: InvalidMeshID, - Contact: contact, + yield(ObjectContact{ + SourceObjectID: NilObjectID, + SourceShapeID: NilObjectShapeID, + TargetObjectID: ObjectID(shape.objectIndex), + TargetShapeID: ObjectShapeID(index), + Contact: contact, }) } } } -func (s *Scene[O, S, M]) collectRectangleMesh(rectangle shape2d.Rectangle, filter Filter, yield ContactCallback) { +func (s *Scene[O, T, S]) collectRectangleTerrain(rectangle shape2d.Rectangle, mask Mask, yield TerrainContactCallback) { initGJKShapeForRectangle(rectangle, &s.tempGJKSource) - for tgtIndex, tgtMesh := range s.iterCandidateMesh(filter) { - s.resolveGJKMesh(s.tempGJKSource, rectangle.BoundingCircle(), tgtMesh, func(contact shape2d.Contact) { - yield(Contact{ - SourceShapeID: InvalidShapeID, - TargetShapeID: InvalidShapeID, - TargetMeshID: MeshID(tgtIndex), - Contact: contact, + for index, shape := range s.iterCandidateTerrainShapes(mask) { + s.resolveTerrainShape(s.tempGJKSource, rectangle.BoundingCircle(), shape, func(contact shape2d.Contact) { + yield(TerrainContact{ + SourceObjectID: NilObjectID, + SourceShapeID: NilObjectShapeID, + TargetTerrainID: TerrainID(shape.terrainIndex), + TargetShapeID: TerrainShapeID(index), + Contact: contact, }) }) } } -func (s *Scene[O, S, M]) collectShapeShape(srcIndex int32, srcShape *shape[S], yield ContactCallback) { +func (s *Scene[O, T, S]) collectObjectObject(srcIndex int32, srcShape *objectShapeState[S], yield ObjectContactCallback) { srcGJKShape := srcShape.gjkShape() - for _, tgtIndex := range s.shapeCandidates { - tgtShape := &s.shapes[tgtIndex] - if !shapesCanIntersect(srcShape, tgtShape) { + for _, tgtIndex := range s.objectShapeCandidates { + tgtShape := &s.objectShapes[tgtIndex] + if !objectShapesCanIntersect(srcShape, tgtShape) { continue } if !isec2d.CheckCircleCircle(srcShape.wsBCircle, tgtShape.wsBCircle) { continue } if contact, ok := s.solver.Resolve(srcGJKShape, tgtShape.gjkShape()); ok { - yield(Contact{ - SourceShapeID: ShapeID(srcIndex), - TargetShapeID: ShapeID(tgtIndex), - TargetMeshID: InvalidMeshID, - Contact: contact, + yield(ObjectContact{ + SourceObjectID: ObjectID(srcShape.objectIndex), + SourceShapeID: ObjectShapeID(srcIndex), + TargetObjectID: ObjectID(tgtShape.objectIndex), + TargetShapeID: ObjectShapeID(tgtIndex), + Contact: contact, }) } } } -func (s *Scene[O, S, M]) collectShapeMesh(srcIndex int32, srcShape *shape[S], yield ContactCallback) { +func (s *Scene[O, T, S]) collectObjectTerrain(srcIndex int32, srcShape *objectShapeState[S], yield TerrainContactCallback) { srcGJKShape := srcShape.gjkShape() - for _, tgtIndex := range s.meshCandidates { - tgtMesh := &s.meshes[tgtIndex] - if !shapeMeshCanIntersect(srcShape, tgtMesh) { + for _, tgtIndex := range s.terrainShapeCandidates { + tgtShape := &s.terrainShapes[tgtIndex] + if !objectTerrainShapesCanIntersect(srcShape, tgtShape) { continue } - s.resolveGJKMesh(srcGJKShape, srcShape.wsBCircle, tgtMesh, func(contact shape2d.Contact) { - yield(Contact{ - SourceShapeID: ShapeID(srcIndex), - TargetShapeID: InvalidShapeID, - TargetMeshID: MeshID(tgtIndex), - Contact: contact, + s.resolveTerrainShape(srcGJKShape, srcShape.wsBCircle, tgtShape, func(contact shape2d.Contact) { + yield(TerrainContact{ + SourceObjectID: ObjectID(srcShape.objectIndex), + SourceShapeID: ObjectShapeID(srcIndex), + TargetTerrainID: TerrainID(tgtShape.terrainIndex), + TargetShapeID: TerrainShapeID(tgtIndex), + Contact: contact, }) }) } } -func (s *Scene[O, S, M]) resolveGJKMesh(srcGJK gjk2d.Shape, srcBC shape2d.Circle, tgtMesh *meshShape[M], yield shape2d.ContactCallback) { - if !isec2d.CheckCircleCircle(srcBC, tgtMesh.wsBCircle) { +// resolveTerrainShape resolves the intersection of the specified convex source +// shape with a terrain shape, by testing it against each of the edges that +// make up the terrain shape. +// +// Only the deepest contact is yielded, and at most once, so that a source +// shape that overlaps many edges of the same terrain shape does not produce a +// pile of nearly identical contacts. +func (s *Scene[O, T, S]) resolveTerrainShape(srcGJK gjk2d.Shape, srcBC shape2d.Circle, tgtShape *terrainShapeState[S], yield shape2d.ContactCallback) { + if !isec2d.CheckCircleCircle(srcBC, tgtShape.wsBCircle) { return } points := initGJKShapeForMesh(&s.tempGJKTarget) var deepestContact shape2d.DeepestContact - for _, edge := range tgtMesh.wsEdges { + for _, edge := range tgtShape.wsEdges { tgtBCircle := edge.BoundingCircle() if !isec2d.CheckCircleCircle(srcBC, tgtBCircle) { continue @@ -747,10 +986,10 @@ func (s *Scene[O, S, M]) resolveGJKMesh(srcGJK gjk2d.Shape, srcBC shape2d.Circle } } -func (s *Scene[O, S, M]) eachCandidateShape(filter Filter, cb func(int32, *shape[S]) bool) { - for _, index := range s.shapeCandidates { - shape := &s.shapes[index] - if !shape.matchesFilter(filter) { +func (s *Scene[O, T, S]) eachCandidateObjectShape(mask Mask, cb func(int32, *objectShapeState[S]) bool) { + for _, index := range s.objectShapeCandidates { + shape := &s.objectShapes[index] + if !shape.satisfiesMask(mask) { continue } if !cb(index, shape) { @@ -759,27 +998,27 @@ func (s *Scene[O, S, M]) eachCandidateShape(filter Filter, cb func(int32, *shape } } -func (s *Scene[O, S, M]) iterCandidateShape(filter Filter) iter.Seq2[int32, *shape[S]] { - return func(yield func(int32, *shape[S]) bool) { - s.eachCandidateShape(filter, yield) +func (s *Scene[O, T, S]) iterCandidateObjectShapes(mask Mask) iter.Seq2[int32, *objectShapeState[S]] { + return func(yield func(int32, *objectShapeState[S]) bool) { + s.eachCandidateObjectShape(mask, yield) } } -func (s *Scene[O, S, M]) eachCandidateMesh(filter Filter, cb func(int32, *meshShape[M]) bool) { - for _, index := range s.meshCandidates { - mesh := &s.meshes[index] - if !mesh.matchesFilter(filter) { +func (s *Scene[O, T, S]) eachCandidateTerrainShape(mask Mask, cb func(int32, *terrainShapeState[S]) bool) { + for _, index := range s.terrainShapeCandidates { + shape := &s.terrainShapes[index] + if !shape.satisfiesMask(mask) { continue } - if !cb(index, mesh) { + if !cb(index, shape) { return } } } -func (s *Scene[O, S, M]) iterCandidateMesh(filter Filter) iter.Seq2[int32, *meshShape[M]] { - return func(yield func(int32, *meshShape[M]) bool) { - s.eachCandidateMesh(filter, yield) +func (s *Scene[O, T, S]) iterCandidateTerrainShapes(mask Mask) iter.Seq2[int32, *terrainShapeState[S]] { + return func(yield func(int32, *terrainShapeState[S]) bool) { + s.eachCandidateTerrainShape(mask, yield) } } diff --git a/core/spatial/placement2d/scene_test.go b/core/spatial/placement2d/scene_test.go index 68d79ae3..1e076283 100644 --- a/core/spatial/placement2d/scene_test.go +++ b/core/spatial/placement2d/scene_test.go @@ -30,8 +30,8 @@ func rectangleAt(x, y, half float64) shape2d.Rectangle { } // lineMesh builds a mesh made of a single edge forming a horizontal line (at -// y == 0 by default), centered at the given point and spanning halfSize in the -// X direction. The edge is wound so that its normal faces -Y. +// the given y), centered at the given point and spanning halfSize in the X +// direction. The edge is wound so that its normal faces -Y. func lineMesh(x, y, halfSize float64) shape2d.Mesh { a := dprec.NewVec2(x-halfSize, y) b := dprec.NewVec2(x+halfSize, y) @@ -53,7 +53,7 @@ var _ = Describe("Scene", func() { Describe("object management", func() { It("creates objects placed at the origin by default", func() { objID := scene.CreateObject(placement2d.ObjectInfo[string]{}) - Expect(objID).NotTo(Equal(placement2d.InvalidObjectID)) + Expect(objID).NotTo(Equal(placement2d.NilObjectID)) transform := scene.GetObjectTransform(objID) Expect(transform.Translation).To(dprectest.HaveVec2Coords(0.0, 0.0)) @@ -94,6 +94,59 @@ var _ = Describe("Scene", func() { }) }) + Describe("terrain management", func() { + It("creates terrains", func() { + terrainID := scene.CreateTerrain(placement2d.TerrainInfo[string]{}) + Expect(terrainID).NotTo(Equal(placement2d.NilTerrainID)) + }) + + It("stores and updates user data", func() { + terrainID := scene.CreateTerrain(placement2d.TerrainInfo[string]{ + UserData: "first", + }) + Expect(scene.GetTerrainUserData(terrainID)).To(Equal("first")) + + scene.SetTerrainUserData(terrainID, "second") + Expect(scene.GetTerrainUserData(terrainID)).To(Equal("second")) + }) + + It("stores and updates terrain shape user data", func() { + terrainID := scene.CreateTerrain(placement2d.TerrainInfo[string]{}) + shapeID := scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ + Mesh: lineMesh(0.0, 0.0, 5.0), + UserData: "a", + }) + Expect(scene.GetTerrainShapeUserData(shapeID)).To(Equal("a")) + + scene.SetTerrainShapeUserData(shapeID, "b") + Expect(scene.GetTerrainShapeUserData(shapeID)).To(Equal("b")) + }) + + It("maps a terrain shape back to its owning terrain", func() { + terrainID := scene.CreateTerrain(placement2d.TerrainInfo[string]{}) + shapeID := scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ + Mesh: lineMesh(0.0, 0.0, 5.0), + }) + Expect(scene.GetTerrainForShape(shapeID)).To(Equal(terrainID)) + }) + + It("reuses the indices of deleted terrains", func() { + first := scene.CreateTerrain(placement2d.TerrainInfo[string]{}) + scene.DeleteTerrain(first) + second := scene.CreateTerrain(placement2d.TerrainInfo[string]{}) + Expect(second).To(Equal(first)) + }) + + It("panics when attaching an empty mesh", func() { + terrainID := scene.CreateTerrain(placement2d.TerrainInfo[string]{}) + Expect(func() { + scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ + Mesh: shape2d.NewMesh(nil), + }) + }).To(Panic()) + }) + }) + Describe("shape iteration", func() { var objID placement2d.ObjectID @@ -110,7 +163,7 @@ var _ = Describe("Scene", func() { }) var found []shape2d.Circle - scene.EachCircle(placement2d.Filter{}, func(c shape2d.Circle) bool { + scene.EachCircle(placement2d.FullMask, func(c shape2d.Circle) bool { found = append(found, c) return true }) @@ -121,15 +174,11 @@ var _ = Describe("Scene", func() { It("yields attached rectangles", func() { scene.AttachRectangle(objID, placement2d.RectangleInfo[string]{ - Rectangle: shape2d.NewRectangle( - dprec.ZeroVec2(), - shape2d.IdentityRotation(), - dprec.NewVec2(1.0, 1.0), - ), + Rectangle: rectangleAt(0.0, 0.0, 1.0), }) count := 0 - scene.EachRectangle(placement2d.Filter{}, func(shape2d.Rectangle) bool { + scene.EachRectangle(placement2d.FullMask, func(shape2d.Rectangle) bool { count++ return true }) @@ -142,7 +191,19 @@ var _ = Describe("Scene", func() { }) count := 0 - for range scene.CircleIter(placement2d.Filter{}) { + for range scene.CircleIter(placement2d.FullMask) { + count++ + } + Expect(count).To(Equal(1)) + }) + + It("exposes a rectangle iterator", func() { + scene.AttachRectangle(objID, placement2d.RectangleInfo[string]{ + Rectangle: rectangleAt(0.0, 0.0, 1.0), + }) + + count := 0 + for range scene.RectangleIter(placement2d.FullMask) { count++ } Expect(count).To(Equal(1)) @@ -153,27 +214,27 @@ var _ = Describe("Scene", func() { Circle: circleAt(0.0, 0.0, 1.0), UserData: "a", }) - Expect(scene.GetShapeUserData(shapeID)).To(Equal("a")) + Expect(scene.GetObjectShapeUserData(shapeID)).To(Equal("a")) - scene.SetShapeUserData(shapeID, "b") - Expect(scene.GetShapeUserData(shapeID)).To(Equal("b")) + scene.SetObjectShapeUserData(shapeID, "b") + Expect(scene.GetObjectShapeUserData(shapeID)).To(Equal("b")) }) It("maps a shape back to its owning object", func() { shapeID := scene.AttachCircle(objID, placement2d.CircleInfo[string]{ Circle: circleAt(0.0, 0.0, 1.0), }) - Expect(scene.GetShapeObject(shapeID)).To(Equal(objID)) + Expect(scene.GetObjectForShape(shapeID)).To(Equal(objID)) }) It("removes a deleted shape from iteration", func() { shapeID := scene.AttachCircle(objID, placement2d.CircleInfo[string]{ Circle: circleAt(0.0, 0.0, 1.0), }) - scene.DeleteShape(shapeID) + scene.DeleteObjectShape(shapeID) count := 0 - scene.EachCircle(placement2d.Filter{}, func(shape2d.Circle) bool { + scene.EachCircle(placement2d.FullMask, func(shape2d.Circle) bool { count++ return true }) @@ -189,7 +250,7 @@ var _ = Describe("Scene", func() { }) count := 0 - scene.EachCircle(placement2d.Filter{}, func(shape2d.Circle) bool { + scene.EachCircle(placement2d.FullMask, func(shape2d.Circle) bool { count++ return false }) @@ -208,7 +269,7 @@ var _ = Describe("Scene", func() { )) var centers []dprec.Vec2 - scene.EachCircle(placement2d.Filter{}, func(c shape2d.Circle) bool { + scene.EachCircle(placement2d.FullMask, func(c shape2d.Circle) bool { centers = append(centers, c.Center) return true }) @@ -219,8 +280,8 @@ var _ = Describe("Scene", func() { }) }) - Describe("shape iteration filters", func() { - It("filters by layer mask", func() { + Describe("shape iteration masks", func() { + BeforeEach(func() { objID := scene.CreateObject(placement2d.ObjectInfo[string]{}) scene.AttachCircle(objID, placement2d.CircleInfo[string]{ Filtering: placement2d.FilterInfo{ @@ -228,24 +289,31 @@ var _ = Describe("Scene", func() { }, Circle: circleAt(0.0, 0.0, 1.0), }) + }) - matching := 0 - scene.EachCircle(placement2d.Filter{Mask: opt.V(uint32(0b01))}, func(shape2d.Circle) bool { - matching++ + countCircles := func(mask placement2d.Mask) int { + count := 0 + scene.EachCircle(mask, func(shape2d.Circle) bool { + count++ return true }) - Expect(matching).To(Equal(1)) + return count + } - nonMatching := 0 - scene.EachCircle(placement2d.Filter{Mask: opt.V(uint32(0b10))}, func(shape2d.Circle) bool { - nonMatching++ - return true - }) - Expect(nonMatching).To(BeZero()) + It("yields shapes that occupy a layer of the mask", func() { + Expect(countCircles(0b01)).To(Equal(1)) + }) + + It("skips shapes that occupy no layer of the mask", func() { + Expect(countCircles(0b10)).To(BeZero()) + }) + + It("yields nothing for the zero mask", func() { + Expect(countCircles(0)).To(BeZero()) }) }) - Describe("CollectIntersections", func() { + Describe("CollectObjectIntersections", func() { // attachOverlappingCircles places two unit circles 1.5 apart (so they // overlap) on freshly created objects and returns the object IDs. attachOverlappingCircles := func() (placement2d.ObjectID, placement2d.ObjectID) { @@ -264,9 +332,9 @@ var _ = Describe("Scene", func() { return first, second } - collect := func() placement2d.ContactList { - var contacts placement2d.ContactList - scene.CollectIntersections(contacts.AddContact) + collect := func() placement2d.ObjectContactList { + var contacts placement2d.ObjectContactList + scene.CollectObjectIntersections(contacts.AddContact) return contacts } @@ -275,11 +343,25 @@ var _ = Describe("Scene", func() { contacts := collect() Expect(contacts).To(HaveLen(1)) Expect([]placement2d.ObjectID{ - scene.GetShapeObject(contacts[0].SourceShapeID), - scene.GetShapeObject(contacts[0].TargetShapeID), + contacts[0].SourceObjectID, + contacts[0].TargetObjectID, }).To(ConsistOf(first, second)) }) + It("reports object IDs that agree with the shape IDs", func() { + attachOverlappingCircles() + contacts := collect() + Expect(contacts).To(HaveLen(1)) + + contact := contacts[0] + Expect(contact.SourceObjectID).To(Equal( + scene.GetObjectForShape(contact.SourceShapeID), + )) + Expect(contact.TargetObjectID).To(Equal( + scene.GetObjectForShape(contact.TargetShapeID), + )) + }) + It("does not report contacts between shapes of the same object", func() { objID := scene.CreateObject(placement2d.ObjectInfo[string]{}) scene.AttachCircle(objID, placement2d.CircleInfo[string]{ @@ -365,273 +447,456 @@ var _ = Describe("Scene", func() { Expect(collect()).To(HaveLen(1)) }) - It("reports a contact between a shape and an overlapping mesh", func() { + It("reports a contact between two overlapping rectangles", func() { + first := scene.CreateObject(placement2d.ObjectInfo[string]{}) + second := scene.CreateObject(placement2d.ObjectInfo[string]{ + Position: opt.V(dprec.NewVec2(1.5, 0.0)), + }) + scene.AttachRectangle(first, placement2d.RectangleInfo[string]{ + Rectangle: rectangleAt(0.0, 0.0, 2.0), + }) + scene.AttachRectangle(second, placement2d.RectangleInfo[string]{ + Rectangle: rectangleAt(0.0, 0.0, 2.0), + }) + Expect(collect()).To(HaveLen(1)) + }) + + It("reports a contact between an overlapping circle and rectangle", func() { + first := scene.CreateObject(placement2d.ObjectInfo[string]{}) + second := scene.CreateObject(placement2d.ObjectInfo[string]{ + Position: opt.V(dprec.NewVec2(1.0, 0.0)), + }) + scene.AttachCircle(first, placement2d.CircleInfo[string]{ + Circle: circleAt(0.0, 0.0, 1.0), + }) + scene.AttachRectangle(second, placement2d.RectangleInfo[string]{ + Rectangle: rectangleAt(0.0, 0.0, 1.0), + }) + Expect(collect()).To(HaveLen(1)) + }) + + It("does not report contacts with terrain shapes", func() { objID := scene.CreateObject(placement2d.ObjectInfo[string]{}) - // The line's edge faces -Y, so the circle is placed just below the - // line (on the front side) where it overlaps and is pushed out. + scene.AttachCircle(objID, placement2d.CircleInfo[string]{ + Circle: circleAt(0.0, -0.5, 1.0), + }) + terrainID := scene.CreateTerrain(placement2d.TerrainInfo[string]{}) + scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ + Mesh: lineMesh(0.0, 0.0, 5.0), + }) + Expect(collect()).To(BeEmpty()) + }) + + It("reports every overlapping pair of two multi-shape objects", func() { + first := scene.CreateObject(placement2d.ObjectInfo[string]{}) + second := scene.CreateObject(placement2d.ObjectInfo[string]{}) + scene.AttachCircle(first, placement2d.CircleInfo[string]{ + Circle: circleAt(0.0, 0.0, 1.0), + }) + scene.AttachCircle(first, placement2d.CircleInfo[string]{ + Circle: circleAt(0.5, 0.0, 1.0), + }) + scene.AttachCircle(second, placement2d.CircleInfo[string]{ + Circle: circleAt(0.25, 0.0, 1.0), + }) + // Both shapes of the first object overlap the single shape of the + // second one, while the two shapes of the first object are not + // tested against each other. + Expect(collect()).To(HaveLen(2)) + }) + + It("does not produce phantom contacts after index reuse", func() { + first, second := attachOverlappingCircles() + Expect(collect()).To(HaveLen(1)) + + scene.DeleteObject(first) + scene.DeleteObject(second) + Expect(collect()).To(BeEmpty()) + + // Recreate, reusing both the object and the shape indices. + third := scene.CreateObject(placement2d.ObjectInfo[string]{}) + scene.AttachCircle(third, placement2d.CircleInfo[string]{ + Circle: circleAt(0.0, 0.0, 1.0), + }) + Expect(collect()).To(BeEmpty()) + }) + }) + + Describe("CollectTerrainIntersections", func() { + var objID placement2d.ObjectID + var terrainID placement2d.TerrainID + + BeforeEach(func() { + objID = scene.CreateObject(placement2d.ObjectInfo[string]{}) + terrainID = scene.CreateTerrain(placement2d.TerrainInfo[string]{}) + }) + + collect := func() placement2d.TerrainContactList { + var contacts placement2d.TerrainContactList + scene.CollectTerrainIntersections(contacts.AddContact) + return contacts + } + + It("reports a contact between a shape and an overlapping mesh", func() { + // The line's normal faces -Y, so the circle is placed just below + // the line (on the front side) where it overlaps and is pushed out. shapeID := scene.AttachCircle(objID, placement2d.CircleInfo[string]{ Circle: circleAt(0.0, -0.5, 1.0), }) - meshID := scene.CreateMesh(placement2d.MeshInfo[string]{ + meshID := scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ Mesh: lineMesh(0.0, 0.0, 5.0), }) contacts := collect() Expect(contacts).To(HaveLen(1)) + Expect(contacts[0].SourceObjectID).To(Equal(objID)) Expect(contacts[0].SourceShapeID).To(Equal(shapeID)) - Expect(contacts[0].TargetShapeID).To(Equal(placement2d.InvalidShapeID)) - Expect(contacts[0].TargetMeshID).To(Equal(meshID)) + Expect(contacts[0].TargetTerrainID).To(Equal(terrainID)) + Expect(contacts[0].TargetShapeID).To(Equal(meshID)) - contact := contacts[0].Contact // The contact normal must push the circle out the front (-Y) side, // never inward into the mesh. - Expect(contact.TargetNormal.Y).To(BeNumerically("<", 0.0)) + Expect(contacts[0].TargetNormal.Y).To(BeNumerically("<", 0.0)) }) It("does not report a shape overlapping a mesh from behind", func() { - // A circle on the +Y (back) side of the -Y-facing line would have to - // be pushed further inward to separate, which the mesh logic + // A circle on the +Y (back) side of the -Y-facing line would have + // to be pushed further inward to separate, which the mesh logic // prevents. - objID := scene.CreateObject(placement2d.ObjectInfo[string]{}) scene.AttachCircle(objID, placement2d.CircleInfo[string]{ Circle: circleAt(0.0, 0.5, 1.0), }) - scene.CreateMesh(placement2d.MeshInfo[string]{ + scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ Mesh: lineMesh(0.0, 0.0, 5.0), }) Expect(collect()).To(BeEmpty()) }) It("does not report a shape disjoint from a mesh", func() { - objID := scene.CreateObject(placement2d.ObjectInfo[string]{}) scene.AttachCircle(objID, placement2d.CircleInfo[string]{ Circle: circleAt(0.0, 10.0, 1.0), }) - scene.CreateMesh(placement2d.MeshInfo[string]{ + scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ Mesh: lineMesh(0.0, 0.0, 5.0), }) Expect(collect()).To(BeEmpty()) }) - }) - Describe("shape-vs-shape with rectangles", func() { - It("reports a contact between two overlapping rectangles", func() { - first := scene.CreateObject(placement2d.ObjectInfo[string]{}) - second := scene.CreateObject(placement2d.ObjectInfo[string]{ - Position: opt.V(dprec.NewVec2(1.5, 0.0)), + It("reports a single contact even when many edges overlap", func() { + // A large rectangle overlaps both edges of the mesh, yet only the + // deepest contact is reported. + scene.AttachRectangle(objID, placement2d.RectangleInfo[string]{ + Rectangle: rectangleAt(0.0, -0.5, 2.0), }) - scene.AttachRectangle(first, placement2d.RectangleInfo[string]{ - Rectangle: shape2d.NewRectangle( - dprec.ZeroVec2(), - shape2d.IdentityRotation(), - dprec.NewVec2(2.0, 2.0), - ), + scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ + Mesh: shape2d.NewMesh([]shape2d.Edge{ + shape2d.NewEdge(dprec.NewVec2(-5.0, 0.0), dprec.NewVec2(0.0, 0.0)), + shape2d.NewEdge(dprec.NewVec2(0.0, 0.0), dprec.NewVec2(5.0, 0.0)), + }), }) - scene.AttachRectangle(second, placement2d.RectangleInfo[string]{ - Rectangle: shape2d.NewRectangle( - dprec.ZeroVec2(), - shape2d.IdentityRotation(), - dprec.NewVec2(2.0, 2.0), - ), + Expect(collect()).To(HaveLen(1)) + }) + + It("reports a contact per terrain shape", func() { + scene.AttachCircle(objID, placement2d.CircleInfo[string]{ + Circle: circleAt(0.0, -0.5, 1.0), + }) + scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ + Mesh: lineMesh(0.0, 0.0, 5.0), + }) + scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ + Mesh: lineMesh(0.0, -0.1, 5.0), }) + Expect(collect()).To(HaveLen(2)) + }) - var contacts placement2d.ContactList - scene.CollectIntersections(contacts.AddContact) - Expect(contacts).To(HaveLen(1)) + It("reports a contact per object shape", func() { + scene.AttachCircle(objID, placement2d.CircleInfo[string]{ + Circle: circleAt(-1.0, -0.5, 1.0), + }) + scene.AttachCircle(objID, placement2d.CircleInfo[string]{ + Circle: circleAt(1.0, -0.5, 1.0), + }) + scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ + Mesh: lineMesh(0.0, 0.0, 5.0), + }) + Expect(collect()).To(HaveLen(2)) }) - It("reports a contact between an overlapping circle and rectangle", func() { - first := scene.CreateObject(placement2d.ObjectInfo[string]{}) - second := scene.CreateObject(placement2d.ObjectInfo[string]{ - Position: opt.V(dprec.NewVec2(1.0, 0.0)), + It("does not report shapes that share a reject group", func() { + scene.AttachCircle(objID, placement2d.CircleInfo[string]{ + Filtering: placement2d.FilterInfo{RejectGroup: 7}, + Circle: circleAt(0.0, -0.5, 1.0), }) - scene.AttachCircle(first, placement2d.CircleInfo[string]{ - Circle: circleAt(0.0, 0.0, 1.0), + scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ + Filtering: placement2d.FilterInfo{RejectGroup: 7}, + Mesh: lineMesh(0.0, 0.0, 5.0), }) - scene.AttachRectangle(second, placement2d.RectangleInfo[string]{ - Rectangle: shape2d.NewRectangle( - dprec.ZeroVec2(), - shape2d.IdentityRotation(), - dprec.NewVec2(1.0, 1.0), - ), + Expect(collect()).To(BeEmpty()) + }) + + It("does not report shapes whose masks do not overlap", func() { + scene.AttachCircle(objID, placement2d.CircleInfo[string]{ + Filtering: placement2d.FilterInfo{ + SourceMask: opt.V(uint32(0b01)), + TargetMask: opt.V(uint32(0b01)), + }, + Circle: circleAt(0.0, -0.5, 1.0), }) + scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ + Filtering: placement2d.FilterInfo{ + SourceMask: opt.V(uint32(0b10)), + TargetMask: opt.V(uint32(0b10)), + }, + Mesh: lineMesh(0.0, 0.0, 5.0), + }) + Expect(collect()).To(BeEmpty()) + }) - var contacts placement2d.ContactList - scene.CollectIntersections(contacts.AddContact) + It("stops reporting once the terrain is deleted", func() { + scene.AttachCircle(objID, placement2d.CircleInfo[string]{ + Circle: circleAt(0.0, -0.5, 1.0), + }) + scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ + Mesh: lineMesh(0.0, 0.0, 5.0), + }) + Expect(collect()).To(HaveLen(1)) + + scene.DeleteTerrain(terrainID) + Expect(collect()).To(BeEmpty()) + }) + + It("stops reporting once the terrain shape is deleted", func() { + scene.AttachCircle(objID, placement2d.CircleInfo[string]{ + Circle: circleAt(0.0, -0.5, 1.0), + }) + meshID := scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ + Mesh: lineMesh(0.0, 0.0, 5.0), + }) + Expect(collect()).To(HaveLen(1)) + + scene.DeleteTerrainShape(meshID) + Expect(collect()).To(BeEmpty()) + }) + + It("reattaches correctly after terrain shape index reuse", func() { + scene.AttachCircle(objID, placement2d.CircleInfo[string]{ + Circle: circleAt(0.0, -0.5, 1.0), + }) + meshID := scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ + Mesh: lineMesh(0.0, 0.0, 5.0), + }) + scene.DeleteTerrainShape(meshID) + + other := scene.CreateTerrain(placement2d.TerrainInfo[string]{}) + reusedID := scene.AttachMesh(other, placement2d.MeshInfo[string]{ + Mesh: lineMesh(0.0, 0.0, 5.0), + }) + + contacts := collect() Expect(contacts).To(HaveLen(1)) + Expect(contacts[0].TargetTerrainID).To(Equal(other)) + Expect(contacts[0].TargetShapeID).To(Equal(reusedID)) + Expect(scene.GetTerrainForShape(reusedID)).To(Equal(other)) + }) + + It("tracks object movement into and out of terrain contact", func() { + scene.AttachCircle(objID, placement2d.CircleInfo[string]{ + Circle: circleAt(0.0, -0.5, 1.0), + }) + scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ + Mesh: lineMesh(0.0, 0.0, 5.0), + }) + Expect(collect()).To(HaveLen(1)) + + scene.SetObjectTransform(objID, shape2d.TranslationTransform( + dprec.NewVec2(0.0, -20.0), + )) + Expect(collect()).To(BeEmpty()) + + scene.SetObjectTransform(objID, shape2d.TranslationTransform( + dprec.NewVec2(0.0, 0.0), + )) + Expect(collect()).To(HaveLen(1)) }) }) - Describe("CheckCircleIntersection", func() { - It("reports a circle overlapping a scene shape", func() { + Describe("shape id spaces", func() { + It("keeps object and terrain shape ids independent", func() { objID := scene.CreateObject(placement2d.ObjectInfo[string]{}) - scene.AttachCircle(objID, placement2d.CircleInfo[string]{ + shapeID := scene.AttachCircle(objID, placement2d.CircleInfo[string]{ + Circle: circleAt(0.0, 0.0, 1.0), + UserData: "object-shape", + }) + terrainID := scene.CreateTerrain(placement2d.TerrainInfo[string]{}) + meshID := scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ + Mesh: lineMesh(0.0, 0.0, 5.0), + UserData: "terrain-shape", + }) + + // Both are the first shape of their kind, hence they share the raw + // index while remaining distinct references. + Expect(int32(shapeID)).To(Equal(int32(meshID))) + Expect(scene.GetObjectShapeUserData(shapeID)).To(Equal("object-shape")) + Expect(scene.GetTerrainShapeUserData(meshID)).To(Equal("terrain-shape")) + }) + }) + + Describe("circle queries", func() { + It("reports a circle overlapping an object shape", func() { + objID := scene.CreateObject(placement2d.ObjectInfo[string]{}) + shapeID := scene.AttachCircle(objID, placement2d.CircleInfo[string]{ Circle: circleAt(0.0, 0.0, 1.0), }) - contact, ok := scene.CheckCircleIntersection( + contact, ok := scene.CheckCircleObjectIntersection( circleAt(1.5, 0.0, 1.0), - placement2d.Filter{}, + placement2d.FullMask, ) Expect(ok).To(BeTrue()) - Expect(contact.SourceShapeID).To(Equal(placement2d.InvalidShapeID)) - Expect(scene.GetShapeObject(contact.TargetShapeID)).To(Equal(objID)) - Expect(contact.TargetMeshID).To(Equal(placement2d.InvalidMeshID)) + Expect(contact.SourceObjectID).To(Equal(placement2d.NilObjectID)) + Expect(contact.SourceShapeID).To(Equal(placement2d.NilObjectShapeID)) + Expect(contact.TargetObjectID).To(Equal(objID)) + Expect(contact.TargetShapeID).To(Equal(shapeID)) }) - It("returns false for a circle disjoint from every shape", func() { + It("returns false for a circle disjoint from every object shape", func() { objID := scene.CreateObject(placement2d.ObjectInfo[string]{}) scene.AttachCircle(objID, placement2d.CircleInfo[string]{ Circle: circleAt(0.0, 0.0, 1.0), }) - _, ok := scene.CheckCircleIntersection( + _, ok := scene.CheckCircleObjectIntersection( circleAt(10.0, 0.0, 1.0), - placement2d.Filter{}, + placement2d.FullMask, ) Expect(ok).To(BeFalse()) }) - It("reports a circle overlapping a mesh from the front", func() { - meshID := scene.CreateMesh(placement2d.MeshInfo[string]{ + It("honors the query mask", func() { + objID := scene.CreateObject(placement2d.ObjectInfo[string]{}) + scene.AttachCircle(objID, placement2d.CircleInfo[string]{ + Filtering: placement2d.FilterInfo{ + SourceMask: opt.V(uint32(0b01)), + }, + Circle: circleAt(0.0, 0.0, 1.0), + }) + + _, ok := scene.CheckCircleObjectIntersection( + circleAt(1.5, 0.0, 1.0), + 0b10, + ) + Expect(ok).To(BeFalse()) + }) + + It("reports a circle overlapping a terrain shape from the front", func() { + terrainID := scene.CreateTerrain(placement2d.TerrainInfo[string]{}) + meshID := scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ Mesh: lineMesh(0.0, 0.0, 5.0), }) // The line faces -Y, so approach it from below (the front side). - contact, ok := scene.CheckCircleIntersection( + contact, ok := scene.CheckCircleTerrainIntersection( circleAt(0.0, -0.5, 1.0), - placement2d.Filter{}, + placement2d.FullMask, ) Expect(ok).To(BeTrue()) - Expect(contact.TargetShapeID).To(Equal(placement2d.InvalidShapeID)) - Expect(contact.TargetMeshID).To(Equal(meshID)) + Expect(contact.SourceShapeID).To(Equal(placement2d.NilObjectShapeID)) + Expect(contact.TargetTerrainID).To(Equal(terrainID)) + Expect(contact.TargetShapeID).To(Equal(meshID)) Expect(contact.TargetNormal.Y).To(BeNumerically("<", 0.0)) }) - It("does not report a circle overlapping a mesh from behind", func() { - scene.CreateMesh(placement2d.MeshInfo[string]{ + It("does not report a circle overlapping a terrain shape from behind", func() { + terrainID := scene.CreateTerrain(placement2d.TerrainInfo[string]{}) + scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ Mesh: lineMesh(0.0, 0.0, 5.0), }) - _, ok := scene.CheckCircleIntersection( + _, ok := scene.CheckCircleTerrainIntersection( circleAt(0.0, 0.5, 1.0), - placement2d.Filter{}, + placement2d.FullMask, ) Expect(ok).To(BeFalse()) }) - It("skips dynamic shapes when SkipDynamic is set", func() { + It("keeps object and terrain queries separate", func() { objID := scene.CreateObject(placement2d.ObjectInfo[string]{}) scene.AttachCircle(objID, placement2d.CircleInfo[string]{ Circle: circleAt(0.0, 0.0, 1.0), }) - _, ok := scene.CheckCircleIntersection( - circleAt(1.5, 0.0, 1.0), - placement2d.Filter{SkipDynamic: true}, - ) - Expect(ok).To(BeFalse()) - }) - - It("skips static meshes when SkipStatic is set", func() { - scene.CreateMesh(placement2d.MeshInfo[string]{ - Mesh: lineMesh(0.0, 0.0, 5.0), - }) - - _, ok := scene.CheckCircleIntersection( - circleAt(0.0, -0.5, 1.0), - placement2d.Filter{SkipStatic: true}, + _, ok := scene.CheckCircleTerrainIntersection( + circleAt(0.5, 0.0, 1.0), + placement2d.FullMask, ) Expect(ok).To(BeFalse()) }) }) - Describe("CheckRectangleIntersection", func() { - It("reports a rectangle overlapping a scene shape", func() { + Describe("rectangle queries", func() { + It("reports a rectangle overlapping an object shape", func() { objID := scene.CreateObject(placement2d.ObjectInfo[string]{}) - scene.AttachCircle(objID, placement2d.CircleInfo[string]{ + shapeID := scene.AttachCircle(objID, placement2d.CircleInfo[string]{ Circle: circleAt(0.0, 0.0, 1.0), }) - contact, ok := scene.CheckRectangleIntersection( + contact, ok := scene.CheckRectangleObjectIntersection( rectangleAt(1.5, 0.0, 1.0), - placement2d.Filter{}, + placement2d.FullMask, ) Expect(ok).To(BeTrue()) - Expect(contact.SourceShapeID).To(Equal(placement2d.InvalidShapeID)) - Expect(scene.GetShapeObject(contact.TargetShapeID)).To(Equal(objID)) - Expect(contact.TargetMeshID).To(Equal(placement2d.InvalidMeshID)) + Expect(contact.SourceShapeID).To(Equal(placement2d.NilObjectShapeID)) + Expect(contact.TargetObjectID).To(Equal(objID)) + Expect(contact.TargetShapeID).To(Equal(shapeID)) }) - It("returns false for a rectangle disjoint from every shape", func() { + It("returns false for a rectangle disjoint from every object shape", func() { objID := scene.CreateObject(placement2d.ObjectInfo[string]{}) scene.AttachCircle(objID, placement2d.CircleInfo[string]{ Circle: circleAt(0.0, 0.0, 1.0), }) - _, ok := scene.CheckRectangleIntersection( + _, ok := scene.CheckRectangleObjectIntersection( rectangleAt(10.0, 0.0, 1.0), - placement2d.Filter{}, + placement2d.FullMask, ) Expect(ok).To(BeFalse()) }) - It("reports a rectangle overlapping a mesh from the front", func() { - meshID := scene.CreateMesh(placement2d.MeshInfo[string]{ + It("reports a rectangle overlapping a terrain shape from the front", func() { + terrainID := scene.CreateTerrain(placement2d.TerrainInfo[string]{}) + meshID := scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ Mesh: lineMesh(0.0, 0.0, 5.0), }) // The line faces -Y, so approach it from below (the front side). - contact, ok := scene.CheckRectangleIntersection( + contact, ok := scene.CheckRectangleTerrainIntersection( rectangleAt(0.0, -0.5, 1.0), - placement2d.Filter{}, + placement2d.FullMask, ) Expect(ok).To(BeTrue()) - Expect(contact.TargetShapeID).To(Equal(placement2d.InvalidShapeID)) - Expect(contact.TargetMeshID).To(Equal(meshID)) + Expect(contact.TargetTerrainID).To(Equal(terrainID)) + Expect(contact.TargetShapeID).To(Equal(meshID)) Expect(contact.TargetNormal.Y).To(BeNumerically("<", 0.0)) }) - It("does not report a rectangle overlapping a mesh from behind", func() { - scene.CreateMesh(placement2d.MeshInfo[string]{ + It("does not report a rectangle overlapping a terrain shape from behind", func() { + terrainID := scene.CreateTerrain(placement2d.TerrainInfo[string]{}) + scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ Mesh: lineMesh(0.0, 0.0, 5.0), }) - _, ok := scene.CheckRectangleIntersection( + _, ok := scene.CheckRectangleTerrainIntersection( rectangleAt(0.0, 0.5, 1.0), - placement2d.Filter{}, - ) - Expect(ok).To(BeFalse()) - }) - - It("skips dynamic shapes when SkipDynamic is set", func() { - objID := scene.CreateObject(placement2d.ObjectInfo[string]{}) - scene.AttachCircle(objID, placement2d.CircleInfo[string]{ - Circle: circleAt(0.0, 0.0, 1.0), - }) - - _, ok := scene.CheckRectangleIntersection( - rectangleAt(1.5, 0.0, 1.0), - placement2d.Filter{SkipDynamic: true}, - ) - Expect(ok).To(BeFalse()) - }) - - It("skips static meshes when SkipStatic is set", func() { - scene.CreateMesh(placement2d.MeshInfo[string]{ - Mesh: lineMesh(0.0, 0.0, 5.0), - }) - - _, ok := scene.CheckRectangleIntersection( - rectangleAt(0.0, -0.5, 1.0), - placement2d.Filter{SkipStatic: true}, + placement2d.FullMask, ) Expect(ok).To(BeFalse()) }) }) - Describe("CollectSegmentIntersections", func() { - It("collects every shape a segment passes through", func() { + Describe("segment queries", func() { + It("collects every object shape a segment passes through", func() { near := scene.CreateObject(placement2d.ObjectInfo[string]{}) far := scene.CreateObject(placement2d.ObjectInfo[string]{}) scene.AttachCircle(near, placement2d.CircleInfo[string]{ @@ -641,98 +906,105 @@ var _ = Describe("Scene", func() { Circle: circleAt(4.0, 0.0, 1.0), }) - var contacts placement2d.ContactList - scene.CollectSegmentIntersections( + var contacts placement2d.ObjectContactList + scene.CollectSegmentObjectIntersections( shape2d.NewSegment( dprec.NewVec2(-5.0, 0.0), dprec.NewVec2(9.0, 0.0), ), - placement2d.Filter{}, + placement2d.FullMask, contacts.AddContact, ) Expect(contacts).To(HaveLen(2)) }) - }) - Describe("CheckSegmentIntersection", func() { - It("finds a circle crossed by the segment", func() { + It("finds an object shape crossed by the segment", func() { objID := scene.CreateObject(placement2d.ObjectInfo[string]{}) - scene.AttachCircle(objID, placement2d.CircleInfo[string]{ + shapeID := scene.AttachCircle(objID, placement2d.CircleInfo[string]{ Circle: circleAt(0.0, 0.0, 1.0), }) - contact, ok := scene.CheckSegmentIntersection( + contact, ok := scene.CheckSegmentObjectIntersection( shape2d.NewSegment( dprec.NewVec2(-5.0, 0.0), dprec.NewVec2(5.0, 0.0), ), - placement2d.Filter{}, + placement2d.FullMask, ) Expect(ok).To(BeTrue()) - Expect(contact.SourceShapeID).To(Equal(placement2d.InvalidShapeID)) - Expect(scene.GetShapeObject(contact.TargetShapeID)).To(Equal(objID)) + Expect(contact.SourceShapeID).To(Equal(placement2d.NilObjectShapeID)) + Expect(contact.TargetObjectID).To(Equal(objID)) + Expect(contact.TargetShapeID).To(Equal(shapeID)) }) - It("finds a mesh crossed by the segment", func() { - meshID := scene.CreateMesh(placement2d.MeshInfo[string]{ - Mesh: lineMesh(0.0, 0.0, 5.0), + It("finds the nearest of two object shapes crossed by the segment", func() { + near := scene.CreateObject(placement2d.ObjectInfo[string]{}) + far := scene.CreateObject(placement2d.ObjectInfo[string]{}) + nearShapeID := scene.AttachCircle(near, placement2d.CircleInfo[string]{ + Circle: circleAt(0.0, 0.0, 1.0), + }) + scene.AttachCircle(far, placement2d.CircleInfo[string]{ + Circle: circleAt(4.0, 0.0, 1.0), }) - contact, ok := scene.CheckSegmentIntersection( + contact, ok := scene.CheckSegmentObjectIntersection( shape2d.NewSegment( - dprec.NewVec2(2.0, -5.0), - dprec.NewVec2(2.0, 5.0), + dprec.NewVec2(-5.0, 0.0), + dprec.NewVec2(9.0, 0.0), ), - placement2d.Filter{}, + placement2d.FullMask, ) Expect(ok).To(BeTrue()) - Expect(contact.TargetShapeID).To(Equal(placement2d.InvalidShapeID)) - Expect(contact.TargetMeshID).To(Equal(meshID)) + Expect(contact.TargetShapeID).To(Equal(nearShapeID)) }) - It("returns false when the segment misses everything", func() { - objID := scene.CreateObject(placement2d.ObjectInfo[string]{}) - scene.AttachCircle(objID, placement2d.CircleInfo[string]{ - Circle: circleAt(0.0, 0.0, 1.0), + It("finds a terrain shape crossed by the segment", func() { + terrainID := scene.CreateTerrain(placement2d.TerrainInfo[string]{}) + meshID := scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ + Mesh: lineMesh(0.0, 0.0, 5.0), }) - _, ok := scene.CheckSegmentIntersection( + contact, ok := scene.CheckSegmentTerrainIntersection( shape2d.NewSegment( - dprec.NewVec2(-5.0, 5.0), - dprec.NewVec2(5.0, 5.0), + dprec.NewVec2(2.0, -5.0), + dprec.NewVec2(2.0, 5.0), ), - placement2d.Filter{}, + placement2d.FullMask, ) - Expect(ok).To(BeFalse()) + Expect(ok).To(BeTrue()) + Expect(contact.SourceShapeID).To(Equal(placement2d.NilObjectShapeID)) + Expect(contact.TargetTerrainID).To(Equal(terrainID)) + Expect(contact.TargetShapeID).To(Equal(meshID)) }) - It("skips dynamic shapes when SkipDynamic is set", func() { + It("returns false when the segment misses everything", func() { objID := scene.CreateObject(placement2d.ObjectInfo[string]{}) scene.AttachCircle(objID, placement2d.CircleInfo[string]{ Circle: circleAt(0.0, 0.0, 1.0), }) - _, ok := scene.CheckSegmentIntersection( + _, ok := scene.CheckSegmentObjectIntersection( shape2d.NewSegment( - dprec.NewVec2(-5.0, 0.0), - dprec.NewVec2(5.0, 0.0), + dprec.NewVec2(-5.0, 5.0), + dprec.NewVec2(5.0, 5.0), ), - placement2d.Filter{SkipDynamic: true}, + placement2d.FullMask, ) Expect(ok).To(BeFalse()) }) - It("skips static meshes when SkipStatic is set", func() { - scene.CreateMesh(placement2d.MeshInfo[string]{ + It("keeps object and terrain queries separate", func() { + terrainID := scene.CreateTerrain(placement2d.TerrainInfo[string]{}) + scene.AttachMesh(terrainID, placement2d.MeshInfo[string]{ Mesh: lineMesh(0.0, 0.0, 5.0), }) - _, ok := scene.CheckSegmentIntersection( + _, ok := scene.CheckSegmentObjectIntersection( shape2d.NewSegment( dprec.NewVec2(0.0, 5.0), dprec.NewVec2(0.0, -5.0), ), - placement2d.Filter{SkipStatic: true}, + placement2d.FullMask, ) Expect(ok).To(BeFalse()) }) diff --git a/core/spatial/placement2d/terrain.go b/core/spatial/placement2d/terrain.go new file mode 100644 index 00000000..d88fc118 --- /dev/null +++ b/core/spatial/placement2d/terrain.go @@ -0,0 +1,23 @@ +package placement2d + +// NilTerrainID indicates a terrain that can never be part of the scene. +const NilTerrainID = TerrainID(nilIndex) + +// TerrainID is a reference to a terrain in the scene. +type TerrainID int32 + +// TerrainInfo contains the information needed to create a terrain in a scene. +// +// Unlike an object, a terrain has no transform of its own. The shapes that are +// attached to it are specified directly in world space. +type TerrainInfo[T any] struct { + + // UserData allows one to attach custom user data to a terrain. + UserData T +} + +type terrainState[T any] struct { + firstShapeIndex int32 + lastShapeIndex int32 + userData T +} diff --git a/core/spatial/placement2d/terrain_shape.go b/core/spatial/placement2d/terrain_shape.go new file mode 100644 index 00000000..981933ae --- /dev/null +++ b/core/spatial/placement2d/terrain_shape.go @@ -0,0 +1,69 @@ +package placement2d + +import ( + "github.com/mokiat/lacking/core/spatial/query2d" + "github.com/mokiat/lacking/core/spatial/shape2d" +) + +// NilTerrainShapeID indicates a terrain shape that can never be part of the +// scene. +const NilTerrainShapeID = TerrainShapeID(nilIndex) + +// TerrainShapeID is a reference to a concave shape that is attached to a +// terrain in the scene. +type TerrainShapeID int32 + +// MeshInfo contains the information needed to create a mesh shape. +type MeshInfo[S any] struct { + + // Filtering holds the collision-filtering metadata for the mesh. + Filtering FilterInfo + + // UserData allows one to attach custom user data to the mesh. + UserData S + + // Mesh contains the mesh information. + // + // The edges of the mesh are specified in world space, since terrains have + // no transform of their own. Use [shape2d.TransformedMesh] to place a mesh + // that is modeled around the origin. + // + // The edge slice is retained rather than copied, so it must not be + // modified afterwards. + // + // The mesh must have at least one edge. An empty mesh has no area to be + // placed in the scene and attaching it panics. + Mesh shape2d.Mesh +} + +type terrainShapeState[S any] struct { + terrainIndex int32 + nextShapeIndex int32 + prevShapeIndex int32 + spatialID query2d.TreeItemID + filterRepresentation + terrainShapeRepresentation + userData S +} + +// objectTerrainShapesCanIntersect reports whether the specified object shape +// and terrain shape are allowed to be checked for intersection. +func objectTerrainShapesCanIntersect[S any](objectShape *objectShapeState[S], terrainShape *terrainShapeState[S]) bool { + return objectShape.canInteractWith(&terrainShape.filterRepresentation) +} + +// TODO: Consider using a different storage mechanism. For example a +// Quadtree or BVH structure. +// +// TODO: Consider abstracting the edges through a resolver that can find +// candidate edges for bounding circles, allowing for heightline or other +// implementations (types of shapes). + +// terrainShapeRepresentation holds the world-space geometry of a terrain +// shape. As terrains cannot be relocated, there is no local-space counterpart +// and the representation never needs to be updated after construction. +type terrainShapeRepresentation struct { + wsBCircle shape2d.Circle + wsAABB shape2d.AABB + wsEdges []shape2d.Edge +} From 3346951c139ca769322e069d434462b9c2395aa2 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 9 Aug 2026 18:06:58 +0300 Subject: [PATCH 69/85] Align physics with new placement API --- game/physics/body.go | 20 ++-- game/physics/collision.go | 13 ++- game/physics/contact_body.go | 93 ++++++++++++++++++ game/physics/contact_terrain.go | 93 ++++++++++++++++++ game/physics/scene.go | 165 +++++++++++++++++++------------- game/physics/terrain.go | 75 +++++++++++---- 6 files changed, 358 insertions(+), 101 deletions(-) create mode 100644 game/physics/contact_body.go create mode 100644 game/physics/contact_terrain.go diff --git a/game/physics/body.go b/game/physics/body.go index 40a1cb9a..fffdea32 100644 --- a/game/physics/body.go +++ b/game/physics/body.go @@ -73,7 +73,7 @@ func (v BodyView) Delete(id BodyID) { } *body = bodyState{ - objectID: placement3d.InvalidObjectID, + objectID: placement3d.NilObjectID, revision: body.revision + 1, // progress revision to invalid (even) value firstBodyAcceleratorIndex: nilIndex, firstSoloConstraintIndex: nilIndex, @@ -176,7 +176,7 @@ func (v BodyView) SetRotation(id BodyID, rotation dprec.Quat) { v.refreshPlacement(body) } -func (v BodyView) AttachCollisionSphere(id BodyID, col CollisionSphere) CollisionShapeID { +func (v BodyView) AttachCollisionSphere(id BodyID, col CollisionSphere) BodyCollisionShapeID { body := v.resolve(id, true) shapeID := v.scene.collisionScene.AttachSphere(body.objectID, placement3d.SphereInfo[shapeData]{ Sphere: col.Shape, @@ -186,13 +186,13 @@ func (v BodyView) AttachCollisionSphere(id BodyID, col CollisionSphere) Collisio restitutionCoefficient: col.RestitutionCoefficient, }, }) - return CollisionShapeID{ + return BodyCollisionShapeID{ bodyID: id, shapeID: shapeID, } } -func (v BodyView) AttachCollisionBox(id BodyID, col CollisionBox) CollisionShapeID { +func (v BodyView) AttachCollisionBox(id BodyID, col CollisionBox) BodyCollisionShapeID { body := v.resolve(id, true) shapeID := v.scene.collisionScene.AttachBox(body.objectID, placement3d.BoxInfo[shapeData]{ Box: col.Shape, @@ -202,17 +202,17 @@ func (v BodyView) AttachCollisionBox(id BodyID, col CollisionBox) CollisionShape restitutionCoefficient: col.RestitutionCoefficient, }, }) - return CollisionShapeID{ + return BodyCollisionShapeID{ bodyID: id, shapeID: shapeID, } } -func (v BodyView) DetachCollisionShape(id BodyID, shapeID CollisionShapeID) { +func (v BodyView) DetachCollisionShape(id BodyID, shapeID BodyCollisionShapeID) { if id != shapeID.bodyID { panic("invalid shape ID for body") } - v.scene.collisionScene.DeleteShape(shapeID.shapeID) + v.scene.collisionScene.DeleteObjectShape(shapeID.shapeID) } func (v BodyView) refreshPlacement(body *bodyState) { @@ -324,15 +324,15 @@ func (h BodyHandle) SetRotation(rotation dprec.Quat) { h.view.SetRotation(h.id, rotation) } -func (h BodyHandle) AttachCollisionSphere(shape CollisionSphere) CollisionShapeID { +func (h BodyHandle) AttachCollisionSphere(shape CollisionSphere) BodyCollisionShapeID { return h.view.AttachCollisionSphere(h.id, shape) } -func (h BodyHandle) AttachCollisionBox(shape CollisionBox) CollisionShapeID { +func (h BodyHandle) AttachCollisionBox(shape CollisionBox) BodyCollisionShapeID { return h.view.AttachCollisionBox(h.id, shape) } -func (h BodyHandle) DetachCollisionShape(shapeID CollisionShapeID) { +func (h BodyHandle) DetachCollisionShape(shapeID BodyCollisionShapeID) { h.view.DetachCollisionShape(h.id, shapeID) } diff --git a/game/physics/collision.go b/game/physics/collision.go index e9658802..8705c260 100644 --- a/game/physics/collision.go +++ b/game/physics/collision.go @@ -5,9 +5,18 @@ import ( "github.com/mokiat/lacking/core/spatial/shape3d" ) -type CollisionShapeID struct { +type Mask = placement3d.Mask + +const FullMask = placement3d.FullMask + +type BodyCollisionShapeID struct { bodyID BodyID - shapeID placement3d.ShapeID + shapeID placement3d.ObjectShapeID +} + +type TerrainCollisionShapeID struct { + terrainID TerrainID + shapeID placement3d.TerrainShapeID } type CollisionShape[T any] struct { diff --git a/game/physics/contact_body.go b/game/physics/contact_body.go new file mode 100644 index 00000000..5d2abb4d --- /dev/null +++ b/game/physics/contact_body.go @@ -0,0 +1,93 @@ +package physics + +import "github.com/mokiat/lacking/core/spatial/shape3d" + +type BodyContact struct { + TargetBodyID BodyID + shape3d.Contact +} + +type BodyContactCallback func(contact BodyContact) + +type DeepestBodyContact struct { + contact BodyContact + hasContact bool +} + +func (c *DeepestBodyContact) Reset() { + c.hasContact = false +} + +func (c *DeepestBodyContact) AddContact(contact BodyContact) { + if !c.hasContact || contact.Depth > c.contact.Depth { + c.contact = contact + c.hasContact = true + } +} + +func (c *DeepestBodyContact) Contact() (BodyContact, bool) { + return c.contact, c.hasContact +} + +type ShallowestBodyContact struct { + contact BodyContact + hasContact bool +} + +func (c *ShallowestBodyContact) Reset() { + c.hasContact = false +} + +func (c *ShallowestBodyContact) AddContact(contact BodyContact) { + if !c.hasContact || contact.Depth < c.contact.Depth { + c.contact = contact + c.hasContact = true + } +} + +func (c *ShallowestBodyContact) Contact() (BodyContact, bool) { + return c.contact, c.hasContact +} + +type BodyContactList []BodyContact + +// Reset clears the retained contacts while preserving the underlying capacity +// so it can be reused without reallocating. +func (l *BodyContactList) Reset() { + *l = (*l)[:0] +} + +// AddContact appends the given contact to the list. +func (l *BodyContactList) AddContact(contact BodyContact) { + *l = append(*l, contact) +} + +// Contacts returns the retained contacts in the order they were added. +// +// The result aliases the internal storage and remains valid until the next +// AddContact or Reset call. +func (l BodyContactList) Contacts() []BodyContact { + return l +} + +type LastBodyContact struct { + contact BodyContact + hasContact bool +} + +// Reset clears any retained contact. +func (c *LastBodyContact) Reset() { + c.hasContact = false +} + +// AddContact retains the given contact, replacing any previously retained one. +func (c *LastBodyContact) AddContact(contact BodyContact) { + c.contact = contact + c.hasContact = true +} + +// Contact returns the retained contact and whether one was added since the +// last Reset. +func (c *LastBodyContact) Contact() (BodyContact, bool) { + return c.contact, c.hasContact +} diff --git a/game/physics/contact_terrain.go b/game/physics/contact_terrain.go new file mode 100644 index 00000000..bdbb131d --- /dev/null +++ b/game/physics/contact_terrain.go @@ -0,0 +1,93 @@ +package physics + +import "github.com/mokiat/lacking/core/spatial/shape3d" + +type TerrainContact struct { + TargetTerrainID TerrainID + shape3d.Contact +} + +type TerrainContactCallback func(contact TerrainContact) + +type DeepestTerrainContact struct { + contact TerrainContact + hasContact bool +} + +func (c *DeepestTerrainContact) Reset() { + c.hasContact = false +} + +func (c *DeepestTerrainContact) AddContact(contact TerrainContact) { + if !c.hasContact || contact.Depth > c.contact.Depth { + c.contact = contact + c.hasContact = true + } +} + +func (c *DeepestTerrainContact) Contact() (TerrainContact, bool) { + return c.contact, c.hasContact +} + +type ShallowestTerrainContact struct { + contact TerrainContact + hasContact bool +} + +func (c *ShallowestTerrainContact) Reset() { + c.hasContact = false +} + +func (c *ShallowestTerrainContact) AddContact(contact TerrainContact) { + if !c.hasContact || contact.Depth < c.contact.Depth { + c.contact = contact + c.hasContact = true + } +} + +func (c *ShallowestTerrainContact) Contact() (TerrainContact, bool) { + return c.contact, c.hasContact +} + +type TerrainContactList []TerrainContact + +// Reset clears the retained contacts while preserving the underlying capacity +// so it can be reused without reallocating. +func (l *TerrainContactList) Reset() { + *l = (*l)[:0] +} + +// AddContact appends the given contact to the list. +func (l *TerrainContactList) AddContact(contact TerrainContact) { + *l = append(*l, contact) +} + +// Contacts returns the retained contacts in the order they were added. +// +// The result aliases the internal storage and remains valid until the next +// AddContact or Reset call. +func (l TerrainContactList) Contacts() []TerrainContact { + return l +} + +type LastTerrainContact struct { + contact TerrainContact + hasContact bool +} + +// Reset clears any retained contact. +func (c *LastTerrainContact) Reset() { + c.hasContact = false +} + +// AddContact retains the given contact, replacing any previously retained one. +func (c *LastTerrainContact) AddContact(contact TerrainContact) { + c.contact = contact + c.hasContact = true +} + +// Contact returns the retained contact and whether one was added since the +// last Reset. +func (c *LastTerrainContact) Contact() (TerrainContact, bool) { + return c.contact, c.hasContact +} diff --git a/game/physics/scene.go b/game/physics/scene.go index 600f2b34..b95861ff 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -18,7 +18,7 @@ import ( // a number of bodies that are independent on any // bodies managed by other scene objects. type Scene struct { - collisionScene *placement3d.Scene[bodyData, shapeData, terrainData] + collisionScene *placement3d.Scene[bodyData, terrainData, shapeData] soloCollisionSubscriptions *observer.SubscriptionSet[SoloCollisionCallback] pairCollisionSubscriptions *observer.SubscriptionSet[PairCollisionCallback] @@ -35,7 +35,8 @@ type Scene struct { newSoloCollisionRefs map[soloCollisionRef]struct{} newPairCollisionRefs map[pairCollisionRef]struct{} - collisionContacts placement3d.ContactList + objectContacts placement3d.ObjectContactList + terrainContacts placement3d.TerrainContactList freeCollisionRejectGroup uint32 @@ -70,7 +71,7 @@ type Scene struct { func NewScene() *Scene { return &Scene{ - collisionScene: placement3d.NewScene[bodyData, shapeData, terrainData](placement3d.SceneSettings{ + collisionScene: placement3d.NewScene[bodyData, terrainData, shapeData](placement3d.SceneSettings{ Size: opt.V(16384.0), MaxDepth: opt.V[uint32](12), InitialNodeCapacity: opt.V[uint32](1024), @@ -92,7 +93,8 @@ func NewScene() *Scene { newSoloCollisionRefs: make(map[soloCollisionRef]struct{}), newPairCollisionRefs: make(map[pairCollisionRef]struct{}), - collisionContacts: make(placement3d.ContactList, 0), + objectContacts: make(placement3d.ObjectContactList, 0), + terrainContacts: make(placement3d.TerrainContactList, 0), freeCollisionRejectGroup: 0, @@ -391,6 +393,44 @@ func (s *Scene) Update(elapsedTime time.Duration) { s.notifyPairCollisions() } +func (s *Scene) CollectSegmentBodyIntersections(segment shape3d.Segment, mask Mask, yield BodyContactCallback) { + bodyView := s.Bodies() + + s.collisionScene.CollectSegmentObjectIntersections(segment, mask, func(contact placement3d.ObjectContact) { + tgtBodyData := s.collisionScene.GetObjectUserData(contact.TargetObjectID) + + yield(BodyContact{ + TargetBodyID: bodyView.idFromIndex(tgtBodyData.index), + Contact: contact.Contact, + }) + }) +} + +func (s *Scene) CheckSegmentBodyIntersection(segment shape3d.Segment, mask Mask) (BodyContact, bool) { + var collection DeepestBodyContact + s.CollectSegmentBodyIntersections(segment, mask, collection.AddContact) + return collection.Contact() +} + +func (s *Scene) CollectSegmentTerrainIntersections(segment shape3d.Segment, mask Mask, yield TerrainContactCallback) { + terrainView := s.Terrains() + + s.collisionScene.CollectSegmentTerrainIntersections(segment, mask, func(contact placement3d.TerrainContact) { + tgtTerrainData := s.collisionScene.GetTerrainUserData(contact.TargetTerrainID) + + yield(TerrainContact{ + TargetTerrainID: terrainView.idFromIndex(tgtTerrainData.index), + Contact: contact.Contact, + }) + }) +} + +func (s *Scene) CheckSegmentTerrainIntersection(segment shape3d.Segment, mask Mask) (TerrainContact, bool) { + var collection DeepestTerrainContact + s.CollectSegmentTerrainIntersections(segment, mask, collection.AddContact) + return collection.Contact() +} + func (s *Scene) allocateGlobalAccelerator() (int32, *globalAcceleratorState) { var index int32 if s.freeGlobalAcceleratorIndices.IsEmpty() { @@ -767,52 +807,59 @@ func (s *Scene) detectCollisions() { s.pairCollisionSolvers = s.pairCollisionSolvers[:0] // Collect new contacts. - s.collisionContacts.Reset() - s.collisionScene.CollectIntersections(s.collisionContacts.AddContact) + s.objectContacts.Reset() + s.collisionScene.CollectObjectIntersections(s.objectContacts.AddContact) + + s.terrainContacts.Reset() + s.collisionScene.CollectTerrainIntersections(s.terrainContacts.AddContact) // Handle contacts. - for _, contact := range s.collisionContacts.Contacts() { - srcBodyObject := s.collisionScene.GetShapeObject(contact.SourceShapeID) - srcShapeData := s.collisionScene.GetShapeUserData(contact.SourceShapeID) - srcBodyData := s.collisionScene.GetObjectUserData(srcBodyObject) - - if contact.TargetMeshID == placement3d.InvalidMeshID { - tgtBodyObject := s.collisionScene.GetShapeObject(contact.TargetShapeID) - tgtShapeData := s.collisionScene.GetShapeUserData(contact.TargetShapeID) - tgtBodyData := s.collisionScene.GetObjectUserData(tgtBodyObject) - s.handlePairCollision( - bodyCollisionData{ - srcBodyData.index, - srcShapeData.frictionCoefficient, - srcShapeData.restitutionCoefficient, - }, - bodyCollisionData{ - tgtBodyData.index, - tgtShapeData.frictionCoefficient, - tgtShapeData.restitutionCoefficient, - }, - contact, - ) - } else { - tgtTerrainData := s.collisionScene.GetMeshUserData(contact.TargetMeshID) - s.handleSoloCollision( - bodyCollisionData{ - srcBodyData.index, - srcShapeData.frictionCoefficient, - srcShapeData.restitutionCoefficient, - }, - terrainCollisionData{ - tgtTerrainData.index, - tgtTerrainData.frictionCoefficient, - tgtTerrainData.restitutionCoefficient, - }, - contact, - ) - } + for _, contact := range s.objectContacts.Contacts() { + srcBodyData := s.collisionScene.GetObjectUserData(contact.SourceObjectID) + srcShapeData := s.collisionScene.GetObjectShapeUserData(contact.SourceShapeID) + + tgtBodyData := s.collisionScene.GetObjectUserData(contact.TargetObjectID) + tgtShapeData := s.collisionScene.GetObjectShapeUserData(contact.TargetShapeID) + + s.handlePairCollision( + bodyCollisionData{ + srcBodyData.index, + srcShapeData.frictionCoefficient, + srcShapeData.restitutionCoefficient, + }, + bodyCollisionData{ + tgtBodyData.index, + tgtShapeData.frictionCoefficient, + tgtShapeData.restitutionCoefficient, + }, + contact, + ) + } + + for _, contact := range s.terrainContacts.Contacts() { + srcBodyData := s.collisionScene.GetObjectUserData(contact.SourceObjectID) + srcShapeData := s.collisionScene.GetObjectShapeUserData(contact.SourceShapeID) + + tgtTerrainData := s.collisionScene.GetTerrainUserData(contact.TargetTerrainID) + tgtShapeData := s.collisionScene.GetTerrainShapeUserData(contact.TargetShapeID) + + s.handleSoloCollision( + bodyCollisionData{ + srcBodyData.index, + srcShapeData.frictionCoefficient, + srcShapeData.restitutionCoefficient, + }, + terrainCollisionData{ + tgtTerrainData.index, + tgtShapeData.frictionCoefficient, + tgtShapeData.restitutionCoefficient, + }, + contact, + ) } } -func (s *Scene) handlePairCollision(primaryData, secondaryData bodyCollisionData, contact placement3d.Contact) { +func (s *Scene) handlePairCollision(primaryData, secondaryData bodyCollisionData, contact placement3d.ObjectContact) { solver := s.allocatePairCollisionSolver() solver.Configure(PairCollisionSolverConfig{ PrimaryFrictionCoefficient: primaryData.frictionCoefficient, @@ -841,7 +888,7 @@ func (s *Scene) handlePairCollision(primaryData, secondaryData bodyCollisionData s.newPairCollisionRefs[ref] = struct{}{} } -func (s *Scene) handleSoloCollision(bodyData bodyCollisionData, terrainData terrainCollisionData, contact placement3d.Contact) { +func (s *Scene) handleSoloCollision(bodyData bodyCollisionData, terrainData terrainCollisionData, contact placement3d.TerrainContact) { solver := s.allocateSoloCollisionSolver() solver.Configure(SoloCollisionSolverConfig{ TerrainFrictionCoefficient: terrainData.frictionCoefficient, @@ -932,9 +979,7 @@ type shapeData struct { } type terrainData struct { - index int32 - frictionCoefficient float64 - restitutionCoefficient float64 + index int32 } type bodyCollisionData struct { @@ -949,11 +994,13 @@ type terrainCollisionData struct { restitutionCoefficient float64 } +// TODO: Consider tracking the shapeID as well. type soloCollisionRef struct { bodyID BodyID terrainID TerrainID } +// TODO: Consider tracking the shapeID as well. type pairCollisionRef struct { primaryBodyID BodyID secondaryBodyID BodyID @@ -994,23 +1041,3 @@ type pairCollisionRef struct { // } // }) // } - -func (s *Scene) CheckSegmentIntersection(segment shape3d.Segment, mask uint32) (BodyID, bool) { - intersection, ok := s.collisionScene.CheckSegmentIntersection(segment, placement3d.Filter{ - Mask: opt.V(mask), - }) - if !ok { - return NilBodyID, false - } - if intersection.TargetShapeID == placement3d.InvalidShapeID { - // A prop. - return NilBodyID, false // FIXME: This should handle props as well. - } - objectID := s.collisionScene.GetShapeObject(intersection.TargetShapeID) - bData := s.collisionScene.GetObjectUserData(objectID) - body := &s.bodies[bData.index] - return BodyID{ - index: bData.index, - revision: body.revision, - }, true -} diff --git a/game/physics/terrain.go b/game/physics/terrain.go index 2a57b986..1c59e1df 100644 --- a/game/physics/terrain.go +++ b/game/physics/terrain.go @@ -1,8 +1,6 @@ package physics import ( - "github.com/mokiat/gog/opt" - "github.com/mokiat/gomath/dprec" "github.com/mokiat/lacking/core/spatial/placement3d" ) @@ -23,24 +21,18 @@ type TerrainView struct { // // However, this requires rework of placement3d API. -func (v TerrainView) Create(position dprec.Vec3, rotation dprec.Quat, mesh CollisionMesh) TerrainID { +func (v TerrainView) Create() TerrainID { index, terrain := v.scene.allocateTerrain() - meshID := v.scene.collisionScene.CreateMesh(placement3d.MeshInfo[terrainData]{ - Position: opt.V(position), - Rotation: opt.V(rotation), - Mesh: mesh.Shape, - Filtering: mesh.Filtering, + terrainID := v.scene.collisionScene.CreateTerrain(placement3d.TerrainInfo[terrainData]{ UserData: terrainData{ - index: index, - frictionCoefficient: mesh.FrictionCoefficient, - restitutionCoefficient: mesh.RestitutionCoefficient, + index: index, }, }) *terrain = terrainState{ - meshID: meshID, - revision: terrain.revision + 1, // progress revision to valid (odd) value + terrainID: terrainID, + revision: terrain.revision + 1, // progress revision to valid (odd) value } return TerrainID{ @@ -53,18 +45,18 @@ func (v TerrainView) Create(position dprec.Vec3, rotation dprec.Quat, mesh Colli // ID in a [TerrainHandle], as returned by [TerrainView.Handle], for // callers that want to keep acting on the new terrain without holding // onto its ID separately. -func (v TerrainView) CreateHandle(position dprec.Vec3, rotation dprec.Quat, mesh CollisionMesh) TerrainHandle { - return v.Handle(v.Create(position, rotation, mesh)) +func (v TerrainView) CreateHandle() TerrainHandle { + return v.Handle(v.Create()) } func (v TerrainView) Delete(id TerrainID) { terrain := v.resolve(id, true) - v.scene.collisionScene.DeleteMesh(terrain.meshID) + v.scene.collisionScene.DeleteTerrain(terrain.terrainID) *terrain = terrainState{ - meshID: placement3d.InvalidMeshID, - revision: terrain.revision + 1, // progress revision to invalid (even) value + terrainID: placement3d.NilTerrainID, + revision: terrain.revision + 1, // progress revision to invalid (even) value } v.scene.releaseTerrain(id.index) @@ -93,6 +85,29 @@ func (v TerrainView) IsValid(id TerrainID) bool { return terrain != nil } +func (v TerrainView) AttachCollisionMesh(id TerrainID, col CollisionMesh) TerrainCollisionShapeID { + terrain := v.resolve(id, true) + shapeID := v.scene.collisionScene.AttachMesh(terrain.terrainID, placement3d.MeshInfo[shapeData]{ + Mesh: col.Shape, + Filtering: col.Filtering, + UserData: shapeData{ + frictionCoefficient: col.FrictionCoefficient, + restitutionCoefficient: col.RestitutionCoefficient, + }, + }) + return TerrainCollisionShapeID{ + terrainID: id, + shapeID: shapeID, + } +} + +func (v TerrainView) DetachCollisionShape(id TerrainID, shapeID TerrainCollisionShapeID) { + if id != shapeID.terrainID { + panic("invalid shape ID for terrain") + } + v.scene.collisionScene.DeleteTerrainShape(shapeID.shapeID) +} + func (v TerrainView) idFromIndex(index int32) TerrainID { terrain := &v.scene.terrains[index] return TerrainID{ @@ -135,9 +150,29 @@ type TerrainHandle struct { id TerrainID } +func (v TerrainHandle) ID() TerrainID { + return v.id +} + +func (v TerrainHandle) Delete() { + v.view.Delete(v.id) +} + +func (v TerrainHandle) IsValid() bool { + return v.view.IsValid(v.id) +} + +func (v TerrainHandle) AttachCollisionMesh(col CollisionMesh) TerrainCollisionShapeID { + return v.view.AttachCollisionMesh(v.id, col) +} + +func (v TerrainHandle) DetachCollisionShape(shapeID TerrainCollisionShapeID) { + v.view.DetachCollisionShape(v.id, shapeID) +} + type terrainState struct { - meshID placement3d.MeshID - revision int32 + terrainID placement3d.TerrainID + revision int32 } func (s *terrainState) isValid() bool { From 1f9fcc69c51b792400e75b542704ae430c0df0ea Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 9 Aug 2026 18:33:40 +0300 Subject: [PATCH 70/85] Add hybrid contact sinks --- core/spatial/placement2d/contact_hybrid.go | 156 +++++++++++++++++++++ core/spatial/placement2d/doc.go | 5 +- core/spatial/placement3d/contact_hybrid.go | 156 +++++++++++++++++++++ core/spatial/placement3d/doc.go | 5 +- 4 files changed, 320 insertions(+), 2 deletions(-) create mode 100644 core/spatial/placement2d/contact_hybrid.go create mode 100644 core/spatial/placement3d/contact_hybrid.go diff --git a/core/spatial/placement2d/contact_hybrid.go b/core/spatial/placement2d/contact_hybrid.go new file mode 100644 index 00000000..077b0f28 --- /dev/null +++ b/core/spatial/placement2d/contact_hybrid.go @@ -0,0 +1,156 @@ +package placement2d + +// DeepestContact is a contact sink that retains the added [ObjectContact] and +// [TerrainContact] with the greatest Depth across both kinds. +// +// Its AddObjectContact method satisfies [ObjectContactCallback] and its +// AddTerrainContact method satisfies [TerrainContactCallback], so both can be +// passed directly to intersection routines. This makes it possible to pick the +// single deepest intersection without having to know in advance whether it will +// be with an object shape or with a terrain shape. +// +// At most one of [DeepestContact.ObjectContact] and +// [DeepestContact.TerrainContact] returns a contact. Ties in Depth are resolved +// in favor of the object contact. +type DeepestContact struct { + objectSink DeepestObjectContact + terrainSink DeepestTerrainContact +} + +// Reset clears any retained contacts. +func (c *DeepestContact) Reset() { + c.objectSink.Reset() + c.terrainSink.Reset() +} + +// AddObjectContact retains the given contact if it is deeper than any +// previously retained object contact. +func (c *DeepestContact) AddObjectContact(contact ObjectContact) { + c.objectSink.AddContact(contact) +} + +// AddTerrainContact retains the given contact if it is deeper than any +// previously retained terrain contact. +func (c *DeepestContact) AddTerrainContact(contact TerrainContact) { + c.terrainSink.AddContact(contact) +} + +// ObjectContact returns the deepest retained object contact and whether it is +// the deepest contact overall. +// +// The result is false when no object contact was added since the last Reset, as +// well as when a strictly deeper terrain contact was added, in which case +// [DeepestContact.TerrainContact] holds the deepest contact instead. +func (c *DeepestContact) ObjectContact() (ObjectContact, bool) { + objectContact, ok := c.objectSink.Contact() + if !ok { + return ObjectContact{}, false + } + terrainContact, ok := c.terrainSink.Contact() + if !ok { + return objectContact, true + } + if objectContact.Depth < terrainContact.Depth { + return ObjectContact{}, false + } + return objectContact, true +} + +// TerrainContact returns the deepest retained terrain contact and whether it is +// the deepest contact overall. +// +// The result is false when no terrain contact was added since the last Reset, +// as well as when an equally deep or deeper object contact was added, in which +// case [DeepestContact.ObjectContact] holds the deepest contact instead. +func (c *DeepestContact) TerrainContact() (TerrainContact, bool) { + terrainContact, ok := c.terrainSink.Contact() + if !ok { + return TerrainContact{}, false + } + objectContact, ok := c.objectSink.Contact() + if !ok { + return terrainContact, true + } + if terrainContact.Depth <= objectContact.Depth { + return TerrainContact{}, false + } + return terrainContact, true +} + +// ShallowestContact is a contact sink that retains the added [ObjectContact] +// and [TerrainContact] with the smallest Depth across both kinds. +// +// Its AddObjectContact method satisfies [ObjectContactCallback] and its +// AddTerrainContact method satisfies [TerrainContactCallback], so both can be +// passed directly to intersection routines. This makes it possible to pick the +// single shallowest intersection without having to know in advance whether it +// will be with an object shape or with a terrain shape. +// +// At most one of [ShallowestContact.ObjectContact] and +// [ShallowestContact.TerrainContact] returns a contact. Ties in Depth are +// resolved in favor of the object contact. +type ShallowestContact struct { + objectSink ShallowestObjectContact + terrainSink ShallowestTerrainContact +} + +// Reset clears any retained contacts. +func (c *ShallowestContact) Reset() { + c.objectSink.Reset() + c.terrainSink.Reset() +} + +// AddObjectContact retains the given contact if it is shallower than any +// previously retained object contact. +func (c *ShallowestContact) AddObjectContact(contact ObjectContact) { + c.objectSink.AddContact(contact) +} + +// AddTerrainContact retains the given contact if it is shallower than any +// previously retained terrain contact. +func (c *ShallowestContact) AddTerrainContact(contact TerrainContact) { + c.terrainSink.AddContact(contact) +} + +// ObjectContact returns the shallowest retained object contact and whether it +// is the shallowest contact overall. +// +// The result is false when no object contact was added since the last Reset, as +// well as when a strictly shallower terrain contact was added, in which case +// [ShallowestContact.TerrainContact] holds the shallowest contact instead. +func (c *ShallowestContact) ObjectContact() (ObjectContact, bool) { + objectContact, ok := c.objectSink.Contact() + if !ok { + return ObjectContact{}, false + } + terrainContact, ok := c.terrainSink.Contact() + if !ok { + return objectContact, true + } + if objectContact.Depth > terrainContact.Depth { + return ObjectContact{}, false + } + return objectContact, true +} + +// TerrainContact returns the shallowest retained terrain contact and whether it +// is the shallowest contact overall. +// +// The result is false when no terrain contact was added since the last Reset, +// as well as when an equally shallow or shallower object contact was added, in +// which case [ShallowestContact.ObjectContact] holds the shallowest contact +// instead. +func (c *ShallowestContact) TerrainContact() (TerrainContact, bool) { + terrainContact, ok := c.terrainSink.Contact() + if !ok { + return TerrainContact{}, false + } + objectContact, ok := c.objectSink.Contact() + if !ok { + return terrainContact, true + } + if terrainContact.Depth >= objectContact.Depth { + return TerrainContact{}, false + } + return terrainContact, true +} diff --git a/core/spatial/placement2d/doc.go b/core/spatial/placement2d/doc.go index fc7b3abb..327b4a3a 100644 --- a/core/spatial/placement2d/doc.go +++ b/core/spatial/placement2d/doc.go @@ -20,7 +20,10 @@ // an [ObjectContactCallback], whereas an intersection with a terrain shape is // reported as a [TerrainContact] through a [TerrainContactCallback]. A number // of contact sinks (for example [DeepestObjectContact] and -// [TerrainContactList]) are provided for common accumulation strategies. +// [TerrainContactList]) are provided for common accumulation strategies. The +// [DeepestContact] and [ShallowestContact] sinks accept both flavors at once, +// for when the extreme intersection is needed regardless of the kind of the +// target. // // Since terrains cannot move, terrain shapes are never tested against one // another. The source of a [TerrainContact] is always either an object shape diff --git a/core/spatial/placement3d/contact_hybrid.go b/core/spatial/placement3d/contact_hybrid.go new file mode 100644 index 00000000..0b0bc741 --- /dev/null +++ b/core/spatial/placement3d/contact_hybrid.go @@ -0,0 +1,156 @@ +package placement3d + +// DeepestContact is a contact sink that retains the added [ObjectContact] and +// [TerrainContact] with the greatest Depth across both kinds. +// +// Its AddObjectContact method satisfies [ObjectContactCallback] and its +// AddTerrainContact method satisfies [TerrainContactCallback], so both can be +// passed directly to intersection routines. This makes it possible to pick the +// single deepest intersection without having to know in advance whether it will +// be with an object shape or with a terrain shape. +// +// At most one of [DeepestContact.ObjectContact] and +// [DeepestContact.TerrainContact] returns a contact. Ties in Depth are resolved +// in favor of the object contact. +type DeepestContact struct { + objectSink DeepestObjectContact + terrainSink DeepestTerrainContact +} + +// Reset clears any retained contacts. +func (c *DeepestContact) Reset() { + c.objectSink.Reset() + c.terrainSink.Reset() +} + +// AddObjectContact retains the given contact if it is deeper than any +// previously retained object contact. +func (c *DeepestContact) AddObjectContact(contact ObjectContact) { + c.objectSink.AddContact(contact) +} + +// AddTerrainContact retains the given contact if it is deeper than any +// previously retained terrain contact. +func (c *DeepestContact) AddTerrainContact(contact TerrainContact) { + c.terrainSink.AddContact(contact) +} + +// ObjectContact returns the deepest retained object contact and whether it is +// the deepest contact overall. +// +// The result is false when no object contact was added since the last Reset, as +// well as when a strictly deeper terrain contact was added, in which case +// [DeepestContact.TerrainContact] holds the deepest contact instead. +func (c *DeepestContact) ObjectContact() (ObjectContact, bool) { + objectContact, ok := c.objectSink.Contact() + if !ok { + return ObjectContact{}, false + } + terrainContact, ok := c.terrainSink.Contact() + if !ok { + return objectContact, true + } + if objectContact.Depth < terrainContact.Depth { + return ObjectContact{}, false + } + return objectContact, true +} + +// TerrainContact returns the deepest retained terrain contact and whether it is +// the deepest contact overall. +// +// The result is false when no terrain contact was added since the last Reset, +// as well as when an equally deep or deeper object contact was added, in which +// case [DeepestContact.ObjectContact] holds the deepest contact instead. +func (c *DeepestContact) TerrainContact() (TerrainContact, bool) { + terrainContact, ok := c.terrainSink.Contact() + if !ok { + return TerrainContact{}, false + } + objectContact, ok := c.objectSink.Contact() + if !ok { + return terrainContact, true + } + if terrainContact.Depth <= objectContact.Depth { + return TerrainContact{}, false + } + return terrainContact, true +} + +// ShallowestContact is a contact sink that retains the added [ObjectContact] +// and [TerrainContact] with the smallest Depth across both kinds. +// +// Its AddObjectContact method satisfies [ObjectContactCallback] and its +// AddTerrainContact method satisfies [TerrainContactCallback], so both can be +// passed directly to intersection routines. This makes it possible to pick the +// single shallowest intersection without having to know in advance whether it +// will be with an object shape or with a terrain shape. +// +// At most one of [ShallowestContact.ObjectContact] and +// [ShallowestContact.TerrainContact] returns a contact. Ties in Depth are +// resolved in favor of the object contact. +type ShallowestContact struct { + objectSink ShallowestObjectContact + terrainSink ShallowestTerrainContact +} + +// Reset clears any retained contacts. +func (c *ShallowestContact) Reset() { + c.objectSink.Reset() + c.terrainSink.Reset() +} + +// AddObjectContact retains the given contact if it is shallower than any +// previously retained object contact. +func (c *ShallowestContact) AddObjectContact(contact ObjectContact) { + c.objectSink.AddContact(contact) +} + +// AddTerrainContact retains the given contact if it is shallower than any +// previously retained terrain contact. +func (c *ShallowestContact) AddTerrainContact(contact TerrainContact) { + c.terrainSink.AddContact(contact) +} + +// ObjectContact returns the shallowest retained object contact and whether it +// is the shallowest contact overall. +// +// The result is false when no object contact was added since the last Reset, as +// well as when a strictly shallower terrain contact was added, in which case +// [ShallowestContact.TerrainContact] holds the shallowest contact instead. +func (c *ShallowestContact) ObjectContact() (ObjectContact, bool) { + objectContact, ok := c.objectSink.Contact() + if !ok { + return ObjectContact{}, false + } + terrainContact, ok := c.terrainSink.Contact() + if !ok { + return objectContact, true + } + if objectContact.Depth > terrainContact.Depth { + return ObjectContact{}, false + } + return objectContact, true +} + +// TerrainContact returns the shallowest retained terrain contact and whether it +// is the shallowest contact overall. +// +// The result is false when no terrain contact was added since the last Reset, +// as well as when an equally shallow or shallower object contact was added, in +// which case [ShallowestContact.ObjectContact] holds the shallowest contact +// instead. +func (c *ShallowestContact) TerrainContact() (TerrainContact, bool) { + terrainContact, ok := c.terrainSink.Contact() + if !ok { + return TerrainContact{}, false + } + objectContact, ok := c.objectSink.Contact() + if !ok { + return terrainContact, true + } + if terrainContact.Depth >= objectContact.Depth { + return TerrainContact{}, false + } + return terrainContact, true +} diff --git a/core/spatial/placement3d/doc.go b/core/spatial/placement3d/doc.go index faa50bac..a44ed0e5 100644 --- a/core/spatial/placement3d/doc.go +++ b/core/spatial/placement3d/doc.go @@ -20,7 +20,10 @@ // an [ObjectContactCallback], whereas an intersection with a terrain shape is // reported as a [TerrainContact] through a [TerrainContactCallback]. A number // of contact sinks (for example [DeepestObjectContact] and -// [TerrainContactList]) are provided for common accumulation strategies. +// [TerrainContactList]) are provided for common accumulation strategies. The +// [DeepestContact] and [ShallowestContact] sinks accept both flavors at once, +// for when the extreme intersection is needed regardless of the kind of the +// target. // // Since terrains cannot move, terrain shapes are never tested against one // another. The source of a [TerrainContact] is always either an object shape From 1173301e449273ef455ee8898a7ce14409553212 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 9 Aug 2026 23:08:46 +0300 Subject: [PATCH 71/85] Add axis-range constraint solver --- .../constraint/clamp_direction_offset.go | 144 -------- game/physics/solver_axis_range.go | 320 ++++++++++++++++++ 2 files changed, 320 insertions(+), 144 deletions(-) delete mode 100644 game/physics/constraint/clamp_direction_offset.go create mode 100644 game/physics/solver_axis_range.go diff --git a/game/physics/constraint/clamp_direction_offset.go b/game/physics/constraint/clamp_direction_offset.go deleted file mode 100644 index 5a76d5fa..00000000 --- a/game/physics/constraint/clamp_direction_offset.go +++ /dev/null @@ -1,144 +0,0 @@ -package constraint - -// import ( -// "github.com/mokiat/gomath/dprec" -// "github.com/mokiat/lacking/game/physics/solver" -// ) - -// // NewClampDirectionOffset creates a new ClampDirectionOffset constraint solver. -// func NewClampDirectionOffset() *ClampDirectionOffset { -// return &ClampDirectionOffset{ -// direction: dprec.BasisYVec3(), -// min: -1.0, -// max: 1.0, -// restitution: 0.0, -// } -// } - -// var _ solver.PairConstraint = (*ClampDirectionOffset)(nil) - -// // ClampDirectionOffset represents the solution for a constraint which ensures that -// // the second body is within certain min and max bounds relative to the first -// // body along a certain direction of the first body. -// type ClampDirectionOffset struct { -// direction dprec.Vec3 -// min float64 -// max float64 -// restitution float64 - -// jacobian solver.PairJacobian -// drift float64 -// } - -// // Direction returns the constraint direction, which is in local space of -// // the first body. -// func (s *ClampDirectionOffset) Direction() dprec.Vec3 { -// return s.direction -// } - -// // SetDirection changes the constraint direction, which must be in local space -// // of the first body. -// func (s *ClampDirectionOffset) SetDirection(direction dprec.Vec3) *ClampDirectionOffset { -// s.direction = dprec.UnitVec3(direction) -// return s -// } - -// // Min returns the lower bounds limit. -// func (s *ClampDirectionOffset) Min() float64 { -// return s.min -// } - -// // SetMin changes the lower bounds limit. -// func (s *ClampDirectionOffset) SetMin(min float64) *ClampDirectionOffset { -// s.min = min -// return s -// } - -// // Max returns the upper bounds limit. -// func (s *ClampDirectionOffset) Max() float64 { -// return s.max -// } - -// // SetMax changes the upper bounds limit. -// func (s *ClampDirectionOffset) SetMax(max float64) *ClampDirectionOffset { -// s.max = max -// return s -// } - -// // Restitution returns the restitution to be used when adjusting the -// // two bodies when the constraint is not met. -// func (s *ClampDirectionOffset) Restitution() float64 { -// return s.restitution -// } - -// // SetRestitution changes the restitution to be used when adjusting the -// // two bodies when the constraint is not met. -// func (s *ClampDirectionOffset) SetRestitution(restitution float64) *ClampDirectionOffset { -// s.restitution = restitution -// return s -// } - -// func (s *ClampDirectionOffset) Reset(ctx solver.PairContext) { -// dirWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.direction) -// deltaPosition := dprec.Vec3Diff(ctx.Source.Position(), ctx.Target.Position()) -// dirDistance := dprec.Vec3Dot(deltaPosition, dirWS) - -// switch { -// case dirDistance > s.max: -// radius := dprec.Vec3Diff( -// deltaPosition, -// dprec.Vec3Prod(dirWS, dirDistance-s.max), -// ) -// s.jacobian = solver.PairJacobian{ -// Target: solver.Jacobian{ -// LinearSlope: dprec.InverseVec3(dirWS), -// AngularSlope: dprec.Vec3Cross(dirWS, radius), -// }, -// Source: solver.Jacobian{ -// LinearSlope: dirWS, -// AngularSlope: dprec.ZeroVec3(), -// }, -// } -// s.drift = dirDistance - s.max - -// case dirDistance < s.min: -// radius := dprec.Vec3Sum( -// deltaPosition, -// dprec.Vec3Prod(dirWS, s.min-dirDistance), -// ) -// s.jacobian = solver.PairJacobian{ -// Target: solver.Jacobian{ -// LinearSlope: dirWS, -// AngularSlope: dprec.Vec3Cross(radius, dirWS), -// }, -// Source: solver.Jacobian{ -// LinearSlope: dprec.InverseVec3(dirWS), -// AngularSlope: dprec.ZeroVec3(), -// }, -// } -// s.drift = s.min - dirDistance - -// default: -// s.jacobian = solver.PairJacobian{} -// s.drift = 0 -// } -// } - -// func (s *ClampDirectionOffset) ApplyImpulses(ctx solver.PairContext) { -// // TODO: Should drift be passed to this check? -// lambda := ctx.JacobianImpulseLambda(s.jacobian, s.drift, s.restitution) -// if lambda > 0.0 { -// return // moving away -// } -// solution := ctx.JacobianImpulseSolution(s.jacobian, s.drift, 0.0) -// ctx.Target.ApplyImpulse(solution.Target) -// ctx.Source.ApplyImpulse(solution.Source) -// } - -// func (s *ClampDirectionOffset) ApplyNudges(ctx solver.PairContext) { -// if s.drift > 0 { -// solution := ctx.JacobianNudgeSolution(s.jacobian, s.drift) -// ctx.Target.ApplyNudge(solution.Target) -// ctx.Source.ApplyNudge(solution.Source) -// } -// } diff --git a/game/physics/solver_axis_range.go b/game/physics/solver_axis_range.go new file mode 100644 index 00000000..06c2fdca --- /dev/null +++ b/game/physics/solver_axis_range.go @@ -0,0 +1,320 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +// AxisRangeSolverConfig holds the parameters with which an +// [AxisRangeSolver] is configured, either through [NewAxisRangeSolver] or +// [AxisRangeSolver.Configure]. +type AxisRangeSolverConfig struct { + + // PrimaryBodyAnchorOffset is the body-local-space offset, relative to + // the primary target's center of mass, of the point at which the + // distance constraint is anchored on the primary body. + PrimaryBodyAnchorOffset dprec.Vec3 + + // PrimaryBodyAxis is the body-local-space direction, relative to the + // primary target's rotation, along which the distance between the two + // anchor points is measured. It need not be unit-length; it is + // normalized when the solver is configured. + PrimaryBodyAxis dprec.Vec3 + + // SecondaryBodyAnchorOffset is the body-local-space offset, relative + // to the secondary target's center of mass, of the point at which + // the distance constraint is anchored on the secondary body. + SecondaryBodyAnchorOffset dprec.Vec3 + + // MinDisplacement is the lowest permitted signed distance, measured along + // PrimaryBodyAxis, between the two anchor points. + MinDisplacement float64 + + // MaxDisplacement is the highest permitted signed distance, measured + // along PrimaryBodyAxis, between the two anchor points. + MaxDisplacement float64 + + // RestitutionCoefficient is the bounciness applied when the + // axis-aligned distance reaches MinDisplacement or MaxDisplacement. + // Negative values are clamped to zero. + RestitutionCoefficient float64 +} + +// AxisRangeSolver is a [PairConstraintSolver] that keeps the signed +// distance between an anchor point on each of its two target bodies, +// measured along an axis fixed to the primary body, within a +// [AxisRangeSolver.MinDisplacement] and [AxisRangeSolver.MaxDisplacement] +// range - acting like a rigid rod between the two only once one of the range's +// limits is reached, and applying no force while the axis-aligned distance +// is within range. +// +// Unlike [DistanceSolver], which holds the anchor points at a fixed +// distance apart measured along the direction connecting them, +// AxisRangeSolver measures the distance along a single axis fixed to the +// primary body's orientation and only constrains that distance to stay +// within a range, rather than at a fixed value. +// +// An AxisRangeSolver must be configured, either through +// [NewAxisRangeSolver] or [AxisRangeSolver.Configure], before being +// registered with a [Scene] through [PairConstraintView.Create]. +type AxisRangeSolver struct { + primaryBodyAnchorOffset dprec.Vec3 + primaryBodyAxis dprec.Vec3 + secondaryBodyAnchorOffset dprec.Vec3 + minDisplacement float64 + maxDisplacement float64 + restitutionCoefficient float64 + + primaryJacobian Jacobian + secondaryJacobian Jacobian + drift float64 +} + +var _ PairConstraintSolver = (*AxisRangeSolver)(nil) + +// NewAxisRangeSolver creates a new [AxisRangeSolver] configured according +// to config. +func NewAxisRangeSolver(config AxisRangeSolverConfig) *AxisRangeSolver { + result := &AxisRangeSolver{} + result.Configure(config) + return result +} + +// Configure configures this solver according to config. PrimaryBodyAxis +// is normalized, as with [AxisRangeSolver.SetPrimaryBodyAxis], and +// negative RestitutionCoefficient values are clamped to zero, as with +// [AxisRangeSolver.SetRestitutionCoefficient]. +// +// Configure must be called before this solver is registered with a +// [Scene] through [PairConstraintView.Create]. Unlike +// [NewAxisRangeSolver], it can be called on an already-allocated solver, +// which allows solvers to be cached (e.g. in a slice) and configured on +// demand. +func (s *AxisRangeSolver) Configure(config AxisRangeSolverConfig) { + s.primaryBodyAnchorOffset = config.PrimaryBodyAnchorOffset + s.primaryBodyAxis = dprec.UnitVec3(config.PrimaryBodyAxis) + s.secondaryBodyAnchorOffset = config.SecondaryBodyAnchorOffset + s.minDisplacement = config.MinDisplacement + s.maxDisplacement = config.MaxDisplacement + s.restitutionCoefficient = max(0.0, config.RestitutionCoefficient) +} + +// PrimaryBodyAnchorOffset returns the body-local-space offset, relative +// to the primary target's center of mass, of the point at which the +// distance constraint is anchored on the primary body. +func (s *AxisRangeSolver) PrimaryBodyAnchorOffset() dprec.Vec3 { + return s.primaryBodyAnchorOffset +} + +// SetPrimaryBodyAnchorOffset changes the body-local-space offset, +// relative to the primary target's center of mass, of the point at +// which the distance constraint is anchored on the primary body. +// +// It returns the solver itself, so that calls can be chained. +func (s *AxisRangeSolver) SetPrimaryBodyAnchorOffset(offset dprec.Vec3) *AxisRangeSolver { + s.primaryBodyAnchorOffset = offset + return s +} + +// PrimaryBodyAxis returns the body-local-space, unit-length direction, +// relative to the primary target's rotation, along which the distance +// between the two anchor points is measured. +func (s *AxisRangeSolver) PrimaryBodyAxis() dprec.Vec3 { + return s.primaryBodyAxis +} + +// SetPrimaryBodyAxis changes the body-local-space direction, relative to +// the primary target's rotation, along which the distance between the +// two anchor points is measured. The provided axis need not be +// unit-length; it is normalized before being stored. +// +// It returns the solver itself, so that calls can be chained. +func (s *AxisRangeSolver) SetPrimaryBodyAxis(axis dprec.Vec3) *AxisRangeSolver { + s.primaryBodyAxis = dprec.UnitVec3(axis) + return s +} + +// SecondaryBodyAnchorOffset returns the body-local-space offset, +// relative to the secondary target's center of mass, of the point at +// which the distance constraint is anchored on the secondary body. +func (s *AxisRangeSolver) SecondaryBodyAnchorOffset() dprec.Vec3 { + return s.secondaryBodyAnchorOffset +} + +// SetSecondaryBodyAnchorOffset changes the body-local-space offset, +// relative to the secondary target's center of mass, of the point at +// which the distance constraint is anchored on the secondary body. +// +// It returns the solver itself, so that calls can be chained. +func (s *AxisRangeSolver) SetSecondaryBodyAnchorOffset(offset dprec.Vec3) *AxisRangeSolver { + s.secondaryBodyAnchorOffset = offset + return s +} + +// RestitutionCoefficient returns the bounciness applied when the +// axis-aligned distance reaches [AxisRangeSolver.MinDisplacement] or +// [AxisRangeSolver.MaxDisplacement]. +func (s *AxisRangeSolver) RestitutionCoefficient() float64 { + return s.restitutionCoefficient +} + +// SetRestitutionCoefficient changes the bounciness applied when the +// axis-aligned distance reaches [AxisRangeSolver.MinDisplacement] or +// [AxisRangeSolver.MaxDisplacement]. Negative values are clamped to zero. +// +// It returns the solver itself, so that calls can be chained. +func (s *AxisRangeSolver) SetRestitutionCoefficient(coefficient float64) *AxisRangeSolver { + s.restitutionCoefficient = max(0.0, coefficient) + return s +} + +// MinDisplacement returns the lowest permitted signed distance, measured +// along [AxisRangeSolver.PrimaryBodyAxis], between the two anchor +// points. +func (s *AxisRangeSolver) MinDisplacement() float64 { + return s.minDisplacement +} + +// SetMinDisplacement changes the lowest permitted signed distance, +// measured along [AxisRangeSolver.PrimaryBodyAxis], between the two +// anchor points. +// +// It returns the solver itself, so that calls can be chained. +func (s *AxisRangeSolver) SetMinDisplacement(min float64) *AxisRangeSolver { + s.minDisplacement = min + return s +} + +// MaxDisplacement returns the highest permitted signed distance, measured +// along [AxisRangeSolver.PrimaryBodyAxis], between the two anchor +// points. +func (s *AxisRangeSolver) MaxDisplacement() float64 { + return s.maxDisplacement +} + +// SetMaxDisplacement changes the highest permitted signed distance, +// measured along [AxisRangeSolver.PrimaryBodyAxis], between the two +// anchor points. +// +// It returns the solver itself, so that calls can be chained. +func (s *AxisRangeSolver) SetMaxDisplacement(max float64) *AxisRangeSolver { + s.maxDisplacement = max + return s +} + +// Reset implements [PairConstraintSolver.Reset]. +// +// It recomputes the constraint's primary and secondary [Jacobian]s and +// current range violation (drift), the same way +// [AxisRangeSolver.recompute] does. +func (s *AxisRangeSolver) Reset(ctx PairConstraintContext) { + s.recompute(ctx) +} + +// ApplyImpulses implements [PairConstraintSolver.ApplyImpulses]. +// +// If the axis-aligned distance between the two anchor points, as of the +// last call to [AxisRangeSolver.Reset] or +// [AxisRangeSolver.ApplyNudges], is within the +// [AxisRangeSolver.MinDisplacement]/[AxisRangeSolver.MaxDisplacement] +// range, it does nothing. Otherwise, it resolves a pair of impulses, combining +// restitution with Baumgarte positional-drift stabilization, that drive +// the anchor points' relative velocity toward bringing the axis-aligned +// distance back within range. If the two targets are already moving +// apart (back towards the permitted range), it returns without applying +// anything, leaving any remaining violation to +// [AxisRangeSolver.ApplyNudges]. +func (s *AxisRangeSolver) ApplyImpulses(ctx PairConstraintContext) { + if s.drift == 0.0 { + return // no constraint violation + } + + bounceLambda, baumgarteLambda := ctx.ImpulseLambdaComponents(s.primaryJacobian, s.secondaryJacobian, s.drift, s.restitutionCoefficient) + if bounceLambda < 0.0 { + return // moving away + } + + lambda := bounceLambda + baumgarteLambda + primaryImpulse := s.primaryJacobian.Impulse(lambda) + secondaryImpulse := s.secondaryJacobian.Impulse(lambda) + + ctx.PrimaryTarget.ApplyImpulse(primaryImpulse) + ctx.SecondaryTarget.ApplyImpulse(secondaryImpulse) +} + +// ApplyNudges implements [PairConstraintSolver.ApplyNudges]. +// +// It first recomputes the constraint's primary and secondary [Jacobian]s +// and current range violation (drift), the same way +// [AxisRangeSolver.recompute] does, since a preceding nudge - by this +// solver's own previous iteration, or by another constraint acting on +// either target - may have moved either target since +// [AxisRangeSolver.Reset] or the last call to this method. If the +// axis-aligned distance is within range, it does nothing; otherwise, it +// nudges both targets' positions and rotations to bring the axis-aligned +// distance back within the +// [AxisRangeSolver.MinDisplacement]/[AxisRangeSolver.MaxDisplacement] +// range. +func (s *AxisRangeSolver) ApplyNudges(ctx PairConstraintContext) { + s.recompute(ctx) + + if s.drift > 0.0 { + primaryNudge, secondaryNudge := ctx.NudgeSolution( + s.primaryJacobian, s.secondaryJacobian, s.drift, + ) + ctx.PrimaryTarget.ApplyNudge(primaryNudge) + ctx.SecondaryTarget.ApplyNudge(secondaryNudge) + } +} + +// recompute recalculates the constraint's primary and secondary +// [Jacobian]s, along with the current range violation (drift), based on +// the targets' current positions and rotations. +// +// It projects the offset between the two anchor points onto +// PrimaryBodyAxis, transformed into world space through the primary +// target's current rotation, to obtain the axis-aligned displacement. If +// that displacement is below MinDisplacement, the Jacobians and drift +// are set up to push the anchor points apart along the axis; if it is +// above MaxDisplacement, they are set up to pull the anchor points +// together along the axis; otherwise both Jacobians and the drift are +// reset to zero, so that [AxisRangeSolver.ApplyImpulses] and +// [AxisRangeSolver.ApplyNudges] apply no correction. +func (s *AxisRangeSolver) recompute(ctx PairConstraintContext) { + primaryAnchorOffsetWS := dprec.QuatVec3Rotation(ctx.PrimaryTarget.Rotation(), s.primaryBodyAnchorOffset) + primaryAnchorWS := dprec.Vec3Sum(ctx.PrimaryTarget.Position(), primaryAnchorOffsetWS) + + secondaryAnchorOffsetWS := dprec.QuatVec3Rotation(ctx.SecondaryTarget.Rotation(), s.secondaryBodyAnchorOffset) + secondaryAnchorWS := dprec.Vec3Sum(ctx.SecondaryTarget.Position(), secondaryAnchorOffsetWS) + + axisWS := dprec.QuatVec3Rotation(ctx.PrimaryTarget.Rotation(), s.primaryBodyAxis) + + delta := dprec.Vec3Diff(secondaryAnchorWS, primaryAnchorWS) + actualDisplacement := dprec.Vec3Dot(axisWS, delta) + + switch { + case actualDisplacement < s.minDisplacement: + s.primaryJacobian = Jacobian{ + LinearSlope: dprec.InverseVec3(axisWS), + AngularSlope: dprec.Vec3Cross(axisWS, primaryAnchorOffsetWS), + } + s.secondaryJacobian = Jacobian{ + LinearSlope: axisWS, + AngularSlope: dprec.Vec3Cross(secondaryAnchorOffsetWS, axisWS), + } + s.drift = s.minDisplacement - actualDisplacement + + case actualDisplacement > s.maxDisplacement: + s.primaryJacobian = Jacobian{ + LinearSlope: axisWS, + AngularSlope: dprec.Vec3Cross(primaryAnchorOffsetWS, axisWS), + } + s.secondaryJacobian = Jacobian{ + LinearSlope: dprec.InverseVec3(axisWS), + AngularSlope: dprec.Vec3Cross(axisWS, secondaryAnchorOffsetWS), + } + s.drift = actualDisplacement - s.maxDisplacement + + default: + s.primaryJacobian = Jacobian{} + s.secondaryJacobian = Jacobian{} + s.drift = 0.0 + } +} From 5a23d224e0731336a57dd9d1c007ce7a9193ce49 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 9 Aug 2026 23:48:36 +0300 Subject: [PATCH 72/85] Add axis-displacement constraint solver --- .../constraint/match_direction_offset.go | 120 --------- game/physics/solver_axis_displacement.go | 237 ++++++++++++++++++ 2 files changed, 237 insertions(+), 120 deletions(-) delete mode 100644 game/physics/constraint/match_direction_offset.go create mode 100644 game/physics/solver_axis_displacement.go diff --git a/game/physics/constraint/match_direction_offset.go b/game/physics/constraint/match_direction_offset.go deleted file mode 100644 index 8d100a8b..00000000 --- a/game/physics/constraint/match_direction_offset.go +++ /dev/null @@ -1,120 +0,0 @@ -package constraint - -// import ( -// "github.com/mokiat/gomath/dprec" -// "github.com/mokiat/lacking/game/physics/solver" -// ) - -// // NewMatchDirectionOffset creates a new MatchDirectionOffset constraint solver. -// func NewMatchDirectionOffset() *MatchDirectionOffset { -// return &MatchDirectionOffset{ -// primaryRadius: dprec.ZeroVec3(), -// secondaryRadius: dprec.ZeroVec3(), -// direction: dprec.BasisYVec3(), -// offset: 0.0, -// } -// } - -// var _ solver.PairConstraint = (*MatchDirectionOffset)(nil) - -// // MatchDirectionOffset represents the solution for a constraint which ensures that -// // the second body is at an exact distance away from the first body along -// // some direction of the first body. -// type MatchDirectionOffset struct { -// primaryRadius dprec.Vec3 -// secondaryRadius dprec.Vec3 -// direction dprec.Vec3 -// offset float64 - -// jacobian solver.PairJacobian -// drift float64 -// } - -// // PrimaryRadius returns the radius vector of the contact point -// // on the primary object. -// // -// // The vector is in the object's local space. -// func (s *MatchDirectionOffset) PrimaryRadius() dprec.Vec3 { -// return s.primaryRadius -// } - -// // SetPrimaryRadius changes the attachment point of the link -// // on the primary body. -// func (s *MatchDirectionOffset) SetPrimaryRadius(radius dprec.Vec3) *MatchDirectionOffset { -// s.primaryRadius = radius -// return s -// } - -// // SecondaryRadius returns the radius vector of the contact point -// // on the secondary object. -// // -// // The vector is in the object's local space. -// func (s *MatchDirectionOffset) SecondaryRadius() dprec.Vec3 { -// return s.secondaryRadius -// } - -// // SetSecondaryRadius changes the radius vector of the contact point -// // on the secondary object. -// // -// // The vector is in the object's local space. -// func (s *MatchDirectionOffset) SetSecondaryRadius(radius dprec.Vec3) *MatchDirectionOffset { -// s.secondaryRadius = radius -// return s -// } - -// // Direction returns the constraint direction, which is in local space of -// // the first body. -// func (s *MatchDirectionOffset) Direction() dprec.Vec3 { -// return s.direction -// } - -// // SetDirection changes the constraint direction, which must be in local space -// // of the first body. -// func (s *MatchDirectionOffset) SetDirection(direction dprec.Vec3) *MatchDirectionOffset { -// s.direction = dprec.UnitVec3(direction) -// return s -// } - -// // Offset returns the directional offset. -// func (s *MatchDirectionOffset) Offset() float64 { -// return s.offset -// } - -// // SetOffset changes the directional offset. -// func (s *MatchDirectionOffset) SetOffset(offset float64) *MatchDirectionOffset { -// s.offset = offset -// return s -// } - -// func (s *MatchDirectionOffset) Reset(ctx solver.PairContext) { -// dirWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.direction) -// primaryRadiusWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.primaryRadius) -// secondaryRadiusWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.secondaryRadius) -// s.jacobian = solver.PairJacobian{ -// Target: solver.Jacobian{ -// LinearSlope: dprec.InverseVec3(dirWS), -// AngularSlope: dprec.Vec3Cross(dirWS, primaryRadiusWS), -// }, -// Source: solver.Jacobian{ -// LinearSlope: dirWS, -// AngularSlope: dprec.Vec3Cross(secondaryRadiusWS, dirWS), -// }, -// } -// deltaPosition := dprec.Vec3Diff( -// dprec.Vec3Sum(ctx.Source.Position(), secondaryRadiusWS), -// dprec.Vec3Sum(ctx.Target.Position(), primaryRadiusWS), -// ) -// s.drift = dprec.Vec3Dot(dirWS, deltaPosition) -// } - -// func (s *MatchDirectionOffset) ApplyImpulses(ctx solver.PairContext) { -// solution := ctx.JacobianImpulseSolution(s.jacobian, s.drift, 0.0) -// ctx.Target.ApplyImpulse(solution.Target) -// ctx.Source.ApplyImpulse(solution.Source) -// } - -// func (s *MatchDirectionOffset) ApplyNudges(ctx solver.PairContext) { -// solution := ctx.JacobianNudgeSolution(s.jacobian, s.drift) -// ctx.Target.ApplyNudge(solution.Target) -// ctx.Source.ApplyNudge(solution.Source) -// } diff --git a/game/physics/solver_axis_displacement.go b/game/physics/solver_axis_displacement.go new file mode 100644 index 00000000..328cb45a --- /dev/null +++ b/game/physics/solver_axis_displacement.go @@ -0,0 +1,237 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +// AxisDisplacementSolverConfig holds the parameters with which an +// [AxisDisplacementSolver] is configured, either through +// [NewAxisDisplacementSolver] or [AxisDisplacementSolver.Configure]. +type AxisDisplacementSolverConfig struct { + + // PrimaryBodyAnchorOffset is the body-local-space offset, relative to + // the primary target's center of mass, of the point at which the + // displacement constraint is anchored on the primary body. + PrimaryBodyAnchorOffset dprec.Vec3 + + // PrimaryBodyAxis is the body-local-space direction, relative to the + // primary target's rotation, along which the displacement between the + // two anchor points is measured. It need not be unit-length; it is + // normalized when the solver is configured. + PrimaryBodyAxis dprec.Vec3 + + // SecondaryBodyAnchorOffset is the body-local-space offset, relative + // to the secondary target's center of mass, of the point at which + // the displacement constraint is anchored on the secondary body. + SecondaryBodyAnchorOffset dprec.Vec3 + + // Displacement is the signed distance, measured along + // PrimaryBodyAxis, at which the two anchor points are held apart. + // Unlike a plain Euclidean distance, it may be negative. + Displacement float64 +} + +// AxisDisplacementSolver is a [PairConstraintSolver] that holds the +// signed distance between an anchor point on each of its two target +// bodies, measured along an axis fixed to the primary body, at a fixed +// [AxisDisplacementSolver.Displacement] - acting like a rigid rod +// between the two, constrained to slide only along that axis. +// +// Unlike [DistanceSolver], which holds the anchor points at a fixed +// distance apart measured along the direction connecting them, +// AxisDisplacementSolver measures and constrains that distance along a +// single axis fixed to the primary body's orientation, leaving any +// separation between the anchor points perpendicular to that axis +// unconstrained. +// +// Unlike [AxisRangeSolver], which only engages once the axis-aligned +// distance leaves a [AxisRangeSolver.MinDisplacement]/ +// [AxisRangeSolver.MaxDisplacement] range, AxisDisplacementSolver always +// enforces a single, exact displacement. +// +// An AxisDisplacementSolver must be configured, either through +// [NewAxisDisplacementSolver] or [AxisDisplacementSolver.Configure], +// before being registered with a [Scene] through +// [PairConstraintView.Create]. +type AxisDisplacementSolver struct { + primaryBodyAnchorOffset dprec.Vec3 + primaryBodyAxis dprec.Vec3 + secondaryBodyAnchorOffset dprec.Vec3 + displacement float64 + + primaryJacobian Jacobian + secondaryJacobian Jacobian + drift float64 +} + +var _ PairConstraintSolver = (*AxisDisplacementSolver)(nil) + +// NewAxisDisplacementSolver creates a new [AxisDisplacementSolver] +// configured according to config. +func NewAxisDisplacementSolver(config AxisDisplacementSolverConfig) *AxisDisplacementSolver { + result := &AxisDisplacementSolver{} + result.Configure(config) + return result +} + +// Configure configures this solver according to config. PrimaryBodyAxis +// is normalized, as with [AxisDisplacementSolver.SetPrimaryBodyAxis]. +// +// Configure must be called before this solver is registered with a +// [Scene] through [PairConstraintView.Create]. Unlike +// [NewAxisDisplacementSolver], it can be called on an already-allocated +// solver, which allows solvers to be cached (e.g. in a slice) and +// configured on demand. +func (s *AxisDisplacementSolver) Configure(config AxisDisplacementSolverConfig) { + s.primaryBodyAnchorOffset = config.PrimaryBodyAnchorOffset + s.primaryBodyAxis = dprec.UnitVec3(config.PrimaryBodyAxis) + s.secondaryBodyAnchorOffset = config.SecondaryBodyAnchorOffset + s.displacement = config.Displacement +} + +// PrimaryBodyAnchorOffset returns the body-local-space offset, relative +// to the primary target's center of mass, of the point at which the +// displacement constraint is anchored on the primary body. +func (s *AxisDisplacementSolver) PrimaryBodyAnchorOffset() dprec.Vec3 { + return s.primaryBodyAnchorOffset +} + +// SetPrimaryBodyAnchorOffset changes the body-local-space offset, +// relative to the primary target's center of mass, of the point at +// which the displacement constraint is anchored on the primary body. +// +// It returns the solver itself, so that calls can be chained. +func (s *AxisDisplacementSolver) SetPrimaryBodyAnchorOffset(offset dprec.Vec3) *AxisDisplacementSolver { + s.primaryBodyAnchorOffset = offset + return s +} + +// PrimaryBodyAxis returns the body-local-space, unit-length direction, +// relative to the primary target's rotation, along which the +// displacement between the two anchor points is measured. +func (s *AxisDisplacementSolver) PrimaryBodyAxis() dprec.Vec3 { + return s.primaryBodyAxis +} + +// SetPrimaryBodyAxis changes the body-local-space direction, relative to +// the primary target's rotation, along which the displacement between +// the two anchor points is measured. The provided axis need not be +// unit-length; it is normalized before being stored. +// +// It returns the solver itself, so that calls can be chained. +func (s *AxisDisplacementSolver) SetPrimaryBodyAxis(axis dprec.Vec3) *AxisDisplacementSolver { + s.primaryBodyAxis = dprec.UnitVec3(axis) + return s +} + +// SecondaryBodyAnchorOffset returns the body-local-space offset, +// relative to the secondary target's center of mass, of the point at +// which the displacement constraint is anchored on the secondary body. +func (s *AxisDisplacementSolver) SecondaryBodyAnchorOffset() dprec.Vec3 { + return s.secondaryBodyAnchorOffset +} + +// SetSecondaryBodyAnchorOffset changes the body-local-space offset, +// relative to the secondary target's center of mass, of the point at +// which the displacement constraint is anchored on the secondary body. +// +// It returns the solver itself, so that calls can be chained. +func (s *AxisDisplacementSolver) SetSecondaryBodyAnchorOffset(offset dprec.Vec3) *AxisDisplacementSolver { + s.secondaryBodyAnchorOffset = offset + return s +} + +// Displacement returns the signed distance, measured along +// [AxisDisplacementSolver.PrimaryBodyAxis], at which the two anchor +// points are held apart. +func (s *AxisDisplacementSolver) Displacement() float64 { + return s.displacement +} + +// SetDisplacement changes the signed distance, measured along +// [AxisDisplacementSolver.PrimaryBodyAxis], at which the two anchor +// points are held apart. Unlike a plain Euclidean distance, it may be +// negative. +// +// It returns the solver itself, so that calls can be chained. +func (s *AxisDisplacementSolver) SetDisplacement(displacement float64) *AxisDisplacementSolver { + s.displacement = displacement + return s +} + +// Reset implements [PairConstraintSolver.Reset]. +// +// It recomputes the constraint's primary and secondary [Jacobian]s and +// current displacement error (drift), the same way +// [AxisDisplacementSolver.recompute] does. +func (s *AxisDisplacementSolver) Reset(ctx PairConstraintContext) { + s.recompute(ctx) +} + +// ApplyImpulses implements [PairConstraintSolver.ApplyImpulses]. +// +// It resolves a pair of impulses, without restitution, that drive the +// anchor points' relative velocity toward closing the axis-aligned +// displacement error (drift) computed by [AxisDisplacementSolver.Reset], +// pulling the anchor points together along the axis when the actual +// displacement is too large and pushing them apart along the axis when +// it is too small. +func (s *AxisDisplacementSolver) ApplyImpulses(ctx PairConstraintContext) { + primaryImpulse, secondaryImpulse := ctx.ImpulseSolution( + s.primaryJacobian, s.secondaryJacobian, s.drift, 0.0, + ) + ctx.PrimaryTarget.ApplyImpulse(primaryImpulse) + ctx.SecondaryTarget.ApplyImpulse(secondaryImpulse) +} + +// ApplyNudges implements [PairConstraintSolver.ApplyNudges]. +// +// It first recomputes the constraint's primary and secondary [Jacobian]s +// and current axis-aligned displacement error (drift), the same way +// [AxisDisplacementSolver.recompute] does, since a preceding nudge - by +// this solver's own previous iteration, or by another constraint acting +// on either target - may have moved either target since +// [AxisDisplacementSolver.Reset] or the last call to this method. It +// then nudges both targets' positions and rotations to reduce any +// remaining displacement error between their anchor points. +func (s *AxisDisplacementSolver) ApplyNudges(ctx PairConstraintContext) { + s.recompute(ctx) + + primaryNudge, secondaryNudge := ctx.NudgeSolution( + s.primaryJacobian, s.secondaryJacobian, s.drift, + ) + ctx.PrimaryTarget.ApplyNudge(primaryNudge) + ctx.SecondaryTarget.ApplyNudge(secondaryNudge) +} + +// recompute recalculates the constraint's primary and secondary +// [Jacobian]s, along with the world-space offset from each target's +// center of mass to its respective anchor point (derived from +// PrimaryBodyAnchorOffset and SecondaryBodyAnchorOffset combined with +// each target's current rotation), and the current displacement error +// (drift) between the actual axis-aligned displacement and +// Displacement, based on the targets' current positions and rotations. +// +// The axis-aligned displacement is obtained by projecting the offset +// between the two anchor points onto PrimaryBodyAxis, transformed into +// world space through the primary target's current rotation. +func (s *AxisDisplacementSolver) recompute(ctx PairConstraintContext) { + primaryAnchorOffsetWS := dprec.QuatVec3Rotation(ctx.PrimaryTarget.Rotation(), s.primaryBodyAnchorOffset) + primaryAnchorWS := dprec.Vec3Sum(ctx.PrimaryTarget.Position(), primaryAnchorOffsetWS) + + secondaryAnchorOffsetWS := dprec.QuatVec3Rotation(ctx.SecondaryTarget.Rotation(), s.secondaryBodyAnchorOffset) + secondaryAnchorWS := dprec.Vec3Sum(ctx.SecondaryTarget.Position(), secondaryAnchorOffsetWS) + + axisWS := dprec.QuatVec3Rotation(ctx.PrimaryTarget.Rotation(), s.primaryBodyAxis) + + delta := dprec.Vec3Diff(secondaryAnchorWS, primaryAnchorWS) + actualDisplacement := dprec.Vec3Dot(axisWS, delta) + + s.primaryJacobian = Jacobian{ + LinearSlope: dprec.InverseVec3(axisWS), + AngularSlope: dprec.Vec3Cross(axisWS, primaryAnchorOffsetWS), + } + s.secondaryJacobian = Jacobian{ + LinearSlope: axisWS, + AngularSlope: dprec.Vec3Cross(secondaryAnchorOffsetWS, axisWS), + } + s.drift = s.displacement - actualDisplacement +} From 995abbaff5a4eeace82666091136168c9af309fc Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Tue, 11 Aug 2026 20:55:53 +0300 Subject: [PATCH 73/85] Rework placement3d filtering --- core/spatial/placement3d/filter.go | 43 +++++-- core/spatial/placement3d/scene.go | 154 +++++++++++++------------ core/spatial/placement3d/scene_test.go | 97 +++++++++++----- game/physics/collision.go | 2 + game/physics/scene.go | 16 +-- 5 files changed, 190 insertions(+), 122 deletions(-) diff --git a/core/spatial/placement3d/filter.go b/core/spatial/placement3d/filter.go index ca08d2ba..4b2eae86 100644 --- a/core/spatial/placement3d/filter.go +++ b/core/spatial/placement3d/filter.go @@ -6,15 +6,36 @@ import "github.com/mokiat/gog/opt" // narrow down the shapes that they consider. // // A shape is considered by a query when at least one bit is set in both the -// mask of the query and the [FilterInfo.SourceMask] of the shape. Note that -// this means that the zero value matches no shape at all. Use [FullMask] to -// consider every shape in the scene. +// mask of the query and the [FilterInfo.SourceMask] of the shape. As a special +// case, a query mask of zero is treated as covering all layers, so a query +// that uses it considers every shape in the scene. type Mask = uint32 // FullMask is a [Mask] with all layer bits set. A query that uses it considers -// every shape in the scene, regardless of the layers that the shape occupies. +// every shape in the scene, regardless of the layers that the shape occupies, +// which is the same behavior as that of the zero mask. const FullMask Mask = 0xFFFFFFFF +// Filter narrows down the shapes that a query considers. +// +// Its zero value considers every shape in the scene. +type Filter struct { + + // Mask specifies the layers that the query covers. A shape is considered + // only if it occupies at least one of those layers, as described by + // [Mask]. + // + // Defaults to all layers. + Mask Mask + + // RejectGroup becomes active if a value larger than zero is specified. + // Shapes whose [FilterInfo.RejectGroup] is the same are not considered by + // the query. + // + // Defaults to no rejection. + RejectGroup uint32 +} + // FilterInfo holds the collision-filtering metadata common to every shape that // can be placed in a scene, whether an object shape (see [SphereInfo] and // [BoxInfo]) or a terrain shape (see [MeshInfo]). @@ -53,10 +74,16 @@ func newFilterRepresentation(info FilterInfo) filterRepresentation { } } -// satisfiesMask reports whether this shape occupies at least one of the layers -// covered by the specified query mask. -func (s *filterRepresentation) satisfiesMask(mask Mask) bool { - return (s.sourceMask & mask) != 0 +// satisfiesFilter reports whether this shape is considered by a query that +// uses the specified filter. +func (s *filterRepresentation) satisfiesFilter(filter Filter) bool { + if (filter.RejectGroup != 0) && (filter.RejectGroup == s.rejectGroup) { + return false + } + if (filter.Mask != 0) && ((s.sourceMask & filter.Mask) == 0) { + return false + } + return true } // canInteractWith reports whether this shape and the specified one are allowed diff --git a/core/spatial/placement3d/scene.go b/core/spatial/placement3d/scene.go index 78e3d994..7b752d41 100644 --- a/core/spatial/placement3d/scene.go +++ b/core/spatial/placement3d/scene.go @@ -258,13 +258,12 @@ func (s *Scene[O, T, S]) SetObjectShapeUserData(shapeID ObjectShapeID, userData shape.userData = userData } -// EachSphere iterates over all sphere shapes in the scene that match the mask -// and yields them, in world space, to the provided callback. Iteration stops -// early if the callback returns false. +// EachSphere iterates over all sphere shapes in the scene that match the +// filter and yields them, in world space, to the provided callback. Iteration +// stops early if the callback returns false. // -// Note that a zero mask matches no shape at all. Use [FullMask] to iterate -// over every sphere in the scene. -func (s *Scene[O, T, S]) EachSphere(mask Mask, yield func(shape3d.Sphere) bool) { +// Note that the zero value of [Filter] matches every sphere in the scene. +func (s *Scene[O, T, S]) EachSphere(filter Filter, yield func(shape3d.Sphere) bool) { for index := range s.objectShapes { shape := &s.objectShapes[index] if shape.spatialID == query3d.InvalidTreeItemID { @@ -273,7 +272,7 @@ func (s *Scene[O, T, S]) EachSphere(mask Mask, yield func(shape3d.Sphere) bool) if shape.kind != objectShapeKindSphere { continue } - if !shape.satisfiesMask(mask) { + if !shape.satisfiesFilter(filter) { continue } if !yield(shape.toSphere()) { @@ -283,20 +282,19 @@ func (s *Scene[O, T, S]) EachSphere(mask Mask, yield func(shape3d.Sphere) bool) } // SphereIter returns an iterator over all sphere shapes in the scene that -// match the mask, as described by [Scene.EachSphere]. -func (s *Scene[O, T, S]) SphereIter(mask Mask) iter.Seq[shape3d.Sphere] { +// match the filter, as described by [Scene.EachSphere]. +func (s *Scene[O, T, S]) SphereIter(filter Filter) iter.Seq[shape3d.Sphere] { return func(yield func(shape3d.Sphere) bool) { - s.EachSphere(mask, yield) + s.EachSphere(filter, yield) } } -// EachBox iterates over all box shapes in the scene that match the mask and +// EachBox iterates over all box shapes in the scene that match the filter and // yields them, in world space, to the provided callback. Iteration stops early // if the callback returns false. // -// Note that a zero mask matches no shape at all. Use [FullMask] to iterate -// over every box in the scene. -func (s *Scene[O, T, S]) EachBox(mask Mask, yield func(shape3d.Box) bool) { +// Note that the zero value of [Filter] matches every box in the scene. +func (s *Scene[O, T, S]) EachBox(filter Filter, yield func(shape3d.Box) bool) { for index := range s.objectShapes { shape := &s.objectShapes[index] if shape.spatialID == query3d.InvalidTreeItemID { @@ -305,7 +303,7 @@ func (s *Scene[O, T, S]) EachBox(mask Mask, yield func(shape3d.Box) bool) { if shape.kind != objectShapeKindBox { continue } - if !shape.satisfiesMask(mask) { + if !shape.satisfiesFilter(filter) { continue } if !yield(shape.toBox()) { @@ -315,10 +313,10 @@ func (s *Scene[O, T, S]) EachBox(mask Mask, yield func(shape3d.Box) bool) { } // BoxIter returns an iterator over all box shapes in the scene that match the -// mask, as described by [Scene.EachBox]. -func (s *Scene[O, T, S]) BoxIter(mask Mask) iter.Seq[shape3d.Box] { +// filter, as described by [Scene.EachBox]. +func (s *Scene[O, T, S]) BoxIter(filter Filter) iter.Seq[shape3d.Box] { return func(yield func(shape3d.Box) bool) { - s.EachBox(mask, yield) + s.EachBox(filter, yield) } } @@ -416,58 +414,60 @@ func (s *Scene[O, T, S]) SetTerrainShapeUserData(shapeID TerrainShapeID, userDat } // CollectSegmentObjectIntersections collects all intersections of the segment -// with the object shapes in the scene that match the mask. +// with the object shapes in the scene that match the filter. // // The reported contacts have no source, since the segment is not part of the // scene. Their Depth is the fraction of the segment that lies beyond the // contact point, as described by [shape3d.Contact]. -func (s *Scene[O, T, S]) CollectSegmentObjectIntersections(segment shape3d.Segment, mask Mask, yield ObjectContactCallback) { +func (s *Scene[O, T, S]) CollectSegmentObjectIntersections(segment shape3d.Segment, filter Filter, yield ObjectContactCallback) { s.objectShapeCandidates = s.objectShapeCandidates[:0] s.objectShapeTree.QuerySegment(segment, func(index int32) bool { s.objectShapeCandidates = append(s.objectShapeCandidates, index) return true }) - s.collectSegmentObject(segment, mask, yield) + s.collectSegmentObject(segment, filter, yield) } // CheckSegmentObjectIntersection returns the intersection of the segment with -// the object shape that it enters first, if any. -func (s *Scene[O, T, S]) CheckSegmentObjectIntersection(segment shape3d.Segment, mask Mask) (ObjectContact, bool) { +// the object shape that it enters first, if any. Only object shapes that match +// the filter are considered. +func (s *Scene[O, T, S]) CheckSegmentObjectIntersection(segment shape3d.Segment, filter Filter) (ObjectContact, bool) { var collection DeepestObjectContact - s.CollectSegmentObjectIntersections(segment, mask, collection.AddContact) + s.CollectSegmentObjectIntersections(segment, filter, collection.AddContact) return collection.Contact() } // CollectSegmentTerrainIntersections collects all intersections of the segment -// with the terrain shapes in the scene that match the mask. At most one +// with the terrain shapes in the scene that match the filter. At most one // contact is reported per terrain shape. // // The reported contacts have no source, since the segment is not part of the // scene. Their Depth is the fraction of the segment that lies beyond the // contact point, as described by [shape3d.Contact]. -func (s *Scene[O, T, S]) CollectSegmentTerrainIntersections(segment shape3d.Segment, mask Mask, yield TerrainContactCallback) { +func (s *Scene[O, T, S]) CollectSegmentTerrainIntersections(segment shape3d.Segment, filter Filter, yield TerrainContactCallback) { s.terrainShapeCandidates = s.terrainShapeCandidates[:0] s.terrainShapeTree.QuerySegment(segment, func(index int32) bool { s.terrainShapeCandidates = append(s.terrainShapeCandidates, index) return true }) - s.collectSegmentTerrain(segment, mask, yield) + s.collectSegmentTerrain(segment, filter, yield) } // CheckSegmentTerrainIntersection returns the intersection of the segment with -// the terrain shape that it enters first, if any. -func (s *Scene[O, T, S]) CheckSegmentTerrainIntersection(segment shape3d.Segment, mask Mask) (TerrainContact, bool) { +// the terrain shape that it enters first, if any. Only terrain shapes that +// match the filter are considered. +func (s *Scene[O, T, S]) CheckSegmentTerrainIntersection(segment shape3d.Segment, filter Filter) (TerrainContact, bool) { var collection DeepestTerrainContact - s.CollectSegmentTerrainIntersections(segment, mask, collection.AddContact) + s.CollectSegmentTerrainIntersections(segment, filter, collection.AddContact) return collection.Contact() } // CollectSphereObjectIntersections collects all intersections of the sphere -// with the object shapes in the scene that match the mask. +// with the object shapes in the scene that match the filter. // // The reported contacts have no source, since the sphere is not part of the // scene. -func (s *Scene[O, T, S]) CollectSphereObjectIntersections(sphere shape3d.Sphere, mask Mask, yield ObjectContactCallback) { +func (s *Scene[O, T, S]) CollectSphereObjectIntersections(sphere shape3d.Sphere, filter Filter, yield ObjectContactCallback) { queryAABB := shape3d.AABBFromSphere(sphere) s.objectShapeCandidates = s.objectShapeCandidates[:0] @@ -475,24 +475,25 @@ func (s *Scene[O, T, S]) CollectSphereObjectIntersections(sphere shape3d.Sphere, s.objectShapeCandidates = append(s.objectShapeCandidates, index) return true }) - s.collectSphereObject(sphere, mask, yield) + s.collectSphereObject(sphere, filter, yield) } // CheckSphereObjectIntersection returns the deepest intersection of the sphere -// with an object shape in the scene, if any. -func (s *Scene[O, T, S]) CheckSphereObjectIntersection(sphere shape3d.Sphere, mask Mask) (ObjectContact, bool) { +// with an object shape in the scene, if any. Only object shapes that match the +// filter are considered. +func (s *Scene[O, T, S]) CheckSphereObjectIntersection(sphere shape3d.Sphere, filter Filter) (ObjectContact, bool) { var collection DeepestObjectContact - s.CollectSphereObjectIntersections(sphere, mask, collection.AddContact) + s.CollectSphereObjectIntersections(sphere, filter, collection.AddContact) return collection.Contact() } // CollectSphereTerrainIntersections collects all intersections of the sphere -// with the terrain shapes in the scene that match the mask. At most one +// with the terrain shapes in the scene that match the filter. At most one // contact is reported per terrain shape. // // The reported contacts have no source, since the sphere is not part of the // scene. -func (s *Scene[O, T, S]) CollectSphereTerrainIntersections(sphere shape3d.Sphere, mask Mask, yield TerrainContactCallback) { +func (s *Scene[O, T, S]) CollectSphereTerrainIntersections(sphere shape3d.Sphere, filter Filter, yield TerrainContactCallback) { queryAABB := shape3d.AABBFromSphere(sphere) s.terrainShapeCandidates = s.terrainShapeCandidates[:0] @@ -500,23 +501,24 @@ func (s *Scene[O, T, S]) CollectSphereTerrainIntersections(sphere shape3d.Sphere s.terrainShapeCandidates = append(s.terrainShapeCandidates, index) return true }) - s.collectSphereTerrain(sphere, mask, yield) + s.collectSphereTerrain(sphere, filter, yield) } // CheckSphereTerrainIntersection returns the deepest intersection of the -// sphere with a terrain shape in the scene, if any. -func (s *Scene[O, T, S]) CheckSphereTerrainIntersection(sphere shape3d.Sphere, mask Mask) (TerrainContact, bool) { +// sphere with a terrain shape in the scene, if any. Only terrain shapes that +// match the filter are considered. +func (s *Scene[O, T, S]) CheckSphereTerrainIntersection(sphere shape3d.Sphere, filter Filter) (TerrainContact, bool) { var collection DeepestTerrainContact - s.CollectSphereTerrainIntersections(sphere, mask, collection.AddContact) + s.CollectSphereTerrainIntersections(sphere, filter, collection.AddContact) return collection.Contact() } // CollectBoxObjectIntersections collects all intersections of the box with the -// object shapes in the scene that match the mask. +// object shapes in the scene that match the filter. // // The reported contacts have no source, since the box is not part of the // scene. -func (s *Scene[O, T, S]) CollectBoxObjectIntersections(box shape3d.Box, mask Mask, yield ObjectContactCallback) { +func (s *Scene[O, T, S]) CollectBoxObjectIntersections(box shape3d.Box, filter Filter, yield ObjectContactCallback) { queryAABB := shape3d.AABBFromBox(box) s.objectShapeCandidates = s.objectShapeCandidates[:0] @@ -524,24 +526,25 @@ func (s *Scene[O, T, S]) CollectBoxObjectIntersections(box shape3d.Box, mask Mas s.objectShapeCandidates = append(s.objectShapeCandidates, index) return true }) - s.collectBoxObject(box, mask, yield) + s.collectBoxObject(box, filter, yield) } // CheckBoxObjectIntersection returns the deepest intersection of the box with -// an object shape in the scene, if any. -func (s *Scene[O, T, S]) CheckBoxObjectIntersection(box shape3d.Box, mask Mask) (ObjectContact, bool) { +// an object shape in the scene, if any. Only object shapes that match the +// filter are considered. +func (s *Scene[O, T, S]) CheckBoxObjectIntersection(box shape3d.Box, filter Filter) (ObjectContact, bool) { var collection DeepestObjectContact - s.CollectBoxObjectIntersections(box, mask, collection.AddContact) + s.CollectBoxObjectIntersections(box, filter, collection.AddContact) return collection.Contact() } // CollectBoxTerrainIntersections collects all intersections of the box with -// the terrain shapes in the scene that match the mask. At most one contact is -// reported per terrain shape. +// the terrain shapes in the scene that match the filter. At most one contact +// is reported per terrain shape. // // The reported contacts have no source, since the box is not part of the // scene. -func (s *Scene[O, T, S]) CollectBoxTerrainIntersections(box shape3d.Box, mask Mask, yield TerrainContactCallback) { +func (s *Scene[O, T, S]) CollectBoxTerrainIntersections(box shape3d.Box, filter Filter, yield TerrainContactCallback) { queryAABB := shape3d.AABBFromBox(box) s.terrainShapeCandidates = s.terrainShapeCandidates[:0] @@ -549,14 +552,15 @@ func (s *Scene[O, T, S]) CollectBoxTerrainIntersections(box shape3d.Box, mask Ma s.terrainShapeCandidates = append(s.terrainShapeCandidates, index) return true }) - s.collectBoxTerrain(box, mask, yield) + s.collectBoxTerrain(box, filter, yield) } // CheckBoxTerrainIntersection returns the deepest intersection of the box with -// a terrain shape in the scene, if any. -func (s *Scene[O, T, S]) CheckBoxTerrainIntersection(box shape3d.Box, mask Mask) (TerrainContact, bool) { +// a terrain shape in the scene, if any. Only terrain shapes that match the +// filter are considered. +func (s *Scene[O, T, S]) CheckBoxTerrainIntersection(box shape3d.Box, filter Filter) (TerrainContact, bool) { var collection DeepestTerrainContact - s.CollectBoxTerrainIntersections(box, mask, collection.AddContact) + s.CollectBoxTerrainIntersections(box, filter, collection.AddContact) return collection.Contact() } @@ -806,8 +810,8 @@ func (s *Scene[O, T, S]) detachTerrainShape(index int32) { s.releaseTerrainShape(index) } -func (s *Scene[O, T, S]) collectSegmentObject(segment shape3d.Segment, mask Mask, yield ObjectContactCallback) { - for index, shape := range s.iterCandidateObjectShapes(mask) { +func (s *Scene[O, T, S]) collectSegmentObject(segment shape3d.Segment, filter Filter, yield ObjectContactCallback) { + for index, shape := range s.iterCandidateObjectShapes(filter) { if !isec3d.CheckSegmentSphereOverlap(segment, shape.wsBSphere) { continue } @@ -831,8 +835,8 @@ func (s *Scene[O, T, S]) collectSegmentObject(segment shape3d.Segment, mask Mask } } -func (s *Scene[O, T, S]) collectSegmentTerrain(segment shape3d.Segment, mask Mask, yield TerrainContactCallback) { - for index, shape := range s.iterCandidateTerrainShapes(mask) { +func (s *Scene[O, T, S]) collectSegmentTerrain(segment shape3d.Segment, filter Filter, yield TerrainContactCallback) { + for index, shape := range s.iterCandidateTerrainShapes(filter) { if !isec3d.CheckSegmentSphereOverlap(segment, shape.wsBSphere) { continue } @@ -852,9 +856,9 @@ func (s *Scene[O, T, S]) collectSegmentTerrain(segment shape3d.Segment, mask Mas } } -func (s *Scene[O, T, S]) collectSphereObject(sphere shape3d.Sphere, mask Mask, yield ObjectContactCallback) { +func (s *Scene[O, T, S]) collectSphereObject(sphere shape3d.Sphere, filter Filter, yield ObjectContactCallback) { initGJKShapeForSphere(sphere, &s.tempGJKSource) - for index, shape := range s.iterCandidateObjectShapes(mask) { + for index, shape := range s.iterCandidateObjectShapes(filter) { if !isec3d.CheckSphereSphere(sphere, shape.wsBSphere) { continue } @@ -870,9 +874,9 @@ func (s *Scene[O, T, S]) collectSphereObject(sphere shape3d.Sphere, mask Mask, y } } -func (s *Scene[O, T, S]) collectSphereTerrain(sphere shape3d.Sphere, mask Mask, yield TerrainContactCallback) { +func (s *Scene[O, T, S]) collectSphereTerrain(sphere shape3d.Sphere, filter Filter, yield TerrainContactCallback) { initGJKShapeForSphere(sphere, &s.tempGJKSource) - for index, shape := range s.iterCandidateTerrainShapes(mask) { + for index, shape := range s.iterCandidateTerrainShapes(filter) { s.resolveTerrainShape(s.tempGJKSource, sphere, shape, func(contact shape3d.Contact) { yield(TerrainContact{ SourceObjectID: NilObjectID, @@ -885,9 +889,9 @@ func (s *Scene[O, T, S]) collectSphereTerrain(sphere shape3d.Sphere, mask Mask, } } -func (s *Scene[O, T, S]) collectBoxObject(box shape3d.Box, mask Mask, yield ObjectContactCallback) { +func (s *Scene[O, T, S]) collectBoxObject(box shape3d.Box, filter Filter, yield ObjectContactCallback) { initGJKShapeForBox(box, &s.tempGJKSource) - for index, shape := range s.iterCandidateObjectShapes(mask) { + for index, shape := range s.iterCandidateObjectShapes(filter) { if !isec3d.CheckSphereSphere(box.BoundingSphere(), shape.wsBSphere) { continue } @@ -903,9 +907,9 @@ func (s *Scene[O, T, S]) collectBoxObject(box shape3d.Box, mask Mask, yield Obje } } -func (s *Scene[O, T, S]) collectBoxTerrain(box shape3d.Box, mask Mask, yield TerrainContactCallback) { +func (s *Scene[O, T, S]) collectBoxTerrain(box shape3d.Box, filter Filter, yield TerrainContactCallback) { initGJKShapeForBox(box, &s.tempGJKSource) - for index, shape := range s.iterCandidateTerrainShapes(mask) { + for index, shape := range s.iterCandidateTerrainShapes(filter) { s.resolveTerrainShape(s.tempGJKSource, box.BoundingSphere(), shape, func(contact shape3d.Contact) { yield(TerrainContact{ SourceObjectID: NilObjectID, @@ -992,10 +996,10 @@ func (s *Scene[O, T, S]) resolveTerrainShape(srcGJK gjk3d.Shape, srcBS shape3d.S } } -func (s *Scene[O, T, S]) eachCandidateObjectShape(mask Mask, cb func(int32, *objectShapeState[S]) bool) { +func (s *Scene[O, T, S]) eachCandidateObjectShape(filter Filter, cb func(int32, *objectShapeState[S]) bool) { for _, index := range s.objectShapeCandidates { shape := &s.objectShapes[index] - if !shape.satisfiesMask(mask) { + if !shape.satisfiesFilter(filter) { continue } if !cb(index, shape) { @@ -1004,16 +1008,16 @@ func (s *Scene[O, T, S]) eachCandidateObjectShape(mask Mask, cb func(int32, *obj } } -func (s *Scene[O, T, S]) iterCandidateObjectShapes(mask Mask) iter.Seq2[int32, *objectShapeState[S]] { +func (s *Scene[O, T, S]) iterCandidateObjectShapes(filter Filter) iter.Seq2[int32, *objectShapeState[S]] { return func(yield func(int32, *objectShapeState[S]) bool) { - s.eachCandidateObjectShape(mask, yield) + s.eachCandidateObjectShape(filter, yield) } } -func (s *Scene[O, T, S]) eachCandidateTerrainShape(mask Mask, cb func(int32, *terrainShapeState[S]) bool) { +func (s *Scene[O, T, S]) eachCandidateTerrainShape(filter Filter, cb func(int32, *terrainShapeState[S]) bool) { for _, index := range s.terrainShapeCandidates { shape := &s.terrainShapes[index] - if !shape.satisfiesMask(mask) { + if !shape.satisfiesFilter(filter) { continue } if !cb(index, shape) { @@ -1022,9 +1026,9 @@ func (s *Scene[O, T, S]) eachCandidateTerrainShape(mask Mask, cb func(int32, *te } } -func (s *Scene[O, T, S]) iterCandidateTerrainShapes(mask Mask) iter.Seq2[int32, *terrainShapeState[S]] { +func (s *Scene[O, T, S]) iterCandidateTerrainShapes(filter Filter) iter.Seq2[int32, *terrainShapeState[S]] { return func(yield func(int32, *terrainShapeState[S]) bool) { - s.eachCandidateTerrainShape(mask, yield) + s.eachCandidateTerrainShape(filter, yield) } } diff --git a/core/spatial/placement3d/scene_test.go b/core/spatial/placement3d/scene_test.go index ee8fad70..73e35299 100644 --- a/core/spatial/placement3d/scene_test.go +++ b/core/spatial/placement3d/scene_test.go @@ -166,7 +166,7 @@ var _ = Describe("Scene", func() { }) var found []shape3d.Sphere - scene.EachSphere(placement3d.FullMask, func(s shape3d.Sphere) bool { + scene.EachSphere(placement3d.Filter{}, func(s shape3d.Sphere) bool { found = append(found, s) return true }) @@ -185,7 +185,7 @@ var _ = Describe("Scene", func() { }) count := 0 - scene.EachBox(placement3d.FullMask, func(shape3d.Box) bool { + scene.EachBox(placement3d.Filter{}, func(shape3d.Box) bool { count++ return true }) @@ -198,7 +198,7 @@ var _ = Describe("Scene", func() { }) count := 0 - for range scene.SphereIter(placement3d.FullMask) { + for range scene.SphereIter(placement3d.Filter{}) { count++ } Expect(count).To(Equal(1)) @@ -210,7 +210,7 @@ var _ = Describe("Scene", func() { }) count := 0 - for range scene.BoxIter(placement3d.FullMask) { + for range scene.BoxIter(placement3d.Filter{}) { count++ } Expect(count).To(Equal(1)) @@ -241,7 +241,7 @@ var _ = Describe("Scene", func() { scene.DeleteObjectShape(shapeID) count := 0 - scene.EachSphere(placement3d.FullMask, func(shape3d.Sphere) bool { + scene.EachSphere(placement3d.Filter{}, func(shape3d.Sphere) bool { count++ return true }) @@ -257,7 +257,7 @@ var _ = Describe("Scene", func() { }) count := 0 - scene.EachSphere(placement3d.FullMask, func(shape3d.Sphere) bool { + scene.EachSphere(placement3d.Filter{}, func(shape3d.Sphere) bool { count++ return false }) @@ -276,7 +276,7 @@ var _ = Describe("Scene", func() { )) var centers []dprec.Vec3 - scene.EachSphere(placement3d.FullMask, func(s shape3d.Sphere) bool { + scene.EachSphere(placement3d.Filter{}, func(s shape3d.Sphere) bool { centers = append(centers, s.Center) return true }) @@ -287,22 +287,23 @@ var _ = Describe("Scene", func() { }) }) - Describe("shape iteration masks", func() { + Describe("shape iteration filters", func() { var objID placement3d.ObjectID BeforeEach(func() { objID = scene.CreateObject(placement3d.ObjectInfo[string]{}) scene.AttachSphere(objID, placement3d.SphereInfo[string]{ Filtering: placement3d.FilterInfo{ - SourceMask: opt.V(uint32(0b01)), + RejectGroup: 7, + SourceMask: opt.V(uint32(0b01)), }, Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), }) }) - countSpheres := func(mask placement3d.Mask) int { + countSpheres := func(filter placement3d.Filter) int { count := 0 - scene.EachSphere(mask, func(shape3d.Sphere) bool { + scene.EachSphere(filter, func(shape3d.Sphere) bool { count++ return true }) @@ -310,15 +311,33 @@ var _ = Describe("Scene", func() { } It("yields shapes that occupy a layer of the mask", func() { - Expect(countSpheres(0b01)).To(Equal(1)) + Expect(countSpheres(placement3d.Filter{Mask: 0b01})).To(Equal(1)) }) It("skips shapes that occupy no layer of the mask", func() { - Expect(countSpheres(0b10)).To(BeZero()) + Expect(countSpheres(placement3d.Filter{Mask: 0b10})).To(BeZero()) + }) + + It("yields everything for the zero mask", func() { + Expect(countSpheres(placement3d.Filter{Mask: 0})).To(Equal(1)) }) - It("yields nothing for the zero mask", func() { - Expect(countSpheres(0)).To(BeZero()) + It("yields everything for the full mask", func() { + Expect(countSpheres(placement3d.Filter{ + Mask: placement3d.FullMask, + })).To(Equal(1)) + }) + + It("skips shapes that share the reject group", func() { + Expect(countSpheres(placement3d.Filter{RejectGroup: 7})).To(BeZero()) + }) + + It("yields shapes that have a different reject group", func() { + Expect(countSpheres(placement3d.Filter{RejectGroup: 8})).To(Equal(1)) + }) + + It("yields everything for the zero filter", func() { + Expect(countSpheres(placement3d.Filter{})).To(Equal(1)) }) }) @@ -757,7 +776,7 @@ var _ = Describe("Scene", func() { contact, ok := scene.CheckSphereObjectIntersection( sphereAt(1.5, 0.0, 0.0, 1.0), - placement3d.FullMask, + placement3d.Filter{}, ) Expect(ok).To(BeTrue()) Expect(contact.SourceObjectID).To(Equal(placement3d.NilObjectID)) @@ -774,7 +793,7 @@ var _ = Describe("Scene", func() { _, ok := scene.CheckSphereObjectIntersection( sphereAt(10.0, 0.0, 0.0, 1.0), - placement3d.FullMask, + placement3d.Filter{}, ) Expect(ok).To(BeFalse()) }) @@ -790,7 +809,23 @@ var _ = Describe("Scene", func() { _, ok := scene.CheckSphereObjectIntersection( sphereAt(1.5, 0.0, 0.0, 1.0), - 0b10, + placement3d.Filter{Mask: 0b10}, + ) + Expect(ok).To(BeFalse()) + }) + + It("honors the query reject group", func() { + objID := scene.CreateObject(placement3d.ObjectInfo[string]{}) + scene.AttachSphere(objID, placement3d.SphereInfo[string]{ + Filtering: placement3d.FilterInfo{ + RejectGroup: 7, + }, + Sphere: sphereAt(0.0, 0.0, 0.0, 1.0), + }) + + _, ok := scene.CheckSphereObjectIntersection( + sphereAt(1.5, 0.0, 0.0, 1.0), + placement3d.Filter{RejectGroup: 7}, ) Expect(ok).To(BeFalse()) }) @@ -804,7 +839,7 @@ var _ = Describe("Scene", func() { // The plane faces -Y, so approach it from below (the front side). contact, ok := scene.CheckSphereTerrainIntersection( sphereAt(0.0, -0.5, 0.0, 1.0), - placement3d.FullMask, + placement3d.Filter{}, ) Expect(ok).To(BeTrue()) Expect(contact.SourceShapeID).To(Equal(placement3d.NilObjectShapeID)) @@ -821,7 +856,7 @@ var _ = Describe("Scene", func() { _, ok := scene.CheckSphereTerrainIntersection( sphereAt(0.0, 0.5, 0.0, 1.0), - placement3d.FullMask, + placement3d.Filter{}, ) Expect(ok).To(BeFalse()) }) @@ -834,7 +869,7 @@ var _ = Describe("Scene", func() { _, ok := scene.CheckSphereTerrainIntersection( sphereAt(0.5, 0.0, 0.0, 1.0), - placement3d.FullMask, + placement3d.Filter{}, ) Expect(ok).To(BeFalse()) }) @@ -849,7 +884,7 @@ var _ = Describe("Scene", func() { contact, ok := scene.CheckBoxObjectIntersection( boxAt(1.5, 0.0, 0.0, 1.0), - placement3d.FullMask, + placement3d.Filter{}, ) Expect(ok).To(BeTrue()) Expect(contact.SourceShapeID).To(Equal(placement3d.NilObjectShapeID)) @@ -865,7 +900,7 @@ var _ = Describe("Scene", func() { _, ok := scene.CheckBoxObjectIntersection( boxAt(10.0, 0.0, 0.0, 1.0), - placement3d.FullMask, + placement3d.Filter{}, ) Expect(ok).To(BeFalse()) }) @@ -879,7 +914,7 @@ var _ = Describe("Scene", func() { // The plane faces -Y, so approach it from below (the front side). contact, ok := scene.CheckBoxTerrainIntersection( boxAt(0.0, -0.5, 0.0, 1.0), - placement3d.FullMask, + placement3d.Filter{}, ) Expect(ok).To(BeTrue()) Expect(contact.TargetTerrainID).To(Equal(terrainID)) @@ -895,7 +930,7 @@ var _ = Describe("Scene", func() { _, ok := scene.CheckBoxTerrainIntersection( boxAt(0.0, 0.5, 0.0, 1.0), - placement3d.FullMask, + placement3d.Filter{}, ) Expect(ok).To(BeFalse()) }) @@ -918,7 +953,7 @@ var _ = Describe("Scene", func() { dprec.NewVec3(-5.0, 0.0, 0.0), dprec.NewVec3(9.0, 0.0, 0.0), ), - placement3d.FullMask, + placement3d.Filter{}, contacts.AddContact, ) Expect(contacts).To(HaveLen(2)) @@ -935,7 +970,7 @@ var _ = Describe("Scene", func() { dprec.NewVec3(-5.0, 0.0, 0.0), dprec.NewVec3(5.0, 0.0, 0.0), ), - placement3d.FullMask, + placement3d.Filter{}, ) Expect(ok).To(BeTrue()) Expect(contact.SourceShapeID).To(Equal(placement3d.NilObjectShapeID)) @@ -958,7 +993,7 @@ var _ = Describe("Scene", func() { dprec.NewVec3(-5.0, 0.0, 0.0), dprec.NewVec3(9.0, 0.0, 0.0), ), - placement3d.FullMask, + placement3d.Filter{}, ) Expect(ok).To(BeTrue()) Expect(contact.TargetShapeID).To(Equal(nearShapeID)) @@ -975,7 +1010,7 @@ var _ = Describe("Scene", func() { dprec.NewVec3(2.0, -5.0, 0.0), dprec.NewVec3(2.0, 5.0, 0.0), ), - placement3d.FullMask, + placement3d.Filter{}, ) Expect(ok).To(BeTrue()) Expect(contact.SourceShapeID).To(Equal(placement3d.NilObjectShapeID)) @@ -994,7 +1029,7 @@ var _ = Describe("Scene", func() { dprec.NewVec3(-5.0, 5.0, 0.0), dprec.NewVec3(5.0, 5.0, 0.0), ), - placement3d.FullMask, + placement3d.Filter{}, ) Expect(ok).To(BeFalse()) }) @@ -1010,7 +1045,7 @@ var _ = Describe("Scene", func() { dprec.NewVec3(0.0, 5.0, 0.0), dprec.NewVec3(0.0, -5.0, 0.0), ), - placement3d.FullMask, + placement3d.Filter{}, ) Expect(ok).To(BeFalse()) }) diff --git a/game/physics/collision.go b/game/physics/collision.go index 8705c260..a3910f06 100644 --- a/game/physics/collision.go +++ b/game/physics/collision.go @@ -9,6 +9,8 @@ type Mask = placement3d.Mask const FullMask = placement3d.FullMask +type Filter = placement3d.Filter + type BodyCollisionShapeID struct { bodyID BodyID shapeID placement3d.ObjectShapeID diff --git a/game/physics/scene.go b/game/physics/scene.go index b95861ff..abd9a9ed 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -393,10 +393,10 @@ func (s *Scene) Update(elapsedTime time.Duration) { s.notifyPairCollisions() } -func (s *Scene) CollectSegmentBodyIntersections(segment shape3d.Segment, mask Mask, yield BodyContactCallback) { +func (s *Scene) CollectSegmentBodyIntersections(segment shape3d.Segment, filter Filter, yield BodyContactCallback) { bodyView := s.Bodies() - s.collisionScene.CollectSegmentObjectIntersections(segment, mask, func(contact placement3d.ObjectContact) { + s.collisionScene.CollectSegmentObjectIntersections(segment, filter, func(contact placement3d.ObjectContact) { tgtBodyData := s.collisionScene.GetObjectUserData(contact.TargetObjectID) yield(BodyContact{ @@ -406,16 +406,16 @@ func (s *Scene) CollectSegmentBodyIntersections(segment shape3d.Segment, mask Ma }) } -func (s *Scene) CheckSegmentBodyIntersection(segment shape3d.Segment, mask Mask) (BodyContact, bool) { +func (s *Scene) CheckSegmentBodyIntersection(segment shape3d.Segment, filter Filter) (BodyContact, bool) { var collection DeepestBodyContact - s.CollectSegmentBodyIntersections(segment, mask, collection.AddContact) + s.CollectSegmentBodyIntersections(segment, filter, collection.AddContact) return collection.Contact() } -func (s *Scene) CollectSegmentTerrainIntersections(segment shape3d.Segment, mask Mask, yield TerrainContactCallback) { +func (s *Scene) CollectSegmentTerrainIntersections(segment shape3d.Segment, filter Filter, yield TerrainContactCallback) { terrainView := s.Terrains() - s.collisionScene.CollectSegmentTerrainIntersections(segment, mask, func(contact placement3d.TerrainContact) { + s.collisionScene.CollectSegmentTerrainIntersections(segment, filter, func(contact placement3d.TerrainContact) { tgtTerrainData := s.collisionScene.GetTerrainUserData(contact.TargetTerrainID) yield(TerrainContact{ @@ -425,9 +425,9 @@ func (s *Scene) CollectSegmentTerrainIntersections(segment shape3d.Segment, mask }) } -func (s *Scene) CheckSegmentTerrainIntersection(segment shape3d.Segment, mask Mask) (TerrainContact, bool) { +func (s *Scene) CheckSegmentTerrainIntersection(segment shape3d.Segment, filter Filter) (TerrainContact, bool) { var collection DeepestTerrainContact - s.CollectSegmentTerrainIntersections(segment, mask, collection.AddContact) + s.CollectSegmentTerrainIntersections(segment, filter, collection.AddContact) return collection.Contact() } From bb91fb6a252e7a9b85abed99fdde27f638b74214 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Tue, 11 Aug 2026 21:02:48 +0300 Subject: [PATCH 74/85] Rework placement2d filtering --- core/spatial/placement2d/filter.go | 43 +++++-- core/spatial/placement2d/scene.go | 154 +++++++++++++------------ core/spatial/placement2d/scene_test.go | 97 +++++++++++----- 3 files changed, 180 insertions(+), 114 deletions(-) diff --git a/core/spatial/placement2d/filter.go b/core/spatial/placement2d/filter.go index 9039ae80..1dc96250 100644 --- a/core/spatial/placement2d/filter.go +++ b/core/spatial/placement2d/filter.go @@ -6,15 +6,36 @@ import "github.com/mokiat/gog/opt" // narrow down the shapes that they consider. // // A shape is considered by a query when at least one bit is set in both the -// mask of the query and the [FilterInfo.SourceMask] of the shape. Note that -// this means that the zero value matches no shape at all. Use [FullMask] to -// consider every shape in the scene. +// mask of the query and the [FilterInfo.SourceMask] of the shape. As a special +// case, a query mask of zero is treated as covering all layers, so a query +// that uses it considers every shape in the scene. type Mask = uint32 // FullMask is a [Mask] with all layer bits set. A query that uses it considers -// every shape in the scene, regardless of the layers that the shape occupies. +// every shape in the scene, regardless of the layers that the shape occupies, +// which is the same behavior as that of the zero mask. const FullMask Mask = 0xFFFFFFFF +// Filter narrows down the shapes that a query considers. +// +// Its zero value considers every shape in the scene. +type Filter struct { + + // Mask specifies the layers that the query covers. A shape is considered + // only if it occupies at least one of those layers, as described by + // [Mask]. + // + // Defaults to all layers. + Mask Mask + + // RejectGroup becomes active if a value larger than zero is specified. + // Shapes whose [FilterInfo.RejectGroup] is the same are not considered by + // the query. + // + // Defaults to no rejection. + RejectGroup uint32 +} + // FilterInfo holds the collision-filtering metadata common to every shape that // can be placed in a scene, whether an object shape (see [CircleInfo] and // [RectangleInfo]) or a terrain shape (see [MeshInfo]). @@ -53,10 +74,16 @@ func newFilterRepresentation(info FilterInfo) filterRepresentation { } } -// satisfiesMask reports whether this shape occupies at least one of the layers -// covered by the specified query mask. -func (s *filterRepresentation) satisfiesMask(mask Mask) bool { - return (s.sourceMask & mask) != 0 +// satisfiesFilter reports whether this shape is considered by a query that +// uses the specified filter. +func (s *filterRepresentation) satisfiesFilter(filter Filter) bool { + if (filter.RejectGroup != 0) && (filter.RejectGroup == s.rejectGroup) { + return false + } + if (filter.Mask != 0) && ((s.sourceMask & filter.Mask) == 0) { + return false + } + return true } // canInteractWith reports whether this shape and the specified one are allowed diff --git a/core/spatial/placement2d/scene.go b/core/spatial/placement2d/scene.go index 088c25e4..4a3459ce 100644 --- a/core/spatial/placement2d/scene.go +++ b/core/spatial/placement2d/scene.go @@ -253,13 +253,12 @@ func (s *Scene[O, T, S]) SetObjectShapeUserData(shapeID ObjectShapeID, userData shape.userData = userData } -// EachCircle iterates over all circle shapes in the scene that match the mask -// and yields them, in world space, to the provided callback. Iteration stops -// early if the callback returns false. +// EachCircle iterates over all circle shapes in the scene that match the +// filter and yields them, in world space, to the provided callback. Iteration +// stops early if the callback returns false. // -// Note that a zero mask matches no shape at all. Use [FullMask] to iterate -// over every circle in the scene. -func (s *Scene[O, T, S]) EachCircle(mask Mask, yield func(shape2d.Circle) bool) { +// Note that the zero value of [Filter] matches every circle in the scene. +func (s *Scene[O, T, S]) EachCircle(filter Filter, yield func(shape2d.Circle) bool) { for index := range s.objectShapes { shape := &s.objectShapes[index] if shape.spatialID == query2d.InvalidTreeItemID { @@ -268,7 +267,7 @@ func (s *Scene[O, T, S]) EachCircle(mask Mask, yield func(shape2d.Circle) bool) if shape.kind != objectShapeKindCircle { continue } - if !shape.satisfiesMask(mask) { + if !shape.satisfiesFilter(filter) { continue } if !yield(shape.toCircle()) { @@ -278,20 +277,19 @@ func (s *Scene[O, T, S]) EachCircle(mask Mask, yield func(shape2d.Circle) bool) } // CircleIter returns an iterator over all circle shapes in the scene that -// match the mask, as described by [Scene.EachCircle]. -func (s *Scene[O, T, S]) CircleIter(mask Mask) iter.Seq[shape2d.Circle] { +// match the filter, as described by [Scene.EachCircle]. +func (s *Scene[O, T, S]) CircleIter(filter Filter) iter.Seq[shape2d.Circle] { return func(yield func(shape2d.Circle) bool) { - s.EachCircle(mask, yield) + s.EachCircle(filter, yield) } } // EachRectangle iterates over all rectangle shapes in the scene that match the -// mask and yields them, in world space, to the provided callback. Iteration +// filter and yields them, in world space, to the provided callback. Iteration // stops early if the callback returns false. // -// Note that a zero mask matches no shape at all. Use [FullMask] to iterate -// over every rectangle in the scene. -func (s *Scene[O, T, S]) EachRectangle(mask Mask, yield func(shape2d.Rectangle) bool) { +// Note that the zero value of [Filter] matches every rectangle in the scene. +func (s *Scene[O, T, S]) EachRectangle(filter Filter, yield func(shape2d.Rectangle) bool) { for index := range s.objectShapes { shape := &s.objectShapes[index] if shape.spatialID == query2d.InvalidTreeItemID { @@ -300,7 +298,7 @@ func (s *Scene[O, T, S]) EachRectangle(mask Mask, yield func(shape2d.Rectangle) if shape.kind != objectShapeKindRectangle { continue } - if !shape.satisfiesMask(mask) { + if !shape.satisfiesFilter(filter) { continue } if !yield(shape.toRectangle()) { @@ -310,10 +308,10 @@ func (s *Scene[O, T, S]) EachRectangle(mask Mask, yield func(shape2d.Rectangle) } // RectangleIter returns an iterator over all rectangle shapes in the scene -// that match the mask, as described by [Scene.EachRectangle]. -func (s *Scene[O, T, S]) RectangleIter(mask Mask) iter.Seq[shape2d.Rectangle] { +// that match the filter, as described by [Scene.EachRectangle]. +func (s *Scene[O, T, S]) RectangleIter(filter Filter) iter.Seq[shape2d.Rectangle] { return func(yield func(shape2d.Rectangle) bool) { - s.EachRectangle(mask, yield) + s.EachRectangle(filter, yield) } } @@ -411,58 +409,60 @@ func (s *Scene[O, T, S]) SetTerrainShapeUserData(shapeID TerrainShapeID, userDat } // CollectSegmentObjectIntersections collects all intersections of the segment -// with the object shapes in the scene that match the mask. +// with the object shapes in the scene that match the filter. // // The reported contacts have no source, since the segment is not part of the // scene. Their Depth is the fraction of the segment that lies beyond the // contact point, as described by [shape2d.Contact]. -func (s *Scene[O, T, S]) CollectSegmentObjectIntersections(segment shape2d.Segment, mask Mask, yield ObjectContactCallback) { +func (s *Scene[O, T, S]) CollectSegmentObjectIntersections(segment shape2d.Segment, filter Filter, yield ObjectContactCallback) { s.objectShapeCandidates = s.objectShapeCandidates[:0] s.objectShapeTree.QuerySegment(segment, func(index int32) bool { s.objectShapeCandidates = append(s.objectShapeCandidates, index) return true }) - s.collectSegmentObject(segment, mask, yield) + s.collectSegmentObject(segment, filter, yield) } // CheckSegmentObjectIntersection returns the intersection of the segment with -// the object shape that it enters first, if any. -func (s *Scene[O, T, S]) CheckSegmentObjectIntersection(segment shape2d.Segment, mask Mask) (ObjectContact, bool) { +// the object shape that it enters first, if any. Only object shapes that match +// the filter are considered. +func (s *Scene[O, T, S]) CheckSegmentObjectIntersection(segment shape2d.Segment, filter Filter) (ObjectContact, bool) { var collection DeepestObjectContact - s.CollectSegmentObjectIntersections(segment, mask, collection.AddContact) + s.CollectSegmentObjectIntersections(segment, filter, collection.AddContact) return collection.Contact() } // CollectSegmentTerrainIntersections collects all intersections of the segment -// with the terrain shapes in the scene that match the mask. At most one +// with the terrain shapes in the scene that match the filter. At most one // contact is reported per terrain shape. // // The reported contacts have no source, since the segment is not part of the // scene. Their Depth is the fraction of the segment that lies beyond the // contact point, as described by [shape2d.Contact]. -func (s *Scene[O, T, S]) CollectSegmentTerrainIntersections(segment shape2d.Segment, mask Mask, yield TerrainContactCallback) { +func (s *Scene[O, T, S]) CollectSegmentTerrainIntersections(segment shape2d.Segment, filter Filter, yield TerrainContactCallback) { s.terrainShapeCandidates = s.terrainShapeCandidates[:0] s.terrainShapeTree.QuerySegment(segment, func(index int32) bool { s.terrainShapeCandidates = append(s.terrainShapeCandidates, index) return true }) - s.collectSegmentTerrain(segment, mask, yield) + s.collectSegmentTerrain(segment, filter, yield) } // CheckSegmentTerrainIntersection returns the intersection of the segment with -// the terrain shape that it enters first, if any. -func (s *Scene[O, T, S]) CheckSegmentTerrainIntersection(segment shape2d.Segment, mask Mask) (TerrainContact, bool) { +// the terrain shape that it enters first, if any. Only terrain shapes that +// match the filter are considered. +func (s *Scene[O, T, S]) CheckSegmentTerrainIntersection(segment shape2d.Segment, filter Filter) (TerrainContact, bool) { var collection DeepestTerrainContact - s.CollectSegmentTerrainIntersections(segment, mask, collection.AddContact) + s.CollectSegmentTerrainIntersections(segment, filter, collection.AddContact) return collection.Contact() } // CollectCircleObjectIntersections collects all intersections of the circle -// with the object shapes in the scene that match the mask. +// with the object shapes in the scene that match the filter. // // The reported contacts have no source, since the circle is not part of the // scene. -func (s *Scene[O, T, S]) CollectCircleObjectIntersections(circle shape2d.Circle, mask Mask, yield ObjectContactCallback) { +func (s *Scene[O, T, S]) CollectCircleObjectIntersections(circle shape2d.Circle, filter Filter, yield ObjectContactCallback) { queryAABB := shape2d.AABBFromCircle(circle) s.objectShapeCandidates = s.objectShapeCandidates[:0] @@ -470,24 +470,25 @@ func (s *Scene[O, T, S]) CollectCircleObjectIntersections(circle shape2d.Circle, s.objectShapeCandidates = append(s.objectShapeCandidates, index) return true }) - s.collectCircleObject(circle, mask, yield) + s.collectCircleObject(circle, filter, yield) } // CheckCircleObjectIntersection returns the deepest intersection of the circle -// with an object shape in the scene, if any. -func (s *Scene[O, T, S]) CheckCircleObjectIntersection(circle shape2d.Circle, mask Mask) (ObjectContact, bool) { +// with an object shape in the scene, if any. Only object shapes that match the +// filter are considered. +func (s *Scene[O, T, S]) CheckCircleObjectIntersection(circle shape2d.Circle, filter Filter) (ObjectContact, bool) { var collection DeepestObjectContact - s.CollectCircleObjectIntersections(circle, mask, collection.AddContact) + s.CollectCircleObjectIntersections(circle, filter, collection.AddContact) return collection.Contact() } // CollectCircleTerrainIntersections collects all intersections of the circle -// with the terrain shapes in the scene that match the mask. At most one +// with the terrain shapes in the scene that match the filter. At most one // contact is reported per terrain shape. // // The reported contacts have no source, since the circle is not part of the // scene. -func (s *Scene[O, T, S]) CollectCircleTerrainIntersections(circle shape2d.Circle, mask Mask, yield TerrainContactCallback) { +func (s *Scene[O, T, S]) CollectCircleTerrainIntersections(circle shape2d.Circle, filter Filter, yield TerrainContactCallback) { queryAABB := shape2d.AABBFromCircle(circle) s.terrainShapeCandidates = s.terrainShapeCandidates[:0] @@ -495,23 +496,24 @@ func (s *Scene[O, T, S]) CollectCircleTerrainIntersections(circle shape2d.Circle s.terrainShapeCandidates = append(s.terrainShapeCandidates, index) return true }) - s.collectCircleTerrain(circle, mask, yield) + s.collectCircleTerrain(circle, filter, yield) } // CheckCircleTerrainIntersection returns the deepest intersection of the -// circle with a terrain shape in the scene, if any. -func (s *Scene[O, T, S]) CheckCircleTerrainIntersection(circle shape2d.Circle, mask Mask) (TerrainContact, bool) { +// circle with a terrain shape in the scene, if any. Only terrain shapes that +// match the filter are considered. +func (s *Scene[O, T, S]) CheckCircleTerrainIntersection(circle shape2d.Circle, filter Filter) (TerrainContact, bool) { var collection DeepestTerrainContact - s.CollectCircleTerrainIntersections(circle, mask, collection.AddContact) + s.CollectCircleTerrainIntersections(circle, filter, collection.AddContact) return collection.Contact() } // CollectRectangleObjectIntersections collects all intersections of the -// rectangle with the object shapes in the scene that match the mask. +// rectangle with the object shapes in the scene that match the filter. // // The reported contacts have no source, since the rectangle is not part of the // scene. -func (s *Scene[O, T, S]) CollectRectangleObjectIntersections(rectangle shape2d.Rectangle, mask Mask, yield ObjectContactCallback) { +func (s *Scene[O, T, S]) CollectRectangleObjectIntersections(rectangle shape2d.Rectangle, filter Filter, yield ObjectContactCallback) { queryAABB := shape2d.AABBFromRectangle(rectangle) s.objectShapeCandidates = s.objectShapeCandidates[:0] @@ -519,24 +521,25 @@ func (s *Scene[O, T, S]) CollectRectangleObjectIntersections(rectangle shape2d.R s.objectShapeCandidates = append(s.objectShapeCandidates, index) return true }) - s.collectRectangleObject(rectangle, mask, yield) + s.collectRectangleObject(rectangle, filter, yield) } // CheckRectangleObjectIntersection returns the deepest intersection of the -// rectangle with an object shape in the scene, if any. -func (s *Scene[O, T, S]) CheckRectangleObjectIntersection(rectangle shape2d.Rectangle, mask Mask) (ObjectContact, bool) { +// rectangle with an object shape in the scene, if any. Only object shapes that +// match the filter are considered. +func (s *Scene[O, T, S]) CheckRectangleObjectIntersection(rectangle shape2d.Rectangle, filter Filter) (ObjectContact, bool) { var collection DeepestObjectContact - s.CollectRectangleObjectIntersections(rectangle, mask, collection.AddContact) + s.CollectRectangleObjectIntersections(rectangle, filter, collection.AddContact) return collection.Contact() } // CollectRectangleTerrainIntersections collects all intersections of the -// rectangle with the terrain shapes in the scene that match the mask. At most -// one contact is reported per terrain shape. +// rectangle with the terrain shapes in the scene that match the filter. At +// most one contact is reported per terrain shape. // // The reported contacts have no source, since the rectangle is not part of the // scene. -func (s *Scene[O, T, S]) CollectRectangleTerrainIntersections(rectangle shape2d.Rectangle, mask Mask, yield TerrainContactCallback) { +func (s *Scene[O, T, S]) CollectRectangleTerrainIntersections(rectangle shape2d.Rectangle, filter Filter, yield TerrainContactCallback) { queryAABB := shape2d.AABBFromRectangle(rectangle) s.terrainShapeCandidates = s.terrainShapeCandidates[:0] @@ -544,14 +547,15 @@ func (s *Scene[O, T, S]) CollectRectangleTerrainIntersections(rectangle shape2d. s.terrainShapeCandidates = append(s.terrainShapeCandidates, index) return true }) - s.collectRectangleTerrain(rectangle, mask, yield) + s.collectRectangleTerrain(rectangle, filter, yield) } // CheckRectangleTerrainIntersection returns the deepest intersection of the -// rectangle with a terrain shape in the scene, if any. -func (s *Scene[O, T, S]) CheckRectangleTerrainIntersection(rectangle shape2d.Rectangle, mask Mask) (TerrainContact, bool) { +// rectangle with a terrain shape in the scene, if any. Only terrain shapes +// that match the filter are considered. +func (s *Scene[O, T, S]) CheckRectangleTerrainIntersection(rectangle shape2d.Rectangle, filter Filter) (TerrainContact, bool) { var collection DeepestTerrainContact - s.CollectRectangleTerrainIntersections(rectangle, mask, collection.AddContact) + s.CollectRectangleTerrainIntersections(rectangle, filter, collection.AddContact) return collection.Contact() } @@ -801,8 +805,8 @@ func (s *Scene[O, T, S]) detachTerrainShape(index int32) { s.releaseTerrainShape(index) } -func (s *Scene[O, T, S]) collectSegmentObject(segment shape2d.Segment, mask Mask, yield ObjectContactCallback) { - for index, shape := range s.iterCandidateObjectShapes(mask) { +func (s *Scene[O, T, S]) collectSegmentObject(segment shape2d.Segment, filter Filter, yield ObjectContactCallback) { + for index, shape := range s.iterCandidateObjectShapes(filter) { if !isec2d.CheckSegmentCircleOverlap(segment, shape.wsBCircle) { continue } @@ -826,8 +830,8 @@ func (s *Scene[O, T, S]) collectSegmentObject(segment shape2d.Segment, mask Mask } } -func (s *Scene[O, T, S]) collectSegmentTerrain(segment shape2d.Segment, mask Mask, yield TerrainContactCallback) { - for index, shape := range s.iterCandidateTerrainShapes(mask) { +func (s *Scene[O, T, S]) collectSegmentTerrain(segment shape2d.Segment, filter Filter, yield TerrainContactCallback) { + for index, shape := range s.iterCandidateTerrainShapes(filter) { if !isec2d.CheckSegmentCircleOverlap(segment, shape.wsBCircle) { continue } @@ -847,9 +851,9 @@ func (s *Scene[O, T, S]) collectSegmentTerrain(segment shape2d.Segment, mask Mas } } -func (s *Scene[O, T, S]) collectCircleObject(circle shape2d.Circle, mask Mask, yield ObjectContactCallback) { +func (s *Scene[O, T, S]) collectCircleObject(circle shape2d.Circle, filter Filter, yield ObjectContactCallback) { initGJKShapeForCircle(circle, &s.tempGJKSource) - for index, shape := range s.iterCandidateObjectShapes(mask) { + for index, shape := range s.iterCandidateObjectShapes(filter) { if !isec2d.CheckCircleCircle(circle, shape.wsBCircle) { continue } @@ -865,9 +869,9 @@ func (s *Scene[O, T, S]) collectCircleObject(circle shape2d.Circle, mask Mask, y } } -func (s *Scene[O, T, S]) collectCircleTerrain(circle shape2d.Circle, mask Mask, yield TerrainContactCallback) { +func (s *Scene[O, T, S]) collectCircleTerrain(circle shape2d.Circle, filter Filter, yield TerrainContactCallback) { initGJKShapeForCircle(circle, &s.tempGJKSource) - for index, shape := range s.iterCandidateTerrainShapes(mask) { + for index, shape := range s.iterCandidateTerrainShapes(filter) { s.resolveTerrainShape(s.tempGJKSource, circle, shape, func(contact shape2d.Contact) { yield(TerrainContact{ SourceObjectID: NilObjectID, @@ -880,9 +884,9 @@ func (s *Scene[O, T, S]) collectCircleTerrain(circle shape2d.Circle, mask Mask, } } -func (s *Scene[O, T, S]) collectRectangleObject(rectangle shape2d.Rectangle, mask Mask, yield ObjectContactCallback) { +func (s *Scene[O, T, S]) collectRectangleObject(rectangle shape2d.Rectangle, filter Filter, yield ObjectContactCallback) { initGJKShapeForRectangle(rectangle, &s.tempGJKSource) - for index, shape := range s.iterCandidateObjectShapes(mask) { + for index, shape := range s.iterCandidateObjectShapes(filter) { if !isec2d.CheckCircleCircle(rectangle.BoundingCircle(), shape.wsBCircle) { continue } @@ -898,9 +902,9 @@ func (s *Scene[O, T, S]) collectRectangleObject(rectangle shape2d.Rectangle, mas } } -func (s *Scene[O, T, S]) collectRectangleTerrain(rectangle shape2d.Rectangle, mask Mask, yield TerrainContactCallback) { +func (s *Scene[O, T, S]) collectRectangleTerrain(rectangle shape2d.Rectangle, filter Filter, yield TerrainContactCallback) { initGJKShapeForRectangle(rectangle, &s.tempGJKSource) - for index, shape := range s.iterCandidateTerrainShapes(mask) { + for index, shape := range s.iterCandidateTerrainShapes(filter) { s.resolveTerrainShape(s.tempGJKSource, rectangle.BoundingCircle(), shape, func(contact shape2d.Contact) { yield(TerrainContact{ SourceObjectID: NilObjectID, @@ -986,10 +990,10 @@ func (s *Scene[O, T, S]) resolveTerrainShape(srcGJK gjk2d.Shape, srcBC shape2d.C } } -func (s *Scene[O, T, S]) eachCandidateObjectShape(mask Mask, cb func(int32, *objectShapeState[S]) bool) { +func (s *Scene[O, T, S]) eachCandidateObjectShape(filter Filter, cb func(int32, *objectShapeState[S]) bool) { for _, index := range s.objectShapeCandidates { shape := &s.objectShapes[index] - if !shape.satisfiesMask(mask) { + if !shape.satisfiesFilter(filter) { continue } if !cb(index, shape) { @@ -998,16 +1002,16 @@ func (s *Scene[O, T, S]) eachCandidateObjectShape(mask Mask, cb func(int32, *obj } } -func (s *Scene[O, T, S]) iterCandidateObjectShapes(mask Mask) iter.Seq2[int32, *objectShapeState[S]] { +func (s *Scene[O, T, S]) iterCandidateObjectShapes(filter Filter) iter.Seq2[int32, *objectShapeState[S]] { return func(yield func(int32, *objectShapeState[S]) bool) { - s.eachCandidateObjectShape(mask, yield) + s.eachCandidateObjectShape(filter, yield) } } -func (s *Scene[O, T, S]) eachCandidateTerrainShape(mask Mask, cb func(int32, *terrainShapeState[S]) bool) { +func (s *Scene[O, T, S]) eachCandidateTerrainShape(filter Filter, cb func(int32, *terrainShapeState[S]) bool) { for _, index := range s.terrainShapeCandidates { shape := &s.terrainShapes[index] - if !shape.satisfiesMask(mask) { + if !shape.satisfiesFilter(filter) { continue } if !cb(index, shape) { @@ -1016,9 +1020,9 @@ func (s *Scene[O, T, S]) eachCandidateTerrainShape(mask Mask, cb func(int32, *te } } -func (s *Scene[O, T, S]) iterCandidateTerrainShapes(mask Mask) iter.Seq2[int32, *terrainShapeState[S]] { +func (s *Scene[O, T, S]) iterCandidateTerrainShapes(filter Filter) iter.Seq2[int32, *terrainShapeState[S]] { return func(yield func(int32, *terrainShapeState[S]) bool) { - s.eachCandidateTerrainShape(mask, yield) + s.eachCandidateTerrainShape(filter, yield) } } diff --git a/core/spatial/placement2d/scene_test.go b/core/spatial/placement2d/scene_test.go index 1e076283..b3d6a12d 100644 --- a/core/spatial/placement2d/scene_test.go +++ b/core/spatial/placement2d/scene_test.go @@ -163,7 +163,7 @@ var _ = Describe("Scene", func() { }) var found []shape2d.Circle - scene.EachCircle(placement2d.FullMask, func(c shape2d.Circle) bool { + scene.EachCircle(placement2d.Filter{}, func(c shape2d.Circle) bool { found = append(found, c) return true }) @@ -178,7 +178,7 @@ var _ = Describe("Scene", func() { }) count := 0 - scene.EachRectangle(placement2d.FullMask, func(shape2d.Rectangle) bool { + scene.EachRectangle(placement2d.Filter{}, func(shape2d.Rectangle) bool { count++ return true }) @@ -191,7 +191,7 @@ var _ = Describe("Scene", func() { }) count := 0 - for range scene.CircleIter(placement2d.FullMask) { + for range scene.CircleIter(placement2d.Filter{}) { count++ } Expect(count).To(Equal(1)) @@ -203,7 +203,7 @@ var _ = Describe("Scene", func() { }) count := 0 - for range scene.RectangleIter(placement2d.FullMask) { + for range scene.RectangleIter(placement2d.Filter{}) { count++ } Expect(count).To(Equal(1)) @@ -234,7 +234,7 @@ var _ = Describe("Scene", func() { scene.DeleteObjectShape(shapeID) count := 0 - scene.EachCircle(placement2d.FullMask, func(shape2d.Circle) bool { + scene.EachCircle(placement2d.Filter{}, func(shape2d.Circle) bool { count++ return true }) @@ -250,7 +250,7 @@ var _ = Describe("Scene", func() { }) count := 0 - scene.EachCircle(placement2d.FullMask, func(shape2d.Circle) bool { + scene.EachCircle(placement2d.Filter{}, func(shape2d.Circle) bool { count++ return false }) @@ -269,7 +269,7 @@ var _ = Describe("Scene", func() { )) var centers []dprec.Vec2 - scene.EachCircle(placement2d.FullMask, func(c shape2d.Circle) bool { + scene.EachCircle(placement2d.Filter{}, func(c shape2d.Circle) bool { centers = append(centers, c.Center) return true }) @@ -280,20 +280,21 @@ var _ = Describe("Scene", func() { }) }) - Describe("shape iteration masks", func() { + Describe("shape iteration filters", func() { BeforeEach(func() { objID := scene.CreateObject(placement2d.ObjectInfo[string]{}) scene.AttachCircle(objID, placement2d.CircleInfo[string]{ Filtering: placement2d.FilterInfo{ - SourceMask: opt.V(uint32(0b01)), + RejectGroup: 7, + SourceMask: opt.V(uint32(0b01)), }, Circle: circleAt(0.0, 0.0, 1.0), }) }) - countCircles := func(mask placement2d.Mask) int { + countCircles := func(filter placement2d.Filter) int { count := 0 - scene.EachCircle(mask, func(shape2d.Circle) bool { + scene.EachCircle(filter, func(shape2d.Circle) bool { count++ return true }) @@ -301,15 +302,33 @@ var _ = Describe("Scene", func() { } It("yields shapes that occupy a layer of the mask", func() { - Expect(countCircles(0b01)).To(Equal(1)) + Expect(countCircles(placement2d.Filter{Mask: 0b01})).To(Equal(1)) }) It("skips shapes that occupy no layer of the mask", func() { - Expect(countCircles(0b10)).To(BeZero()) + Expect(countCircles(placement2d.Filter{Mask: 0b10})).To(BeZero()) + }) + + It("yields everything for the zero mask", func() { + Expect(countCircles(placement2d.Filter{Mask: 0})).To(Equal(1)) }) - It("yields nothing for the zero mask", func() { - Expect(countCircles(0)).To(BeZero()) + It("yields everything for the full mask", func() { + Expect(countCircles(placement2d.Filter{ + Mask: placement2d.FullMask, + })).To(Equal(1)) + }) + + It("skips shapes that share the reject group", func() { + Expect(countCircles(placement2d.Filter{RejectGroup: 7})).To(BeZero()) + }) + + It("yields shapes that have a different reject group", func() { + Expect(countCircles(placement2d.Filter{RejectGroup: 8})).To(Equal(1)) + }) + + It("yields everything for the zero filter", func() { + Expect(countCircles(placement2d.Filter{})).To(Equal(1)) }) }) @@ -751,7 +770,7 @@ var _ = Describe("Scene", func() { contact, ok := scene.CheckCircleObjectIntersection( circleAt(1.5, 0.0, 1.0), - placement2d.FullMask, + placement2d.Filter{}, ) Expect(ok).To(BeTrue()) Expect(contact.SourceObjectID).To(Equal(placement2d.NilObjectID)) @@ -768,7 +787,7 @@ var _ = Describe("Scene", func() { _, ok := scene.CheckCircleObjectIntersection( circleAt(10.0, 0.0, 1.0), - placement2d.FullMask, + placement2d.Filter{}, ) Expect(ok).To(BeFalse()) }) @@ -784,7 +803,23 @@ var _ = Describe("Scene", func() { _, ok := scene.CheckCircleObjectIntersection( circleAt(1.5, 0.0, 1.0), - 0b10, + placement2d.Filter{Mask: 0b10}, + ) + Expect(ok).To(BeFalse()) + }) + + It("honors the query reject group", func() { + objID := scene.CreateObject(placement2d.ObjectInfo[string]{}) + scene.AttachCircle(objID, placement2d.CircleInfo[string]{ + Filtering: placement2d.FilterInfo{ + RejectGroup: 7, + }, + Circle: circleAt(0.0, 0.0, 1.0), + }) + + _, ok := scene.CheckCircleObjectIntersection( + circleAt(1.5, 0.0, 1.0), + placement2d.Filter{RejectGroup: 7}, ) Expect(ok).To(BeFalse()) }) @@ -798,7 +833,7 @@ var _ = Describe("Scene", func() { // The line faces -Y, so approach it from below (the front side). contact, ok := scene.CheckCircleTerrainIntersection( circleAt(0.0, -0.5, 1.0), - placement2d.FullMask, + placement2d.Filter{}, ) Expect(ok).To(BeTrue()) Expect(contact.SourceShapeID).To(Equal(placement2d.NilObjectShapeID)) @@ -815,7 +850,7 @@ var _ = Describe("Scene", func() { _, ok := scene.CheckCircleTerrainIntersection( circleAt(0.0, 0.5, 1.0), - placement2d.FullMask, + placement2d.Filter{}, ) Expect(ok).To(BeFalse()) }) @@ -828,7 +863,7 @@ var _ = Describe("Scene", func() { _, ok := scene.CheckCircleTerrainIntersection( circleAt(0.5, 0.0, 1.0), - placement2d.FullMask, + placement2d.Filter{}, ) Expect(ok).To(BeFalse()) }) @@ -843,7 +878,7 @@ var _ = Describe("Scene", func() { contact, ok := scene.CheckRectangleObjectIntersection( rectangleAt(1.5, 0.0, 1.0), - placement2d.FullMask, + placement2d.Filter{}, ) Expect(ok).To(BeTrue()) Expect(contact.SourceShapeID).To(Equal(placement2d.NilObjectShapeID)) @@ -859,7 +894,7 @@ var _ = Describe("Scene", func() { _, ok := scene.CheckRectangleObjectIntersection( rectangleAt(10.0, 0.0, 1.0), - placement2d.FullMask, + placement2d.Filter{}, ) Expect(ok).To(BeFalse()) }) @@ -873,7 +908,7 @@ var _ = Describe("Scene", func() { // The line faces -Y, so approach it from below (the front side). contact, ok := scene.CheckRectangleTerrainIntersection( rectangleAt(0.0, -0.5, 1.0), - placement2d.FullMask, + placement2d.Filter{}, ) Expect(ok).To(BeTrue()) Expect(contact.TargetTerrainID).To(Equal(terrainID)) @@ -889,7 +924,7 @@ var _ = Describe("Scene", func() { _, ok := scene.CheckRectangleTerrainIntersection( rectangleAt(0.0, 0.5, 1.0), - placement2d.FullMask, + placement2d.Filter{}, ) Expect(ok).To(BeFalse()) }) @@ -912,7 +947,7 @@ var _ = Describe("Scene", func() { dprec.NewVec2(-5.0, 0.0), dprec.NewVec2(9.0, 0.0), ), - placement2d.FullMask, + placement2d.Filter{}, contacts.AddContact, ) Expect(contacts).To(HaveLen(2)) @@ -929,7 +964,7 @@ var _ = Describe("Scene", func() { dprec.NewVec2(-5.0, 0.0), dprec.NewVec2(5.0, 0.0), ), - placement2d.FullMask, + placement2d.Filter{}, ) Expect(ok).To(BeTrue()) Expect(contact.SourceShapeID).To(Equal(placement2d.NilObjectShapeID)) @@ -952,7 +987,7 @@ var _ = Describe("Scene", func() { dprec.NewVec2(-5.0, 0.0), dprec.NewVec2(9.0, 0.0), ), - placement2d.FullMask, + placement2d.Filter{}, ) Expect(ok).To(BeTrue()) Expect(contact.TargetShapeID).To(Equal(nearShapeID)) @@ -969,7 +1004,7 @@ var _ = Describe("Scene", func() { dprec.NewVec2(2.0, -5.0), dprec.NewVec2(2.0, 5.0), ), - placement2d.FullMask, + placement2d.Filter{}, ) Expect(ok).To(BeTrue()) Expect(contact.SourceShapeID).To(Equal(placement2d.NilObjectShapeID)) @@ -988,7 +1023,7 @@ var _ = Describe("Scene", func() { dprec.NewVec2(-5.0, 5.0), dprec.NewVec2(5.0, 5.0), ), - placement2d.FullMask, + placement2d.Filter{}, ) Expect(ok).To(BeFalse()) }) @@ -1004,7 +1039,7 @@ var _ = Describe("Scene", func() { dprec.NewVec2(0.0, 5.0), dprec.NewVec2(0.0, -5.0), ), - placement2d.FullMask, + placement2d.Filter{}, ) Expect(ok).To(BeFalse()) }) From c5555c4aba6630b65e7c2d3ca438c4b29579586c Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Tue, 11 Aug 2026 21:44:43 +0300 Subject: [PATCH 75/85] Add ball-joint constraint solver --- game/physics/constraint/pair_attachment.go | 62 -------- game/physics/solver_ball_joint.go | 157 +++++++++++++++++++++ 2 files changed, 157 insertions(+), 62 deletions(-) delete mode 100644 game/physics/constraint/pair_attachment.go create mode 100644 game/physics/solver_ball_joint.go diff --git a/game/physics/constraint/pair_attachment.go b/game/physics/constraint/pair_attachment.go deleted file mode 100644 index 1934b5ac..00000000 --- a/game/physics/constraint/pair_attachment.go +++ /dev/null @@ -1,62 +0,0 @@ -package constraint - -// import ( -// "github.com/mokiat/gomath/dprec" -// "github.com/mokiat/lacking/game/physics/solver" -// ) - -// // NewPairAttachment creates a new PairAttachment constraint solver, which -// // can be used to attach two bodies together at given offsets. Each body -// // is still free to rotate independently. -// func NewPairAttachment() *PairAttachment { -// solverX := NewMatchDirectionOffset().SetDirection(dprec.BasisXVec3()).SetOffset(0.0) -// solverY := NewMatchDirectionOffset().SetDirection(dprec.BasisYVec3()).SetOffset(0.0) -// solverZ := NewMatchDirectionOffset().SetDirection(dprec.BasisZVec3()).SetOffset(0.0) -// return &PairAttachment{ -// solverX: *solverX, -// solverY: *solverY, -// solverZ: *solverZ, -// } -// } - -// var _ solver.PairConstraint = (*PairAttachment)(nil) - -// // TODO: Implement the following constraint independently. - -// type PairAttachment struct { -// solverX MatchDirectionOffset -// solverY MatchDirectionOffset -// solverZ MatchDirectionOffset -// } - -// func (s *PairAttachment) SetPrimaryOffset(offset dprec.Vec3) *PairAttachment { -// s.solverX.SetPrimaryRadius(offset) -// s.solverY.SetPrimaryRadius(offset) -// s.solverZ.SetPrimaryRadius(offset) -// return s -// } - -// func (s *PairAttachment) SetSecondaryOffset(offset dprec.Vec3) *PairAttachment { -// s.solverX.SetSecondaryRadius(offset) -// s.solverY.SetSecondaryRadius(offset) -// s.solverZ.SetSecondaryRadius(offset) -// return s -// } - -// func (s *PairAttachment) Reset(ctx solver.PairContext) { -// s.solverX.Reset(ctx) -// s.solverY.Reset(ctx) -// s.solverZ.Reset(ctx) -// } - -// func (s *PairAttachment) ApplyImpulses(ctx solver.PairContext) { -// s.solverX.ApplyImpulses(ctx) -// s.solverY.ApplyImpulses(ctx) -// s.solverZ.ApplyImpulses(ctx) -// } - -// func (s *PairAttachment) ApplyNudges(ctx solver.PairContext) { -// s.solverX.ApplyNudges(ctx) -// s.solverY.ApplyNudges(ctx) -// s.solverZ.ApplyNudges(ctx) -// } diff --git a/game/physics/solver_ball_joint.go b/game/physics/solver_ball_joint.go new file mode 100644 index 00000000..d7e4976f --- /dev/null +++ b/game/physics/solver_ball_joint.go @@ -0,0 +1,157 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +// BallJointSolverConfig holds the parameters with which a +// [BallJointSolver] is configured, either through [NewBallJointSolver] +// or [BallJointSolver.Configure]. +type BallJointSolverConfig struct { + + // PrimaryBodyAnchorOffset is the body-local-space offset, relative to + // the primary target's center of mass, of the point at which the + // ball joint is anchored on the primary body. + PrimaryBodyAnchorOffset dprec.Vec3 + + // SecondaryBodyAnchorOffset is the body-local-space offset, relative + // to the secondary target's center of mass, of the point at which + // the ball joint is anchored on the secondary body. + SecondaryBodyAnchorOffset dprec.Vec3 +} + +// BallJointSolver is a [PairConstraintSolver] that holds an anchor point +// on each of its two target bodies coincident in world space, acting +// like a ball-and-socket joint between the two - it pins the anchor +// points together while leaving all relative rotation between the +// targets unconstrained. +// +// Unlike [DistanceSolver], which only resists the anchor points moving +// closer together or farther apart, BallJointSolver fully constrains +// their relative position, leaving only rotation free. +// +// Internally, it is implemented as three [AxisDisplacementSolver]s, one +// for each of the primary body's local X, Y and Z axes, each configured +// with a zero [AxisDisplacementSolver.Displacement]. Together, since +// those axes span the primary body's entire local frame, they pin the +// anchor points coincident. +// +// A BallJointSolver must be configured, either through +// [NewBallJointSolver] or [BallJointSolver.Configure], before being +// registered with a [Scene] through [PairConstraintView.Create]. +type BallJointSolver struct { + solverX AxisDisplacementSolver + solverY AxisDisplacementSolver + solverZ AxisDisplacementSolver +} + +var _ PairConstraintSolver = (*BallJointSolver)(nil) + +// NewBallJointSolver creates a new [BallJointSolver] configured +// according to config. +func NewBallJointSolver(config BallJointSolverConfig) *BallJointSolver { + result := &BallJointSolver{} + result.Configure(config) + return result +} + +// Configure configures this solver according to config. +// +// Configure must be called before this solver is registered with a +// [Scene] through [PairConstraintView.Create]. Unlike +// [NewBallJointSolver], it can be called on an already-allocated solver, +// which allows solvers to be cached (e.g. in a slice) and configured on +// demand. +func (s *BallJointSolver) Configure(config BallJointSolverConfig) { + s.solverX.Configure(AxisDisplacementSolverConfig{ + PrimaryBodyAnchorOffset: config.PrimaryBodyAnchorOffset, + PrimaryBodyAxis: dprec.BasisXVec3(), + SecondaryBodyAnchorOffset: config.SecondaryBodyAnchorOffset, + Displacement: 0.0, + }) + s.solverY.Configure(AxisDisplacementSolverConfig{ + PrimaryBodyAnchorOffset: config.PrimaryBodyAnchorOffset, + PrimaryBodyAxis: dprec.BasisYVec3(), + SecondaryBodyAnchorOffset: config.SecondaryBodyAnchorOffset, + Displacement: 0.0, + }) + s.solverZ.Configure(AxisDisplacementSolverConfig{ + PrimaryBodyAnchorOffset: config.PrimaryBodyAnchorOffset, + PrimaryBodyAxis: dprec.BasisZVec3(), + SecondaryBodyAnchorOffset: config.SecondaryBodyAnchorOffset, + Displacement: 0.0, + }) +} + +// PrimaryBodyAnchorOffset returns the body-local-space offset, relative +// to the primary target's center of mass, of the point at which the +// ball joint is anchored on the primary body. +func (s *BallJointSolver) PrimaryBodyAnchorOffset() dprec.Vec3 { + return s.solverX.PrimaryBodyAnchorOffset() +} + +// SetPrimaryBodyAnchorOffset changes the body-local-space offset, +// relative to the primary target's center of mass, of the point at +// which the ball joint is anchored on the primary body. +// +// It returns the solver itself, so that calls can be chained. +func (s *BallJointSolver) SetPrimaryBodyAnchorOffset(offset dprec.Vec3) *BallJointSolver { + s.solverX.SetPrimaryBodyAnchorOffset(offset) + s.solverY.SetPrimaryBodyAnchorOffset(offset) + s.solverZ.SetPrimaryBodyAnchorOffset(offset) + return s +} + +// SecondaryBodyAnchorOffset returns the body-local-space offset, +// relative to the secondary target's center of mass, of the point at +// which the ball joint is anchored on the secondary body. +func (s *BallJointSolver) SecondaryBodyAnchorOffset() dprec.Vec3 { + return s.solverX.SecondaryBodyAnchorOffset() +} + +// SetSecondaryBodyAnchorOffset changes the body-local-space offset, +// relative to the secondary target's center of mass, of the point at +// which the ball joint is anchored on the secondary body. +// +// It returns the solver itself, so that calls can be chained. +func (s *BallJointSolver) SetSecondaryBodyAnchorOffset(offset dprec.Vec3) *BallJointSolver { + s.solverX.SetSecondaryBodyAnchorOffset(offset) + s.solverY.SetSecondaryBodyAnchorOffset(offset) + s.solverZ.SetSecondaryBodyAnchorOffset(offset) + return s +} + +// Reset implements [PairConstraintSolver.Reset]. +// +// It calls [AxisDisplacementSolver.Reset] on each of the three per-axis +// sub-solvers, in X, Y, Z order, so that each recomputes its own +// Jacobians and drift from the targets' current positions and +// rotations. +func (s *BallJointSolver) Reset(ctx PairConstraintContext) { + s.solverX.Reset(ctx) + s.solverY.Reset(ctx) + s.solverZ.Reset(ctx) +} + +// ApplyImpulses implements [PairConstraintSolver.ApplyImpulses]. +// +// It calls [AxisDisplacementSolver.ApplyImpulses] on each of the three +// per-axis sub-solvers, in X, Y, Z order, so that their combined +// impulses drive the anchor points' relative velocity toward zero along +// all three of the primary body's local axes. +func (s *BallJointSolver) ApplyImpulses(ctx PairConstraintContext) { + s.solverX.ApplyImpulses(ctx) + s.solverY.ApplyImpulses(ctx) + s.solverZ.ApplyImpulses(ctx) +} + +// ApplyNudges implements [PairConstraintSolver.ApplyNudges]. +// +// It calls [AxisDisplacementSolver.ApplyNudges] on each of the three +// per-axis sub-solvers, in X, Y, Z order. Each sub-solver recomputes its +// own Jacobians and drift before nudging, as required by +// [AxisDisplacementSolver.ApplyNudges], so a nudge applied by an earlier +// sub-solver in this call is correctly observed by the ones that follow. +func (s *BallJointSolver) ApplyNudges(ctx PairConstraintContext) { + s.solverX.ApplyNudges(ctx) + s.solverY.ApplyNudges(ctx) + s.solverZ.ApplyNudges(ctx) +} From 53f3e2d0ff0217abf58343c3ad05fb9b990e2543 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Tue, 11 Aug 2026 22:41:19 +0300 Subject: [PATCH 76/85] add match-axes constraint solver --- game/physics/constraint/match_direction.go | 107 ---------- game/physics/solver_match_axes.go | 227 +++++++++++++++++++++ 2 files changed, 227 insertions(+), 107 deletions(-) delete mode 100644 game/physics/constraint/match_direction.go create mode 100644 game/physics/solver_match_axes.go diff --git a/game/physics/constraint/match_direction.go b/game/physics/constraint/match_direction.go deleted file mode 100644 index bcd1a191..00000000 --- a/game/physics/constraint/match_direction.go +++ /dev/null @@ -1,107 +0,0 @@ -package constraint - -// import ( -// "github.com/mokiat/gomath/dprec" -// "github.com/mokiat/lacking/game/physics/solver" -// ) - -// // NewMatchDirections creates a new MatchDirections constraint solver. -// func NewMatchDirections() *MatchDirections { -// return &MatchDirections{ -// primaryDirection: dprec.BasisYVec3(), -// secondaryDirection: dprec.BasisYVec3(), -// } -// } - -// var _ solver.PairConstraint = (*MatchDirections)(nil) - -// // MatchDirections represents the solution for a constraint -// // that keeps the direction of two bodies pointing in the same -// // direction. -// type MatchDirections struct { -// primaryDirection dprec.Vec3 -// secondaryDirection dprec.Vec3 - -// jacobian1 solver.PairJacobian -// jacobian2 solver.PairJacobian -// drift1 float64 -// drift2 float64 -// } - -// // PrimaryDirection returns the direction of the primary body that will be -// // used in the alignment. -// func (s *MatchDirections) PrimaryDirection() dprec.Vec3 { -// return s.primaryDirection -// } - -// // SetPrimaryDirection changes the direction of the primary body to be used -// // in the alignment. -// func (s *MatchDirections) SetPrimaryDirection(direction dprec.Vec3) *MatchDirections { -// s.primaryDirection = dprec.UnitVec3(direction) -// return s -// } - -// // SecondaryDirection returns the direction of the secondary body that will be -// // used in the alignment. -// func (s *MatchDirections) SecondaryDirection() dprec.Vec3 { -// return s.secondaryDirection -// } - -// // SetSecondaryDirection changes the direction of the secondary body to be -// // used in the alignment. -// func (s *MatchDirections) SetSecondaryDirection(direction dprec.Vec3) *MatchDirections { -// s.secondaryDirection = dprec.UnitVec3(direction) -// return s -// } - -// func (s *MatchDirections) Reset(ctx solver.PairContext) { -// primaryDirWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.primaryDirection) -// secondaryDirWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.secondaryDirection) -// secondaryNorm1 := dprec.NormalVec3(secondaryDirWS) -// secondaryNorm2 := dprec.Vec3Cross(secondaryDirWS, secondaryNorm1) - -// // FIXME: This jacobian converges better than the original one-tier -// // but produces a wrong result if the second object flips all the way -// // around. -// s.jacobian1 = solver.PairJacobian{ -// Target: solver.Jacobian{ -// LinearSlope: dprec.ZeroVec3(), -// AngularSlope: dprec.Vec3Cross(primaryDirWS, secondaryNorm1), -// }, -// Source: solver.Jacobian{ -// LinearSlope: dprec.ZeroVec3(), -// AngularSlope: dprec.Vec3Cross(secondaryNorm1, primaryDirWS), -// }, -// } -// s.jacobian2 = solver.PairJacobian{ -// Target: solver.Jacobian{ -// LinearSlope: dprec.ZeroVec3(), -// AngularSlope: dprec.Vec3Cross(primaryDirWS, secondaryNorm2), -// }, -// Source: solver.Jacobian{ -// LinearSlope: dprec.ZeroVec3(), -// AngularSlope: dprec.Vec3Cross(secondaryNorm2, primaryDirWS), -// }, -// } - -// s.drift1 = dprec.Vec3Dot(primaryDirWS, secondaryNorm1) -// s.drift2 = dprec.Vec3Dot(primaryDirWS, secondaryNorm2) -// } - -// func (s *MatchDirections) ApplyImpulses(ctx solver.PairContext) { -// solution := ctx.JacobianImpulseSolution(s.jacobian1, s.drift1, 0.0) -// ctx.Target.ApplyImpulse(solution.Target) -// ctx.Source.ApplyImpulse(solution.Source) -// solution = ctx.JacobianImpulseSolution(s.jacobian2, s.drift2, 0.0) -// ctx.Target.ApplyImpulse(solution.Target) -// ctx.Source.ApplyImpulse(solution.Source) -// } - -// func (s *MatchDirections) ApplyNudges(ctx solver.PairContext) { -// solution := ctx.JacobianNudgeSolution(s.jacobian1, s.drift1) -// ctx.Target.ApplyNudge(solution.Target) -// ctx.Source.ApplyNudge(solution.Source) -// solution = ctx.JacobianNudgeSolution(s.jacobian2, s.drift2) -// ctx.Target.ApplyNudge(solution.Target) -// ctx.Source.ApplyNudge(solution.Source) -// } diff --git a/game/physics/solver_match_axes.go b/game/physics/solver_match_axes.go new file mode 100644 index 00000000..5019eda0 --- /dev/null +++ b/game/physics/solver_match_axes.go @@ -0,0 +1,227 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +// MatchAxesSolverConfig holds the parameters with which a +// [MatchAxesSolver] is configured, either through [NewMatchAxesSolver] +// or [MatchAxesSolver.Configure]. +type MatchAxesSolverConfig struct { + + // PrimaryBodyAxis is the body-local-space direction, relative to the + // primary target's rotation, that is driven to become parallel with + // SecondaryBodyAxis. It need not be unit-length; it is normalized + // when the solver is configured. + PrimaryBodyAxis dprec.Vec3 + + // SecondaryBodyAxis is the body-local-space direction, relative to + // the secondary target's rotation, that PrimaryBodyAxis is driven to + // become parallel with. It need not be unit-length; it is normalized + // when the solver is configured. + SecondaryBodyAxis dprec.Vec3 +} + +// MatchAxesSolver is a [PairConstraintSolver] that rotates its two +// target bodies so that a body-fixed axis on the primary target becomes +// parallel to a body-fixed axis on the secondary target - it aligns the +// two axes as lines, without caring which of the two directions along +// that shared line either axis actually points. +// +// Unlike [CopyRotationSolver], which unconditionally makes the primary +// target track the secondary target's entire rotation, MatchAxesSolver +// only removes the 2 rotational degrees of freedom that would otherwise +// let the two axes drift apart; both targets remain free to spin about +// their now-shared axis, and their relative translation is left +// entirely unconstrained. +// +// A MatchAxesSolver must be configured, either through +// [NewMatchAxesSolver] or [MatchAxesSolver.Configure], before being +// registered with a [Scene] through [PairConstraintView.Create]. +type MatchAxesSolver struct { + primaryBodyAxis dprec.Vec3 + secondaryBodyAxis dprec.Vec3 + + primaryJacobian1 Jacobian + primaryJacobian2 Jacobian + secondaryJacobian1 Jacobian + secondaryJacobian2 Jacobian + drift1 float64 + drift2 float64 +} + +var _ PairConstraintSolver = (*MatchAxesSolver)(nil) + +// NewMatchAxesSolver creates a new [MatchAxesSolver] configured according +// to config. +func NewMatchAxesSolver(config MatchAxesSolverConfig) *MatchAxesSolver { + result := &MatchAxesSolver{} + result.Configure(config) + return result +} + +// Configure configures this solver according to config. PrimaryBodyAxis +// and SecondaryBodyAxis are each normalized to unit length. +// +// Configure must be called before this solver is registered with a +// [Scene] through [PairConstraintView.Create]. Unlike +// [NewMatchAxesSolver], it can be called on an already-allocated solver, +// which allows solvers to be cached (e.g. in a slice) and configured on +// demand. +func (s *MatchAxesSolver) Configure(config MatchAxesSolverConfig) { + s.primaryBodyAxis = dprec.UnitVec3(config.PrimaryBodyAxis) + s.secondaryBodyAxis = dprec.UnitVec3(config.SecondaryBodyAxis) +} + +// PrimaryBodyAxis returns the body-local-space, unit-length direction, +// relative to the primary target's rotation, that is driven to become +// parallel with [MatchAxesSolver.SecondaryBodyAxis]. +func (s *MatchAxesSolver) PrimaryBodyAxis() dprec.Vec3 { + return s.primaryBodyAxis +} + +// SetPrimaryBodyAxis changes the body-local-space direction, relative to +// the primary target's rotation, that is driven to become parallel with +// [MatchAxesSolver.SecondaryBodyAxis]. The provided axis need not be +// unit-length; it is normalized before being stored. +// +// It returns the solver itself, so that calls can be chained. +func (s *MatchAxesSolver) SetPrimaryBodyAxis(axis dprec.Vec3) *MatchAxesSolver { + s.primaryBodyAxis = dprec.UnitVec3(axis) + return s +} + +// SecondaryBodyAxis returns the body-local-space, unit-length direction, +// relative to the secondary target's rotation, that +// [MatchAxesSolver.PrimaryBodyAxis] is driven to become parallel with. +func (s *MatchAxesSolver) SecondaryBodyAxis() dprec.Vec3 { + return s.secondaryBodyAxis +} + +// SetSecondaryBodyAxis changes the body-local-space direction, relative +// to the secondary target's rotation, that +// [MatchAxesSolver.PrimaryBodyAxis] is driven to become parallel with. +// The provided axis need not be unit-length; it is normalized before +// being stored. +// +// It returns the solver itself, so that calls can be chained. +func (s *MatchAxesSolver) SetSecondaryBodyAxis(axis dprec.Vec3) *MatchAxesSolver { + s.secondaryBodyAxis = dprec.UnitVec3(axis) + return s +} + +// Reset implements [PairConstraintSolver.Reset]. +// +// It recomputes the constraint's Jacobians and current alignment error +// (drift) between the two axes, the same way +// [MatchAxesSolver.recompute] does. +func (s *MatchAxesSolver) Reset(ctx PairConstraintContext) { + s.recompute(ctx) +} + +// ApplyImpulses implements [PairConstraintSolver.ApplyImpulses]. +// +// It resolves impulses, without restitution, that drive the two +// targets' relative angular velocity toward closing the alignment error +// (drift) computed by [MatchAxesSolver.Reset], rotating the two axes +// back toward each other whenever they have drifted apart. +func (s *MatchAxesSolver) ApplyImpulses(ctx PairConstraintContext) { + primaryImpulse1, secondaryImpulse1 := ctx.ImpulseSolution( + s.primaryJacobian1, + s.secondaryJacobian1, + s.drift1, + 0.0, + ) + primaryImpulse2, secondaryImpulse2 := ctx.ImpulseSolution( + s.primaryJacobian2, + s.secondaryJacobian2, + s.drift2, + 0.0, + ) + + primaryImpulse := Impulse{ + Linear: dprec.Vec3Sum(primaryImpulse1.Linear, primaryImpulse2.Linear), + Angular: dprec.Vec3Sum(primaryImpulse1.Angular, primaryImpulse2.Angular), + } + secondaryImpulse := Impulse{ + Linear: dprec.Vec3Sum(secondaryImpulse1.Linear, secondaryImpulse2.Linear), + Angular: dprec.Vec3Sum(secondaryImpulse1.Angular, secondaryImpulse2.Angular), + } + + ctx.PrimaryTarget.ApplyImpulse(primaryImpulse) + ctx.SecondaryTarget.ApplyImpulse(secondaryImpulse) +} + +// ApplyNudges implements [PairConstraintSolver.ApplyNudges]. +// +// It first recomputes the constraint's Jacobians and current alignment +// error (drift), the same way [MatchAxesSolver.recompute] does, since a +// preceding nudge - by this solver's own previous iteration, or by +// another constraint acting on either target - may have rotated either +// target since [MatchAxesSolver.Reset] or the last call to this method. +// It then nudges both targets' rotations to reduce any remaining +// alignment error between the two axes. +func (s *MatchAxesSolver) ApplyNudges(ctx PairConstraintContext) { + s.recompute(ctx) + + primaryNudge1, secondaryNudge1 := ctx.NudgeSolution( + s.primaryJacobian1, + s.secondaryJacobian1, + s.drift1, + ) + primaryNudge2, secondaryNudge2 := ctx.NudgeSolution( + s.primaryJacobian2, + s.secondaryJacobian2, + s.drift2, + ) + + primaryNudge := Nudge{ + Linear: dprec.Vec3Sum(primaryNudge1.Linear, primaryNudge2.Linear), + Angular: dprec.Vec3Sum(primaryNudge1.Angular, primaryNudge2.Angular), + } + secondaryNudge := Nudge{ + Linear: dprec.Vec3Sum(secondaryNudge1.Linear, secondaryNudge2.Linear), + Angular: dprec.Vec3Sum(secondaryNudge1.Angular, secondaryNudge2.Angular), + } + + ctx.PrimaryTarget.ApplyNudge(primaryNudge) + ctx.SecondaryTarget.ApplyNudge(secondaryNudge) +} + +// recompute recalculates the constraint's Jacobians, along with the +// world-space direction of each target's axis (derived from +// PrimaryBodyAxis and SecondaryBodyAxis combined with each target's +// current rotation), and the current alignment error (drift) between +// the two axes, based on the targets' current rotations. +// +// The alignment error is measured as the two components of the primary +// axis along secondaryAxisNorm1 and secondaryAxisNorm2, an arbitrary +// orthonormal basis of the plane perpendicular to the secondary axis - +// both components are zero exactly when the primary axis is parallel +// (or antiparallel) to the secondary axis, regardless of which +// orthonormal basis of that plane is chosen. +func (s *MatchAxesSolver) recompute(ctx PairConstraintContext) { + primaryAxisWS := dprec.QuatVec3Rotation(ctx.PrimaryTarget.Rotation(), s.primaryBodyAxis) + secondaryAxisWS := dprec.QuatVec3Rotation(ctx.SecondaryTarget.Rotation(), s.secondaryBodyAxis) + secondaryAxisNorm1 := dprec.NormalVec3(secondaryAxisWS) + secondaryAxisNorm2 := dprec.Vec3Cross(secondaryAxisWS, secondaryAxisNorm1) + + s.primaryJacobian1 = Jacobian{ + LinearSlope: dprec.ZeroVec3(), + AngularSlope: dprec.Vec3Cross(secondaryAxisNorm1, primaryAxisWS), + } + s.secondaryJacobian1 = Jacobian{ + LinearSlope: dprec.ZeroVec3(), + AngularSlope: dprec.Vec3Cross(primaryAxisWS, secondaryAxisNorm1), + } + + s.primaryJacobian2 = Jacobian{ + LinearSlope: dprec.ZeroVec3(), + AngularSlope: dprec.Vec3Cross(secondaryAxisNorm2, primaryAxisWS), + } + s.secondaryJacobian2 = Jacobian{ + LinearSlope: dprec.ZeroVec3(), + AngularSlope: dprec.Vec3Cross(primaryAxisWS, secondaryAxisNorm2), + } + + s.drift1 = dprec.Vec3Dot(primaryAxisWS, secondaryAxisNorm1) + s.drift2 = dprec.Vec3Dot(primaryAxisWS, secondaryAxisNorm2) +} From 46691456dd426561d71d3b8dec1f40a7a00e5a96 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Tue, 11 Aug 2026 23:05:03 +0300 Subject: [PATCH 77/85] add match-rotations constraint --- game/physics/constraint/match_rotation.go | 20 ----- game/physics/solver_ball_joint.go | 32 ++++---- game/physics/solver_match_rotations.go | 95 +++++++++++++++++++++++ 3 files changed, 108 insertions(+), 39 deletions(-) delete mode 100644 game/physics/constraint/match_rotation.go create mode 100644 game/physics/solver_match_rotations.go diff --git a/game/physics/constraint/match_rotation.go b/game/physics/constraint/match_rotation.go deleted file mode 100644 index d60accd0..00000000 --- a/game/physics/constraint/match_rotation.go +++ /dev/null @@ -1,20 +0,0 @@ -package constraint - -// import ( -// "github.com/mokiat/gomath/dprec" -// "github.com/mokiat/lacking/game/physics/solver" -// ) - -// // NewMatchRotation creates a new constraint solver that keeps -// // two bodies oriented in the same direction on all axis. -// func NewMatchRotation() solver.PairConstraint { -// // TODO: Do a three-jacobian solution here -// return NewPairCombined( -// NewMatchDirections(). -// SetPrimaryDirection(dprec.BasisXVec3()). -// SetSecondaryDirection(dprec.BasisXVec3()), -// NewMatchDirections(). -// SetPrimaryDirection(dprec.BasisZVec3()). -// SetSecondaryDirection(dprec.BasisZVec3()), -// ) -// } diff --git a/game/physics/solver_ball_joint.go b/game/physics/solver_ball_joint.go index d7e4976f..4f38248b 100644 --- a/game/physics/solver_ball_joint.go +++ b/game/physics/solver_ball_joint.go @@ -28,12 +28,6 @@ type BallJointSolverConfig struct { // closer together or farther apart, BallJointSolver fully constrains // their relative position, leaving only rotation free. // -// Internally, it is implemented as three [AxisDisplacementSolver]s, one -// for each of the primary body's local X, Y and Z axes, each configured -// with a zero [AxisDisplacementSolver.Displacement]. Together, since -// those axes span the primary body's entire local frame, they pin the -// anchor points coincident. -// // A BallJointSolver must be configured, either through // [NewBallJointSolver] or [BallJointSolver.Configure], before being // registered with a [Scene] through [PairConstraintView.Create]. @@ -121,10 +115,9 @@ func (s *BallJointSolver) SetSecondaryBodyAnchorOffset(offset dprec.Vec3) *BallJ // Reset implements [PairConstraintSolver.Reset]. // -// It calls [AxisDisplacementSolver.Reset] on each of the three per-axis -// sub-solvers, in X, Y, Z order, so that each recomputes its own -// Jacobians and drift from the targets' current positions and -// rotations. +// It recomputes the constraint's internal state - the Jacobians and +// drift needed to correct any remaining separation between the anchor +// points - from the targets' current positions and rotations. func (s *BallJointSolver) Reset(ctx PairConstraintContext) { s.solverX.Reset(ctx) s.solverY.Reset(ctx) @@ -133,10 +126,9 @@ func (s *BallJointSolver) Reset(ctx PairConstraintContext) { // ApplyImpulses implements [PairConstraintSolver.ApplyImpulses]. // -// It calls [AxisDisplacementSolver.ApplyImpulses] on each of the three -// per-axis sub-solvers, in X, Y, Z order, so that their combined -// impulses drive the anchor points' relative velocity toward zero along -// all three of the primary body's local axes. +// It resolves impulses, without restitution, that drive the anchor +// points' relative velocity toward closing any remaining separation +// between them, based on the state [BallJointSolver.Reset] computed. func (s *BallJointSolver) ApplyImpulses(ctx PairConstraintContext) { s.solverX.ApplyImpulses(ctx) s.solverY.ApplyImpulses(ctx) @@ -145,11 +137,13 @@ func (s *BallJointSolver) ApplyImpulses(ctx PairConstraintContext) { // ApplyNudges implements [PairConstraintSolver.ApplyNudges]. // -// It calls [AxisDisplacementSolver.ApplyNudges] on each of the three -// per-axis sub-solvers, in X, Y, Z order. Each sub-solver recomputes its -// own Jacobians and drift before nudging, as required by -// [AxisDisplacementSolver.ApplyNudges], so a nudge applied by an earlier -// sub-solver in this call is correctly observed by the ones that follow. +// It first recomputes the constraint's internal state, the same way +// [BallJointSolver.Reset] does, since a preceding nudge - by this +// solver's own previous iteration, or by another constraint acting on +// either target - may have moved either target since +// [BallJointSolver.Reset] or the last call to this method. It then +// nudges both targets' positions and rotations to reduce any remaining +// separation between the anchor points. func (s *BallJointSolver) ApplyNudges(ctx PairConstraintContext) { s.solverX.ApplyNudges(ctx) s.solverY.ApplyNudges(ctx) diff --git a/game/physics/solver_match_rotations.go b/game/physics/solver_match_rotations.go new file mode 100644 index 00000000..ba41c4cb --- /dev/null +++ b/game/physics/solver_match_rotations.go @@ -0,0 +1,95 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +// MatchRotationsSolver is a [PairConstraintSolver] that rotates its two +// target bodies so that their local X, Y and Z axes each become +// pairwise parallel to one another - driving the two targets toward a +// shared orientation. +// +// Each axis is matched as a line, without regard for which of its two +// directions it points, so the constraint's stable solutions include +// not just an exact orientation match, but also any relative +// orientation reachable from it by a 180-degree rotation about a shared +// axis - four equilibria in total. In practice, provided the two +// targets start out reasonably close to already being aligned, the +// constraint converges to the exact match rather than to one of the +// 180-degree-off alternatives. +// +// Unlike [CopyRotationSolver], which unconditionally overwrites the +// primary target's rotation and angular velocity with the secondary +// target's on every step, MatchRotationsSolver is a genuine two-way +// dynamic constraint: it only applies corrective impulses and nudges, +// scaled by each target's mass and inertia, so external torques can +// still influence the primary target's motion, and either target can +// push back against the other. +type MatchRotationsSolver struct { + axisXSolver *MatchAxesSolver + axisYSolver *MatchAxesSolver + axisZSolver *MatchAxesSolver +} + +var _ PairConstraintSolver = (*MatchRotationsSolver)(nil) + +// NewMatchRotationsSolver creates a new [MatchRotationsSolver], ready to +// match the primary and secondary targets' orientations as soon as it +// is registered with a [Scene] through [PairConstraintView.Create]. +// +// MatchRotationsSolver holds no configurable state of its own, so +// unlike most other solvers in this package, it has no Config type or +// Configure method; NewMatchRotationsSolver is the only way to obtain +// one. +func NewMatchRotationsSolver() *MatchRotationsSolver { + return &MatchRotationsSolver{ + axisXSolver: NewMatchAxesSolver(MatchAxesSolverConfig{ + PrimaryBodyAxis: dprec.BasisXVec3(), + SecondaryBodyAxis: dprec.BasisXVec3(), + }), + axisYSolver: NewMatchAxesSolver(MatchAxesSolverConfig{ + PrimaryBodyAxis: dprec.BasisYVec3(), + SecondaryBodyAxis: dprec.BasisYVec3(), + }), + axisZSolver: NewMatchAxesSolver(MatchAxesSolverConfig{ + PrimaryBodyAxis: dprec.BasisZVec3(), + SecondaryBodyAxis: dprec.BasisZVec3(), + }), + } +} + +// Reset implements [PairConstraintSolver.Reset]. +// +// It recomputes the constraint's internal state - the Jacobians and +// drift needed to correct any remaining misalignment between the +// targets' axes - from the targets' current rotations. +func (s *MatchRotationsSolver) Reset(ctx PairConstraintContext) { + s.axisXSolver.Reset(ctx) + s.axisYSolver.Reset(ctx) + s.axisZSolver.Reset(ctx) +} + +// ApplyImpulses implements [PairConstraintSolver.ApplyImpulses]. +// +// It resolves impulses, without restitution, that drive the two +// targets' relative angular velocity toward bringing their axes back +// into alignment, based on the state [MatchRotationsSolver.Reset] +// computed. +func (s *MatchRotationsSolver) ApplyImpulses(ctx PairConstraintContext) { + s.axisXSolver.ApplyImpulses(ctx) + s.axisYSolver.ApplyImpulses(ctx) + s.axisZSolver.ApplyImpulses(ctx) +} + +// ApplyNudges implements [PairConstraintSolver.ApplyNudges]. +// +// It first recomputes the constraint's internal state, the same way +// [MatchRotationsSolver.Reset] does, since a preceding nudge - by this +// solver's own previous iteration, or by another constraint acting on +// either target - may have rotated either target since +// [MatchRotationsSolver.Reset] or the last call to this method. It then +// nudges both targets' rotations to reduce any remaining misalignment +// between their axes. +func (s *MatchRotationsSolver) ApplyNudges(ctx PairConstraintContext) { + s.axisXSolver.ApplyNudges(ctx) + s.axisYSolver.ApplyNudges(ctx) + s.axisZSolver.ApplyNudges(ctx) +} From a6f74ba50bab06e42aa1a853032caaeb4c8f9190 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Wed, 12 Aug 2026 01:06:11 +0300 Subject: [PATCH 78/85] Add copy-axis constraint solver --- game/physics/constraint.go | 2 +- game/physics/constraint/copy_direction.go | 79 --------- game/physics/solver_copy_axis.go | 188 ++++++++++++++++++++++ 3 files changed, 189 insertions(+), 80 deletions(-) delete mode 100644 game/physics/constraint/copy_direction.go create mode 100644 game/physics/solver_copy_axis.go diff --git a/game/physics/constraint.go b/game/physics/constraint.go index 69dcae08..aef5b6f1 100644 --- a/game/physics/constraint.go +++ b/game/physics/constraint.go @@ -126,7 +126,7 @@ func (t ConstraintTarget) Rotation() dprec.Quat { // SetRotation changes the target's rotation. func (t ConstraintTarget) SetRotation(rotation dprec.Quat) { - t.body.rotation = rotation + t.body.rotation = dprec.UnitQuat(rotation) } // Rotate applies rotation on top of the target's current rotation. diff --git a/game/physics/constraint/copy_direction.go b/game/physics/constraint/copy_direction.go deleted file mode 100644 index 14ba7322..00000000 --- a/game/physics/constraint/copy_direction.go +++ /dev/null @@ -1,79 +0,0 @@ -package constraint - -// import ( -// "math" - -// "github.com/mokiat/gomath/dprec" -// "github.com/mokiat/lacking/game/physics/solver" -// ) - -// // NewCopyDirection creates a new CopyDirection constraint solver. -// func NewCopyDirection() *CopyDirection { -// return &CopyDirection{ -// primaryDirection: dprec.BasisYVec3(), -// secondaryDirection: dprec.BasisYVec3(), -// } -// } - -// var _ solver.PairConstraint = (*CopyDirection)(nil) - -// // CopyDirection ensures that the second body has the same direction as -// // the first one. -// // This solver is immediate - it does not use impulses or nudges. -// type CopyDirection struct { -// primaryDirection dprec.Vec3 -// secondaryDirection dprec.Vec3 -// } - -// // PrimaryDirection returns the direction of the primary body. -// func (s *CopyDirection) PrimaryDirection() dprec.Vec3 { -// return s.primaryDirection -// } - -// // SetPrimaryDirection changes the direction of the primary body. -// func (s *CopyDirection) SetPrimaryDirection(direction dprec.Vec3) *CopyDirection { -// s.primaryDirection = dprec.UnitVec3(direction) -// return s -// } - -// // SecondaryDirection returns the direction of the secondary body. -// func (s *CopyDirection) SecondaryDirection() dprec.Vec3 { -// return s.secondaryDirection -// } - -// // SetSecondaryDirection changes the direction of the secondary body. -// func (s *CopyDirection) SetSecondaryDirection(direction dprec.Vec3) *CopyDirection { -// s.secondaryDirection = dprec.UnitVec3(direction) -// return s -// } - -// func (s *CopyDirection) Reset(ctx solver.PairContext) {} - -// func (s *CopyDirection) ApplyImpulses(ctx solver.PairContext) { -// // The secondary body will have its direction aligned with the primary body's -// // direction. As such, we need to ensure that the secondary's body angular -// // velocity is only aligned with the primary body's direction (i.e. there is -// // no rotation component that tries to move it away). - -// primaryDirWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.primaryDirection) -// angularVelocityAmount := dprec.Vec3Dot(primaryDirWS, ctx.Target.AngularVelocity()) -// ctx.Target.SetAngularVelocity(dprec.Vec3Prod(primaryDirWS, angularVelocityAmount)) -// } - -// func (s *CopyDirection) ApplyNudges(ctx solver.PairContext) { -// primaryDirWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), s.primaryDirection) -// secondaryDirWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), s.secondaryDirection) - -// rotationAxis := dprec.Vec3Cross(secondaryDirWS, primaryDirWS) -// cos := dprec.Vec3Dot(secondaryDirWS, primaryDirWS) -// sin := rotationAxis.Length() - -// angle := dprec.Abs(dprec.Radians(math.Atan2(sin, cos))) -// if angle > dprec.Angle(solver.Epsilon) { -// rotation := dprec.RotationQuat(angle, dprec.UnitVec3(rotationAxis)) -// ctx.Target.SetRotation(dprec.UnitQuat(dprec.QuatProd( -// rotation, -// ctx.Target.Rotation(), -// ))) -// } -// } diff --git a/game/physics/solver_copy_axis.go b/game/physics/solver_copy_axis.go new file mode 100644 index 00000000..45db364a --- /dev/null +++ b/game/physics/solver_copy_axis.go @@ -0,0 +1,188 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +// CopyAxisSolverConfig holds the parameters with which a +// [CopyAxisSolver] is configured, either through [NewCopyAxisSolver] or +// [CopyAxisSolver.Configure]. +type CopyAxisSolverConfig struct { + + // PrimaryBodyAxis is the body-local-space direction, relative to the + // primary target's rotation, that is driven to point in the same + // direction as SecondaryBodyAxis. It need not be unit-length; it is + // normalized when the solver is configured, so it must not be the + // zero vector. + PrimaryBodyAxis dprec.Vec3 + + // SecondaryBodyAxis is the body-local-space direction, relative to + // the secondary target's rotation, that PrimaryBodyAxis is driven to + // point in the same direction as. It need not be unit-length; it is + // normalized when the solver is configured, so it must not be the + // zero vector. + SecondaryBodyAxis dprec.Vec3 +} + +// CopyAxisSolver is a [PairConstraintSolver] that makes a body-fixed +// axis on the primary target point in the same direction as a +// body-fixed axis on the secondary target, regardless of any forces or +// torques acting on the primary target. +// +// It is a kinematic constraint, in the same vein as +// [CopyRotationSolver], except that it copies the direction of a single +// axis instead of the secondary target's entire rotation. The primary +// target is left free to spin about that shared axis, keeping whatever +// spin it already had about it; only the 2 rotational degrees of +// freedom that would otherwise let the two axes drift apart are taken +// away. The primary target's position and linear velocity are left +// untouched, and the secondary target is never modified at all - see +// [CopyPositionSolver] for the positional counterpart. +// +// Unlike [MatchAxesSolver], which aligns the two axes as lines and +// rotates both targets in proportion to their inertia, CopyAxisSolver +// aligns them as directions - the primary axis ends up pointing the +// same way as the secondary axis, not merely along the same line - and +// achieves that by driving the primary target alone. Consequently, two +// exactly antiparallel axes are not an equilibrium for this solver, but +// they are an ambiguous configuration: the primary target is turned +// halfway around an arbitrary axis perpendicular to it, so callers that +// can reach that state should not rely on which way the primary target +// swings through it. +// +// A CopyAxisSolver must be configured, either through +// [NewCopyAxisSolver] or [CopyAxisSolver.Configure], before being +// registered with a [Scene] through [PairConstraintView.Create]. +type CopyAxisSolver struct { + primaryBodyAxis dprec.Vec3 + secondaryBodyAxis dprec.Vec3 +} + +var _ PairConstraintSolver = (*CopyAxisSolver)(nil) + +// NewCopyAxisSolver creates a new [CopyAxisSolver] configured according +// to config. +func NewCopyAxisSolver(config CopyAxisSolverConfig) *CopyAxisSolver { + result := &CopyAxisSolver{} + result.Configure(config) + return result +} + +// Configure configures this solver according to config. PrimaryBodyAxis +// and SecondaryBodyAxis are each normalized to unit length. +// +// Configure must be called before this solver is registered with a +// [Scene] through [PairConstraintView.Create]. Unlike +// [NewCopyAxisSolver], it can be called on an already-allocated solver, +// which allows solvers to be cached (e.g. in a slice) and configured on +// demand. +func (s *CopyAxisSolver) Configure(config CopyAxisSolverConfig) { + s.primaryBodyAxis = dprec.UnitVec3(config.PrimaryBodyAxis) + s.secondaryBodyAxis = dprec.UnitVec3(config.SecondaryBodyAxis) +} + +// PrimaryBodyAxis returns the body-local-space, unit-length direction, +// relative to the primary target's rotation, that is driven to point in +// the same direction as [CopyAxisSolver.SecondaryBodyAxis]. +func (s *CopyAxisSolver) PrimaryBodyAxis() dprec.Vec3 { + return s.primaryBodyAxis +} + +// SetPrimaryBodyAxis changes the body-local-space direction, relative to +// the primary target's rotation, that is driven to point in the same +// direction as [CopyAxisSolver.SecondaryBodyAxis]. The provided axis +// need not be unit-length; it is normalized before being stored. +// +// It returns the solver itself, so that calls can be chained. +func (s *CopyAxisSolver) SetPrimaryBodyAxis(axis dprec.Vec3) *CopyAxisSolver { + s.primaryBodyAxis = dprec.UnitVec3(axis) + return s +} + +// SecondaryBodyAxis returns the body-local-space, unit-length direction, +// relative to the secondary target's rotation, that +// [CopyAxisSolver.PrimaryBodyAxis] is driven to point in the same +// direction as. +func (s *CopyAxisSolver) SecondaryBodyAxis() dprec.Vec3 { + return s.secondaryBodyAxis +} + +// SetSecondaryBodyAxis changes the body-local-space direction, relative +// to the secondary target's rotation, that +// [CopyAxisSolver.PrimaryBodyAxis] is driven to point in the same +// direction as. The provided axis need not be unit-length; it is +// normalized before being stored. +// +// It returns the solver itself, so that calls can be chained. +func (s *CopyAxisSolver) SetSecondaryBodyAxis(axis dprec.Vec3) *CopyAxisSolver { + s.secondaryBodyAxis = dprec.UnitVec3(axis) + return s +} + +// Reset implements [PairConstraintSolver.Reset]. +// +// It is a no-op, since this solver holds no per-step state that needs to +// be derived from the targets' current rotations or velocities. +func (s *CopyAxisSolver) Reset(ctx PairConstraintContext) {} + +// ApplyImpulses implements [PairConstraintSolver.ApplyImpulses]. +// +// It unconditionally overwrites the primary target's angular velocity +// with the one that keeps the two axes from drifting apart: the +// secondary target's angular velocity about all directions +// perpendicular to the axis, combined with the primary target's own, +// preserved rotational speed about the axis itself. The secondary +// target's rotational speed about the axis is deliberately not copied, +// since spinning about an axis does not move that axis. +func (s *CopyAxisSolver) ApplyImpulses(ctx PairConstraintContext) { + // The primary body has its axis aligned with the secondary body's axis. + // The axis is dragged along by whatever angular velocity the two bodies + // do not have in common, so to keep the two aligned it is the relative + // angular velocity - not the primary body's own - that has to be + // collinear with the axis (i.e. there is no rotational component that + // tries to move the axes apart). + // + // That means the primary body has to take over the secondary body's + // angular velocity perpendicular to the axis, while keeping its own + // rotational speed about the axis, since spinning about an axis does + // not move that axis. + // + // Note that dprec.Vec3Projection flattens the vector onto the plane + // described by the normal, so it yields the perpendicular component. + + secondaryAxisWS := dprec.QuatVec3Rotation(ctx.SecondaryTarget.Rotation(), s.secondaryBodyAxis) + secondaryOrthogonalAngularVelocity := dprec.Vec3Projection(ctx.SecondaryTarget.AngularVelocity(), secondaryAxisWS) + + result := dprec.Vec3Prod(secondaryAxisWS, dprec.Vec3Dot(ctx.PrimaryTarget.AngularVelocity(), secondaryAxisWS)) + result = dprec.Vec3Sum(result, secondaryOrthogonalAngularVelocity) + ctx.PrimaryTarget.SetAngularVelocity(result) +} + +// ApplyNudges implements [PairConstraintSolver.ApplyNudges]. +// +// It unconditionally rotates the primary target by the smallest +// rotation that brings its axis onto the secondary target's, which +// leaves the primary target's spin about the resulting shared axis +// unchanged. Since the correction is applied in full, rather than being +// scaled by [PairConstraintContext.NudgeBeta], a single call is enough +// to align the two axes exactly; any subsequent call within the same +// step merely counteracts whatever misalignment other constraints have +// introduced in the meantime. +// +// If the two axes happen to be exactly antiparallel, the smallest +// rotation is not unique - the correction is then a half turn about an +// arbitrary axis perpendicular to the primary axis. +func (s *CopyAxisSolver) ApplyNudges(ctx PairConstraintContext) { + primaryAxisWS := dprec.QuatVec3Rotation(ctx.PrimaryTarget.Rotation(), s.primaryBodyAxis) + secondaryAxisWS := dprec.QuatVec3Rotation(ctx.SecondaryTarget.Rotation(), s.secondaryBodyAxis) + + rotationAxis := dprec.Vec3Cross(primaryAxisWS, secondaryAxisWS) + if rotationAxis.SqrLength() < Epsilon*Epsilon { + rotationAxis = dprec.NormalVec3(primaryAxisWS) + } + rotationAngle := dprec.Vec3Angle(primaryAxisWS, secondaryAxisWS) + + rotation := dprec.RotationQuat(rotationAngle, rotationAxis) + ctx.PrimaryTarget.SetRotation(dprec.QuatProd( + rotation, + ctx.PrimaryTarget.Rotation(), + )) +} From c534baa33e4cd03e3a42b21df9762a5293cdffdf Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Wed, 12 Aug 2026 21:21:23 +0300 Subject: [PATCH 79/85] Add axis-angle-range constraint solver --- .../constraint/limit_relative_angle.go | 119 ------ game/physics/solver_axis_angle_range.go | 349 ++++++++++++++++++ 2 files changed, 349 insertions(+), 119 deletions(-) delete mode 100644 game/physics/constraint/limit_relative_angle.go create mode 100644 game/physics/solver_axis_angle_range.go diff --git a/game/physics/constraint/limit_relative_angle.go b/game/physics/constraint/limit_relative_angle.go deleted file mode 100644 index c83a51d2..00000000 --- a/game/physics/constraint/limit_relative_angle.go +++ /dev/null @@ -1,119 +0,0 @@ -package constraint - -// import ( -// "github.com/mokiat/gomath/dprec" -// "github.com/mokiat/lacking/game/physics/solver" -// ) - -// func NewLimitRelativeAngle() *LimitRelativeAngle { -// return &LimitRelativeAngle{} -// } - -// var _ solver.PairConstraint = (*LimitRelativeAngle)(nil) - -// type LimitRelativeAngle struct { -// primaryDirection dprec.Vec3 -// secondaryDirection dprec.Vec3 -// axis dprec.Vec3 -// minAngle dprec.Angle -// maxAngle dprec.Angle - -// jacobian solver.PairJacobian -// drift float64 -// } - -// func (c *LimitRelativeAngle) PrimaryDirection() dprec.Vec3 { -// return c.primaryDirection -// } - -// func (c *LimitRelativeAngle) SetPrimaryDirection(direction dprec.Vec3) *LimitRelativeAngle { -// c.primaryDirection = dprec.UnitVec3(direction) -// return c -// } - -// func (c *LimitRelativeAngle) SecondaryDirection() dprec.Vec3 { -// return c.secondaryDirection -// } - -// func (c *LimitRelativeAngle) SetSecondaryDirection(direction dprec.Vec3) *LimitRelativeAngle { -// c.secondaryDirection = dprec.UnitVec3(direction) -// return c -// } - -// func (c *LimitRelativeAngle) Axis() dprec.Vec3 { -// return c.axis -// } - -// func (c *LimitRelativeAngle) SetAxis(axis dprec.Vec3) *LimitRelativeAngle { -// c.axis = dprec.UnitVec3(axis) -// return c -// } - -// func (c *LimitRelativeAngle) MinAngle() dprec.Angle { -// return c.minAngle -// } - -// func (c *LimitRelativeAngle) SetMinAngle(angle dprec.Angle) *LimitRelativeAngle { -// c.minAngle = angle -// return c -// } - -// func (c *LimitRelativeAngle) MaxAngle() dprec.Angle { -// return c.maxAngle -// } - -// func (c *LimitRelativeAngle) SetMaxAngle(angle dprec.Angle) *LimitRelativeAngle { -// c.maxAngle = angle -// return c -// } - -// func (c *LimitRelativeAngle) Reset(ctx solver.PairContext) { -// axisWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), c.axis) -// primaryDirectionWS := dprec.QuatVec3Rotation(ctx.Target.Rotation(), c.primaryDirection) -// secondaryDirectionWS := dprec.QuatVec3Rotation(ctx.Source.Rotation(), c.secondaryDirection) - -// if dprec.Abs(dprec.Vec3Dot(axisWS, secondaryDirectionWS)) > 0.99 { -// c.jacobian = solver.PairJacobian{} -// c.drift = 0.0 -// return // secondary direction is parallel to axis -// } -// angle := dprec.Vec3ProjectionAngle(primaryDirectionWS, secondaryDirectionWS, axisWS) - -// switch { -// case angle > c.maxAngle: -// c.drift = (angle - c.maxAngle).Radians() -// c.jacobian = solver.PairJacobian{ -// Target: solver.Jacobian{ -// AngularSlope: dprec.InverseVec3(axisWS), -// }, -// Source: solver.Jacobian{ -// AngularSlope: axisWS, -// }, -// } -// case angle < c.minAngle: -// c.drift = (c.minAngle - angle).Radians() -// c.jacobian = solver.PairJacobian{ -// Target: solver.Jacobian{ -// AngularSlope: axisWS, -// }, -// Source: solver.Jacobian{ -// AngularSlope: dprec.InverseVec3(axisWS), -// }, -// } -// default: -// c.drift = 0.0 -// c.jacobian = solver.PairJacobian{} -// } -// } - -// func (c *LimitRelativeAngle) ApplyImpulses(ctx solver.PairContext) { -// if lambda := ctx.JacobianImpulseLambda(c.jacobian, 0.0, 0.0); lambda >= 0.0 { -// return // moving away -// } -// solution := ctx.JacobianImpulseSolution(c.jacobian, c.drift, 0.0) -// ctx.Target.ApplyImpulse(solution.Target) -// ctx.Source.ApplyImpulse(solution.Source) -// } - -// func (c *LimitRelativeAngle) ApplyNudges(ctx solver.PairContext) { -// } diff --git a/game/physics/solver_axis_angle_range.go b/game/physics/solver_axis_angle_range.go new file mode 100644 index 00000000..3ea058b8 --- /dev/null +++ b/game/physics/solver_axis_angle_range.go @@ -0,0 +1,349 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +// AxisAngleRangeSolverConfig holds the parameters with which an +// [AxisAngleRangeSolver] is configured, either through +// [NewAxisAngleRangeSolver] or [AxisAngleRangeSolver.Configure]. +type AxisAngleRangeSolverConfig struct { + + // PrimaryBodyAxis is the body-local-space direction, relative to the + // primary target's rotation, from which the angle is measured. It need + // not be unit-length; it is normalized when the solver is configured, + // so it must not be the zero vector, nor may it be collinear with + // RotationAxis. + PrimaryBodyAxis dprec.Vec3 + + // SecondaryBodyAxis is the body-local-space direction, relative to the + // secondary target's rotation, to which the angle is measured. It need + // not be unit-length; it is normalized when the solver is configured, + // so it must not be the zero vector. + SecondaryBodyAxis dprec.Vec3 + + // RotationAxis is the body-local-space direction, relative to the + // primary target's rotation, about which the angle between + // PrimaryBodyAxis and SecondaryBodyAxis is measured. It need not be + // unit-length; it is normalized when the solver is configured, so it + // must not be the zero vector, nor may it be collinear with + // PrimaryBodyAxis. + RotationAxis dprec.Vec3 + + // MinAngle is the lowest permitted signed angle, measured about + // RotationAxis, from PrimaryBodyAxis to SecondaryBodyAxis. It must lie + // within the (-180, 180] degrees range. + MinAngle dprec.Angle + + // MaxAngle is the highest permitted signed angle, measured about + // RotationAxis, from PrimaryBodyAxis to SecondaryBodyAxis. It must lie + // within the (-180, 180] degrees range. + MaxAngle dprec.Angle + + // RestitutionCoefficient is the bounciness applied when the angle + // reaches MinAngle or MaxAngle. Negative values are clamped to zero. + RestitutionCoefficient float64 +} + +// AxisAngleRangeSolver is a [PairConstraintSolver] that keeps the signed +// angle from an axis fixed to the primary body to an axis fixed to the +// secondary body, measured about a rotation axis fixed to the primary +// body, within a [AxisAngleRangeSolver.MinAngle] and +// [AxisAngleRangeSolver.MaxAngle] range - acting like a rigid stop only +// once one of the range's limits is reached, and applying no torque while +// the angle is within range. +// +// It is the angular counterpart to [AxisRangeSolver], and is what turns a +// hinge - a [BallJointSolver] combined with a [MatchAxesSolver], or +// similar - into a hinge with end stops. Only rotation about the rotation +// axis is restricted; the targets' positions and linear velocities are +// left untouched, as are the two rotational degrees of freedom that tilt +// the rotation axis itself. +// +// The angle is measured after both axes are projected onto the plane +// perpendicular to the rotation axis, and is therefore only defined +// within the (-180, 180] degrees range. MinAngle and MaxAngle must lie +// within that range to be reachable, and a target that swings past the +// 180 degrees discontinuity will be driven toward the opposite limit, +// since the measured angle wraps around to the other end of the range. +// +// Whenever the secondary axis becomes collinear with the rotation axis, +// its projection vanishes and the angle is undefined; the solver then +// applies nothing at all until the two separate again. +// +// An AxisAngleRangeSolver must be configured, either through +// [NewAxisAngleRangeSolver] or [AxisAngleRangeSolver.Configure], before +// being registered with a [Scene] through [PairConstraintView.Create]. +type AxisAngleRangeSolver struct { + primaryBodyAxis dprec.Vec3 + secondaryBodyAxis dprec.Vec3 + rotationAxis dprec.Vec3 + minAngle dprec.Angle + maxAngle dprec.Angle + restitutionCoefficient float64 + + primaryJacobian Jacobian + secondaryJacobian Jacobian + drift float64 +} + +var _ PairConstraintSolver = (*AxisAngleRangeSolver)(nil) + +// NewAxisAngleRangeSolver creates a new [AxisAngleRangeSolver] configured +// according to config. +func NewAxisAngleRangeSolver(config AxisAngleRangeSolverConfig) *AxisAngleRangeSolver { + result := &AxisAngleRangeSolver{} + result.Configure(config) + return result +} + +// Configure configures this solver according to config. PrimaryBodyAxis, +// SecondaryBodyAxis and RotationAxis are each normalized to unit length, +// and negative RestitutionCoefficient values are clamped to zero, as with +// [AxisAngleRangeSolver.SetRestitutionCoefficient]. +// +// Configure must be called before this solver is registered with a +// [Scene] through [PairConstraintView.Create]. Unlike +// [NewAxisAngleRangeSolver], it can be called on an already-allocated +// solver, which allows solvers to be cached (e.g. in a slice) and +// configured on demand. +func (s *AxisAngleRangeSolver) Configure(config AxisAngleRangeSolverConfig) { + s.primaryBodyAxis = dprec.UnitVec3(config.PrimaryBodyAxis) + s.secondaryBodyAxis = dprec.UnitVec3(config.SecondaryBodyAxis) + s.rotationAxis = dprec.UnitVec3(config.RotationAxis) + s.minAngle = config.MinAngle + s.maxAngle = config.MaxAngle + s.restitutionCoefficient = max(0.0, config.RestitutionCoefficient) +} + +// PrimaryBodyAxis returns the body-local-space, unit-length direction, +// relative to the primary target's rotation, from which the angle is +// measured. +func (s *AxisAngleRangeSolver) PrimaryBodyAxis() dprec.Vec3 { + return s.primaryBodyAxis +} + +// SetPrimaryBodyAxis changes the body-local-space direction, relative to +// the primary target's rotation, from which the angle is measured. The +// provided axis need not be unit-length; it is normalized before being +// stored. It must not be collinear with +// [AxisAngleRangeSolver.RotationAxis]. +// +// It returns the solver itself, so that calls can be chained. +func (s *AxisAngleRangeSolver) SetPrimaryBodyAxis(axis dprec.Vec3) *AxisAngleRangeSolver { + s.primaryBodyAxis = dprec.UnitVec3(axis) + return s +} + +// SecondaryBodyAxis returns the body-local-space, unit-length direction, +// relative to the secondary target's rotation, to which the angle is +// measured. +func (s *AxisAngleRangeSolver) SecondaryBodyAxis() dprec.Vec3 { + return s.secondaryBodyAxis +} + +// SetSecondaryBodyAxis changes the body-local-space direction, relative +// to the secondary target's rotation, to which the angle is measured. The +// provided axis need not be unit-length; it is normalized before being +// stored. +// +// It returns the solver itself, so that calls can be chained. +func (s *AxisAngleRangeSolver) SetSecondaryBodyAxis(axis dprec.Vec3) *AxisAngleRangeSolver { + s.secondaryBodyAxis = dprec.UnitVec3(axis) + return s +} + +// RotationAxis returns the body-local-space, unit-length direction, +// relative to the primary target's rotation, about which the angle +// between [AxisAngleRangeSolver.PrimaryBodyAxis] and +// [AxisAngleRangeSolver.SecondaryBodyAxis] is measured. +func (s *AxisAngleRangeSolver) RotationAxis() dprec.Vec3 { + return s.rotationAxis +} + +// SetRotationAxis changes the body-local-space direction, relative to the +// primary target's rotation, about which the angle between +// [AxisAngleRangeSolver.PrimaryBodyAxis] and +// [AxisAngleRangeSolver.SecondaryBodyAxis] is measured. The provided axis +// need not be unit-length; it is normalized before being stored. It must +// not be collinear with [AxisAngleRangeSolver.PrimaryBodyAxis]. +// +// It returns the solver itself, so that calls can be chained. +func (s *AxisAngleRangeSolver) SetRotationAxis(axis dprec.Vec3) *AxisAngleRangeSolver { + s.rotationAxis = dprec.UnitVec3(axis) + return s +} + +// MinAngle returns the lowest permitted signed angle, measured about +// [AxisAngleRangeSolver.RotationAxis], from +// [AxisAngleRangeSolver.PrimaryBodyAxis] to +// [AxisAngleRangeSolver.SecondaryBodyAxis]. +func (s *AxisAngleRangeSolver) MinAngle() dprec.Angle { + return s.minAngle +} + +// SetMinAngle changes the lowest permitted signed angle, measured about +// [AxisAngleRangeSolver.RotationAxis], from +// [AxisAngleRangeSolver.PrimaryBodyAxis] to +// [AxisAngleRangeSolver.SecondaryBodyAxis]. It must lie within the +// (-180, 180] degrees range to be reachable. +// +// It returns the solver itself, so that calls can be chained. +func (s *AxisAngleRangeSolver) SetMinAngle(angle dprec.Angle) *AxisAngleRangeSolver { + s.minAngle = angle + return s +} + +// MaxAngle returns the highest permitted signed angle, measured about +// [AxisAngleRangeSolver.RotationAxis], from +// [AxisAngleRangeSolver.PrimaryBodyAxis] to +// [AxisAngleRangeSolver.SecondaryBodyAxis]. +func (s *AxisAngleRangeSolver) MaxAngle() dprec.Angle { + return s.maxAngle +} + +// SetMaxAngle changes the highest permitted signed angle, measured about +// [AxisAngleRangeSolver.RotationAxis], from +// [AxisAngleRangeSolver.PrimaryBodyAxis] to +// [AxisAngleRangeSolver.SecondaryBodyAxis]. It must lie within the +// (-180, 180] degrees range to be reachable. +// +// It returns the solver itself, so that calls can be chained. +func (s *AxisAngleRangeSolver) SetMaxAngle(angle dprec.Angle) *AxisAngleRangeSolver { + s.maxAngle = angle + return s +} + +// RestitutionCoefficient returns the bounciness applied when the angle +// reaches [AxisAngleRangeSolver.MinAngle] or +// [AxisAngleRangeSolver.MaxAngle]. +func (s *AxisAngleRangeSolver) RestitutionCoefficient() float64 { + return s.restitutionCoefficient +} + +// SetRestitutionCoefficient changes the bounciness applied when the angle +// reaches [AxisAngleRangeSolver.MinAngle] or +// [AxisAngleRangeSolver.MaxAngle]. Negative values are clamped to zero. +// +// It returns the solver itself, so that calls can be chained. +func (s *AxisAngleRangeSolver) SetRestitutionCoefficient(coefficient float64) *AxisAngleRangeSolver { + s.restitutionCoefficient = max(0.0, coefficient) + return s +} + +// Reset implements [PairConstraintSolver.Reset]. +// +// It recomputes the constraint's primary and secondary [Jacobian]s and +// current range violation (drift), the same way +// [AxisAngleRangeSolver.recompute] does. +func (s *AxisAngleRangeSolver) Reset(ctx PairConstraintContext) { + s.recompute(ctx) +} + +// ApplyImpulses implements [PairConstraintSolver.ApplyImpulses]. +// +// If the angle between the two body axes, as of the last call to +// [AxisAngleRangeSolver.Reset] or [AxisAngleRangeSolver.ApplyNudges], is +// within the +// [AxisAngleRangeSolver.MinAngle]/[AxisAngleRangeSolver.MaxAngle] range, +// it does nothing. Otherwise, it resolves a pair of impulses, combining +// restitution with Baumgarte positional-drift stabilization, that drive +// the targets' relative angular velocity toward bringing the angle back +// within range. If the two targets are already rotating apart (back +// towards the permitted range), it returns without applying anything, +// leaving any remaining violation to [AxisAngleRangeSolver.ApplyNudges]. +func (s *AxisAngleRangeSolver) ApplyImpulses(ctx PairConstraintContext) { + if s.drift == 0.0 { + return // no constraint violation + } + + bounceLambda, baumgarteLambda := ctx.ImpulseLambdaComponents(s.primaryJacobian, s.secondaryJacobian, s.drift, s.restitutionCoefficient) + if bounceLambda < 0.0 { + return // moving away + } + + lambda := bounceLambda + baumgarteLambda + primaryImpulse := s.primaryJacobian.Impulse(lambda) + secondaryImpulse := s.secondaryJacobian.Impulse(lambda) + + ctx.PrimaryTarget.ApplyImpulse(primaryImpulse) + ctx.SecondaryTarget.ApplyImpulse(secondaryImpulse) +} + +// ApplyNudges implements [PairConstraintSolver.ApplyNudges]. +// +// It first recomputes the constraint's primary and secondary [Jacobian]s +// and current range violation (drift), the same way +// [AxisAngleRangeSolver.recompute] does, since a preceding nudge - by +// this solver's own previous iteration, or by another constraint acting +// on either target - may have rotated either target since +// [AxisAngleRangeSolver.Reset] or the last call to this method. If the +// angle is within range, it does nothing; otherwise, it nudges both +// targets' rotations to bring the angle back within the +// [AxisAngleRangeSolver.MinAngle]/[AxisAngleRangeSolver.MaxAngle] range. +func (s *AxisAngleRangeSolver) ApplyNudges(ctx PairConstraintContext) { + s.recompute(ctx) + + if s.drift > 0.0 { + primaryNudge, secondaryNudge := ctx.NudgeSolution( + s.primaryJacobian, s.secondaryJacobian, s.drift, + ) + ctx.PrimaryTarget.ApplyNudge(primaryNudge) + ctx.SecondaryTarget.ApplyNudge(secondaryNudge) + } +} + +// recompute recalculates the constraint's primary and secondary +// [Jacobian]s, along with the current range violation (drift), based on +// the targets' current rotations. +// +// It measures the signed angle from PrimaryBodyAxis to +// SecondaryBodyAxis, each transformed into world space through its own +// target's current rotation, about RotationAxis, transformed into world +// space through the primary target's current rotation. If that angle is +// below MinAngle, the Jacobians and drift are set up to rotate the two +// axes apart about the rotation axis; if it is above MaxAngle, they are +// set up to rotate them together; otherwise both Jacobians and the drift +// are reset to zero, so that [AxisAngleRangeSolver.ApplyImpulses] and +// [AxisAngleRangeSolver.ApplyNudges] apply no correction. +// +// The Jacobians and drift are similarly reset to zero whenever the +// secondary axis is collinear with the rotation axis, since the secondary +// axis then has no projection onto the plane in which the angle is +// measured, leaving the angle undefined. +func (s *AxisAngleRangeSolver) recompute(ctx PairConstraintContext) { + primaryAxisWS := dprec.QuatVec3Rotation(ctx.PrimaryTarget.Rotation(), s.primaryBodyAxis) + secondaryAxisWS := dprec.QuatVec3Rotation(ctx.SecondaryTarget.Rotation(), s.secondaryBodyAxis) + axisWS := dprec.QuatVec3Rotation(ctx.PrimaryTarget.Rotation(), s.rotationAxis) + + if dprec.Abs(dprec.Vec3Dot(axisWS, secondaryAxisWS)) > 1.0-Epsilon { + s.primaryJacobian = Jacobian{} + s.secondaryJacobian = Jacobian{} + s.drift = 0.0 + return // secondary direction is parallel to axis + } + angle := dprec.Vec3ProjectionAngle(primaryAxisWS, secondaryAxisWS, axisWS) + + switch { + case angle < s.minAngle: + s.primaryJacobian = Jacobian{ + AngularSlope: dprec.InverseVec3(axisWS), + } + s.secondaryJacobian = Jacobian{ + AngularSlope: axisWS, + } + s.drift = (s.minAngle - angle).Radians() + + case angle > s.maxAngle: + s.primaryJacobian = Jacobian{ + AngularSlope: axisWS, + } + s.secondaryJacobian = Jacobian{ + AngularSlope: dprec.InverseVec3(axisWS), + } + s.drift = (angle - s.maxAngle).Radians() + + default: + s.primaryJacobian = Jacobian{} + s.secondaryJacobian = Jacobian{} + s.drift = 0.0 + } +} From ef9b56b0ade3fc41e1cf441bc3245fb01e4e18d6 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Wed, 12 Aug 2026 23:42:49 +0300 Subject: [PATCH 80/85] Add airfoil acceleration solver --- game/physics/scene.go | 36 -------- game/physics/solver_airfoil.go | 159 +++++++++++++++++++++++++++++++++ 2 files changed, 159 insertions(+), 36 deletions(-) create mode 100644 game/physics/solver_airfoil.go diff --git a/game/physics/scene.go b/game/physics/scene.go index abd9a9ed..ea6aa9f7 100644 --- a/game/physics/scene.go +++ b/game/physics/scene.go @@ -1005,39 +1005,3 @@ type pairCollisionRef struct { primaryBodyID BodyID secondaryBodyID BodyID } - -/////// OLD BELOW ------------ (TODO: DELETE COMMENT) - -// func (s *Scene) applyAerodynamicAccelerations() { -// s.eachBody(func(index int, body *bodyState) { -// if len(body.aerodynamicShapes) == 0 { -// return -// } -// target := &s.bodyAccelerationTargets[index] -// mediumDensity := s.mediumSolver.Density(body.position) -// mediumVelocity := s.mediumSolver.Velocity(body.position) - -// deltaVelocity := dprec.Vec3Diff(mediumVelocity, body.velocity) -// dragForce := dprec.Vec3Prod(deltaVelocity, deltaVelocity.Length()*mediumDensity*body.dragFactor) -// target.ApplyForce(dragForce) - -// angularDragForce := dprec.Vec3Prod(body.angularVelocity, -body.angularVelocity.Length()*mediumDensity*body.angularDragFactor) -// target.ApplyTorque(angularDragForce) - -// bodyTransform := NewTransform(body.position, body.rotation) -// for _, aerodynamicShape := range body.aerodynamicShapes { -// // TODO: Take shape velocity into account. This also means that wings should be -// // split into two, to benefit from that. - -// aerodynamicShape = aerodynamicShape.Transformed(bodyTransform) -// relativeSpeed := dprec.QuatVec3Rotation(dprec.InverseQuat(aerodynamicShape.Rotation()), deltaVelocity) - -// force := aerodynamicShape.solver.Force(relativeSpeed, mediumDensity) -// absoluteForce := dprec.QuatVec3Rotation(aerodynamicShape.Rotation(), force) - -// offset := dprec.Vec3Diff(aerodynamicShape.Position(), bodyTransform.Position()) -// target.ApplyOffsetForce(offset, absoluteForce) -// // target.ApplyOffsetForce(absoluteForce, aerodynamicShape.Position()) -// } -// }) -// } diff --git a/game/physics/solver_airfoil.go b/game/physics/solver_airfoil.go new file mode 100644 index 00000000..58bc65ba --- /dev/null +++ b/game/physics/solver_airfoil.go @@ -0,0 +1,159 @@ +package physics + +import ( + "github.com/mokiat/gomath/dprec" +) + +type AirfoilSolverConfig struct { + RelativePosition dprec.Vec3 + RelativeRotation dprec.Quat + SurfaceArea float64 + StallAngle dprec.Angle + LiftCoefficient float64 +} + +type AirfoilSolver struct { + relativePosition dprec.Vec3 + relativeRotation dprec.Quat + surfaceArea float64 + stallAngle dprec.Angle + liftCoefficient float64 +} + +var _ AccelerationSolver = (*AirfoilSolver)(nil) + +func NewAirfoilSolver(config AirfoilSolverConfig) *AirfoilSolver { + result := &AirfoilSolver{} + result.Configure(config) + return result +} + +func (s *AirfoilSolver) Configure(config AirfoilSolverConfig) { + s.relativePosition = config.RelativePosition + s.relativeRotation = config.RelativeRotation + s.surfaceArea = max(0.0, config.SurfaceArea) + s.stallAngle = max(0.0, config.StallAngle) + s.liftCoefficient = max(0.0, config.LiftCoefficient) +} + +func (s *AirfoilSolver) RelativePosition() dprec.Vec3 { + return s.relativePosition +} + +func (s *AirfoilSolver) SetRelativePosition(position dprec.Vec3) *AirfoilSolver { + s.relativePosition = position + return s +} + +func (s *AirfoilSolver) RelativeRotation() dprec.Quat { + return s.relativeRotation +} + +func (s *AirfoilSolver) SetRelativeRotation(rotation dprec.Quat) *AirfoilSolver { + s.relativeRotation = rotation + return s +} + +func (s *AirfoilSolver) SurfaceArea() float64 { + return s.surfaceArea +} + +func (s *AirfoilSolver) SetSurfaceArea(area float64) *AirfoilSolver { + s.surfaceArea = max(0.0, area) + return s +} + +func (s *AirfoilSolver) StallAngle() dprec.Angle { + return s.stallAngle +} + +func (s *AirfoilSolver) SetStallAngle(angle dprec.Angle) *AirfoilSolver { + s.stallAngle = max(0.0, angle) + return s +} + +func (s *AirfoilSolver) LiftCoefficient() float64 { + return s.liftCoefficient +} + +func (s *AirfoilSolver) SetLiftCoefficient(coefficient float64) *AirfoilSolver { + s.liftCoefficient = max(0.0, coefficient) + return s +} + +func (s *AirfoilSolver) ApplyAcceleration(ctx AccelerationContext) { + bodyRotation := ctx.Target.Rotation() + + airfoilVelocity := dprec.Vec3Sum( + ctx.Target.LinearVelocity(), + dprec.Vec3Cross( + ctx.Target.AngularVelocity(), + s.relativePosition, + ), + ) + + relWindVelocity := dprec.Vec3Diff(ctx.MediumVelocity, airfoilVelocity) + relWindVelocityLng := relWindVelocity.Length() + if relWindVelocityLng < Epsilon { + return // no significant wind + } + + airfoilOffsetWS := dprec.QuatVec3Rotation(bodyRotation, s.relativePosition) + airfoilRotationWS := dprec.QuatProd(bodyRotation, s.relativeRotation) + + basisX := airfoilRotationWS.OrientationX() + basisY := airfoilRotationWS.OrientationY() + basisZ := airfoilRotationWS.OrientationZ() + + windX := dprec.Vec3Dot(relWindVelocity, basisX) + windY := dprec.Vec3Dot(relWindVelocity, basisY) + windZ := dprec.Vec3Dot(relWindVelocity, basisZ) + + var ( + directAmount float64 + lateralAmount float64 + ) + planarWindVelocity := dprec.NewVec3(windX, 0.0, windZ) + if planarLng := planarWindVelocity.Length(); planarLng > Epsilon { + directAmount = dprec.Abs(planarWindVelocity.Z / planarLng) + lateralAmount = dprec.Abs(planarWindVelocity.X / planarLng) + } else { + directAmount = 1.0 + lateralAmount = 0.0 + } + + // Wind along the chord line (direct). + if directAmount > Epsilon { + effWindVelocity := relWindVelocityLng * directAmount + angleOfAttack := dprec.Atan2(windY, -windZ) + + coef := s.localLiftCoefficient(angleOfAttack, true) + magnitude := coef * 0.5 * ctx.MediumDensity * dprec.Sqr(effWindVelocity) * s.surfaceArea + + ctx.Target.ApplyOffsetForce(airfoilOffsetWS, dprec.Vec3Prod(basisY, magnitude)) + } + + // Wind along the span (lateral). + if lateralAmount > Epsilon { + effWindVelocity := relWindVelocityLng * lateralAmount + angleOfAttack := dprec.Atan2(windY, dprec.Abs(windX)) // keep symmetric + + coef := s.localLiftCoefficient(angleOfAttack, false) + magnitude := coef * 0.5 * ctx.MediumDensity * dprec.Sqr(effWindVelocity) * s.surfaceArea + + ctx.Target.ApplyOffsetForce(airfoilOffsetWS, dprec.Vec3Prod(basisY, magnitude)) + } +} + +func (s *AirfoilSolver) localLiftCoefficient(angle dprec.Angle, isDirect bool) float64 { + if angle < 0.0 { + return -s.localLiftCoefficient(-angle, isDirect) // flipped symmetric + } + degrees := angle.Degrees() + result := (s.liftCoefficient / 2.0) * dprec.Sin(angle) + if isDirect && (s.stallAngle > 0.0) { // add the lift coefficient bump prior to stall + stallDegrees := s.stallAngle.Degrees() + result += (s.liftCoefficient / 2.0) * max(0.0, degrees*(2.0*stallDegrees-degrees)) / dprec.Sqr(stallDegrees) + } + return result +} From c8aadbfaba9f34b61db08109640ec5806367b815 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Wed, 12 Aug 2026 23:43:36 +0300 Subject: [PATCH 81/85] Remove old airfoil solver --- game/physics/aerodynamics/airfoil.go | 94 ---------------------------- 1 file changed, 94 deletions(-) delete mode 100644 game/physics/aerodynamics/airfoil.go diff --git a/game/physics/aerodynamics/airfoil.go b/game/physics/aerodynamics/airfoil.go deleted file mode 100644 index e7d96f57..00000000 --- a/game/physics/aerodynamics/airfoil.go +++ /dev/null @@ -1,94 +0,0 @@ -package aerodynamics - -import ( - "github.com/mokiat/gomath/dprec" - "github.com/mokiat/lacking/game/physics" -) - -func NewAirfoilSolver(width, length float64) *AirfoilSolver { - return &AirfoilSolver{ - width: width, - length: length, - - stallAngle: dprec.Degrees(20.0), - liftCoefficient: 2.4, - } -} - -var _ physics.AerodynamicSolver = (*AirfoilSolver)(nil) - -type AirfoilSolver struct { - width float64 - length float64 - - stallAngle dprec.Angle - liftCoefficient float64 -} - -func (s *AirfoilSolver) StallAngle() dprec.Angle { - return s.stallAngle -} - -func (s *AirfoilSolver) SetStallAngle(angle dprec.Angle) *AirfoilSolver { - s.stallAngle = angle - return s -} - -func (s *AirfoilSolver) LiftCoefficient() float64 { - return s.liftCoefficient -} - -func (s *AirfoilSolver) SetLiftCoefficient(coefficient float64) *AirfoilSolver { - s.liftCoefficient = coefficient - return s -} - -func (s *AirfoilSolver) Force(windSpeed dprec.Vec3, density float64) dprec.Vec3 { - windSpeedLng := windSpeed.Length() - if windSpeedLng < 0.01 { - return dprec.ZeroVec3() - } - - area := s.width * s.length - windX := dprec.Vec3Dot(windSpeed, dprec.BasisXVec3()) - windY := dprec.Vec3Dot(windSpeed, dprec.BasisYVec3()) - windZ := dprec.Vec3Dot(windSpeed, dprec.BasisZVec3()) - planarWindDir := dprec.UnitVec3(dprec.NewVec3(windX, 0.0, windZ)) - - var result dprec.Vec3 - - // direct (chordwise) - directAmount := dprec.Abs(dprec.Vec3Dot(planarWindDir, dprec.BasisZVec3())) - if directAmount > 0.01 { - effWindVelocity := windSpeedLng * directAmount - angleOfAttack := dprec.Atan2(windY, -windZ) - - coef := s.localLiftCoefficient(angleOfAttack) - force := 0.5 * density * area * dprec.Sqr(effWindVelocity) * coef - result = dprec.Vec3Sum(result, dprec.Vec3Prod(dprec.BasisYVec3(), force)) - } - - // lateral - lateralAmount := dprec.Abs(dprec.Vec3Dot(planarWindDir, dprec.BasisXVec3())) - if lateralAmount > 0.01 { - effWindVelocity := windSpeedLng * lateralAmount - angleOfAttack := dprec.Atan2(windY, dprec.Abs(windX)) // keep symmetric - - coef := s.localLiftCoefficient(angleOfAttack) / 2.0 // reduce lateral coefficient - force := 0.5 * density * area * dprec.Sqr(effWindVelocity) * coef - result = dprec.Vec3Sum(result, dprec.Vec3Prod(dprec.BasisYVec3(), force)) - } - - return result -} - -func (s *AirfoilSolver) localLiftCoefficient(angle dprec.Angle) float64 { - if angle < 0.0 { - return -s.localLiftCoefficient(-angle) // flipped symmetric - } - degrees := angle.Degrees() - stallDegrees := s.stallAngle.Degrees() - base := (s.liftCoefficient / 2.0) * dprec.Sin(angle) - addition := (s.liftCoefficient / 2.0) * max(0.0, degrees*(2.0*stallDegrees-degrees)) / dprec.Sqr(stallDegrees) - return base + addition -} From 665bb3e0445e49f3a72171aad2726ad4a03bb603 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Thu, 13 Aug 2026 00:00:53 +0300 Subject: [PATCH 82/85] Fixes to airfoil solver --- game/physics/solver_airfoil.go | 152 +++++++++++++++++++++++++++++++-- 1 file changed, 145 insertions(+), 7 deletions(-) diff --git a/game/physics/solver_airfoil.go b/game/physics/solver_airfoil.go index 58bc65ba..dadc0513 100644 --- a/game/physics/solver_airfoil.go +++ b/game/physics/solver_airfoil.go @@ -4,14 +4,79 @@ import ( "github.com/mokiat/gomath/dprec" ) +// AirfoilSolverConfig holds the parameters with which an [AirfoilSolver] +// is configured, either through [NewAirfoilSolver] or +// [AirfoilSolver.Configure]. type AirfoilSolverConfig struct { + + // RelativePosition is the body-local-space offset, relative to the + // target's center of mass, at which the airfoil is mounted. The lift + // force is applied at this point, so an offset airfoil induces torque + // on the target in addition to accelerating it. RelativePosition dprec.Vec3 + + // RelativeRotation is the body-local-space orientation of the airfoil, + // relative to the target's rotation. See [AirfoilSolver] for the axis + // convention that this orientation establishes. + // + // It must be a unit quaternion. Note that the zero value of this field + // is the zero quaternion, which is not a valid rotation; use + // [dprec.IdentityQuat] for an airfoil that is aligned with the target + // itself. RelativeRotation dprec.Quat - SurfaceArea float64 - StallAngle dprec.Angle - LiftCoefficient float64 + + // SurfaceArea is the area of the airfoil, in square meters, that the + // lift force is scaled by. Negative values are clamped to zero. + SurfaceArea float64 + + // StallAngle is the angle of attack at which the airfoil produces its + // peak lift and past which it starts to stall. Negative values are + // clamped to zero, which disables the pre-stall lift bump entirely and + // leaves only the flat-plate behavior described on [AirfoilSolver]. + StallAngle dprec.Angle + + // LiftCoefficient is the dimensionless coefficient that scales the + // lift produced by the airfoil. Negative values are clamped to zero. + LiftCoefficient float64 } +// AirfoilSolver is an [AccelerationSolver] that models a lift-producing +// surface - a wing, a tail plane, a rudder - mounted at a fixed position +// and orientation on its target body. +// +// The airfoil has its own coordinate frame, established by +// [AirfoilSolver.RelativeRotation] on top of the target's rotation, in +// which the chord line runs along Z, the span runs along X, and lift acts +// along Y. Forward flight is along positive Z, which places the oncoming +// wind along negative Z and puts the airfoil at a zero angle of attack. +// The profile is symmetric, so a zero angle of attack produces no lift, +// and inverting the airfoil inverts the lift. +// +// The lift force is applied at [AirfoilSolver.RelativePosition] rather +// than at the center of mass, so an airfoil mounted off-center also +// induces torque. The oncoming wind is sampled at that same mounted +// position, and therefore includes the velocity the target's own rotation +// imparts there. That is what makes a pair of offset airfoils damp the +// target's rotation rather than merely turn it. +// +// The force always acts along the airfoil's Y axis, which is fixed to the +// airfoil rather than to the airflow. Only at a zero angle of attack is +// that axis perpendicular to the oncoming wind, making the force pure +// lift; as the angle of attack grows, the axis tilts back relative to the +// wind and an increasing share of the force opposes the direction of +// travel. The airfoil therefore produces drag that grows with the angle +// of attack, and keeps producing it once stalled, without drag having to +// be modeled as a separate term. +// +// What is not covered is the parasitic drag that a real airfoil produces +// even at a zero angle of attack, where the force is entirely +// perpendicular to the wind. That contribution is small enough to be +// folded into a drag contributor on the body itself, rather than being +// accounted for per airfoil. +// +// An AirfoilSolver must be configured, either through [NewAirfoilSolver] +// or [AirfoilSolver.Configure], before being registered with a [Scene] +// through [BodyAcceleratorView.Create]. type AirfoilSolver struct { relativePosition dprec.Vec3 relativeRotation dprec.Quat @@ -22,12 +87,22 @@ type AirfoilSolver struct { var _ AccelerationSolver = (*AirfoilSolver)(nil) +// NewAirfoilSolver creates a new [AirfoilSolver] configured according to +// config. func NewAirfoilSolver(config AirfoilSolverConfig) *AirfoilSolver { result := &AirfoilSolver{} result.Configure(config) return result } +// Configure configures this solver according to config. Negative +// SurfaceArea, StallAngle and LiftCoefficient values are clamped to zero, +// as with the respective setters. +// +// Configure must be called before this solver is registered with a +// [Scene] through [BodyAcceleratorView.Create]. Unlike [NewAirfoilSolver], +// it can be called on an already-allocated solver, which allows solvers to +// be cached (e.g. in a slice) and configured on demand. func (s *AirfoilSolver) Configure(config AirfoilSolverConfig) { s.relativePosition = config.RelativePosition s.relativeRotation = config.RelativeRotation @@ -36,59 +111,110 @@ func (s *AirfoilSolver) Configure(config AirfoilSolverConfig) { s.liftCoefficient = max(0.0, config.LiftCoefficient) } +// RelativePosition returns the body-local-space offset, relative to the +// target's center of mass, at which the airfoil is mounted. func (s *AirfoilSolver) RelativePosition() dprec.Vec3 { return s.relativePosition } +// SetRelativePosition changes the body-local-space offset, relative to the +// target's center of mass, at which the airfoil is mounted. +// +// It returns the solver itself, so that calls can be chained. func (s *AirfoilSolver) SetRelativePosition(position dprec.Vec3) *AirfoilSolver { s.relativePosition = position return s } +// RelativeRotation returns the body-local-space orientation of the +// airfoil, relative to the target's rotation. func (s *AirfoilSolver) RelativeRotation() dprec.Quat { return s.relativeRotation } +// SetRelativeRotation changes the body-local-space orientation of the +// airfoil, relative to the target's rotation. The provided rotation must +// be a unit quaternion. +// +// It returns the solver itself, so that calls can be chained. func (s *AirfoilSolver) SetRelativeRotation(rotation dprec.Quat) *AirfoilSolver { s.relativeRotation = rotation return s } +// SurfaceArea returns the area of the airfoil, in square meters, that the +// lift force is scaled by. func (s *AirfoilSolver) SurfaceArea() float64 { return s.surfaceArea } +// SetSurfaceArea changes the area of the airfoil, in square meters, that +// the lift force is scaled by. Negative values are clamped to zero. +// +// It returns the solver itself, so that calls can be chained. func (s *AirfoilSolver) SetSurfaceArea(area float64) *AirfoilSolver { s.surfaceArea = max(0.0, area) return s } +// StallAngle returns the angle of attack at which the airfoil produces +// its peak lift and past which it starts to stall. func (s *AirfoilSolver) StallAngle() dprec.Angle { return s.stallAngle } +// SetStallAngle changes the angle of attack at which the airfoil produces +// its peak lift and past which it starts to stall. Negative values are +// clamped to zero, which disables the pre-stall lift bump entirely. +// +// It returns the solver itself, so that calls can be chained. func (s *AirfoilSolver) SetStallAngle(angle dprec.Angle) *AirfoilSolver { s.stallAngle = max(0.0, angle) return s } +// LiftCoefficient returns the dimensionless coefficient that scales the +// lift produced by the airfoil. func (s *AirfoilSolver) LiftCoefficient() float64 { return s.liftCoefficient } +// SetLiftCoefficient changes the dimensionless coefficient that scales the +// lift produced by the airfoil. Negative values are clamped to zero. +// +// It returns the solver itself, so that calls can be chained. func (s *AirfoilSolver) SetLiftCoefficient(coefficient float64) *AirfoilSolver { s.liftCoefficient = max(0.0, coefficient) return s } +// ApplyAcceleration implements [AccelerationSolver.ApplyAcceleration]. +// +// It determines the wind that the airfoil experiences at its mounted +// position, relative to the medium, and converts it into a force along +// the airfoil's Y axis, applied at that same mounted position. +// +// The wind is split into the component that flows along the chord line +// and the component that flows along the span, each of which contributes +// its own force. The chordwise component is the one that behaves like a +// wing, in that it stalls; the spanwise component only ever produces the +// flat-plate force described on [AirfoilSolver.localLiftCoefficient], +// since an airfoil is not shaped to exploit air flowing along its span. +// +// If the airfoil is at rest relative to the medium, it does nothing. Wind +// that flows purely along the Y axis, having no chordwise or spanwise +// component to split, is treated as flowing along the chord line. func (s *AirfoilSolver) ApplyAcceleration(ctx AccelerationContext) { bodyRotation := ctx.Target.Rotation() + airfoilOffsetWS := dprec.QuatVec3Rotation(bodyRotation, s.relativePosition) + airfoilRotationWS := dprec.QuatProd(bodyRotation, s.relativeRotation) + airfoilVelocity := dprec.Vec3Sum( ctx.Target.LinearVelocity(), dprec.Vec3Cross( ctx.Target.AngularVelocity(), - s.relativePosition, + airfoilOffsetWS, ), ) @@ -98,9 +224,6 @@ func (s *AirfoilSolver) ApplyAcceleration(ctx AccelerationContext) { return // no significant wind } - airfoilOffsetWS := dprec.QuatVec3Rotation(bodyRotation, s.relativePosition) - airfoilRotationWS := dprec.QuatProd(bodyRotation, s.relativeRotation) - basisX := airfoilRotationWS.OrientationX() basisY := airfoilRotationWS.OrientationY() basisZ := airfoilRotationWS.OrientationZ() @@ -145,6 +268,21 @@ func (s *AirfoilSolver) ApplyAcceleration(ctx AccelerationContext) { } } +// localLiftCoefficient returns the coefficient that scales the force the +// airfoil produces along its Y axis at the specified angle of attack. +// +// The coefficient is odd-symmetric about a zero angle of attack, so an +// airfoil that is turned upside down produces the same force in the +// opposite direction. +// +// Every airfoil produces the flat-plate force that any inclined surface +// does, which grows with the sine of the angle of attack and is largest +// when the surface meets the wind broadside. When isDirect is set, and +// the airfoil has been given a stall angle, the profile additionally +// produces the extra force that its shape is meant to generate: a bump +// that grows from zero at a zero angle of attack, peaks at the stall +// angle, and decays back to zero at twice the stall angle, past which the +// airfoil is fully stalled and only the flat-plate force remains. func (s *AirfoilSolver) localLiftCoefficient(angle dprec.Angle, isDirect bool) float64 { if angle < 0.0 { return -s.localLiftCoefficient(-angle, isDirect) // flipped symmetric From f9f6b342a8d2040e7854b967e42bb7d10e149eee Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Thu, 13 Aug 2026 00:43:28 +0300 Subject: [PATCH 83/85] Add drag acceleration solver --- game/physics/solver_drag.go | 322 ++++++++++++++++++++++++++++++++++++ 1 file changed, 322 insertions(+) create mode 100644 game/physics/solver_drag.go diff --git a/game/physics/solver_drag.go b/game/physics/solver_drag.go new file mode 100644 index 00000000..22118abd --- /dev/null +++ b/game/physics/solver_drag.go @@ -0,0 +1,322 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +// DragSolverConfig holds the parameters with which a [DragSolver] is +// configured, either through [NewDragSolver] or [DragSolver.Configure]. +type DragSolverConfig struct { + + // RelativePosition is the body-local-space offset, relative to the + // target's center of mass, at which the box is centered. The drag + // force is applied at this point, so an offset box induces torque on + // the target in addition to decelerating it. + RelativePosition dprec.Vec3 + + // RelativeRotation is the body-local-space orientation of the box, + // relative to the target's rotation. See [DragSolver] for the axis + // convention that this orientation establishes. + // + // It must be a unit quaternion, as must every rotation handed to the + // engine. + RelativeRotation dprec.Quat + + // Width is the extent of the box along its X axis, in meters. + // Negative values are clamped to zero. + Width float64 + + // Height is the extent of the box along its Y axis, in meters. + // Negative values are clamped to zero. + Height float64 + + // Length is the extent of the box along its Z axis, in meters. + // Negative values are clamped to zero. + Length float64 + + // DragCoefficientX is the dimensionless drag coefficient for wind that + // flows along the box's X axis, and hence acts on its Height by Length + // cross-section. Negative values are clamped to zero. + DragCoefficientX float64 + + // DragCoefficientY is the dimensionless drag coefficient for wind that + // flows along the box's Y axis, and hence acts on its Width by Length + // cross-section. Negative values are clamped to zero. + DragCoefficientY float64 + + // DragCoefficientZ is the dimensionless drag coefficient for wind that + // flows along the box's Z axis, and hence acts on its Width by Height + // cross-section. Negative values are clamped to zero. + DragCoefficientZ float64 +} + +// DragSolver is an [AccelerationSolver] that models the aerodynamic drag +// of a box-shaped volume mounted at a fixed position and orientation on +// its target body. +// +// The box has its own coordinate frame, established by +// [DragSolver.RelativeRotation] on top of the target's rotation, in which +// [DragSolver.Width] is the extent along X, [DragSolver.Height] the +// extent along Y, and [DragSolver.Length] the extent along Z. The box is +// centered on [DragSolver.RelativePosition]. +// +// Each of the three axes resists the medium independently, in proportion +// to the square of the wind component along that axis, the +// cross-sectional area perpendicular to it, and the drag coefficient +// configured for it. A box can therefore be made more streamlined along +// one axis than another, and the resulting force is in general not +// antiparallel to the wind. +// +// Both translation and rotation are resisted. The wind is sampled at the +// box's mounted position and the force is applied there, so a box mounted +// off-center also induces torque; on top of that, a couple opposes the +// target's rotation about the box's own center. A box that is centered on +// the target's center of mass is subject to the couple alone. +// +// Only pressure drag is modeled, never skin friction, so a face that the +// medium merely slides along contributes nothing. A thin plate spun about +// its own normal is the degenerate case of this, and meets almost no +// resistance. +// +// Only drag is modeled. See [AirfoilSolver] for a surface that produces +// lift, which is meant to be combined with this solver rather than +// replaced by it. +// +// A DragSolver must be configured, either through [NewDragSolver] or +// [DragSolver.Configure], before being registered with a [Scene] through +// [BodyAcceleratorView.Create]. +type DragSolver struct { + relativePosition dprec.Vec3 + relativeRotation dprec.Quat + width float64 + height float64 + length float64 + dragCoefficientX float64 + dragCoefficientY float64 + dragCoefficientZ float64 +} + +var _ AccelerationSolver = (*DragSolver)(nil) + +// NewDragSolver creates a new [DragSolver] configured according to config. +func NewDragSolver(config DragSolverConfig) *DragSolver { + result := &DragSolver{} + result.Configure(config) + return result +} + +// Configure configures this solver according to config. Negative Width, +// Height, Length and drag coefficient values are clamped to zero, as with +// the respective setters. +// +// Configure must be called before this solver is registered with a +// [Scene] through [BodyAcceleratorView.Create]. Unlike [NewDragSolver], it +// can be called on an already-allocated solver, which allows solvers to be +// cached (e.g. in a slice) and configured on demand. +func (s *DragSolver) Configure(config DragSolverConfig) { + s.relativePosition = config.RelativePosition + s.relativeRotation = config.RelativeRotation + s.width = max(0.0, config.Width) + s.height = max(0.0, config.Height) + s.length = max(0.0, config.Length) + s.dragCoefficientX = max(0.0, config.DragCoefficientX) + s.dragCoefficientY = max(0.0, config.DragCoefficientY) + s.dragCoefficientZ = max(0.0, config.DragCoefficientZ) +} + +// RelativePosition returns the body-local-space offset, relative to the +// target's center of mass, at which the box is centered. +func (s *DragSolver) RelativePosition() dprec.Vec3 { + return s.relativePosition +} + +// SetRelativePosition changes the body-local-space offset, relative to the +// target's center of mass, at which the box is centered. +// +// It returns the solver itself, so that calls can be chained. +func (s *DragSolver) SetRelativePosition(position dprec.Vec3) *DragSolver { + s.relativePosition = position + return s +} + +// RelativeRotation returns the body-local-space orientation of the box, +// relative to the target's rotation. +func (s *DragSolver) RelativeRotation() dprec.Quat { + return s.relativeRotation +} + +// SetRelativeRotation changes the body-local-space orientation of the box, +// relative to the target's rotation. The provided rotation must be a unit +// quaternion. +// +// It returns the solver itself, so that calls can be chained. +func (s *DragSolver) SetRelativeRotation(rotation dprec.Quat) *DragSolver { + s.relativeRotation = rotation + return s +} + +// Width returns the extent of the box along its X axis, in meters. +func (s *DragSolver) Width() float64 { + return s.width +} + +// SetWidth changes the extent of the box along its X axis, in meters. +// Negative values are clamped to zero. +// +// It returns the solver itself, so that calls can be chained. +func (s *DragSolver) SetWidth(width float64) *DragSolver { + s.width = max(0.0, width) + return s +} + +// Height returns the extent of the box along its Y axis, in meters. +func (s *DragSolver) Height() float64 { + return s.height +} + +// SetHeight changes the extent of the box along its Y axis, in meters. +// Negative values are clamped to zero. +// +// It returns the solver itself, so that calls can be chained. +func (s *DragSolver) SetHeight(height float64) *DragSolver { + s.height = max(0.0, height) + return s +} + +// Length returns the extent of the box along its Z axis, in meters. +func (s *DragSolver) Length() float64 { + return s.length +} + +// SetLength changes the extent of the box along its Z axis, in meters. +// Negative values are clamped to zero. +// +// It returns the solver itself, so that calls can be chained. +func (s *DragSolver) SetLength(length float64) *DragSolver { + s.length = max(0.0, length) + return s +} + +// DragCoefficientX returns the dimensionless drag coefficient for wind +// that flows along the box's X axis. +func (s *DragSolver) DragCoefficientX() float64 { + return s.dragCoefficientX +} + +// SetDragCoefficientX changes the dimensionless drag coefficient for wind +// that flows along the box's X axis. Negative values are clamped to zero. +// +// It returns the solver itself, so that calls can be chained. +func (s *DragSolver) SetDragCoefficientX(coefficient float64) *DragSolver { + s.dragCoefficientX = max(0.0, coefficient) + return s +} + +// DragCoefficientY returns the dimensionless drag coefficient for wind +// that flows along the box's Y axis. +func (s *DragSolver) DragCoefficientY() float64 { + return s.dragCoefficientY +} + +// SetDragCoefficientY changes the dimensionless drag coefficient for wind +// that flows along the box's Y axis. Negative values are clamped to zero. +// +// It returns the solver itself, so that calls can be chained. +func (s *DragSolver) SetDragCoefficientY(coefficient float64) *DragSolver { + s.dragCoefficientY = max(0.0, coefficient) + return s +} + +// DragCoefficientZ returns the dimensionless drag coefficient for wind +// that flows along the box's Z axis. +func (s *DragSolver) DragCoefficientZ() float64 { + return s.dragCoefficientZ +} + +// SetDragCoefficientZ changes the dimensionless drag coefficient for wind +// that flows along the box's Z axis. Negative values are clamped to zero. +// +// It returns the solver itself, so that calls can be chained. +func (s *DragSolver) SetDragCoefficientZ(coefficient float64) *DragSolver { + s.dragCoefficientZ = max(0.0, coefficient) + return s +} + +// ApplyAcceleration implements [AccelerationSolver.ApplyAcceleration]. +// +// It applies both the force that resists the target's motion through the +// medium and the couple that resists the target's rotation. +// +// The force is derived from the wind that the box experiences at its +// mounted position, which includes the velocity that the target's own +// rotation imparts there, and is applied at that same position. It is +// resolved onto the box's three axes, each of which contributes +// independently. If the box is at rest relative to the medium, no force is +// applied. +// +// The couple is derived from the target's angular velocity alone, since a +// medium that moves uniformly carries no rotation of its own to be +// measured against. It is applied whether or not the target is also +// translating. +func (s *DragSolver) ApplyAcceleration(ctx AccelerationContext) { + bodyRotation := ctx.Target.Rotation() + + boxOffsetWS := dprec.QuatVec3Rotation(bodyRotation, s.relativePosition) + boxRotationWS := dprec.QuatProd(bodyRotation, s.relativeRotation) + + basisX := boxRotationWS.OrientationX() + basisY := boxRotationWS.OrientationY() + basisZ := boxRotationWS.OrientationZ() + + { // Linear drag + boxVelocity := dprec.Vec3Sum( + ctx.Target.LinearVelocity(), + dprec.Vec3Cross( + ctx.Target.AngularVelocity(), + boxOffsetWS, + ), + ) + + relWindVelocity := dprec.Vec3Diff(ctx.MediumVelocity, boxVelocity) + relWindVelocityLng := relWindVelocity.Length() + if relWindVelocityLng > Epsilon { + windX := dprec.Vec3Dot(relWindVelocity, basisX) + windY := dprec.Vec3Dot(relWindVelocity, basisY) + windZ := dprec.Vec3Dot(relWindVelocity, basisZ) + + magnitudeX := s.dragCoefficientX * 0.5 * ctx.MediumDensity * windX * dprec.Abs(windX) * (s.height * s.length) + magnitudeY := s.dragCoefficientY * 0.5 * ctx.MediumDensity * windY * dprec.Abs(windY) * (s.width * s.length) + magnitudeZ := s.dragCoefficientZ * 0.5 * ctx.MediumDensity * windZ * dprec.Abs(windZ) * (s.width * s.height) + + ctx.Target.ApplyOffsetForce(boxOffsetWS, dprec.Vec3Sum( + dprec.Vec3Prod(basisX, magnitudeX), + dprec.Vec3Sum( + dprec.Vec3Prod(basisY, magnitudeY), + dprec.Vec3Prod(basisZ, magnitudeZ), + ), + )) + } + } + + { // Angular drag + angularVelocity := ctx.Target.AngularVelocity() + angularWindX := dprec.Vec3Dot(angularVelocity, basisX) + angularWindY := dprec.Vec3Dot(angularVelocity, basisY) + angularWindZ := dprec.Vec3Dot(angularVelocity, basisZ) + + halfWidth := s.width * 0.5 + halfHeight := s.height * 0.5 + halfLength := s.length * 0.5 + + factorWidth := dprec.Sqr(dprec.Sqr(halfWidth)) + factorHeight := dprec.Sqr(dprec.Sqr(halfHeight)) + factorLength := dprec.Sqr(dprec.Sqr(halfLength)) + + torque := dprec.Vec3{ + X: -angularWindX * dprec.Abs(angularWindX) * halfWidth * (s.dragCoefficientY*factorLength + s.dragCoefficientZ*factorHeight), + Y: -angularWindY * dprec.Abs(angularWindY) * halfHeight * (s.dragCoefficientZ*factorWidth + s.dragCoefficientX*factorLength), + Z: -angularWindZ * dprec.Abs(angularWindZ) * halfLength * (s.dragCoefficientX*factorHeight + s.dragCoefficientY*factorWidth), + } + torque = dprec.Vec3Prod(torque, ctx.MediumDensity) + + ctx.Target.ApplyTorque(dprec.QuatVec3Rotation(boxRotationWS, torque)) + } +} From 15b4f35209f7104497326a3fd51eaa8c8723ba90 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Thu, 13 Aug 2026 00:44:28 +0300 Subject: [PATCH 84/85] Remove old aerodynamics file --- game/physics/aerodynamics.go | 139 ----------------------------------- 1 file changed, 139 deletions(-) delete mode 100644 game/physics/aerodynamics.go diff --git a/game/physics/aerodynamics.go b/game/physics/aerodynamics.go deleted file mode 100644 index 7c93d051..00000000 --- a/game/physics/aerodynamics.go +++ /dev/null @@ -1,139 +0,0 @@ -package physics - -import ( - "github.com/mokiat/gomath/dprec" -) - -// TODO: Move under aerodynamics package - -// IdentityTransform returns a new Transform that represents the origin. -func IdentityTransform() Transform { - return Transform{ - position: dprec.ZeroVec3(), - rotation: dprec.IdentityQuat(), - } -} - -// NewTransform creates a new Transform with the specified position and -// rotation. -func NewTransform(position dprec.Vec3, rotation dprec.Quat) Transform { - return Transform{ - position: position, - rotation: rotation, - } -} - -// Transform represents a shape transformation - translation and rotation. -type Transform struct { - position dprec.Vec3 - rotation dprec.Quat -} - -// Position returns the translation of this Transform. -func (t Transform) Position() dprec.Vec3 { - return t.position -} - -// Rotation returns the orientation of this Transform. -func (t Transform) Rotation() dprec.Quat { - return t.rotation -} - -// Transformed returns a new Transform that is based on this one but has the -// specified Transform applied to it. -func (t Transform) Transformed(transform Transform) Transform { - // Note: Doing an identity check on the current or parent transform, - // as a form of quick return, actually worsens the performance. - return Transform{ - position: dprec.Vec3Sum( - transform.position, - dprec.QuatVec3Rotation(transform.rotation, t.position), - ), - rotation: dprec.QuatProd(transform.rotation, t.rotation), - } -} - -func NewAerodynamicShape(transform Transform, solver AerodynamicSolver) AerodynamicShape { - return AerodynamicShape{ - Transform: transform, - solver: solver, - } -} - -type AerodynamicShape struct { - Transform - solver AerodynamicSolver -} - -// Transformed returns a new Placement that is based on this one but has -// the specified transform applied to it. -func (p AerodynamicShape) Transformed(parent Transform) AerodynamicShape { - p.Transform = p.Transform.Transformed(parent) - return p -} - -// AerodynamicSolver represents a shape that is affected -// by air or liquid motion and inflicts a force on the body. -type AerodynamicSolver interface { - Force(windSpeed dprec.Vec3, density float64) dprec.Vec3 -} - -func NewSurfaceAerodynamicShape(width, height, length float64) *SurfaceAerodynamicShape { - return &SurfaceAerodynamicShape{ - width: width, - height: height, - length: length, - - dragCoefficient: 0.5, - liftCoefficient: 2.1, - } -} - -var _ AerodynamicSolver = (*SurfaceAerodynamicShape)(nil) - -type SurfaceAerodynamicShape struct { - width float64 - height float64 - length float64 - - dragCoefficient float64 - liftCoefficient float64 -} - -func (s *SurfaceAerodynamicShape) SetDragCoefficient(coefficient float64) *SurfaceAerodynamicShape { - s.dragCoefficient = coefficient - return s -} - -func (s *SurfaceAerodynamicShape) SetLiftCoefficient(coefficient float64) *SurfaceAerodynamicShape { - s.liftCoefficient = coefficient - return s -} - -func (s *SurfaceAerodynamicShape) Force(windSpeed dprec.Vec3, density float64) dprec.Vec3 { - // DRAG - dragX := s.length * s.height * dprec.Vec3Dot(windSpeed, dprec.BasisXVec3()) - dragY := s.width * s.length * dprec.Vec3Dot(windSpeed, dprec.BasisYVec3()) - dragZ := s.width * s.height * dprec.Vec3Dot(windSpeed, dprec.BasisZVec3()) - result := dprec.Vec3Prod(dprec.NewVec3(dragX, dragY, dragZ), windSpeed.Length()*density*s.dragCoefficient/2.0) - - // LIFT - liftVelocity := -dprec.Vec3Dot(windSpeed, dprec.BasisZVec3()) - if liftVelocity > 0 { - stallSN := dprec.Sin(dprec.Degrees(40)) - sn := dprec.Vec3Dot(dprec.UnitVec3(windSpeed), dprec.BasisYVec3()) - if dprec.Abs(sn) < stallSN { - result = dprec.Vec3Sum(result, dprec.NewVec3( - 0.0, - sn*density*s.liftCoefficient*liftVelocity*liftVelocity*s.width*s.length, - 0.0, - )) - } - } - - return result -} - -func (s *SurfaceAerodynamicShape) BoundingSphereRadius() float64 { - return dprec.Sqrt(s.width*s.width+s.height*s.height+s.length*s.length) / 2.0 -} From 497a78ed5f15afb56dc3224751f64714d658a5c2 Mon Sep 17 00:00:00 2001 From: Momchil Atanasov Date: Sun, 16 Aug 2026 14:40:30 +0300 Subject: [PATCH 85/85] Load terrains from asset --- game/asset/conv/physics.go | 120 +++++------ game/asset/dsl/provider_model.go | 27 ++- game/asset/dto/physics.go | 86 +++----- game/asset/mdl/model.go | 34 ++- game/asset/mdl/node.go | 14 ++ game/asset/mdl/physics.go | 204 ++++++++---------- game/asset_mesh.go | 21 ++ game/asset_model.go | 63 ++---- game/asset_physics.go | 357 ++++++------------------------- 9 files changed, 319 insertions(+), 607 deletions(-) diff --git a/game/asset/conv/physics.go b/game/asset/conv/physics.go index 8e4d69db..e9d0f0fa 100644 --- a/game/asset/conv/physics.go +++ b/game/asset/conv/physics.go @@ -9,9 +9,8 @@ import ( ) type PhysicsSource interface { - AllPhysicsBodyMaterials() []*mdl.BodyMaterial - AllPhysicsBodyDefinitions() []*mdl.BodyDefinition - AllPhysicsBodyPlacements() []mdl.Placed[*mdl.Body] + AllPhysicsBodyPlacements() []mdl.Placed[*mdl.PhysicsBody] + AllPhysicsTerrainPlacements() []mdl.Placed[*mdl.PhysicsTerrain] } func NewPhysicsConverter() *PhysicsConverter { @@ -34,65 +33,61 @@ func (c *PhysicsConverter) Convert(target *ds.List[chunked.Chunk], asset any) er } func (c *PhysicsConverter) CreatePhysicsChunk(src PhysicsSource) (*dto.PhysicsChunk, error) { - allMaterials := src.AllPhysicsBodyMaterials() - dtoBodyMaterials := make([]dto.BodyMaterial, len(allMaterials)) - for i, material := range allMaterials { - dtoBodyMaterials[i] = c.convertBodyMaterial(material) - } - - allDefinitions := src.AllPhysicsBodyDefinitions() - dtoBodyDefinitions := make([]dto.BodyDefinition, len(allDefinitions)) - for i, definition := range allDefinitions { - dtoBodyDefinitions[i] = c.convertBodyDefinition(definition) - } - allBodyPlacements := src.AllPhysicsBodyPlacements() - dtoBodies := make([]dto.Body, len(allBodyPlacements)) + + dtoBodies := make([]dto.PhysicsBody, len(allBodyPlacements)) for i, placement := range allBodyPlacements { body := placement.Value - dtoBodies[i] = c.convertBody(placement.Node, body) - } - return &dto.PhysicsChunk{ - BodyMaterials: dtoBodyMaterials, - BodyDefinitions: dtoBodyDefinitions, - Bodies: dtoBodies, - }, nil -} - -func (c *PhysicsConverter) convertBodyMaterial(material *mdl.BodyMaterial) dto.BodyMaterial { - return dto.BodyMaterial{ - ID: material.ID(), - FrictionCoefficient: material.FrictionCoefficient(), - RestitutionCoefficient: material.RestitutionCoefficient(), - } -} + var dtoCollisionSpheres []dto.CollisionSphere + for _, sphere := range body.CollisionSpheres() { + dtoCollisionSpheres = append(dtoCollisionSpheres, dto.CollisionSphere{ + CollisionShape: dto.CollisionShape{ + FrictionCoefficient: sphere.FrictionCoefficient(), + RestitutionCoefficient: sphere.RestitutionCoefficient(), + }, + Translation: sphere.Translation(), + Radius: sphere.Radius(), + }) + } -func (c *PhysicsConverter) convertBodyDefinition(definition *mdl.BodyDefinition) dto.BodyDefinition { - return dto.BodyDefinition{ - ID: definition.ID(), - MaterialID: definition.Material().ID(), - Mass: definition.Mass(), - MomentOfInertia: definition.MomentOfInertia(), - DragFactor: definition.DragFactor(), - AngularDragFactor: definition.AngularDragFactor(), - CollisionBoxes: gog.Map(definition.CollisionBoxes(), func(box *mdl.CollisionBox) dto.CollisionBox { - return dto.CollisionBox{ + var dtoCollisionBoxes []dto.CollisionBox + for _, box := range body.CollisionBoxes() { + dtoCollisionBoxes = append(dtoCollisionBoxes, dto.CollisionBox{ + CollisionShape: dto.CollisionShape{ + FrictionCoefficient: box.FrictionCoefficient(), + RestitutionCoefficient: box.RestitutionCoefficient(), + }, Translation: box.Translation(), Rotation: box.Rotation(), Width: box.Width(), Height: box.Height(), Length: box.Length(), - } - }), - CollisionSpheres: gog.Map(definition.CollisionSpheres(), func(sphere *mdl.CollisionSphere) dto.CollisionSphere { - return dto.CollisionSphere{ - Translation: sphere.Translation(), - Radius: sphere.Radius(), - } - }), - CollisionMeshes: gog.Map(definition.CollisionMeshes(), func(mesh *mdl.CollisionMesh) dto.CollisionMesh { - return dto.CollisionMesh{ + }) + } + + dtoBodies[i] = dto.PhysicsBody{ + ID: body.ID(), + NodeID: placement.Node.ID(), + Mass: body.Mass(), + MomentOfInertia: body.MomentOfInertia(), + CollisionSpheres: dtoCollisionSpheres, + CollisionBoxes: dtoCollisionBoxes, + } + } + + allTerrainPlacements := src.AllPhysicsTerrainPlacements() + dtoTerrains := make([]dto.PhysicsTerrain, len(allTerrainPlacements)) + for i, placement := range allTerrainPlacements { + terrain := placement.Value + + var dtoCollisionMeshes []dto.CollisionMesh + for _, mesh := range terrain.CollisionMeshes() { + dtoCollisionMeshes = append(dtoCollisionMeshes, dto.CollisionMesh{ + CollisionShape: dto.CollisionShape{ + FrictionCoefficient: mesh.FrictionCoefficient(), + RestitutionCoefficient: mesh.RestitutionCoefficient(), + }, Translation: mesh.Translation(), Rotation: mesh.Rotation(), Triangles: gog.Map(mesh.Triangles(), func(triangle mdl.CollisionTriangle) dto.CollisionTriangle { @@ -102,15 +97,18 @@ func (c *PhysicsConverter) convertBodyDefinition(definition *mdl.BodyDefinition) C: triangle.C, } }), - } - }), - } -} + }) + } -func (c *PhysicsConverter) convertBody(node *mdl.Node, body *mdl.Body) dto.Body { - return dto.Body{ - ID: body.ID(), - NodeID: node.ID(), - BodyDefinitionID: body.Definition().ID(), + dtoTerrains[i] = dto.PhysicsTerrain{ + ID: terrain.ID(), + NodeID: placement.Node.ID(), + CollisionMeshes: dtoCollisionMeshes, + } } + + return &dto.PhysicsChunk{ + Bodies: dtoBodies, + Terrains: dtoTerrains, + }, nil } diff --git a/game/asset/dsl/provider_model.go b/game/asset/dsl/provider_model.go index b3a19404..d7c05c70 100644 --- a/game/asset/dsl/provider_model.go +++ b/game/asset/dsl/provider_model.go @@ -304,12 +304,9 @@ func BuildModelResource(gltfDoc *gltf.Document, forceCollision, onlyAnimations b // build mesh definitions meshDefinitionFromIndex := make(map[int]*mdl.MeshDefinition) - bodyDefinitionFromIndex := make(map[int]*mdl.BodyDefinition) + terrainMeshesFromIndex := make(map[int][]*mdl.CollisionMesh) if !onlyAnimations { for i, gltfMesh := range gltfDoc.Meshes { - bodyMaterial := mdl.NewBodyMaterial() - bodyDefinition := mdl.NewBodyDefinition(bodyMaterial) - metadata := mdl.Metadata(gltfutil.Properties(gltfMesh.Extras)) geometry := mdl.NewGeometry() @@ -458,17 +455,13 @@ func BuildModelResource(gltfDoc *gltf.Document, forceCollision, onlyAnimations b geometry.AddFragment(fragment) if (geometry.Metadata().HasCollision() || forceCollision) && !fragment.Metadata().HasSkipCollision() { - bodyDefinition.AddCollisionMeshes(createCollisionMeshes(geometry, fragment)) + terrainMeshesFromIndex[i] = append(terrainMeshesFromIndex[i], createCollisionMeshes(geometry, fragment)...) } } meshDefinition.SetName(gltfMesh.Name) meshDefinition.SetGeometry(geometry) meshDefinitionFromIndex[i] = meshDefinition - - if (geometry.Metadata().HasCollision() || forceCollision) && len(bodyDefinition.CollisionMeshes()) > 0 { - bodyDefinitionFromIndex[i] = bodyDefinition - } } } @@ -545,12 +538,14 @@ func BuildModelResource(gltfDoc *gltf.Document, forceCollision, onlyAnimations b return mesh } - createBody := func(gltfNode *gltf.Node) *mdl.Body { - bodyDefinition, ok := bodyDefinitionFromIndex[*gltfNode.Mesh] + createTerrain := func(gltfNode *gltf.Node) *mdl.PhysicsTerrain { + collisionMeshes, ok := terrainMeshesFromIndex[*gltfNode.Mesh] if !ok { - return nil // no collision mesh + return nil // no collision meshes } - return mdl.NewBody(bodyDefinition) + terrain := mdl.NewPhysicsTerrain() + terrain.SetCollisionMeshes(collisionMeshes) + return terrain } // ensure unique node names @@ -574,8 +569,8 @@ func BuildModelResource(gltfDoc *gltf.Document, forceCollision, onlyAnimations b switch { case gltfNodeHasMesh(gltfNode): node.AddAttachment(createMesh(gltfNode)) - if body := createBody(gltfNode); body != nil { - node.AddAttachment(body) + if terrain := createTerrain(gltfNode); terrain != nil { + node.AddAttachment(terrain) } case gltfNodeHasLight(gltfNode): node.AddAttachment(createLight(gltfNode)) @@ -891,6 +886,8 @@ func createCollisionMeshes(geometry *mdl.Geometry, fragment *mdl.Fragment) []*md mesh := mdl.NewCollisionMesh() mesh.SetTranslation(center) mesh.SetRotation(dprec.IdentityQuat()) + mesh.SetFrictionCoefficient(1.0) + mesh.SetRestitutionCoefficient(0.3) mesh.SetTriangles(triangles) return mesh }) diff --git a/game/asset/dto/physics.go b/game/asset/dto/physics.go index fc99553e..f0ec64fd 100644 --- a/game/asset/dto/physics.go +++ b/game/asset/dto/physics.go @@ -1,6 +1,8 @@ package dto -import "github.com/mokiat/gomath/dprec" +import ( + "github.com/mokiat/gomath/dprec" +) const PhysicsChunkID = "lacking:physics" @@ -9,42 +11,18 @@ type PhysicsChunkHolder struct { } type PhysicsChunk struct { + Bodies []PhysicsBody - // BodyMaterials is the collection of body materials that are part of the - // scene. - BodyMaterials []BodyMaterial - - // BodyDefinitions is the collection of body definitions that are part of - // the scene. - BodyDefinitions []BodyDefinition - - // Bodies is the collection of body instances that are part of the scene. - Bodies []Body + Terrains []PhysicsTerrain } -// Body represents a physical body. -type Body struct { - +type PhysicsBody struct { // ID is the unique identifier of the body within the file. ID uint32 // NodeID is the ID of the node that this body is attached to. NodeID uint32 - // BodyDefinitionID is the ID of the body definition that this - // body uses. - BodyDefinitionID uint32 -} - -// BodyDefinition represents the physical properties of a body. -type BodyDefinition struct { - - // ID is the unique identifier of the body definition within the file. - ID uint32 - - // MaterialID is the ID of the physics material that this body uses. - MaterialID uint32 - // Mass is the mass of the body. Mass float64 @@ -52,31 +30,21 @@ type BodyDefinition struct { // as 3x3 tensor. MomentOfInertia dprec.Mat3 - // DragFactor is the linear drag factor of the body. - DragFactor float64 - - // AngularDragFactor is the angular drag factor of the body. - AngularDragFactor float64 + CollisionSpheres []CollisionSphere - // CollisionBoxes is a list of collision boxes that define the - // collision shape of the body. CollisionBoxes []CollisionBox +} - // CollisionSpheres is a list of collision spheres that define the - // collision shape of the body. - CollisionSpheres []CollisionSphere +type PhysicsTerrain struct { + // ID is the unique identifier of the body within the file. + ID uint32 + + NodeID uint32 - // CollisionMeshes is a list of collision meshes that define the - // collision shape of the body. CollisionMeshes []CollisionMesh } -// BodyMaterial represents a physical material. -type BodyMaterial struct { - - // ID is the unique identifier of the body material within the file. - ID uint32 - +type CollisionShape struct { // FrictionCoefficient is the coefficient of friction of this material. // Lower values mean more slippery surfaces. FrictionCoefficient float64 @@ -86,8 +54,20 @@ type BodyMaterial struct { RestitutionCoefficient float64 } +// CollisionSphere represents a sphere-shaped collision volume. +type CollisionSphere struct { + CollisionShape + + // Translation is the position of the sphere. + Translation dprec.Vec3 + + // Radius is the radius of the sphere. + Radius float64 +} + // CollisionBox represents a box-shaped collision volume. type CollisionBox struct { + CollisionShape // Translation is the position of the box. Translation dprec.Vec3 @@ -105,18 +85,9 @@ type CollisionBox struct { Length float64 } -// CollisionSphere represents a sphere-shaped collision volume. -type CollisionSphere struct { - - // Translation is the position of the sphere. - Translation dprec.Vec3 - - // Radius is the radius of the sphere. - Radius float64 -} - // CollisionMesh represents a mesh-shaped collision volume. type CollisionMesh struct { + CollisionShape // Translation is the position of the mesh. Translation dprec.Vec3 @@ -141,4 +112,7 @@ type CollisionTriangle struct { // C is the third vertex of the triangle. C dprec.Vec3 + + // TODO: Add clipping normals so that junctures between triangles can be handled. + // Or maybe edge fold angles through which clipping normals can be derived. } diff --git a/game/asset/mdl/model.go b/game/asset/mdl/model.go index 7590cd3e..4871ba11 100644 --- a/game/asset/mdl/model.go +++ b/game/asset/mdl/model.go @@ -202,30 +202,24 @@ func (s *Model) AllMeshPlacements() []Placed[*Mesh] { return result } -func (s *Model) AllPhysicsBodyMaterials() []*BodyMaterial { - var result []*BodyMaterial - for _, definition := range s.AllPhysicsBodyDefinitions() { - material := definition.Material() - result = append(result, material) - } - return gog.Dedupe(result) -} - -func (s *Model) AllPhysicsBodyDefinitions() []*BodyDefinition { - var result []*BodyDefinition - for _, placement := range s.AllPhysicsBodyPlacements() { - body := placement.Value - definition := body.Definition() - result = append(result, definition) +func (s *Model) AllPhysicsBodyPlacements() []Placed[*PhysicsBody] { + var result []Placed[*PhysicsBody] + for _, node := range s.NodesIter() { + for body := range NodeAttachmentsOfType[*PhysicsBody](node) { + result = append(result, Placed[*PhysicsBody]{ + Node: node, + Value: body, + }) + } } - return gog.Dedupe(result) + return result } -func (s *Model) AllPhysicsBodyPlacements() []Placed[*Body] { - var result []Placed[*Body] +func (s *Model) AllPhysicsTerrainPlacements() []Placed[*PhysicsTerrain] { + var result []Placed[*PhysicsTerrain] for _, node := range s.NodesIter() { - for body := range NodeAttachmentsOfType[*Body](node) { - result = append(result, Placed[*Body]{ + for body := range NodeAttachmentsOfType[*PhysicsTerrain](node) { + result = append(result, Placed[*PhysicsTerrain]{ Node: node, Value: body, }) diff --git a/game/asset/mdl/node.go b/game/asset/mdl/node.go index bbd70565..d658540b 100644 --- a/game/asset/mdl/node.go +++ b/game/asset/mdl/node.go @@ -117,6 +117,20 @@ func (n *Node) RemoveNode(node *Node) { }) } +func (n *Node) Matrix() dprec.Mat4 { + return dprec.TRSMat4(n.translation, n.rotation, n.scale) +} + +func (n *Node) AbsoluteMatrix() dprec.Mat4 { + if n.parent == nil { + return n.Matrix() + } + return dprec.Mat4Prod( + n.parent.AbsoluteMatrix(), + n.Matrix(), + ) +} + func NodeAttachmentsOfType[T any](node *Node) iter.Seq[T] { return func(yield func(T) bool) { for _, attachment := range node.attachments { diff --git a/game/asset/mdl/physics.go b/game/asset/mdl/physics.go index a348238e..bc31d345 100644 --- a/game/asset/mdl/physics.go +++ b/game/asset/mdl/physics.go @@ -2,131 +2,135 @@ package mdl import "github.com/mokiat/gomath/dprec" -func NewBodyMaterial() *BodyMaterial { - return &BodyMaterial{ - Object: NewObject(), - frictionCoefficient: 1.0, - restitutionCoefficient: 0.5, +type PhysicsBody struct { + *Object + mass float64 + momentOfInertia dprec.Mat3 + collisionSpheres []*CollisionSphere + collisionBoxes []*CollisionBox +} + +func NewPhysicsBody() *PhysicsBody { + return &PhysicsBody{ + Object: NewObject(), + mass: 1.0, + momentOfInertia: dprec.IdentityMat3(), + collisionSpheres: []*CollisionSphere{}, + collisionBoxes: []*CollisionBox{}, } } -type BodyMaterial struct { - *Object - frictionCoefficient float64 - restitutionCoefficient float64 +func (b *PhysicsBody) Mass() float64 { + return b.mass } -func (m *BodyMaterial) FrictionCoefficient() float64 { - return m.frictionCoefficient +func (b *PhysicsBody) SetMass(value float64) { + b.mass = value } -func (m *BodyMaterial) SetFrictionCoefficient(value float64) { - m.frictionCoefficient = value +func (b *PhysicsBody) MomentOfInertia() dprec.Mat3 { + return b.momentOfInertia } -func (m *BodyMaterial) RestitutionCoefficient() float64 { - return m.restitutionCoefficient +func (b *PhysicsBody) SetMomentOfInertia(value dprec.Mat3) { + b.momentOfInertia = value } -func (m *BodyMaterial) SetRestitutionCoefficient(value float64) { - m.restitutionCoefficient = value +func (b *PhysicsBody) CollisionSpheres() []*CollisionSphere { + return b.collisionSpheres } -func NewBodyDefinition(material *BodyMaterial) *BodyDefinition { - return &BodyDefinition{ - Object: NewObject(), - material: material, - } +func (b *PhysicsBody) AddCollisionSphere(value *CollisionSphere) { + b.collisionSpheres = append(b.collisionSpheres, value) } -type BodyDefinition struct { - *Object - material *BodyMaterial - mass float64 - momentOfInertia dprec.Mat3 - dragFactor float64 - angularDragFactor float64 - collisionBoxes []*CollisionBox - collisionSpheres []*CollisionSphere - collisionMeshes []*CollisionMesh +func (b *PhysicsBody) CollisionBoxes() []*CollisionBox { + return b.collisionBoxes } -func (d *BodyDefinition) Material() *BodyMaterial { - return d.material +func (b *PhysicsBody) AddCollisionBox(value *CollisionBox) { + b.collisionBoxes = append(b.collisionBoxes, value) } -func (d *BodyDefinition) Mass() float64 { - return d.mass +type PhysicsTerrain struct { + *Object + collisionMeshes []*CollisionMesh } -func (d *BodyDefinition) SetMass(value float64) { - d.mass = value +func NewPhysicsTerrain() *PhysicsTerrain { + return &PhysicsTerrain{ + Object: NewObject(), + collisionMeshes: []*CollisionMesh{}, + } } -func (d *BodyDefinition) MomentOfInertia() dprec.Mat3 { - return d.momentOfInertia +func (t *PhysicsTerrain) CollisionMeshes() []*CollisionMesh { + return t.collisionMeshes } -func (d *BodyDefinition) SetMomentOfInertia(value dprec.Mat3) { - d.momentOfInertia = value +func (t *PhysicsTerrain) SetCollisionMeshes(collisionMeshes []*CollisionMesh) { + t.collisionMeshes = collisionMeshes } -func (d *BodyDefinition) DragFactor() float64 { - return d.dragFactor +func (t *PhysicsTerrain) AddCollisionMesh(value *CollisionMesh) { + t.collisionMeshes = append(t.collisionMeshes, value) } -func (d *BodyDefinition) SetDragFactor(value float64) { - d.dragFactor = value +func (t *PhysicsTerrain) AddCollisionMeshes(collisionMeshes []*CollisionMesh) { + t.collisionMeshes = append(t.collisionMeshes, collisionMeshes...) } -func (d *BodyDefinition) AngularDragFactor() float64 { - return d.angularDragFactor +type CollisionShape struct { + frictionCoefficient float64 + restitutionCoefficient float64 } -func (d *BodyDefinition) SetAngularDragFactor(value float64) { - d.angularDragFactor = value +func (s *CollisionShape) FrictionCoefficient() float64 { + return s.frictionCoefficient } -func (d *BodyDefinition) CollisionBoxes() []*CollisionBox { - return d.collisionBoxes +func (s *CollisionShape) SetFrictionCoefficient(value float64) { + s.frictionCoefficient = value } -func (d *BodyDefinition) AddCollisionBox(value *CollisionBox) { - d.collisionBoxes = append(d.collisionBoxes, value) +func (s *CollisionShape) RestitutionCoefficient() float64 { + return s.restitutionCoefficient } -func (d *BodyDefinition) CollisionSpheres() []*CollisionSphere { - return d.collisionSpheres +func (s *CollisionShape) SetRestitutionCoefficient(value float64) { + s.restitutionCoefficient = value } -func (d *BodyDefinition) AddCollisionSphere(value *CollisionSphere) { - d.collisionSpheres = append(d.collisionSpheres, value) +type CollisionSphere struct { + CollisionShape + translation dprec.Vec3 + radius float64 } -func (d *BodyDefinition) CollisionMeshes() []*CollisionMesh { - return d.collisionMeshes +func NewCollisionSphere() *CollisionSphere { + return &CollisionSphere{ + translation: dprec.ZeroVec3(), + } } -func (d *BodyDefinition) SetCollisionMeshes(collisionMeshes []*CollisionMesh) { - d.collisionMeshes = collisionMeshes +func (s *CollisionSphere) Translation() dprec.Vec3 { + return s.translation } -func (d *BodyDefinition) AddCollisionMesh(value *CollisionMesh) { - d.collisionMeshes = append(d.collisionMeshes, value) +func (s *CollisionSphere) SetTranslation(value dprec.Vec3) { + s.translation = value } -func (d *BodyDefinition) AddCollisionMeshes(collisionMeshes []*CollisionMesh) { - d.collisionMeshes = append(d.collisionMeshes, collisionMeshes...) +func (s *CollisionSphere) Radius() float64 { + return s.radius } -func NewCollisionBox() *CollisionBox { - return &CollisionBox{ - translation: dprec.ZeroVec3(), - rotation: dprec.IdentityQuat(), - } +func (s *CollisionSphere) SetRadius(value float64) { + s.radius = value } type CollisionBox struct { + CollisionShape translation dprec.Vec3 rotation dprec.Quat width float64 @@ -134,6 +138,13 @@ type CollisionBox struct { length float64 } +func NewCollisionBox() *CollisionBox { + return &CollisionBox{ + translation: dprec.ZeroVec3(), + rotation: dprec.IdentityQuat(), + } +} + func (b *CollisionBox) Translation() dprec.Vec3 { return b.translation } @@ -174,31 +185,11 @@ func (b *CollisionBox) SetLength(value float64) { b.length = value } -func NewCollisionSphere() *CollisionSphere { - return &CollisionSphere{ - translation: dprec.ZeroVec3(), - } -} - -type CollisionSphere struct { +type CollisionMesh struct { + CollisionShape translation dprec.Vec3 - radius float64 -} - -func (s *CollisionSphere) Translation() dprec.Vec3 { - return s.translation -} - -func (s *CollisionSphere) SetTranslation(value dprec.Vec3) { - s.translation = value -} - -func (s *CollisionSphere) Radius() float64 { - return s.radius -} - -func (s *CollisionSphere) SetRadius(value float64) { - s.radius = value + rotation dprec.Quat + triangles []CollisionTriangle } func NewCollisionMesh() *CollisionMesh { @@ -208,12 +199,6 @@ func NewCollisionMesh() *CollisionMesh { } } -type CollisionMesh struct { - translation dprec.Vec3 - rotation dprec.Quat - triangles []CollisionTriangle -} - func (m *CollisionMesh) Translation() dprec.Vec3 { return m.translation } @@ -248,22 +233,7 @@ type CollisionTriangle struct { C dprec.Vec3 } -func NewBody(definition *BodyDefinition) *Body { - return &Body{ - Object: NewObject(), - definition: definition, - } -} - -type Body struct { - *Object - definition *BodyDefinition -} - -func (b *Body) Definition() *BodyDefinition { - return b.definition -} - +// TODO: Move somewhere else. type Placed[T any] struct { Node *Node Value T diff --git a/game/asset_mesh.go b/game/asset_mesh.go index 4f19934c..271d5587 100644 --- a/game/asset_mesh.go +++ b/game/asset_mesh.go @@ -6,9 +6,11 @@ import ( "github.com/mokiat/gog" "github.com/mokiat/gog/opt" "github.com/mokiat/gomath/sprec" + "github.com/mokiat/lacking/core/spatial/shape3d" "github.com/mokiat/lacking/game/asset/dto" "github.com/mokiat/lacking/game/graphics" "github.com/mokiat/lacking/game/hierarchy" + "github.com/mokiat/lacking/game/physics" "github.com/mokiat/lacking/render" "golang.org/x/sync/errgroup" ) @@ -91,6 +93,25 @@ func UnloadArmatureTemplates(loader *AssetLoader, idTemplates IdentifiableList[A return nil } +func InstantiatePhysicsTerrainTemplate(scene *Scene, template TerrainTemplate, nodes IdentifiableList[hierarchy.NodeID]) { + nodeID := nodes.GetByID(template.NodeID) + absoluteMatrix := scene.Hierarchy().NodeAbsoluteMatrix(nodeID) + + terrain := scene.Physics().Terrains().CreateHandle() + for _, colMesh := range template.CollisionMeshes { + transform := shape3d.Transform{ + Translation: absoluteMatrix.Translation(), + Rotation: shape3d.RotationFromQuat(absoluteMatrix.Rotation()), + } + terrain.AttachCollisionMesh(physics.CollisionMesh{ + Shape: shape3d.TransformedMesh(colMesh.Shape, transform), + FrictionCoefficient: colMesh.FrictionCoefficient, + RestitutionCoefficient: colMesh.RestitutionCoefficient, + Filtering: colMesh.Filtering, + }) + } +} + // InstantiateArmatureTemplate creates an armature in the given scene from the // provided armature template. // diff --git a/game/asset_model.go b/game/asset_model.go index 1a9f7f87..3ae50c73 100644 --- a/game/asset_model.go +++ b/game/asset_model.go @@ -17,17 +17,15 @@ import ( // ModelTemplate represents a template for a model that can be instantiated // in a Scene. type ModelTemplate struct { - Recordings IdentifiableList[*animation.Recording] - Shaders IdentifiableList[*graphics.Shader] - Textures IdentifiableList[render.Texture] - Materials IdentifiableList[*graphics.Material] - // BodyMaterials IdentifiableList[*physics.Material] - // BodyDefinitions IdentifiableList[*physics.BodyDefinition] + Recordings IdentifiableList[*animation.Recording] + Shaders IdentifiableList[*graphics.Shader] + Textures IdentifiableList[render.Texture] + Materials IdentifiableList[*graphics.Material] MeshGeometries IdentifiableList[*graphics.MeshGeometry] MeshDefinitions IdentifiableList[*graphics.MeshDefinition] - Nodes IdentifiableList[NodeTemplate] - // Bodies IdentifiableList[BodyTemplate] + Nodes IdentifiableList[NodeTemplate] + Terrains IdentifiableList[TerrainTemplate] Armatures IdentifiableList[ArmatureTemplate] Meshes IdentifiableList[MeshTemplate] AmbientLights IdentifiableList[AmbientLightTemplate] @@ -70,16 +68,6 @@ func LoadModelTemplate(loader *AssetLoader, assetModel dto.Model) (*ModelTemplat return nil, fmt.Errorf("failed to resolve materials: %w", err) } - // bodyMaterials, err := LoadPhysicsMaterials(loader, assetModel.PhysicsChunk.BodyMaterials) - // if err != nil { - // return nil, fmt.Errorf("failed to resolve body materials: %w", err) - // } - - // bodyDefinitions, err := LoadPhysicsBodyDefinitions(loader, assetModel.PhysicsChunk.BodyDefinitions, bodyMaterials) - // if err != nil { - // return nil, fmt.Errorf("failed to resolve body definitions: %w", err) - // } - meshGeometries, err := LoadMeshGeometries(loader, assetModel.MeshChunk.Geometries) if err != nil { return nil, fmt.Errorf("failed to resolve mesh geometries: %w", err) @@ -95,10 +83,10 @@ func LoadModelTemplate(loader *AssetLoader, assetModel dto.Model) (*ModelTemplat return nil, fmt.Errorf("failed to resolve node templates: %w", err) } - // bodies, err := LoadPhysicsBodyTemplates(loader, assetModel.PhysicsChunk.Bodies, bodyDefinitions) - // if err != nil { - // return nil, fmt.Errorf("failed to resolve physics body templates: %w", err) - // } + terrains, err := LoadPhysicsTerrainTemplates(loader, assetModel.PhysicsChunk.Terrains) + if err != nil { + return nil, fmt.Errorf("failed to resolve physics terrain templates: %w", err) + } armatures, err := LoadArmatureTemplates(loader, assetModel.MeshChunk.Armatures) if err != nil { @@ -138,17 +126,15 @@ func LoadModelTemplate(loader *AssetLoader, assetModel dto.Model) (*ModelTemplat } return &ModelTemplate{ - Recordings: recordings, - Shaders: shaders, - Textures: textures, - Materials: materials, - // BodyMaterials: bodyMaterials, - // BodyDefinitions: bodyDefinitions, + Recordings: recordings, + Shaders: shaders, + Textures: textures, + Materials: materials, MeshGeometries: meshGeometries, MeshDefinitions: meshDefinitions, - Nodes: nodes, - // Bodies: bodies, + Nodes: nodes, + Terrains: terrains, Armatures: armatures, Meshes: meshes, AmbientLights: ambientLights, @@ -168,13 +154,10 @@ func UnloadModelTemplate(loader *AssetLoader, template *ModelTemplate) error { UnloadShaders(loader, template.Shaders), UnloadTextures(loader, template.Textures), UnloadMaterials(loader, template.Materials), - // UnloadPhysicsMaterials(loader, template.BodyMaterials), - // UnloadPhysicsBodyDefinitions(loader, template.BodyDefinitions), UnloadMeshGeometries(loader, template.MeshGeometries), UnloadMeshDefinitions(loader, template.MeshDefinitions), UnloadNodeTemplates(loader, template.Nodes), - // UnloadPhysicsBodyTemplates(loader, template.Bodies), UnloadArmatureTemplates(loader, template.Armatures), UnloadMeshTemplates(loader, template.Meshes), UnloadAmbientLightTemplates(loader, template.AmbientLights), @@ -315,15 +298,11 @@ func InstantiateModel(scene *Scene, info ModelInfo) *Model { recordings := definition.Recordings meshDefinitions := definition.MeshDefinitions - // for template := range definition.Bodies.Values() { - // if nodes.HasID(template.NodeID) { - // if info.IsDynamic { - // InstantiatePhysicsBodyTemplateDynamic(scene, template, nodes) - // } else { - // InstantiatePhysicsBodyTemplateStatic(scene, template, nodes) - // } - // } - // } + for template := range definition.Terrains.Values() { + if nodes.HasID(template.NodeID) { + InstantiatePhysicsTerrainTemplate(scene, template, nodes) + } + } armatures := make(IdentifiableList[*graphics.Armature], 0, len(definition.Armatures)) for id, template := range definition.Armatures.Iter() { diff --git a/game/asset_physics.go b/game/asset_physics.go index c92e18c3..7adbe86b 100644 --- a/game/asset_physics.go +++ b/game/asset_physics.go @@ -1,298 +1,63 @@ package game -// import ( -// "fmt" - -// "github.com/mokiat/gog/opt" -// "github.com/mokiat/lacking/core/spatial/shape3d" -// "github.com/mokiat/lacking/game/asset/dto" -// "github.com/mokiat/lacking/game/hierarchy" -// "github.com/mokiat/lacking/game/physics" -// "golang.org/x/sync/errgroup" -// ) - -// // LoadPhysicsMaterial loads a physics material from the given asset data. -// // -// // This is a blocking operation and should be called from a worker thread. -// func LoadPhysicsMaterial(loader *AssetLoader, assetMaterial dto.BodyMaterial) (Identifiable[*physics.Material], error) { -// materialInfo := physics.MaterialInfo{ -// FrictionCoefficient: assetMaterial.FrictionCoefficient, -// RestitutionCoefficient: assetMaterial.RestitutionCoefficient, -// } - -// var material *physics.Material -// allocateMaterial := func() error { -// material = physics.NewMaterial(materialInfo) -// return nil -// } -// if err := loader.ScheduleMain(allocateMaterial).Wait(); err != nil { -// return Identifiable[*physics.Material]{}, err -// } - -// return Identifiable[*physics.Material]{ -// ID: assetMaterial.ID, -// Value: material, -// }, nil -// } - -// // LoadPhysicsMaterials loads a list of physics materials from the given asset -// // materials. -// // -// // This is a blocking operation and should be called from a worker thread. -// func LoadPhysicsMaterials(loader *AssetLoader, assetMaterials []dto.BodyMaterial) (IdentifiableList[*physics.Material], error) { -// materials := make(IdentifiableList[*physics.Material], len(assetMaterials)) -// var group errgroup.Group -// for i, assetMaterial := range assetMaterials { -// group.Go(func() error { -// material, err := LoadPhysicsMaterial(loader, assetMaterial) -// materials[i] = material -// return err -// }) -// } -// return materials, group.Wait() -// } - -// // UnloadPhysicsMaterial unloads a physics material from the asset loader. -// // -// // This is a blocking operation and should be called from a worker thread. -// func UnloadPhysicsMaterial(loader *AssetLoader, idMaterial Identifiable[*physics.Material]) error { -// // At the time being this is a no-op. -// return nil -// } - -// // UnloadPhysicsMaterials unloads a list of physics materials from the asset -// // loader. -// // -// // This is a blocking operation and should be called from a worker thread. -// func UnloadPhysicsMaterials(loader *AssetLoader, idMaterials IdentifiableList[*physics.Material]) error { -// for _, idMaterial := range idMaterials { -// if err := UnloadPhysicsMaterial(loader, idMaterial); err != nil { -// return err -// } -// } -// return nil -// } - -// // LoadPhysicsBodyDefinition loads a physics body definition from the given -// // asset data. -// // -// // This is a blocking operation and should be called from a worker thread. -// func LoadPhysicsBodyDefinition(loader *AssetLoader, assetBodyDefinition dto.BodyDefinition, materials IdentifiableList[*physics.Material]) (Identifiable[*physics.BodyDefinition], error) { -// material, ok := materials.FindByID(assetBodyDefinition.MaterialID) -// if !ok { -// return Identifiable[*physics.BodyDefinition]{}, fmt.Errorf("physics material with ID %d not found", assetBodyDefinition.MaterialID) -// } - -// bodyDefinitionInfo := physics.BodyDefinitionInfo{ -// Mass: assetBodyDefinition.Mass, -// MomentOfInertia: assetBodyDefinition.MomentOfInertia, -// FrictionCoefficient: material.FrictionCoefficient(), -// RestitutionCoefficient: material.RestitutionCoefficient(), -// DragFactor: assetBodyDefinition.DragFactor, -// AngularDragFactor: assetBodyDefinition.AngularDragFactor, -// AerodynamicShapes: nil, // TODO -// CollisionSpheres: resolveCollisionSpheres(assetBodyDefinition), -// CollisionBoxes: resolveCollisionBoxes(assetBodyDefinition), -// CollisionMeshes: resolveCollisionMeshes(assetBodyDefinition), -// } - -// var bodyDefinition *physics.BodyDefinition -// allocateDefinition := func() error { -// bodyDefinition = physics.NewBodyDefinition(bodyDefinitionInfo) -// return nil -// } -// if err := loader.ScheduleMain(allocateDefinition).Wait(); err != nil { -// return Identifiable[*physics.BodyDefinition]{}, err -// } - -// return Identifiable[*physics.BodyDefinition]{ -// ID: assetBodyDefinition.ID, -// Value: bodyDefinition, -// }, nil -// } - -// // LoadPhysicsBodyDefinitions loads a list of physics body definitions from the -// // given asset body definitions. -// // -// // This is a blocking operation and should be called from a worker thread. -// func LoadPhysicsBodyDefinitions(loader *AssetLoader, assetBodyDefinitions []dto.BodyDefinition, materials IdentifiableList[*physics.Material]) (IdentifiableList[*physics.BodyDefinition], error) { -// bodyDefinitions := make(IdentifiableList[*physics.BodyDefinition], len(assetBodyDefinitions)) -// var group errgroup.Group -// for i, assetBodyDefinition := range assetBodyDefinitions { -// group.Go(func() error { -// bodyDefinition, err := LoadPhysicsBodyDefinition(loader, assetBodyDefinition, materials) -// bodyDefinitions[i] = bodyDefinition -// return err -// }) -// } -// return bodyDefinitions, group.Wait() -// } - -// // UnloadPhysicsBodyDefinition unloads a physics body definition from the asset -// // loader. -// // -// // This is a blocking operation and should be called from a worker thread. -// func UnloadPhysicsBodyDefinition(loader *AssetLoader, idBodyDefinition Identifiable[*physics.BodyDefinition]) error { -// // At the time being this is a no-op. -// return nil -// } - -// // UnloadPhysicsBodyDefinitions unloads a list of physics body definitions from -// // the asset loader. -// // -// // This is a blocking operation and should be called from a worker thread. -// func UnloadPhysicsBodyDefinitions(loader *AssetLoader, idBodyDefinitions IdentifiableList[*physics.BodyDefinition]) error { -// for _, idBodyDefinition := range idBodyDefinitions { -// if err := UnloadPhysicsBodyDefinition(loader, idBodyDefinition); err != nil { -// return err -// } -// } -// return nil -// } - -// func resolveCollisionSpheres(bodyDef dto.BodyDefinition) []shape3d.Sphere { -// result := make([]shape3d.Sphere, len(bodyDef.CollisionSpheres)) -// for i, collisionSphereAsset := range bodyDef.CollisionSpheres { -// result[i] = shape3d.Sphere{ -// Center: collisionSphereAsset.Translation, -// Radius: collisionSphereAsset.Radius, -// } -// } -// return result -// } - -// func resolveCollisionBoxes(bodyDef dto.BodyDefinition) []shape3d.Box { -// result := make([]shape3d.Box, len(bodyDef.CollisionBoxes)) -// for i, collisionBoxAsset := range bodyDef.CollisionBoxes { -// result[i] = shape3d.Box{ -// Center: collisionBoxAsset.Translation, -// Rotation: shape3d.RotationFromQuat(collisionBoxAsset.Rotation), -// HalfWidth: collisionBoxAsset.Width / 2.0, -// HalfHeight: collisionBoxAsset.Height / 2.0, -// HalfLength: collisionBoxAsset.Length / 2.0, -// } -// } -// return result -// } - -// func resolveCollisionMeshes(bodyDef dto.BodyDefinition) []shape3d.Mesh { -// result := make([]shape3d.Mesh, len(bodyDef.CollisionMeshes)) -// for i, collisionMeshAsset := range bodyDef.CollisionMeshes { -// transform := shape3d.TRTransform( -// collisionMeshAsset.Translation, -// shape3d.RotationFromQuat(collisionMeshAsset.Rotation), -// ) -// triangles := make([]shape3d.Triangle, len(collisionMeshAsset.Triangles)) -// for j, triangleAsset := range collisionMeshAsset.Triangles { -// triangles[j] = shape3d.Triangle{ -// A: transform.Apply(triangleAsset.A), -// B: transform.Apply(triangleAsset.B), -// C: transform.Apply(triangleAsset.C), -// } -// } -// result[i] = shape3d.Mesh{ -// Triangles: triangles, -// } -// } -// return result -// } - -// // BodyTemplate represents a template for physics body that can be -// // instantiated in a scene. -// type BodyTemplate struct { -// NodeID uint32 -// Definition *physics.BodyDefinition -// } - -// // LoadPhysicsBodyTemplate resolves a physics body template from the given asset -// // data. -// // -// // This is a blocking operation and should be called from a worker thread. -// func LoadPhysicsBodyTemplate(loader *AssetLoader, assetBody dto.Body, bodyDefinitions IdentifiableList[*physics.BodyDefinition]) (Identifiable[BodyTemplate], error) { -// bodyDefinition, ok := bodyDefinitions.FindByID(assetBody.BodyDefinitionID) -// if !ok { -// return Identifiable[BodyTemplate]{}, fmt.Errorf("body definition with ID %d not found", assetBody.BodyDefinitionID) -// } -// return Identifiable[BodyTemplate]{ -// ID: assetBody.ID, -// Value: BodyTemplate{ -// NodeID: assetBody.NodeID, -// Definition: bodyDefinition, -// }, -// }, nil -// } - -// // LoadPhysicsBodyTemplates resolves a list of physics body templates from the -// // given asset bodies. -// // -// // This is a blocking operation and should be called from a worker thread. -// func LoadPhysicsBodyTemplates(loader *AssetLoader, assetBodies []dto.Body, bodyDefinitions IdentifiableList[*physics.BodyDefinition]) (IdentifiableList[BodyTemplate], error) { -// bodyTemplates := make(IdentifiableList[BodyTemplate], len(assetBodies)) -// for i, assetBody := range assetBodies { -// template, err := LoadPhysicsBodyTemplate(loader, assetBody, bodyDefinitions) -// if err != nil { -// return IdentifiableList[BodyTemplate]{}, err -// } -// bodyTemplates[i] = template -// } -// return bodyTemplates, nil -// } - -// // UnloadPhysicsBodyTemplate unloads a physics body template from the asset -// // loader. -// // -// // This is a blocking operation and should be called from a worker thread. -// func UnloadPhysicsBodyTemplate(loader *AssetLoader, idBody Identifiable[BodyTemplate]) error { -// // At the time being this is a no-op. -// return nil -// } - -// // UnloadPhysicsBodyTemplates unloads a list of physics body templates from the -// // asset loader. -// // -// // This is a blocking operation and should be called from a worker thread. -// func UnloadPhysicsBodyTemplates(loader *AssetLoader, idBodies IdentifiableList[BodyTemplate]) error { -// for _, idBody := range idBodies { -// if err := UnloadPhysicsBodyTemplate(loader, idBody); err != nil { -// return err -// } -// } -// return nil -// } - -// // InstantiatePhysicsBodyTemplateStatic creates a static physics body in the -// // given scene from the provided body template. -// // -// // This operation needs to be called from the main thread. -// func InstantiatePhysicsBodyTemplateStatic(scene *Scene, template BodyTemplate, nodes IdentifiableList[hierarchy.NodeID]) { -// node := nodes.GetByID(template.NodeID) -// absMatrix := scene.Hierarchy().NodeAbsoluteMatrix(node) -// nodeName := scene.Hierarchy().NodeName(node) -// scene.physicsScene.CreateProp(physics.PropInfo{ -// Name: nodeName, -// Position: opt.V(absMatrix.Translation()), -// Rotation: opt.V(absMatrix.Rotation()), -// CollisionSpheres: template.Definition.CollisionSpheres(), -// CollisionBoxes: template.Definition.CollisionBoxes(), -// CollisionMeshes: template.Definition.CollisionMeshes(), -// }) -// } - -// // InstantiatePhysicsBodyTemplateDynamic creates a dynamic physics body in the -// // given scene from the provided body template and returns it. -// // -// // This operation needs to be called from the main thread. -// func InstantiatePhysicsBodyTemplateDynamic(scene *Scene, template BodyTemplate, nodes IdentifiableList[hierarchy.NodeID]) physics.Body { -// node := nodes.GetByID(template.NodeID) -// absMatrix := scene.Hierarchy().NodeAbsoluteMatrix(node) -// nodeName := scene.Hierarchy().NodeName(node) -// translation, rotation, _ := absMatrix.TRS() -// body := scene.physicsScene.CreateBody(physics.BodyInfo{ -// Name: nodeName, -// Definition: template.Definition, -// Position: translation, -// Rotation: rotation, -// }) -// scene.bodyBindingSet.Bind(node, body) -// return body -// } +import ( + "github.com/mokiat/gog" + "github.com/mokiat/lacking/core/spatial/placement3d" + "github.com/mokiat/lacking/core/spatial/shape3d" + "github.com/mokiat/lacking/game/asset/dto" + "github.com/mokiat/lacking/game/physics" +) + +type TerrainTemplate struct { + NodeID uint32 + + CollisionMeshes []physics.CollisionMesh +} + +// LoadPhysicsTerrainTemplate resolves a physics body template from the given asset +// data. +// +// This is a blocking operation and should be called from a worker thread. +func LoadPhysicsTerrainTemplate(loader *AssetLoader, assetTerrain dto.PhysicsTerrain) (Identifiable[TerrainTemplate], error) { + return Identifiable[TerrainTemplate]{ + ID: assetTerrain.ID, + Value: TerrainTemplate{ + NodeID: assetTerrain.NodeID, + CollisionMeshes: gog.Map(assetTerrain.CollisionMeshes, func(collisionMesh dto.CollisionMesh) physics.CollisionMesh { + transform := shape3d.TRTransform( + collisionMesh.Translation, + shape3d.RotationFromQuat(collisionMesh.Rotation), + ) + triangles := gog.Map(collisionMesh.Triangles, func(triangle dto.CollisionTriangle) shape3d.Triangle { + return shape3d.Triangle{ + A: transform.Apply(triangle.A), + B: transform.Apply(triangle.B), + C: transform.Apply(triangle.C), + } + }) + return physics.CollisionMesh{ + Shape: shape3d.NewMesh(triangles), + FrictionCoefficient: collisionMesh.FrictionCoefficient, + RestitutionCoefficient: collisionMesh.RestitutionCoefficient, + Filtering: placement3d.FilterInfo{}, + } + }), + }, + }, nil +} + +// LoadPhysicsTerrainTemplates resolves a list of physics body templates from the +// given asset bodies. +// +// This is a blocking operation and should be called from a worker thread. +func LoadPhysicsTerrainTemplates(loader *AssetLoader, assetTerrains []dto.PhysicsTerrain) (IdentifiableList[TerrainTemplate], error) { + terrainTemplates := make(IdentifiableList[TerrainTemplate], len(assetTerrains)) + for i, assetTerrain := range assetTerrains { + template, err := LoadPhysicsTerrainTemplate(loader, assetTerrain) + if err != nil { + return IdentifiableList[TerrainTemplate]{}, err + } + terrainTemplates[i] = template + } + return terrainTemplates, nil +}