mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add BallAndSocketJointComponents class

Browse Source
This commit is contained in:
Daniel Chappuis 2019-09-02 14:15:03 +02:00
parent cef1f6cd22
commit 8187c19fa3
13 changed files with 666 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -147,6 +147,7 @@ SET (REACTPHYSICS3D_HEADERS
"src/components/TransformComponents.h"
"src/components/ProxyShapeComponents.h"
"src/components/JointComponents.h"
"src/components/BallAndSocketJointComponents.h"
"src/collision/CollisionCallback.h"
"src/collision/OverlapCallback.h"
"src/mathematics/mathematics.h"
@ -240,6 +241,7 @@ SET (REACTPHYSICS3D_SOURCES
"src/components/TransformComponents.cpp"
"src/components/ProxyShapeComponents.cpp"
"src/components/JointComponents.cpp"
"src/components/BallAndSocketJointComponents.cpp"
"src/collision/CollisionCallback.cpp"
"src/collision/OverlapCallback.cpp"
"src/mathematics/mathematics_functions.cpp"

3
src/collision/shapes/CollisionShape.h
View File

@ -123,9 +123,6 @@ class CollisionShape {
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const=0;
/// Return the scaling vector of the collision shape
Vector3 getLocalScaling() const;
/// Return the id of the shape
uint getId() const;

2
src/collision/shapes/ConcaveMeshShape.h
View File

@ -175,7 +175,7 @@ class ConcaveMeshShape : public ConcaveShape {
ConcaveMeshShape(TriangleMesh* triangleMesh, const Vector3& scaling = Vector3(1, 1, 1));
/// Destructor
virtual ~ConcaveMeshShape() = default;
virtual ~ConcaveMeshShape() override = default;
/// Deleted copy-constructor
ConcaveMeshShape(const ConcaveMeshShape& shape) = delete;

2
src/collision/shapes/SphereShape.h
View File

@ -110,7 +110,7 @@ inline decimal SphereShape::getRadius() const {
* @return False because the sphere shape is not a polyhedron
*/
inline bool SphereShape::isPolyhedron() const {
return false;
return false;
}
// Return the number of bytes used by the collision shape

226
src/components/BallAndSocketJointComponents.cpp Normal file
View File

@ -0,0 +1,226 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "BallAndSocketJointComponents.h"
#include "engine/EntityManager.h"
#include "mathematics/Matrix3x3.h"
#include <cassert>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
BallAndSocketJointComponents::BallAndSocketJointComponents(MemoryAllocator& allocator)
:Components(allocator, sizeof(Entity) + sizeof(BallAndSocketJoint*) + sizeof(Vector3) +
sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
sizeof(Matrix3x3) + sizeof(Matrix3x3) + sizeof(Vector3) +
sizeof(Matrix3x3) + sizeof(Vector3)) {
// Allocate memory for the components data
allocate(INIT_NB_ALLOCATED_COMPONENTS);
}
// Allocate memory for a given number of components
void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) {
assert(nbComponentsToAllocate > mNbAllocatedComponents);
// Size for the data of a single component (in bytes)
const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize;
// Allocate memory
void* newBuffer = mMemoryAllocator.allocate(totalSizeBytes);
assert(newBuffer != nullptr);
// New pointers to components data
Entity* newJointEntities = static_cast<Entity*>(newBuffer);
BallAndSocketJoint** newJoints = reinterpret_cast<BallAndSocketJoint**>(newJointEntities + nbComponentsToAllocate);
Vector3* newLocalAnchorPointBody1 = reinterpret_cast<Vector3*>(newJoints + nbComponentsToAllocate);
Vector3* newLocalAnchorPointBody2 = reinterpret_cast<Vector3*>(newLocalAnchorPointBody1 + nbComponentsToAllocate);
Vector3* newR1World = reinterpret_cast<Vector3*>(newLocalAnchorPointBody2 + nbComponentsToAllocate);
Vector3* newR2World = reinterpret_cast<Vector3*>(newR1World + nbComponentsToAllocate);
Matrix3x3* newI1 = reinterpret_cast<Matrix3x3*>(newR2World + nbComponentsToAllocate);
Matrix3x3* newI2 = reinterpret_cast<Matrix3x3*>(newI1 + nbComponentsToAllocate);
Vector3* newBiasVector = reinterpret_cast<Vector3*>(newI2 + nbComponentsToAllocate);
Matrix3x3* newInverseMassMatrix = reinterpret_cast<Matrix3x3*>(newBiasVector + nbComponentsToAllocate);
Vector3* newImpulse = reinterpret_cast<Vector3*>(newInverseMassMatrix + nbComponentsToAllocate);
// If there was already components before
if (mNbComponents > 0) {
// Copy component data from the previous buffer to the new one
memcpy(newJointEntities, mJointEntities, mNbComponents * sizeof(Entity));
memcpy(newJoints, mJoints, mNbComponents * sizeof(BallAndSocketJoint*));
memcpy(newLocalAnchorPointBody1, mLocalAnchorPointBody1, mNbComponents * sizeof(Vector3));
memcpy(newLocalAnchorPointBody2, mLocalAnchorPointBody2, mNbComponents * sizeof(Vector3));
memcpy(newR1World, mR1World, mNbComponents * sizeof(Vector3));
memcpy(newR2World, mR2World, mNbComponents * sizeof(Vector3));
memcpy(newI1, mI1, mNbComponents * sizeof(Matrix3x3));
memcpy(newI2, mI2, mNbComponents * sizeof(Matrix3x3));
memcpy(newBiasVector, mBiasVector, mNbComponents * sizeof(Vector3));
memcpy(newInverseMassMatrix, mInverseMassMatrix, mNbComponents * sizeof(Matrix3x3));
memcpy(newImpulse, mImpulse, mNbComponents * sizeof(Vector3));
// Deallocate previous memory
mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize);
}
mBuffer = newBuffer;
mJointEntities = newJointEntities;
mJoints = newJoints;
mNbAllocatedComponents = nbComponentsToAllocate;
mLocalAnchorPointBody1 = newLocalAnchorPointBody1;
mLocalAnchorPointBody2 = newLocalAnchorPointBody2;
mR1World = newR1World;
mR2World = newR2World;
mI1 = newI1;
mI2 = newI2;
mBiasVector = newBiasVector;
mInverseMassMatrix = newInverseMassMatrix;
mImpulse = newImpulse;
}
// Add a component
void BallAndSocketJointComponents::addComponent(Entity jointEntity, bool isSleeping, const BallAndSocketJointComponent& component) {
// Prepare to add new component (allocate memory if necessary and compute insertion index)
uint32 index = prepareAddComponent(isSleeping);
// Insert the new component data
new (mJointEntities + index) Entity(jointEntity);
mJoints[index] = nullptr;
new (mLocalAnchorPointBody1 + index) Vector3(0, 0, 0);
new (mLocalAnchorPointBody2 + index) Vector3(0, 0, 0);
new (mR1World + index) Vector3(0, 0, 0);
new (mR2World + index) Vector3(0, 0, 0);
new (mI1 + index) Matrix3x3();
new (mI2 + index) Matrix3x3();
new (mBiasVector + index) Vector3(0, 0, 0);
new (mInverseMassMatrix + index) Matrix3x3();
new (mImpulse + index) Vector3(0, 0, 0);
// Map the entity with the new component lookup index
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(jointEntity, index));
mNbComponents++;
assert(mDisabledStartIndex <= mNbComponents);
assert(mNbComponents == static_cast<uint32>(mMapEntityToComponentIndex.size()));
}
// Move a component from a source to a destination index in the components array
// The destination location must contain a constructed object
void BallAndSocketJointComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) {
const Entity entity = mJointEntities[srcIndex];
// Copy the data of the source component to the destination location
new (mJointEntities + destIndex) Entity(mJointEntities[srcIndex]);
mJoints[destIndex] = mJoints[srcIndex];
new (mLocalAnchorPointBody1 + destIndex) Vector3(mLocalAnchorPointBody1[srcIndex]);
new (mLocalAnchorPointBody2 + destIndex) Vector3(mLocalAnchorPointBody2[srcIndex]);
new (mR1World + destIndex) Vector3(mR1World[srcIndex]);
new (mR2World + destIndex) Vector3(mR2World[srcIndex]);
new (mI1 + destIndex) Matrix3x3(mI1[srcIndex]);
new (mI2 + destIndex) Matrix3x3(mI2[srcIndex]);
new (mBiasVector + destIndex) Vector3(mBiasVector[srcIndex]);
new (mInverseMassMatrix + destIndex) Matrix3x3(mInverseMassMatrix[srcIndex]);
new (mImpulse + destIndex) Vector3(mImpulse[srcIndex]);
// Destroy the source component
destroyComponent(srcIndex);
assert(!mMapEntityToComponentIndex.containsKey(entity));
// Update the entity to component index mapping
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(entity, destIndex));
assert(mMapEntityToComponentIndex[mJointEntities[destIndex]] == destIndex);
}
// Swap two components in the array
void BallAndSocketJointComponents::swapComponents(uint32 index1, uint32 index2) {
// Copy component 1 data
Entity jointEntity1(mJointEntities[index1]);
BallAndSocketJoint* joint1 = mJoints[index1];
Vector3 localAnchorPointBody1(mLocalAnchorPointBody1[index1]);
Vector3 localAnchorPointBody2(mLocalAnchorPointBody2[index1]);
Vector3 r1World1(mR1World[index1]);
Vector3 r2World1(mR2World[index1]);
Matrix3x3 i11(mI1[index1]);
Matrix3x3 i21(mI2[index1]);
Vector3 biasVector1(mBiasVector[index1]);
Matrix3x3 inverseMassMatrix1(mInverseMassMatrix[index1]);
Vector3 impulse1(mImpulse[index1]);
// Destroy component 1
destroyComponent(index1);
moveComponentToIndex(index2, index1);
// Reconstruct component 1 at component 2 location
new (mJointEntities + index2) Entity(jointEntity1);
mJoints[index2] = joint1;
new (mLocalAnchorPointBody1 + index2) Vector3(localAnchorPointBody1);
new (mLocalAnchorPointBody2 + index2) Vector3(localAnchorPointBody2);
new (mR1World + index2) Vector3(r1World1);
new (mR2World + index2) Vector3(r2World1);
new (mI1 + index2) Matrix3x3(i11);
new (mI2 + index2) Matrix3x3(i21);
new (mBiasVector + index2) Vector3(biasVector1);
new (mInverseMassMatrix + index2) Matrix3x3(inverseMassMatrix1);
new (mImpulse + index2) Vector3(impulse1);
// Update the entity to component index mapping
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(jointEntity1, index2));
assert(mMapEntityToComponentIndex[mJointEntities[index1]] == index1);
assert(mMapEntityToComponentIndex[mJointEntities[index2]] == index2);
assert(mNbComponents == static_cast<uint32>(mMapEntityToComponentIndex.size()));
}
// Destroy a component at a given index
void BallAndSocketJointComponents::destroyComponent(uint32 index) {
Components::destroyComponent(index);
assert(mMapEntityToComponentIndex[mJointEntities[index]] == index);
mMapEntityToComponentIndex.remove(mJointEntities[index]);
mJointEntities[index].~Entity();
mJoints[index] = nullptr;
mLocalAnchorPointBody1[index].~Vector3();
mLocalAnchorPointBody2[index].~Vector3();
mR1World[index].~Vector3();
mR2World[index].~Vector3();
mI1[index].~Matrix3x3();
mI2[index].~Matrix3x3();
mBiasVector[index].~Vector3();
mInverseMassMatrix[index].~Matrix3x3();
mImpulse[index].~Vector3();
}

