Havok: Add hkpConstraintInternal stub and getters/setters

lib/hkStubs/Havok/Common/Base/Types/hkBaseTypes.h

@@ -3,6 +3,7 @@
 #include <Havok/Common/Base/Types/hkBaseDefs.h>
 #include <cstddef>
 #include <cstdint>
+#include <type_traits>
 
 using hkFloat32 = float;
 using hkDouble64 = double;
@@ -213,6 +214,17 @@ HK_FORCE_INLINE hkLong hkGetByteOffset(const void* base, const void* pntr) {
     return hkLong(pntr) - hkLong(base);
 }
 
+template <typename T>
+using hkAddConstPointer =
+    std::conditional_t<std::is_pointer_v<T>,
+                       std::add_pointer_t<std::add_const_t<std::remove_pointer_t<T>>>,
+                       std::add_const_t<T>>;
+
+template <typename T>
+HK_ALWAYS_INLINE T hkSelectOther(hkAddConstPointer<T> a, T pairA, T pairB) {
+    return reinterpret_cast<T>(hkUlong(a) ^ hkUlong(pairA) ^ hkUlong(pairB));
+}
+
 class hkClass;
 
 struct hkVariant {

lib/hkStubs/Havok/Physics/Constraint/hkpConstraintInstance.h

@@ -1,8 +1,11 @@
 #pragma once
 
 #include <Havok/Common/Base/hkBase.h>
+#include <Havok/Physics/Constraint/Data/hkpConstraintInfo.h>
 
+struct hkpConstraintAtom;
 class hkpConstraintData;
+struct hkConstraintInternal;
 class hkpConstraintListener;
 class hkpConstraintOwner;
 struct hkpConstraintRuntime;
@@ -101,8 +104,8 @@ public:
     virtual void entityRemovedCallback(hkpEntity* entity);
     virtual void entityDeletedCallback(hkpEntity* entity);
 
-    inline void setOwner(hkpConstraintOwner* island);
+    inline void setOwner(hkpConstraintOwner* owner);
-    inline struct hkConstraintInternal* getInternal();
+    inline hkConstraintInternal* getInternal();
 
     void pointNullsToFixedRigidBody();
     void setFixedRigidBodyPointersToZero(hkpWorld* world);
@@ -138,10 +141,52 @@ public:
     hkSmallArray<hkpConstraintListener*> m_listeners;
     hkStringPtr m_name;
     hkUlong m_userData;
-    struct hkConstraintInternal* m_internal;
+    hkConstraintInternal* m_internal;
     hkUint32 m_uid;
 };
 
+struct alignas(16) hkConstraintInternal {
+    HK_DECLARE_CLASS_ALLOCATOR(hkConstraintInternal)
+
+    inline hkpEntity* getOtherEntity(const hkpEntity* entity) const;
+    inline hkpEntity* getMasterEntity() const;
+    inline hkpEntity* getSlaveEntity() const;
+
+    inline hkpConstraintAtom* getAtoms() const { return m_atoms; }
+    inline hkUint16 getAtomsSize() const { return m_atomsSize; }
+
+    inline void getConstraintInfo(hkpConstraintInfo& info) const;
+    inline void addConstraintInfo(const hkpConstraintInfo& delta);
+    inline void subConstraintInfo(const hkpConstraintInfo& delta);
+    inline void clearConstraintInfo();
+
+    hkpConstraintInstance* m_constraint;
+    hkpEntity* m_entities[2];
+
+protected:
+    friend class hkpSaveContactPointsUtil;
+    friend class hkpWorldConstraintUtil;
+
+    hkpConstraintAtom* m_atoms;
+    hkUint16 m_atomsSize;
+
+public:
+    hkUint8 m_callbackRequest;
+
+    hkEnum<hkpConstraintInstance::ConstraintPriority, hkUint8> m_priority;
+
+    hkUint16 m_sizeOfSchemas;
+    hkUint16 m_numSolverResults;
+    hkUint16 m_numSolverElemTemps;
+
+    hkUint8 m_whoIsMaster;
+    hkEnum<hkpConstraintInstance::InstanceType, hkUint8> m_constraintType;
+
+    hkpConstraintRuntime* m_runtime;
+    hkUint16 m_runtimeSize;
+    hkObjectIndex m_slaveIndex;
+};
+
 inline const hkpConstraintData* hkpConstraintInstance::getData() const {
     return m_data;
 }
