Havok: Add hkpContactListener and related events

lib/hkStubs/CMakeLists.txt

@@ -57,6 +57,7 @@ add_library(hkStubs OBJECT
   Havok/Common/Base/Types/hkRefVariant.h
   Havok/Common/Base/Types/Geometry/Aabb/hkAabb.h
   Havok/Common/Base/Types/Physics/hkStepInfo.h
+  Havok/Common/Base/Types/Physics/ContactPoint/hkContactPointMaterial.h
   Havok/Common/Base/Types/Physics/MotionState/hkMotionState.h
   Havok/Common/Base/Types/Properties/hkSimpleProperty.h
 
@@ -71,7 +72,9 @@ add_library(hkStubs OBJECT
   Havok/Physics/Constraint/Atom/hkpConstraintAtom.h
   Havok/Physics/Constraint/Data/hkpConstraintData.h
   Havok/Physics/Constraint/Data/hkpConstraintInfo.h
+  Havok/Physics/ConstraintSolver/Accumulator/hkpVelocityAccumulator.h
   Havok/Physics/ConstraintSolver/Solve/hkpSolverInfo.h
+  Havok/Physics/ConstraintSolver/Solve/hkpSolverResults.h
 
   Havok/Physics2012/Collide/Agent/hkpCollisionInput.h
   Havok/Physics2012/Collide/Agent/hkpCollisionQualityInfo.h
@@ -108,10 +111,14 @@ add_library(hkStubs OBJECT
 
   Havok/Physics2012/Dynamics/Action/hkpAction.h
   Havok/Physics2012/Dynamics/Collide/hkpResponseModifier.h
+  Havok/Physics2012/Dynamics/Collide/ContactListener/hkpCollisionEvent.h
+  Havok/Physics2012/Dynamics/Collide/ContactListener/hkpContactListener.h
+  Havok/Physics2012/Dynamics/Collide/ContactListener/hkpContactPointEvent.h
   Havok/Physics2012/Dynamics/Common/hkpMaterial.h
   Havok/Physics2012/Dynamics/Common/hkpProperty.h
   Havok/Physics2012/Dynamics/Constraint/hkpConstraintInstance.h
   Havok/Physics2012/Dynamics/Constraint/hkpConstraintOwner.h
+  Havok/Physics2012/Dynamics/Constraint/Contact/hkpContactPointProperties.h
   Havok/Physics2012/Dynamics/Entity/hkpEntity.h
   Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h
   Havok/Physics2012/Dynamics/Entity/hkpRigidBodyCinfo.h

lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h

@@ -101,7 +101,9 @@ public:
 
     // ========== Sign, comparisons, clamping
 
-    void setAbs(hkVector4fParameter a);
+    HK_FORCE_INLINE void setAbs(hkVector4fParameter a);
+    template <int N>
+    HK_FORCE_INLINE void setNeg(hkVector4fParameter a);
 
     // ========== Matrix operations (out-of-line)
 

lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl

@@ -312,6 +312,41 @@ inline void hkVector4f::setAbs(hkVector4fParameter a) {
 #endif
 }
 
+template <int N>
+inline void hkVector4f::setNeg(hkVector4fParameter a) {
+    static_assert(1 <= N && N <= 4, "invalid N");
+
+#ifdef HK_VECTOR4F_AARCH64_NEON
+    switch (N) {
+    case 1: {
+        auto xy = vget_low_f32(a.v);
+        v = vsetq_lane_f32(vget_lane_f32(vneg_f32(xy), 0), a.v, 0);
+        break;
+    }
+    case 2: {
+        auto xy = vget_low_f32(a.v);
+        auto zw = vget_high_f32(a.v);
+        v = vcombine_f32(vneg_f32(xy), zw);
+        break;
+    }
+    case 3: {
+        auto zw = vget_high_f32(a.v);
+        v = vnegq_f32(v);
+        v = vsetq_lane_f32(vget_lane_f32(zw, 1), v, 3);
+        break;
+    }
+    case 4:
+        v = vnegq_f32(a.v);
+        break;
+    default:
+        break;
+    }
+#else
+    for (int i = 0; i < N; ++i)
+        v[i] = -a[i];
+#endif
+}
+
 inline void hkVector4f::_setRotatedDir(const hkMatrix3f& a, hkVector4fParameter b) {
 #ifdef HK_VECTOR4F_AARCH64_NEON
     auto col0 = vmulq_laneq_f32(a.m_col0.v, b.v, 0);

lib/hkStubs/Havok/Common/Base/Math/hkMath.h

@@ -22,4 +22,8 @@ HK_FORCE_INLINE T1 max2(T1 x, T2 y) {
     return x > static_cast<T1>(y) ? x : static_cast<T1>(y);
 }
 
+HK_FORCE_INLINE int hkToIntFast(hkFloat32 r) {
+    return int(r);
+}
+
 }  // namespace hkMath

lib/hkStubs/Havok/Common/Base/Types/Physics/ContactPoint/hkContactPoint.h

@@ -0,0 +1,113 @@
+#pragma once
+
+#include <Havok/Common/Base/hkBase.h>
+
+using hkContactPointId = hkUint16;
+
+constexpr hkContactPointId HK_INVALID_CONTACT_POINT = 0xffff;
+
+class hkContactPoint {
+public:
+    HK_DECLARE_CLASS_ALLOCATOR(hkContactPoint)
+    HK_DECLARE_REFLECTION()
+
+    inline const hkVector4& getPosition() const;
+    inline void setPosition(const hkVector4& position);
+
+    inline const hkVector4& getNormal() const;
+    inline const hkVector4& getSeparatingNormal() const;
+    inline void setSeparatingNormal(const hkVector4& normal, hkReal dist);
+    inline void setSeparatingNormal(hkVector4Parameter normal, hkSimdRealParameter dist);
+    inline void setSeparatingNormal(const hkVector4& separatingNormal);
+    inline void setNormalOnly(const hkVector4& normal);
+
+    inline hkReal getDistance() const;
+    inline hkSimdReal getDistanceSimdReal() const;
+    inline void setDistance(hkReal d);
+    inline void setDistanceSimdReal(hkSimdRealParameter newValue);
+
+    inline void set(const hkVector4& position, const hkVector4& normal, hkReal dist);
+    inline void setPositionNormalAndDistance(hkVector4Parameter position, hkVector4Parameter normal,
+                                             hkSimdRealParameter dist);
+
+    inline void setFlipped(const hkContactPoint& point);
+    inline void flip();
+
+protected:
+    hkVector4 m_position;
+    // w = distance
+    hkVector4 m_separatingNormal;
+};
+
+inline const hkVector4& hkContactPoint::getPosition() const {
+    return m_position;
+}
+
+inline void hkContactPoint::setPosition(const hkVector4& position) {
+    m_position = position;
+}
+
+inline const hkVector4& hkContactPoint::getNormal() const {
+    return m_separatingNormal;
+}
+
+inline const hkVector4& hkContactPoint::getSeparatingNormal() const {
+    return m_separatingNormal;
+}
+
+inline void hkContactPoint::setSeparatingNormal(const hkVector4& normal, hkReal dist) {
+    m_separatingNormal = normal;
+    m_separatingNormal(3) = dist;
+}
+
+inline void hkContactPoint::setSeparatingNormal(hkVector4Parameter normal,
+                                                hkSimdRealParameter dist) {
+    m_separatingNormal.setXYZ_W(normal, dist);
+}
+
+inline void hkContactPoint::setSeparatingNormal(const hkVector4& separatingNormal) {
+    m_separatingNormal = separatingNormal;
+}
+
+inline void hkContactPoint::setNormalOnly(const hkVector4& normal) {
+    m_separatingNormal.setXYZ(normal);
+}
+
+inline hkReal hkContactPoint::getDistance() const {
+    return m_separatingNormal(3);
+}
+
+inline hkSimdReal hkContactPoint::getDistanceSimdReal() const {
+    return m_separatingNormal.getComponent<3>();
+}
+
+inline void hkContactPoint::setDistance(hkReal d) {
+    m_separatingNormal(3) = d;
+}
+
+inline void hkContactPoint::setDistanceSimdReal(hkSimdRealParameter newValue) {
+    m_separatingNormal.setW(newValue);
+}
+
+inline void hkContactPoint::set(const hkVector4& position, const hkVector4& normal, hkReal dist) {
+    m_position = position;
+    m_separatingNormal = normal;
+    m_separatingNormal(3) = dist;
+}
+
+inline void hkContactPoint::setPositionNormalAndDistance(hkVector4Parameter position,
+                                                         hkVector4Parameter normal,
+                                                         hkSimdRealParameter dist) {
+    m_position = position;
+    m_separatingNormal.setXYZ_W(normal, dist);
+}
+
+inline void hkContactPoint::setFlipped(const hkContactPoint& point) {
+    m_position.setAddMul(point.m_position, point.m_separatingNormal,
+                         point.m_separatingNormal.getComponent<3>());
+    m_separatingNormal.setNeg<3>(point.m_separatingNormal);
+}
+
+inline void hkContactPoint::flip() {
+    setFlipped(*this);
+}

lib/hkStubs/Havok/Common/Base/Types/Physics/ContactPoint/hkContactPointMaterial.h

@@ -0,0 +1,82 @@
+#pragma once
+
+#include <Havok/Common/Base/hkBase.h>
+
+class hkContactPointMaterial {
+public:
+    HK_DECLARE_CLASS_ALLOCATOR(hkContactPointMaterial)
+    HK_DECLARE_REFLECTION()
+
+    enum FlagEnum {
+        CONTACT_IS_NEW = 1 << 0,
+        CONTACT_USES_SOLVER_PATH2 = 1 << 1,
+        CONTACT_BREAKOFF_OBJECT_ID_SMALLER = 1 << 2,
+        CONTACT_IS_DISABLED = 1 << 3,
+    };
+
+    inline hkReal getFriction() const;
+    inline void setFriction(hkReal r);
+
+    inline hkReal getRestitution() const;
+    inline void setRestitution(hkReal r);
+
+    inline hkUlong getUserData() const;
+    inline void setUserData(hkUlong data);
+
+    inline hkBool32 isPotential();
+
+    inline hkReal getMaxImpulsePerStep();
+    inline void setMaxImpulsePerStep(hkUFloat8 impulse);
+
+    inline void reset();
+
+    hkUlong m_userData;
+    hkUFloat8 m_friction;
+    hkUint8 m_restitution;
+    hkUFloat8 m_maxImpulse;
+    hkUint8 m_flags;
+};
+
+inline hkReal hkContactPointMaterial::getFriction() const {
+    return m_friction;
+}
+
+inline void hkContactPointMaterial::setFriction(hkReal r) {
+    m_friction = float(r);
+}
+
+inline hkReal hkContactPointMaterial::getRestitution() const {
+    return float(m_restitution) * (1.0f / 128.0f);
+}
+
+inline void hkContactPointMaterial::setRestitution(hkReal r) {
+    m_restitution = hkUchar(hkMath::hkToIntFast(r * 128.0f));
+}
+
+inline hkUlong hkContactPointMaterial::getUserData() const {
+    return m_userData;
+}
+
+inline void hkContactPointMaterial::setUserData(hkUlong data) {
+    m_userData = data;
+}
+
+inline hkBool32 hkContactPointMaterial::isPotential() {
+    return m_flags & CONTACT_IS_NEW;
+}
+
+inline hkReal hkContactPointMaterial::getMaxImpulsePerStep() {
+    return m_maxImpulse;
+}
+
+inline void hkContactPointMaterial::setMaxImpulsePerStep(hkUFloat8 maxImpulse) {
+    m_flags &= ~hkContactPointMaterial::CONTACT_USES_SOLVER_PATH2;
+    m_maxImpulse = maxImpulse;
+}
+
+inline void hkContactPointMaterial::reset() {
+    m_friction.m_value = 0;
+    m_maxImpulse.m_value = 0;
+    m_restitution = 0;
+    m_flags = CONTACT_IS_NEW;
+}

lib/hkStubs/Havok/Physics/ConstraintSolver/Accumulator/hkpVelocityAccumulator.h

@@ -0,0 +1,35 @@
+#pragma once
+
+#include <Havok/Common/Base/hkBase.h>
+
+/// This is a simplified rigid body as it is used by the constraint solver.
+/// Note: This class has different purposes, whether it is used for setting up the constraint solver
+/// input (hkConstraintDate::buildJacobian()), or when it is used as a solver output.
+class hkpVelocityAccumulator {
+public:
+    HK_DECLARE_CLASS_ALLOCATOR(hkpVelocityAccumulator)
+
+    enum hkpAccumulatorType {
+        HK_RIGID_BODY,
+        HK_KEYFRAMED_RIGID_BODY,
+        HK_NO_GRAVITY_RIGID_BODY,
+        HK_END
+    };
+
+    enum hkpAccumulatorContext {
+        ACCUMULATOR_CONTEXT_BUILD_JACOBIANS,
+        ACCUMULATOR_CONTEXT_SOLVER,
+    };
+
+    hkEnum<hkpAccumulatorType, hkUint8> m_type;
+    hkEnum<hkpAccumulatorContext, hkUint8> m_context;
+    hkUint32 m_deactivationClass;
+    hkReal m_gravityFactor;
+    hkVector4 m_linearVel;
+    hkVector4 m_angularVel;
+    hkVector4 m_invMasses;
+    hkVector4 m_scratch0;
+    hkVector4 m_scratch1;
+    hkVector4 m_scratch2;
+    hkVector4 m_scratch3;
+};

lib/hkStubs/Havok/Physics/ConstraintSolver/Solve/hkpSolverResults.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include <Havok/Common/Base/hkBase.h>
+
+class hkpSolverResults {
+public:
+    HK_DECLARE_CLASS_ALLOCATOR(hkpSolverResults)
+
+    HK_FORCE_INLINE hkReal getImpulseApplied() const;
+
+    hkReal m_impulseApplied = 0.0;
+    hkReal m_internalSolverData = 0.0;
+};
+
+hkReal hkpSolverResults::getImpulseApplied() const {
+    return m_impulseApplied;
+}

lib/hkStubs/Havok/Physics2012/Dynamics/Collide/ContactListener/hkpCollisionEvent.h

@@ -0,0 +1,45 @@
+#pragma once
+
+#include <Havok/Common/Base/hkBase.h>
+#include <Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h>
+#include <Havok/Physics2012/Dynamics/World/hkpSimulationIsland.h>
+
+class hkpSimpleConstraintContactMgr;
+
+class hkpCollisionEvent {
+public:
+    HK_DECLARE_CLASS_ALLOCATOR(hkpCollisionEvent)
+
+    enum CallbackSource {
+        SOURCE_A = 0,
+        SOURCE_B = 1,
+        SOURCE_WORLD,
+    };
+
+    inline hkpCollisionEvent(CallbackSource source, hkpRigidBody* bodyA, hkpRigidBody* bodyB,
+                             hkpSimpleConstraintContactMgr* mgr);
+
+    inline hkpRigidBody* getBody(int bodyIdx) const;
+    inline hkpSimulationIsland* getSimulationIsland() const;
+
+    CallbackSource m_source;
+    hkpRigidBody* m_bodies[2];
+    hkpSimpleConstraintContactMgr* m_contactMgr;
+};
+
+inline hkpCollisionEvent::hkpCollisionEvent(hkpCollisionEvent::CallbackSource source,
+                                            hkpRigidBody* bodyA, hkpRigidBody* bodyB,
+                                            hkpSimpleConstraintContactMgr* mgr)
+    : m_source(source), m_bodies{bodyA, bodyB}, m_contactMgr(mgr) {}
+
+inline hkpRigidBody* hkpCollisionEvent::getBody(int bodyIdx) const {
+    return m_bodies[bodyIdx];
+}
+
+inline hkpSimulationIsland* hkpCollisionEvent::getSimulationIsland() const {
+    auto* island = m_bodies[0]->getSimulationIsland();
+    if (island->isFixed()) {
+        island = m_bodies[1]->getSimulationIsland();
+    }
+    return island;
+}

lib/hkStubs/Havok/Physics2012/Dynamics/Collide/ContactListener/hkpContactListener.h

@@ -0,0 +1,30 @@
+#pragma once
+
+class hkpCollisionEvent;
+class hkpContactPointAddedEvent;
+class hkpContactPointEvent;
+class hkpContactPointRemovedEvent;
+class hkpContactProcessEvent;
+
+class hkpContactListener {
+public:
+    virtual void contactPointCallback(const hkpContactPointEvent& event) {}
+
+    virtual void collisionAddedCallback(const hkpCollisionEvent& event) {}
+
+    virtual void collisionRemovedCallback(const hkpCollisionEvent& event) {}
+
+    void registerForEndOfStepContactPointCallbacks(const hkpCollisionEvent& event);
+
+    void unregisterForEndOfStepContactPointCallbacks(const hkpCollisionEvent& event);
+
+    virtual ~hkpContactListener() = default;
+
+public:
+    // Deprecated.
+    virtual void contactPointAddedCallback(hkpContactPointAddedEvent& event) {}
+    // Deprecated.
+    virtual void contactPointRemovedCallback(hkpContactPointRemovedEvent& event) {}
+    // Deprecated.
+    virtual void contactProcessCallback(hkpContactProcessEvent& event) {}
+};

lib/hkStubs/Havok/Physics2012/Dynamics/Collide/ContactListener/hkpContactPointEvent.h

@@ -0,0 +1,92 @@
+#pragma once
+
+#include <Havok/Common/Base/Types/Physics/ContactPoint/hkContactPoint.h>
+#include <Havok/Common/Base/hkBase.h>
+#include <Havok/Physics2012/Dynamics/Collide/ContactListener/hkpCollisionEvent.h>
+
+class hkpContactPointProperties;
+class hkpVelocityAccumulator;
+
+class hkpContactPointEvent : public hkpCollisionEvent {
+public:
+    HK_DECLARE_CLASS_ALLOCATOR(hkpContactPointEvent)
+
+    enum Type {
+        TYPE_TOI,
+        TYPE_EXPAND_MANIFOLD,
+        TYPE_MANIFOLD,
+        TYPE_MANIFOLD_AT_END_OF_STEP,
+        TYPE_MANIFOLD_FROM_SAVED_CONTACT_POINT,
+    };
+
+    inline hkpContactPointEvent(CallbackSource source, hkpRigidBody* a, hkpRigidBody* b,
+                                hkpSimpleConstraintContactMgr* mgr, Type type, hkContactPoint* cp,
+                                hkpContactPointProperties* cpp, hkReal* sepVel, hkReal* rotNorm,
+                                hkBool fullManifold, hkBool firstCallback, hkBool lastCallback,
+                                hkpShapeKey* keyStorage, hkpVelocityAccumulator* accA,
+                                hkpVelocityAccumulator* accB);
+
+    inline hkBool isToi() const;
+
+    inline hkpShapeKey* getShapeKeys(int bodyIdx) const;
+
+    hkContactPointId getContactPointId() const;
+
+    inline void setNormal(const hkVector4& normal);
+
+    inline hkReal getSeparatingVelocity() const;
+    inline void setSeparatingVelocity(hkReal sepVel) const;
+
+    inline hkReal getRotateNormal() const;
+    inline void setRotateNormal(hkReal rotNorm) const;
+
+    inline void accessVelocities(int bodyIdx) const;
+    inline void updateVelocities(int bodyIdx) const;
+
+    Type m_type;
+    hkContactPoint* m_contactPoint;
+    hkpContactPointProperties* m_contactPointProperties;
+    hkBool m_firingCallbacksForFullManifold;
+    hkBool m_firstCallbackForFullManifold;
+    hkBool m_lastCallbackForFullManifold;
+
+protected:
+    hkReal* m_separatingVelocity;
+    hkReal* m_rotateNormal;
+    hkpShapeKey* m_shapeKeyStorage;
+    hkpVelocityAccumulator* m_accumulators[2];
+};
+
+inline hkpContactPointEvent::hkpContactPointEvent(
+    CallbackSource source, hkpRigidBody* a, hkpRigidBody* b, hkpSimpleConstraintContactMgr* mgr,
+    Type type, hkContactPoint* cp, hkpContactPointProperties* cpp, hkReal* sepVel, hkReal* rotNorm,
+    hkBool fullManifold, hkBool firstCallback, hkBool lastCallback, hkpShapeKey* keyStorage,
+    hkpVelocityAccumulator* accA, hkpVelocityAccumulator* accB)
+    : hkpCollisionEvent(source, a, b, mgr), m_type(type), m_contactPoint(cp),
+      m_contactPointProperties(cpp), m_firingCallbacksForFullManifold(fullManifold),
+      m_firstCallbackForFullManifold(firstCallback), m_lastCallbackForFullManifold(lastCallback),
+      m_separatingVelocity(sepVel), m_rotateNormal(rotNorm), m_shapeKeyStorage(keyStorage) {
+    m_accumulators[0] = accA;
+    m_accumulators[1] = accB;
+}
+
+inline hkBool hkpContactPointEvent::isToi() const {
+    return m_type <= TYPE_TOI;
+}
+
+inline hkpShapeKey* hkpContactPointEvent::getShapeKeys(int bodyIdx) const {
+    hkpShapeKey* shapeKeys = nullptr;
+    if (getBody(bodyIdx)->m_numShapeKeysInContactPointProperties > 0) {
+        shapeKeys =
+            m_shapeKeyStorage + bodyIdx * m_bodies[0]->m_numShapeKeysInContactPointProperties;
+    }
+    return shapeKeys;
+}
+
+inline hkReal hkpContactPointEvent::getRotateNormal() const {
+    return *m_rotateNormal;
+}
+
+inline void hkpContactPointEvent::setRotateNormal(hkReal rotNorm) const {
+    *m_rotateNormal = rotNorm;
+}

lib/hkStubs/Havok/Physics2012/Dynamics/Constraint/Contact/hkpContactPointProperties.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#include <Havok/Common/Base/Types/Physics/ContactPoint/hkContactPointMaterial.h>
+#include <Havok/Common/Base/hkBase.h>
+#include <Havok/Physics/ConstraintSolver/Solve/hkpSolverResults.h>
+
+class hkpSimpleContactConstraintAtom;
+
+class hkpContactPointProperties : public hkpSolverResults, public hkContactPointMaterial {
+public:
+    HK_DECLARE_CLASS_ALLOCATOR(hkpContactPointProperties)
+
+    using UserData = hkUint32;
+
+    hkReal m_internalDataA;
+};