331
src/components/BallAndSocketJointComponents.h Normal file
View File

@ -0,0 +1,331 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_BALL_AND_SOCKET_JOINT_COMPONENTS_H
#define REACTPHYSICS3D_BALL_AND_SOCKET_JOINT_COMPONENTS_H
// Libraries
#include "mathematics/Transform.h"
#include "mathematics/Matrix3x3.h"
#include "engine/Entity.h"
#include "components/Components.h"
#include "containers/Map.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
// Class declarations
class MemoryAllocator;
class EntityManager;
class BallAndSocketJoint;
enum class JointType;
// Class BallAndSocketJointComponents
/**
* This class represent the component of the ECS with data for the BallAndSocketJoint.
*/
class BallAndSocketJointComponents : public Components {
private:
// -------------------- Attributes -------------------- //
/// Array of joint entities
Entity* mJointEntities;
/// Array of pointers to the joints
BallAndSocketJoint** mJoints;
/// Anchor point of body 1 (in local-space coordinates of body 1)
Vector3* mLocalAnchorPointBody1;
/// Anchor point of body 2 (in local-space coordinates of body 2)
Vector3* mLocalAnchorPointBody2;
/// Vector from center of body 2 to anchor point in world-space
Vector3* mR1World;
/// Vector from center of body 2 to anchor point in world-space
Vector3* mR2World;
/// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3* mI1;
/// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3* mI2;
/// Bias vector for the constraint
Vector3* mBiasVector;
/// Inverse mass matrix K=JM^-1J^-t of the constraint
Matrix3x3* mInverseMassMatrix;
/// Accumulated impulse
Vector3* mImpulse;
// -------------------- Methods -------------------- //
/// Allocate memory for a given number of components
virtual void allocate(uint32 nbComponentsToAllocate) override;
/// Destroy a component at a given index
virtual void destroyComponent(uint32 index) override;
/// Move a component from a source to a destination index in the components array
virtual void moveComponentToIndex(uint32 srcIndex, uint32 destIndex) override;
/// Swap two components in the array
virtual void swapComponents(uint32 index1, uint32 index2) override;
public:
/// Structure for the data of a transform component
struct BallAndSocketJointComponent {
/// Constructor
BallAndSocketJointComponent() {
}
};
// -------------------- Methods -------------------- //
/// Constructor
BallAndSocketJointComponents(MemoryAllocator& allocator);
/// Destructor
virtual ~BallAndSocketJointComponents() override = default;
/// Add a component
void addComponent(Entity jointEntity, bool isSleeping, const BallAndSocketJointComponent& component);
/// Return a pointer to a given joint
BallAndSocketJoint* getJoint(Entity jointEntity) const;
/// Set the joint pointer to a given joint
void setJoint(Entity jointEntity, BallAndSocketJoint* joint) const;
/// Return the local anchor point of body 1 for a given joint
const Vector3& getLocalAnchoirPointBody1(Entity jointEntity) const;
/// Set the local anchor point of body 1 for a given joint
void setLocalAnchoirPointBody1(Entity jointEntity, const Vector3& localAnchoirPointBody1);
/// Return the local anchor point of body 2 for a given joint
const Vector3& getLocalAnchoirPointBody2(Entity jointEntity) const;
/// Set the local anchor point of body 2 for a given joint
void setLocalAnchoirPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2);
/// Return the vector from center of body 1 to anchor point in world-space
const Vector3& getR1World(Entity jointEntity) const;
/// Set the vector from center of body 1 to anchor point in world-space
void setR1World(Entity jointEntity, const Vector3& r1World);
/// Return the vector from center of body 2 to anchor point in world-space
const Vector3& getR2World(Entity jointEntity) const;
/// Set the vector from center of body 2 to anchor point in world-space
void setR2World(Entity jointEntity, const Vector3& r2World);
/// Return the inertia tensor of body 1 (in world-space coordinates)
const Matrix3x3& getI1(Entity jointEntity) const;
/// Set the inertia tensor of body 1 (in world-space coordinates)
void setI1(Entity jointEntity, const Matrix3x3& i1);
/// Return the inertia tensor of body 2 (in world-space coordinates)
const Matrix3x3& getI2(Entity jointEntity) const;
/// Set the inertia tensor of body 2 (in world-space coordinates)
void setI2(Entity jointEntity, const Matrix3x3& i2);
/// Return the bias vector for the constraint
Vector3& getBiasVector(Entity jointEntity);
/// Set the bias vector for the constraint
void setBiasVector(Entity jointEntity, const Vector3& biasVector);
/// Return the inverse mass matrix K=JM^-1J^-t of the constraint
Matrix3x3& getInverseMassMatrix(Entity jointEntity);
/// Set the inverse mass matrix K=JM^-1J^-t of the constraint
void setInverseMassMatrix(Entity jointEntity, const Matrix3x3& inverseMassMatrix);
/// Return the accumulated impulse
Vector3& getImpulse(Entity jointEntity);
/// Set the accumulated impulse
void setImpulse(Entity jointEntity, const Vector3& impulse);
// -------------------- Friendship -------------------- //
friend class BroadPhaseSystem;
};
// Return a pointer to a given joint
inline BallAndSocketJoint* BallAndSocketJointComponents::getJoint(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mJoints[mMapEntityToComponentIndex[jointEntity]];
}
// Set the joint pointer to a given joint
inline void BallAndSocketJointComponents::setJoint(Entity jointEntity, BallAndSocketJoint* joint) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mJoints[mMapEntityToComponentIndex[jointEntity]] = joint;
}
// Return the local anchor point of body 1 for a given joint
inline const Vector3& BallAndSocketJointComponents::getLocalAnchoirPointBody1(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]];
}
// Set the local anchor point of body 1 for a given joint
inline void BallAndSocketJointComponents::setLocalAnchoirPointBody1(Entity jointEntity, const Vector3& localAnchoirPointBody1) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]] = localAnchoirPointBody1;
}
// Return the local anchor point of body 2 for a given joint
inline const Vector3& BallAndSocketJointComponents::getLocalAnchoirPointBody2(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]];
}
// Set the local anchor point of body 2 for a given joint
inline void BallAndSocketJointComponents::setLocalAnchoirPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]] = localAnchoirPointBody2;
}
// Return the vector from center of body 1 to anchor point in world-space
inline const Vector3& BallAndSocketJointComponents::getR1World(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mR1World[mMapEntityToComponentIndex[jointEntity]];
}
// Set the vector from center of body 1 to anchor point in world-space
inline void BallAndSocketJointComponents::setR1World(Entity jointEntity, const Vector3& r1World) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mR1World[mMapEntityToComponentIndex[jointEntity]] = r1World;
}
// Return the vector from center of body 2 to anchor point in world-space
inline const Vector3& BallAndSocketJointComponents::getR2World(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mR2World[mMapEntityToComponentIndex[jointEntity]];
}
// Set the vector from center of body 2 to anchor point in world-space
inline void BallAndSocketJointComponents::setR2World(Entity jointEntity, const Vector3& r2World) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mR2World[mMapEntityToComponentIndex[jointEntity]] = r2World;
}
// Return the inertia tensor of body 1 (in world-space coordinates)
inline const Matrix3x3& BallAndSocketJointComponents::getI1(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mI1[mMapEntityToComponentIndex[jointEntity]];
}
// Set the inertia tensor of body 1 (in world-space coordinates)
inline void BallAndSocketJointComponents::setI1(Entity jointEntity, const Matrix3x3& i1) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mI1[mMapEntityToComponentIndex[jointEntity]] = i1;
}
// Return the inertia tensor of body 2 (in world-space coordinates)
inline const Matrix3x3& BallAndSocketJointComponents::getI2(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mI2[mMapEntityToComponentIndex[jointEntity]];
}
// Set the inertia tensor of body 2 (in world-space coordinates)
inline void BallAndSocketJointComponents::setI2(Entity jointEntity, const Matrix3x3& i2) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mI2[mMapEntityToComponentIndex[jointEntity]] = i2;
}
// Return the bias vector for the constraint
inline Vector3 &BallAndSocketJointComponents::getBiasVector(Entity jointEntity) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mBiasVector[mMapEntityToComponentIndex[jointEntity]];
}
// Set the bias vector for the constraint
inline void BallAndSocketJointComponents::setBiasVector(Entity jointEntity, const Vector3& biasVector) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mBiasVector[mMapEntityToComponentIndex[jointEntity]] = biasVector;
}
// Return the inverse mass matrix K=JM^-1J^-t of the constraint
inline Matrix3x3& BallAndSocketJointComponents::getInverseMassMatrix(Entity jointEntity) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mInverseMassMatrix[mMapEntityToComponentIndex[jointEntity]];
}
// Set the inverse mass matrix K=JM^-1J^-t of the constraint
inline void BallAndSocketJointComponents::setInverseMassMatrix(Entity jointEntity, const Matrix3x3& inverseMassMatrix) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mInverseMassMatrix[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix;
}
// Return the accumulated impulse
inline Vector3 &BallAndSocketJointComponents::getImpulse(Entity jointEntity) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mImpulse[mMapEntityToComponentIndex[jointEntity]];
}
// Set the accumulated impulse
inline void BallAndSocketJointComponents::setImpulse(Entity jointEntity, const Vector3& impulse) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mImpulse[mMapEntityToComponentIndex[jointEntity]] = impulse;
}
}
#endif