@@ -149,3 +194,128 @@ inline const hkpConstraintData* hkpConstraintInstance::getData() const {
 inline hkpConstraintData* hkpConstraintInstance::getDataRw() const {
     return m_data;
 }
+
+inline hkpEntity* hkpConstraintInstance::getEntityA() const {
+    return m_entities[0];
+}
+
+inline hkpEntity* hkpConstraintInstance::getEntity(int index) const {
+    return m_entities[index];
+}
+
+inline hkpEntity* hkpConstraintInstance::getEntityB() const {
+    return m_entities[1];
+}
+
+inline hkpRigidBody* hkpConstraintInstance::getRigidBodyA() const {
+    return reinterpret_cast<hkpRigidBody*>(m_entities[0]);
+}
+
+inline hkpRigidBody* hkpConstraintInstance::getRigidBodyB() const {
+    return reinterpret_cast<hkpRigidBody*>(m_entities[1]);
+}
+
+inline hkpEntity* hkpConstraintInstance::getOtherEntity(const hkpEntity* entity) const {
+    return hkSelectOther(entity, m_entities[0], m_entities[1]);
+}
+
+inline hkpModifierConstraintAtom* hkpConstraintInstance::getConstraintModifiers() const {
+    return m_constraintModifiers;
+}
+
+inline hkpConstraintRuntime* hkpConstraintInstance::getRuntime() const {
+    return m_internal->m_runtime;
+}
+
+inline void hkpConstraintInstance::setWantRuntime(hkBool b) {
+    m_wantRuntime = b;
+}
+
+inline hkBool hkpConstraintInstance::getWantRuntime() const {
+    return m_wantRuntime;
+}
+
+inline hkUlong hkpConstraintInstance::getUserData() const {
+    return m_userData;
+}
+
+inline void hkpConstraintInstance::setUserData(hkUlong data) {
+    m_userData = data;
+}
+
+inline hkpConstraintInstance::ConstraintPriority hkpConstraintInstance::getPriority() const {
+    return m_priority;
+}
+
+inline const char* hkpConstraintInstance::getName() const {
+    return m_name;
+}
+
+inline void hkpConstraintInstance::setName(const char* name) {
+    m_name = name;
+}
+
+inline hkpConstraintOwner* hkpConstraintInstance::getOwner() const {
+    return m_owner;
+}
+
+inline hkpEntity* hkpConstraintInstance::getMasterEntity() const {
+    return m_internal->getMasterEntity();
+}
+
+inline hkpEntity* hkpConstraintInstance::getSlaveEntity() const {
+    return m_internal->getSlaveEntity();
+}
+
+inline void hkpConstraintInstance::setOwner(hkpConstraintOwner* owner) {
+    m_owner = owner;
+}
+
+inline hkConstraintInternal* hkpConstraintInstance::getInternal() {
+    return m_internal;
+}
+
+inline hkpEntity* hkConstraintInternal::getOtherEntity(const hkpEntity* entity) const {
+    return hkSelectOther(entity, m_entities[0], m_entities[1]);
+}
+
+inline hkpEntity* hkConstraintInternal::getMasterEntity() const {
+    return m_entities[m_whoIsMaster];
+}
+
+inline hkpEntity* hkConstraintInternal::getSlaveEntity() const {
+    return m_entities[1 - m_whoIsMaster];
+}
+
+inline void hkConstraintInternal::getConstraintInfo(hkpConstraintInfo& info) const {
+    info.m_maxSizeOfSchema = m_sizeOfSchemas;
+    info.m_sizeOfSchemas = m_sizeOfSchemas;
+    info.m_numSolverResults = m_numSolverResults;
+    info.m_numSolverElemTemps = m_numSolverElemTemps;
+}
+
+inline void hkConstraintInternal::addConstraintInfo(const hkpConstraintInfo& delta) {
+    const hkUint32 numSolverResults = m_numSolverResults + delta.m_numSolverResults;
+    const hkUint32 numSolverElemTemps = m_numSolverElemTemps + delta.m_numSolverElemTemps;
+    const hkUint32 sizeOfSchemas = m_sizeOfSchemas + delta.m_sizeOfSchemas;
+
+    m_numSolverResults = hkUint16(numSolverResults);
+    m_numSolverElemTemps = hkUint16(numSolverElemTemps);
+    m_sizeOfSchemas = hkUint16(sizeOfSchemas);
+}
+
+inline void hkConstraintInternal::subConstraintInfo(const hkpConstraintInfo& delta) {
+    const hkUint32 numSolverResults = m_numSolverResults - delta.m_numSolverResults;
+    const hkUint32 numSolverElemTemps = m_numSolverElemTemps - delta.m_numSolverElemTemps;
+    const hkUint32 sizeOfSchemas = m_sizeOfSchemas - delta.m_sizeOfSchemas;
+
+    m_numSolverResults = hkUint16(numSolverResults);
+    m_numSolverElemTemps = hkUint16(numSolverElemTemps);
+    m_sizeOfSchemas = hkUint16(sizeOfSchemas);
+}
+
+inline void hkConstraintInternal::clearConstraintInfo() {
+    m_numSolverResults = 0;
+    m_numSolverElemTemps = 0;
+    m_sizeOfSchemas = 0;
+}