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_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/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..327b4a3a 100644 --- a/core/spatial/placement2d/doc.go +++ b/core/spatial/placement2d/doc.go @@ -1,12 +1,31 @@ -// 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. 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 +// or a query primitive. package placement2d diff --git a/core/spatial/placement2d/filter.go b/core/spatial/placement2d/filter.go index 94800c72..1dc96250 100644 --- a/core/spatial/placement2d/filter.go +++ b/core/spatial/placement2d/filter.go @@ -2,38 +2,61 @@ 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 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. 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, +// which is the same behavior as that of the zero mask. +const FullMask Mask = 0xFFFFFFFF - // Mask is a bitmask used to filter shapes based on their assigned layers. - Mask opt.T[uint32] +// Filter narrows down the shapes that a query considers. +// +// Its zero value considers every shape in the scene. +type Filter struct { - // SkipDynamic indicates whether dynamic shapes should be excluded from the - // results. - SkipDynamic bool + // 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 - // SkipStatic indicates whether static shapes should be excluded from the - // results. - SkipStatic bool + // 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 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 +74,20 @@ 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 - } +// 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 +// 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 c07d81ab..00000000 --- a/core/spatial/placement2d/mesh.go +++ /dev/null @@ -1,69 +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. - 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) -} - -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 -} - -func newMeshRepresentation(mesh shape2d.Mesh) meshRepresentation { - return meshRepresentation{ - wsBCircle: mesh.BoundingCircle(), - wsEdges: mesh.Edges, - } -} - -func (s *meshRepresentation) boundingCircle() shape2d.Circle { - return s.wsBCircle -} 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 83ff37db..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,11 +90,7 @@ func (s *shapeRepresentation) update(parentTransform shape2d.Transform) { ) } -func (s *shapeRepresentation) boundingCircle() shape2d.Circle { - return s.wsBCircle -} - -func (s *shapeRepresentation) gjkShape() gjk2d.Shape { +func (s *objectShapeRepresentation) gjkShape() gjk2d.Shape { return gjk2d.Shape{ Position: s.wsTransform.Translation, Rotation: s.wsTransform.Rotation, @@ -90,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) @@ -111,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 4af71e52..4a3459ce 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) - bc := shape.boundingCircle() - s.shapeTree.Update(shape.spatialID, query2d.AreaFromCircle(bc)) + area := shape2d.AABBFromCircle(shape.wsBCircle) + 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,43 @@ 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] +// filter and yields them, in world space, to the provided callback. Iteration +// stops early if the callback returns false. +// +// 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 { continue } - if shape.kind != shapeKindCircle { + if shape.kind != objectShapeKindCircle { continue } - if !shape.matchesFilter(filter) { + if !shape.satisfiesFilter(filter) { continue } if !yield(shape.toCircle()) { @@ -252,29 +276,29 @@ 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 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(filter, 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] +// filter and yields them, in world space, to the provided callback. Iteration +// stops early if the callback returns false. +// +// 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 { continue } - if shape.kind != shapeKindRectangle { + if shape.kind != objectShapeKindRectangle { continue } - if !shape.matchesFilter(filter) { + if !shape.satisfiesFilter(filter) { continue } if !yield(shape.toRectangle()) { @@ -283,262 +307,385 @@ 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 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(filter, yield) } } -// CreateMesh creates a new static mesh in the scene. +// CreateTerrain creates a new terrain. // -// 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. -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)), - ), +// 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, } - representation := newMeshRepresentation(shape2d.TransformedMesh(info.Mesh, transform)) - area := query2d.AreaFromCircle(representation.boundingCircle()) + return TerrainID(index) +} - index := s.allocateMesh() - s.meshes[index] = meshShape[M]{ - spatialID: s.meshTree.Insert(area, index), - filterRepresentation: newFilterRepresentation(info.Filtering), - meshRepresentation: representation, - userData: info.UserData, - } +// 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) +} - return MeshID(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 } -// 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) +// 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 } -// 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 +// 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) } -// 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 +// AttachMesh creates a mesh shape and attaches it to the terrain to be used +// for intersection tests. +// +// 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, T, S]) AttachMesh(terrainID TerrainID, info MeshInfo[S]) TerrainShapeID { + index := int32(terrainID) + + mesh := info.Mesh + bCircle := mesh.BoundingCircle() + aabb := mesh.BoundingAABB() + + return s.attachTerrainShape(index, info.Filtering, terrainShapeRepresentation{ + wsBCircle: bCircle, + wsAABB: aabb, + wsEdges: mesh.Edges, + }, info.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) { - querySegment := query2d.NewSegment(segment.A, segment.B) +// 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) +} - if !filter.SkipDynamic { - s.shapeCandidates = s.shapeCandidates[:0] - s.shapeTree.QuerySegment(querySegment, func(index int32) bool { - s.shapeCandidates = append(s.shapeCandidates, index) - return true - }) - s.collectSegmentShape(segment, filter, yield) - } +// 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 +} - if !filter.SkipStatic { - s.meshCandidates = s.meshCandidates[:0] - s.meshTree.QuerySegment(querySegment, func(index int32) bool { - s.meshCandidates = append(s.meshCandidates, index) - return true - }) - s.collectSegmentMesh(segment, filter, yield) - } +// 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 +} + +// CollectSegmentObjectIntersections collects all intersections of the segment +// 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, 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, filter, 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) +// CheckSegmentObjectIntersection returns the intersection of the segment with +// 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, filter, 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) { - queryAABB := query2d.AABBFromCircle(circle) +// CollectSegmentTerrainIntersections collects all intersections of the segment +// 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, 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, filter, yield) +} - 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) - } +// CheckSegmentTerrainIntersection returns the intersection of the segment with +// 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, filter, collection.AddContact) + return collection.Contact() +} - 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) - } +// CollectCircleObjectIntersections collects all intersections of the circle +// 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, filter Filter, yield ObjectContactCallback) { + queryAABB := shape2d.AABBFromCircle(circle) + + s.objectShapeCandidates = s.objectShapeCandidates[:0] + s.objectShapeTree.QueryAABB(queryAABB, func(index int32) bool { + s.objectShapeCandidates = append(s.objectShapeCandidates, index) + return true + }) + s.collectCircleObject(circle, filter, 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) +// CheckCircleObjectIntersection returns the deepest intersection of the circle +// 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, filter, 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) { - queryAABB := query2d.AABBFromRectangle(rectangle) +// CollectCircleTerrainIntersections collects all intersections of the circle +// 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, filter Filter, 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, filter, yield) +} - 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) - } +// CheckCircleTerrainIntersection returns the deepest intersection of the +// 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, filter, collection.AddContact) + return collection.Contact() +} - 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) - } +// CollectRectangleObjectIntersections collects all intersections of the +// 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, filter Filter, yield ObjectContactCallback) { + queryAABB := shape2d.AABBFromRectangle(rectangle) + + s.objectShapeCandidates = s.objectShapeCandidates[:0] + s.objectShapeTree.QueryAABB(queryAABB, func(index int32) bool { + s.objectShapeCandidates = append(s.objectShapeCandidates, index) + return true + }) + s.collectRectangleObject(rectangle, filter, 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) +// CheckRectangleObjectIntersection returns the deepest intersection of the +// 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, filter, 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 { +// CollectRectangleTerrainIntersections collects all intersections of the +// 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, filter Filter, 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, filter, yield) +} + +// CheckRectangleTerrainIntersection returns the deepest intersection of the +// 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, filter, collection.AddContact) + return collection.Contact() +} + +// 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 := query2d.AABBFromCircle(srcShape.boundingCircle()) + 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 := query2d.AreaFromCircle(representation.boundingCircle()) - - 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, + area := shape2d.AABBFromCircle(representation.wsBCircle) + + 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] @@ -549,185 +696,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, S, M]) releaseMesh(index int32) { - s.freeMeshIndices.Push(index) +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]) collectSegmentShape(segment shape2d.Segment, filter Filter, yield ContactCallback) { - for index, shape := range s.iterCandidateShape(filter) { +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, T, S]) collectSegmentObject(segment shape2d.Segment, filter Filter, yield ObjectContactCallback) { + for index, shape := range s.iterCandidateObjectShapes(filter) { 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, filter Filter, yield TerrainContactCallback) { + for index, shape := range s.iterCandidateTerrainShapes(filter) { + 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, filter Filter, yield ObjectContactCallback) { initGJKShapeForCircle(circle, &s.tempGJKSource) - for index, shape := range s.iterCandidateShape(filter) { + for index, shape := range s.iterCandidateObjectShapes(filter) { 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, filter Filter, 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(filter) { + 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, filter Filter, yield ObjectContactCallback) { initGJKShapeForRectangle(rectangle, &s.tempGJKSource) - for index, shape := range s.iterCandidateShape(filter) { + for index, shape := range s.iterCandidateObjectShapes(filter) { 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, filter Filter, 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(filter) { + 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 @@ -746,10 +990,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(filter Filter, cb func(int32, *objectShapeState[S]) bool) { + for _, index := range s.objectShapeCandidates { + shape := &s.objectShapes[index] + if !shape.satisfiesFilter(filter) { continue } if !cb(index, shape) { @@ -758,27 +1002,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(filter Filter) iter.Seq2[int32, *objectShapeState[S]] { + return func(yield func(int32, *objectShapeState[S]) bool) { + s.eachCandidateObjectShape(filter, 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(filter Filter, cb func(int32, *terrainShapeState[S]) bool) { + for _, index := range s.terrainShapeCandidates { + shape := &s.terrainShapes[index] + if !shape.satisfiesFilter(filter) { 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(filter Filter) iter.Seq2[int32, *terrainShapeState[S]] { + return func(yield func(int32, *terrainShapeState[S]) bool) { + s.eachCandidateTerrainShape(filter, yield) } } diff --git a/core/spatial/placement2d/scene_test.go b/core/spatial/placement2d/scene_test.go index 68d79ae3..b3d6a12d 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 @@ -121,11 +174,7 @@ 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 @@ -148,29 +197,41 @@ var _ = Describe("Scene", func() { 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.Filter{}) { + count++ + } + Expect(count).To(Equal(1)) + }) + It("stores and updates shape user data", func() { shapeID := scene.AttachCircle(objID, placement2d.CircleInfo[string]{ 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 { @@ -220,32 +281,58 @@ var _ = Describe("Scene", func() { }) Describe("shape iteration filters", func() { - It("filters by layer mask", 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), }) + }) - matching := 0 - scene.EachCircle(placement2d.Filter{Mask: opt.V(uint32(0b01))}, func(shape2d.Circle) bool { - matching++ + countCircles := func(filter placement2d.Filter) int { + count := 0 + scene.EachCircle(filter, 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(placement2d.Filter{Mask: 0b01})).To(Equal(1)) + }) + + It("skips shapes that occupy no layer of the mask", func() { + 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 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)) }) }) - 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 +351,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 +362,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 +466,472 @@ 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()) + }) + + 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)) - var contacts placement2d.ContactList - scene.CollectIntersections(contacts.AddContact) + 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{}, ) 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{}, ) 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), + 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()) + }) + + 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{}, ) 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{}, ) 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.Filter{}, ) 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{}, ) 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{}, ) 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{}, ) 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}, - ) - 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,8 +941,8 @@ 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), @@ -652,16 +952,14 @@ var _ = Describe("Scene", func() { ) 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), @@ -669,70 +967,79 @@ var _ = Describe("Scene", func() { placement2d.Filter{}, ) 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{}, ) 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{}, ) - 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.Filter{}, ) 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.Filter{}, ) 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 +} 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_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/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..a44ed0e5 100644 --- a/core/spatial/placement3d/doc.go +++ b/core/spatial/placement3d/doc.go @@ -1,12 +1,31 @@ -// 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. 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 +// or a query primitive. package placement3d diff --git a/core/spatial/placement3d/filter.go b/core/spatial/placement3d/filter.go index eabf07ca..4b2eae86 100644 --- a/core/spatial/placement3d/filter.go +++ b/core/spatial/placement3d/filter.go @@ -2,38 +2,61 @@ 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 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. 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, +// which is the same behavior as that of the zero mask. +const FullMask Mask = 0xFFFFFFFF - // Mask is a bitmask used to filter shapes based on their assigned layers. - Mask opt.T[uint32] +// Filter narrows down the shapes that a query considers. +// +// Its zero value considers every shape in the scene. +type Filter struct { - // SkipDynamic indicates whether dynamic shapes should be excluded from the - // results. - SkipDynamic bool + // 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 - // SkipStatic indicates whether static shapes should be excluded from the - // results. - SkipStatic bool + // 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 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 +74,20 @@ 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 - } +// 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 +// 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 e89538dc..00000000 --- a/core/spatial/placement3d/mesh.go +++ /dev/null @@ -1,69 +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. - 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) -} - -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. - wsTriangles []shape3d.Triangle -} - -func newMeshRepresentation(mesh shape3d.Mesh) meshRepresentation { - return meshRepresentation{ - wsBSphere: mesh.BoundingSphere(), - wsTriangles: mesh.Triangles, - } -} - -func (s *meshRepresentation) boundingSphere() shape3d.Sphere { - return s.wsBSphere -} 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 535f2eb8..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,11 +88,7 @@ func (s *shapeRepresentation) update(parentTransform shape3d.Transform) { ) } -func (s *shapeRepresentation) boundingSphere() shape3d.Sphere { - return s.wsBSphere -} - -func (s *shapeRepresentation) gjkShape() gjk3d.Shape { +func (s *objectShapeRepresentation) gjkShape() gjk3d.Shape { return gjk3d.Shape{ Position: s.wsTransform.Translation, Rotation: s.wsTransform.Rotation, @@ -90,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) @@ -113,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 c4c6175c..7b752d41 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) - bs := shape.boundingSphere() - s.shapeTree.Update(shape.spatialID, query3d.AreaFromSphere(bs)) + area := shape3d.AABBFromSphere(shape.wsBSphere) + 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,43 @@ 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] +// filter and yields them, in world space, to the provided callback. Iteration +// stops early if the callback returns false. +// +// 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 { continue } - if shape.kind != shapeKindSphere { + if shape.kind != objectShapeKindSphere { continue } - if !shape.matchesFilter(filter) { + if !shape.satisfiesFilter(filter) { continue } if !yield(shape.toSphere()) { @@ -257,29 +281,29 @@ 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 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(filter, 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 filter and +// yields them, in world space, to the provided callback. Iteration stops early +// if the callback returns false. +// +// 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 { continue } - if shape.kind != shapeKindBox { + if shape.kind != objectShapeKindBox { continue } - if !shape.matchesFilter(filter) { + if !shape.satisfiesFilter(filter) { continue } if !yield(shape.toBox()) { @@ -288,262 +312,385 @@ 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 +// 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(filter, yield) } } -// CreateMesh creates a new static mesh in the scene. +// CreateTerrain creates a new terrain. // -// 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. -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()), - ), +// 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, } - representation := newMeshRepresentation(shape3d.TransformedMesh(info.Mesh, transform)) - area := query3d.AreaFromSphere(representation.boundingSphere()) + return TerrainID(index) +} - index := s.allocateMesh() - s.meshes[index] = meshShape[M]{ - spatialID: s.meshTree.Insert(area, index), - filterRepresentation: newFilterRepresentation(info.Filtering), - meshRepresentation: representation, - userData: info.UserData, - } +// 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) +} - return MeshID(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 } -// 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) +// 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 } -// 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 +// 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) } -// 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 +// AttachMesh creates a mesh shape and attaches it to the terrain to be used +// for intersection tests. +// +// 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, T, S]) AttachMesh(terrainID TerrainID, info MeshInfo[S]) TerrainShapeID { + index := int32(terrainID) + + mesh := info.Mesh + bSphere := mesh.BoundingSphere() + aabb := mesh.BoundingAABB() + + return s.attachTerrainShape(index, info.Filtering, terrainShapeRepresentation{ + wsBSphere: bSphere, + wsAABB: aabb, + wsTriangles: mesh.Triangles, + }, info.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) { - querySegment := query3d.NewSegment(segment.A, segment.B) +// 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) +} - if !filter.SkipDynamic { - s.shapeCandidates = s.shapeCandidates[:0] - s.shapeTree.QuerySegment(querySegment, func(index int32) bool { - s.shapeCandidates = append(s.shapeCandidates, index) - return true - }) - s.collectSegmentShape(segment, filter, yield) - } +// 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 +} - if !filter.SkipStatic { - s.meshCandidates = s.meshCandidates[:0] - s.meshTree.QuerySegment(querySegment, func(index int32) bool { - s.meshCandidates = append(s.meshCandidates, index) - return true - }) - s.collectSegmentMesh(segment, filter, yield) - } +// 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 } -// 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) +// CollectSegmentObjectIntersections collects all intersections of the segment +// 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, 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, filter, yield) +} + +// CheckSegmentObjectIntersection returns the intersection of the segment with +// 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, filter, 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) { - queryAABB := query3d.AABBFromSphere(sphere) +// CollectSegmentTerrainIntersections collects all intersections of the segment +// 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, 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, filter, yield) +} - 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) - } +// CheckSegmentTerrainIntersection returns the intersection of the segment with +// 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, filter, collection.AddContact) + return collection.Contact() +} - 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) - } +// CollectSphereObjectIntersections collects all intersections of the sphere +// 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, filter Filter, yield ObjectContactCallback) { + queryAABB := shape3d.AABBFromSphere(sphere) + + s.objectShapeCandidates = s.objectShapeCandidates[:0] + s.objectShapeTree.QueryAABB(queryAABB, func(index int32) bool { + s.objectShapeCandidates = append(s.objectShapeCandidates, index) + return true + }) + s.collectSphereObject(sphere, filter, 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) +// CheckSphereObjectIntersection returns the deepest intersection of the sphere +// 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, filter, 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) { - queryAABB := query3d.AABBFromBox(box) +// CollectSphereTerrainIntersections collects all intersections of the sphere +// 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, filter Filter, 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, filter, yield) +} - 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) - } +// CheckSphereTerrainIntersection returns the deepest intersection of the +// 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, filter, collection.AddContact) + return collection.Contact() +} - 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) - } +// CollectBoxObjectIntersections collects all intersections of the box with the +// 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, filter Filter, yield ObjectContactCallback) { + queryAABB := shape3d.AABBFromBox(box) + + s.objectShapeCandidates = s.objectShapeCandidates[:0] + s.objectShapeTree.QueryAABB(queryAABB, func(index int32) bool { + s.objectShapeCandidates = append(s.objectShapeCandidates, index) + return true + }) + s.collectBoxObject(box, filter, yield) +} + +// CheckBoxObjectIntersection returns the deepest intersection of the box with +// 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, filter, collection.AddContact) + return collection.Contact() +} + +// CollectBoxTerrainIntersections collects all intersections of the box 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 box is not part of the +// scene. +func (s *Scene[O, T, S]) CollectBoxTerrainIntersections(box shape3d.Box, filter Filter, 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, filter, 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. 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, filter, 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 := query3d.AABBFromSphere(srcShape.boundingSphere()) + 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) + } +} - s.meshCandidates = s.meshCandidates[:0] - s.meshTree.QueryAABB(queryAABB, func(tgtIndex int32) bool { - s.meshCandidates = append(s.meshCandidates, tgtIndex) +// 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.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 := query3d.AreaFromSphere(representation.boundingSphere()) - - 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, + area := shape3d.AABBFromSphere(representation.wsBSphere) + + 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] @@ -554,185 +701,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, 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]) allocateTerrain() int32 { + if s.freeTerrainIndices.IsEmpty() { + index := len(s.terrains) + s.terrains = append(s.terrains, terrainState[T]{}) return int32(index) } else { - return s.freeMeshIndices.Pop() + return s.freeTerrainIndices.Pop() } } -func (s *Scene[O, S, M]) releaseMesh(index int32) { - s.freeMeshIndices.Push(index) +func (s *Scene[O, T, S]) releaseTerrain(index int32) { + s.freeTerrainIndices.Push(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]) 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, 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.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, 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, T, S]) collectSegmentObject(segment shape3d.Segment, filter Filter, yield ObjectContactCallback) { + for index, shape := range s.iterCandidateObjectShapes(filter) { 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, filter Filter, yield TerrainContactCallback) { + for index, shape := range s.iterCandidateTerrainShapes(filter) { + 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, filter Filter, yield ObjectContactCallback) { initGJKShapeForSphere(sphere, &s.tempGJKSource) - for index, shape := range s.iterCandidateShape(filter) { + for index, shape := range s.iterCandidateObjectShapes(filter) { 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, filter Filter, 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(filter) { + 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, filter Filter, yield ObjectContactCallback) { initGJKShapeForBox(box, &s.tempGJKSource) - for index, shape := range s.iterCandidateShape(filter) { + for index, shape := range s.iterCandidateObjectShapes(filter) { 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, filter Filter, 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(filter) { + 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 @@ -752,10 +996,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(filter Filter, cb func(int32, *objectShapeState[S]) bool) { + for _, index := range s.objectShapeCandidates { + shape := &s.objectShapes[index] + if !shape.satisfiesFilter(filter) { continue } if !cb(index, shape) { @@ -764,27 +1008,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(filter Filter) iter.Seq2[int32, *objectShapeState[S]] { + return func(yield func(int32, *objectShapeState[S]) bool) { + s.eachCandidateObjectShape(filter, 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(filter Filter, cb func(int32, *terrainShapeState[S]) bool) { + for _, index := range s.terrainShapeCandidates { + shape := &s.terrainShapes[index] + if !shape.satisfiesFilter(filter) { 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(filter Filter) iter.Seq2[int32, *terrainShapeState[S]] { + return func(yield func(int32, *terrainShapeState[S]) bool) { + s.eachCandidateTerrainShape(filter, yield) } } diff --git a/core/spatial/placement3d/scene_test.go b/core/spatial/placement3d/scene_test.go index 26880a55..73e35299 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 @@ -151,29 +204,41 @@ var _ = Describe("Scene", func() { 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.Filter{}) { + count++ + } + Expect(count).To(Equal(1)) + }) + It("stores and updates shape user data", func() { shapeID := scene.AttachSphere(objID, placement3d.SphereInfo[string]{ 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 { @@ -223,32 +288,60 @@ var _ = Describe("Scene", func() { }) Describe("shape iteration filters", func() { - It("filters by layer mask", func() { - objID := scene.CreateObject(placement3d.ObjectInfo[string]{}) + 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), }) + }) - matching := 0 - scene.EachSphere(placement3d.Filter{Mask: opt.V(uint32(0b01))}, func(shape3d.Sphere) bool { - matching++ + countSpheres := func(filter placement3d.Filter) int { + count := 0 + scene.EachSphere(filter, 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(placement3d.Filter{Mask: 0b01})).To(Equal(1)) + }) + + It("skips shapes that occupy no layer of the mask", func() { + 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 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)) }) }) - 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 +360,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 +371,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 +475,469 @@ 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) - var contacts placement3d.ContactList - scene.CollectIntersections(contacts.AddContact) + other := scene.CreateTerrain(placement3d.TerrainInfo[string]{}) + reusedID := scene.AttachMesh(other, placement3d.MeshInfo[string]{ + Mesh: planeMesh(0.0, 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.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{}, ) 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{}, ) Expect(ok).To(BeFalse()) }) - It("reports a sphere overlapping a mesh from the front", func() { - meshID := scene.CreateMesh(placement3d.MeshInfo[string]{ + 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), + 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()) + }) + + 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{}, ) 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{}, ) 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.Filter{}, ) 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{}, ) 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{}, ) 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{}, ) 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}, - ) - 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,8 +947,8 @@ 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), @@ -655,16 +958,14 @@ var _ = Describe("Scene", func() { ) 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), @@ -672,70 +973,79 @@ var _ = Describe("Scene", func() { placement3d.Filter{}, ) 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{}, ) 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{}, ) - 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.Filter{}, ) 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.Filter{}, ) 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 +} 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..f6cac3a3 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]. @@ -15,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. @@ -77,11 +80,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 +131,64 @@ 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 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") + } + + tightArea := newQuadtreeAABBFromAABB(aabb) + nodeIndex := t.pickNodeForItem(tightArea) 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) + tightArea := newQuadtreeAABBFromAABB(aabb) + item.tightArea = tightArea oldNodeIndex := item.node t.decreaseNodeItems(item.node) // previous node - item.node = t.pickNodeForItem(area) + item.node = t.pickNodeForItem(tightArea) t.increaseNodeItems(item.node) // new node t.gcNode(oldNodeIndex) } @@ -188,7 +210,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 +220,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 +270,9 @@ func (t *Quadtree[T]) itemsAtDepth(nodeIndex int32, currentDepth, depth uint32) return result } -func (t *Quadtree[T]) pickNodeForItem(area Area) 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 @@ -263,45 +287,55 @@ func (t *Quadtree[T]) pickNodeForItem(area Area) int32 { return bestNodeIndex } -func (t *Quadtree[T]) pickChildNode(parentNodeIndex int32, area Area) 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 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 - 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.r / 4.0 - if area.x < parentLooseArea.x { + childOffset := parentLooseArea.halfSize / 4.0 + 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] } 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 +463,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 +508,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 +546,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 +568,19 @@ 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. 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 - r float64 + x float64 + y float64 + halfSize float64 } 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..aa651466 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", ) }) @@ -75,17 +123,20 @@ 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, })) }) 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 +148,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,23 +199,23 @@ 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), ) }) 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, })) }) 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) @@ -195,17 +246,17 @@ 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, })) }) 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 +266,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) @@ -236,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 @@ -243,7 +336,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 +364,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 +384,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 +435,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, - } -} 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..0f1272a7 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]. @@ -15,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. @@ -77,12 +80,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 +132,63 @@ 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 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") + } + + tightArea := newOctreeAABBFromAABB(aabb) + nodeIndex := t.pickNodeForItem(tightArea) 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) + tightArea := newOctreeAABBFromAABB(aabb) + item.tightArea = tightArea oldNodeIndex := item.node t.decreaseNodeItems(item.node) // previous node - item.node = t.pickNodeForItem(area) + item.node = t.pickNodeForItem(tightArea) t.increaseNodeItems(item.node) // new node t.gcNode(oldNodeIndex) } @@ -189,7 +210,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 +220,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 +270,9 @@ func (t *Octree[T]) itemsAtDepth(nodeIndex int32, currentDepth, depth uint32) ui return result } -func (t *Octree[T]) pickNodeForItem(area Area) 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 @@ -264,53 +287,66 @@ func (t *Octree[T]) pickNodeForItem(area Area) int32 { return bestNodeIndex } -func (t *Octree[T]) pickChildNode(parentNodeIndex int32, area Area) 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 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 - return nullOctreeIndex - } - - // It has to be inside one of the four 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 childY = parentLooseArea.y childZ = parentLooseArea.z ) - childOffset := parentLooseArea.r / 4.0 - if area.x < parentLooseArea.x { + childOffset := parentLooseArea.halfSize / 4.0 + 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] } 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 +474,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 +519,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 +560,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 +582,20 @@ 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. 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 - z float64 - r float64 + x float64 + y float64 + z float64 + halfSize float64 } type octreeAABB struct { @@ -572,14 +618,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 +644,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 +699,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..955c2d6a 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", ) }) @@ -77,17 +123,20 @@ 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, })) }) 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 +148,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,23 +199,23 @@ 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), ) }) 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, })) }) 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) @@ -197,17 +246,17 @@ 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, })) }) 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 +266,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) @@ -238,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 @@ -245,7 +336,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 +364,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 +384,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 +435,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, - } -} diff --git a/core/spatial/shape2d/aabb.go b/core/spatial/shape2d/aabb.go new file mode 100644 index 00000000..3775e105 --- /dev/null +++ b/core/spatial/shape2d/aabb.go @@ -0,0 +1,75 @@ +package shape2d + +import ( + "math" + + "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, + } +} + +// 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 { + 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..8b1f365d --- /dev/null +++ b/core/spatial/shape2d/aabb_test.go @@ -0,0 +1,126 @@ +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("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) + 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()) + }) + }) + +}) 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 new file mode 100644 index 00000000..6e27ffa7 --- /dev/null +++ b/core/spatial/shape3d/aabb.go @@ -0,0 +1,92 @@ +package shape3d + +import ( + "math" + + "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, + } +} + +// 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 { + 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..c6236b36 --- /dev/null +++ b/core/spatial/shape3d/aabb_test.go @@ -0,0 +1,146 @@ +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("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) + 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()) + }) + }) + +}) 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()) + }) + }) }) 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 4613ce15..3ae50c73 100644 --- a/game/asset_model.go +++ b/game/asset_model.go @@ -11,7 +11,6 @@ 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" ) @@ -22,13 +21,11 @@ type ModelTemplate struct { 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] + Terrains IdentifiableList[TerrainTemplate] Armatures IdentifiableList[ArmatureTemplate] Meshes IdentifiableList[MeshTemplate] AmbientLights IdentifiableList[AmbientLightTemplate] @@ -71,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) @@ -96,9 +83,9 @@ 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) + terrains, err := LoadPhysicsTerrainTemplates(loader, assetModel.PhysicsChunk.Terrains) if err != nil { - return nil, fmt.Errorf("failed to resolve physics body templates: %w", err) + return nil, fmt.Errorf("failed to resolve physics terrain templates: %w", err) } armatures, err := LoadArmatureTemplates(loader, assetModel.MeshChunk.Armatures) @@ -143,13 +130,11 @@ func LoadModelTemplate(loader *AssetLoader, assetModel dto.Model) (*ModelTemplat Shaders: shaders, Textures: textures, Materials: materials, - BodyMaterials: bodyMaterials, - BodyDefinitions: bodyDefinitions, MeshGeometries: meshGeometries, MeshDefinitions: meshDefinitions, Nodes: nodes, - Bodies: bodies, + Terrains: terrains, Armatures: armatures, Meshes: meshes, AmbientLights: ambientLights, @@ -169,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), @@ -316,13 +298,9 @@ func InstantiateModel(scene *Scene, info ModelInfo) *Model { recordings := definition.Recordings meshDefinitions := definition.MeshDefinitions - for template := range definition.Bodies.Values() { + for template := range definition.Terrains.Values() { if nodes.HasID(template.NodeID) { - if info.IsDynamic { - InstantiatePhysicsBodyTemplateDynamic(scene, template, nodes) - } else { - InstantiatePhysicsBodyTemplateStatic(scene, template, nodes) - } + InstantiatePhysicsTerrainTemplate(scene, template, nodes) } } diff --git a/game/asset_physics.go b/game/asset_physics.go index d2aaa503..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/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/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 -} +type TerrainTemplate struct { + NodeID uint32 -// 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 + CollisionMeshes []physics.CollisionMesh } -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 +// 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 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, +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 } -// LoadPhysicsBodyTemplates resolves a list of physics body templates from the +// 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 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) +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[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 IdentifiableList[TerrainTemplate]{}, err } + terrainTemplates[i] = template } - 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 + return terrainTemplates, nil } 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/acceleration.go b/game/physics/acceleration.go index e93a8446..aa92aa14 100644 --- a/game/physics/acceleration.go +++ b/game/physics/acceleration.go @@ -13,44 +13,14 @@ 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 -// 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. -func newAccelerationTarget( - invMass float64, - invInertia dprec.Mat3, - position dprec.Vec3, - rotation dprec.Quat, - linearVelocity dprec.Vec3, - angularVelocity dprec.Vec3, -) AccelerationTarget { +// newAccelerationTarget creates a new [AccelerationTarget] backed by the +// specified body state. +func newAccelerationTarget(body *bodyState) AccelerationTarget { return AccelerationTarget{ - invMass: invMass, - invInertia: invInertia, - position: position, - rotation: rotation, - linearVelocity: linearVelocity, - angularVelocity: angularVelocity, + body: body, } } @@ -59,16 +29,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 +48,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 +57,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 +94,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,49 +103,58 @@ 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 -// 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 + // 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 // 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. @@ -184,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/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 new file mode 100644 index 00000000..582b459e --- /dev/null +++ b/game/physics/accelerator_body.go @@ -0,0 +1,306 @@ +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 = bodyAcceleratorState{ + 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, + revision: accelerator.revision, + } +} + +// 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. +// +// 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 + } 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 = bodyAcceleratorState{ + 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) +} + +// 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{ + 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. +func (v BodyAcceleratorView) Handle(id BodyAcceleratorID) BodyAcceleratorHandle { + return BodyAcceleratorHandle{ + view: v, + id: id, + } +} + +// 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 + body := &v.scene.bodies[bodyIndex] + return BodyID{ + index: bodyIndex, + revision: body.revision, + } +} + +// 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{ + index: index, + revision: accelerator.revision, + } +} + +// 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") + } + return nil + } + accelerator := &v.scene.bodyAccelerators[id.index] + if accelerator.revision != id.revision { + if required { + panic("invalid body accelerator ID") + } + return nil + } + 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]. +// +// 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 +} + +// 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) +} + +// 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 + nextIndex int32 + isEnabled 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/accelerator_global.go b/game/physics/accelerator_global.go new file mode 100644 index 00000000..6fbc9e56 --- /dev/null +++ b/game/physics/accelerator_global.go @@ -0,0 +1,238 @@ +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, accelerator := v.scene.allocateGlobalAccelerator() + + *accelerator = globalAcceleratorState{ + solver: solver, + revision: accelerator.revision + 1, // progress revision to valid (odd) value + isEnabled: true, + } + + return GlobalAcceleratorID{ + index: index, + revision: accelerator.revision, + } +} + +// 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 +// 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) { + accelerator := v.resolve(id, true) + + *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) +} + +// 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{ + 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. +func (v GlobalAcceleratorView) Handle(id GlobalAcceleratorID) GlobalAcceleratorHandle { + return GlobalAcceleratorHandle{ + view: v, + id: id, + } +} + +// 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.isEnabled +} + +// 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.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 { + 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 +} + +// 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]. +// +// 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 +} + +// 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) +} + +// 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 +} 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 -} 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 -} diff --git a/game/physics/body.go b/game/physics/body.go index 90d16eff..fffdea32 100644 --- a/game/physics/body.go +++ b/game/physics/body.go @@ -5,454 +5,432 @@ 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 invalidBodyState = &bodyState{} - -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 BodyID struct { + index int32 + revision int32 +} + +var NilBodyID = BodyID{} + +type BodyView struct { + scene *Scene +} + +func (v BodyView) Create(position dprec.Vec3, rotation dprec.Quat) BodyID { + index, body := v.scene.allocateBody() + + objectID := v.scene.collisionScene.CreateObject(placement3d.ObjectInfo[bodyData]{ + Position: opt.V(position), + Rotation: opt.V(rotation), + UserData: bodyData{ + index: index, + }, + }) + + *body = bodyState{ + objectID: objectID, + revision: body.revision + 1, // progress revision to valid (odd) value + firstBodyAcceleratorIndex: nilIndex, + firstSoloConstraintIndex: nilIndex, + firstPairConstraintIndex: nilIndex, + invInertiaLocal: dprec.IdentityMat3(), + invInertia: dprec.IdentityMat3(), + invMass: 1.0, + linearVelocity: dprec.ZeroVec3(), + angularVelocity: dprec.ZeroVec3(), + position: position, + rotation: rotation, + } + + return BodyID{ + index: index, + revision: body.revision, } } -func (d *BodyDefinition) CollisionSpheres() []shape3d.Sphere { - return d.collisionSpheres +func (v BodyView) CreateHandle(position dprec.Vec3, rotation dprec.Quat) BodyHandle { + return v.Handle(v.Create(position, rotation)) } -func (d *BodyDefinition) CollisionBoxes() []shape3d.Box { - return d.collisionBoxes +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)) + } + soloConstraintView := v.scene.SoloConstraints() + for body.firstSoloConstraintIndex != nilIndex { + soloConstraintView.Delete(soloConstraintView.idFromIndex(body.firstSoloConstraintIndex)) + } + pairConstraintView := v.scene.PairConstraints() + for body.firstPairConstraintIndex != nilIndex { + pairConstraintView.Delete(pairConstraintView.idFromIndex(body.firstPairConstraintIndex)) + } + + *body = bodyState{ + objectID: placement3d.NilObjectID, + 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) } -func (d *BodyDefinition) CollisionMeshes() []shape3d.Mesh { - return d.collisionMeshes +// 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{ + index: int32(index), + revision: body.revision, + }) + }) } -type BodyInfo struct { - Name string - Definition *BodyDefinition - Position dprec.Vec3 - Rotation dprec.Quat +func (v BodyView) Handle(id BodyID) BodyHandle { + return BodyHandle{ + view: v, + id: id, + } +} + +func (v BodyView) IsValid(id BodyID) bool { + body := v.resolve(id, false) + return body != nil } -// Body represents a physical body that has physics -// act upon it. -type Body struct { - scene *Scene - reference indexReference +func (v BodyView) Mass(id BodyID) float64 { + body := v.resolve(id, true) + return 1.0 / body.invMass } -// Name returns the name of this body. -func (b Body) Name() string { - state := b.state() - return state.name +func (v BodyView) SetMass(id BodyID, mass float64) { + body := v.resolve(id, true) + body.invMass = 1.0 / mass } -// SetName sets a new name for this body. -func (b Body) SetName(name string) { - state := b.state() - state.name = name +func (v BodyView) MomentOfInertia(id BodyID) dprec.Mat3 { + body := v.resolve(id, true) + return dprec.InverseMat3(body.invInertiaLocal) } -// Mass returns the mass of this body in kg. -func (b Body) Mass() float64 { - state := b.state() - return state.mass +func (v BodyView) SetMomentOfInertia(id BodyID, inertia dprec.Mat3) { + body := v.resolve(id, true) + body.invInertiaLocal = dprec.InverseMat3(inertia) + body.recalculateInertia() } -// SetMass changes the mass of this body. -func (b Body) SetMass(mass float64) { - state := b.state() - state.mass = mass +func (v BodyView) Velocity(id BodyID) dprec.Vec3 { + body := v.resolve(id, true) + return body.linearVelocity } -// 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) SetVelocity(id BodyID, velocity dprec.Vec3) { + body := v.resolve(id, true) + body.linearVelocity = velocity } -// 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) AngularVelocity(id BodyID) dprec.Vec3 { + body := v.resolve(id, true) + return body.angularVelocity } -// // 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) SetAngularVelocity(id BodyID, angularVelocity dprec.Vec3) { + body := v.resolve(id, true) + body.angularVelocity = angularVelocity +} + +func (v BodyView) Position(id BodyID) dprec.Vec3 { + body := v.resolve(id, true) + return body.position +} -// // SetRestitutionCoefficient changes the restitution -// // coefficient for this body. -// func (b *Body) SetRestitutionCoefficient(coefficient float64) { -// b.restitutionCoefficient = coefficient -// } +func (v BodyView) SetPosition(id BodyID, position dprec.Vec3) { + body := v.resolve(id, true) + body.position = position + v.refreshPlacement(body) +} -// // DragCoefficient returns the drag factor of this body. -// func (b *Body) DragFactor() float64 { -// return b.dragFactor -// } +func (v BodyView) Rotation(id BodyID) dprec.Quat { + body := v.resolve(id, true) + return body.rotation +} -// // 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) SetRotation(id BodyID, rotation dprec.Quat) { + body := v.resolve(id, true) + body.rotation = rotation + body.recalculateInertia() + v.refreshPlacement(body) +} -// // AngularDragFactor returns the angular drag factor -// // for this body. -// func (b *Body) AngularDragFactor() float64 { -// return b.angularDragFactor -// } +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, + Filtering: col.Filtering, + UserData: shapeData{ + frictionCoefficient: col.FrictionCoefficient, + restitutionCoefficient: col.RestitutionCoefficient, + }, + }) + return BodyCollisionShapeID{ + 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) AttachCollisionBox(id BodyID, col CollisionBox) BodyCollisionShapeID { + 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 BodyCollisionShapeID{ + bodyID: id, + shapeID: shapeID, + } +} -// Position returns the body's position in world space. -func (b Body) Position() dprec.Vec3 { - state := b.state() - return state.position -} - -// SetPosition changes the position of this body. -func (b Body) SetPosition(position dprec.Vec3) { - state := b.state() - state.position = position - - // FIXME: Invalidate shape placement! -} - -// 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 (v BodyView) DetachCollisionShape(id BodyID, shapeID BodyCollisionShapeID) { + if id != shapeID.bodyID { + panic("invalid shape ID for body") + } + v.scene.collisionScene.DeleteObjectShape(shapeID.shapeID) +} -// Delete removes this physical body. -func (b Body) Delete() { - deleteBody(b.scene, b.reference) +func (v BodyView) refreshPlacement(body *bodyState) { + v.scene.collisionScene.SetObjectTransform(body.objectID, shape3d.Transform{ + Translation: body.position, + Rotation: shape3d.RotationFromQuat(body.rotation), + }) } -func (b Body) state() *bodyState { - index := b.reference.Index - state := &b.scene.bodies[index] - if state.reference != b.reference { - return invalidBodyState +func (v BodyView) idFromIndex(index int32) BodyID { + body := &v.scene.bodies[index] + return BodyID{ + index: index, + revision: body.revision, } - return state } -// func (b *Body) applyOffsetForce(offset, force dprec.Vec3) { -// b.applyForce(force) -// b.applyTorque(dprec.Vec3Cross(offset, force)) -// } +func (v BodyView) resolve(id BodyID, required bool) *bodyState { + if id.revision == 0 { + if required { + panic("invalid body ID") + } + return nil + } + body := &v.scene.bodies[id.index] + if body.revision != id.revision { + if required { + panic("invalid body ID") + } + return nil + } + return body +} -// func (b *Body) applyImpulse(impulse dprec.Vec3) { -// b.addVelocity(dprec.Vec3Quot(impulse, b.mass)) -// } +// 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 +} -// 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) ID() BodyID { + return h.id +} -// func (b *Body) applyOffsetImpulse(offset, impulse dprec.Vec3) { -// b.applyImpulse(impulse) -// b.applyAngularImpulse(dprec.Vec3Cross(offset, impulse)) -// } +func (h BodyHandle) Delete() { + h.view.Delete(h.id) +} -// func (b *Body) applyNudge(nudge dprec.Vec3) { -// b.translate(dprec.Vec3Quot(nudge, b.mass)) -// } +func (h BodyHandle) IsValid() bool { + return h.view.IsValid(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) Mass() float64 { + return h.view.Mass(h.id) +} -// func (b *Body) applyOffsetNudge(offset, nudge dprec.Vec3) { -// b.applyNudge(nudge) -// b.applyAngularNudge(dprec.Vec3Cross(offset, nudge)) -// } +func (h BodyHandle) SetMass(mass float64) { + h.view.SetMass(h.id, mass) +} -type bodyState struct { - reference indexReference +func (h BodyHandle) MomentOfInertia() dprec.Mat3 { + return h.view.MomentOfInertia(h.id) +} - objectID placement3d.ObjectID +func (h BodyHandle) SetMomentOfInertia(inertia dprec.Mat3) { + h.view.SetMomentOfInertia(h.id, inertia) +} - name string - definition *BodyDefinition +func (h BodyHandle) Velocity() dprec.Vec3 { + return h.view.Velocity(h.id) +} - mass float64 - momentOfInertia dprec.Mat3 +func (h BodyHandle) SetVelocity(velocity dprec.Vec3) { + h.view.SetVelocity(h.id, velocity) +} - // TODO: Move friction and restitution to the collision Set through - // a material. +func (h BodyHandle) AngularVelocity() dprec.Vec3 { + return h.view.AngularVelocity(h.id) +} - frictionCoefficient float64 - restitutionCoefficient float64 +func (h BodyHandle) SetAngularVelocity(angularVelocity dprec.Vec3) { + h.view.SetAngularVelocity(h.id, angularVelocity) +} - // TODO: dragFactor and angularDragFactor should be moved to the - // aerodynamic shapes. +func (h BodyHandle) Position() dprec.Vec3 { + return h.view.Position(h.id) +} - dragFactor float64 - angularDragFactor float64 +func (h BodyHandle) SetPosition(position dprec.Vec3) { + h.view.SetPosition(h.id, position) +} - position dprec.Vec3 - rotation dprec.Quat +func (h BodyHandle) Rotation() dprec.Quat { + return h.view.Rotation(h.id) +} - velocity dprec.Vec3 - angularVelocity dprec.Vec3 +func (h BodyHandle) SetRotation(rotation dprec.Quat) { + h.view.SetRotation(h.id, rotation) +} - aerodynamicShapes []AerodynamicShape +func (h BodyHandle) AttachCollisionSphere(shape CollisionSphere) BodyCollisionShapeID { + return h.view.AttachCollisionSphere(h.id, shape) } -func (s bodyState) IsActive() bool { - return s.reference.IsValid() +func (h BodyHandle) AttachCollisionBox(shape CollisionBox) BodyCollisionShapeID { + return h.view.AttachCollisionBox(h.id, shape) } -func (b *bodyState) AddVelocity(amount dprec.Vec3) { - b.velocity = dprec.Vec3Sum(b.velocity, amount) +func (h BodyHandle) DetachCollisionShape(shapeID BodyCollisionShapeID) { + h.view.DetachCollisionShape(h.id, shapeID) } -func (b *bodyState) AddAngularVelocity(amount dprec.Vec3) { - b.angularVelocity = dprec.Vec3Sum(b.angularVelocity, amount) +type bodyState struct { + objectID placement3d.ObjectID + + revision int32 + firstBodyAcceleratorIndex int32 + firstSoloConstraintIndex int32 + firstPairConstraintIndex int32 + + invInertiaLocal dprec.Mat3 + invInertia dprec.Mat3 + invMass float64 + + linearAcceleration dprec.Vec3 + angularAcceleration dprec.Vec3 + + linearVelocity dprec.Vec3 + angularVelocity dprec.Vec3 + + position dprec.Vec3 + rotation dprec.Quat } -func (b *bodyState) ClampVelocity(max float64) { - if b.velocity.SqrLength() > max*max { - b.velocity = dprec.ResizedVec3(b.velocity, max) - } +func (s bodyState) isValid() bool { + return s.revision%2 == 1 // only odd revisions are valid } -func (b *bodyState) ClampAngularVelocity(max float64) { - if b.angularVelocity.SqrLength() > max*max { - b.angularVelocity = dprec.ResizedVec3(b.angularVelocity, max) - } +func (s *bodyState) mass() float64 { + return 1.0 / s.invMass } -func (b *bodyState) Translate(offset dprec.Vec3) { - b.position = dprec.Vec3Sum(b.position, offset) +func (s *bodyState) inertia() dprec.Mat3 { + return dprec.InverseMat3(s.invInertia) } -func (b *bodyState) VectorRotate(vector dprec.Vec3) { - const angularEpsilon = float64(0.00001) - if radians := vector.Length(); dprec.Abs(radians) > angularEpsilon { - b.Rotate(dprec.RotationQuat(dprec.Radians(radians), vector)) - } +func (b *bodyState) recalculateInertia() { + b.invInertia = RotatedMomentOfInertia(b.invInertiaLocal, b.rotation) } -func (b *bodyState) Rotate(quat dprec.Quat) { - b.rotation = dprec.UnitQuat(dprec.QuatProd(quat, b.rotation)) +func (b *bodyState) addLinearAcceleration(amount dprec.Vec3) { + b.linearAcceleration = dprec.Vec3Sum(b.linearAcceleration, amount) } -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() - } +func (b *bodyState) addAngularAcceleration(amount dprec.Vec3) { + b.angularAcceleration = dprec.Vec3Sum(b.angularAcceleration, amount) +} - 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, - }) +func (b *bodyState) clampLinearAcceleration(max float64) { + if b.linearAcceleration.SqrLength() > max*max { + b.linearAcceleration = dprec.ResizedVec3(b.linearAcceleration, max) } - 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, +func (b *bodyState) clampAngularAcceleration(max float64) { + if b.angularAcceleration.SqrLength() > max*max { + b.angularAcceleration = dprec.ResizedVec3(b.angularAcceleration, max) + } +} - name: info.Name, - definition: info.Definition, +func (b *bodyState) applyForce(force dprec.Vec3) { + b.addLinearAcceleration(dprec.Vec3Prod(force, b.invMass)) +} - mass: info.Definition.mass, - momentOfInertia: info.Definition.momentOfInertia, +func (b *bodyState) applyTorque(torque dprec.Vec3) { + b.addAngularAcceleration(dprec.Mat3Vec3Prod(b.invInertia, torque)) +} - frictionCoefficient: info.Definition.frictionCoefficient, - restitutionCoefficient: info.Definition.restitutionCoefficient, +func (b *bodyState) applyOffsetForce(offset, force dprec.Vec3) { + b.applyForce(force) + b.applyTorque(dprec.Vec3Cross(offset, force)) +} - dragFactor: info.Definition.dragFactor, - angularDragFactor: info.Definition.angularDragFactor, +func (b *bodyState) addLinearVelocity(amount dprec.Vec3) { + b.linearVelocity = dprec.Vec3Sum(b.linearVelocity, amount) +} - position: info.Position, - rotation: info.Rotation, +func (b *bodyState) addAngularVelocity(amount dprec.Vec3) { + b.angularVelocity = dprec.Vec3Sum(b.angularVelocity, amount) +} - aerodynamicShapes: info.Definition.aerodynamicShapes, +func (b *bodyState) clampLinearVelocity(max float64) { + if b.linearVelocity.SqrLength() > max*max { + b.linearVelocity = dprec.ResizedVec3(b.linearVelocity, max) } - scene.bodies[freeIndex] = body +} - return Body{ - scene: scene, - reference: reference, +func (b *bodyState) clampAngularVelocity(max float64) { + if b.angularVelocity.SqrLength() > max*max { + b.angularVelocity = dprec.ResizedVec3(b.angularVelocity, max) } } -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) - } +func (b *bodyState) translate(offset dprec.Vec3) { + b.position = dprec.Vec3Sum(b.position, offset) +} + +func (b *bodyState) rotate(quat dprec.Quat) { + b.rotation = dprec.UnitQuat(dprec.QuatProd(quat, b.rotation)) } diff --git a/game/physics/callback.go b/game/physics/callback.go index 7aee2beb..0b637011 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" -) +// 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) -// 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]() -} +// SoloCollisionSubscription represents a notification subscription +// for single body collisions. +type SoloCollisionSubscription = observer.Subscription[SoloCollisionCallback] -// DoubleBodyCollisionCallback is a mechanism to receive notifications +// PairCollisionCallback is a mechanism to receive notifications // about collisions between two bodies. -type DoubleBodyCollisionCallback func(first, second Body, active bool) +type PairCollisionCallback func(firstBodyID, secondBodyID BodyID, active bool) -// DoubleBodyCollisionSubscription represents a notification subscription +// PairCollisionSubscription 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 -// about collisions between a body and a prop in the scene. -type SingleBodyCollisionCallback func(body Body, prop Prop, active bool) - -// SingleBodyCollisionSubscription represents a notification subscription -// for single body collisions. -type SingleBodyCollisionSubscription = observer.Subscription[SingleBodyCollisionCallback] - -// SingleBodyCollisionSubscriptionSet represents a set of single body -// collision subscriptions. -type SingleBodyCollisionSubscriptionSet = observer.SubscriptionSet[SingleBodyCollisionCallback] - -// NewSingleBodyCollisionSubscriptionSet creates a new -// SingleBodyCollisionSubscriptionSet. -func NewSingleBodyCollisionSubscriptionSet() *SingleBodyCollisionSubscriptionSet { - return observer.NewSubscriptionSet[SingleBodyCollisionCallback]() -} +type PairCollisionSubscription = observer.Subscription[PairCollisionCallback] diff --git a/game/physics/collision.go b/game/physics/collision.go new file mode 100644 index 00000000..a3910f06 --- /dev/null +++ b/game/physics/collision.go @@ -0,0 +1,35 @@ +package physics + +import ( + "github.com/mokiat/lacking/core/spatial/placement3d" + "github.com/mokiat/lacking/core/spatial/shape3d" +) + +type Mask = placement3d.Mask + +const FullMask = placement3d.FullMask + +type Filter = placement3d.Filter + +type BodyCollisionShapeID struct { + bodyID BodyID + shapeID placement3d.ObjectShapeID +} + +type TerrainCollisionShapeID struct { + terrainID TerrainID + shapeID placement3d.TerrainShapeID +} + +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/constraint.go b/game/physics/constraint.go new file mode 100644 index 00000000..aef5b6f1 --- /dev/null +++ b/game/physics/constraint.go @@ -0,0 +1,194 @@ +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 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 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 = dprec.UnitQuat(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, split into the 1x3 blocks that act on linear and angular +// velocity respectively. +type Jacobian struct { + + // 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 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()) + return linear + angular +} + +// InverseEffectiveMass returns the inverse of the effective mass with which +// 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 +// 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/constraint/chandelier.go b/game/physics/constraint/chandelier.go deleted file mode 100644 index c0a92b4e..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/clamp_direction_offset.go b/game/physics/constraint/clamp_direction_offset.go deleted file mode 100644 index 7090c766..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/constraint/coilover.go b/game/physics/constraint/coilover.go deleted file mode 100644 index 71e23898..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/constraint/collision.go b/game/physics/constraint/collision.go deleted file mode 100644 index b17192ff..00000000 --- a/game/physics/constraint/collision.go +++ /dev/null @@ -1,227 +0,0 @@ -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 -} diff --git a/game/physics/constraint/combined.go b/game/physics/constraint/combined.go deleted file mode 100644 index 7f1ef084..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/copy_direction.go b/game/physics/constraint/copy_direction.go deleted file mode 100644 index cfccb65e..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/constraint/copy_position.go b/game/physics/constraint/copy_position.go deleted file mode 100644 index 84ac70bc..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/constraint/copy_rotation.go b/game/physics/constraint/copy_rotation.go deleted file mode 100644 index aa62e080..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/constraint/differential.go b/game/physics/constraint/differential.go deleted file mode 100644 index 4b57030c..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) {} diff --git a/game/physics/constraint/hinged_rod.go b/game/physics/constraint/hinged_rod.go deleted file mode 100644 index 26abdd09..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/constraint/limit_relative_angle.go b/game/physics/constraint/limit_relative_angle.go deleted file mode 100644 index 1e403a4d..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/constraint/match_direction.go b/game/physics/constraint/match_direction.go deleted file mode 100644 index 9e751a63..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/constraint/match_direction_offset.go b/game/physics/constraint/match_direction_offset.go deleted file mode 100644 index 2460fd39..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/constraint/match_rotation.go b/game/physics/constraint/match_rotation.go deleted file mode 100644 index 8abba7b9..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/constraint/pair_attachment.go b/game/physics/constraint/pair_attachment.go deleted file mode 100644 index abede2c7..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/constraint/static_position.go b/game/physics/constraint/static_position.go deleted file mode 100644 index b77d39e1..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/constraint/static_rotation.go b/game/physics/constraint/static_rotation.go deleted file mode 100644 index 866ad20e..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/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 new file mode 100644 index 00000000..1a1dbbe0 --- /dev/null +++ b/game/physics/constraint_pair.go @@ -0,0 +1,575 @@ +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 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 +} + +// 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 { + 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 +} + +// 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 { + 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 +} + +// 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 { + return 0.0 + } + 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) +} + +// 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 and + // recomputes any data (e.g. Jacobians) that is derived from the + // current position and orientation of the target bodies. + // + // 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 amortizes that recomputation across + // every iteration of the loop, rather than repeating it on each one. + // + // 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 + // 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, 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 + // 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, 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) +} + +// 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, 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) + + 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.firstPairConstraintIndex, + secondaryNextIndex: secondaryBody.firstPairConstraintIndex, + isEnabled: true, + } + primaryBody.firstPairConstraintIndex = index + secondaryBody.firstPairConstraintIndex = index + + return PairConstraintID{ + index: index, + revision: constraint.revision, + } +} + +// 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. +// +// 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. + primaryBodyIndex := constraint.primaryBodyIndex + primaryBody := &v.scene.bodies[primaryBodyIndex] + if primaryBody.firstPairConstraintIndex == id.index { + primaryBody.firstPairConstraintIndex = constraint.nextIndexForBody(primaryBodyIndex) + } else { + prevIndex := primaryBody.firstPairConstraintIndex + for prevIndex != nilIndex { + prevConstraint := &v.scene.pairConstraints[prevIndex] + 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") + } + } + } + + // Unlink the constraint from the secondary body. + secondaryBodyIndex := constraint.secondaryBodyIndex + secondaryBody := &v.scene.bodies[secondaryBodyIndex] + if secondaryBody.firstPairConstraintIndex == id.index { + secondaryBody.firstPairConstraintIndex = constraint.nextIndexForBody(secondaryBodyIndex) + } else { + prevIndex := secondaryBody.firstPairConstraintIndex + for prevIndex != nilIndex { + prevConstraint := &v.scene.pairConstraints[prevIndex] + 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") + } + } + } + + *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) +} + +// 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{ + 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. +func (v PairConstraintView) Handle(id PairConstraintID) PairConstraintHandle { + return PairConstraintHandle{ + view: v, + id: id, + } +} + +// 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 + body := &v.scene.bodies[bodyIndex] + return BodyID{ + index: bodyIndex, + revision: body.revision, + } +} + +// 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 + body := &v.scene.bodies[bodyIndex] + return BodyID{ + index: bodyIndex, + revision: body.revision, + } +} + +// 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{ + index: index, + revision: state.revision, + } +} + +// 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 { + 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 +} + +// PairConstraintHandle is an object-oriented alternative to +// [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 +} + +// 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 + primaryBodyIndex int32 + secondaryBodyIndex int32 + primaryNextIndex int32 + secondaryNextIndex int32 + 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 { + return s.revision%2 == 1 // only odd revisions are valid +} 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/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/constraint_solo.go b/game/physics/constraint_solo.go new file mode 100644 index 00000000..dd409389 --- /dev/null +++ b/game/physics/constraint_solo.go @@ -0,0 +1,444 @@ +package physics + +// 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 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 [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 +} + +// 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 { + invEffMass := jacobian.InverseEffectiveMass(c.Target) + if invEffMass < Epsilon { + return 0.0 + } + effVelocity := jacobian.EffectiveVelocity(c.Target) + restitution := 1 + restitutionCoef*RestitutionClamp(effVelocity) + driftBias := c.ImpulseBeta * drift / c.DeltaSeconds + return -(restitution*effVelocity - driftBias) / invEffMass +} + +// 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) + 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 { + invEffMass := jacobian.InverseEffectiveMass(c.Target) + if invEffMass < Epsilon { + return 0.0 + } + 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) +} + +// 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 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 before the first + // [SoloConstraintSolver.ApplyImpulses] iteration of a step, since the + // target body's position and orientation remain unchanged throughout + // that loop. This amortizes that recomputation across every + // iteration of the loop, rather than repeating it on each one. + // + // 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 + // 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 step, once for each impulse + // 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 + // 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 step, once for each nudge + // 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) +} + +// 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], 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]. +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) + + index, constraint := v.scene.allocateSoloConstraint() + + *constraint = soloConstraintState{ + 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, + revision: constraint.revision, + } +} + +// 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. +// +// 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 + } 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 = soloConstraintState{ + 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) +} + +// 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{ + 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. +func (v SoloConstraintView) Handle(id SoloConstraintID) SoloConstraintHandle { + return SoloConstraintHandle{ + view: v, + id: id, + } +} + +// 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 + body := &v.scene.bodies[bodyIndex] + return BodyID{ + index: bodyIndex, + revision: body.revision, + } +} + +// 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{ + index: index, + revision: constraint.revision, + } +} + +// 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 { + 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 +} + +// SoloConstraintHandle is an object-oriented alternative to +// [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 +} + +// 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) +} + +// 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 soloConstraintState struct { + solver SoloConstraintSolver + revision int32 + bodyIndex int32 + nextIndex int32 + 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/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/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/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/prop.go b/game/physics/prop.go deleted file mode 100644 index 1ead4249..00000000 --- a/game/physics/prop.go +++ /dev/null @@ -1,31 +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 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 { - reference indexReference - meshID placement3d.MeshID - name string -} diff --git a/game/physics/scene.go b/game/physics/scene.go index 1be9808c..ea6aa9f7 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" @@ -10,963 +11,997 @@ 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/game/physics/solver" + "github.com/mokiat/lacking/util/observer" ) -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), +// Scene represents a physics scene that contains +// a number of bodies that are independent on any +// bodies managed by other scene objects. +type Scene struct { + collisionScene *placement3d.Scene[bodyData, terrainData, shapeData] - // bodyAccelerators []any // TOOD - // areaAccelerators []any // TODO - globalAccelerators: make([]globalAcceleratorState, 0, 64), + soloCollisionSubscriptions *observer.SubscriptionSet[SoloCollisionCallback] + pairCollisionSubscriptions *observer.SubscriptionSet[PairCollisionCallback] - freeBodyAcceleratorIndices: ds.PreallocatedStack[uint32](16), - freeAreaAcceleratorIndices: ds.PreallocatedStack[uint32](16), - freeGlobalAcceleratorIndices: ds.PreallocatedStack[uint32](16), + soloCollisionConstraintIDs []SoloConstraintID + pairCollisionConstraintIDs []PairConstraintID - sbConstraints: make([]sbConstraintState, 0, 64), - dbConstraints: make([]dbConstraintState, 0, 64), + soloCollisionSolvers []SoloCollisionSolver + pairCollisionSolvers []PairCollisionSolver - freeSBConstraintIndices: ds.PreallocatedStack[uint32](16), - freeDBConstraintIndices: ds.PreallocatedStack[uint32](16), + oldSoloCollisionRefs map[soloCollisionRef]struct{} + oldPairCollisionRefs map[pairCollisionRef]struct{} - collisionSet: make(placement3d.ContactList, 0, 128), + newSoloCollisionRefs map[soloCollisionRef]struct{} + newPairCollisionRefs map[pairCollisionRef]struct{} - oldSBCollisions: make(map[sbCollisionPair]struct{}, 32), - newSBCollisions: make(map[sbCollisionPair]struct{}, 32), + objectContacts placement3d.ObjectContactList + terrainContacts placement3d.TerrainContactList - oldDBCollisions: make(map[dbCollisionPair]struct{}, 32), - newDBCollisions: make(map[dbCollisionPair]struct{}, 32), - } -} + freeCollisionRejectGroup uint32 -// Scene represents a physics scene that contains -// a number of bodies that are independent on any -// bodies managed by other scene objects. -type Scene struct { - shapeScene *placement3d.Scene[bodyRef, struct{}, propRef] + mediumSolver MediumSolver - sbCollisionSubscriptions *SingleBodyCollisionSubscriptionSet - dbCollisionSubscriptions *DoubleBodyCollisionSubscriptionSet + freeGlobalAcceleratorIndices *ds.Stack[int32] + freeBodyAcceleratorIndices *ds.Stack[int32] + freeSoloConstraintIndices *ds.Stack[int32] + freePairConstraintIndices *ds.Stack[int32] + freeBodyIndices *ds.Stack[int32] + freeTerrainIndices *ds.Stack[int32] - timeSpeed float64 + globalAccelerators []globalAcceleratorState + bodyAccelerators []bodyAcceleratorState + soloConstraints []soloConstraintState + pairConstraints []pairConstraintState + bodies []bodyState + terrains []terrainState maxLinearAcceleration float64 maxAngularAcceleration float64 maxLinearVelocity float64 maxAngularVelocity float64 - mediumSolver MediumSolver - - props []propState + impulseIterationCount int + impulseDriftAdjustmentRatio float64 + nudgeIterationCount int + nudgeDriftAdjustmentRatio float64 - bodies []bodyState - bodyAccelerationTargets []AccelerationTarget - bodyConstraintPlaceholders []solver.Placeholder - freeBodyIndices *ds.Stack[uint32] - - // bodyAccelerators []any // TOOD - freeBodyAcceleratorIndices *ds.Stack[uint32] + timeScale float64 +} - // areaAccelerators []any // TODO - freeAreaAcceleratorIndices *ds.Stack[uint32] +func NewScene() *Scene { + return &Scene{ + collisionScene: placement3d.NewScene[bodyData, terrainData, shapeData](placement3d.SceneSettings{ + Size: opt.V(16384.0), + MaxDepth: opt.V[uint32](12), + InitialNodeCapacity: opt.V[uint32](1024), + InitialItemCapacity: opt.V[uint32](1024), + }), - globalAccelerators []globalAcceleratorState - freeGlobalAcceleratorIndices *ds.Stack[uint32] + soloCollisionSubscriptions: observer.NewSubscriptionSet[SoloCollisionCallback](), + pairCollisionSubscriptions: observer.NewSubscriptionSet[PairCollisionCallback](), - sbConstraints []sbConstraintState - freeSBConstraintIndices *ds.Stack[uint32] + soloCollisionConstraintIDs: make([]SoloConstraintID, 0), + pairCollisionConstraintIDs: make([]PairConstraintID, 0), - dbConstraints []dbConstraintState - freeDBConstraintIndices *ds.Stack[uint32] + soloCollisionSolvers: make([]SoloCollisionSolver, 0), + pairCollisionSolvers: make([]PairCollisionSolver, 0), - sbCollisionConstraints []SBConstraint - sbCollisionSolvers []constraint.Collision + oldSoloCollisionRefs: make(map[soloCollisionRef]struct{}), + oldPairCollisionRefs: make(map[pairCollisionRef]struct{}), - dbCollisionConstraints []DBConstraint - dbCollisionSolvers []constraint.PairCollision + newSoloCollisionRefs: make(map[soloCollisionRef]struct{}), + newPairCollisionRefs: make(map[pairCollisionRef]struct{}), - collisionSet placement3d.ContactList + objectContacts: make(placement3d.ObjectContactList, 0), + terrainContacts: make(placement3d.TerrainContactList, 0), - oldSBCollisions map[sbCollisionPair]struct{} - newSBCollisions map[sbCollisionPair]struct{} + freeCollisionRejectGroup: 0, - oldDBCollisions map[dbCollisionPair]struct{} - newDBCollisions map[dbCollisionPair]struct{} + mediumSolver: NewStaticAirSolver(), - freeCollisionRejectGroup uint32 - freeRevision uint32 + freeGlobalAcceleratorIndices: ds.EmptyStack[int32](), + freeBodyAcceleratorIndices: ds.EmptyStack[int32](), + 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, + maxLinearVelocity: math.MaxFloat64, + maxAngularVelocity: math.MaxFloat64, + + impulseIterationCount: 8, + impulseDriftAdjustmentRatio: 0.2, + nudgeIterationCount: 4, + nudgeDriftAdjustmentRatio: 0.2, + + timeScale: 1.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.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 +// 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) +} - s.dbConstraints = nil - s.freeDBConstraintIndices = nil +// 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) +} - s.sbCollisionConstraints = nil - s.sbCollisionSolvers = nil +// 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 +} - s.dbCollisionConstraints = nil - s.dbCollisionSolvers = 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 +} - s.collisionSet = nil +// 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() + } +} - s.oldSBCollisions = nil - s.newSBCollisions = nil +// 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, + } +} - s.oldDBCollisions = nil - s.newDBCollisions = nil +// 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, + } } -// SubscribeSingleBodyCollision registers a callback that is invoked when a body -// collides with a static object. -func (s *Scene) SubscribeSingleBodyCollision(callback SingleBodyCollisionCallback) *SingleBodyCollisionSubscription { - return s.sbCollisionSubscriptions.Subscribe(callback) +// 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, + } } -// SubscribeDoubleBodyCollision registers a callback that is invoked when two -// bodies collide. -func (s *Scene) SubscribeDoubleBodyCollision(callback DoubleBodyCollisionCallback) *DoubleBodyCollisionSubscription { - return s.dbCollisionSubscriptions.Subscribe(callback) +// 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, + } } -// 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 +// 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, + } } -// SetTimeSpeed changes the rate at which time runs. -func (s *Scene) SetTimeSpeed(timeSpeed float64) { - s.timeSpeed = timeSpeed +// 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 linear acceleration that a body -// can have. +// 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 linear acceleration that a body -// can have. +// 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 angular acceleration that a body -// can have. +// 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 angular acceleration that a -// body can have. +// SetMaxAngularAcceleration changes the maximum magnitude that the angular +// acceleration of a body can reach. func (s *Scene) SetMaxAngularAcceleration(acceleration float64) { s.maxAngularAcceleration = acceleration } -// MediumSolver returns the solver that is used to calculate the medium -// properties of the scene. +// MaxLinearVelocity returns the maximum magnitude that the linear velocity +// of a body can reach. Velocities that exceed it are clamped on every +// simulation step. // -// The returned solver is never nil. A scene starts off with a default -// [StaticAirSolver]. -func (s *Scene) MediumSolver() MediumSolver { - return s.mediumSolver +// Defaults to math.MaxFloat64, which is effectively unbounded. +func (s *Scene) MaxLinearVelocity() float64 { + return s.maxLinearVelocity } -// SetMediumSolver changes the solver that is used to calculate the medium -// properties of the scene. +// 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. // -// 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() - } +// Defaults to math.MaxFloat64, which is effectively unbounded. +func (s *Scene) MaxAngularVelocity() float64 { + return s.maxAngularVelocity } -// 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. +// SetMaxAngularVelocity changes the maximum magnitude that the angular +// velocity of a body can reach. +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. // -// 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. +// 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. // -// 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 +// Defaults to 0.2. +func (s *Scene) ImpulseDriftAdjustmentRatio() float64 { + return s.impulseDriftAdjustmentRatio } -// 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) { - // TODO: createProp(s, info) - - // objectID := s.shapeScene.CreateObject(placement3d.ObjectInfo[internalRef]{ - // Position: info.Position, - // Rotation: info.Rotation, - // UserData: internalRef{ - // index: propIndex, - // 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)) - - meshID := s.shapeScene.CreateMesh(placement3d.MeshInfo[propRef]{ - Position: info.Position, - Rotation: info.Rotation, - Mesh: mesh, - UserData: propRef{ - index: propIndex, - }, - }) +// SetImpulseDriftAdjustmentRatio changes the Baumgarte stabilization +// factor used to correct positional drift through impulses. +func (s *Scene) SetImpulseDriftAdjustmentRatio(ratio float64) { + s.impulseDriftAdjustmentRatio = ratio +} - s.props = append(s.props, propState{ - reference: newIndexReference(propIndex, s.nextRevision()), - meshID: meshID, - name: info.Name, - }) - } +// 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 } -// CreateBody creates a new physics body and places -// it within this scene. -func (s *Scene) CreateBody(info BodyInfo) Body { - return createBody(s, info) +// SetNudgeIterationCount changes the number of nudge resolution +// iterations performed per physics simulation step. +func (s *Scene) SetNudgeIterationCount(count int) { + s.nudgeIterationCount = count } -// CreateConstraintSet creates a new ConstraintSet. -func (s *Scene) CreateConstraintSet() *ConstraintSet { - return &ConstraintSet{ - scene: s, - } +// 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 } -// 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) +// 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 } -// 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) +// 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.notifySingleBodyCollisions() - s.notifyDoubleBodyCollisions() + s.runSimulation(elapsedSeconds * s.timeScale) + s.notifySoloCollisions() + s.notifyPairCollisions() } -func (s *Scene) Each(cb func(b Body)) { - s.eachBodyState(func(_ int, b *bodyState) { - cb(Body{ - scene: s, - reference: b.reference, +func (s *Scene) CollectSegmentBodyIntersections(segment shape3d.Segment, filter Filter, yield BodyContactCallback) { + bodyView := s.Bodies() + + s.collisionScene.CollectSegmentObjectIntersections(segment, filter, func(contact placement3d.ObjectContact) { + tgtBodyData := s.collisionScene.GetObjectUserData(contact.TargetObjectID) + + yield(BodyContact{ + TargetBodyID: bodyView.idFromIndex(tgtBodyData.index), + Contact: contact.Contact, }) }) } -func (s *Scene) CheckSegmentIntersection(segment shape3d.Segment, mask uint32) (Body, bool) { - intersection, ok := s.shapeScene.CheckSegmentIntersection(segment, placement3d.Filter{ - Mask: opt.V(mask), +func (s *Scene) CheckSegmentBodyIntersection(segment shape3d.Segment, filter Filter) (BodyContact, bool) { + var collection DeepestBodyContact + s.CollectSegmentBodyIntersections(segment, filter, collection.AddContact) + return collection.Contact() +} + +func (s *Scene) CollectSegmentTerrainIntersections(segment shape3d.Segment, filter Filter, yield TerrainContactCallback) { + terrainView := s.Terrains() + + s.collisionScene.CollectSegmentTerrainIntersections(segment, filter, func(contact placement3d.TerrainContact) { + tgtTerrainData := s.collisionScene.GetTerrainUserData(contact.TargetTerrainID) + + yield(TerrainContact{ + TargetTerrainID: terrainView.idFromIndex(tgtTerrainData.index), + Contact: contact.Contact, + }) }) - if !ok { - return Body{}, false - } - if intersection.TargetShapeID == placement3d.InvalidShapeID { - // A prop. - return Body{}, 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, - }, 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) { - // TODO: body -> acceleration targets -> impulse targets -> positioning targets -> body -> check for collisions (maybe reposition to first) +func (s *Scene) CheckSegmentTerrainIntersection(segment shape3d.Segment, filter Filter) (TerrainContact, bool) { + var collection DeepestTerrainContact + s.CollectSegmentTerrainIntersections(segment, filter, collection.AddContact) + return collection.Contact() +} - if elapsedSeconds > 0.0001 { - s.applyAcceleration(elapsedSeconds) - s.applyImpulses(elapsedSeconds) - s.applyMotion(elapsedSeconds) - s.applyNudges(elapsedSeconds) - s.detectCollisions() +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) 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( - 1.0/body.mass, - dprec.InverseMat3( - RotatedMomentOfInertia(body.momentOfInertia, body.rotation), - ), - body.position, - body.rotation, - body.velocity, - body.angularVelocity, - ) - }) +func (s *Scene) releaseGlobalAccelerator(index int32) { + s.freeGlobalAcceleratorIndices.Push(index) } -func (s *Scene) applyBodyAccelerators() { - // TODO +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) applyAreaAccelerators() { - // TODO +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) 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 - } - ctx := AccelerationContext{ - MediumVelocity: s.mediumSolver.Velocity(position), - MediumDensity: s.mediumSolver.Density(position), - } - accelerator.logic.ApplyAcceleration(ctx, target) +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) applyAerodynamicAccelerations() { - s.eachBodyState(func(index int, body *bodyState) { - if len(body.aerodynamicShapes) == 0 { - return +func (s *Scene) eachEnabledBodyAccelerator(body *bodyState, cb func(index int, accelerator *bodyAcceleratorState)) { + index := body.firstBodyAcceleratorIndex + for index != nilIndex { + accelerator := &s.bodyAccelerators[index] + if accelerator.isValid() && accelerator.isEnabled { + cb(int(index), accelerator) } - target := &s.bodyAccelerationTargets[index] - mediumDensity := s.mediumSolver.Density(body.position) - mediumVelocity := s.mediumSolver.Velocity(body.position) + 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] +} - deltaVelocity := dprec.Vec3Diff(mediumVelocity, body.velocity) - dragForce := dprec.Vec3Prod(deltaVelocity, deltaVelocity.Length()*mediumDensity*body.dragFactor) - target.ApplyForce(dragForce) +func (s *Scene) releaseSoloConstraint(index int32) { + s.freeSoloConstraintIndices.Push(index) +} - angularDragForce := dprec.Vec3Prod(body.angularVelocity, -body.angularVelocity.Length()*mediumDensity*body.angularDragFactor) - target.ApplyTorque(angularDragForce) +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) + } + } +} - 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. +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] +} - aerodynamicShape = aerodynamicShape.Transformed(bodyTransform) - relativeSpeed := dprec.QuatVec3Rotation(dprec.InverseQuat(aerodynamicShape.Rotation()), deltaVelocity) +func (s *Scene) releasePairConstraint(index int32) { + s.freePairConstraintIndices.Push(index) +} - force := aerodynamicShape.solver.Force(relativeSpeed, mediumDensity) - absoluteForce := dprec.QuatVec3Rotation(aerodynamicShape.Rotation(), force) +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) + } + } +} - offset := dprec.Vec3Diff(aerodynamicShape.Position(), bodyTransform.Position()) - target.ApplyOffsetForce(offset, absoluteForce) - // target.ApplyOffsetForce(absoluteForce, aerodynamicShape.Position()) +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) applyAccelerationTargets(elapsedSeconds float64) { - s.eachBodyState(func(index int, body *bodyState) { - target := s.bodyAccelerationTargets[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() + } + return index, &s.bodies[index] +} - linearAcceleration := target.LinearAcceleration() - if linearAcceleration.Length() > s.maxLinearAcceleration { - linearAcceleration = dprec.ResizedVec3(linearAcceleration, s.maxLinearAcceleration) +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) } - body.AddVelocity(dprec.Vec3Prod(linearAcceleration, elapsedSeconds)) + } +} - angularAcceleration := target.AngularAcceleration() - if angularAcceleration.Length() > s.maxAngularAcceleration { - angularAcceleration = dprec.ResizedVec3(angularAcceleration, s.maxAngularAcceleration) +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) } - body.AddAngularVelocity(dprec.Vec3Prod(angularAcceleration, elapsedSeconds)) + } +} + +func (s *Scene) runSimulation(elapsedSeconds float64) { + if elapsedSeconds > 0.0001 { + s.applyAcceleration(elapsedSeconds) + s.applyImpulses(elapsedSeconds) + s.applyMotion(elapsedSeconds) + s.applyNudges(elapsedSeconds) + s.applyPlacement() + s.detectCollisions() + } +} + +func (s *Scene) applyAcceleration(elapsedSeconds float64) { + defer metric.BeginRegion("acceleration").End() + + s.eachBody(func(index int, body *bodyState) { + // Create acceleration context. + ctx := AccelerationContext{ + DeltaSeconds: elapsedSeconds, + MediumVelocity: s.mediumSolver.Velocity(body.position), + MediumDensity: s.mediumSolver.Density(body.position), + Target: newAccelerationTarget(body), + } + + // Reset accumulated accelerations. + body.linearAcceleration = dprec.ZeroVec3() + body.angularAcceleration = dprec.ZeroVec3() + + // Apply global accelerators. + s.eachEnabledGlobalAccelerator(func(_ int, accelerator *globalAcceleratorState) { + accelerator.solver.ApplyAcceleration(ctx) + }) + + // Apply body accelerators. + s.eachEnabledBodyAccelerator(body, func(_ int, accelerator *bodyAcceleratorState) { + accelerator.solver.ApplyAcceleration(ctx) + }) + + // 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)) }) } 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.eachSBConstraintState(func(_ int, constraint *sbConstraintState) { - if s.resolveBodyState(constraint.body.reference) == nil { - deleteSBConstraint(s, constraint.reference) - return + // Reset constraint solvers. + 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.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.eachEnabledSoloConstraint(func(index int, constraint *soloConstraintState) { + body := &s.bodies[constraint.bodyIndex] + + ctx := SoloConstraintContext{ + DeltaSeconds: elapsedSeconds, + ImpulseBeta: s.impulseDriftAdjustmentRatio, + NudgeBeta: s.nudgeDriftAdjustmentRatio, + Target: newConstraintTarget(body), } - }) - 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, - }) - }) - 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, - }) + constraint.solver.Reset(ctx) }) - for i := 0; i < ImpulseIterationCount; i++ { - 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, - }) + // Apply impulses multiple times in a row. + for range s.impulseIterationCount { + 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) }) } - - 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.velocity, elapsedSeconds) - body.Translate(deltaPosition) - deltaRotation := dprec.Vec3Prod(body.angularVelocity, elapsedSeconds) - body.VectorRotate(deltaRotation) - - s.shapeScene.SetObjectTransform(body.objectID, shape3d.Transform{ - Translation: body.position, - Rotation: shape3d.RotationFromQuat(body.rotation), - }) + + s.eachBody(func(_ int, body *bodyState) { + // Clamp the velocity to the maximum allowed values. + body.clampLinearVelocity(s.maxLinearVelocity) + body.clampAngularVelocity(s.maxAngularVelocity) + + // 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() }) } 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 _, 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: ImpulseDriftAdjustmentRatio, - NudgeBeta: NudgeDriftAdjustmentRatio, - } - constraint.logic.Reset(ctx) - constraint.logic.ApplyNudges(ctx) - } - for _, constraint := range s.sbConstraints { - if !constraint.IsActive() { - continue + for range s.nudgeIterationCount { + 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), } - target := &s.bodyConstraintPlaceholders[constraint.body.reference.Index] - ctx := solver.Context{ - Target: target, - DeltaTime: elapsedSeconds, - ImpulseBeta: ImpulseDriftAdjustmentRatio, - NudgeBeta: NudgeDriftAdjustmentRatio, + + 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) - } - } - for i := range s.bodies { - placeholder := s.bodyConstraintPlaceholders[i] - if body := &s.bodies[i]; body.IsActive() { - s.deinitPlaceholder(&placeholder, body) - } + constraint.solver.ApplyNudges(ctx) + }) + + s.eachBody(func(_ int, body *bodyState) { + body.recalculateInertia() + }) } } +func (s *Scene) applyPlacement() { + defer metric.BeginRegion("placement").End() + + bodies := s.Bodies() + s.eachBody(func(_ int, body *bodyState) { + bodies.refreshPlacement(body) + }) +} + 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.shapeScene.CollectIntersections(s.collisionSet.AddContact) - for _, intersection := range s.collisionSet.Contacts() { - srcBodyObject := s.shapeScene.GetShapeObject(intersection.SourceShapeID) - srcBodyRef := s.shapeScene.GetObjectUserData(srcBodyObject) + s.soloCollisionConstraintIDs = s.soloCollisionConstraintIDs[:0] + s.pairCollisionConstraintIDs = s.pairCollisionConstraintIDs[:0] - if intersection.TargetMeshID == placement3d.InvalidMeshID { - tgtBodyObject := s.shapeScene.GetShapeObject(intersection.TargetShapeID) - tgtBodyRef := s.shapeScene.GetObjectUserData(tgtBodyObject) - s.detectBodyBodyCollision(srcBodyRef.index, tgtBodyRef.index, intersection) - } else { - tgtPropMesh := s.shapeScene.GetMeshUserData(intersection.TargetMeshID) - s.detectBodyPropCollision(srcBodyRef.index, tgtPropMesh.index, intersection) - } - } -} + s.soloCollisionSolvers = s.soloCollisionSolvers[:0] + s.pairCollisionSolvers = s.pairCollisionSolvers[:0] -func (s *Scene) detectBodyBodyCollision(primaryIndex, secondaryIndex uint32, intersection placement3d.Contact) { - primary := &s.bodies[primaryIndex] - secondary := &s.bodies[secondaryIndex] + // Collect new contacts. + s.objectContacts.Reset() + s.collisionScene.CollectObjectIntersections(s.objectContacts.AddContact) - solver := s.allocateDualCollisionSolver() - solver.Init(constraint.PairCollisionState{ - PrimaryNormal: intersection.TargetNormal, - PrimaryPoint: intersection.EvalSourcePoint(), - PrimaryFrictionCoefficient: primary.frictionCoefficient, - PrimaryRestitutionCoefficient: primary.restitutionCoefficient, + s.terrainContacts.Reset() + s.collisionScene.CollectTerrainIntersections(s.terrainContacts.AddContact) - SecondaryNormal: intersection.EvalSourceNormal(), - SecondaryPoint: intersection.TargetPoint, - SecondaryFrictionCoefficient: secondary.frictionCoefficient, - SecondaryRestitutionCoefficient: secondary.restitutionCoefficient, + // Handle contacts. + for _, contact := range s.objectContacts.Contacts() { + srcBodyData := s.collisionScene.GetObjectUserData(contact.SourceObjectID) + srcShapeData := s.collisionScene.GetObjectShapeUserData(contact.SourceShapeID) - Depth: intersection.Depth, - }) + tgtBodyData := s.collisionScene.GetObjectUserData(contact.TargetObjectID) + tgtShapeData := s.collisionScene.GetObjectShapeUserData(contact.TargetShapeID) - pair := dbCollisionPair{ - PrimaryRef: primary.reference, - SecondaryRef: secondary.reference, + s.handlePairCollision( + bodyCollisionData{ + srcBodyData.index, + srcShapeData.frictionCoefficient, + srcShapeData.restitutionCoefficient, + }, + bodyCollisionData{ + tgtBodyData.index, + tgtShapeData.frictionCoefficient, + tgtShapeData.restitutionCoefficient, + }, + contact, + ) } - s.newDBCollisions[pair] = struct{}{} - primaryBody := Body{ - scene: s, - reference: primary.reference, - } - secondaryBody := Body{ - scene: s, - reference: secondary.reference, + 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, + ) } - s.dbCollisionConstraints = append(s.dbCollisionConstraints, s.CreateDoubleBodyConstraint(primaryBody, secondaryBody, solver)) } -func (s *Scene) detectBodyPropCollision(bodyIndex, propIndex uint32, 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, +func (s *Scene) handlePairCollision(primaryData, secondaryData bodyCollisionData, contact placement3d.ObjectContact) { + solver := s.allocatePairCollisionSolver() + solver.Configure(PairCollisionSolverConfig{ + PrimaryFrictionCoefficient: primaryData.frictionCoefficient, + PrimaryRestitutionCoefficient: primaryData.restitutionCoefficient, + PrimaryContactNormal: contact.EvalSourceNormal(), + PrimaryContactPoint: contact.EvalSourcePoint(), - PropFrictionCoefficient: 1.0, // TODO: Take from prop or shape material - PropRestitutionCoefficient: 0.5, // TODO: Take from prop or shape material + SecondaryFrictionCoefficient: secondaryData.frictionCoefficient, + SecondaryRestitutionCoefficient: secondaryData.restitutionCoefficient, + SecondaryContactNormal: contact.TargetNormal, + SecondaryContactPoint: contact.TargetPoint, - Depth: intersection.Depth, + ContactDepth: contact.Depth, }) - pair := sbCollisionPair{ - BodyRef: primary.reference, - PropRef: secondary.reference, - } - s.newSBCollisions[pair] = struct{}{} + primaryID := s.Bodies().idFromIndex(primaryData.index) + secondaryID := s.Bodies().idFromIndex(secondaryData.index) - primaryBody := Body{ - scene: s, - reference: primary.reference, + constraintID := s.PairConstraints().Create(primaryID, secondaryID, solver) + s.pairCollisionConstraintIDs = append(s.pairCollisionConstraintIDs, constraintID) + + ref := pairCollisionRef{ + primaryBodyID: primaryID, + secondaryBodyID: secondaryID, } - s.sbCollisionConstraints = append(s.sbCollisionConstraints, s.CreateSingleBodyConstraint(primaryBody, solver)) + s.newPairCollisionRefs[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{}) +func (s *Scene) handleSoloCollision(bodyData bodyCollisionData, terrainData terrainCollisionData, contact placement3d.TerrainContact) { + solver := s.allocateSoloCollisionSolver() + solver.Configure(SoloCollisionSolverConfig{ + TerrainFrictionCoefficient: terrainData.frictionCoefficient, + TerrainRestitutionCoefficient: terrainData.restitutionCoefficient, + TerrainContactNormal: contact.TargetNormal, + + BodyFrictionCoefficient: bodyData.frictionCoefficient, + BodyRestitutionCoefficient: bodyData.restitutionCoefficient, + BodyContactPoint: contact.EvalSourcePoint(), + + ContactDepth: contact.Depth, + }) + + bodyID := s.Bodies().idFromIndex(bodyData.index) + terrainID := s.Terrains().idFromIndex(terrainData.index) + + constraintID := s.SoloConstraints().Create(bodyID, solver) + s.soloCollisionConstraintIDs = append(s.soloCollisionConstraintIDs, constraintID) + + ref := soloCollisionRef{ + bodyID: bodyID, + terrainID: terrainID, } - return &s.sbCollisionSolvers[len(s.sbCollisionSolvers)-1] + s.newSoloCollisionRefs[ref] = struct{}{} } -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) 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) nextRevision() uint32 { - s.freeRevision++ - return s.freeRevision -} - -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, - } - s.sbCollisionSubscriptions.Each(func(callback SingleBodyCollisionCallback) { - callback(primary, prop, 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, - } - s.sbCollisionSubscriptions.Each(func(callback SingleBodyCollisionCallback) { - callback(primary, prop, false) - }) - } - } - clear(s.oldSBCollisions) - maps.Copy(s.oldSBCollisions, s.newSBCollisions) - clear(s.newSBCollisions) +func (s *Scene) allocatePairCollisionSolver() *PairCollisionSolver { + index := len(s.pairCollisionSolvers) + s.pairCollisionSolvers = append(s.pairCollisionSolvers, PairCollisionSolver{}) + return &s.pairCollisionSolvers[index] } -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, - } - s.dbCollisionSubscriptions.Each(func(callback DoubleBodyCollisionCallback) { - callback(primary, secondary, true) +func (s *Scene) allocateSoloCollisionSolver() *SoloCollisionSolver { + index := len(s.soloCollisionSolvers) + s.soloCollisionSolvers = append(s.soloCollisionSolvers, SoloCollisionSolver{}) + return &s.soloCollisionSolvers[index] +} + +func (s *Scene) notifySoloCollisions() { + for newRef := range s.newSoloCollisionRefs { + if _, ok := s.oldSoloCollisionRefs[newRef]; !ok { + s.soloCollisionSubscriptions.Each(func(callback SoloCollisionCallback) { + callback(newRef.bodyID, newRef.terrainID, 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, - } - s.dbCollisionSubscriptions.Each(func(callback DoubleBodyCollisionCallback) { - callback(primary, secondary, false) + for oldRef := range s.oldSoloCollisionRefs { + if _, ok := s.newSoloCollisionRefs[oldRef]; !ok { + s.soloCollisionSubscriptions.Each(func(callback SoloCollisionCallback) { + callback(oldRef.bodyID, oldRef.terrainID, false) }) } } - clear(s.oldDBCollisions) - maps.Copy(s.oldDBCollisions, s.newDBCollisions) - clear(s.newDBCollisions) + clear(s.oldSoloCollisionRefs) + maps.Copy(s.oldSoloCollisionRefs, s.newSoloCollisionRefs) + clear(s.newSoloCollisionRefs) } -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) notifyPairCollisions() { + for newRef := range s.newPairCollisionRefs { + if _, ok := s.oldPairCollisionRefs[newRef]; !ok { + s.pairCollisionSubscriptions.Each(func(callback PairCollisionCallback) { + callback(newRef.primaryBodyID, newRef.secondaryBodyID, true) + }) } } -} - -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) + for oldRef := range s.oldPairCollisionRefs { + if _, ok := s.newPairCollisionRefs[oldRef]; !ok { + s.pairCollisionSubscriptions.Each(func(callback PairCollisionCallback) { + callback(oldRef.primaryBodyID, oldRef.secondaryBodyID, false) + }) } } + clear(s.oldPairCollisionRefs) + maps.Copy(s.oldPairCollisionRefs, s.newPairCollisionRefs) + clear(s.newPairCollisionRefs) } -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) - } - } -} +var nilIndex int32 = -1 -func (s *Scene) resolveBodyState(reference indexReference) *bodyState { - state := &s.bodies[reference.Index] - if !state.IsActive() || state.reference.Revision != reference.Revision { - return nil - } - return state +type bodyData struct { + index int32 } -func (s *Scene) initPlaceholder(placeholder *solver.Placeholder, body *bodyState) { - placeholder.Init(solver.PlaceholderState{ - Mass: body.mass, - MomentOfInertia: body.momentOfInertia, - LinearVelocity: body.velocity, - AngularVelocity: body.angularVelocity, - Position: body.position, - Rotation: body.rotation, - }) +type shapeData struct { + frictionCoefficient float64 + restitutionCoefficient float64 } -func (s *Scene) deinitPlaceholder(placeholder *solver.Placeholder, body *bodyState) { - body.velocity = placeholder.LinearVelocity() - body.angularVelocity = placeholder.AngularVelocity() - body.position = placeholder.Position() - body.rotation = placeholder.Rotation() - - s.shapeScene.SetObjectTransform(body.objectID, shape3d.Transform{ - Translation: body.position, - Rotation: shape3d.RotationFromQuat(body.rotation), - }) +type terrainData struct { + index int32 } -type bodyRef struct { - index uint32 +type bodyCollisionData struct { + index int32 + frictionCoefficient float64 + restitutionCoefficient float64 } -type propRef struct { - index uint32 +type terrainCollisionData struct { + index int32 + frictionCoefficient float64 + restitutionCoefficient float64 } -type sbCollisionPair struct { - BodyRef indexReference - PropRef indexReference +// TODO: Consider tracking the shapeID as well. +type soloCollisionRef struct { + bodyID BodyID + terrainID TerrainID } -type dbCollisionPair struct { - PrimaryRef indexReference - SecondaryRef indexReference +// TODO: Consider tracking the shapeID as well. +type pairCollisionRef struct { + primaryBodyID BodyID + secondaryBodyID BodyID } 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 -) diff --git a/game/physics/solver/change.go b/game/physics/solver/change.go deleted file mode 100644 index 3d6ac7c8..00000000 --- a/game/physics/solver/change.go +++ /dev/null @@ -1,23 +0,0 @@ -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/context.go b/game/physics/solver/context.go deleted file mode 100644 index c2c8a1e6..00000000 --- a/game/physics/solver/context.go +++ /dev/null @@ -1,96 +0,0 @@ -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 { - 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 1955b275..00000000 --- a/game/physics/solver/jacobian.go +++ /dev/null @@ -1,93 +0,0 @@ -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 { - 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/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/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/solver_airfoil.go b/game/physics/solver_airfoil.go new file mode 100644 index 00000000..dadc0513 --- /dev/null +++ b/game/physics/solver_airfoil.go @@ -0,0 +1,297 @@ +package physics + +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 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 + surfaceArea float64 + stallAngle dprec.Angle + liftCoefficient float64 +} + +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 + s.surfaceArea = max(0.0, config.SurfaceArea) + s.stallAngle = max(0.0, config.StallAngle) + 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(), + airfoilOffsetWS, + ), + ) + + relWindVelocity := dprec.Vec3Diff(ctx.MediumVelocity, airfoilVelocity) + relWindVelocityLng := relWindVelocity.Length() + if relWindVelocityLng < Epsilon { + return // no significant wind + } + + 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)) + } +} + +// 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 + } + 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 +} 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 + } +} 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 +} 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) +} 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 + } +} diff --git a/game/physics/solver_ball_joint.go b/game/physics/solver_ball_joint.go new file mode 100644 index 00000000..4f38248b --- /dev/null +++ b/game/physics/solver_ball_joint.go @@ -0,0 +1,151 @@ +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. +// +// 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 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) + s.solverZ.Reset(ctx) +} + +// ApplyImpulses implements [PairConstraintSolver.ApplyImpulses]. +// +// 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) + s.solverZ.ApplyImpulses(ctx) +} + +// ApplyNudges implements [PairConstraintSolver.ApplyNudges]. +// +// 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) + s.solverZ.ApplyNudges(ctx) +} diff --git a/game/physics/solver_coilover.go b/game/physics/solver_coilover.go new file mode 100644 index 00000000..e3e1cb4f --- /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 = s.relaxedLength - actualDistance +} + +// 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) {} diff --git a/game/physics/solver_collision_pair.go b/game/physics/solver_collision_pair.go new file mode 100644 index 00000000..a9b569e2 --- /dev/null +++ b/game/physics/solver_collision_pair.go @@ -0,0 +1,232 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +// PairCollisionSolverConfig holds the parameters with which a +// [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 +// 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 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 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 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, either through +// [NewPairCollisionSolver] or [PairCollisionSolver.Configure], before +// being registered with a [Scene] through [PairConstraintView.Create]. +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) + +// 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. +// +// 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 + s.secondaryContactPoint = config.SecondaryContactPoint + s.contactDepth = config.ContactDepth + + s.frictionCoefficient = CombinedFrictionCoefficient( + config.PrimaryFrictionCoefficient, + config.SecondaryFrictionCoefficient, + ) + s.restitutionCoefficient = CombinedRestitutionCoefficient( + config.PrimaryRestitutionCoefficient, + config.SecondaryRestitutionCoefficient, + ) +} + +// Reset implements [PairConstraintSolver.Reset]. +// +// It recomputes the contact's primary and secondary [Jacobian]s, the +// same way [PairCollisionSolver.recompute] does. +func (s *PairCollisionSolver) Reset(ctx PairConstraintContext) { + s.recompute(ctx) +} + +// 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) + 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) +} + +// ApplyNudges implements [PairConstraintSolver.ApplyNudges]. +// +// 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 new file mode 100644 index 00000000..f230db59 --- /dev/null +++ b/game/physics/solver_collision_solo.go @@ -0,0 +1,187 @@ +package physics + +import "github.com/mokiat/gomath/dprec" + +// SoloCollisionSolverConfig holds the parameters with which a +// [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 +// the per-surface material properties (friction, restitution) that are +// combined into the solver's effective coefficients. +type SoloCollisionSolverConfig struct { + + // 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 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 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, either through +// [NewSoloCollisionSolver] or [SoloCollisionSolver.Configure], before +// being registered with a [Scene] through [SoloConstraintView.Create]. +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) + +// 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. +// +// 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 + + s.frictionCoefficient = CombinedFrictionCoefficient( + config.BodyFrictionCoefficient, + config.TerrainFrictionCoefficient, + ) + s.restitutionCoefficient = CombinedRestitutionCoefficient( + config.BodyRestitutionCoefficient, + config.TerrainRestitutionCoefficient, + ) +} + +// Reset implements [SoloConstraintSolver.Reset]. +// +// It recomputes the contact's [Jacobian], the same way +// [SoloCollisionSolver.recompute] does. +func (s *SoloCollisionSolver) Reset(ctx SoloConstraintContext) { + s.recompute(ctx) +} + +// 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) + 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 frictionImpulse Impulse + if lng := pointLateralVelocity.Length(); lng > Epsilon { + velocityLateralDirection := dprec.UnitVec3(pointLateralVelocity) + frictionJacobian := Jacobian{ + 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) + 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(frictionImpulse) +} + +// ApplyNudges implements [SoloConstraintSolver.ApplyNudges]. +// +// 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 new file mode 100644 index 00000000..cee55d0a --- /dev/null +++ b/game/physics/solver_composite_pair.go @@ -0,0 +1,74 @@ +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.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.ApplyNudges(ctx) + } +} diff --git a/game/physics/solver_composite_solo.go b/game/physics/solver_composite_solo.go new file mode 100644 index 00000000..6e77a071 --- /dev/null +++ b/game/physics/solver_composite_solo.go @@ -0,0 +1,74 @@ +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.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.ApplyNudges(ctx) + } +} 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(), + )) +} diff --git a/game/physics/solver_copy_position.go b/game/physics/solver_copy_position.go new file mode 100644 index 00000000..a51d9e57 --- /dev/null +++ b/game/physics/solver_copy_position.go @@ -0,0 +1,52 @@ +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; 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; +// [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()) +} 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()) +} 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) +} 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 +} 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)) + } +} diff --git a/game/physics/solver_fixed_distance.go b/game/physics/solver_fixed_distance.go new file mode 100644 index 00000000..29614db6 --- /dev/null +++ b/game/physics/solver_fixed_distance.go @@ -0,0 +1,172 @@ +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] and current distance error +// (drift), the same way [FixedDistanceSolver.recompute] does. +func (s *FixedDistanceSolver) Reset(ctx SoloConstraintContext) { + s.recompute(ctx) +} + +// 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 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 +} diff --git a/game/physics/solver_fixed_position.go b/game/physics/solver_fixed_position.go new file mode 100644 index 00000000..1e3c1998 --- /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 rotation - see [FixedRotationSolver] for that. +// +// 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) +} 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) +} diff --git a/game/physics/solver_gravity.go b/game/physics/solver_gravity.go index 55528c77..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() { 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) +} 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) +} diff --git a/game/physics/terrain.go b/game/physics/terrain.go new file mode 100644 index 00000000..1c59e1df --- /dev/null +++ b/game/physics/terrain.go @@ -0,0 +1,180 @@ +package physics + +import ( + "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() TerrainID { + index, terrain := v.scene.allocateTerrain() + + terrainID := v.scene.collisionScene.CreateTerrain(placement3d.TerrainInfo[terrainData]{ + UserData: terrainData{ + index: index, + }, + }) + + *terrain = terrainState{ + terrainID: terrainID, + revision: terrain.revision + 1, // progress revision to valid (odd) value + } + + return TerrainID{ + index: index, + revision: terrain.revision, + } +} + +// 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() TerrainHandle { + return v.Handle(v.Create()) +} + +func (v TerrainView) Delete(id TerrainID) { + terrain := v.resolve(id, true) + + v.scene.collisionScene.DeleteTerrain(terrain.terrainID) + + *terrain = terrainState{ + terrainID: placement3d.NilTerrainID, + revision: terrain.revision + 1, // progress revision to invalid (even) value + } + + 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{ + index: int32(index), + revision: terrain.revision, + }) + }) +} + +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) 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{ + index: index, + revision: terrain.revision, + } +} + +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 +} + +// 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 +} + +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 { + terrainID placement3d.TerrainID + revision int32 +} + +func (s *terrainState) isValid() bool { + return s.revision%2 == 1 // only odd revisions are valid +} diff --git a/game/physics/util.go b/game/physics/util.go new file mode 100644 index 00000000..0f14ecca --- /dev/null +++ b/game/physics/util.go @@ -0,0 +1,69 @@ +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() + if dprec.Abs(radians) < Epsilon { + return dprec.IdentityQuat() + } + 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. +// +// 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/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 }