135
src/constraint/BallAndSocketJoint.cpp
View File

@ -36,15 +36,15 @@ const decimal BallAndSocketJoint::BETA = decimal(0.2);
// Constructor
BallAndSocketJoint::BallAndSocketJoint(Entity entity, DynamicsWorld& world, const BallAndSocketJointInfo& jointInfo)
: Joint(entity, world, jointInfo), mImpulse(Vector3(0, 0, 0)) {
: Joint(entity, world, jointInfo) {
// Get the transforms of the two bodies
Transform& body1Transform = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity());
Transform& body2Transform = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity());
// Compute the local-space anchor point for each body
mLocalAnchorPointBody1 = body1Transform.getInverse() * jointInfo.anchorPointWorldSpace;
mLocalAnchorPointBody2 = body2Transform.getInverse() * jointInfo.anchorPointWorldSpace;
mWorld.mBallAndSocketJointsComponents.setLocalAnchoirPointBody1(entity, body1Transform.getInverse() * jointInfo.anchorPointWorldSpace);
mWorld.mBallAndSocketJointsComponents.setLocalAnchoirPointBody2(entity, body2Transform.getInverse() * jointInfo.anchorPointWorldSpace);
}
// Initialize before solving the constraint
@ -65,46 +65,55 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
const Quaternion& orientationBody2 = body2->getTransform().getOrientation();
// Get the inertia tensor of bodies
mI1 = body1->getInertiaTensorInverseWorld();
mI2 = body2->getInertiaTensorInverseWorld();
mWorld.mBallAndSocketJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld());
mWorld.mBallAndSocketJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld());
// Compute the vector from body center to the anchor point in world-space
mR1World = orientationBody1 * mLocalAnchorPointBody1;
mR2World = orientationBody2 * mLocalAnchorPointBody2;
const Vector3 localAnchorPointBody1 = mWorld.mBallAndSocketJointsComponents.getLocalAnchoirPointBody1(mEntity);
const Vector3 localAnchorPointBody2 = mWorld.mBallAndSocketJointsComponents.getLocalAnchoirPointBody2(mEntity);
mWorld.mBallAndSocketJointsComponents.setR1World(mEntity, orientationBody1 * localAnchorPointBody1);
mWorld.mBallAndSocketJointsComponents.setR2World(mEntity, orientationBody2 * localAnchorPointBody2);
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
const Vector3& r1World = mWorld.mBallAndSocketJointsComponents.getR1World(mEntity);
const Vector3& r2World = mWorld.mBallAndSocketJointsComponents.getR2World(mEntity);
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix)
decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1->getEntity());
decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2->getEntity());
decimal inverseMassBodies = body1MassInverse + body2MassInverse;
const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1->getEntity());
const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2->getEntity());
const decimal inverseMassBodies = body1MassInverse + body2MassInverse;
const Matrix3x3& i1 = mWorld.mBallAndSocketJointsComponents.getI1(mEntity);
const Matrix3x3& i2 = mWorld.mBallAndSocketJointsComponents.getI2(mEntity);
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
skewSymmetricMatrixU1 * i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * i2 * skewSymmetricMatrixU2.getTranspose();
// Compute the inverse mass matrix K^-1
mInverseMassMatrix.setToZero();
Matrix3x3& inverseMassMatrix = mWorld.mBallAndSocketJointsComponents.getInverseMassMatrix(mEntity);
inverseMassMatrix.setToZero();
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mInverseMassMatrix = massMatrix.getInverse();
mWorld.mBallAndSocketJointsComponents.setInverseMassMatrix(mEntity, massMatrix.getInverse());
}
// Compute the bias "b" of the constraint
mBiasVector.setToZero();
Vector3& biasVector = mWorld.mBallAndSocketJointsComponents.getBiasVector(mEntity);
biasVector.setToZero();
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
decimal biasFactor = (BETA / constraintSolverData.timeStep);
mBiasVector = biasFactor * (x2 + mR2World - x1 - mR1World);
mWorld.mBallAndSocketJointsComponents.setBiasVector(mEntity, biasFactor * (x2 + r2World - x1 - r1World));
}
// If warm-starting is not enabled
if (!constraintSolverData.isWarmStartingActive) {
// Reset the accumulated impulse
mImpulse.setToZero();
Vector3& impulse = mWorld.mBallAndSocketJointsComponents.getImpulse(mEntity);
impulse.setToZero();
}
}
@ -123,20 +132,27 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD
Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
const Vector3& r1World = mWorld.mBallAndSocketJointsComponents.getR1World(mEntity);
const Vector3& r2World = mWorld.mBallAndSocketJointsComponents.getR2World(mEntity);
const Matrix3x3& i1 = mWorld.mBallAndSocketJointsComponents.getI1(mEntity);
const Matrix3x3& i2 = mWorld.mBallAndSocketJointsComponents.getI2(mEntity);
// Compute the impulse P=J^T * lambda for the body 1
const Vector3 linearImpulseBody1 = -mImpulse;
const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World);
const Vector3& impulse = mWorld.mBallAndSocketJointsComponents.getImpulse(mEntity);
const Vector3 linearImpulseBody1 = -impulse;
const Vector3 angularImpulseBody1 = impulse.cross(r1World);
// Apply the impulse to the body 1
v1 += constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity) * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
w1 += i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the body 2
const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World);
const Vector3 angularImpulseBody2 = -impulse.cross(r2World);
// Apply the impulse to the body to the body 2
v2 += constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity) * mImpulse;
w2 += mI2 * angularImpulseBody2;
v2 += constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity) * impulse;
w2 += i2 * angularImpulseBody2;
}
// Solve the velocity constraint
@ -154,27 +170,36 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con
Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
const Vector3& r1World = mWorld.mBallAndSocketJointsComponents.getR1World(mEntity);
const Vector3& r2World = mWorld.mBallAndSocketJointsComponents.getR2World(mEntity);
const Matrix3x3& i1 = mWorld.mBallAndSocketJointsComponents.getI1(mEntity);
const Matrix3x3& i2 = mWorld.mBallAndSocketJointsComponents.getI2(mEntity);
const Matrix3x3& inverseMassMatrix = mWorld.mBallAndSocketJointsComponents.getInverseMassMatrix(mEntity);
const Vector3& biasVector = mWorld.mBallAndSocketJointsComponents.getBiasVector(mEntity);
// Compute J*v
const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World);
const Vector3 Jv = v2 + w2.cross(r2World) - v1 - w1.cross(r1World);
// Compute the Lagrange multiplier lambda
const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector);
mImpulse += deltaLambda;
const Vector3 deltaLambda = inverseMassMatrix * (-Jv - biasVector);
mWorld.mBallAndSocketJointsComponents.setImpulse(mEntity, mWorld.mBallAndSocketJointsComponents.getImpulse(mEntity) + deltaLambda);
// Compute the impulse P=J^T * lambda for the body 1
const Vector3 linearImpulseBody1 = -deltaLambda;
const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
const Vector3 angularImpulseBody1 = deltaLambda.cross(r1World);
// Apply the impulse to the body 1
v1 += constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity) * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
w1 += i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the body 2
const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
const Vector3 angularImpulseBody2 = -deltaLambda.cross(r2World);
// Apply the impulse to the body 2
v2 += constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity) * deltaLambda;
w2 += mI2 * angularImpulseBody2;
w2 += i2 * angularImpulseBody2;
}
// Solve the position constraint (for position error correction)
@ -201,46 +226,53 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
const Vector3& r1World = mWorld.mBallAndSocketJointsComponents.getR1World(mEntity);
const Vector3& r2World = mWorld.mBallAndSocketJointsComponents.getR2World(mEntity);
const Matrix3x3& i1 = mWorld.mBallAndSocketJointsComponents.getI1(mEntity);
const Matrix3x3& i2 = mWorld.mBallAndSocketJointsComponents.getI2(mEntity);
// Recompute the inverse inertia tensors
mI1 = body1->getInertiaTensorInverseWorld();
mI2 = body2->getInertiaTensorInverseWorld();
mWorld.mBallAndSocketJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld());
mWorld.mBallAndSocketJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld());
// Compute the vector from body center to the anchor point in world-space
mR1World = q1 * mLocalAnchorPointBody1;
mR2World = q2 * mLocalAnchorPointBody2;
mWorld.mBallAndSocketJointsComponents.setR1World(mEntity, q1 * mWorld.mBallAndSocketJointsComponents.getLocalAnchoirPointBody1(mEntity));
mWorld.mBallAndSocketJointsComponents.setR2World(mEntity, q2 * mWorld.mBallAndSocketJointsComponents.getLocalAnchoirPointBody2(mEntity));
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r2World);
// Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints
decimal inverseMassBodies = inverseMassBody1 + inverseMassBody2;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
mInverseMassMatrix.setToZero();
skewSymmetricMatrixU1 * i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * i2 * skewSymmetricMatrixU2.getTranspose();
Matrix3x3& inverseMassMatrix = mWorld.mBallAndSocketJointsComponents.getInverseMassMatrix(mEntity);
inverseMassMatrix.setToZero();
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mInverseMassMatrix = massMatrix.getInverse();
mWorld.mBallAndSocketJointsComponents.setInverseMassMatrix(mEntity, massMatrix.getInverse());
}
// Compute the constraint error (value of the C(x) function)
const Vector3 constraintError = (x2 + mR2World - x1 - mR1World);
const Vector3 constraintError = (x2 + r2World - x1 - r1World);
// Compute the Lagrange multiplier lambda
// TODO : Do not solve the system by computing the inverse each time and multiplying with the
// right-hand side vector but instead use a method to directly solve the linear system.
const Vector3 lambda = mInverseMassMatrix * (-constraintError);
const Vector3 lambda = inverseMassMatrix * (-constraintError);
// Compute the impulse of body 1
const Vector3 linearImpulseBody1 = -lambda;
const Vector3 angularImpulseBody1 = lambda.cross(mR1World);
const Vector3 angularImpulseBody1 = lambda.cross(r1World);
// Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
const Vector3 w1 = i1 * angularImpulseBody1;
// Update the body center of mass and orientation of body 1
x1 += v1;
@ -248,11 +280,11 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
q1.normalize();
// Compute the impulse of body 2
const Vector3 angularImpulseBody2 = -lambda.cross(mR2World);
const Vector3 angularImpulseBody2 = -lambda.cross(r2World);
// Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * lambda;
const Vector3 w2 = mI2 * angularImpulseBody2;
const Vector3 w2 = i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
x2 += v2;
@ -265,3 +297,10 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body2Entity, q2);
}
// Return a string representation
std::string BallAndSocketJoint::to_string() const {
return "BallAndSocketJoint{ localAnchorPointBody1=" + mWorld.mBallAndSocketJointsComponents.getLocalAnchoirPointBody1(mEntity).to_string() +
", localAnchorPointBody2=" + mWorld.mBallAndSocketJointsComponents.getLocalAnchoirPointBody2(mEntity).to_string() + "}";
}

32
src/constraint/BallAndSocketJoint.h
View File

@ -76,32 +76,6 @@ class BallAndSocketJoint : public Joint {
// -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local-space coordinates of body 1)
Vector3 mLocalAnchorPointBody1;
/// Anchor point of body 2 (in local-space coordinates of body 2)
Vector3 mLocalAnchorPointBody2;
/// Vector from center of body 2 to anchor point in world-space
Vector3 mR1World;
/// Vector from center of body 2 to anchor point in world-space
Vector3 mR2World;
/// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 mI1;
/// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 mI2;
/// Bias vector for the constraint
Vector3 mBiasVector;
/// Inverse mass matrix K=JM^-1J^-t of the constraint
Matrix3x3 mInverseMassMatrix;
/// Accumulated impulse
Vector3 mImpulse;
// -------------------- Methods -------------------- //
@ -145,12 +119,6 @@ inline size_t BallAndSocketJoint::getSizeInBytes() const {
return sizeof(BallAndSocketJoint);
}
// Return a string representation
inline std::string BallAndSocketJoint::to_string() const {
return "BallAndSocketJoint{ localAnchorPointBody1=" + mLocalAnchorPointBody1.to_string() +
", localAnchorPointBody2=" + mLocalAnchorPointBody2.to_string() + "}";
}
}
#endif

3
src/constraint/Joint.h
View File

@ -129,9 +129,6 @@ class Joint {
/// True if the joint has already been added into an island
bool mIsAlreadyInIsland;
/// Total number of joints
static uint mNbTotalNbJoints;
// -------------------- Methods -------------------- //
/// Return true if the joint has already been added into an island

2
src/engine/CollisionWorld.cpp
View File

@ -40,7 +40,7 @@ CollisionWorld::CollisionWorld(const WorldSettings& worldSettings, Logger* logge
: mConfig(worldSettings), mEntityManager(mMemoryManager.getPoolAllocator()),
mCollisionBodyComponents(mMemoryManager.getBaseAllocator()), mRigidBodyComponents(mMemoryManager.getBaseAllocator()),
mTransformComponents(mMemoryManager.getBaseAllocator()), mProxyShapesComponents(mMemoryManager.getBaseAllocator()),
mJointsComponents(mMemoryManager.getBaseAllocator()),
mJointsComponents(mMemoryManager.getBaseAllocator()), mBallAndSocketJointsComponents(mMemoryManager.getBaseAllocator()),
mCollisionDetection(this, mProxyShapesComponents, mTransformComponents, mRigidBodyComponents, mMemoryManager),
mBodies(mMemoryManager.getPoolAllocator()), mEventListener(nullptr),
mName(worldSettings.worldName), mIsProfilerCreatedByUser(profiler != nullptr),

4
src/engine/CollisionWorld.h
View File

@ -38,6 +38,7 @@
#include "components/TransformComponents.h"
#include "components/ProxyShapeComponents.h"
#include "components/JointComponents.h"
#include "components/BallAndSocketJointComponents.h"
#include "collision/CollisionCallback.h"
#include "collision/OverlapCallback.h"
@ -92,6 +93,9 @@ class CollisionWorld {
/// Joint Components
JointComponents mJointsComponents;
/// Ball And Socket joints Components
BallAndSocketJointComponents mBallAndSocketJointsComponents;
/// Reference to the collision detection
CollisionDetectionSystem mCollisionDetection;

17
src/engine/DynamicsWorld.cpp
View File

@ -322,17 +322,26 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
Joint* newJoint = nullptr;
const bool isJointDisabled = mRigidBodyComponents.getIsEntityDisabled(jointInfo.body1->getEntity()) &&
mRigidBodyComponents.getIsEntityDisabled(jointInfo.body2->getEntity());
// Allocate memory to create the new joint
switch(jointInfo.type) {
// Ball-and-Socket joint
case JointType::BALLSOCKETJOINT:
{
// Create a BallAndSocketJoint component
BallAndSocketJointComponents::BallAndSocketJointComponent ballAndSocketJointComponent;
mBallAndSocketJointsComponents.addComponent(entity, isJointDisabled, ballAndSocketJointComponent);
void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
sizeof(BallAndSocketJoint));
const BallAndSocketJointInfo& info = static_cast<const BallAndSocketJointInfo&>(
jointInfo);
newJoint = new (allocatedMemory) BallAndSocketJoint(entity, *this, info);
const BallAndSocketJointInfo& info = static_cast<const BallAndSocketJointInfo&>(jointInfo);
BallAndSocketJoint* joint = new (allocatedMemory) BallAndSocketJoint(entity, *this, info);
newJoint = joint;
mBallAndSocketJointsComponents.setJoint(entity, joint);
break;
}
@ -373,8 +382,6 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
}
}
bool isJointDisabled = mRigidBodyComponents.getIsEntityDisabled(jointInfo.body1->getEntity()) &&
mRigidBodyComponents.getIsEntityDisabled(jointInfo.body2->getEntity());
JointComponents::JointComponent jointComponent(jointInfo.body1->getEntity(), jointInfo.body2->getEntity(), newJoint, jointInfo.type,
jointInfo.positionCorrectionTechnique, jointInfo.isCollisionEnabled);
mJointsComponents.addComponent(entity, isJointDisabled, jointComponent);

1
src/engine/DynamicsWorld.h
View File

@ -80,6 +80,7 @@ class DynamicsWorld : public CollisionWorld {
List<RigidBody*> mRigidBodies;
/// All the joints of the world
// TODO : We probably do not need this list anymore
List<Joint*> mJoints;
/// Gravity vector of the world