atria-soft/ephysics
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

[DEV] remove all math from here and use etk versions

Browse Source
This commit is contained in:
Edouard DUPIN 2017-06-15 21:27:15 +02:00
parent c58ef697ce
commit 572d64845c
165 changed files with 4914 additions and 7522 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ephysics/body/Body.h
View File

@ -170,11 +170,11 @@ inline void Body::setIsActive(bool isActive) {
inline void Body::setIsSleeping(bool isSleeping) {
if (isSleeping) {
m_sleepTime = float(0.0);
m_sleepTime = 0.0f;
}
else {
if (m_isSleeping) {
m_sleepTime = float(0.0);
m_sleepTime = 0.0f;
}
}

17
ephysics/body/CollisionBody.cpp
View File

@ -18,7 +18,7 @@ using namespace reactphysics3d;
* @param world The physics world where the body is created
* @param id ID of the body
*/
CollisionBody::CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id)
CollisionBody::CollisionBody(const etk::Transform3D& transform, CollisionWorld& world, bodyindex id)
: Body(id), m_type(DYNAMIC), m_transform(transform), m_proxyCollisionShapes(NULL),
m_numberCollisionShapes(0), m_contactManifoldsList(NULL), m_world(world) {
@ -32,6 +32,15 @@ CollisionBody::~CollisionBody() {
removeAllCollisionShapes();
}
inline void CollisionBody::setType(BodyType _type) {
m_type = _type;
if (m_type == STATIC) {
// Update the broad-phase state of the body
updateBroadPhaseState();
}
}
// Add a collision shape to the body. Note that you can share a collision
// shape between several bodies using the same collision shape instance to
// when you add the shape to the different bodies. Do not forget to delete
@ -48,7 +57,7 @@ CollisionBody::~CollisionBody() {
* the new collision shape you have added.
*/
ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape,
const Transform& transform) {
const etk::Transform3D& transform) {
// Create a new proxy collision shape to attach the collision shape to the body
ProxyShape* proxyShape = new (m_world.m_memoryAllocator.allocate(
@ -188,7 +197,7 @@ void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool fo
proxyShape->getCollisionShape()->computeAABB(aabb, m_transform * proxyShape->getLocalToBodyTransform());
// Update the broad-phase state for the proxy collision shape
m_world.m_collisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert);
m_world.m_collisionDetection.updateProxyCollisionShape(proxyShape, aabb, vec3(0, 0, 0), forceReinsert);
}
// Set whether or not the body is active
@ -267,7 +276,7 @@ int32_t CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
* @param worldPoint The point to test (in world-space coordinates)
* @return True if the point is inside the body
*/
bool CollisionBody::testPointInside(const Vector3& worldPoint) const {
bool CollisionBody::testPointInside(const vec3& worldPoint) const {
// For each collision shape of the body
for (ProxyShape* shape = m_proxyCollisionShapes; shape != NULL; shape = shape->m_next) {

73
ephysics/body/CollisionBody.h
View File

@ -9,7 +9,7 @@
#include <stdexcept>
#include <cassert>
#include <ephysics/body/Body.h>
#include <ephysics/mathematics/Transform.h>
#include <etk/math/Transform3D.hpp>
#include <ephysics/collision/shapes/AABB.h>
#include <ephysics/collision/shapes/CollisionShape.h>
#include <ephysics/collision/RaycastInfo.h>
@ -50,7 +50,7 @@ class CollisionBody : public Body {
BodyType m_type;
/// Position and orientation of the body
Transform m_transform;
etk::Transform3D m_transform;
/// First element of the linked list of proxy collision shapes of this body
ProxyShape* m_proxyCollisionShapes;
@ -96,29 +96,40 @@ class CollisionBody : public Body {
// -------------------- Methods -------------------- //
/// Constructor
CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id);
CollisionBody(const etk::Transform3D& transform, CollisionWorld& world, bodyindex id);
/// Destructor
virtual ~CollisionBody();
/// Return the type of the body
BodyType getType() const;
/// Set the type of the body
void setType(BodyType type);
/**
* @brief Set the type of the body
* The type of the body can either STATIC, KINEMATIC or DYNAMIC as described bellow:
* STATIC : A static body has infinite mass, zero velocity but the position can be
* changed manually. A static body does not collide with other static or kinematic bodies.
* KINEMATIC : A kinematic body has infinite mass, the velocity can be changed manually and its
* position is computed by the physics engine. A kinematic body does not collide with
* other static or kinematic bodies.
* DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its
* position is determined by the physics engine. A dynamic body can collide with other
* dynamic, static or kinematic bodies.
* @param[in] type The type of the body (STATIC, KINEMATIC, DYNAMIC)
*/
virtual void setType(BodyType _type);
/// Set whether or not the body is active
virtual void setIsActive(bool isActive);
/// Return the current position and orientation
const Transform& getTransform() const;
const etk::Transform3D& getTransform() const;
/// Set the current position and orientation
virtual void setTransform(const Transform& transform);
virtual void setTransform(const etk::Transform3D& transform);
/// Add a collision shape to the body.
virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape,
const Transform& transform);
const etk::Transform3D& transform);
/// Remove a collision shape from the body
virtual void removeCollisionShape(const ProxyShape* proxyShape);
@ -127,7 +138,7 @@ class CollisionBody : public Body {
const ContactManifoldListElement* getContactManifoldsList() const;
/// Return true if a point is inside the collision body
bool testPointInside(const Vector3& worldPoint) const;
bool testPointInside(const vec3& worldPoint) const;
/// Raycast method with feedback information
bool raycast(const Ray& ray, RaycastInfo& raycastInfo);
@ -142,16 +153,16 @@ class CollisionBody : public Body {
const ProxyShape* getProxyShapesList() const;
/// Return the world-space coordinates of a point given the local-space coordinates of the body
Vector3 getWorldPoint(const Vector3& localPoint) const;
vec3 getWorldPoint(const vec3& localPoint) const;
/// Return the world-space vector of a vector given in local-space coordinates of the body
Vector3 getWorldVector(const Vector3& localVector) const;
vec3 getWorldVector(const vec3& localVector) const;
/// Return the body local-space coordinates of a point given in the world-space coordinates
Vector3 getLocalPoint(const Vector3& worldPoint) const;
vec3 getLocalPoint(const vec3& worldPoint) const;
/// Return the body local-space coordinates of a vector given in the world-space coordinates
Vector3 getLocalVector(const Vector3& worldVector) const;
vec3 getLocalVector(const vec3& worldVector) const;
// -------------------- Friendship -------------------- //
@ -171,35 +182,13 @@ inline BodyType CollisionBody::getType() const {
return m_type;
}
// Set the type of the body
/// The type of the body can either STATIC, KINEMATIC or DYNAMIC as described bellow:
/// STATIC : A static body has infinite mass, zero velocity but the position can be
/// changed manually. A static body does not collide with other static or kinematic bodies.
/// KINEMATIC : A kinematic body has infinite mass, the velocity can be changed manually and its
/// position is computed by the physics engine. A kinematic body does not collide with
/// other static or kinematic bodies.
/// DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its
/// position is determined by the physics engine. A dynamic body can collide with other
/// dynamic, static or kinematic bodies.
/**
* @param type The type of the body (STATIC, KINEMATIC, DYNAMIC)
*/
inline void CollisionBody::setType(BodyType type) {
m_type = type;
if (m_type == STATIC) {
// Update the broad-phase state of the body
updateBroadPhaseState();
}
}
// Return the current position and orientation
/**
* @return The current transformation of the body that transforms the local-space
* of the body int32_to world-space
*/
inline const Transform& CollisionBody::getTransform() const {
inline const etk::Transform3D& CollisionBody::getTransform() const {
return m_transform;
}
@ -208,7 +197,7 @@ inline const Transform& CollisionBody::getTransform() const {
* @param transform The transformation of the body that transforms the local-space
* of the body int32_to world-space
*/
inline void CollisionBody::setTransform(const Transform& transform) {
inline void CollisionBody::setTransform(const etk::Transform3D& transform) {
// Update the transform of the body
m_transform = transform;
@ -249,7 +238,7 @@ inline const ProxyShape* CollisionBody::getProxyShapesList() const {
* @param localPoint A point in the local-space coordinates of the body
* @return The point in world-space coordinates
*/
inline Vector3 CollisionBody::getWorldPoint(const Vector3& localPoint) const {
inline vec3 CollisionBody::getWorldPoint(const vec3& localPoint) const {
return m_transform * localPoint;
}
@ -258,7 +247,7 @@ inline Vector3 CollisionBody::getWorldPoint(const Vector3& localPoint) const {
* @param localVector A vector in the local-space coordinates of the body
* @return The vector in world-space coordinates
*/
inline Vector3 CollisionBody::getWorldVector(const Vector3& localVector) const {
inline vec3 CollisionBody::getWorldVector(const vec3& localVector) const {
return m_transform.getOrientation() * localVector;
}
@ -267,7 +256,7 @@ inline Vector3 CollisionBody::getWorldVector(const Vector3& localVector) const {
* @param worldPoint A point in world-space coordinates
* @return The point in the local-space coordinates of the body
*/
inline Vector3 CollisionBody::getLocalPoint(const Vector3& worldPoint) const {
inline vec3 CollisionBody::getLocalPoint(const vec3& worldPoint) const {
return m_transform.getInverse() * worldPoint;
}
@ -276,7 +265,7 @@ inline Vector3 CollisionBody::getLocalPoint(const Vector3& worldPoint) const {
* @param worldVector A vector in world-space coordinates
* @return The vector in the local-space coordinates of the body
*/
inline Vector3 CollisionBody::getLocalVector(const Vector3& worldVector) const {
inline vec3 CollisionBody::getLocalVector(const vec3& worldVector) const {
return m_transform.getOrientation().getInverse() * worldVector;
}

294
ephysics/body/RigidBody.cpp
View File

@ -13,20 +13,15 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
/**
* @param transform The transformation of the body
* @param world The world where the body has been added
* @param id The ID of the body
*/
RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id)
: CollisionBody(transform, world, id), m_initMass(float(1.0)),
RigidBody::RigidBody(const etk::Transform3D& transform, CollisionWorld& world, bodyindex id)
: CollisionBody(transform, world, id), m_initMass(1.0f),
m_centerOfMassLocal(0, 0, 0), m_centerOfMassWorld(transform.getPosition()),
m_isGravityEnabled(true), m_linearDamping(float(0.0)), m_angularDamping(float(0.0)),
m_isGravityEnabled(true), m_linearDamping(0.0f), m_angularDamping(float(0.0)),
m_jointsList(NULL) {
// Compute the inverse mass
m_massInverse = float(1.0) / m_initMass;
m_massInverse = 1.0f / m_initMass;
}
// Destructor
@ -34,63 +29,34 @@ RigidBody::~RigidBody() {
assert(m_jointsList == NULL);
}
// Set the type of the body
/// The type of the body can either STATIC, KINEMATIC or DYNAMIC as described bellow:
/// STATIC : A static body has infinite mass, zero velocity but the position can be
/// changed manually. A static body does not collide with other static or kinematic bodies.
/// KINEMATIC : A kinematic body has infinite mass, the velocity can be changed manually and its
/// position is computed by the physics engine. A kinematic body does not collide with
/// other static or kinematic bodies.
/// DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its
/// position is determined by the physics engine. A dynamic body can collide with other
/// dynamic, static or kinematic bodies.
/**
* @param type The type of the body (STATIC, KINEMATIC, DYNAMIC)
*/
void RigidBody::setType(BodyType type) {
if (m_type == type) return;
CollisionBody::setType(type);
// Recompute the total mass, center of mass and inertia tensor
void RigidBody::setType(BodyType _type) {
if (m_type == _type) {
return;
}
CollisionBody::setType(_type);
recomputeMassInformation();
// If it is a static body
if (m_type == STATIC) {
// Reset the velocity to zero
m_linearVelocity.setToZero();
m_angularVelocity.setToZero();
m_linearVelocity.setZero();
m_angularVelocity.setZero();
}
// If it is a static or a kinematic body
if (m_type == STATIC || m_type == KINEMATIC) {
if ( m_type == STATIC
|| m_type == KINEMATIC) {
// Reset the inverse mass and inverse inertia tensor to zero
m_massInverse = float(0.0);
m_inertiaTensorLocal.setToZero();
m_inertiaTensorLocalInverse.setToZero();
}
else { // If it is a dynamic body
m_massInverse = float(1.0) / m_initMass;
m_massInverse = 0.0f;
m_inertiaTensorLocal.setZero();
m_inertiaTensorLocalInverse.setZero();
} else {
m_massInverse = 1.0f / m_initMass;
m_inertiaTensorLocalInverse = m_inertiaTensorLocal.getInverse();
}
// Awake the body
setIsSleeping(false);
// Remove all the contacts with this body
resetContactManifoldsList();
// Ask the broad-phase to test again the collision shapes of the body for collision
// detection (as if the body has moved)
// Ask the broad-phase to test again the collision shapes of the body for collision detection (as if the body has moved)
askForBroadPhaseCollisionCheck();
// Reset the force and torque on the body
m_externalForce.setToZero();
m_externalTorque.setToZero();
m_externalForce.setZero();
m_externalTorque.setZero();
}
// Set the local inertia tensor of the body (in local-space coordinates)
@ -98,7 +64,7 @@ void RigidBody::setType(BodyType type) {
* @param inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space
* coordinates
*/
void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
void RigidBody::setInertiaTensorLocal(const etk::Matrix3x3& inertiaTensorLocal) {
if (m_type != DYNAMIC) return;
@ -113,11 +79,11 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
* @param centerOfMassLocal The center of mass of the body in local-space
* coordinates
*/
void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) {
void RigidBody::setCenterOfMassLocal(const vec3& centerOfMassLocal) {
if (m_type != DYNAMIC) return;
const Vector3 oldCenterOfMass = m_centerOfMassWorld;
const vec3 oldCenterOfMass = m_centerOfMassWorld;
m_centerOfMassLocal = centerOfMassLocal;
// Compute the center of mass in world-space coordinates
@ -127,31 +93,23 @@ void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) {
m_linearVelocity += m_angularVelocity.cross(m_centerOfMassWorld - oldCenterOfMass);
}
// Set the mass of the rigid body
/**
* @param mass The mass (in kilograms) of the body
*/
void RigidBody::setMass(float mass) {
if (m_type != DYNAMIC) return;
m_initMass = mass;
if (m_initMass > float(0.0)) {
m_massInverse = float(1.0) / m_initMass;
void RigidBody::setMass(float _mass) {
if (m_type != DYNAMIC) {
return;
}
else {
m_initMass = float(1.0);
m_massInverse = float(1.0);
m_initMass = _mass;
if (m_initMass > 0.0f) {
m_massInverse = 1.0f / m_initMass;
} else {
m_initMass = 1.0f;
m_massInverse = 1.0f;
}
}
// Remove a joint from the joints list
void RigidBody::removeJointFrom_jointsList(MemoryAllocator& memoryAllocator, const Joint* joint) {
assert(joint != NULL);
assert(m_jointsList != NULL);
assert(joint != nullptr);
assert(m_jointsList != nullptr);
// Remove the joint from the linked list of the joints of the first body
if (m_jointsList->joint == joint) { // If the first element is the one to remove
JointListElement* elementToRemove = m_jointsList;
@ -161,7 +119,7 @@ void RigidBody::removeJointFrom_jointsList(MemoryAllocator& memoryAllocator, con
}
else { // If the element to remove is not the first one in the list
JointListElement* currentElement = m_jointsList;
while (currentElement->next != NULL) {
while (currentElement->next != nullptr) {
if (currentElement->next->joint == joint) {
JointListElement* elementToRemove = currentElement->next;
currentElement->next = elementToRemove->next;
@ -191,10 +149,10 @@ void RigidBody::removeJointFrom_jointsList(MemoryAllocator& memoryAllocator, con
* the new collision shape you have added.
*/
ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
const Transform& transform,
const etk::Transform3D& transform,
float mass) {
assert(mass > float(0.0));
assert(mass > 0.0f);
// Create a new proxy collision shape to attach the collision shape to the body
ProxyShape* proxyShape = new (m_world.m_memoryAllocator.allocate(
@ -235,154 +193,120 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
* @param proxyShape The pointer of the proxy shape you want to remove
*/
void RigidBody::removeCollisionShape(const ProxyShape* proxyShape) {
// Remove the collision shape
CollisionBody::removeCollisionShape(proxyShape);
// Recompute the total mass, center of mass and inertia tensor
recomputeMassInformation();
}
// Set the linear velocity of the rigid body.
/**
* @param linearVelocity Linear velocity vector of the body
*/
void RigidBody::setLinearVelocity(const Vector3& linearVelocity) {
// If it is a static body, we do nothing
if (m_type == STATIC) return;
// Update the linear velocity of the current body state
m_linearVelocity = linearVelocity;
// If the linear velocity is not zero, awake the body
if (m_linearVelocity.lengthSquare() > float(0.0)) {
void RigidBody::setLinearVelocity(const vec3& _linearVelocity) {
if (m_type == STATIC) {
return;
}
m_linearVelocity = _linearVelocity;
if (m_linearVelocity.length2() > 0.0f) {
setIsSleeping(false);
}
}
// Set the angular velocity.
/**
* @param angularVelocity The angular velocity vector of the body
*/
void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
// If it is a static body, we do nothing
if (m_type == STATIC) return;
// Set the angular velocity
m_angularVelocity = angularVelocity;
// If the velocity is not zero, awake the body
if (m_angularVelocity.lengthSquare() > float(0.0)) {
void RigidBody::setAngularVelocity(const vec3& _angularVelocity) {
if (m_type == STATIC) {
return;
}
m_angularVelocity = _angularVelocity;
if (m_angularVelocity.length2() > 0.0f) {
setIsSleeping(false);
}
}
// Set the current position and orientation
/**
* @param transform The transformation of the body that transforms the local-space
* of the body int32_to world-space
*/
void RigidBody::setTransform(const Transform& transform) {
void RigidBody::setIsSleeping(bool _isSleeping) {
if (_isSleeping) {
m_linearVelocity.setZero();
m_angularVelocity.setZero();
m_externalForce.setZero();
m_externalTorque.setZero();
}
Body::setIsSleeping(_isSleeping);
}
// Update the transform of the body
m_transform = transform;
const Vector3 oldCenterOfMass = m_centerOfMassWorld;
void RigidBody::setTransform(const etk::Transform3D& _transform) {
m_transform = _transform;
const vec3 oldCenterOfMass = m_centerOfMassWorld;
// Compute the new center of mass in world-space coordinates
m_centerOfMassWorld = m_transform * m_centerOfMassLocal;
// Update the linear velocity of the center of mass
m_linearVelocity += m_angularVelocity.cross(m_centerOfMassWorld - oldCenterOfMass);
// Update the broad-phase state of the body
updateBroadPhaseState();
}
// Recompute the center of mass, total mass and inertia tensor of the body using all
// the collision shapes attached to the body.
void RigidBody::recomputeMassInformation() {
m_initMass = float(0.0);
m_massInverse = float(0.0);
m_inertiaTensorLocal.setToZero();
m_inertiaTensorLocalInverse.setToZero();
m_centerOfMassLocal.setToZero();
m_initMass = 0.0f;
m_massInverse = 0.0f;
m_inertiaTensorLocal.setZero();
m_inertiaTensorLocalInverse.setZero();
m_centerOfMassLocal.setZero();
// If it is STATIC or KINEMATIC body
if (m_type == STATIC || m_type == KINEMATIC) {
m_centerOfMassWorld = m_transform.getPosition();
return;
}
assert(m_type == DYNAMIC);
// Compute the total mass of the body
for (ProxyShape* shape = m_proxyCollisionShapes; shape != NULL; shape = shape->m_next) {
m_initMass += shape->getMass();
m_centerOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass();
}
if (m_initMass > float(0.0)) {
m_massInverse = float(1.0) / m_initMass;
if (m_initMass > 0.0f) {
m_massInverse = 1.0f / m_initMass;
} else {
m_initMass = 1.0f;
m_massInverse = 1.0f;
}
else {
m_initMass = float(1.0);
m_massInverse = float(1.0);
}
// Compute the center of mass
const Vector3 oldCenterOfMass = m_centerOfMassWorld;
const vec3 oldCenterOfMass = m_centerOfMassWorld;
m_centerOfMassLocal *= m_massInverse;
m_centerOfMassWorld = m_transform * m_centerOfMassLocal;
// Compute the total mass and inertia tensor using all the collision shapes
for (ProxyShape* shape = m_proxyCollisionShapes; shape != NULL; shape = shape->m_next) {
for (ProxyShape* shape = m_proxyCollisionShapes; shape != nullptr; shape = shape->m_next) {
// Get the inertia tensor of the collision shape in its local-space
Matrix3x3 inertiaTensor;
etk::Matrix3x3 inertiaTensor;
shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass());
// Convert the collision shape inertia tensor int32_to the local-space of the body
const Transform& shapeTransform = shape->getLocalToBodyTransform();
Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix();
const etk::Transform3D& shapeTransform = shape->getLocalToBodyTransform();
etk::Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix();
inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose();
// Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape
// center int32_to a inertia tensor w.r.t to the body origin.
Vector3 offset = shapeTransform.getPosition() - m_centerOfMassLocal;
float offsetSquare = offset.lengthSquare();
Matrix3x3 offsetMatrix;
offsetMatrix[0].setAllValues(offsetSquare, float(0.0), float(0.0));
offsetMatrix[1].setAllValues(float(0.0), offsetSquare, float(0.0));
offsetMatrix[2].setAllValues(float(0.0), float(0.0), offsetSquare);
offsetMatrix[0] += offset * (-offset.x);
offsetMatrix[1] += offset * (-offset.y);
offsetMatrix[2] += offset * (-offset.z);
vec3 offset = shapeTransform.getPosition() - m_centerOfMassLocal;
float offsetSquare = offset.length2();
vec3 off1 = offset * (-offset.x());
vec3 off2 = offset * (-offset.y());
vec3 off3 = offset * (-offset.z());
etk::Matrix3x3 offsetMatrix(off1.x()+offsetSquare, off1.y(), off1.z(),
off2.x(), off2.y()+offsetSquare, off2.z(),
off3.x(), off3.y(), off3.z()+offsetSquare);
offsetMatrix *= shape->getMass();
m_inertiaTensorLocal += inertiaTensor + offsetMatrix;
}
// Compute the local inverse inertia tensor
m_inertiaTensorLocalInverse = m_inertiaTensorLocal.getInverse();
// Update the linear velocity of the center of mass
m_linearVelocity += m_angularVelocity.cross(m_centerOfMassWorld - oldCenterOfMass);
}
// Update the broad-phase state for this body (because it has moved for instance)
void RigidBody::updateBroadPhaseState() const {
PROFILE("RigidBody::updateBroadPhaseState()");
DynamicsWorld& world = static_cast<DynamicsWorld&>(m_world);
const Vector3 displacement = world.m_timeStep * m_linearVelocity;
const vec3 displacement = world.m_timeStep * m_linearVelocity;
// For all the proxy collision shapes of the body
for (ProxyShape* shape = m_proxyCollisionShapes; shape != NULL; shape = shape->m_next) {
for (ProxyShape* shape = m_proxyCollisionShapes; shape != nullptr; shape = shape->m_next) {
// Recompute the world-space AABB of the collision shape
AABB aabb;
@ -393,3 +317,43 @@ void RigidBody::updateBroadPhaseState() const {
}
}
void RigidBody::applyForceToCenterOfMass(const vec3& _force) {
// If it is not a dynamic body, we do nothing
if (m_type != DYNAMIC) {
return;
}
// Awake the body if it was sleeping
if (m_isSleeping) {
setIsSleeping(false);
}
// Add the force
m_externalForce += _force;
}
void RigidBody::applyForce(const vec3& _force, const vec3& _point) {
// If it is not a dynamic body, we do nothing
if (m_type != DYNAMIC) {
return;
}
// Awake the body if it was sleeping
if (m_isSleeping) {
setIsSleeping(false);
}
// Add the force and torque
m_externalForce += _force;
m_externalTorque += (_point - m_centerOfMassWorld).cross(_force);
}
void RigidBody::applyTorque(const vec3& _torque) {
// If it is not a dynamic body, we do nothing
if (m_type != DYNAMIC) {
return;
}
// Awake the body if it was sleeping
if (m_isSleeping) {
setIsSleeping(false);
}
// Add the torque
m_externalTorque += _torque;
}

607
ephysics/body/RigidBody.h
View File

@ -5,14 +5,12 @@
*/
#pragma once
// Libraries
#include <cassert>
#include <ephysics/body/CollisionBody.h>
#include <ephysics/engine/Material.h>
#include <ephysics/mathematics/mathematics.h>
#include <ephysics/memory/MemoryAllocator.h>
/// Namespace reactphysics3d
namespace reactphysics3d {
// Class declarations
@ -20,177 +18,259 @@ struct JointListElement;
class Joint;
class DynamicsWorld;
// Class RigidBody
/**
* This class represents a rigid body of the physics
* @brief This class represents a rigid body of the physics
* engine. A rigid body is a non-deformable body that
* has a constant mass. This class inherits from the
* CollisionBody class.
*/
*/
class RigidBody : public CollisionBody {
protected :
// -------------------- Attributes -------------------- //
/// Intial mass of the body
float m_initMass;
/// Center of mass of the body in local-space coordinates.
/// The center of mass can therefore be different from the body origin
Vector3 m_centerOfMassLocal;
/// Center of mass of the body in world-space coordinates
Vector3 m_centerOfMassWorld;
/// Linear velocity of the body
Vector3 m_linearVelocity;
/// Angular velocity of the body
Vector3 m_angularVelocity;
/// Current external force on the body
Vector3 m_externalForce;
/// Current external torque on the body
Vector3 m_externalTorque;
/// Local inertia tensor of the body (in local-space) with respect to the
/// center of mass of the body
Matrix3x3 m_inertiaTensorLocal;
/// Inverse of the inertia tensor of the body
Matrix3x3 m_inertiaTensorLocalInverse;
/// Inverse of the mass of the body
float m_massInverse;
/// True if the gravity needs to be applied to this rigid body
bool m_isGravityEnabled;
/// Material properties of the rigid body
Material m_material;
/// Linear velocity damping factor
float m_linearDamping;
/// Angular velocity damping factor
float m_angularDamping;
/// First element of the linked list of joints involving this body
JointListElement* m_jointsList;
// -------------------- Methods -------------------- //
float m_initMass; //!< Intial mass of the body
vec3 m_centerOfMassLocal; //!< Center of mass of the body in local-space coordinates. The center of mass can therefore be different from the body origin
vec3 m_centerOfMassWorld; //!< Center of mass of the body in world-space coordinates
vec3 m_linearVelocity; //!< Linear velocity of the body
vec3 m_angularVelocity; //!< Angular velocity of the body
vec3 m_externalForce; //!< Current external force on the body
vec3 m_externalTorque; //!< Current external torque on the body
etk::Matrix3x3 m_inertiaTensorLocal; //!< Local inertia tensor of the body (in local-space) with respect to the center of mass of the body
etk::Matrix3x3 m_inertiaTensorLocalInverse; //!< Inverse of the inertia tensor of the body
float m_massInverse; //!< Inverse of the mass of the body
bool m_isGravityEnabled; //!< True if the gravity needs to be applied to this rigid body
Material m_material; //!< Material properties of the rigid body
float m_linearDamping; //!< Linear velocity damping factor
float m_angularDamping; //!< Angular velocity damping factor
JointListElement* m_jointsList; //!< First element of the linked list of joints involving this body
/// Private copy-constructor
RigidBody(const RigidBody& body);
/// Private assignment operator
RigidBody& operator=(const RigidBody& body);
/// Remove a joint from the joints list
void removeJointFrom_jointsList(MemoryAllocator& memoryAllocator, const Joint* joint);
/// Update the transform of the body after a change of the center of mass
void updateTransformWithCenterOfMass();
/// Update the broad-phase state for this body (because it has moved for instance)
/**
* @brief Remove a joint from the joints list
*/
void removeJointFrom_jointsList(MemoryAllocator& _memoryAllocator, const Joint* _joint);
/**
* @brief Update the transform of the body after a change of the center of mass
*/
void updateTransformWithCenterOfMass() {
// Translate the body according to the translation of the center of mass position
m_transform.setPosition(m_centerOfMassWorld - m_transform.getOrientation() * m_centerOfMassLocal);
}
/**
* @brief Update the broad-phase state for this body (because it has moved for instance)
*/
virtual void updateBroadPhaseState() const;
public :
// -------------------- Methods -------------------- //
/// Constructor
RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id);
/**
* @brief Constructor
* @param transform The transformation of the body
* @param world The world where the body has been added
* @param id The ID of the body
*/
RigidBody(const etk::Transform3D& transform, CollisionWorld& world, bodyindex id);
/// Destructor
virtual ~RigidBody();
/// Set the type of the body (static, kinematic or dynamic)
void setType(BodyType type);
/// Set the current position and orientation
virtual void setTransform(const Transform& transform);
/// Return the mass of the body
float getMass() const;
/// Return the linear velocity
Vector3 getLinearVelocity() const;
/// Set the linear velocity of the body.
void setLinearVelocity(const Vector3& linearVelocity);
/// Return the angular velocity
Vector3 getAngularVelocity() const;
/// Set the angular velocity.
void setAngularVelocity(const Vector3& angularVelocity);
/// Set the variable to know whether or not the body is sleeping
virtual void setIsSleeping(bool isSleeping);
/// Return the local inertia tensor of the body (in body coordinates)
const Matrix3x3& getInertiaTensorLocal() const;
void setType(BodyType _type); // TODO: override
/**
* @brief Set the current position and orientation
* @param[in] _transform The transformation of the body that transforms the local-space of the body int32_to world-space
*/
virtual void setTransform(const etk::Transform3D& _transform);
/**
* @brief Get the mass of the body
* @return The mass (in kilograms) of the body
*/
float getMass() const {
return m_initMass;
}
/**
* @brief Get the linear velocity
* @return The linear velocity vector of the body
*/
vec3 getLinearVelocity() const {
return m_linearVelocity;
}
/**
* @brief Set the linear velocity of the rigid body.
* @param[in] _linearVelocity Linear velocity vector of the body
*/
void setLinearVelocity(const vec3& _linearVelocity);
/**
* @brief Get the angular velocity of the body
* @return The angular velocity vector of the body
*/
vec3 getAngularVelocity() const {
return m_angularVelocity;
}
/**
* @brief Set the angular velocity.
* @param[in] _angularVelocity The angular velocity vector of the body
*/
void setAngularVelocity(const vec3& _angularVelocity);
/**
* @brief Set the variable to know whether or not the body is sleeping
* @param[in] _isSleeping New sleeping state of the body
*/
virtual void setIsSleeping(bool _isSleeping);
/**
* @brief Get the local inertia tensor of the body (in local-space coordinates)
* @return The 3x3 inertia tensor matrix of the body
*/
const etk::Matrix3x3& getInertiaTensorLocal() const {
return m_inertiaTensorLocal;
}
/// Set the local inertia tensor of the body (in body coordinates)
void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal);
void setInertiaTensorLocal(const etk::Matrix3x3& inertiaTensorLocal);
/// Set the local center of mass of the body (in local-space coordinates)
void setCenterOfMassLocal(const Vector3& centerOfMassLocal);
void setCenterOfMassLocal(const vec3& centerOfMassLocal);
/**
* @brief Set the mass of the rigid body
* @param[in] _mass The mass (in kilograms) of the body
*/
void setMass(float _mass);
/// Set the mass of the rigid body
void setMass(float mass);
/// Return the inertia tensor in world coordinates.
Matrix3x3 getInertiaTensorWorld() const;
/// Return the inverse of the inertia tensor in world coordinates.
Matrix3x3 getInertiaTensorInverseWorld() const;
/// Return true if the gravity needs to be applied to this rigid body
bool isGravityEnabled() const;
/// Set the variable to know if the gravity is applied to this rigid body
void enableGravity(bool isEnabled);
/// Return a reference to the material properties of the rigid body
Material& getMaterial();
/// Set a new material for this rigid body
void setMaterial(const Material& material);
/// Return the linear velocity damping factor
float getLinearDamping() const;
/// Set the linear damping factor
void setLinearDamping(float linearDamping);
/// Return the angular velocity damping factor
float getAngularDamping() const;
/// Set the angular damping factor
void setAngularDamping(float angularDamping);
/// Return the first element of the linked list of joints involving this body
const JointListElement* getJointsList() const;
/// Return the first element of the linked list of joints involving this body
JointListElement* getJointsList();
/// Apply an external force to the body at its center of mass.
void applyForceToCenterOfMass(const Vector3& force);
/// Apply an external force to the body at a given point (in world-space coordinates).
void applyForce(const Vector3& force, const Vector3& point);
/// Apply an external torque to the body.
void applyTorque(const Vector3& torque);
/**
* @brief Get the inertia tensor in world coordinates.
* The inertia tensor I_w in world coordinates is computed
* with the local inertia tensor I_b in body coordinates
* by I_w = R * I_b * R^T
* where R is the rotation matrix (and R^T its transpose) of
* the current orientation quaternion of the body
* @return The 3x3 inertia tensor matrix of the body in world-space coordinates
*/
etk::Matrix3x3 getInertiaTensorWorld() const {
// Compute and return the inertia tensor in world coordinates
return m_transform.getOrientation().getMatrix() * m_inertiaTensorLocal *
m_transform.getOrientation().getMatrix().getTranspose();
}
/**
* @brief Get the inverse of the inertia tensor in world coordinates.
* The inertia tensor I_w in world coordinates is computed with the
* local inverse inertia tensor I_b^-1 in body coordinates
* by I_w = R * I_b^-1 * R^T
* where R is the rotation matrix (and R^T its transpose) of the
* current orientation quaternion of the body
* @return The 3x3 inverse inertia tensor matrix of the body in world-space coordinates
*/
etk::Matrix3x3 getInertiaTensorInverseWorld() const {
// TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE
// INVERSE WORLD TENSOR IN THE CLASS AND UPLDATE IT WHEN THE ORIENTATION OF THE BODY CHANGES
// Compute and return the inertia tensor in world coordinates
return m_transform.getOrientation().getMatrix() * m_inertiaTensorLocalInverse *
m_transform.getOrientation().getMatrix().getTranspose();
}
/**
* @brief get the need of gravity appling to this rigid body
* @return True if the gravity is applied to the body
*/
bool isGravityEnabled() const {
return m_isGravityEnabled;
}
/**
* @brief Set the variable to know if the gravity is applied to this rigid body
* @param[in] _isEnabled True if you want the gravity to be applied to this body
*/
void enableGravity(bool _isEnabled) {
m_isGravityEnabled = _isEnabled;
}
/**
* @brief get a reference to the material properties of the rigid body
* @return A reference to the material of the body
*/
Material& getMaterial() {
return m_material;
}
/**
* @brief Set a new material for this rigid body
* @param[in] _material The material you want to set to the body
*/
void setMaterial(const Material& _material) {
m_material = _material;
}
/**
* @brief Get the linear velocity damping factor
* @return The linear damping factor of this body
*/
float getLinearDamping() const {
return m_linearDamping;
}
/**
* @brief Set the linear damping factor. This is the ratio of the linear velocity that the body will lose every at seconds of simulation.
* @param[in] _linearDamping The linear damping factor of this body
*/
void setLinearDamping(float _linearDamping) {
assert(_linearDamping >= 0.0f);
m_linearDamping = _linearDamping;
}
/**
* @brief Get the angular velocity damping factor
* @return The angular damping factor of this body
*/
float getAngularDamping() const {
return m_angularDamping;
}
/**
* @brief Set the angular damping factor. This is the ratio of the angular velocity that the body will lose at every seconds of simulation.
* @param[in] _angularDamping The angular damping factor of this body
*/
void setAngularDamping(float _angularDamping) {
assert(_angularDamping >= 0.0f);
m_angularDamping = _angularDamping;
}
/**
* @brief Get the first element of the linked list of joints involving this body
* @return The first element of the linked-list of all the joints involving this body
*/
const JointListElement* getJointsList() const {
return m_jointsList;
}
/**
* @brief Get the first element of the linked list of joints involving this body
* @return The first element of the linked-list of all the joints involving this body
*/
JointListElement* getJointsList() {
return m_jointsList;
}
/**
* @brief Apply an external force to the body at its center of mass.
* If the body is sleeping, calling this method will wake it up. Note that the
* force will we added to the sum of the applied forces and that this sum will be
* reset to zero at the end of each call of the DynamicsWorld::update() method.
* You can only apply a force to a dynamic body otherwise, this method will do nothing.
* @param[in] _force The external force to apply on the center of mass of the body
*/
void applyForceToCenterOfMass(const vec3& _force);
/**
* @brief Apply an external force to the body at a given point (in world-space coordinates).
* If the point is not at the center of mass of the body, it will also
* generate some torque and therefore, change the angular velocity of the body.
* If the body is sleeping, calling this method will wake it up. Note that the
* force will we added to the sum of the applied forces and that this sum will be
* reset to zero at the end of each call of the DynamicsWorld::update() method.
* You can only apply a force to a dynamic body otherwise, this method will do nothing.
* @param[in] _force The force to apply on the body
* @param[in] _point The point where the force is applied (in world-space coordinates)
*/
void applyForce(const vec3& _force, const vec3& _point);
/**
* @brief Apply an external torque to the body.
* If the body is sleeping, calling this method will wake it up. Note that the
* force will we added to the sum of the applied torques and that this sum will be
* reset to zero at the end of each call of the DynamicsWorld::update() method.
* You can only apply a force to a dynamic body otherwise, this method will do nothing.
* @param[in] _torque The external torque to apply on the body
*/
void applyTorque(const vec3& _torque);
/// Add a collision shape to the body.
virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape,
const Transform& transform,
const etk::Transform3D& transform,
float mass);
/// Remove a collision shape from the body
@ -200,8 +280,6 @@ class RigidBody : public CollisionBody {
/// the collision shapes attached to the body.
void recomputeMassInformation();
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
friend class ContactSolver;
friend class BallAndSocketJoint;
@ -210,247 +288,18 @@ class RigidBody : public CollisionBody {
friend class FixedJoint;
};
// Method that return the mass of the body
/**
* @return The mass (in kilograms) of the body
*/
inline float RigidBody::getMass() const {
return m_initMass;
}
// Return the linear velocity
/**
* @return The linear velocity vector of the body
*/
inline Vector3 RigidBody::getLinearVelocity() const {
return m_linearVelocity;
}
// Return the angular velocity of the body
/**
* @return The angular velocity vector of the body
*/
inline Vector3 RigidBody::getAngularVelocity() const {
return m_angularVelocity;
}
// Return the local inertia tensor of the body (in local-space coordinates)
/**
* @return The 3x3 inertia tensor matrix of the body (in local-space coordinates)
*/
inline const Matrix3x3& RigidBody::getInertiaTensorLocal() const {
return m_inertiaTensorLocal;
}
// Return the inertia tensor in world coordinates.
/// The inertia tensor I_w in world coordinates is computed
/// with the local inertia tensor I_b in body coordinates
/// by I_w = R * I_b * R^T
/// where R is the rotation matrix (and R^T its transpose) of
/// the current orientation quaternion of the body
/**
* @return The 3x3 inertia tensor matrix of the body in world-space coordinates
*/
inline Matrix3x3 RigidBody::getInertiaTensorWorld() const {
// Compute and return the inertia tensor in world coordinates
return m_transform.getOrientation().getMatrix() * m_inertiaTensorLocal *
m_transform.getOrientation().getMatrix().getTranspose();
}
// Return the inverse of the inertia tensor in world coordinates.
/// The inertia tensor I_w in world coordinates is computed with the
/// local inverse inertia tensor I_b^-1 in body coordinates
/// by I_w = R * I_b^-1 * R^T
/// where R is the rotation matrix (and R^T its transpose) of the
/// current orientation quaternion of the body
/**
* @return The 3x3 inverse inertia tensor matrix of the body in world-space
* coordinates
*/
inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
// TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE
// INVERSE WORLD TENSOR IN THE CLASS AND UPLDATE IT WHEN THE ORIENTATION OF THE BODY CHANGES
// Compute and return the inertia tensor in world coordinates
return m_transform.getOrientation().getMatrix() * m_inertiaTensorLocalInverse *
m_transform.getOrientation().getMatrix().getTranspose();
}
// Return true if the gravity needs to be applied to this rigid body
/**
* @return True if the gravity is applied to the body
*/
inline bool RigidBody::isGravityEnabled() const {
return m_isGravityEnabled;
}
// Set the variable to know if the gravity is applied to this rigid body
/**
* @param isEnabled True if you want the gravity to be applied to this body
*/
inline void RigidBody::enableGravity(bool isEnabled) {
m_isGravityEnabled = isEnabled;
}
// Return a reference to the material properties of the rigid body
/**
* @return A reference to the material of the body
*/
inline Material& RigidBody::getMaterial() {
return m_material;
}
// Set a new material for this rigid body
/**
* @param material The material you want to set to the body
*/
inline void RigidBody::setMaterial(const Material& material) {
m_material = material;
}
// Return the linear velocity damping factor
/**
* @return The linear damping factor of this body
*/
inline float RigidBody::getLinearDamping() const {
return m_linearDamping;
}
// Set the linear damping factor. This is the ratio of the linear velocity
// that the body will lose every at seconds of simulation.
/**
* @param linearDamping The linear damping factor of this body
*/
inline void RigidBody::setLinearDamping(float linearDamping) {
assert(linearDamping >= float(0.0));
m_linearDamping = linearDamping;
}
// Return the angular velocity damping factor
/**
* @return The angular damping factor of this body
*/
inline float RigidBody::getAngularDamping() const {
return m_angularDamping;
}
// Set the angular damping factor. This is the ratio of the angular velocity
// that the body will lose at every seconds of simulation.
/**
* @param angularDamping The angular damping factor of this body
*/
inline void RigidBody::setAngularDamping(float angularDamping) {
assert(angularDamping >= float(0.0));
m_angularDamping = angularDamping;
}
// Return the first element of the linked list of joints involving this body
/**
* @return The first element of the linked-list of all the joints involving this body
*/
inline const JointListElement* RigidBody::getJointsList() const {
return m_jointsList;
}
// Return the first element of the linked list of joints involving this body
/**
* @return The first element of the linked-list of all the joints involving this body
*/
inline JointListElement* RigidBody::getJointsList() {
return m_jointsList;
}
// Set the variable to know whether or not the body is sleeping
inline void RigidBody::setIsSleeping(bool isSleeping) {
if (isSleeping) {
m_linearVelocity.setToZero();
m_angularVelocity.setToZero();
m_externalForce.setToZero();
m_externalTorque.setToZero();
}
Body::setIsSleeping(isSleeping);
}
// Apply an external force to the body at its center of mass.
/// If the body is sleeping, calling this method will wake it up. Note that the
/// force will we added to the sum of the applied forces and that this sum will be
/// reset to zero at the end of each call of the DynamicsWorld::update() method.
/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
/**
* @param force The external force to apply on the center of mass of the body
*/
inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) {
// If it is not a dynamic body, we do nothing
if (m_type != DYNAMIC) return;
// Awake the body if it was sleeping
if (m_isSleeping) {
setIsSleeping(false);
}
// Add the force
m_externalForce += force;
}
// Apply an external force to the body at a given point (in world-space coordinates).
/// If the point is not at the center of mass of the body, it will also
/// generate some torque and therefore, change the angular velocity of the body.
/// If the body is sleeping, calling this method will wake it up. Note that the
/// force will we added to the sum of the applied forces and that this sum will be
/// reset to zero at the end of each call of the DynamicsWorld::update() method.
/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
/**
* @param force The force to apply on the body
* @param point The point where the force is applied (in world-space coordinates)
*/
inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
// If it is not a dynamic body, we do nothing
if (m_type != DYNAMIC) return;
// Awake the body if it was sleeping
if (m_isSleeping) {
setIsSleeping(false);
}
// Add the force and torque
m_externalForce += force;
m_externalTorque += (point - m_centerOfMassWorld).cross(force);
}
// Apply an external torque to the body.
/// If the body is sleeping, calling this method will wake it up. Note that the
/// force will we added to the sum of the applied torques and that this sum will be
/// reset to zero at the end of each call of the DynamicsWorld::update() method.
/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
/**
* @param torque The external torque to apply on the body
*/
inline void RigidBody::applyTorque(const Vector3& torque) {
// If it is not a dynamic body, we do nothing
if (m_type != DYNAMIC) return;
// Awake the body if it was sleeping
if (m_isSleeping) {
setIsSleeping(false);
}
// Add the torque
m_externalTorque += torque;
}
/// Update the transform of the body after a change of the center of mass
inline void RigidBody::updateTransformWithCenterOfMass() {
// Translate the body according to the translation of the center of mass position
m_transform.setPosition(m_centerOfMassWorld - m_transform.getOrientation() * m_centerOfMassLocal);
}
}

4
ephysics/collision/CollisionDetection.h
View File

@ -145,7 +145,7 @@ class CollisionDetection : public NarrowPhaseCallback {
/// Update a proxy collision shape (that has moved for instance)
void updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb,
const Vector3& displacement = Vector3(0, 0, 0), bool forceReinsert = false);
const vec3& displacement = vec3(0, 0, 0), bool forceReinsert = false);
/// Add a pair of bodies that cannot collide with each other
void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2);
@ -257,7 +257,7 @@ inline void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape
// Update a proxy collision shape (that has moved for instance)
inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb,
const Vector3& displacement, bool forceReinsert) {
const vec3& displacement, bool forceReinsert) {
m_broadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement);
}

6
ephysics/collision/CollisionShapeInfo.h
View File

@ -31,15 +31,15 @@ struct CollisionShapeInfo {
/// Pointer to the collision shape
const CollisionShape* collisionShape;
/// Transform that maps from collision shape local-space to world-space
Transform shapeToWorldTransform;
/// etk::Transform3D that maps from collision shape local-space to world-space
etk::Transform3D shapeToWorldTransform;
/// Cached collision data of the proxy shape
void** cachedCollisionData;
/// Constructor
CollisionShapeInfo(ProxyShape* proxyCollisionShape, const CollisionShape* shape,
const Transform& shapeLocalToWorldTransform, OverlappingPair* pair,
const etk::Transform3D& shapeLocalToWorldTransform, OverlappingPair* pair,
void** cachedData)
: overlappingPair(pair), proxyShape(proxyCollisionShape), collisionShape(shape),
shapeToWorldTransform(shapeLocalToWorldTransform),

44
ephysics/collision/ContactManifold.cpp
View File

@ -35,7 +35,7 @@ void ContactManifold::addContactPoint(ContactPoint* contact) {
// Check if the new point point does not correspond to a same contact point
// already in the manifold.
float distance = (m_contactPoints[i]->getWorldPointOnBody1() -
contact->getWorldPointOnBody1()).lengthSquare();
contact->getWorldPointOnBody1()).length2();
if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) {
// Delete the new contact
@ -86,7 +86,7 @@ void ContactManifold::removeContactPoint(uint32_t index) {
/// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also
/// the contacts with a too large distance between the contact points in the plane orthogonal to the
/// contact normal.
void ContactManifold::update(const Transform& transform1, const Transform& transform2) {
void ContactManifold::update(const etk::Transform3D& transform1, const etk::Transform3D& transform2) {
if (m_nbContactPoints == 0) return;
@ -117,13 +117,13 @@ void ContactManifold::update(const Transform& transform1, const Transform& trans
else {
// Compute the distance of the two contact points in the plane
// orthogonal to the contact normal
Vector3 projOfPoint1 = m_contactPoints[i]->getWorldPointOnBody1() +
vec3 projOfPoint1 = m_contactPoints[i]->getWorldPointOnBody1() +
m_contactPoints[i]->getNormal() * distanceNormal;
Vector3 projDifference = m_contactPoints[i]->getWorldPointOnBody2() - projOfPoint1;
vec3 projDifference = m_contactPoints[i]->getWorldPointOnBody2() - projOfPoint1;
// If the orthogonal distance is larger than the valid distance
// threshold, we remove the contact
if (projDifference.lengthSquare() > squarePersistentContactThreshold) {
if (projDifference.length2() > squarePersistentContactThreshold) {
removeContactPoint(i);
}
}
@ -162,7 +162,7 @@ int32_t ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact)
/// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore,
/// this is only a guess that is faster to compute. This idea comes from the Bullet Physics library
/// by Erwin Coumans (http://wwww.bulletphysics.org).
int32_t ContactManifold::getIndexToRemove(int32_t indexMaxPenetration, const Vector3& newPoint) const {
int32_t ContactManifold::getIndexToRemove(int32_t indexMaxPenetration, const vec3& newPoint) const {
assert(m_nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
@ -173,35 +173,35 @@ int32_t ContactManifold::getIndexToRemove(int32_t indexMaxPenetration, const Vec
if (indexMaxPenetration != 0) {
// Compute the area
Vector3 vector1 = newPoint - m_contactPoints[1]->getLocalPointOnBody1();
Vector3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() -
vec3 vector1 = newPoint - m_contactPoints[1]->getLocalPointOnBody1();
vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() -
m_contactPoints[2]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area0 = crossProduct.lengthSquare();
vec3 crossProduct = vector1.cross(vector2);
area0 = crossProduct.length2();
}
if (indexMaxPenetration != 1) {
// Compute the area
Vector3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1();
Vector3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() -
vec3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1();
vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() -
m_contactPoints[2]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area1 = crossProduct.lengthSquare();
vec3 crossProduct = vector1.cross(vector2);
area1 = crossProduct.length2();
}
if (indexMaxPenetration != 2) {
// Compute the area
Vector3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1();
Vector3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() -
vec3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1();
vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() -
m_contactPoints[1]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area2 = crossProduct.lengthSquare();
vec3 crossProduct = vector1.cross(vector2);
area2 = crossProduct.length2();
}
if (indexMaxPenetration != 3) {
// Compute the area
Vector3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1();
Vector3 vector2 = m_contactPoints[2]->getLocalPointOnBody1() -
vec3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1();
vec3 vector2 = m_contactPoints[2]->getLocalPointOnBody1() -
m_contactPoints[1]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area3 = crossProduct.lengthSquare();
vec3 crossProduct = vector1.cross(vector2);
area3 = crossProduct.length2();
}
// Return the index of the contact to remove

42
ephysics/collision/ContactManifold.h
View File

@ -84,10 +84,10 @@ class ContactManifold {
uint32_t m_nbContactPoints;
/// First friction vector of the contact manifold
Vector3 m_frictionVector1;
vec3 m_frictionVector1;
/// Second friction vector of the contact manifold
Vector3 m_frictionVector2;
vec3 m_frictionvec2;
/// First friction constraint accumulated impulse
float m_frictionImpulse1;
@ -99,7 +99,7 @@ class ContactManifold {
float m_frictionTwistImpulse;
/// Accumulated rolling resistance impulse
Vector3 m_rollingResistanceImpulse;
vec3 m_rollingResistanceImpulse;
/// True if the contact manifold has already been added int32_to an island
bool m_isAlreadyInIsland;
@ -122,7 +122,7 @@ class ContactManifold {
int32_t getIndexOfDeepestPenetration(ContactPoint* newContact) const;
/// Return the index that will be removed.
int32_t getIndexToRemove(int32_t indexMaxPenetration, const Vector3& newPoint) const;
int32_t getIndexToRemove(int32_t indexMaxPenetration, const vec3& newPoint) const;
/// Remove a contact point from the manifold
void removeContactPoint(uint32_t index);
@ -160,7 +160,7 @@ class ContactManifold {
void addContactPoint(ContactPoint* contact);
/// Update the contact manifold.
void update(const Transform& transform1, const Transform& transform2);
void update(const etk::Transform3D& transform1, const etk::Transform3D& transform2);
/// Clear the contact manifold
void clear();
@ -169,16 +169,16 @@ class ContactManifold {
uint32_t getNbContactPoints() const;
/// Return the first friction vector at the center of the contact manifold
const Vector3& getFrictionVector1() const;
const vec3& getFrictionVector1() const;
/// set the first friction vector at the center of the contact manifold
void setFrictionVector1(const Vector3& m_frictionVector1);
void setFrictionVector1(const vec3& m_frictionVector1);
/// Return the second friction vector at the center of the contact manifold
const Vector3& getFrictionVector2() const;
const vec3& getFrictionvec2() const;
/// set the second friction vector at the center of the contact manifold
void setFrictionVector2(const Vector3& m_frictionVector2);
void setFrictionvec2(const vec3& m_frictionvec2);
/// Return the first friction accumulated impulse
float getFrictionImpulse1() const;
@ -199,13 +199,13 @@ class ContactManifold {
void setFrictionTwistImpulse(float frictionTwistImpulse);
/// Set the accumulated rolling resistance impulse
void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse);
void setRollingResistanceImpulse(const vec3& rollingResistanceImpulse);
/// Return a contact point of the manifold
ContactPoint* getContactPoint(uint32_t index) const;
/// Return the normalized averaged normal vector
Vector3 getAverageContactNormal() const;
vec3 getAverageContactNormal() const;
/// Return the largest depth of all the contact points
float getLargestContactDepth() const;
@ -248,23 +248,23 @@ inline uint32_t ContactManifold::getNbContactPoints() const {
}
// Return the first friction vector at the center of the contact manifold
inline const Vector3& ContactManifold::getFrictionVector1() const {
inline const vec3& ContactManifold::getFrictionVector1() const {
return m_frictionVector1;
}
// set the first friction vector at the center of the contact manifold
inline void ContactManifold::setFrictionVector1(const Vector3& frictionVector1) {
inline void ContactManifold::setFrictionVector1(const vec3& frictionVector1) {
m_frictionVector1 = frictionVector1;
}
// Return the second friction vector at the center of the contact manifold
inline const Vector3& ContactManifold::getFrictionVector2() const {
return m_frictionVector2;
inline const vec3& ContactManifold::getFrictionvec2() const {
return m_frictionvec2;
}
// set the second friction vector at the center of the contact manifold
inline void ContactManifold::setFrictionVector2(const Vector3& frictionVector2) {
m_frictionVector2 = frictionVector2;
inline void ContactManifold::setFrictionvec2(const vec3& frictionvec2) {
m_frictionvec2 = frictionvec2;
}
// Return the first friction accumulated impulse
@ -298,7 +298,7 @@ inline void ContactManifold::setFrictionTwistImpulse(float frictionTwistImpulse)
}
// Set the accumulated rolling resistance impulse
inline void ContactManifold::setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse) {
inline void ContactManifold::setRollingResistanceImpulse(const vec3& rollingResistanceImpulse) {
m_rollingResistanceImpulse = rollingResistanceImpulse;
}
@ -314,14 +314,14 @@ inline bool ContactManifold::isAlreadyInIsland() const {
}
// Return the normalized averaged normal vector
inline Vector3 ContactManifold::getAverageContactNormal() const {
Vector3 averageNormal;
inline vec3 ContactManifold::getAverageContactNormal() const {
vec3 averageNormal;
for (uint32_t i=0; i<m_nbContactPoints; i++) {
averageNormal += m_contactPoints[i]->getNormal();
}
return averageNormal.getUnit();
return averageNormal.safeNormalized();
}
// Return the largest depth of all the contact points

30
ephysics/collision/ContactManifoldSet.cpp
View File

@ -129,29 +129,29 @@ int32_t ContactManifoldSet::selectManifoldWithSimilarNormal(int16_t normalDirect
// Map the normal vector int32_to a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided int32_to 4x4 buckets. This method maps the
// normal vector int32_to of the of the bucket and returns a unique Id for the bucket
int16_t ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) const {
int16_t ContactManifoldSet::computeCubemapNormalId(const vec3& normal) const {
assert(normal.lengthSquare() > MACHINE_EPSILON);
assert(normal.length2() > MACHINE_EPSILON);
int32_t faceNo;
float u, v;
float max = max3(fabs(normal.x), fabs(normal.y), fabs(normal.z));
Vector3 normalScaled = normal / max;
float max = max3(fabs(normal.x()), fabs(normal.y()), fabs(normal.z()));
vec3 normalScaled = normal / max;
if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) {
faceNo = normalScaled.x > 0 ? 0 : 1;
u = normalScaled.y;
v = normalScaled.z;
if (normalScaled.x() >= normalScaled.y() && normalScaled.x() >= normalScaled.z()) {
faceNo = normalScaled.x() > 0 ? 0 : 1;
u = normalScaled.y();
v = normalScaled.z();
}
else if (normalScaled.y >= normalScaled.x && normalScaled.y >= normalScaled.z) {
faceNo = normalScaled.y > 0 ? 2 : 3;
u = normalScaled.x;
v = normalScaled.z;
else if (normalScaled.y() >= normalScaled.x() && normalScaled.y() >= normalScaled.z()) {
faceNo = normalScaled.y() > 0 ? 2 : 3;
u = normalScaled.x();
v = normalScaled.z();
}
else {
faceNo = normalScaled.z > 0 ? 4 : 5;
u = normalScaled.x;
v = normalScaled.y;
faceNo = normalScaled.z() > 0 ? 4 : 5;
u = normalScaled.x();
v = normalScaled.y();
}
int32_t indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);

2
ephysics/collision/ContactManifoldSet.h
View File

@ -59,7 +59,7 @@ class ContactManifoldSet {
// Map the normal vector int32_to a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided int32_to 4x4 buckets. This method maps the
// normal vector int32_to of the of the bucket and returns a unique Id for the bucket
int16_t computeCubemapNormalId(const Vector3& normal) const;
int16_t computeCubemapNormalId(const vec3& normal) const;
public:

14
ephysics/collision/ProxyShape.cpp
View File

@ -13,10 +13,10 @@ using namespace reactphysics3d;
/**
* @param body Pointer to the parent body
* @param shape Pointer to the collision shape
* @param transform Transformation from collision shape local-space to body local-space
* @param transform etk::Transform3Dation from collision shape local-space to body local-space
* @param mass Mass of the collision shape (in kilograms)
*/
ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, float mass)
ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const etk::Transform3D& transform, float mass)
:m_body(body), m_collisionShape(shape), m_localToBodyTransform(transform), m_mass(mass),
m_next(NULL), m_broadPhaseID(-1), m_cachedCollisionData(NULL), m_userData(NULL),
m_collisionCategoryBits(0x0001), m_collideWithMaskBits(0xFFFF) {
@ -37,9 +37,9 @@ ProxyShape::~ProxyShape() {
* @param worldPoint Point to test in world-space coordinates
* @return True if the point is inside the collision shape
*/
bool ProxyShape::testPointInside(const Vector3& worldPoint) {
const Transform localToWorld = m_body->getTransform() * m_localToBodyTransform;
const Vector3 localPoint = localToWorld.getInverse() * worldPoint;
bool ProxyShape::testPointInside(const vec3& worldPoint) {
const etk::Transform3D localToWorld = m_body->getTransform() * m_localToBodyTransform;
const vec3 localPoint = localToWorld.getInverse() * worldPoint;
return m_collisionShape->testPointInside(localPoint, this);
}
@ -56,8 +56,8 @@ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
if (!m_body->isActive()) return false;
// Convert the ray int32_to the local-space of the collision shape
const Transform localToWorldTransform = getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
const etk::Transform3D localToWorldTransform = getLocalToWorldTransform();
const etk::Transform3D worldToLocalTransform = localToWorldTransform.getInverse();
Ray rayLocal(worldToLocalTransform * ray.point1,
worldToLocalTransform * ray.point2,
ray.maxFraction);

26
ephysics/collision/ProxyShape.h
View File

@ -34,7 +34,7 @@ class ProxyShape {
CollisionShape* m_collisionShape;
/// Local-space to parent body-space transform (does not change over time)
Transform m_localToBodyTransform;
etk::Transform3D m_localToBodyTransform;
/// Mass (in kilogramms) of the corresponding collision shape
float m_mass;
@ -78,7 +78,7 @@ class ProxyShape {
/// Constructor
ProxyShape(CollisionBody* body, CollisionShape* shape,
const Transform& transform, float mass);
const etk::Transform3D& transform, float mass);
/// Destructor
virtual ~ProxyShape();
@ -99,16 +99,16 @@ class ProxyShape {
void setUserData(void* userData);
/// Return the local to parent body transform
const Transform& getLocalToBodyTransform() const;
const etk::Transform3D& getLocalToBodyTransform() const;
/// Set the local to parent body transform
void setLocalToBodyTransform(const Transform& transform);
void setLocalToBodyTransform(const etk::Transform3D& transform);
/// Return the local to world transform
const Transform getLocalToWorldTransform() const;
const etk::Transform3D getLocalToWorldTransform() const;
/// Return true if a point is inside the collision shape
bool testPointInside(const Vector3& worldPoint);
bool testPointInside(const vec3& worldPoint);
/// Raycast method with feedback information
bool raycast(const Ray& ray, RaycastInfo& raycastInfo);
@ -135,10 +135,10 @@ class ProxyShape {
void** getCachedCollisionData();
/// Return the local scaling vector of the collision shape
Vector3 getLocalScaling() const;
vec3 getLocalScaling() const;
/// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
virtual void setLocalScaling(const vec3& scaling);
// -------------------- Friendship -------------------- //
@ -206,12 +206,12 @@ inline void ProxyShape::setUserData(void* userData) {
* @return The transformation that transforms the local-space of the collision shape
* to the local-space of the parent body
*/
inline const Transform& ProxyShape::getLocalToBodyTransform() const {
inline const etk::Transform3D& ProxyShape::getLocalToBodyTransform() const {
return m_localToBodyTransform;
}
// Set the local to parent body transform
inline void ProxyShape::setLocalToBodyTransform(const Transform& transform) {
inline void ProxyShape::setLocalToBodyTransform(const etk::Transform3D& transform) {
m_localToBodyTransform = transform;
@ -226,7 +226,7 @@ inline void ProxyShape::setLocalToBodyTransform(const Transform& transform) {
* @return The transformation that transforms the local-space of the collision
* shape to the world-space
*/
inline const Transform ProxyShape::getLocalToWorldTransform() const {
inline const etk::Transform3D ProxyShape::getLocalToWorldTransform() const {
return m_body->m_transform * m_localToBodyTransform;
}
@ -282,7 +282,7 @@ inline void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBit
/**
* @return The local scaling vector
*/
inline Vector3 ProxyShape::getLocalScaling() const {
inline vec3 ProxyShape::getLocalScaling() const {
return m_collisionShape->getScaling();
}
@ -290,7 +290,7 @@ inline Vector3 ProxyShape::getLocalScaling() const {
/**
* @param scaling The new local scaling vector
*/
inline void ProxyShape::setLocalScaling(const Vector3& scaling) {
inline void ProxyShape::setLocalScaling(const vec3& scaling) {
// Set the local scaling of the collision shape
m_collisionShape->setLocalScaling(scaling);

6
ephysics/collision/RaycastInfo.h
View File

@ -6,7 +6,7 @@
#pragma once
// Libraries
#include <ephysics/mathematics/Vector3.h>
#include <etk/math/Vector3D.hpp>
#include <ephysics/mathematics/Ray.h>
/// ReactPhysics3D namespace
@ -38,10 +38,10 @@ struct RaycastInfo {
// -------------------- Attributes -------------------- //
/// Hit point in world-space coordinates
Vector3 worldPoint;
vec3 worldPoint;
/// Surface normal at hit point in world-space coordinates
Vector3 worldNormal;
vec3 worldNormal;
/// Fraction distance of the hit point between point1 and point2 of the ray
/// The hit point "p" is such that p = point1 + hitFraction * (point2 - point1)

2
ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp
View File

@ -124,7 +124,7 @@ void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
// Notify the broad-phase that a collision shape has moved and need to be updated
void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb,
const Vector3& displacement, bool forceReinsert) {
const vec3& displacement, bool forceReinsert) {
int32_t broadPhaseID = proxyShape->m_broadPhaseID;

2
ephysics/collision/broadphase/BroadPhaseAlgorithm.h
View File

@ -165,7 +165,7 @@ class BroadPhaseAlgorithm {
/// Notify the broad-phase that a collision shape has moved and need to be updated
void updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb,
const Vector3& displacement, bool forceReinsert = false);
const vec3& displacement, bool forceReinsert = false);
/// Add a collision shape in the array of shapes that have moved in the last simulation step
/// and that need to be tested again for broad-phase overlapping.

351
ephysics/collision/broadphase/DynamicAABBTree.cpp
View File

@ -16,7 +16,7 @@ using namespace reactphysics3d;
const int32_t TreeNode::NULL_TREE_NODE = -1;
// Constructor
DynamicAABBTree::DynamicAABBTree(float extraAABBGap) : mExtraAABBGap(extraAABBGap) {
DynamicAABBTree::DynamicAABBTree(float extraAABBGap) : m_extraAABBGap(extraAABBGap) {
init();
}
@ -25,36 +25,36 @@ DynamicAABBTree::DynamicAABBTree(float extraAABBGap) : mExtraAABBGap(extraAABBGa
DynamicAABBTree::~DynamicAABBTree() {
// Free the allocated memory for the nodes
free(mNodes);
free(m_nodes);
}
// Initialize the tree
void DynamicAABBTree::init() {
m_rootNodeID = TreeNode::NULL_TREE_NODE;
mNbNodes = 0;
mNbAllocatedNodes = 8;
m_numberNodes = 0;
m_numberAllocatedNodes = 8;
// Allocate memory for the nodes of the tree
mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode));
assert(mNodes);
memset(mNodes, 0, mNbAllocatedNodes * sizeof(TreeNode));
m_nodes = (TreeNode*) malloc(m_numberAllocatedNodes * sizeof(TreeNode));
assert(m_nodes);
memset(m_nodes, 0, m_numberAllocatedNodes * sizeof(TreeNode));
// Initialize the allocated nodes
for (int32_t i=0; i<mNbAllocatedNodes - 1; i++) {
mNodes[i].nextNodeID = i + 1;
mNodes[i].height = -1;
for (int32_t i=0; i<m_numberAllocatedNodes - 1; i++) {
m_nodes[i].nextNodeID = i + 1;
m_nodes[i].height = -1;
}
mNodes[mNbAllocatedNodes - 1].nextNodeID = TreeNode::NULL_TREE_NODE;
mNodes[mNbAllocatedNodes - 1].height = -1;
mFreeNodeID = 0;
m_nodes[m_numberAllocatedNodes - 1].nextNodeID = TreeNode::NULL_TREE_NODE;
m_nodes[m_numberAllocatedNodes - 1].height = -1;
m_freeNodeID = 0;
}
// Clear all the nodes and reset the tree
void DynamicAABBTree::reset() {
// Free the allocated memory for the nodes
free(mNodes);
free(m_nodes);
// Initialize the tree
init();
@ -64,34 +64,34 @@ void DynamicAABBTree::reset() {
int32_t DynamicAABBTree::allocateNode() {
// If there is no more allocated node to use
if (mFreeNodeID == TreeNode::NULL_TREE_NODE) {
if (m_freeNodeID == TreeNode::NULL_TREE_NODE) {
assert(mNbNodes == mNbAllocatedNodes);
assert(m_numberNodes == m_numberAllocatedNodes);
// Allocate more nodes in the tree
mNbAllocatedNodes *= 2;
TreeNode* oldNodes = mNodes;
mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode));
assert(mNodes);
memcpy(mNodes, oldNodes, mNbNodes * sizeof(TreeNode));
m_numberAllocatedNodes *= 2;
TreeNode* oldNodes = m_nodes;
m_nodes = (TreeNode*) malloc(m_numberAllocatedNodes * sizeof(TreeNode));
assert(m_nodes);
memcpy(m_nodes, oldNodes, m_numberNodes * sizeof(TreeNode));
free(oldNodes);
// Initialize the allocated nodes
for (int32_t i=mNbNodes; i<mNbAllocatedNodes - 1; i++) {
mNodes[i].nextNodeID = i + 1;
mNodes[i].height = -1;
for (int32_t i=m_numberNodes; i<m_numberAllocatedNodes - 1; i++) {
m_nodes[i].nextNodeID = i + 1;
m_nodes[i].height = -1;
}
mNodes[mNbAllocatedNodes - 1].nextNodeID = TreeNode::NULL_TREE_NODE;
mNodes[mNbAllocatedNodes - 1].height = -1;
mFreeNodeID = mNbNodes;
m_nodes[m_numberAllocatedNodes - 1].nextNodeID = TreeNode::NULL_TREE_NODE;
m_nodes[m_numberAllocatedNodes - 1].height = -1;
m_freeNodeID = m_numberNodes;
}
// Get the next free node
int32_t freeNodeID = mFreeNodeID;
mFreeNodeID = mNodes[freeNodeID].nextNodeID;
mNodes[freeNodeID].parentID = TreeNode::NULL_TREE_NODE;
mNodes[freeNodeID].height = 0;
mNbNodes++;
int32_t freeNodeID = m_freeNodeID;
m_freeNodeID = m_nodes[freeNodeID].nextNodeID;
m_nodes[freeNodeID].parentID = TreeNode::NULL_TREE_NODE;
m_nodes[freeNodeID].height = 0;
m_numberNodes++;
return freeNodeID;
}
@ -99,13 +99,13 @@ int32_t DynamicAABBTree::allocateNode() {
// Release a node
void DynamicAABBTree::releaseNode(int32_t nodeID) {
assert(mNbNodes > 0);
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].height >= 0);
mNodes[nodeID].nextNodeID = mFreeNodeID;
mNodes[nodeID].height = -1;
mFreeNodeID = nodeID;
mNbNodes--;
assert(m_numberNodes > 0);
assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes);
assert(m_nodes[nodeID].height >= 0);
m_nodes[nodeID].nextNodeID = m_freeNodeID;
m_nodes[nodeID].height = -1;
m_freeNodeID = nodeID;
m_numberNodes--;
}
// Internally add an object int32_to the tree
@ -115,16 +115,16 @@ int32_t DynamicAABBTree::addObjectInternal(const AABB& aabb) {
int32_t nodeID = allocateNode();
// Create the fat aabb to use in the tree
const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap);
mNodes[nodeID].aabb.setMin(aabb.getMin() - gap);
mNodes[nodeID].aabb.setMax(aabb.getMax() + gap);
const vec3 gap(m_extraAABBGap, m_extraAABBGap, m_extraAABBGap);
m_nodes[nodeID].aabb.setMin(aabb.getMin() - gap);
m_nodes[nodeID].aabb.setMax(aabb.getMax() + gap);
// Set the height of the node in the tree
mNodes[nodeID].height = 0;
m_nodes[nodeID].height = 0;
// Insert the new leaf node in the tree
insertLeafNode(nodeID);
assert(mNodes[nodeID].isLeaf());
assert(m_nodes[nodeID].isLeaf());
assert(nodeID >= 0);
@ -135,8 +135,8 @@ int32_t DynamicAABBTree::addObjectInternal(const AABB& aabb) {
// Remove an object from the tree
void DynamicAABBTree::removeObject(int32_t nodeID) {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf());
assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes);
assert(m_nodes[nodeID].isLeaf());
// Remove the node from the tree
removeLeafNode(nodeID);
@ -150,16 +150,16 @@ void DynamicAABBTree::removeObject(int32_t nodeID) {
/// argument is the linear velocity of the AABB multiplied by the elapsed time between two
/// frames. If the "forceReinsert" parameter is true, we force a removal and reinsertion of the node
/// (this can be useful if the shape AABB has become much smaller than the previous one for instance).
bool DynamicAABBTree::updateObject(int32_t nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert) {
bool DynamicAABBTree::updateObject(int32_t nodeID, const AABB& newAABB, const vec3& displacement, bool forceReinsert) {
PROFILE("DynamicAABBTree::updateObject()");
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf());
assert(mNodes[nodeID].height >= 0);
assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes);
assert(m_nodes[nodeID].isLeaf());
assert(m_nodes[nodeID].height >= 0);
// If the new AABB is still inside the fat AABB of the node
if (!forceReinsert && mNodes[nodeID].aabb.contains(newAABB)) {
if (!forceReinsert && m_nodes[nodeID].aabb.contains(newAABB)) {
return false;
}
@ -167,32 +167,29 @@ bool DynamicAABBTree::updateObject(int32_t nodeID, const AABB& newAABB, const Ve
removeLeafNode(nodeID);
// Compute the fat AABB by inflating the AABB with a constant gap
mNodes[nodeID].aabb = newAABB;
const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap);
mNodes[nodeID].aabb.m_minCoordinates -= gap;
mNodes[nodeID].aabb.m_maxCoordinates += gap;
m_nodes[nodeID].aabb = newAABB;
const vec3 gap(m_extraAABBGap, m_extraAABBGap, m_extraAABBGap);
m_nodes[nodeID].aabb.m_minCoordinates -= gap;
m_nodes[nodeID].aabb.m_maxCoordinates += gap;
// Inflate the fat AABB in direction of the linear motion of the AABB
if (displacement.x < float(0.0)) {
mNodes[nodeID].aabb.m_minCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x;
if (displacement.x() < 0.0f) {
m_nodes[nodeID].aabb.m_minCoordinates.setX(m_nodes[nodeID].aabb.m_minCoordinates.x() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x());
} else {
m_nodes[nodeID].aabb.m_maxCoordinates.setY(m_nodes[nodeID].aabb.m_maxCoordinates.x() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x());
}
else {
mNodes[nodeID].aabb.m_maxCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x;
if (displacement.y() < 0.0f) {
m_nodes[nodeID].aabb.m_minCoordinates.setY(m_nodes[nodeID].aabb.m_minCoordinates.y() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y());
} else {
m_nodes[nodeID].aabb.m_maxCoordinates.setY(m_nodes[nodeID].aabb.m_maxCoordinates.y() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y());
}
if (displacement.y < float(0.0)) {
mNodes[nodeID].aabb.m_minCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y;
}
else {
mNodes[nodeID].aabb.m_maxCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y;
}
if (displacement.z < float(0.0)) {
mNodes[nodeID].aabb.m_minCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z;
}
else {
mNodes[nodeID].aabb.m_maxCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z;
if (displacement.z() < 0.0f) {
m_nodes[nodeID].aabb.m_minCoordinates.setZ(m_nodes[nodeID].aabb.m_minCoordinates.z() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z());
} else {
m_nodes[nodeID].aabb.m_maxCoordinates.setZ(m_nodes[nodeID].aabb.m_maxCoordinates.z() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z());
}
assert(mNodes[nodeID].aabb.contains(newAABB));
assert(m_nodes[nodeID].aabb.contains(newAABB));
// Reinsert the node int32_to the tree
insertLeafNode(nodeID);
@ -208,24 +205,24 @@ void DynamicAABBTree::insertLeafNode(int32_t nodeID) {
// If the tree is empty
if (m_rootNodeID == TreeNode::NULL_TREE_NODE) {
m_rootNodeID = nodeID;
mNodes[m_rootNodeID].parentID = TreeNode::NULL_TREE_NODE;
m_nodes[m_rootNodeID].parentID = TreeNode::NULL_TREE_NODE;
return;
}
assert(m_rootNodeID != TreeNode::NULL_TREE_NODE);
// Find the best sibling node for the new node
AABB newNodeAABB = mNodes[nodeID].aabb;
AABB newNodeAABB = m_nodes[nodeID].aabb;
int32_t currentNodeID = m_rootNodeID;
while (!mNodes[currentNodeID].isLeaf()) {
while (!m_nodes[currentNodeID].isLeaf()) {
int32_t leftChild = mNodes[currentNodeID].children[0];
int32_t rightChild = mNodes[currentNodeID].children[1];
int32_t leftChild = m_nodes[currentNodeID].children[0];
int32_t rightChild = m_nodes[currentNodeID].children[1];
// Compute the merged AABB
float volumeAABB = mNodes[currentNodeID].aabb.getVolume();
float volumeAABB = m_nodes[currentNodeID].aabb.getVolume();
AABB mergedAABBs;
mergedAABBs.mergeTwoAABBs(mNodes[currentNodeID].aabb, newNodeAABB);
mergedAABBs.mergeTwoAABBs(m_nodes[currentNodeID].aabb, newNodeAABB);
float mergedVolume = mergedAABBs.getVolume();
// Compute the cost of making the current node the sibbling of the new node
@ -237,24 +234,24 @@ void DynamicAABBTree::insertLeafNode(int32_t nodeID) {
// Compute the cost of descending int32_to the left child
float costLeft;
AABB currentAndLeftAABB;
currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, mNodes[leftChild].aabb);
if (mNodes[leftChild].isLeaf()) { // If the left child is a leaf
currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, m_nodes[leftChild].aabb);
if (m_nodes[leftChild].isLeaf()) { // If the left child is a leaf
costLeft = currentAndLeftAABB.getVolume() + costI;
}
else {
float leftChildVolume = mNodes[leftChild].aabb.getVolume();
float leftChildVolume = m_nodes[leftChild].aabb.getVolume();
costLeft = costI + currentAndLeftAABB.getVolume() - leftChildVolume;
}
// Compute the cost of descending int32_to the right child
float costRight;
AABB currentAndRightAABB;
currentAndRightAABB.mergeTwoAABBs(newNodeAABB, mNodes[rightChild].aabb);
if (mNodes[rightChild].isLeaf()) { // If the right child is a leaf
currentAndRightAABB.mergeTwoAABBs(newNodeAABB, m_nodes[rightChild].aabb);
if (m_nodes[rightChild].isLeaf()) { // If the right child is a leaf
costRight = currentAndRightAABB.getVolume() + costI;
}
else {
float rightChildVolume = mNodes[rightChild].aabb.getVolume();
float rightChildVolume = m_nodes[rightChild].aabb.getVolume();
costRight = costI + currentAndRightAABB.getVolume() - rightChildVolume;
}
@ -274,69 +271,69 @@ void DynamicAABBTree::insertLeafNode(int32_t nodeID) {
int32_t siblingNode = currentNodeID;
// Create a new parent for the new node and the sibling node
int32_t oldParentNode = mNodes[siblingNode].parentID;
int32_t oldParentNode = m_nodes[siblingNode].parentID;
int32_t newParentNode = allocateNode();
mNodes[newParentNode].parentID = oldParentNode;
mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB);
mNodes[newParentNode].height = mNodes[siblingNode].height + 1;
assert(mNodes[newParentNode].height > 0);
m_nodes[newParentNode].parentID = oldParentNode;
m_nodes[newParentNode].aabb.mergeTwoAABBs(m_nodes[siblingNode].aabb, newNodeAABB);
m_nodes[newParentNode].height = m_nodes[siblingNode].height + 1;
assert(m_nodes[newParentNode].height > 0);
// If the sibling node was not the root node
if (oldParentNode != TreeNode::NULL_TREE_NODE) {
assert(!mNodes[oldParentNode].isLeaf());
if (mNodes[oldParentNode].children[0] == siblingNode) {
mNodes[oldParentNode].children[0] = newParentNode;
assert(!m_nodes[oldParentNode].isLeaf());
if (m_nodes[oldParentNode].children[0] == siblingNode) {
m_nodes[oldParentNode].children[0] = newParentNode;
}
else {
mNodes[oldParentNode].children[1] = newParentNode;
m_nodes[oldParentNode].children[1] = newParentNode;
}
mNodes[newParentNode].children[0] = siblingNode;
mNodes[newParentNode].children[1] = nodeID;
mNodes[siblingNode].parentID = newParentNode;
mNodes[nodeID].parentID = newParentNode;
m_nodes[newParentNode].children[0] = siblingNode;
m_nodes[newParentNode].children[1] = nodeID;
m_nodes[siblingNode].parentID = newParentNode;
m_nodes[nodeID].parentID = newParentNode;
}
else { // If the sibling node was the root node
mNodes[newParentNode].children[0] = siblingNode;
mNodes[newParentNode].children[1] = nodeID;
mNodes[siblingNode].parentID = newParentNode;
mNodes[nodeID].parentID = newParentNode;
m_nodes[newParentNode].children[0] = siblingNode;
m_nodes[newParentNode].children[1] = nodeID;
m_nodes[siblingNode].parentID = newParentNode;
m_nodes[nodeID].parentID = newParentNode;
m_rootNodeID = newParentNode;
}
// Move up in the tree to change the AABBs that have changed
currentNodeID = mNodes[nodeID].parentID;
assert(!mNodes[currentNodeID].isLeaf());
currentNodeID = m_nodes[nodeID].parentID;
assert(!m_nodes[currentNodeID].isLeaf());
while (currentNodeID != TreeNode::NULL_TREE_NODE) {
// Balance the sub-tree of the current node if it is not balanced
currentNodeID = balanceSubTreeAtNode(currentNodeID);
assert(mNodes[nodeID].isLeaf());
assert(m_nodes[nodeID].isLeaf());
assert(!mNodes[currentNodeID].isLeaf());
int32_t leftChild = mNodes[currentNodeID].children[0];
int32_t rightChild = mNodes[currentNodeID].children[1];
assert(!m_nodes[currentNodeID].isLeaf());
int32_t leftChild = m_nodes[currentNodeID].children[0];
int32_t rightChild = m_nodes[currentNodeID].children[1];
assert(leftChild != TreeNode::NULL_TREE_NODE);
assert(rightChild != TreeNode::NULL_TREE_NODE);
// Recompute the height of the node in the tree
mNodes[currentNodeID].height = std::max(mNodes[leftChild].height,
mNodes[rightChild].height) + 1;
assert(mNodes[currentNodeID].height > 0);
m_nodes[currentNodeID].height = std::max(m_nodes[leftChild].height,
m_nodes[rightChild].height) + 1;
assert(m_nodes[currentNodeID].height > 0);
// Recompute the AABB of the node
mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
m_nodes[currentNodeID].aabb.mergeTwoAABBs(m_nodes[leftChild].aabb, m_nodes[rightChild].aabb);
currentNodeID = mNodes[currentNodeID].parentID;
currentNodeID = m_nodes[currentNodeID].parentID;
}
assert(mNodes[nodeID].isLeaf());
assert(m_nodes[nodeID].isLeaf());
}
// Remove a leaf node from the tree
void DynamicAABBTree::removeLeafNode(int32_t nodeID) {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf());
assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes);
assert(m_nodes[nodeID].isLeaf());
// If we are removing the root node (root node is a leaf in this case)
if (m_rootNodeID == nodeID) {
@ -344,28 +341,28 @@ void DynamicAABBTree::removeLeafNode(int32_t nodeID) {
return;
}
int32_t parentNodeID = mNodes[nodeID].parentID;
int32_t grandParentNodeID = mNodes[parentNodeID].parentID;
int32_t parentNodeID = m_nodes[nodeID].parentID;
int32_t grandParentNodeID = m_nodes[parentNodeID].parentID;
int32_t siblingNodeID;
if (mNodes[parentNodeID].children[0] == nodeID) {
siblingNodeID = mNodes[parentNodeID].children[1];
if (m_nodes[parentNodeID].children[0] == nodeID) {
siblingNodeID = m_nodes[parentNodeID].children[1];
}
else {
siblingNodeID = mNodes[parentNodeID].children[0];
siblingNodeID = m_nodes[parentNodeID].children[0];
}
// If the parent of the node to remove is not the root node
if (grandParentNodeID != TreeNode::NULL_TREE_NODE) {
// Destroy the parent node
if (mNodes[grandParentNodeID].children[0] == parentNodeID) {
mNodes[grandParentNodeID].children[0] = siblingNodeID;
if (m_nodes[grandParentNodeID].children[0] == parentNodeID) {
m_nodes[grandParentNodeID].children[0] = siblingNodeID;
}
else {
assert(mNodes[grandParentNodeID].children[1] == parentNodeID);
mNodes[grandParentNodeID].children[1] = siblingNodeID;
assert(m_nodes[grandParentNodeID].children[1] == parentNodeID);
m_nodes[grandParentNodeID].children[1] = siblingNodeID;
}
mNodes[siblingNodeID].parentID = grandParentNodeID;
m_nodes[siblingNodeID].parentID = grandParentNodeID;
releaseNode(parentNodeID);
// Now, we need to recompute the AABBs of the node on the path back to the root
@ -376,27 +373,27 @@ void DynamicAABBTree::removeLeafNode(int32_t nodeID) {
// Balance the current sub-tree if necessary
currentNodeID = balanceSubTreeAtNode(currentNodeID);
assert(!mNodes[currentNodeID].isLeaf());
assert(!m_nodes[currentNodeID].isLeaf());
// Get the two children of the current node
int32_t leftChildID = mNodes[currentNodeID].children[0];
int32_t rightChildID = mNodes[currentNodeID].children[1];
int32_t leftChildID = m_nodes[currentNodeID].children[0];
int32_t rightChildID = m_nodes[currentNodeID].children[1];
// Recompute the AABB and the height of the current node
mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChildID].aabb,
mNodes[rightChildID].aabb);
mNodes[currentNodeID].height = std::max(mNodes[leftChildID].height,
mNodes[rightChildID].height) + 1;
assert(mNodes[currentNodeID].height > 0);
m_nodes[currentNodeID].aabb.mergeTwoAABBs(m_nodes[leftChildID].aabb,
m_nodes[rightChildID].aabb);
m_nodes[currentNodeID].height = std::max(m_nodes[leftChildID].height,
m_nodes[rightChildID].height) + 1;
assert(m_nodes[currentNodeID].height > 0);
currentNodeID = mNodes[currentNodeID].parentID;
currentNodeID = m_nodes[currentNodeID].parentID;
}
}
else { // If the parent of the node to remove is the root node
// The sibling node becomes the new root node
m_rootNodeID = siblingNodeID;
mNodes[siblingNodeID].parentID = TreeNode::NULL_TREE_NODE;
m_nodes[siblingNodeID].parentID = TreeNode::NULL_TREE_NODE;
releaseNode(parentNodeID);
}
}
@ -408,7 +405,7 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) {
assert(nodeID != TreeNode::NULL_TREE_NODE);
TreeNode* nodeA = mNodes + nodeID;
TreeNode* nodeA = m_nodes + nodeID;
// If the node is a leaf or the height of A's sub-tree is less than 2
if (nodeA->isLeaf() || nodeA->height < 2) {
@ -420,10 +417,10 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) {
// Get the two children nodes
int32_t nodeBID = nodeA->children[0];
int32_t nodeCID = nodeA->children[1];
assert(nodeBID >= 0 && nodeBID < mNbAllocatedNodes);
assert(nodeCID >= 0 && nodeCID < mNbAllocatedNodes);
TreeNode* nodeB = mNodes + nodeBID;
TreeNode* nodeC = mNodes + nodeCID;
assert(nodeBID >= 0 && nodeBID < m_numberAllocatedNodes);
assert(nodeCID >= 0 && nodeCID < m_numberAllocatedNodes);
TreeNode* nodeB = m_nodes + nodeBID;
TreeNode* nodeC = m_nodes + nodeCID;
// Compute the factor of the left and right sub-trees
int32_t balanceFactor = nodeC->height - nodeB->height;
@ -435,10 +432,10 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) {
int32_t nodeFID = nodeC->children[0];
int32_t nodeGID = nodeC->children[1];
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
TreeNode* nodeF = mNodes + nodeFID;
TreeNode* nodeG = mNodes + nodeGID;
assert(nodeFID >= 0 && nodeFID < m_numberAllocatedNodes);
assert(nodeGID >= 0 && nodeGID < m_numberAllocatedNodes);
TreeNode* nodeF = m_nodes + nodeFID;
TreeNode* nodeG = m_nodes + nodeGID;
nodeC->children[0] = nodeID;
nodeC->parentID = nodeA->parentID;
@ -446,12 +443,12 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) {
if (nodeC->parentID != TreeNode::NULL_TREE_NODE) {
if (mNodes[nodeC->parentID].children[0] == nodeID) {
mNodes[nodeC->parentID].children[0] = nodeCID;
if (m_nodes[nodeC->parentID].children[0] == nodeID) {
m_nodes[nodeC->parentID].children[0] = nodeCID;
}
else {
assert(mNodes[nodeC->parentID].children[1] == nodeID);
mNodes[nodeC->parentID].children[1] = nodeCID;
assert(m_nodes[nodeC->parentID].children[1] == nodeID);
m_nodes[nodeC->parentID].children[1] = nodeCID;
}
}
else {
@ -505,10 +502,10 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) {
int32_t nodeFID = nodeB->children[0];
int32_t nodeGID = nodeB->children[1];
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
TreeNode* nodeF = mNodes + nodeFID;
TreeNode* nodeG = mNodes + nodeGID;
assert(nodeFID >= 0 && nodeFID < m_numberAllocatedNodes);
assert(nodeGID >= 0 && nodeGID < m_numberAllocatedNodes);
TreeNode* nodeF = m_nodes + nodeFID;
TreeNode* nodeG = m_nodes + nodeGID;
nodeB->children[0] = nodeID;
nodeB->parentID = nodeA->parentID;
@ -516,12 +513,12 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) {
if (nodeB->parentID != TreeNode::NULL_TREE_NODE) {
if (mNodes[nodeB->parentID].children[0] == nodeID) {
mNodes[nodeB->parentID].children[0] = nodeBID;
if (m_nodes[nodeB->parentID].children[0] == nodeID) {
m_nodes[nodeB->parentID].children[0] = nodeBID;
}
else {
assert(mNodes[nodeB->parentID].children[1] == nodeID);
mNodes[nodeB->parentID].children[1] = nodeBID;
assert(m_nodes[nodeB->parentID].children[1] == nodeID);
m_nodes[nodeB->parentID].children[1] = nodeBID;
}
}
else {
@ -590,7 +587,7 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb,
if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) continue;
// Get the corresponding node
const TreeNode* nodeToVisit = mNodes + nodeIDToVisit;
const TreeNode* nodeToVisit = m_nodes + nodeIDToVisit;
// If the AABB in parameter overlaps with the AABB of the node to visit
if (aabb.testCollision(nodeToVisit->aabb)) {
@ -632,7 +629,7 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &ca
if (nodeID == TreeNode::NULL_TREE_NODE) continue;
// Get the corresponding node
const TreeNode* node = mNodes + nodeID;
const TreeNode* node = m_nodes + nodeID;
Ray rayTemp(ray.point1, ray.point2, maxFraction);
@ -647,12 +644,12 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &ca
// If the user returned a hitFraction of zero, it means that
// the raycasting should stop here
if (hitFraction == float(0.0)) {
if (hitFraction == 0.0f) {
return;
}
// If the user returned a positive fraction
if (hitFraction > float(0.0)) {
if (hitFraction > 0.0f) {
// We update the maxFraction value and the ray
// AABB using the new maximum fraction
@ -682,16 +679,16 @@ void DynamicAABBTree::check() const {
checkNode(m_rootNodeID);
int32_t nbFreeNodes = 0;
int32_t freeNodeID = mFreeNodeID;
int32_t freeNodeID = m_freeNodeID;
// Check the free nodes
while(freeNodeID != TreeNode::NULL_TREE_NODE) {
assert(0 <= freeNodeID && freeNodeID < mNbAllocatedNodes);
freeNodeID = mNodes[freeNodeID].nextNodeID;
assert(0 <= freeNodeID && freeNodeID < m_numberAllocatedNodes);
freeNodeID = m_nodes[freeNodeID].nextNodeID;
nbFreeNodes++;
}
assert(mNbNodes + nbFreeNodes == mNbAllocatedNodes);
assert(m_numberNodes + nbFreeNodes == m_numberAllocatedNodes);
}
// Check if the node structure is valid (for debugging purpose)
@ -701,11 +698,11 @@ void DynamicAABBTree::checkNode(int32_t nodeID) const {
// If it is the root
if (nodeID == m_rootNodeID) {
assert(mNodes[nodeID].parentID == TreeNode::NULL_TREE_NODE);
assert(m_nodes[nodeID].parentID == TreeNode::NULL_TREE_NODE);
}
// Get the children nodes
TreeNode* pNode = mNodes + nodeID;
TreeNode* pNode = m_nodes + nodeID;
assert(!pNode->isLeaf());
int32_t leftChild = pNode->children[0];
int32_t rightChild = pNode->children[1];
@ -724,22 +721,22 @@ void DynamicAABBTree::checkNode(int32_t nodeID) const {
else {
// Check that the children node IDs are valid
assert(0 <= leftChild && leftChild < mNbAllocatedNodes);
assert(0 <= rightChild && rightChild < mNbAllocatedNodes);
assert(0 <= leftChild && leftChild < m_numberAllocatedNodes);
assert(0 <= rightChild && rightChild < m_numberAllocatedNodes);
// Check that the children nodes have the correct parent node
assert(mNodes[leftChild].parentID == nodeID);
assert(mNodes[rightChild].parentID == nodeID);
assert(m_nodes[leftChild].parentID == nodeID);
assert(m_nodes[rightChild].parentID == nodeID);
// Check the height of node
int32_t height = 1 + std::max(mNodes[leftChild].height, mNodes[rightChild].height);
assert(mNodes[nodeID].height == height);
int32_t height = 1 + std::max(m_nodes[leftChild].height, m_nodes[rightChild].height);
assert(m_nodes[nodeID].height == height);
// Check the AABB of the node
AABB aabb;
aabb.mergeTwoAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
assert(aabb.getMin() == mNodes[nodeID].aabb.getMin());
assert(aabb.getMax() == mNodes[nodeID].aabb.getMax());
aabb.mergeTwoAABBs(m_nodes[leftChild].aabb, m_nodes[rightChild].aabb);
assert(aabb.getMin() == m_nodes[nodeID].aabb.getMin());
assert(aabb.getMax() == m_nodes[nodeID].aabb.getMax());
// Recursively check the children nodes
checkNode(leftChild);
@ -754,8 +751,8 @@ int32_t DynamicAABBTree::computeHeight() {
// Compute the height of a given node in the tree
int32_t DynamicAABBTree::computeHeight(int32_t nodeID) {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
TreeNode* node = mNodes + nodeID;
assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes);
TreeNode* node = m_nodes + nodeID;
// If the node is a leaf, its height is zero
if (node->isLeaf()) {

36
ephysics/collision/broadphase/DynamicAABBTree.h
View File

@ -113,23 +113,23 @@ class DynamicAABBTree {
// -------------------- Attributes -------------------- //
/// Pointer to the memory location of the nodes of the tree
TreeNode* mNodes;
TreeNode* m_nodes;
/// ID of the root node of the tree
int32_t m_rootNodeID;
/// ID of the first node of the list of free (allocated) nodes in the tree that we can use
int32_t mFreeNodeID;
int32_t m_freeNodeID;
/// Number of allocated nodes in the tree
int32_t mNbAllocatedNodes;
int32_t m_numberAllocatedNodes;
/// Number of nodes in the tree
int32_t mNbNodes;
int32_t m_numberNodes;
/// Extra AABB Gap used to allow the collision shape to move a little bit
/// without triggering a large modification of the tree which can be costly
float mExtraAABBGap;
float m_extraAABBGap;
// -------------------- Methods -------------------- //
@ -172,7 +172,7 @@ class DynamicAABBTree {
// -------------------- Methods -------------------- //
/// Constructor
DynamicAABBTree(float extraAABBGap = float(0.0));
DynamicAABBTree(float extraAABBGap = 0.0f);
/// Destructor
virtual ~DynamicAABBTree();
@ -187,7 +187,7 @@ class DynamicAABBTree {
void removeObject(int32_t nodeID);
/// Update the dynamic tree after an object has moved.
bool updateObject(int32_t nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert = false);
bool updateObject(int32_t nodeID, const AABB& newAABB, const vec3& displacement, bool forceReinsert = false);
/// Return the fat AABB corresponding to a given node ID
const AABB& getFatAABB(int32_t nodeID) const;
@ -222,22 +222,22 @@ inline bool TreeNode::isLeaf() const {
// Return the fat AABB corresponding to a given node ID
inline const AABB& DynamicAABBTree::getFatAABB(int32_t nodeID) const {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
return mNodes[nodeID].aabb;
assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes);
return m_nodes[nodeID].aabb;
}
// Return the pointer to the data array of a given leaf node of the tree
inline int32_t* DynamicAABBTree::getNodeDataInt(int32_t nodeID) const {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf());
return mNodes[nodeID].dataInt;
assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes);
assert(m_nodes[nodeID].isLeaf());
return m_nodes[nodeID].dataInt;
}
// Return the pointer to the data pointer of a given leaf node of the tree
inline void* DynamicAABBTree::getNodeDataPointer(int32_t nodeID) const {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf());
return mNodes[nodeID].dataPointer;
assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes);
assert(m_nodes[nodeID].isLeaf());
return m_nodes[nodeID].dataPointer;
}
// Return the root AABB of the tree
@ -251,8 +251,8 @@ inline int32_t DynamicAABBTree::addObject(const AABB& aabb, int32_t data1, int32
int32_t nodeId = addObjectInternal(aabb);
mNodes[nodeId].dataInt[0] = data1;
mNodes[nodeId].dataInt[1] = data2;
m_nodes[nodeId].dataInt[0] = data1;
m_nodes[nodeId].dataInt[1] = data2;
return nodeId;
}
@ -263,7 +263,7 @@ inline int32_t DynamicAABBTree::addObject(const AABB& aabb, void* data) {
int32_t nodeId = addObjectInternal(aabb);
mNodes[nodeId].dataPointer = data;
m_nodes[nodeId].dataPointer = data;
return nodeId;
}

36
ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
View File

@ -85,7 +85,7 @@ void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Inf
}
// Test collision between a triangle and the convex mesh shape
void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) {
void ConvexVsTriangleCallback::testTriangle(const vec3* trianglePoints) {
// Create a triangle collision shape
float margin = mConcaveShape->getTriangleMargin();
@ -120,7 +120,7 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
NarrowPhaseCallback* narrowPhaseCallback) {
// Set with the triangle vertices already processed to void further contacts with same triangle
std::unordered_multimap<int32_t, Vector3> processTriangleVertices;
std::unordered_multimap<int32_t, vec3> processTriangleVertices;
// Sort the list of narrow-phase contacts according to their penetration depth
std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare());
@ -130,7 +130,7 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
const SmoothMeshContactInfo info = *it;
const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
const vec3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
// Compute the barycentric coordinates of the point in the triangle
float u, v, w;
@ -149,7 +149,7 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
// If it is a vertex contact
if (nbZeros == 2) {
Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]);
vec3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]);
// Check that this triangle vertex has not been processed yet
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
@ -160,8 +160,8 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
}
else if (nbZeros == 1) { // If it is an edge contact
Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]);
Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]);
vec3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]);
vec3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]);
// Check that this triangle edge has not been processed yet
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) &&
@ -188,12 +188,12 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
}
// We use the triangle normal as the contact normal
Vector3 a = info.triangleVertices[1] - info.triangleVertices[0];
Vector3 b = info.triangleVertices[2] - info.triangleVertices[0];
Vector3 localNormal = a.cross(b);
vec3 a = info.triangleVertices[1] - info.triangleVertices[0];
vec3 b = info.triangleVertices[2] - info.triangleVertices[0];
vec3 localNormal = a.cross(b);
newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal;
Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint;
vec3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
vec3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint;
newContactInfo.normal.normalize();
if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) {
newContactInfo.normal = -newContactInfo.normal;
@ -202,13 +202,13 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
// We recompute the contact point on the second body with the new normal as described in
// the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and
// Dirk Gregorius) to avoid adding torque
Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse();
etk::Transform3D worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse();
if (info.isFirstShapeTriangle) {
Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal;
vec3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal;
newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint;
}
else {
Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal;
vec3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal;
newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
}
@ -225,13 +225,13 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
}
// Return true if the vertex is in the set of already processed vertices
bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap<int32_t, Vector3>& processTriangleVertices, const Vector3& vertex) const {
bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap<int32_t, vec3>& processTriangleVertices, const vec3& vertex) const {
int32_t key = int32_t(vertex.x * vertex.y * vertex.z);
int32_t key = int32_t(vertex.x() * vertex.y() * vertex.z());
auto range = processTriangleVertices.equal_range(key);
for (auto it = range.first; it != range.second; ++it) {
if (vertex.x == it->second.x && vertex.y == it->second.y && vertex.z == it->second.z) return true;
if (vertex.x() == it->second.x() && vertex.y() == it->second.y() && vertex.z() == it->second.z()) return true;
}
return false;
@ -240,7 +240,7 @@ bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multi
// Called by a narrow-phase collision algorithm when a new contact has been found
void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo) {
Vector3 triangleVertices[3];
vec3 triangleVertices[3];
bool isFirstShapeTriangle;
// If the collision shape 1 is the triangle

20
ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.h
View File

@ -83,7 +83,7 @@ class ConvexVsTriangleCallback : public TriangleCallback {
}
/// Test collision between a triangle and the convex mesh shape
virtual void testTriangle(const Vector3* trianglePoints);
virtual void testTriangle(const vec3* trianglePoints);
};
// Class SmoothMeshContactInfo
@ -97,11 +97,11 @@ class SmoothMeshContactInfo {
ContactPointInfo contactInfo;
bool isFirstShapeTriangle;
Vector3 triangleVertices[3];
vec3 triangleVertices[3];
/// Constructor
SmoothMeshContactInfo(const ContactPointInfo& contact, bool firstShapeTriangle, const Vector3& trianglePoint1,
const Vector3& trianglePoint2, const Vector3& trianglePoint3)
SmoothMeshContactInfo(const ContactPointInfo& contact, bool firstShapeTriangle, const vec3& trianglePoint1,
const vec3& trianglePoint2, const vec3& trianglePoint3)
: contactInfo(contact) {
isFirstShapeTriangle = firstShapeTriangle;
triangleVertices[0] = trianglePoint1;
@ -179,12 +179,12 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
NarrowPhaseCallback* narrowPhaseCallback);
/// Add a triangle vertex int32_to the set of processed triangles
void addProcessedVertex(std::unordered_multimap<int32_t, Vector3>& processTriangleVertices,
const Vector3& vertex);
void addProcessedVertex(std::unordered_multimap<int32_t, vec3>& processTriangleVertices,
const vec3& vertex);
/// Return true if the vertex is in the set of already processed vertices
bool hasVertexBeenProcessed(const std::unordered_multimap<int32_t, Vector3>& processTriangleVertices,
const Vector3& vertex) const;
bool hasVertexBeenProcessed(const std::unordered_multimap<int32_t, vec3>& processTriangleVertices,
const vec3& vertex) const;
public :
@ -203,8 +203,8 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
};
// Add a triangle vertex int32_to the set of processed triangles
inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap<int32_t, Vector3>& processTriangleVertices, const Vector3& vertex) {
processTriangleVertices.insert(std::make_pair(int32_t(vertex.x * vertex.y * vertex.z), vertex));
inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap<int32_t, vec3>& processTriangleVertices, const vec3& vertex) {
processTriangleVertices.insert(std::make_pair(int32_t(vertex.x() * vertex.y() * vertex.z()), vertex));
}
}

58
ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp
View File

@ -26,29 +26,29 @@ EPAAlgorithm::~EPAAlgorithm() {
// Decide if the origin is in the tetrahedron.
/// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of
/// the vertex that is wrong if the origin is not in the tetrahedron
int32_t EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
const Vector3& p3, const Vector3& p4) const {
int32_t EPAAlgorithm::isOriginInTetrahedron(const vec3& p1, const vec3& p2,
const vec3& p3, const vec3& p4) const {
// Check vertex 1
Vector3 normal1 = (p2-p1).cross(p3-p1);
vec3 normal1 = (p2-p1).cross(p3-p1);
if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) {
return 4;
}
// Check vertex 2
Vector3 normal2 = (p4-p2).cross(p3-p2);
vec3 normal2 = (p4-p2).cross(p3-p2);
if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) {
return 1;
}
// Check vertex 3
Vector3 normal3 = (p4-p3).cross(p1-p3);
vec3 normal3 = (p4-p3).cross(p1-p3);
if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) {
return 2;
}
// Check vertex 4
Vector3 normal4 = (p2-p4).cross(p1-p4);
vec3 normal4 = (p2-p4).cross(p1-p4);
if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) {
return 3;
}
@ -65,10 +65,10 @@ int32_t EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2
/// the correct penetration depth
void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex,
CollisionShapeInfo shape1Info,
const Transform& transform1,
const etk::Transform3D& transform1,
CollisionShapeInfo shape2Info,
const Transform& transform2,
Vector3& v,
const etk::Transform3D& transform2,
vec3& v,
NarrowPhaseCallback* narrowPhaseCallback) {
PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()");
@ -82,20 +82,20 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
Vector3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
Vector3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
Vector3 points[MAX_SUPPORT_POINTS]; // Current points
vec3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
vec3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
vec3 points[MAX_SUPPORT_POINTS]; // Current points
TrianglesStore triangleStore; // Store the triangles
TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face
// candidate of the EPA algorithm
// Transform a point from local space of body 2 to local
// etk::Transform3D a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2Tobody1 = transform1.getInverse() * transform2;
etk::Transform3D body2Tobody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local
// space of body 1 int32_to local space of body 2
Quaternion rotateToBody2 = transform2.getOrientation().getInverse() *
etk::Quaternion rotateToBody2 = transform2.getOrientation().getInverse() *
transform1.getOrientation();
// Get the simplex computed previously by the GJK algorithm
@ -130,22 +130,22 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// v1, v2 and v3.
// Direction of the segment
Vector3 d = (points[1] - points[0]).getUnit();
vec3 d = (points[1] - points[0]).safeNormalized();
// Choose the coordinate axis from the minimal absolute component of the vector d
int32_t minAxis = d.getAbsoluteVector().getMinAxis();
int32_t minAxis = d.absolute().getMinAxis();
// Compute sin(60)
const float sin60 = float(sqrt(3.0)) * float(0.5);
const float sin60 = float(sqrt(3.0)) * 0.5f;
// Create a rotation quaternion to rotate the vector v1 to get the vectors
// v2 and v3
Quaternion rotationQuat(d.x * sin60, d.y * sin60, d.z * sin60, 0.5);
etk::Quaternion rotationQuat(d.x() * sin60, d.y() * sin60, d.z() * sin60, 0.5);
// Compute the vector v1, v2, v3
Vector3 v1 = d.cross(Vector3(minAxis == 0, minAxis == 1, minAxis == 2));
Vector3 v2 = rotationQuat * v1;
Vector3 v3 = rotationQuat * v2;
vec3 v1 = d.cross(vec3(minAxis == 0, minAxis == 1, minAxis == 2));
vec3 v2 = rotationQuat * v1;
vec3 v3 = rotationQuat * v2;
// Compute the support point in the direction of v1
suppPointsA[2] = shape1->getLocalSupportPointWithMargin(v1, shape1CachedCollisionData);
@ -256,9 +256,9 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// tetrahedron that contains the origin.
// Compute the normal of the triangle
Vector3 v1 = points[1] - points[0];
Vector3 v2 = points[2] - points[0];
Vector3 n = v1.cross(v2);
vec3 v1 = points[1] - points[0];
vec3 v2 = points[2] - points[0];
vec3 n = v1.cross(v2);
// Compute the two new vertices to obtain a hexahedron
suppPointsA[3] = shape1->getLocalSupportPointWithMargin(n, shape1CachedCollisionData);
@ -404,13 +404,13 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// Compute the contact info
v = transform1.getOrientation() * triangle->getClosestPoint();
Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
Vector3 normal = v.getUnit();
vec3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
vec3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
vec3 normal = v.safeNormalized();
float penetrationDepth = v.length();
assert(penetrationDepth > 0.0);
if (normal.lengthSquare() < MACHINE_EPSILON) return;
if (normal.length2() < MACHINE_EPSILON) return;
// Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,

10
ephysics/collision/narrowphase/EPA/EPAAlgorithm.h
View File

@ -85,8 +85,8 @@ class EPAAlgorithm {
float upperBoundSquarePenDepth);
/// Decide if the origin is in the tetrahedron.
int32_t isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
const Vector3& p3, const Vector3& p4) const;
int32_t isOriginInTetrahedron(const vec3& p1, const vec3& p2,
const vec3& p3, const vec3& p4) const;
public:
@ -104,10 +104,10 @@ class EPAAlgorithm {
/// Compute the penetration depth with EPA algorithm.
void computePenetrationDepthAndContactPoints(const Simplex& simplex,
CollisionShapeInfo shape1Info,
const Transform& transform1,
const etk::Transform3D& transform1,
CollisionShapeInfo shape2Info,
const Transform& transform2,
Vector3& v,
const etk::Transform3D& transform2,
vec3& v,
NarrowPhaseCallback* narrowPhaseCallback);
};

2
ephysics/collision/narrowphase/EPA/EdgeEPA.cpp
View File

@ -47,7 +47,7 @@ uint32_t EdgeEPA::getTargetVertexIndex() const {
}
// Execute the recursive silhouette algorithm from this edge
bool EdgeEPA::computeSilhouette(const Vector3* vertices, uint32_t indexNewVertex,
bool EdgeEPA::computeSilhouette(const vec3* vertices, uint32_t indexNewVertex,
TrianglesStore& triangleStore) {
// If the edge has not already been visited
if (!mOwnerTriangle->getIsObsolete()) {

2
ephysics/collision/narrowphase/EPA/EdgeEPA.h
View File

@ -62,7 +62,7 @@ class EdgeEPA {
uint32_t getTargetVertexIndex() const;
/// Execute the recursive silhouette algorithm from this edge
bool computeSilhouette(const Vector3* vertices, uint32_t index, TrianglesStore& triangleStore);
bool computeSilhouette(const vec3* vertices, uint32_t index, TrianglesStore& triangleStore);
/// Assignment operator
EdgeEPA& operator=(const EdgeEPA& edge);

12
ephysics/collision/narrowphase/EPA/TriangleEPA.cpp
View File

@ -31,11 +31,11 @@ TriangleEPA::~TriangleEPA() {
}
// Compute the point v closest to the origin of this triangle
bool TriangleEPA::computeClosestPoint(const Vector3* vertices) {
const Vector3& p0 = vertices[mIndicesVertices[0]];
bool TriangleEPA::computeClosestPoint(const vec3* vertices) {
const vec3& p0 = vertices[mIndicesVertices[0]];
Vector3 v1 = vertices[mIndicesVertices[1]] - p0;
Vector3 v2 = vertices[mIndicesVertices[2]] - p0;
vec3 v1 = vertices[mIndicesVertices[1]] - p0;
vec3 v2 = vertices[mIndicesVertices[2]] - p0;
float v1Dotv1 = v1.dot(v1);
float v1Dotv2 = v1.dot(v2);
float v2Dotv2 = v2.dot(v2);
@ -52,7 +52,7 @@ bool TriangleEPA::computeClosestPoint(const Vector3* vertices) {
// If the determinant is positive
if (mDet > 0.0) {
// Compute the closest point v
mClosestPoint = p0 + float(1.0) / mDet * (mLambda1 * v1 + mLambda2 * v2);
mClosestPoint = p0 + 1.0f / mDet * (mLambda1 * v1 + mLambda2 * v2);
// Compute the square distance of closest point to the origin
mDistSquare = mClosestPoint.dot(mClosestPoint);
@ -101,7 +101,7 @@ void reactphysics3d::halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1) {
/// face from the new vertex, computes the silhouette and create the new faces from the new vertex in
/// order that we always have a convex polytope. The faces visible from the new vertex are set
/// obselete and will not be considered as being a candidate face in the future.
bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint32_t indexNewVertex,
bool TriangleEPA::computeSilhouette(const vec3* vertices, uint32_t indexNewVertex,
TrianglesStore& triangleStore) {
uint32_t first = triangleStore.getNbTriangles();

24
ephysics/collision/narrowphase/EPA/TriangleEPA.h
View File

@ -42,7 +42,7 @@ class TriangleEPA {
float mDet;
/// Point v closest to the origin on the affine hull of the triangle
Vector3 mClosestPoint;
vec3 mClosestPoint;
/// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
float mLambda1;
@ -90,22 +90,22 @@ class TriangleEPA {
bool getIsObsolete() const;
/// Return the point closest to the origin
const Vector3& getClosestPoint() const;
const vec3& getClosestPoint() const;
// Return true if the closest point on affine hull is inside the triangle
bool isClosestPointInternalToTriangle() const;
/// Return true if the triangle is visible from a given vertex
bool isVisibleFromVertex(const Vector3* vertices, uint32_t index) const;
bool isVisibleFromVertex(const vec3* vertices, uint32_t index) const;
/// Compute the point v closest to the origin of this triangle
bool computeClosestPoint(const Vector3* vertices);
bool computeClosestPoint(const vec3* vertices);
/// Compute the point of an object closest to the origin
Vector3 computeClosestPointOfObject(const Vector3* supportPointsOfObject) const;
vec3 computeClosestPointOfObject(const vec3* supportPointsOfObject) const;
/// Execute the recursive silhouette algorithm from this triangle face.
bool computeSilhouette(const Vector3* vertices, uint32_t index, TrianglesStore& triangleStore);
bool computeSilhouette(const vec3* vertices, uint32_t index, TrianglesStore& triangleStore);
/// Access operator
uint32_t operator[](int32_t i) const;
@ -145,7 +145,7 @@ inline bool TriangleEPA::getIsObsolete() const {
}
// Return the point closest to the origin
inline const Vector3& TriangleEPA::getClosestPoint() const {
inline const vec3& TriangleEPA::getClosestPoint() const {
return mClosestPoint;
}
@ -155,15 +155,15 @@ inline bool TriangleEPA::isClosestPointInternalToTriangle() const {
}
// Return true if the triangle is visible from a given vertex
inline bool TriangleEPA::isVisibleFromVertex(const Vector3* vertices, uint32_t index) const {
Vector3 closestToVert = vertices[index] - mClosestPoint;
inline bool TriangleEPA::isVisibleFromVertex(const vec3* vertices, uint32_t index) const {
vec3 closestToVert = vertices[index] - mClosestPoint;
return (mClosestPoint.dot(closestToVert) > 0.0);
}
// Compute the point of an object closest to the origin
inline Vector3 TriangleEPA::computeClosestPointOfObject(const Vector3* supportPointsOfObject) const{
const Vector3& p0 = supportPointsOfObject[mIndicesVertices[0]];
return p0 + float(1.0)/mDet * (mLambda1 * (supportPointsOfObject[mIndicesVertices[1]] - p0) +
inline vec3 TriangleEPA::computeClosestPointOfObject(const vec3* supportPointsOfObject) const{
const vec3& p0 = supportPointsOfObject[mIndicesVertices[0]];
return p0 + 1.0f/mDet * (mLambda1 * (supportPointsOfObject[mIndicesVertices[1]] - p0) +
mLambda2 * (supportPointsOfObject[mIndicesVertices[2]] - p0));
}

4
ephysics/collision/narrowphase/EPA/TrianglesStore.h
View File

@ -64,7 +64,7 @@ class TrianglesStore {
TriangleEPA& last();
/// Create a new triangle
TriangleEPA* newTriangle(const Vector3* vertices, uint32_t v0, uint32_t v1, uint32_t v2);
TriangleEPA* newTriangle(const vec3* vertices, uint32_t v0, uint32_t v1, uint32_t v2);
/// Access operator
TriangleEPA& operator[](int32_t i);
@ -92,7 +92,7 @@ inline TriangleEPA& TrianglesStore::last() {
}
// Create a new triangle
inline TriangleEPA* TrianglesStore::newTriangle(const Vector3* vertices,
inline TriangleEPA* TrianglesStore::newTriangle(const vec3* vertices,
uint32_t v0,uint32_t v1, uint32_t v2) {
TriangleEPA* newTriangle = NULL;

96
ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

@ -43,11 +43,11 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
PROFILE("GJKAlgorithm::testCollision()");
Vector3 suppA; // Support point of object A
Vector3 suppB; // Support point of object B
Vector3 w; // Support point of Minkowski difference A-B
Vector3 pA; // Closest point of object A
Vector3 pB; // Closest point of object B
vec3 suppA; // Support point of object A
vec3 suppB; // Support point of object B
vec3 w; // Support point of Minkowski difference A-B
vec3 pA; // Closest point of object A
vec3 pB; // Closest point of object B
float vDotw;
float prevDistSquare;
@ -61,16 +61,16 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
// Get the local-space to world-space transforms
const Transform transform1 = shape1Info.shapeToWorldTransform;
const Transform transform2 = shape2Info.shapeToWorldTransform;
const etk::Transform3D transform1 = shape1Info.shapeToWorldTransform;
const etk::Transform3D transform2 = shape2Info.shapeToWorldTransform;
// Transform a point from local space of body 2 to local
// etk::Transform3D a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2Tobody1 = transform1.getInverse() * transform2;
etk::Transform3D body2Tobody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local
// space of body 1 int32_to local space of body 2
Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
etk::Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
transform1.getOrientation().getMatrix();
// Initialize the margin (sum of margins of both objects)
@ -82,7 +82,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
Simplex simplex;
// Get the previous point V (last cached separating axis)
Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis();
vec3 v = mCurrentOverlappingPair->getCachedSeparatingAxis();
// Initialize the upper bound for the square distance
float distSquare = DECIMAL_LARGEST;
@ -123,7 +123,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
vec3 normal = transform1.getOrientation() * (-v.safeNormalized());
float penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
@ -156,7 +156,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
vec3 normal = transform1.getOrientation() * (-v.safeNormalized());
float penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
@ -187,7 +187,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
vec3 normal = transform1.getOrientation() * (-v.safeNormalized());
float penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
@ -205,14 +205,14 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
// Store and update the squared distance of the closest point
prevDistSquare = distSquare;
distSquare = v.lengthSquare();
distSquare = v.length2();
// If the distance to the closest point doesn't improve a lot
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
simplex.backupClosestPointInSimplex(v);
// Get the new squared distance
distSquare = v.lengthSquare();
distSquare = v.length2();
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
@ -225,7 +225,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
vec3 normal = transform1.getOrientation() * (-v.safeNormalized());
float penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
@ -257,17 +257,17 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
/// a polytope must exist. Then, we give that polytope to the EPA algorithm to
/// compute the correct penetration depth and contact points of the enlarged objects.
void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
const Transform& transform1,
const etk::Transform3D& transform1,
const CollisionShapeInfo& shape2Info,
const Transform& transform2,
const etk::Transform3D& transform2,
NarrowPhaseCallback* narrowPhaseCallback,
Vector3& v) {
vec3& v) {
PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()");
Simplex simplex;
Vector3 suppA;
Vector3 suppB;
Vector3 w;
vec3 suppA;
vec3 suppB;
vec3 w;
float vDotw;
float distSquare = DECIMAL_LARGEST;
float prevDistSquare;
@ -281,12 +281,12 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
// Transform a point from local space of body 2 to local space
// etk::Transform3D a point from local space of body 2 to local space
// of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2ToBody1 = transform1.getInverse() * transform2;
etk::Transform3D body2ToBody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local space of body 1 int32_to local space of body 2
Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
etk::Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
transform1.getOrientation().getMatrix();
do {
@ -319,7 +319,7 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
// Store and update the square distance
prevDistSquare = distSquare;
distSquare = v.lengthSquare();
distSquare = v.length2();
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
return;
@ -337,10 +337,10 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
}
// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) {
bool GJKAlgorithm::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) {
Vector3 suppA; // Support point of object A
Vector3 w; // Support point of Minkowski difference A-B
vec3 suppA; // Support point of object A
vec3 w; // Support point of Minkowski difference A-B
float prevDistSquare;
assert(proxyShape->getCollisionShape()->isConvex());
@ -350,13 +350,13 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
void** shapeCachedCollisionData = proxyShape->getCachedCollisionData();
// Support point of object B (object B is a single point)
const Vector3 suppB(localPoint);
const vec3 suppB(localPoint);
// Create a simplex set
Simplex simplex;
// Initial supporting direction
Vector3 v(1, 1, 1);
vec3 v(1, 1, 1);
// Initialize the upper bound for the square distance
float distSquare = DECIMAL_LARGEST;
@ -387,7 +387,7 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
// Store and update the squared distance of the closest point
prevDistSquare = distSquare;
distSquare = v.lengthSquare();
distSquare = v.length2();
// If the distance to the closest point doesn't improve a lot
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
@ -413,29 +413,29 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
void** shapeCachedCollisionData = proxyShape->getCachedCollisionData();
Vector3 suppA; // Current lower bound point on the ray (starting at ray's origin)
Vector3 suppB; // Support point on the collision shape
vec3 suppA; // Current lower bound point on the ray (starting at ray's origin)
vec3 suppB; // Support point on the collision shape
const float machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON;
const float epsilon = float(0.0001);
// Convert the ray origin and direction int32_to the local-space of the collision shape
Vector3 rayDirection = ray.point2 - ray.point1;
vec3 rayDirection = ray.point2 - ray.point1;
// If the points of the segment are two close, return no hit
if (rayDirection.lengthSquare() < machineEpsilonSquare) return false;
if (rayDirection.length2() < machineEpsilonSquare) return false;
Vector3 w;
vec3 w;
// Create a simplex set
Simplex simplex;
Vector3 n(float(0.0), float(0.0), float(0.0));
float lambda = float(0.0);
vec3 n(0.0f, float(0.0), float(0.0));
float lambda = 0.0f;
suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin)
suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData);
Vector3 v = suppA - suppB;
vec3 v = suppA - suppB;
float vDotW, vDotR;
float distSquare = v.lengthSquare();
float distSquare = v.length2();
int32_t nbIterations = 0;
// GJK Algorithm loop
@ -472,10 +472,10 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
// Compute the closest point
if (simplex.computeClosestPoint(v)) {
distSquare = v.lengthSquare();
distSquare = v.length2();
}
else {
distSquare = float(0.0);
distSquare = 0.0f;
}
// If the current lower bound distance is larger than the maximum raycasting distance
@ -488,8 +488,8 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
if (lambda < MACHINE_EPSILON) return false;
// Compute the closet points of both objects (without the margins)
Vector3 pointA;
Vector3 pointB;
vec3 pointA;
vec3 pointB;
simplex.computeClosestPointsOfAandB(pointA, pointB);
// A raycast hit has been found, we fill in the raycast info
@ -498,11 +498,11 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
if (n.lengthSquare() >= machineEpsilonSquare) { // The normal vector is valid
if (n.length2() >= machineEpsilonSquare) { // The normal vector is valid
raycastInfo.worldNormal = n;
}
else { // Degenerated normal vector, we return a zero normal vector
raycastInfo.worldNormal = Vector3(float(0), float(0), float(0));
raycastInfo.worldNormal = vec3(float(0), float(0), float(0));
}
return true;

8
ephysics/collision/narrowphase/GJK/GJKAlgorithm.h
View File

@ -55,11 +55,11 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
/// Compute the penetration depth for enlarged objects.
void computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
const Transform& transform1,
const etk::Transform3D& transform1,
const CollisionShapeInfo& shape2Info,
const Transform& transform2,
const etk::Transform3D& transform2,
NarrowPhaseCallback* narrowPhaseCallback,
Vector3& v);
vec3& v);
public :
@ -81,7 +81,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
NarrowPhaseCallback* narrowPhaseCallback);
/// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape);
bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape);
/// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo);

28
ephysics/collision/narrowphase/GJK/Simplex.cpp
View File

@ -25,7 +25,7 @@ Simplex::~Simplex() {
/// suppPointA : support point of object A in a direction -v
/// suppPointB : support point of object B in a direction v
/// point : support point of object (A-B) => point = suppPointA - suppPointB
void Simplex::addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB) {
void Simplex::addPoint(const vec3& point, const vec3& suppPointA, const vec3& suppPointB) {
assert(!isFull());
mLastFound = 0;
@ -57,7 +57,7 @@ void Simplex::addPoint(const Vector3& point, const Vector3& suppPointA, const Ve
}
// Return true if the point is in the simplex
bool Simplex::isPointInSimplex(const Vector3& point) const {
bool Simplex::isPointInSimplex(const vec3& point) const {
int32_t i;
Bits bit;
@ -93,8 +93,8 @@ void Simplex::updateCache() {
}
// Return the points of the simplex
uint32_t Simplex::getSimplex(Vector3* suppPointsA, Vector3* suppPointsB,
Vector3* points) const {
uint32_t Simplex::getSimplex(vec3* suppPointsA, vec3* suppPointsB,
vec3* points) const {
uint32_t nbVertices = 0;
int32_t i;
Bits bit;
@ -272,10 +272,10 @@ bool Simplex::isValidSubset(Bits subset) const {
/// pA = sum(lambda_i * a_i) where "a_i" are the support points of object A
/// pB = sum(lambda_i * b_i) where "b_i" are the support points of object B
/// with lambda_i = deltaX_i / deltaX
void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {
void Simplex::computeClosestPointsOfAandB(vec3& pA, vec3& pB) const {
float deltaX = 0.0;
pA.setAllValues(0.0, 0.0, 0.0);
pB.setAllValues(0.0, 0.0, 0.0);
pA.setValue(0.0, 0.0, 0.0);
pB.setValue(0.0, 0.0, 0.0);
int32_t i;
Bits bit;
@ -290,7 +290,7 @@ void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {
}
assert(deltaX > 0.0);
float factor = float(1.0) / deltaX;
float factor = 1.0f / deltaX;
pA *= factor;
pB *= factor;
}
@ -299,7 +299,7 @@ void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {
/// This method executes the Jonhnson's algorithm for computing the point
/// "v" of simplex that is closest to the origin. The method returns true
/// if a closest point has been found.
bool Simplex::computeClosestPoint(Vector3& v) {
bool Simplex::computeClosestPoint(vec3& v) {
Bits subset;
// For each possible simplex set
@ -326,13 +326,13 @@ bool Simplex::computeClosestPoint(Vector3& v) {
}
// Backup the closest point
void Simplex::backupClosestPointInSimplex(Vector3& v) {
void Simplex::backupClosestPointInSimplex(vec3& v) {
float minDistSquare = DECIMAL_LARGEST;
Bits bit;
for (bit = mAllBits; bit != 0x0; bit--) {
if (isSubset(bit, mAllBits) && isProperSubset(bit)) {
Vector3 u = computeClosestPointForSubset(bit);
vec3 u = computeClosestPointForSubset(bit);
float distSquare = u.dot(u);
if (distSquare < minDistSquare) {
minDistSquare = distSquare;
@ -345,8 +345,8 @@ void Simplex::backupClosestPointInSimplex(Vector3& v) {
// Return the closest point "v" in the convex hull of the points in the subset
// represented by the bits "subset"
Vector3 Simplex::computeClosestPointForSubset(Bits subset) {
Vector3 v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i])
vec3 Simplex::computeClosestPointForSubset(Bits subset) {
vec3 v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i])
mMaxLengthSquare = 0.0;
float deltaX = 0.0; // deltaX = sum of all det[subset][i]
int32_t i;
@ -371,5 +371,5 @@ Vector3 Simplex::computeClosestPointForSubset(Bits subset) {
assert(deltaX > 0.0);
// Return the closet point "v" in the convex hull for the given subset
return (float(1.0) / deltaX) * v;
return (1.0f / deltaX) * v;
}

24
ephysics/collision/narrowphase/GJK/Simplex.h
View File

@ -31,7 +31,7 @@ class Simplex {
// -------------------- Attributes -------------------- //
/// Current points
Vector3 mPoints[4];
vec3 mPoints[4];
/// pointsLengthSquare[i] = (points[i].length)^2
float mPointsLengthSquare[4];
@ -40,13 +40,13 @@ class Simplex {
float mMaxLengthSquare;
/// Support points of object A in local coordinates
Vector3 mSuppPointsA[4];
vec3 mSuppPointsA[4];
/// Support points of object B in local coordinates
Vector3 mSuppPointsB[4];
vec3 mSuppPointsB[4];
/// diff[i][j] contains points[i] - points[j]
Vector3 mDiffLength[4][4];
vec3 mDiffLength[4][4];
/// Cached determinant values
float mDet[16][4];
@ -94,7 +94,7 @@ class Simplex {
void computeDeterminants();
/// Return the closest point "v" in the convex hull of a subset of points
Vector3 computeClosestPointForSubset(Bits subset);
vec3 computeClosestPointForSubset(Bits subset);
public:
@ -113,29 +113,29 @@ class Simplex {
bool isEmpty() const;
/// Return the points of the simplex
uint32_t getSimplex(Vector3* mSuppPointsA, Vector3* mSuppPointsB,
Vector3* mPoints) const;
uint32_t getSimplex(vec3* mSuppPointsA, vec3* mSuppPointsB,
vec3* mPoints) const;
/// Return the maximum squared length of a point
float getMaxLengthSquareOfAPoint() const;
/// Add a new support point of (A-B) int32_to the simplex.
void addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB);
void addPoint(const vec3& point, const vec3& suppPointA, const vec3& suppPointB);
/// Return true if the point is in the simplex
bool isPointInSimplex(const Vector3& point) const;
bool isPointInSimplex(const vec3& point) const;
/// Return true if the set is affinely dependent
bool isAffinelyDependent() const;
/// Backup the closest point
void backupClosestPointInSimplex(Vector3& point);
void backupClosestPointInSimplex(vec3& point);
/// Compute the closest points "pA" and "pB" of object A and B.
void computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const;
void computeClosestPointsOfAandB(vec3& pA, vec3& pB) const;
/// Compute the closest point to the origin of the current simplex.
bool computeClosestPoint(Vector3& v);
bool computeClosestPoint(vec3& v);
};
// Return true if some bits of "a" overlap with bits of "b"

22
ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp
View File

@ -29,26 +29,26 @@ void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info
const SphereShape* sphereShape1 = static_cast<const SphereShape*>(shape1Info.collisionShape);
const SphereShape* sphereShape2 = static_cast<const SphereShape*>(shape2Info.collisionShape);
// Get the local-space to world-space transforms
const Transform& transform1 = shape1Info.shapeToWorldTransform;
const Transform& transform2 = shape2Info.shapeToWorldTransform;
const etk::Transform3D& transform1 = shape1Info.shapeToWorldTransform;
const etk::Transform3D& transform2 = shape2Info.shapeToWorldTransform;
// Compute the distance between the centers
Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
float squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
vec3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
float squaredDistanceBetweenCenters = vectorBetweenCenters.length2();
// Compute the sum of the radius
float sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius();
// If the sphere collision shapes int32_tersect
if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
Vector3 int32_tersectionOnBody1 = sphereShape1->getRadius() *
centerSphere2InBody1LocalSpace.getUnit();
Vector3 int32_tersectionOnBody2 = sphereShape2->getRadius() *
centerSphere1InBody2LocalSpace.getUnit();
vec3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
vec3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
vec3 int32_tersectionOnBody1 = sphereShape1->getRadius() *
centerSphere2InBody1LocalSpace.safeNormalized();
vec3 int32_tersectionOnBody2 = sphereShape2->getRadius() *
centerSphere1InBody2LocalSpace.safeNormalized();
float penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
// Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, vectorBetweenCenters.getUnit(), penetrationDepth,
shape2Info.collisionShape, vectorBetweenCenters.safeNormalized(), penetrationDepth,
int32_tersectionOnBody1, int32_tersectionOnBody2);
// Notify about the new contact
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);

157
ephysics/collision/shapes/AABB.cpp
View File

@ -19,15 +19,17 @@ AABB::AABB() {
}
// Constructor
AABB::AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates)
:m_minCoordinates(minCoordinates), m_maxCoordinates(maxCoordinates) {
AABB::AABB(const vec3& minCoordinates, const vec3& maxCoordinates):
m_minCoordinates(minCoordinates),
m_maxCoordinates(maxCoordinates) {
}
// Copy-constructor
AABB::AABB(const AABB& aabb)
: m_minCoordinates(aabb.m_minCoordinates), m_maxCoordinates(aabb.m_maxCoordinates) {
AABB::AABB(const AABB& aabb):
m_minCoordinates(aabb.m_minCoordinates),
m_maxCoordinates(aabb.m_maxCoordinates) {
}
// Destructor
@ -37,62 +39,76 @@ AABB::~AABB() {
// Merge the AABB in parameter with the current one
void AABB::mergeWithAABB(const AABB& aabb) {
m_minCoordinates.x = std::min(m_minCoordinates.x, aabb.m_minCoordinates.x);
m_minCoordinates.y = std::min(m_minCoordinates.y, aabb.m_minCoordinates.y);
m_minCoordinates.z = std::min(m_minCoordinates.z, aabb.m_minCoordinates.z);
m_maxCoordinates.x = std::max(m_maxCoordinates.x, aabb.m_maxCoordinates.x);
m_maxCoordinates.y = std::max(m_maxCoordinates.y, aabb.m_maxCoordinates.y);
m_maxCoordinates.z = std::max(m_maxCoordinates.z, aabb.m_maxCoordinates.z);
m_minCoordinates.setX(std::min(m_minCoordinates.x(), aabb.m_minCoordinates.x()));
m_minCoordinates.setY(std::min(m_minCoordinates.y(), aabb.m_minCoordinates.y()));
m_minCoordinates.setZ(std::min(m_minCoordinates.z(), aabb.m_minCoordinates.z()));
m_maxCoordinates.setX(std::max(m_maxCoordinates.x(), aabb.m_maxCoordinates.x()));
m_maxCoordinates.setY(std::max(m_maxCoordinates.y(), aabb.m_maxCoordinates.y()));
m_maxCoordinates.setZ(std::max(m_maxCoordinates.z(), aabb.m_maxCoordinates.z()));
}
// Replace the current AABB with a new AABB that is the union of two AABBs in parameters
void AABB::mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2) {
m_minCoordinates.x = std::min(aabb1.m_minCoordinates.x, aabb2.m_minCoordinates.x);
m_minCoordinates.y = std::min(aabb1.m_minCoordinates.y, aabb2.m_minCoordinates.y);
m_minCoordinates.z = std::min(aabb1.m_minCoordinates.z, aabb2.m_minCoordinates.z);
m_maxCoordinates.x = std::max(aabb1.m_maxCoordinates.x, aabb2.m_maxCoordinates.x);
m_maxCoordinates.y = std::max(aabb1.m_maxCoordinates.y, aabb2.m_maxCoordinates.y);
m_maxCoordinates.z = std::max(aabb1.m_maxCoordinates.z, aabb2.m_maxCoordinates.z);
m_minCoordinates.setX(std::min(aabb1.m_minCoordinates.x(), aabb2.m_minCoordinates.x()));
m_minCoordinates.setY(std::min(aabb1.m_minCoordinates.y(), aabb2.m_minCoordinates.y()));
m_minCoordinates.setZ(std::min(aabb1.m_minCoordinates.z(), aabb2.m_minCoordinates.z()));
m_maxCoordinates.setX(std::max(aabb1.m_maxCoordinates.x(), aabb2.m_maxCoordinates.x()));
m_maxCoordinates.setY(std::max(aabb1.m_maxCoordinates.y(), aabb2.m_maxCoordinates.y()));
m_maxCoordinates.setZ(std::max(aabb1.m_maxCoordinates.z(), aabb2.m_maxCoordinates.z()));
}
// Return true if the current AABB contains the AABB given in parameter
bool AABB::contains(const AABB& aabb) const {
bool isInside = true;
isInside = isInside && m_minCoordinates.x <= aabb.m_minCoordinates.x;
isInside = isInside && m_minCoordinates.y <= aabb.m_minCoordinates.y;
isInside = isInside && m_minCoordinates.z <= aabb.m_minCoordinates.z;
isInside = isInside && m_maxCoordinates.x >= aabb.m_maxCoordinates.x;
isInside = isInside && m_maxCoordinates.y >= aabb.m_maxCoordinates.y;
isInside = isInside && m_maxCoordinates.z >= aabb.m_maxCoordinates.z;
isInside = isInside && m_minCoordinates.x() <= aabb.m_minCoordinates.x();
isInside = isInside && m_minCoordinates.y() <= aabb.m_minCoordinates.y();
isInside = isInside && m_minCoordinates.z() <= aabb.m_minCoordinates.z();
isInside = isInside && m_maxCoordinates.x() >= aabb.m_maxCoordinates.x();
isInside = isInside && m_maxCoordinates.y() >= aabb.m_maxCoordinates.y();
isInside = isInside && m_maxCoordinates.z() >= aabb.m_maxCoordinates.z();
return isInside;
}
// Create and return an AABB for a triangle
AABB AABB::createAABBForTriangle(const Vector3* trianglePoints) {
Vector3 minCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z);
Vector3 maxCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z);
if (trianglePoints[1].x < minCoords.x) minCoords.x = trianglePoints[1].x;
if (trianglePoints[1].y < minCoords.y) minCoords.y = trianglePoints[1].y;
if (trianglePoints[1].z < minCoords.z) minCoords.z = trianglePoints[1].z;
if (trianglePoints[2].x < minCoords.x) minCoords.x = trianglePoints[2].x;
if (trianglePoints[2].y < minCoords.y) minCoords.y = trianglePoints[2].y;
if (trianglePoints[2].z < minCoords.z) minCoords.z = trianglePoints[2].z;
if (trianglePoints[1].x > maxCoords.x) maxCoords.x = trianglePoints[1].x;
if (trianglePoints[1].y > maxCoords.y) maxCoords.y = trianglePoints[1].y;
if (trianglePoints[1].z > maxCoords.z) maxCoords.z = trianglePoints[1].z;
if (trianglePoints[2].x > maxCoords.x) maxCoords.x = trianglePoints[2].x;
if (trianglePoints[2].y > maxCoords.y) maxCoords.y = trianglePoints[2].y;
if (trianglePoints[2].z > maxCoords.z) maxCoords.z = trianglePoints[2].z;
AABB AABB::createAABBForTriangle(const vec3* trianglePoints) {
vec3 minCoords(trianglePoints[0].x(), trianglePoints[0].y(), trianglePoints[0].z());
vec3 maxCoords(trianglePoints[0].x(), trianglePoints[0].y(), trianglePoints[0].z());
if (trianglePoints[1].x() < minCoords.x()) {
minCoords.setX(trianglePoints[1].x());
}
if (trianglePoints[1].y() < minCoords.y()) {
minCoords.setY(trianglePoints[1].y());
}
if (trianglePoints[1].z() < minCoords.z()) {
minCoords.setZ(trianglePoints[1].z());
}
if (trianglePoints[2].x() < minCoords.x()) {
minCoords.setX(trianglePoints[2].x());
}
if (trianglePoints[2].y() < minCoords.y()) {
minCoords.setY(trianglePoints[2].y());
}
if (trianglePoints[2].z() < minCoords.z()) {
minCoords.setZ(trianglePoints[2].z());
}
if (trianglePoints[1].x() > maxCoords.x()) {
maxCoords.setX(trianglePoints[1].x());
}
if (trianglePoints[1].y() > maxCoords.y()) {
maxCoords.setY(trianglePoints[1].y());
}
if (trianglePoints[1].z() > maxCoords.z()) {
maxCoords.setZ(trianglePoints[1].z());
}
if (trianglePoints[2].x() > maxCoords.x()) {
maxCoords.setX(trianglePoints[2].x());
}
if (trianglePoints[2].y() > maxCoords.y()) {
maxCoords.setY(trianglePoints[2].y());
}
if (trianglePoints[2].z() > maxCoords.z()) {
maxCoords.setZ(trianglePoints[2].z());
}
return AABB(minCoords, maxCoords);
}
@ -100,33 +116,40 @@ AABB AABB::createAABBForTriangle(const Vector3* trianglePoints) {
/// This method use the line vs AABB raycasting technique described in
/// Real-time Collision Detection by Christer Ericson.
bool AABB::testRayIntersect(const Ray& ray) const {
const Vector3 point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
const Vector3 e = m_maxCoordinates - m_minCoordinates;
const Vector3 d = point2 - ray.point1;
const Vector3 m = ray.point1 + point2 - m_minCoordinates - m_maxCoordinates;
const vec3 point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
const vec3 e = m_maxCoordinates - m_minCoordinates;
const vec3 d = point2 - ray.point1;
const vec3 m = ray.point1 + point2 - m_minCoordinates - m_maxCoordinates;
// Test if the AABB face normals are separating axis
float adx = std::abs(d.x);
if (std::abs(m.x) > e.x + adx) return false;
float ady = std::abs(d.y);
if (std::abs(m.y) > e.y + ady) return false;
float adz = std::abs(d.z);
if (std::abs(m.z) > e.z + adz) return false;
float adx = std::abs(d.x());
if (std::abs(m.x()) > e.x() + adx) {
return false;
}
float ady = std::abs(d.y());
if (std::abs(m.y()) > e.y() + ady) {
return false;
}
float adz = std::abs(d.z());
if (std::abs(m.z()) > e.z() + adz) {
return false;
}
// Add in an epsilon term to counteract arithmetic errors when segment is
// (near) parallel to a coordinate axis (see text for detail)
const float epsilon = 0.00001;
adx += epsilon;
ady += epsilon;
adz += epsilon;
// Test if the cross products between face normals and ray direction are
// separating axis
if (std::abs(m.y * d.z - m.z * d.y) > e.y * adz + e.z * ady) return false;
if (std::abs(m.z * d.x - m.x * d.z) > e.x * adz + e.z * adx) return false;
if (std::abs(m.x * d.y - m.y * d.x) > e.x * ady + e.y * adx) return false;
if (std::abs(m.y() * d.z() - m.z() * d.y()) > e.y() * adz + e.z() * ady) {
return false;
}
if (std::abs(m.z() * d.x() - m.x() * d.z()) > e.x() * adz + e.z() * adx) {
return false;
}
if (std::abs(m.x() * d.y() - m.y() * d.x()) > e.x() * ady + e.y() * adx) {
return false;
}
// No separating axis has been found
return true;
}

80
ephysics/collision/shapes/AABB.h
View File

@ -25,10 +25,10 @@ class AABB {
// -------------------- Attributes -------------------- //
/// Minimum world coordinates of the AABB on the x,y and z axis
Vector3 m_minCoordinates;
vec3 m_minCoordinates;
/// Maximum world coordinates of the AABB on the x,y and z axis
Vector3 m_maxCoordinates;
vec3 m_maxCoordinates;
public :
@ -38,7 +38,7 @@ class AABB {
AABB();
/// Constructor
AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates);
AABB(const vec3& minCoordinates, const vec3& maxCoordinates);
/// Copy-constructor
AABB(const AABB& aabb);
@ -47,22 +47,22 @@ class AABB {
~AABB();
/// Return the center point
Vector3 getCenter() const;
vec3 getCenter() const;
/// Return the minimum coordinates of the AABB
const Vector3& getMin() const;
const vec3& getMin() const;
/// Set the minimum coordinates of the AABB
void setMin(const Vector3& min);
void setMin(const vec3& min);
/// Return the maximum coordinates of the AABB
const Vector3& getMax() const;
const vec3& getMax() const;
/// Set the maximum coordinates of the AABB
void setMax(const Vector3& max);
void setMax(const vec3& max);
/// Return the size of the AABB in the three dimension x, y and z
Vector3 getExtent() const;
vec3 getExtent() const;
/// Inflate each side of the AABB by a given size
void inflate(float dx, float dy, float dz);
@ -83,16 +83,16 @@ class AABB {
bool contains(const AABB& aabb) const;
/// Return true if a point is inside the AABB
bool contains(const Vector3& point) const;
bool contains(const vec3& point) const;
/// Return true if the AABB of a triangle int32_tersects the AABB
bool testCollisionTriangleAABB(const Vector3* trianglePoints) const;
bool testCollisionTriangleAABB(const vec3* trianglePoints) const;
/// Return true if the ray int32_tersects the AABB
bool testRayIntersect(const Ray& ray) const;
/// Create and return an AABB for a triangle
static AABB createAABBForTriangle(const Vector3* trianglePoints);
static AABB createAABBForTriangle(const vec3* trianglePoints);
/// Assignment operator
AABB& operator=(const AABB& aabb);
@ -103,79 +103,79 @@ class AABB {
};
// Return the center point of the AABB in world coordinates
inline Vector3 AABB::getCenter() const {
return (m_minCoordinates + m_maxCoordinates) * float(0.5);
inline vec3 AABB::getCenter() const {
return (m_minCoordinates + m_maxCoordinates) * 0.5f;
}
// Return the minimum coordinates of the AABB
inline const Vector3& AABB::getMin() const {
inline const vec3& AABB::getMin() const {
return m_minCoordinates;
}
// Set the minimum coordinates of the AABB
inline void AABB::setMin(const Vector3& min) {
inline void AABB::setMin(const vec3& min) {
m_minCoordinates = min;
}
// Return the maximum coordinates of the AABB
inline const Vector3& AABB::getMax() const {
inline const vec3& AABB::getMax() const {
return m_maxCoordinates;
}
// Set the maximum coordinates of the AABB
inline void AABB::setMax(const Vector3& max) {
inline void AABB::setMax(const vec3& max) {
m_maxCoordinates = max;
}
// Return the size of the AABB in the three dimension x, y and z
inline Vector3 AABB::getExtent() const {
inline vec3 AABB::getExtent() const {
return m_maxCoordinates - m_minCoordinates;
}
// Inflate each side of the AABB by a given size
inline void AABB::inflate(float dx, float dy, float dz) {
m_maxCoordinates += Vector3(dx, dy, dz);
m_minCoordinates -= Vector3(dx, dy, dz);
m_maxCoordinates += vec3(dx, dy, dz);
m_minCoordinates -= vec3(dx, dy, dz);
}
// Return true if the current AABB is overlapping with the AABB in argument.
/// Two AABBs overlap if they overlap in the three x, y and z axis at the same time
inline bool AABB::testCollision(const AABB& aabb) const {
if (m_maxCoordinates.x < aabb.m_minCoordinates.x ||
aabb.m_maxCoordinates.x < m_minCoordinates.x) return false;
if (m_maxCoordinates.y < aabb.m_minCoordinates.y ||
aabb.m_maxCoordinates.y < m_minCoordinates.y) return false;
if (m_maxCoordinates.z < aabb.m_minCoordinates.z||
aabb.m_maxCoordinates.z < m_minCoordinates.z) return false;
if (m_maxCoordinates.x() < aabb.m_minCoordinates.x() ||
aabb.m_maxCoordinates.x() < m_minCoordinates.x()) return false;
if (m_maxCoordinates.y() < aabb.m_minCoordinates.y() ||
aabb.m_maxCoordinates.y() < m_minCoordinates.y()) return false;
if (m_maxCoordinates.z() < aabb.m_minCoordinates.z()||
aabb.m_maxCoordinates.z() < m_minCoordinates.z()) return false;
return true;
}
// Return the volume of the AABB
inline float AABB::getVolume() const {
const Vector3 diff = m_maxCoordinates - m_minCoordinates;
return (diff.x * diff.y * diff.z);
const vec3 diff = m_maxCoordinates - m_minCoordinates;
return (diff.x() * diff.y() * diff.z());
}
// Return true if the AABB of a triangle int32_tersects the AABB
inline bool AABB::testCollisionTriangleAABB(const Vector3* trianglePoints) const {
inline bool AABB::testCollisionTriangleAABB(const vec3* trianglePoints) const {
if (min3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) > m_maxCoordinates.x) return false;
if (min3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) > m_maxCoordinates.y) return false;
if (min3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) > m_maxCoordinates.z) return false;
if (min3(trianglePoints[0].x(), trianglePoints[1].x(), trianglePoints[2].x()) > m_maxCoordinates.x()) return false;
if (min3(trianglePoints[0].y(), trianglePoints[1].y(), trianglePoints[2].y()) > m_maxCoordinates.y()) return false;
if (min3(trianglePoints[0].z(), trianglePoints[1].z(), trianglePoints[2].z()) > m_maxCoordinates.z()) return false;
if (max3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) < m_minCoordinates.x) return false;
if (max3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) < m_minCoordinates.y) return false;
if (max3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) < m_minCoordinates.z) return false;
if (max3(trianglePoints[0].x(), trianglePoints[1].x(), trianglePoints[2].x()) < m_minCoordinates.x()) return false;
if (max3(trianglePoints[0].y(), trianglePoints[1].y(), trianglePoints[2].y()) < m_minCoordinates.y()) return false;
if (max3(trianglePoints[0].z(), trianglePoints[1].z(), trianglePoints[2].z()) < m_minCoordinates.z()) return false;
return true;
}
// Return true if a point is inside the AABB
inline bool AABB::contains(const Vector3& point) const {
inline bool AABB::contains(const vec3& point) const {
return (point.x >= m_minCoordinates.x - MACHINE_EPSILON && point.x <= m_maxCoordinates.x + MACHINE_EPSILON &&
point.y >= m_minCoordinates.y - MACHINE_EPSILON && point.y <= m_maxCoordinates.y + MACHINE_EPSILON &&
point.z >= m_minCoordinates.z - MACHINE_EPSILON && point.z <= m_maxCoordinates.z + MACHINE_EPSILON);
return (point.x() >= m_minCoordinates.x() - MACHINE_EPSILON && point.x() <= m_maxCoordinates.x() + MACHINE_EPSILON &&
point.y() >= m_minCoordinates.y() - MACHINE_EPSILON && point.y() <= m_maxCoordinates.y() + MACHINE_EPSILON &&
point.z() >= m_minCoordinates.z() - MACHINE_EPSILON && point.z() <= m_maxCoordinates.z() + MACHINE_EPSILON);
}
// Assignment operator

42
ephysics/collision/shapes/BoxShape.cpp
View File

@ -18,11 +18,11 @@ using namespace reactphysics3d;
* @param extent The vector with the three extents of the box (in meters)
* @param margin The collision margin (in meters) around the collision shape
*/
BoxShape::BoxShape(const Vector3& extent, float margin)
: ConvexShape(BOX, margin), m_extent(extent - Vector3(margin, margin, margin)) {
assert(extent.x > float(0.0) && extent.x > margin);
assert(extent.y > float(0.0) && extent.y > margin);
assert(extent.z > float(0.0) && extent.z > margin);
BoxShape::BoxShape(const vec3& extent, float margin)
: ConvexShape(BOX, margin), m_extent(extent - vec3(margin, margin, margin)) {
assert(extent.x() > 0.0f && extent.x() > margin);
assert(extent.y() > 0.0f && extent.y() > margin);
assert(extent.z() > 0.0f && extent.z() > margin);
}
// Destructor
@ -36,13 +36,13 @@ BoxShape::~BoxShape() {
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
float factor = (float(1.0) / float(3.0)) * mass;
Vector3 realExtent = m_extent + Vector3(mMargin, mMargin, mMargin);
float xSquare = realExtent.x * realExtent.x;
float ySquare = realExtent.y * realExtent.y;
float zSquare = realExtent.z * realExtent.z;
tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0,
void BoxShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const {
float factor = (1.0f / float(3.0)) * mass;
vec3 realExtent = m_extent + vec3(m_margin, m_margin, m_margin);
float xSquare = realExtent.x() * realExtent.x();
float ySquare = realExtent.y() * realExtent.y();
float zSquare = realExtent.z() * realExtent.z();
tensor.setValue(factor * (ySquare + zSquare), 0.0, 0.0,
0.0, factor * (xSquare + zSquare), 0.0,
0.0, 0.0, factor * (xSquare + ySquare));
}
@ -50,11 +50,11 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
// Raycast method with feedback information
bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
Vector3 rayDirection = ray.point2 - ray.point1;
vec3 rayDirection = ray.point2 - ray.point1;
float tMin = DECIMAL_SMALLEST;
float tMax = DECIMAL_LARGEST;
Vector3 normalDirection(float(0), float(0), float(0));
Vector3 currentNormal;
vec3 normalDirection(float(0), float(0), float(0));
vec3 currentNormal;
// For each of the three slabs
for (int32_t i=0; i<3; i++) {
@ -68,12 +68,12 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro
else {
// Compute the int32_tersection of the ray with the near and far plane of the slab
float oneOverD = float(1.0) / rayDirection[i];
float oneOverD = 1.0f / rayDirection[i];
float t1 = (-m_extent[i] - ray.point1[i]) * oneOverD;
float t2 = (m_extent[i] - ray.point1[i]) * oneOverD;
currentNormal[0] = (i == 0) ? -m_extent[i] : float(0.0);
currentNormal[1] = (i == 1) ? -m_extent[i] : float(0.0);
currentNormal[2] = (i == 2) ? -m_extent[i] : float(0.0);
currentNormal[0] = (i == 0) ? -m_extent[i] : 0.0f;
currentNormal[1] = (i == 1) ? -m_extent[i] : 0.0f;
currentNormal[2] = (i == 2) ? -m_extent[i] : 0.0f;
// Swap t1 and t2 if need so that t1 is int32_tersection with near plane and
// t2 with far plane
@ -98,10 +98,10 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro
}
// If tMin is negative, we return no hit
if (tMin < float(0.0) || tMin > ray.maxFraction) return false;
if (tMin < 0.0f || tMin > ray.maxFraction) return false;
// The ray int32_tersects the three slabs, we compute the hit point
Vector3 localHitPoint = ray.point1 + tMin * rayDirection;
vec3 localHitPoint = ray.point1 + tMin * rayDirection;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;

44
ephysics/collision/shapes/BoxShape.h
View File

@ -36,7 +36,7 @@ class BoxShape : public ConvexShape {
// -------------------- Attributes -------------------- //
/// Extent sizes of the box in the x, y and z direction
Vector3 m_extent;
vec3 m_extent;
// -------------------- Methods -------------------- //
@ -47,11 +47,11 @@ class BoxShape : public ConvexShape {
BoxShape& operator=(const BoxShape& shape);
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
@ -64,36 +64,36 @@ class BoxShape : public ConvexShape {
// -------------------- Methods -------------------- //
/// Constructor
BoxShape(const Vector3& extent, float margin = OBJECT_MARGIN);
BoxShape(const vec3& extent, float margin = OBJECT_MARGIN);
/// Destructor
virtual ~BoxShape();
/// Return the extents of the box
Vector3 getExtent() const;
vec3 getExtent() const;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
virtual void setLocalScaling(const vec3& scaling);
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
virtual void getLocalBounds(vec3& min, vec3& max) const;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const;
};
// Return the extents of the box
/**
* @return The vector with the three extents of the box shape (in meters)
*/
inline Vector3 BoxShape::getExtent() const {
return m_extent + Vector3(mMargin, mMargin, mMargin);
inline vec3 BoxShape::getExtent() const {
return m_extent + vec3(m_margin, m_margin, m_margin);
}
// Set the scaling vector of the collision shape
inline void BoxShape::setLocalScaling(const Vector3& scaling) {
inline void BoxShape::setLocalScaling(const vec3& scaling) {
m_extent = (m_extent / mScaling) * scaling;
m_extent = (m_extent / m_scaling) * scaling;
CollisionShape::setLocalScaling(scaling);
}
@ -104,10 +104,10 @@ inline void BoxShape::setLocalScaling(const Vector3& scaling) {
* @param min The minimum bounds of the shape in local-space coordinates
* @param max The maximum bounds of the shape in local-space coordinates
*/
inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const {
inline void BoxShape::getLocalBounds(vec3& min, vec3& max) const {
// Maximum bounds
max = m_extent + Vector3(mMargin, mMargin, mMargin);
max = m_extent + vec3(m_margin, m_margin, m_margin);
// Minimum bounds
min = -max;
@ -119,19 +119,19 @@ inline size_t BoxShape::getSizeInBytes() const {
}
// Return a local support point in a given direction without the objec margin
inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
inline vec3 BoxShape::getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const {
return Vector3(direction.x < 0.0 ? -m_extent.x : m_extent.x,
direction.y < 0.0 ? -m_extent.y : m_extent.y,
direction.z < 0.0 ? -m_extent.z : m_extent.z);
return vec3(direction.x() < 0.0 ? -m_extent.x() : m_extent.x(),
direction.y() < 0.0 ? -m_extent.y() : m_extent.y(),
direction.z() < 0.0 ? -m_extent.z() : m_extent.z());
}
// Return true if a point is inside the collision shape
inline bool BoxShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
return (localPoint.x < m_extent[0] && localPoint.x > -m_extent[0] &&
localPoint.y < m_extent[1] && localPoint.y > -m_extent[1] &&
localPoint.z < m_extent[2] && localPoint.z > -m_extent[2]);
inline bool BoxShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const {
return (localPoint.x() < m_extent[0] && localPoint.x() > -m_extent[0] &&
localPoint.y() < m_extent[1] && localPoint.y() > -m_extent[1] &&
localPoint.z() < m_extent[2] && localPoint.z() > -m_extent[2]);
}
}

108
ephysics/collision/shapes/CapsuleShape.cpp
View File

@ -18,9 +18,9 @@ using namespace reactphysics3d;
* @param height The height of the capsule (in meters)
*/
CapsuleShape::CapsuleShape(float radius, float height)
: ConvexShape(CAPSULE, radius), mHalfHeight(height * float(0.5)) {
assert(radius > float(0.0));
assert(height > float(0.0));
: ConvexShape(CAPSULE, radius), m_halfHeight(height * 0.5f) {
assert(radius > 0.0f);
assert(height > 0.0f);
}
// Destructor
@ -34,38 +34,38 @@ CapsuleShape::~CapsuleShape() {
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
void CapsuleShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const {
// The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1
float height = mHalfHeight + mHalfHeight;
float radiusSquare = mMargin * mMargin;
float height = m_halfHeight + m_halfHeight;
float radiusSquare = m_margin * m_margin;
float heightSquare = height * height;
float radiusSquareDouble = radiusSquare + radiusSquare;
float factor1 = float(2.0) * mMargin / (float(4.0) * mMargin + float(3.0) * height);
float factor2 = float(3.0) * height / (float(4.0) * mMargin + float(3.0) * height);
float factor1 = float(2.0) * m_margin / (float(4.0) * m_margin + float(3.0) * height);
float factor2 = float(3.0) * height / (float(4.0) * m_margin + float(3.0) * height);
float sum1 = float(0.4) * radiusSquareDouble;
float sum2 = float(0.75) * height * mMargin + float(0.5) * heightSquare;
float sum2 = float(0.75) * height * m_margin + 0.5f * heightSquare;
float sum3 = float(0.25) * radiusSquare + float(1.0 / 12.0) * heightSquare;
float IxxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3;
float Iyy = factor1 * mass * sum1 + factor2 * mass * float(0.25) * radiusSquareDouble;
tensor.setAllValues(IxxAndzz, 0.0, 0.0,
tensor.setValue(IxxAndzz, 0.0, 0.0,
0.0, Iyy, 0.0,
0.0, 0.0, IxxAndzz);
}
// Return true if a point is inside the collision shape
bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
bool CapsuleShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const {
const float diffYCenterSphere1 = localPoint.y - mHalfHeight;
const float diffYCenterSphere2 = localPoint.y + mHalfHeight;
const float xSquare = localPoint.x * localPoint.x;
const float zSquare = localPoint.z * localPoint.z;
const float squareRadius = mMargin * mMargin;
const float diffYCenterSphere1 = localPoint.y() - m_halfHeight;
const float diffYCenterSphere2 = localPoint.y() + m_halfHeight;
const float xSquare = localPoint.x() * localPoint.x();
const float zSquare = localPoint.z() * localPoint.z();
const float squareRadius = m_margin * m_margin;
// Return true if the point is inside the cylinder or one of the two spheres of the capsule
return ((xSquare + zSquare) < squareRadius &&
localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) ||
localPoint.y() < m_halfHeight && localPoint.y() > -m_halfHeight) ||
(xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius ||
(xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius;
}
@ -73,13 +73,13 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
// Raycast method with feedback information
bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const Vector3 n = ray.point2 - ray.point1;
const vec3 n = ray.point2 - ray.point1;
const float epsilon = float(0.01);
Vector3 p(float(0), -mHalfHeight, float(0));
Vector3 q(float(0), mHalfHeight, float(0));
Vector3 d = q - p;
Vector3 m = ray.point1 - p;
vec3 p(float(0), -m_halfHeight, float(0));
vec3 q(float(0), m_halfHeight, float(0));
vec3 d = q - p;
vec3 m = ray.point1 - p;
float t;
float mDotD = m.dot(d);
@ -87,38 +87,38 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
float dDotD = d.dot(d);
// Test if the segment is outside the cylinder
float vec1DotD = (ray.point1 - Vector3(float(0.0), -mHalfHeight - mMargin, float(0.0))).dot(d);
if (vec1DotD < float(0.0) && vec1DotD + nDotD < float(0.0)) return false;
float ddotDExtraCaps = float(2.0) * mMargin * d.y;
float vec1DotD = (ray.point1 - vec3(0.0f, -m_halfHeight - m_margin, float(0.0))).dot(d);
if (vec1DotD < 0.0f && vec1DotD + nDotD < float(0.0)) return false;
float ddotDExtraCaps = float(2.0) * m_margin * d.y();
if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) return false;
float nDotN = n.dot(n);
float mDotN = m.dot(n);
float a = dDotD * nDotN - nDotD * nDotD;
float k = m.dot(m) - mMargin * mMargin;
float k = m.dot(m) - m_margin * m_margin;
float c = dDotD * k - mDotD * mDotD;
// If the ray is parallel to the capsule axis
if (std::abs(a) < epsilon) {
// If the origin is outside the surface of the capusle's cylinder, we return no hit
if (c > float(0.0)) return false;
if (c > 0.0f) return false;
// Here we know that the segment int32_tersect an endcap of the capsule
// If the ray int32_tersects with the "p" endcap of the capsule
if (mDotD < float(0.0)) {
if (mDotD < 0.0f) {
// Check int32_tersection between the ray and the "p" sphere endcap of the capsule
Vector3 hitLocalPoint;
vec3 hitLocalPoint;
float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint;
Vector3 normalDirection = hitLocalPoint - p;
vec3 normalDirection = hitLocalPoint - p;
raycastInfo.worldNormal = normalDirection;
return true;
@ -129,14 +129,14 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
else if (mDotD > dDotD) { // If the ray int32_tersects with the "q" endcap of the cylinder
// Check int32_tersection between the ray and the "q" sphere endcap of the capsule
Vector3 hitLocalPoint;
vec3 hitLocalPoint;
float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint;
Vector3 normalDirection = hitLocalPoint - q;
vec3 normalDirection = hitLocalPoint - q;
raycastInfo.worldNormal = normalDirection;
return true;
@ -152,24 +152,24 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
float discriminant = b * b - a * c;
// If the discriminant is negative, no real roots and therfore, no hit
if (discriminant < float(0.0)) return false;
if (discriminant < 0.0f) return false;
// Compute the smallest root (first int32_tersection along the ray)
float t0 = t = (-b - std::sqrt(discriminant)) / a;
// If the int32_tersection is outside the finite cylinder of the capsule on "p" endcap side
float value = mDotD + t * nDotD;
if (value < float(0.0)) {
if (value < 0.0f) {
// Check int32_tersection between the ray and the "p" sphere endcap of the capsule
Vector3 hitLocalPoint;
vec3 hitLocalPoint;
float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint;
Vector3 normalDirection = hitLocalPoint - p;
vec3 normalDirection = hitLocalPoint - p;
raycastInfo.worldNormal = normalDirection;
return true;
@ -180,14 +180,14 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
else if (value > dDotD) { // If the int32_tersection is outside the finite cylinder on the "q" side
// Check int32_tersection between the ray and the "q" sphere endcap of the capsule
Vector3 hitLocalPoint;
vec3 hitLocalPoint;
float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint;
Vector3 normalDirection = hitLocalPoint - q;
vec3 normalDirection = hitLocalPoint - q;
raycastInfo.worldNormal = normalDirection;
return true;
@ -200,52 +200,52 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
// If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < float(0.0) || t > ray.maxFraction) return false;
if (t < 0.0f || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n;
vec3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
Vector3 v = localHitPoint - p;
Vector3 w = (v.dot(d) / d.lengthSquare()) * d;
Vector3 normalDirection = (localHitPoint - (p + w)).getUnit();
vec3 v = localHitPoint - p;
vec3 w = (v.dot(d) / d.length2()) * d;
vec3 normalDirection = (localHitPoint - (p + w)).safeNormalized();
raycastInfo.worldNormal = normalDirection;
return true;
}
// Raycasting method between a ray one of the two spheres end cap of the capsule
bool CapsuleShape::raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,
const Vector3& sphereCenter, float maxFraction,
Vector3& hitLocalPoint, float& hitFraction) const {
bool CapsuleShape::raycastWithSphereEndCap(const vec3& point1, const vec3& point2,
const vec3& sphereCenter, float maxFraction,
vec3& hitLocalPoint, float& hitFraction) const {
const Vector3 m = point1 - sphereCenter;
float c = m.dot(m) - mMargin * mMargin;
const vec3 m = point1 - sphereCenter;
float c = m.dot(m) - m_margin * m_margin;
// If the origin of the ray is inside the sphere, we return no int32_tersection
if (c < float(0.0)) return false;
if (c < 0.0f) return false;
const Vector3 rayDirection = point2 - point1;
const vec3 rayDirection = point2 - point1;
float b = m.dot(rayDirection);
// If the origin of the ray is outside the sphere and the ray
// is pointing away from the sphere, there is no int32_tersection
if (b > float(0.0)) return false;
if (b > 0.0f) return false;
float raySquareLength = rayDirection.lengthSquare();
float raySquareLength = rayDirection.length2();
// Compute the discriminant of the quadratic equation
float discriminant = b * b - raySquareLength * c;
// If the discriminant is negative or the ray length is very small, there is no int32_tersection
if (discriminant < float(0.0) || raySquareLength < MACHINE_EPSILON) return false;
if (discriminant < 0.0f || raySquareLength < MACHINE_EPSILON) return false;
// Compute the solution "t" closest to the origin
float t = -b - std::sqrt(discriminant);
assert(t >= float(0.0));
assert(t >= 0.0f);
// If the hit point is withing the segment ray fraction
if (t < maxFraction * raySquareLength) {

52
ephysics/collision/shapes/CapsuleShape.h
View File

@ -30,7 +30,7 @@ class CapsuleShape : public ConvexShape {
// -------------------- Attributes -------------------- //
/// Half height of the capsule (height = distance between the centers of the two spheres)
float mHalfHeight;
float m_halfHeight;
// -------------------- Methods -------------------- //
@ -41,19 +41,19 @@ class CapsuleShape : public ConvexShape {
CapsuleShape& operator=(const CapsuleShape& shape);
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
/// Raycasting method between a ray one of the two spheres end cap of the capsule
bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,
const Vector3& sphereCenter, float maxFraction,
Vector3& hitLocalPoint, float& hitFraction) const;
bool raycastWithSphereEndCap(const vec3& point1, const vec3& point2,
const vec3& sphereCenter, float maxFraction,
vec3& hitLocalPoint, float& hitFraction) const;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
@ -75,13 +75,13 @@ class CapsuleShape : public ConvexShape {
float getHeight() const;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
virtual void setLocalScaling(const vec3& scaling);
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
virtual void getLocalBounds(vec3& min, vec3& max) const;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const;
};
// Get the radius of the capsule
@ -89,7 +89,7 @@ class CapsuleShape : public ConvexShape {
* @return The radius of the capsule shape (in meters)
*/
inline float CapsuleShape::getRadius() const {
return mMargin;
return m_margin;
}
// Return the height of the capsule
@ -97,14 +97,14 @@ inline float CapsuleShape::getRadius() const {
* @return The height of the capsule shape (in meters)
*/
inline float CapsuleShape::getHeight() const {
return mHalfHeight + mHalfHeight;
return m_halfHeight + m_halfHeight;
}
// Set the scaling vector of the collision shape
inline void CapsuleShape::setLocalScaling(const Vector3& scaling) {
inline void CapsuleShape::setLocalScaling(const vec3& scaling) {
mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y;
mMargin = (mMargin / mScaling.x) * scaling.x;
m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y();
m_margin = (m_margin / m_scaling.x()) * scaling.x();
CollisionShape::setLocalScaling(scaling);
}
@ -120,17 +120,17 @@ inline size_t CapsuleShape::getSizeInBytes() const {
* @param min The minimum bounds of the shape in local-space coordinates
* @param max The maximum bounds of the shape in local-space coordinates
*/
inline void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const {
inline void CapsuleShape::getLocalBounds(vec3& min, vec3& max) const {
// Maximum bounds
max.x = mMargin;
max.y = mHalfHeight + mMargin;
max.z = mMargin;
max.setX(m_margin);
max.setY(m_halfHeight + m_margin);
max.setZ(m_margin);
// Minimum bounds
min.x = -mMargin;
min.y = -max.y;
min.z = min.x;
min.setX(-m_margin);
min.setY(-max.y());
min.setZ(min.x());
}
// Return a local support point in a given direction without the object margin.
@ -140,21 +140,21 @@ inline void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const {
/// Therefore, in this method, we compute the support points of both top and bottom spheres of
/// the capsule and return the point with the maximum dot product with the direction vector. Note
/// that the object margin is implicitly the radius and height of the capsule.
inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
inline vec3 CapsuleShape::getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const {
// Support point top sphere
float dotProductTop = mHalfHeight * direction.y;
float dotProductTop = m_halfHeight * direction.y();
// Support point bottom sphere
float dotProductBottom = -mHalfHeight * direction.y;
float dotProductBottom = -m_halfHeight * direction.y();
// Return the point with the maximum dot product
if (dotProductTop > dotProductBottom) {
return Vector3(0, mHalfHeight, 0);
return vec3(0, m_halfHeight, 0);
}
else {
return Vector3(0, -mHalfHeight, 0);
return vec3(0, -m_halfHeight, 0);
}
}

20
ephysics/collision/shapes/CollisionShape.cpp
View File

@ -13,7 +13,7 @@
using namespace reactphysics3d;
// Constructor
CollisionShape::CollisionShape(CollisionShapeType type) : m_type(type), mScaling(1.0, 1.0, 1.0) {
CollisionShape::CollisionShape(CollisionShapeType type) : m_type(type), m_scaling(1.0, 1.0, 1.0) {
}
@ -26,29 +26,29 @@ CollisionShape::~CollisionShape() {
/**
* @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape
* computed in world-space coordinates
* @param transform Transform used to compute the AABB of the collision shape
* @param transform etk::Transform3D used to compute the AABB of the collision shape
*/
void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const {
void CollisionShape::computeAABB(AABB& aabb, const etk::Transform3D& transform) const {
PROFILE("CollisionShape::computeAABB()");
// Get the local bounds in x,y and z direction
Vector3 minBounds;
Vector3 maxBounds;
vec3 minBounds;
vec3 maxBounds;
getLocalBounds(minBounds, maxBounds);
// Rotate the local bounds according to the orientation of the body
Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsoluteMatrix();
Vector3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds),
etk::Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsolute();
vec3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds),
worldAxis.getColumn(1).dot(minBounds),
worldAxis.getColumn(2).dot(minBounds));
Vector3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds),
vec3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds),
worldAxis.getColumn(1).dot(maxBounds),
worldAxis.getColumn(2).dot(maxBounds));
// Compute the minimum and maximum coordinates of the rotated extents
Vector3 minCoordinates = transform.getPosition() + worldMinBounds;
Vector3 maxCoordinates = transform.getPosition() + worldMaxBounds;
vec3 minCoordinates = transform.getPosition() + worldMinBounds;
vec3 maxCoordinates = transform.getPosition() + worldMaxBounds;
// Update the AABB with the new minimum and maximum coordinates
aabb.setMin(minCoordinates);

26
ephysics/collision/shapes/CollisionShape.h
View File

@ -8,8 +8,8 @@
// Libraries
#include <cassert>
#include <typeinfo>
#include <ephysics/mathematics/Vector3.h>
#include <ephysics/mathematics/Matrix3x3.h>
#include <etk/math/Vector3D.hpp>
#include <etk/math/Matrix3x3.hpp>
#include <ephysics/mathematics/Ray.h>
#include <ephysics/collision/shapes/AABB.h>
#include <ephysics/collision/RaycastInfo.h>
@ -42,7 +42,7 @@ class CollisionShape {
CollisionShapeType m_type;
/// Scaling vector of the collision shape
Vector3 mScaling;
vec3 m_scaling;
// -------------------- Methods -------------------- //
@ -53,7 +53,7 @@ class CollisionShape {
CollisionShape& operator=(const CollisionShape& shape);
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0;
virtual bool testPointInside(const vec3& worldPoint, ProxyShape* proxyShape) const=0;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const=0;
@ -78,19 +78,19 @@ class CollisionShape {
virtual bool isConvex() const=0;
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const=0;
virtual void getLocalBounds(vec3& min, vec3& max) const=0;
/// Return the scaling vector of the collision shape
Vector3 getScaling() const;
vec3 getScaling() const;
/// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
virtual void setLocalScaling(const vec3& scaling);
/// Return the local inertia tensor of the collision shapes
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const=0;
virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const=0;
/// Compute the world-space AABB of the collision shape given a transform
virtual void computeAABB(AABB& aabb, const Transform& transform) const;
virtual void computeAABB(AABB& aabb, const etk::Transform3D& transform) const;
/// Return true if the collision shape type is a convex shape
static bool isConvex(CollisionShapeType shapeType);
@ -119,13 +119,13 @@ inline bool CollisionShape::isConvex(CollisionShapeType shapeType) {
}
// Return the scaling vector of the collision shape
inline Vector3 CollisionShape::getScaling() const {
return mScaling;
inline vec3 CollisionShape::getScaling() const {
return m_scaling;
}
// Set the scaling vector of the collision shape
inline void CollisionShape::setLocalScaling(const Vector3& scaling) {
mScaling = scaling;
inline void CollisionShape::setLocalScaling(const vec3& scaling) {
m_scaling = scaling;
}
// Return the maximum number of contact manifolds allowed in an overlapping

32
ephysics/collision/shapes/ConcaveMeshShape.cpp
View File

@ -40,7 +40,7 @@ void ConcaveMeshShape::initBVHTree() {
// For each triangle of the concave mesh
for (uint32_t triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) {
void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
Vector3 trianglePoints[3];
vec3 trianglePoints[3];
// For each vertex of the triangle
for (int32_t k=0; k < 3; k++) {
// Get the index of the current vertex in the triangle
@ -55,14 +55,14 @@ void ConcaveMeshShape::initBVHTree() {
// Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
trianglePoints[k][0] = float(vertices[0]) * mScaling.x;
trianglePoints[k][1] = float(vertices[1]) * mScaling.y;
trianglePoints[k][2] = float(vertices[2]) * mScaling.z;
trianglePoints[k][0] = float(vertices[0]) * m_scaling.x();
trianglePoints[k][1] = float(vertices[1]) * m_scaling.y();
trianglePoints[k][2] = float(vertices[2]) * m_scaling.z();
} else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
trianglePoints[k][0] = float(vertices[0]) * mScaling.x;
trianglePoints[k][1] = float(vertices[1]) * mScaling.y;
trianglePoints[k][2] = float(vertices[2]) * mScaling.z;
trianglePoints[k][0] = float(vertices[0]) * m_scaling.x();
trianglePoints[k][1] = float(vertices[1]) * m_scaling.y();
trianglePoints[k][2] = float(vertices[2]) * m_scaling.z();
} else {
assert(false);
}
@ -78,7 +78,7 @@ void ConcaveMeshShape::initBVHTree() {
// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
// given the start vertex index pointer of the triangle
void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32_t subPart, int32_t triangleIndex, Vector3* outTriangleVertices) const {
void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32_t subPart, int32_t triangleIndex, vec3* outTriangleVertices) const {
// Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(subPart);
if (triangleVertexArray == nullptr) {
@ -106,14 +106,14 @@ void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32_t subPart, int3
// Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
outTriangleVertices[k][0] = float(vertices[0]) * mScaling.x;
outTriangleVertices[k][1] = float(vertices[1]) * mScaling.y;
outTriangleVertices[k][2] = float(vertices[2]) * mScaling.z;
outTriangleVertices[k][0] = float(vertices[0]) * m_scaling.x();
outTriangleVertices[k][1] = float(vertices[1]) * m_scaling.y();
outTriangleVertices[k][2] = float(vertices[2]) * m_scaling.z();
} else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
outTriangleVertices[k][0] = float(vertices[0]) * mScaling.x;
outTriangleVertices[k][1] = float(vertices[1]) * mScaling.y;
outTriangleVertices[k][2] = float(vertices[2]) * mScaling.z;
outTriangleVertices[k][0] = float(vertices[0]) * m_scaling.x();
outTriangleVertices[k][1] = float(vertices[1]) * m_scaling.y();
outTriangleVertices[k][2] = float(vertices[2]) * m_scaling.z();
} else {
assert(false);
}
@ -158,7 +158,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
// Get the node data (triangle index and mesh subpart index)
int32_t* data = m_dynamicAABBTree.getNodeDataInt(*it);
// Get the triangle vertices for this node from the concave mesh shape
Vector3 trianglePoints[3];
vec3 trianglePoints[3];
m_concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
// Create a triangle collision shape
float margin = m_concaveMeshShape.getTriangleMargin();
@ -169,7 +169,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
bool isTriangleHit = triangleShape.raycast(m_ray, raycastInfo, m_proxyShape);
// If the ray hit the collision shape
if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) {
assert(raycastInfo.hitFraction >= float(0.0));
assert(raycastInfo.hitFraction >= 0.0f);
m_raycastInfo.body = raycastInfo.body;
m_raycastInfo.proxyShape = raycastInfo.proxyShape;
m_raycastInfo.hitFraction = raycastInfo.hitFraction;

18
ephysics/collision/shapes/ConcaveMeshShape.h
View File

@ -117,7 +117,7 @@ class ConcaveMeshShape : public ConcaveShape {
/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
/// given the start vertex index pointer of the triangle.
void getTriangleVerticesWithIndexPointer(int32_t subPart, int32_t triangleIndex,
Vector3* outTriangleVertices) const;
vec3* outTriangleVertices) const;
public:
@ -128,13 +128,13 @@ class ConcaveMeshShape : public ConcaveShape {
~ConcaveMeshShape();
/// Return the local bounds of the shape in x, y and z directions.
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
virtual void getLocalBounds(vec3& min, vec3& max) const;
/// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
virtual void setLocalScaling(const vec3& scaling);
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const;
/// Use a callback method on all triangles of the concave shape inside a given AABB
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const;
@ -156,7 +156,7 @@ inline size_t ConcaveMeshShape::getSizeInBytes() const {
* @param min The minimum bounds of the shape in local-space coordinates
* @param max The maximum bounds of the shape in local-space coordinates
*/
inline void ConcaveMeshShape::getLocalBounds(Vector3& min, Vector3& max) const {
inline void ConcaveMeshShape::getLocalBounds(vec3& min, vec3& max) const {
// Get the AABB of the whole tree
AABB treeAABB = m_dynamicAABBTree.getRootAABB();
@ -166,7 +166,7 @@ inline void ConcaveMeshShape::getLocalBounds(Vector3& min, Vector3& max) const {
}
// Set the local scaling vector of the collision shape
inline void ConcaveMeshShape::setLocalScaling(const Vector3& scaling) {
inline void ConcaveMeshShape::setLocalScaling(const vec3& scaling) {
CollisionShape::setLocalScaling(scaling);
@ -183,13 +183,13 @@ inline void ConcaveMeshShape::setLocalScaling(const Vector3& scaling) {
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
inline void ConcaveMeshShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const {
// Default inertia tensor
// Note that this is not very realistic for a concave triangle mesh.
// However, in most cases, it will only be used static bodies and therefore,
// the inertia tensor is not used.
tensor.setAllValues(mass, 0, 0,
tensor.setValue(mass, 0, 0,
0, mass, 0,
0, 0, mass);
}
@ -202,7 +202,7 @@ inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int32_t nod
int32_t* data = m_dynamicAABBTree.getNodeDataInt(nodeId);
// Get the triangle vertices for this node from the concave mesh shape
Vector3 trianglePoints[3];
vec3 trianglePoints[3];
m_concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
// Call the callback to test narrow-phase collision with this triangle

6
ephysics/collision/shapes/ConcaveShape.h
View File

@ -23,7 +23,7 @@ class TriangleCallback {
virtual ~TriangleCallback() = default;
/// Report a triangle
virtual void testTriangle(const Vector3* trianglePoints)=0;
virtual void testTriangle(const vec3* trianglePoints)=0;
};
@ -57,7 +57,7 @@ class ConcaveShape : public CollisionShape {
ConcaveShape& operator=(const ConcaveShape& shape);
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const;
public :
@ -102,7 +102,7 @@ inline bool ConcaveShape::isConvex() const {
}
// Return true if a point is inside the collision shape
inline bool ConcaveShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
inline bool ConcaveShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const {
return false;
}

88
ephysics/collision/shapes/ConeShape.cpp
View File

@ -19,9 +19,9 @@ using namespace reactphysics3d;
* @param margin Collision margin (in meters) around the collision shape
*/
ConeShape::ConeShape(float radius, float height, float margin)
: ConvexShape(CONE, margin), mRadius(radius), mHalfHeight(height * float(0.5)) {
assert(mRadius > float(0.0));
assert(mHalfHeight > float(0.0));
: ConvexShape(CONE, margin), mRadius(radius), m_halfHeight(height * 0.5f) {
assert(mRadius > 0.0f);
assert(m_halfHeight > 0.0f);
// Compute the sine of the semi-angle at the apex point
mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height));
@ -33,24 +33,24 @@ ConeShape::~ConeShape() {
}
// Return a local support point in a given direction without the object margin
Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
vec3 ConeShape::getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const {
const Vector3& v = direction;
const vec3& v = direction;
float sinThetaTimesLengthV = mSinTheta * v.length();
Vector3 supportPoint;
vec3 supportPoint;
if (v.y > sinThetaTimesLengthV) {
supportPoint = Vector3(0.0, mHalfHeight, 0.0);
if (v.y() > sinThetaTimesLengthV) {
supportPoint = vec3(0.0, m_halfHeight, 0.0);
}
else {
float projectedLength = sqrt(v.x * v.x + v.z * v.z);
float projectedLength = sqrt(v.x() * v.x() + v.z() * v.z());
if (projectedLength > MACHINE_EPSILON) {
float d = mRadius / projectedLength;
supportPoint = Vector3(v.x * d, -mHalfHeight, v.z * d);
supportPoint = vec3(v.x() * d, -m_halfHeight, v.z() * d);
}
else {
supportPoint = Vector3(0.0, -mHalfHeight, 0.0);
supportPoint = vec3(0.0, -m_halfHeight, 0.0);
}
}
@ -63,33 +63,33 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
// http://www.geometrictools.com/Documentation/IntersectionLineCone.pdf
bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const Vector3 r = ray.point2 - ray.point1;
const vec3 r = ray.point2 - ray.point1;
const float epsilon = float(0.00001);
Vector3 V(0, mHalfHeight, 0);
Vector3 centerBase(0, -mHalfHeight, 0);
Vector3 axis(0, float(-1.0), 0);
float heightSquare = float(4.0) * mHalfHeight * mHalfHeight;
vec3 V(0, m_halfHeight, 0);
vec3 centerBase(0, -m_halfHeight, 0);
vec3 axis(0, float(-1.0), 0);
float heightSquare = float(4.0) * m_halfHeight * m_halfHeight;
float cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius);
float factor = float(1.0) - cosThetaSquare;
Vector3 delta = ray.point1 - V;
float c0 = -cosThetaSquare * delta.x * delta.x + factor * delta.y * delta.y -
cosThetaSquare * delta.z * delta.z;
float c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z;
float c2 = -cosThetaSquare * r.x * r.x + factor * r.y * r.y - cosThetaSquare * r.z * r.z;
float factor = 1.0f - cosThetaSquare;
vec3 delta = ray.point1 - V;
float c0 = -cosThetaSquare * delta.x() * delta.x() + factor * delta.y() * delta.y() -
cosThetaSquare * delta.z() * delta.z();
float c1 = -cosThetaSquare * delta.x() * r.x() + factor * delta.y() * r.y() - cosThetaSquare * delta.z() * r.z();
float c2 = -cosThetaSquare * r.x() * r.x() + factor * r.y() * r.y() - cosThetaSquare * r.z() * r.z();
float tHit[] = {float(-1.0), float(-1.0), float(-1.0)};
Vector3 localHitPoint[3];
Vector3 localNormal[3];
vec3 localHitPoint[3];
vec3 localNormal[3];
// If c2 is different from zero
if (std::abs(c2) > MACHINE_EPSILON) {
float gamma = c1 * c1 - c0 * c2;
// If there is no real roots in the quadratic equation
if (gamma < float(0.0)) {
if (gamma < 0.0f) {
return false;
}
else if (gamma > float(0.0)) { // The equation has two real roots
else if (gamma > 0.0f) { // The equation has two real roots
// Compute two int32_tersections
float sqrRoot = std::sqrt(gamma);
@ -124,31 +124,31 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
localHitPoint[1] = ray.point1 + tHit[1] * r;
// Only keep hit points in one side of the double cone (the cone we are int32_terested in)
if (axis.dot(localHitPoint[0] - V) < float(0.0)) {
if (axis.dot(localHitPoint[0] - V) < 0.0f) {
tHit[0] = float(-1.0);
}
if (axis.dot(localHitPoint[1] - V) < float(0.0)) {
if (axis.dot(localHitPoint[1] - V) < 0.0f) {
tHit[1] = float(-1.0);
}
// Only keep hit points that are within the correct height of the cone
if (localHitPoint[0].y < float(-mHalfHeight)) {
if (localHitPoint[0].y() < float(-m_halfHeight)) {
tHit[0] = float(-1.0);
}
if (localHitPoint[1].y < float(-mHalfHeight)) {
if (localHitPoint[1].y() < float(-m_halfHeight)) {
tHit[1] = float(-1.0);
}
// If the ray is in direction of the base plane of the cone
if (r.y > epsilon) {
if (r.y() > epsilon) {
// Compute the int32_tersection with the base plane of the cone
tHit[2] = (-ray.point1.y - mHalfHeight) / (r.y);
tHit[2] = (-ray.point1.y() - m_halfHeight) / (r.y());
// Only keep this int32_tersection if it is inside the cone radius
localHitPoint[2] = ray.point1 + tHit[2] * r;
if ((localHitPoint[2] - centerBase).lengthSquare() > mRadius * mRadius) {
if ((localHitPoint[2] - centerBase).length2() > mRadius * mRadius) {
tHit[2] = float(-1.0);
}
@ -160,7 +160,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
int32_t hitIndex = -1;
float t = DECIMAL_LARGEST;
for (int32_t i=0; i<3; i++) {
if (tHit[i] < float(0.0)) continue;
if (tHit[i] < 0.0f) continue;
if (tHit[i] < t) {
hitIndex = i;
t = tHit[hitIndex];
@ -172,17 +172,17 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
// Compute the normal direction for hit against side of the cone
if (hitIndex != 2) {
float h = float(2.0) * mHalfHeight;
float value1 = (localHitPoint[hitIndex].x * localHitPoint[hitIndex].x +
localHitPoint[hitIndex].z * localHitPoint[hitIndex].z);
float h = float(2.0) * m_halfHeight;
float value1 = (localHitPoint[hitIndex].x() * localHitPoint[hitIndex].x() +
localHitPoint[hitIndex].z() * localHitPoint[hitIndex].z());
float rOverH = mRadius / h;
float value2 = float(1.0) + rOverH * rOverH;
float factor = float(1.0) / std::sqrt(value1 * value2);
float x = localHitPoint[hitIndex].x * factor;
float z = localHitPoint[hitIndex].z * factor;
localNormal[hitIndex].x = x;
localNormal[hitIndex].y = std::sqrt(x * x + z * z) * rOverH;
localNormal[hitIndex].z = z;
float value2 = 1.0f + rOverH * rOverH;
float factor = 1.0f / std::sqrt(value1 * value2);
float x = localHitPoint[hitIndex].x() * factor;
float z = localHitPoint[hitIndex].z() * factor;
localNormal[hitIndex].setX(x);
localNormal[hitIndex].setY(std::sqrt(x * x + z * z) * rOverH);
localNormal[hitIndex].setZ(z);
}
raycastInfo.body = proxyShape->getBody();

50
ephysics/collision/shapes/ConeShape.h
View File

@ -38,7 +38,7 @@ class ConeShape : public ConvexShape {
float mRadius;
/// Half height of the cone
float mHalfHeight;
float m_halfHeight;
/// sine of the semi angle at the apex point
float mSinTheta;
@ -52,11 +52,11 @@ class ConeShape : public ConvexShape {
ConeShape& operator=(const ConeShape& shape);
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
@ -81,13 +81,13 @@ class ConeShape : public ConvexShape {
float getHeight() const;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
virtual void setLocalScaling(const vec3& scaling);
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
virtual void getLocalBounds(vec3& min, vec3& max) const;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const;
};
// Return the radius
@ -103,14 +103,14 @@ inline float ConeShape::getRadius() const {
* @return Height of the cone (in meters)
*/
inline float ConeShape::getHeight() const {
return float(2.0) * mHalfHeight;
return float(2.0) * m_halfHeight;
}
// Set the scaling vector of the collision shape
inline void ConeShape::setLocalScaling(const Vector3& scaling) {
inline void ConeShape::setLocalScaling(const vec3& scaling) {
mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y;
mRadius = (mRadius / mScaling.x) * scaling.x;
m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y();
mRadius = (mRadius / m_scaling.x()) * scaling.x();
CollisionShape::setLocalScaling(scaling);
}
@ -125,17 +125,17 @@ inline size_t ConeShape::getSizeInBytes() const {
* @param min The minimum bounds of the shape in local-space coordinates
* @param max The maximum bounds of the shape in local-space coordinates
*/
inline void ConeShape::getLocalBounds(Vector3& min, Vector3& max) const {
inline void ConeShape::getLocalBounds(vec3& min, vec3& max) const {
// Maximum bounds
max.x = mRadius + mMargin;
max.y = mHalfHeight + mMargin;
max.z = max.x;
max.setX(mRadius + m_margin);
max.setY(m_halfHeight + m_margin);
max.setZ(max.x());
// Minimum bounds
min.x = -max.x;
min.y = -max.y;
min.z = min.x;
min.setX(-max.x());
min.setY(-max.y());
min.setZ(min.x());
}
// Return the local inertia tensor of the collision shape
@ -144,20 +144,20 @@ inline void ConeShape::getLocalBounds(Vector3& min, Vector3& max) const {
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
inline void ConeShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const {
float rSquare = mRadius * mRadius;
float diagXZ = float(0.15) * mass * (rSquare + mHalfHeight);
tensor.setAllValues(diagXZ, 0.0, 0.0,
float diagXZ = float(0.15) * mass * (rSquare + m_halfHeight);
tensor.setValue(diagXZ, 0.0, 0.0,
0.0, float(0.3) * mass * rSquare,
0.0, 0.0, 0.0, diagXZ);
}
// Return true if a point is inside the collision shape
inline bool ConeShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
const float radiusHeight = mRadius * (-localPoint.y + mHalfHeight) /
(mHalfHeight * float(2.0));
return (localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) &&
(localPoint.x * localPoint.x + localPoint.z * localPoint.z < radiusHeight *radiusHeight);
inline bool ConeShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const {
const float radiusHeight = mRadius * (-localPoint.y() + m_halfHeight) /
(m_halfHeight * float(2.0));
return (localPoint.y() < m_halfHeight && localPoint.y() > -m_halfHeight) &&
(localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z() < radiusHeight *radiusHeight);
}
}

58
ephysics/collision/shapes/ConvexMeshShape.cpp
View File

@ -30,7 +30,7 @@ ConvexMeshShape::ConvexMeshShape(const float* arrayVertices, uint32_t nbVertices
// Copy all the vertices int32_to the int32_ternal array
for (uint32_t i=0; i<m_numberVertices; i++) {
const float* newPoint = (const float*) vertexPointer;
m_vertices.push_back(Vector3(newPoint[0], newPoint[1], newPoint[2]));
m_vertices.push_back(vec3(newPoint[0], newPoint[1], newPoint[2]));
vertexPointer += stride;
}
@ -63,15 +63,15 @@ ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool
if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + v * vertexStride);
Vector3 vertex(vertices[0], vertices[1], vertices[2] );
vertex = vertex * mScaling;
vec3 vertex(vertices[0], vertices[1], vertices[2] );
vertex = vertex * m_scaling;
m_vertices.push_back(vertex);
}
else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + v * vertexStride);
Vector3 vertex(vertices[0], vertices[1], vertices[2] );
vertex = vertex * mScaling;
vec3 vertex(vertices[0], vertices[1], vertices[2] );
vertex = vertex * m_scaling;
m_vertices.push_back(vertex);
}
}
@ -134,7 +134,7 @@ ConvexMeshShape::~ConvexMeshShape() {
/// it as a start in a hill-climbing (local search) process to find the new support vertex which
/// will be in most of the cases very close to the previous one. Using hill-climbing, this method
/// runs in almost constant time.
Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
vec3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const {
assert(m_numberVertices == m_vertices.size());
@ -184,7 +184,7 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct
*((int32_t*)(*cachedCollisionData)) = maxVertex;
// Return the support vertex
return m_vertices[maxVertex] * mScaling;
return m_vertices[maxVertex] * m_scaling;
}
else { // If the edges information is not used
@ -204,10 +204,10 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct
}
}
assert(maxDotProduct >= float(0.0));
assert(maxDotProduct >= 0.0f);
// Return the vertex with the largest dot product in the support direction
return m_vertices[indexMaxDotProduct] * mScaling;
return m_vertices[indexMaxDotProduct] * m_scaling;
}
}
@ -217,33 +217,43 @@ void ConvexMeshShape::recalculateBounds() {
// TODO : Only works if the local origin is inside the mesh
// => Make it more robust (init with first vertex of mesh instead)
m_minBounds.setToZero();
m_maxBounds.setToZero();
m_minBounds.setZero();
m_maxBounds.setZero();
// For each vertex of the mesh
for (uint32_t i=0; i<m_numberVertices; i++) {
if (m_vertices[i].x > m_maxBounds.x) m_maxBounds.x = m_vertices[i].x;
if (m_vertices[i].x < m_minBounds.x) m_minBounds.x = m_vertices[i].x;
if (m_vertices[i].x() > m_maxBounds.x()) {
m_maxBounds.setX(m_vertices[i].x());
}
if (m_vertices[i].x() < m_minBounds.x()) {
m_minBounds.setX(m_vertices[i].x());
}
if (m_vertices[i].y() > m_maxBounds.y()) {
m_maxBounds.setY(m_vertices[i].y());
}
if (m_vertices[i].y() < m_minBounds.y()) {
m_minBounds.setY(m_vertices[i].y());
}
if (m_vertices[i].y > m_maxBounds.y) m_maxBounds.y = m_vertices[i].y;
if (m_vertices[i].y < m_minBounds.y) m_minBounds.y = m_vertices[i].y;
if (m_vertices[i].z > m_maxBounds.z) m_maxBounds.z = m_vertices[i].z;
if (m_vertices[i].z < m_minBounds.z) m_minBounds.z = m_vertices[i].z;
if (m_vertices[i].z() > m_maxBounds.z()) {
m_maxBounds.setZ(m_vertices[i].z());
}
if (m_vertices[i].z() < m_minBounds.z()) {
m_minBounds.setZ(m_vertices[i].z());
}
}
// Apply the local scaling factor
m_maxBounds = m_maxBounds * mScaling;
m_minBounds = m_minBounds * mScaling;
m_maxBounds = m_maxBounds * m_scaling;
m_minBounds = m_minBounds * m_scaling;
// Add the object margin to the bounds
m_maxBounds += Vector3(mMargin, mMargin, mMargin);
m_minBounds -= Vector3(mMargin, mMargin, mMargin);
m_maxBounds += vec3(m_margin, m_margin, m_margin);
m_minBounds -= vec3(m_margin, m_margin, m_margin);
}
// Raycast method with feedback information
bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
return proxyShape->m_body->m_world.m_collisionDetection.m_narrowPhaseGJKAlgorithm.raycast(
ray, proxyShape, raycastInfo);
return proxyShape->m_body->m_world.m_collisionDetection.m_narrowPhaseGJKAlgorithm.raycast(ray, proxyShape, raycastInfo);
}

66
ephysics/collision/shapes/ConvexMeshShape.h
View File

@ -44,16 +44,16 @@ class ConvexMeshShape : public ConvexShape {
// -------------------- Attributes -------------------- //
/// Array with the vertices of the mesh
std::vector<Vector3> m_vertices;
std::vector<vec3> m_vertices;
/// Number of vertices in the mesh
uint32_t m_numberVertices;
/// Mesh minimum bounds in the three local x, y and z directions
Vector3 m_minBounds;
vec3 m_minBounds;
/// Mesh maximum bounds in the three local x, y and z directions
Vector3 m_maxBounds;
vec3 m_maxBounds;
/// True if the shape contains the edges of the convex mesh in order to
/// make the collision detection faster
@ -74,14 +74,14 @@ class ConvexMeshShape : public ConvexShape {
void recalculateBounds();
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
virtual void setLocalScaling(const vec3& scaling);
/// Return a local support point in a given direction without the object margin.
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
@ -108,13 +108,13 @@ class ConvexMeshShape : public ConvexShape {
virtual ~ConvexMeshShape();
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
virtual void getLocalBounds(vec3& min, vec3& max) const;
/// Return the local inertia tensor of the collision shape.
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const;
/// Add a vertex int32_to the convex mesh
void addVertex(const Vector3& vertex);
void addVertex(const vec3& vertex);
/// Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge.
void addEdge(uint32_t v1, uint32_t v2);
@ -128,7 +128,7 @@ class ConvexMeshShape : public ConvexShape {
};
/// Set the scaling vector of the collision shape
inline void ConvexMeshShape::setLocalScaling(const Vector3& scaling) {
inline void ConvexMeshShape::setLocalScaling(const vec3& scaling) {
ConvexShape::setLocalScaling(scaling);
recalculateBounds();
}
@ -143,7 +143,7 @@ inline size_t ConvexMeshShape::getSizeInBytes() const {
* @param min The minimum bounds of the shape in local-space coordinates
* @param max The maximum bounds of the shape in local-space coordinates
*/
inline void ConvexMeshShape::getLocalBounds(Vector3& min, Vector3& max) const {
inline void ConvexMeshShape::getLocalBounds(vec3& min, vec3& max) const {
min = m_minBounds;
max = m_maxBounds;
}
@ -156,14 +156,14 @@ inline void ConvexMeshShape::getLocalBounds(Vector3& min, Vector3& max) const {
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
float factor = (float(1.0) / float(3.0)) * mass;
Vector3 realExtent = float(0.5) * (m_maxBounds - m_minBounds);
assert(realExtent.x > 0 && realExtent.y > 0 && realExtent.z > 0);
float xSquare = realExtent.x * realExtent.x;
float ySquare = realExtent.y * realExtent.y;
float zSquare = realExtent.z * realExtent.z;
tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0,
inline void ConvexMeshShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const {
float factor = (1.0f / float(3.0)) * mass;
vec3 realExtent = 0.5f * (m_maxBounds - m_minBounds);
assert(realExtent.x() > 0 && realExtent.y() > 0 && realExtent.z() > 0);
float xSquare = realExtent.x() * realExtent.x();
float ySquare = realExtent.y() * realExtent.y();
float zSquare = realExtent.z() * realExtent.z();
tensor.setValue(factor * (ySquare + zSquare), 0.0, 0.0,
0.0, factor * (xSquare + zSquare), 0.0,
0.0, 0.0, factor * (xSquare + ySquare));
}
@ -172,19 +172,31 @@ inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, float
/**
* @param vertex Vertex to be added
*/
inline void ConvexMeshShape::addVertex(const Vector3& vertex) {
inline void ConvexMeshShape::addVertex(const vec3& vertex) {
// Add the vertex in to vertices array
m_vertices.push_back(vertex);
m_numberVertices++;
// Update the bounds of the mesh
if (vertex.x * mScaling.x > m_maxBounds.x) m_maxBounds.x = vertex.x * mScaling.x;
if (vertex.x * mScaling.x < m_minBounds.x) m_minBounds.x = vertex.x * mScaling.x;
if (vertex.y * mScaling.y > m_maxBounds.y) m_maxBounds.y = vertex.y * mScaling.y;
if (vertex.y * mScaling.y < m_minBounds.y) m_minBounds.y = vertex.y * mScaling.y;
if (vertex.z * mScaling.z > m_maxBounds.z) m_maxBounds.z = vertex.z * mScaling.z;
if (vertex.z * mScaling.z < m_minBounds.z) m_minBounds.z = vertex.z * mScaling.z;
if (vertex.x() * m_scaling.x() > m_maxBounds.x()) {
m_maxBounds.setX(vertex.x() * m_scaling.x());
}
if (vertex.x() * m_scaling.x() < m_minBounds.x()) {
m_minBounds.setX(vertex.x() * m_scaling.x());
}
if (vertex.y() * m_scaling.y() > m_maxBounds.y()) {
m_maxBounds.setY(vertex.y() * m_scaling.y());
}
if (vertex.y() * m_scaling.y() < m_minBounds.y()) {
m_minBounds.setY(vertex.y() * m_scaling.y());
}
if (vertex.z() * m_scaling.z() > m_maxBounds.z()) {
m_maxBounds.setZ(vertex.z() * m_scaling.z());
}
if (vertex.z() * m_scaling.z() < m_minBounds.z()) {
m_minBounds.setZ(vertex.z() * m_scaling.z());
}
}
// Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge.
@ -231,7 +243,7 @@ inline void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) {
}
// Return true if a point is inside the collision shape
inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint,
inline bool ConvexMeshShape::testPointInside(const vec3& localPoint,
ProxyShape* proxyShape) const {
// Use the GJK algorithm to test if the point is inside the convex mesh

16
ephysics/collision/shapes/ConvexShape.cpp
View File

@ -13,7 +13,7 @@ using namespace reactphysics3d;
// Constructor
ConvexShape::ConvexShape(CollisionShapeType type, float margin)
: CollisionShape(type), mMargin(margin) {
: CollisionShape(type), m_margin(margin) {
}
@ -23,20 +23,20 @@ ConvexShape::~ConvexShape() {
}
// Return a local support point in a given direction with the object margin
Vector3 ConvexShape::getLocalSupportPointWithMargin(const Vector3& direction,
vec3 ConvexShape::getLocalSupportPointWithMargin(const vec3& direction,
void** cachedCollisionData) const {
// Get the support point without margin
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
vec3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
if (mMargin != float(0.0)) {
if (m_margin != 0.0f) {
// Add the margin to the support point
Vector3 unitVec(0.0, -1.0, 0.0);
if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) {
unitVec = direction.getUnit();
vec3 unitVec(0.0, -1.0, 0.0);
if (direction.length2() > MACHINE_EPSILON * MACHINE_EPSILON) {
unitVec = direction.safeNormalized();
}
supportPoint += unitVec * mMargin;
supportPoint += unitVec * m_margin;
}
return supportPoint;

10
ephysics/collision/shapes/ConvexShape.h
View File

@ -23,7 +23,7 @@ class ConvexShape : public CollisionShape {
// -------------------- Attributes -------------------- //
/// Margin used for the GJK collision detection algorithm
float mMargin;
float m_margin;
// -------------------- Methods -------------------- //
@ -34,15 +34,15 @@ class ConvexShape : public CollisionShape {
ConvexShape& operator=(const ConvexShape& shape);
// Return a local support point in a given direction with the object margin
Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
vec3 getLocalSupportPointWithMargin(const vec3& direction,
void** cachedCollisionData) const;
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const=0;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0;
virtual bool testPointInside(const vec3& worldPoint, ProxyShape* proxyShape) const=0;
public :
@ -76,7 +76,7 @@ inline bool ConvexShape::isConvex() const {
* @return The margin (in meters) around the collision shape
*/
inline float ConvexShape::getMargin() const {
return mMargin;
return m_margin;
}
}

99
ephysics/collision/shapes/CylinderShape.cpp
View File

@ -19,9 +19,9 @@ using namespace reactphysics3d;
*/
CylinderShape::CylinderShape(float radius, float height, float margin)
: ConvexShape(CYLINDER, margin), mRadius(radius),
mHalfHeight(height/float(2.0)) {
assert(radius > float(0.0));
assert(height > float(0.0));
m_halfHeight(height/float(2.0)) {
assert(radius > 0.0f);
assert(height > 0.0f);
}
// Destructor
@ -30,24 +30,25 @@ CylinderShape::~CylinderShape() {
}
// Return a local support point in a given direction without the object margin
Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
Vector3 supportPoint(0.0, 0.0, 0.0);
float uDotv = direction.y;
Vector3 w(direction.x, 0.0, direction.z);
float lengthW = sqrt(direction.x * direction.x + direction.z * direction.z);
vec3 CylinderShape::getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const {
vec3 supportPoint(0.0, 0.0, 0.0);
float uDotv = _direction.y();
vec3 w(_direction.x(), 0.0, _direction.z());
float lengthW = sqrt(_direction.x() * _direction.x() + _direction.z() * _direction.z());
if (lengthW > MACHINE_EPSILON) {
if (uDotv < 0.0) supportPoint.y = -mHalfHeight;
else supportPoint.y = mHalfHeight;
if (uDotv < 0.0) {
supportPoint.setY(-m_halfHeight);
} else {
supportPoint.setY(m_halfHeight);
}
supportPoint += (mRadius / lengthW) * w;
} else {
if (uDotv < 0.0) {
supportPoint.setY(-m_halfHeight);
} else {
supportPoint.setY(m_halfHeight);
}
}
else {
if (uDotv < 0.0) supportPoint.y = -mHalfHeight;
else supportPoint.y = mHalfHeight;
}
return supportPoint;
}
@ -56,13 +57,13 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio
/// Morgan Kaufmann.
bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const Vector3 n = ray.point2 - ray.point1;
const vec3 n = ray.point2 - ray.point1;
const float epsilon = float(0.01);
Vector3 p(float(0), -mHalfHeight, float(0));
Vector3 q(float(0), mHalfHeight, float(0));
Vector3 d = q - p;
Vector3 m = ray.point1 - p;
vec3 p(float(0), -m_halfHeight, float(0));
vec3 q(float(0), m_halfHeight, float(0));
vec3 d = q - p;
vec3 m = ray.point1 - p;
float t;
float mDotD = m.dot(d);
@ -70,7 +71,7 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
float dDotD = d.dot(d);
// Test if the segment is outside the cylinder
if (mDotD < float(0.0) && mDotD + nDotD < float(0.0)) return false;
if (mDotD < 0.0f && mDotD + nDotD < float(0.0)) return false;
if (mDotD > dDotD && mDotD + nDotD > dDotD) return false;
float nDotN = n.dot(n);
@ -84,26 +85,26 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
if (std::abs(a) < epsilon) {
// If the origin is outside the surface of the cylinder, we return no hit
if (c > float(0.0)) return false;
if (c > 0.0f) return false;
// Here we know that the segment int32_tersect an endcap of the cylinder
// If the ray int32_tersects with the "p" endcap of the cylinder
if (mDotD < float(0.0)) {
if (mDotD < 0.0f) {
t = -mDotN / nDotN;
// If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < float(0.0) || t > ray.maxFraction) return false;
if (t < 0.0f || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n;
vec3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, float(-1), 0);
vec3 normalDirection(0, float(-1), 0);
raycastInfo.worldNormal = normalDirection;
return true;
@ -114,15 +115,15 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
// If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < float(0.0) || t > ray.maxFraction) return false;
if (t < 0.0f || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n;
vec3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, float(1.0), 0);
vec3 normalDirection(0, 1.0f, 0);
raycastInfo.worldNormal = normalDirection;
return true;
@ -135,35 +136,35 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
float discriminant = b * b - a * c;
// If the discriminant is negative, no real roots and therfore, no hit
if (discriminant < float(0.0)) return false;
if (discriminant < 0.0f) return false;
// Compute the smallest root (first int32_tersection along the ray)
float t0 = t = (-b - std::sqrt(discriminant)) / a;
// If the int32_tersection is outside the cylinder on "p" endcap side
float value = mDotD + t * nDotD;
if (value < float(0.0)) {
if (value < 0.0f) {
// If the ray is pointing away from the "p" endcap, we return no hit
if (nDotD <= float(0.0)) return false;
if (nDotD <= 0.0f) return false;
// Compute the int32_tersection against the "p" endcap (int32_tersection agains whole plane)
t = -mDotD / nDotD;
// Keep the int32_tersection if the it is inside the cylinder radius
if (k + t * (float(2.0) * mDotN + t) > float(0.0)) return false;
if (k + t * (float(2.0) * mDotN + t) > 0.0f) return false;
// If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < float(0.0) || t > ray.maxFraction) return false;
if (t < 0.0f || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n;
vec3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, float(-1.0), 0);
vec3 normalDirection(0, float(-1.0), 0);
raycastInfo.worldNormal = normalDirection;
return true;
@ -171,26 +172,26 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
else if (value > dDotD) { // If the int32_tersection is outside the cylinder on the "q" side
// If the ray is pointing away from the "q" endcap, we return no hit
if (nDotD >= float(0.0)) return false;
if (nDotD >= 0.0f) return false;
// Compute the int32_tersection against the "q" endcap (int32_tersection against whole plane)
t = (dDotD - mDotD) / nDotD;
// Keep the int32_tersection if it is inside the cylinder radius
if (k + dDotD - float(2.0) * mDotD + t * (float(2.0) * (mDotN - nDotD) + t) >
float(0.0)) return false;
0.0f) return false;
// If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < float(0.0) || t > ray.maxFraction) return false;
if (t < 0.0f || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n;
vec3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, float(1.0), 0);
vec3 normalDirection(0, 1.0f, 0);
raycastInfo.worldNormal = normalDirection;
return true;
@ -200,17 +201,17 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
// If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < float(0.0) || t > ray.maxFraction) return false;
if (t < 0.0f || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n;
vec3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
Vector3 v = localHitPoint - p;
Vector3 w = (v.dot(d) / d.lengthSquare()) * d;
Vector3 normalDirection = (localHitPoint - (p + w));
vec3 v = localHitPoint - p;
vec3 w = (v.dot(d) / d.length2()) * d;
vec3 normalDirection = (localHitPoint - (p + w));
raycastInfo.worldNormal = normalDirection;
return true;

53
ephysics/collision/shapes/CylinderShape.h
View File

@ -38,7 +38,7 @@ class CylinderShape : public ConvexShape {
float mRadius;
/// Half height of the cylinder
float mHalfHeight;
float m_halfHeight;
// -------------------- Methods -------------------- //
@ -49,11 +49,11 @@ class CylinderShape : public ConvexShape {
CylinderShape& operator=(const CylinderShape& shape);
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
@ -78,13 +78,13 @@ class CylinderShape : public ConvexShape {
float getHeight() const;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
virtual void setLocalScaling(const vec3& scaling);
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
virtual void getLocalBounds(vec3& min, vec3& max) const;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const;
};
// Return the radius
@ -100,14 +100,14 @@ inline float CylinderShape::getRadius() const {
* @return Height of the cylinder (in meters)
*/
inline float CylinderShape::getHeight() const {
return mHalfHeight + mHalfHeight;
return m_halfHeight + m_halfHeight;
}
// Set the scaling vector of the collision shape
inline void CylinderShape::setLocalScaling(const Vector3& scaling) {
inline void CylinderShape::setLocalScaling(const vec3& scaling) {
mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y;
mRadius = (mRadius / mScaling.x) * scaling.x;
m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y();
mRadius = (mRadius / m_scaling.x()) * scaling.x();
CollisionShape::setLocalScaling(scaling);
}
@ -122,17 +122,15 @@ inline size_t CylinderShape::getSizeInBytes() const {
* @param min The minimum bounds of the shape in local-space coordinates
* @param max The maximum bounds of the shape in local-space coordinates
*/
inline void CylinderShape::getLocalBounds(Vector3& min, Vector3& max) const {
inline void CylinderShape::getLocalBounds(vec3& min, vec3& max) const {
// Maximum bounds
max.x = mRadius + mMargin;
max.y = mHalfHeight + mMargin;
max.z = max.x;
max.setX(mRadius + m_margin);
max.setY(m_halfHeight + m_margin);
max.setZ(max.x());
// Minimum bounds
min.x = -max.x;
min.y = -max.y;
min.z = min.x;
min.setX(-max.x());
min.setY(-max.y());
min.setZ(min.x());
}
// Return the local inertia tensor of the cylinder
@ -141,18 +139,19 @@ inline void CylinderShape::getLocalBounds(Vector3& min, Vector3& max) const {
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
float height = float(2.0) * mHalfHeight;
float diag = (float(1.0) / float(12.0)) * mass * (3 * mRadius * mRadius + height * height);
tensor.setAllValues(diag, 0.0, 0.0, 0.0,
float(0.5) * mass * mRadius * mRadius, 0.0,
inline void CylinderShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const {
float height = float(2.0) * m_halfHeight;
float diag = (1.0f / float(12.0)) * mass * (3 * mRadius * mRadius + height * height);
tensor.setValue(diag, 0.0, 0.0, 0.0,
0.5f * mass * mRadius * mRadius, 0.0,
0.0, 0.0, diag);
}
// Return true if a point is inside the collision shape
inline bool CylinderShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const{
return ((localPoint.x * localPoint.x + localPoint.z * localPoint.z) < mRadius * mRadius &&
localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight);
inline bool CylinderShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const{
return ( (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z()) < mRadius * mRadius
&& localPoint.y() < m_halfHeight
&& localPoint.y() > -m_halfHeight);
}
}

148
ephysics/collision/shapes/HeightFieldShape.cpp
View File

@ -23,35 +23,35 @@ using namespace reactphysics3d;
HeightFieldShape::HeightFieldShape(int32_t nbGridColumns, int32_t nbGridRows, float minHeight, float maxHeight,
const void* heightFieldData, HeightDataType dataType, int32_t upAxis,
float int32_tegerHeightScale)
: ConcaveShape(HEIGHTFIELD), mNbColumns(nbGridColumns), mNbRows(nbGridRows),
mWidth(nbGridColumns - 1), mLength(nbGridRows - 1), mMinHeight(minHeight),
mMaxHeight(maxHeight), mUpAxis(upAxis), mIntegerHeightScale(int32_tegerHeightScale),
mHeightDataType(dataType) {
: ConcaveShape(HEIGHTFIELD), m_numberColumns(nbGridColumns), m_numberRows(nbGridRows),
m_width(nbGridColumns - 1), m_length(nbGridRows - 1), m_minHeight(minHeight),
m_maxHeight(maxHeight), m_upAxis(upAxis), m_integerHeightScale(int32_tegerHeightScale),
m_heightDataType(dataType) {
assert(nbGridColumns >= 2);
assert(nbGridRows >= 2);
assert(mWidth >= 1);
assert(mLength >= 1);
assert(m_width >= 1);
assert(m_length >= 1);
assert(minHeight <= maxHeight);
assert(upAxis == 0 || upAxis == 1 || upAxis == 2);
mHeightFieldData = heightFieldData;
m_heightFieldData = heightFieldData;
float halfHeight = (mMaxHeight - mMinHeight) * float(0.5);
float halfHeight = (m_maxHeight - m_minHeight) * 0.5f;
assert(halfHeight >= 0);
// Compute the local AABB of the height field
if (mUpAxis == 0) {
mAABB.setMin(Vector3(-halfHeight, -mWidth * float(0.5), -mLength * float(0.5)));
mAABB.setMax(Vector3(halfHeight, mWidth * float(0.5), mLength* float(0.5)));
if (m_upAxis == 0) {
m_AABB.setMin(vec3(-halfHeight, -m_width * 0.5f, -m_length * float(0.5)));
m_AABB.setMax(vec3(halfHeight, m_width * 0.5f, m_length* float(0.5)));
}
else if (mUpAxis == 1) {
mAABB.setMin(Vector3(-mWidth * float(0.5), -halfHeight, -mLength * float(0.5)));
mAABB.setMax(Vector3(mWidth * float(0.5), halfHeight, mLength * float(0.5)));
else if (m_upAxis == 1) {
m_AABB.setMin(vec3(-m_width * 0.5f, -halfHeight, -m_length * float(0.5)));
m_AABB.setMax(vec3(m_width * 0.5f, halfHeight, m_length * float(0.5)));
}
else if (mUpAxis == 2) {
mAABB.setMin(Vector3(-mWidth * float(0.5), -mLength * float(0.5), -halfHeight));
mAABB.setMax(Vector3(mWidth * float(0.5), mLength * float(0.5), halfHeight));
else if (m_upAxis == 2) {
m_AABB.setMin(vec3(-m_width * 0.5f, -m_length * float(0.5), -halfHeight));
m_AABB.setMax(vec3(m_width * 0.5f, m_length * float(0.5), halfHeight));
}
}
@ -66,9 +66,9 @@ HeightFieldShape::~HeightFieldShape() {
* @param min The minimum bounds of the shape in local-space coordinates
* @param max The maximum bounds of the shape in local-space coordinates
*/
void HeightFieldShape::getLocalBounds(Vector3& min, Vector3& max) const {
min = mAABB.getMin() * mScaling;
max = mAABB.getMax() * mScaling;
void HeightFieldShape::getLocalBounds(vec3& min, vec3& max) const {
min = m_AABB.getMin() * m_scaling;
max = m_AABB.getMax() * m_scaling;
}
// Test collision with the triangles of the height field shape. The idea is to use the AABB
@ -78,7 +78,7 @@ void HeightFieldShape::getLocalBounds(Vector3& min, Vector3& max) const {
void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const {
// Compute the non-scaled AABB
Vector3 inverseScaling(float(1.0) / mScaling.x, float(1.0) / mScaling.y, float(1.0) / mScaling.z);
vec3 inverseScaling(1.0f / m_scaling.x(), 1.0f / m_scaling.y(), float(1.0) / m_scaling.z());
AABB aabb(localAABB.getMin() * inverseScaling, localAABB.getMax() * inverseScaling);
// Compute the int32_teger grid coordinates inside the area we need to test for collision
@ -91,41 +91,41 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB&
int32_t iMax = 0;
int32_t jMin = 0;
int32_t jMax = 0;
switch(mUpAxis) {
case 0 : iMin = clamp(minGridCoords[1], 0, mNbColumns - 1);
iMax = clamp(maxGridCoords[1], 0, mNbColumns - 1);
jMin = clamp(minGridCoords[2], 0, mNbRows - 1);
jMax = clamp(maxGridCoords[2], 0, mNbRows - 1);
switch(m_upAxis) {
case 0 : iMin = clamp(minGridCoords[1], 0, m_numberColumns - 1);
iMax = clamp(maxGridCoords[1], 0, m_numberColumns - 1);
jMin = clamp(minGridCoords[2], 0, m_numberRows - 1);
jMax = clamp(maxGridCoords[2], 0, m_numberRows - 1);
break;
case 1 : iMin = clamp(minGridCoords[0], 0, mNbColumns - 1);
iMax = clamp(maxGridCoords[0], 0, mNbColumns - 1);
jMin = clamp(minGridCoords[2], 0, mNbRows - 1);
jMax = clamp(maxGridCoords[2], 0, mNbRows - 1);
case 1 : iMin = clamp(minGridCoords[0], 0, m_numberColumns - 1);
iMax = clamp(maxGridCoords[0], 0, m_numberColumns - 1);
jMin = clamp(minGridCoords[2], 0, m_numberRows - 1);
jMax = clamp(maxGridCoords[2], 0, m_numberRows - 1);
break;
case 2 : iMin = clamp(minGridCoords[0], 0, mNbColumns - 1);
iMax = clamp(maxGridCoords[0], 0, mNbColumns - 1);
jMin = clamp(minGridCoords[1], 0, mNbRows - 1);
jMax = clamp(maxGridCoords[1], 0, mNbRows - 1);
case 2 : iMin = clamp(minGridCoords[0], 0, m_numberColumns - 1);
iMax = clamp(maxGridCoords[0], 0, m_numberColumns - 1);
jMin = clamp(minGridCoords[1], 0, m_numberRows - 1);
jMax = clamp(maxGridCoords[1], 0, m_numberRows - 1);
break;
}
assert(iMin >= 0 && iMin < mNbColumns);
assert(iMax >= 0 && iMax < mNbColumns);
assert(jMin >= 0 && jMin < mNbRows);
assert(jMax >= 0 && jMax < mNbRows);
assert(iMin >= 0 && iMin < m_numberColumns);
assert(iMax >= 0 && iMax < m_numberColumns);
assert(jMin >= 0 && jMin < m_numberRows);
assert(jMax >= 0 && jMax < m_numberRows);
// For each sub-grid points (except the last ones one each dimension)
for (int32_t i = iMin; i < iMax; i++) {
for (int32_t j = jMin; j < jMax; j++) {
// Compute the four point of the current quad
Vector3 p1 = getVertexAt(i, j);
Vector3 p2 = getVertexAt(i, j + 1);
Vector3 p3 = getVertexAt(i + 1, j);
Vector3 p4 = getVertexAt(i + 1, j + 1);
vec3 p1 = getVertexAt(i, j);
vec3 p2 = getVertexAt(i, j + 1);
vec3 p3 = getVertexAt(i + 1, j);
vec3 p4 = getVertexAt(i + 1, j + 1);
// Generate the first triangle for the current grid rectangle
Vector3 trianglePoints[3] = {p1, p2, p3};
vec3 trianglePoints[3] = {p1, p2, p3};
// Test collision against the first triangle
callback.testTriangle(trianglePoints);
@ -146,29 +146,29 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB&
void HeightFieldShape::computeMinMaxGridCoordinates(int32_t* minCoords, int32_t* maxCoords, const AABB& aabbToCollide) const {
// Clamp the min/max coords of the AABB to collide inside the height field AABB
Vector3 minPoint = Vector3::max(aabbToCollide.getMin(), mAABB.getMin());
minPoint = Vector3::min(minPoint, mAABB.getMax());
vec3 minPoint = etk::max(aabbToCollide.getMin(), m_AABB.getMin());
minPoint = etk::min(minPoint, m_AABB.getMax());
Vector3 maxPoint = Vector3::min(aabbToCollide.getMax(), mAABB.getMax());
maxPoint = Vector3::max(maxPoint, mAABB.getMin());
vec3 maxPoint = etk::min(aabbToCollide.getMax(), m_AABB.getMax());
maxPoint = etk::max(maxPoint, m_AABB.getMin());
// Translate the min/max points such that the we compute grid points from [0 ... mNbWidthGridPoints]
// and from [0 ... mNbLengthGridPoints] because the AABB coordinates range are [-mWdith/2 ... mWidth/2]
// and [-mLength/2 ... mLength/2]
const Vector3 translateVec = mAABB.getExtent() * float(0.5);
// and from [0 ... mNbLengthGridPoints] because the AABB coordinates range are [-mWdith/2 ... m_width/2]
// and [-m_length/2 ... m_length/2]
const vec3 translateVec = m_AABB.getExtent() * 0.5f;
minPoint += translateVec;
maxPoint += translateVec;
// Convert the floating min/max coords of the AABB int32_to closest int32_teger
// grid values (note that we use the closest grid coordinate that is out
// of the AABB)
minCoords[0] = computeIntegerGridValue(minPoint.x) - 1;
minCoords[1] = computeIntegerGridValue(minPoint.y) - 1;
minCoords[2] = computeIntegerGridValue(minPoint.z) - 1;
minCoords[0] = computeIntegerGridValue(minPoint.x()) - 1;
minCoords[1] = computeIntegerGridValue(minPoint.y()) - 1;
minCoords[2] = computeIntegerGridValue(minPoint.z()) - 1;
maxCoords[0] = computeIntegerGridValue(maxPoint.x) + 1;
maxCoords[1] = computeIntegerGridValue(maxPoint.y) + 1;
maxCoords[2] = computeIntegerGridValue(maxPoint.z) + 1;
maxCoords[0] = computeIntegerGridValue(maxPoint.x()) + 1;
maxCoords[1] = computeIntegerGridValue(maxPoint.y()) + 1;
maxCoords[2] = computeIntegerGridValue(maxPoint.z()) + 1;
}
// Raycast method with feedback information
@ -184,8 +184,8 @@ bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh
TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this);
// Compute the AABB for the ray
const Vector3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
const AABB rayAABB(Vector3::min(ray.point1, rayEnd), Vector3::max(ray.point1, rayEnd));
const vec3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
const AABB rayAABB(etk::min(ray.point1, rayEnd), etk::max(ray.point1, rayEnd));
testAllTriangles(triangleCallback, rayAABB);
@ -193,46 +193,46 @@ bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh
}
// Return the vertex (local-coordinates) of the height field at a given (x,y) position
Vector3 HeightFieldShape::getVertexAt(int32_t x, int32_t y) const {
vec3 HeightFieldShape::getVertexAt(int32_t x, int32_t y) const {
// Get the height value
const float height = getHeightAt(x, y);
// Height values origin
const float heightOrigin = -(mMaxHeight - mMinHeight) * float(0.5) - mMinHeight;
const float heightOrigin = -(m_maxHeight - m_minHeight) * 0.5f - m_minHeight;
Vector3 vertex;
switch (mUpAxis) {
case 0: vertex = Vector3(heightOrigin + height, -mWidth * float(0.5) + x, -mLength * float(0.5) + y);
vec3 vertex;
switch (m_upAxis) {
case 0: vertex = vec3(heightOrigin + height, -m_width * 0.5f + x, -m_length * float(0.5) + y);
break;
case 1: vertex = Vector3(-mWidth * float(0.5) + x, heightOrigin + height, -mLength * float(0.5) + y);
case 1: vertex = vec3(-m_width * 0.5f + x, heightOrigin + height, -m_length * float(0.5) + y);
break;
case 2: vertex = Vector3(-mWidth * float(0.5) + x, -mLength * float(0.5) + y, heightOrigin + height);
case 2: vertex = vec3(-m_width * 0.5f + x, -m_length * float(0.5) + y, heightOrigin + height);
break;
default: assert(false);
}
assert(mAABB.contains(vertex));
assert(m_AABB.contains(vertex));
return vertex * mScaling;
return vertex * m_scaling;
}
// Raycast test between a ray and a triangle of the heightfield
void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) {
void TriangleOverlapCallback::testTriangle(const vec3* trianglePoints) {
// Create a triangle collision shape
float margin = mHeightFieldShape.getTriangleMargin();
float margin = m_heightFieldShape.getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType());
triangleShape.setRaycastTestType(m_heightFieldShape.getRaycastTestType());
// Ray casting test against the collision shape
RaycastInfo raycastInfo;
bool isTriangleHit = triangleShape.raycast(m_ray, raycastInfo, m_proxyShape);
// If the ray hit the collision shape
if (isTriangleHit && raycastInfo.hitFraction <= mSmallestHitFraction) {
if (isTriangleHit && raycastInfo.hitFraction <= m_smallestHitFraction) {
assert(raycastInfo.hitFraction >= float(0.0));
assert(raycastInfo.hitFraction >= 0.0f);
m_raycastInfo.body = raycastInfo.body;
m_raycastInfo.proxyShape = raycastInfo.proxyShape;
@ -242,7 +242,7 @@ void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) {
m_raycastInfo.meshSubpart = -1;
m_raycastInfo.triangleIndex = -1;
mSmallestHitFraction = raycastInfo.hitFraction;
mIsHit = true;
m_smallestHitFraction = raycastInfo.hitFraction;
m_isHit = true;
}
}

121
ephysics/collision/shapes/HeightFieldShape.h
View File

@ -11,45 +11,40 @@
#include <ephysics/engine/Profiler.h>
namespace reactphysics3d {
class HeightFieldShape;
/**
* @brief This class is used for testing AABB and triangle overlap for raycasting
*/
class TriangleOverlapCallback : public TriangleCallback {
protected:
const Ray& m_ray;
ProxyShape* m_proxyShape;
RaycastInfo& m_raycastInfo;
bool m_isHit;
float m_smallestHitFraction;
const HeightFieldShape& m_heightFieldShape;
public:
TriangleOverlapCallback(const Ray& _ray,
ProxyShape* _proxyShape,
RaycastInfo& _raycastInfo,
const HeightFieldShape& _heightFieldShape):
m_ray(_ray),
m_proxyShape(_proxyShape),
m_raycastInfo(_raycastInfo),
m_heightFieldShape(_heightFieldShape) {
m_isHit = false;
m_smallestHitFraction = m_ray.maxFraction;
}
bool getIsHit() const {
return m_isHit;
}
/// Raycast test between a ray and a triangle of the heightfield
virtual void testTriangle(const vec3* _trianglePoints);
};
class HeightFieldShape;
// Class TriangleOverlapCallback
/**
* This class is used for testing AABB and triangle overlap for raycasting
*/
class TriangleOverlapCallback : public TriangleCallback {
protected:
const Ray& m_ray;
ProxyShape* m_proxyShape;
RaycastInfo& m_raycastInfo;
bool mIsHit;
float mSmallestHitFraction;
const HeightFieldShape& mHeightFieldShape;
public:
// Constructor
TriangleOverlapCallback(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo,
const HeightFieldShape& heightFieldShape)
: m_ray(ray), m_proxyShape(proxyShape), m_raycastInfo(raycastInfo),
mHeightFieldShape (heightFieldShape) {
mIsHit = false;
mSmallestHitFraction = m_ray.maxFraction;
}
bool getIsHit() const {return mIsHit;}
/// Raycast test between a ray and a triangle of the heightfield
virtual void testTriangle(const Vector3* trianglePoints);
};
// Class HeightFieldShape
/**
* This class represents a static height field that can be used to represent
* @brief This class represents a static height field that can be used to represent
* a terrain. The height field is made of a grid with rows and columns with a
* height value at each grid point. Note that the height values are not copied int32_to the shape
* but are shared instead. The height values can be of type int32_teger, float or double.
@ -70,37 +65,37 @@ class HeightFieldShape : public ConcaveShape {
// -------------------- Attributes -------------------- //
/// Number of columns in the grid of the height field
int32_t mNbColumns;
int32_t m_numberColumns;
/// Number of rows in the grid of the height field
int32_t mNbRows;
int32_t m_numberRows;
/// Height field width
float mWidth;
float m_width;
/// Height field length
float mLength;
float m_length;
/// Minimum height of the height field
float mMinHeight;
float m_minHeight;
/// Maximum height of the height field
float mMaxHeight;
float m_maxHeight;
/// Up axis direction (0 => x, 1 => y, 2 => z)
int32_t mUpAxis;
int32_t m_upAxis;
/// Height values scale for height field with int32_teger height values
float mIntegerHeightScale;
float m_integerHeightScale;
/// Data type of the height values
HeightDataType mHeightDataType;
HeightDataType m_heightDataType;
/// Array of data with all the height values of the height field
const void* mHeightFieldData;
const void* m_heightFieldData;
/// Local AABB of the height field (without scaling)
AABB mAABB;
AABB m_AABB;
// -------------------- Methods -------------------- //
@ -122,10 +117,10 @@ class HeightFieldShape : public ConcaveShape {
/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
/// given the start vertex index pointer of the triangle.
void getTriangleVerticesWithIndexPointer(int32_t subPart, int32_t triangleIndex,
Vector3* outTriangleVertices) const;
vec3* outTriangleVertices) const;
/// Return the vertex (local-coordinates) of the height field at a given (x,y) position
Vector3 getVertexAt(int32_t x, int32_t y) const;
vec3 getVertexAt(int32_t x, int32_t y) const;
/// Return the height of a given (x,y) point in the height field
float getHeightAt(int32_t x, int32_t y) const;
@ -156,13 +151,13 @@ class HeightFieldShape : public ConcaveShape {
HeightDataType getHeightDataType() const;
/// Return the local bounds of the shape in x, y and z directions.
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
virtual void getLocalBounds(vec3& min, vec3& max) const;
/// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
virtual void setLocalScaling(const vec3& scaling);
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const;
/// Use a callback method on all triangles of the concave shape inside a given AABB
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const;
@ -175,17 +170,17 @@ class HeightFieldShape : public ConcaveShape {
// Return the number of rows in the height field
inline int32_t HeightFieldShape::getNbRows() const {
return mNbRows;
return m_numberRows;
}
// Return the number of columns in the height field
inline int32_t HeightFieldShape::getNbColumns() const {
return mNbColumns;
return m_numberColumns;
}
// Return the type of height value in the height field
inline HeightFieldShape::HeightDataType HeightFieldShape::getHeightDataType() const {
return mHeightDataType;
return m_heightDataType;
}
// Return the number of bytes used by the collision shape
@ -194,24 +189,24 @@ inline size_t HeightFieldShape::getSizeInBytes() const {
}
// Set the local scaling vector of the collision shape
inline void HeightFieldShape::setLocalScaling(const Vector3& scaling) {
inline void HeightFieldShape::setLocalScaling(const vec3& scaling) {
CollisionShape::setLocalScaling(scaling);
}
// Return the height of a given (x,y) point in the height field
inline float HeightFieldShape::getHeightAt(int32_t x, int32_t y) const {
switch(mHeightDataType) {
case HEIGHT_FLOAT_TYPE : return ((float*)mHeightFieldData)[y * mNbColumns + x];
case HEIGHT_DOUBLE_TYPE : return ((double*)mHeightFieldData)[y * mNbColumns + x];
case HEIGHT_INT_TYPE : return ((int32_t*)mHeightFieldData)[y * mNbColumns + x] * mIntegerHeightScale;
switch(m_heightDataType) {
case HEIGHT_FLOAT_TYPE : return ((float*)m_heightFieldData)[y * m_numberColumns + x];
case HEIGHT_DOUBLE_TYPE : return ((double*)m_heightFieldData)[y * m_numberColumns + x];
case HEIGHT_INT_TYPE : return ((int32_t*)m_heightFieldData)[y * m_numberColumns + x] * m_integerHeightScale;
default: assert(false); return 0;
}
}
// Return the closest inside int32_teger grid value of a given floating grid value
inline int32_t HeightFieldShape::computeIntegerGridValue(float value) const {
return (value < float(0.0)) ? value - float(0.5) : value + float(0.5);
return (value < 0.0f) ? value - 0.5f : value + float(0.5);
}
// Return the local inertia tensor
@ -220,13 +215,13 @@ inline int32_t HeightFieldShape::computeIntegerGridValue(float value) const {
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void HeightFieldShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
inline void HeightFieldShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const {
// Default inertia tensor
// Note that this is not very realistic for a concave triangle mesh.
// However, in most cases, it will only be used static bodies and therefore,
// the inertia tensor is not used.
tensor.setAllValues(mass, 0, 0,
tensor.setValue(mass, 0, 0,
0, mass, 0,
0, 0, mass);
}

18
ephysics/collision/shapes/SphereShape.cpp
View File

@ -17,7 +17,7 @@ using namespace reactphysics3d;
* @param radius Radius of the sphere (in meters)
*/
SphereShape::SphereShape(float radius) : ConvexShape(SPHERE, radius) {
assert(radius > float(0.0));
assert(radius > 0.0f);
}
// Destructor
@ -28,31 +28,31 @@ SphereShape::~SphereShape() {
// Raycast method with feedback information
bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const Vector3 m = ray.point1;
float c = m.dot(m) - mMargin * mMargin;
const vec3 m = ray.point1;
float c = m.dot(m) - m_margin * m_margin;
// If the origin of the ray is inside the sphere, we return no int32_tersection
if (c < float(0.0)) return false;
if (c < 0.0f) return false;
const Vector3 rayDirection = ray.point2 - ray.point1;
const vec3 rayDirection = ray.point2 - ray.point1;
float b = m.dot(rayDirection);
// If the origin of the ray is outside the sphere and the ray
// is pointing away from the sphere, there is no int32_tersection
if (b > float(0.0)) return false;
if (b > 0.0f) return false;
float raySquareLength = rayDirection.lengthSquare();
float raySquareLength = rayDirection.length2();
// Compute the discriminant of the quadratic equation
float discriminant = b * b - raySquareLength * c;
// If the discriminant is negative or the ray length is very small, there is no int32_tersection
if (discriminant < float(0.0) || raySquareLength < MACHINE_EPSILON) return false;
if (discriminant < 0.0f || raySquareLength < MACHINE_EPSILON) return false;
// Compute the solution "t" closest to the origin
float t = -b - std::sqrt(discriminant);
assert(t >= float(0.0));
assert(t >= 0.0f);
// If the hit point is withing the segment ray fraction
if (t < ray.maxFraction * raySquareLength) {

58
ephysics/collision/shapes/SphereShape.h
View File

@ -37,11 +37,11 @@ class SphereShape : public ConvexShape {
SphereShape& operator=(const SphereShape& shape);
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
@ -63,16 +63,16 @@ class SphereShape : public ConvexShape {
float getRadius() const;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
virtual void setLocalScaling(const vec3& scaling);
/// Return the local bounds of the shape in x, y and z directions.
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
virtual void getLocalBounds(vec3& min, vec3& max) const;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const;
/// Update the AABB of a body using its collision shape
virtual void computeAABB(AABB& aabb, const Transform& transform) const;
virtual void computeAABB(AABB& aabb, const etk::Transform3D& transform) const;
};
// Get the radius of the sphere
@ -80,13 +80,13 @@ class SphereShape : public ConvexShape {
* @return Radius of the sphere (in meters)
*/
inline float SphereShape::getRadius() const {
return mMargin;
return m_margin;
}
// Set the scaling vector of the collision shape
inline void SphereShape::setLocalScaling(const Vector3& scaling) {
inline void SphereShape::setLocalScaling(const vec3& scaling) {
mMargin = (mMargin / mScaling.x) * scaling.x;
m_margin = (m_margin / m_scaling.x()) * scaling.x();
CollisionShape::setLocalScaling(scaling);
}
@ -97,11 +97,11 @@ inline size_t SphereShape::getSizeInBytes() const {
}
// Return a local support point in a given direction without the object margin
inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
inline vec3 SphereShape::getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const {
// Return the center of the sphere (the radius is taken int32_to account in the object margin)
return Vector3(0.0, 0.0, 0.0);
return vec3(0.0, 0.0, 0.0);
}
// Return the local bounds of the shape in x, y and z directions.
@ -110,17 +110,15 @@ inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& dir
* @param min The minimum bounds of the shape in local-space coordinates
* @param max The maximum bounds of the shape in local-space coordinates
*/
inline void SphereShape::getLocalBounds(Vector3& min, Vector3& max) const {
inline void SphereShape::getLocalBounds(vec3& min, vec3& max) const {
// Maximum bounds
max.x = mMargin;
max.y = mMargin;
max.z = mMargin;
max.setX(m_margin);
max.setY(m_margin);
max.setZ(m_margin);
// Minimum bounds
min.x = -mMargin;
min.y = min.x;
min.z = min.x;
min.setX(-m_margin);
min.setY(min.x());
min.setZ(min.x());
}
// Return the local inertia tensor of the sphere
@ -129,23 +127,23 @@ inline void SphereShape::getLocalBounds(Vector3& min, Vector3& max) const {
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
float diag = float(0.4) * mass * mMargin * mMargin;
tensor.setAllValues(diag, 0.0, 0.0,
0.0, diag, 0.0,
0.0, 0.0, diag);
inline void SphereShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const {
float diag = float(0.4) * mass * m_margin * m_margin;
tensor.setValue(diag, 0.0, 0.0,
0.0, diag, 0.0,
0.0, 0.0, diag);
}
// Update the AABB of a body using its collision shape
/**
* @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape
* computed in world-space coordinates
* @param transform Transform used to compute the AABB of the collision shape
* @param transform etk::Transform3D used to compute the AABB of the collision shape
*/
inline void SphereShape::computeAABB(AABB& aabb, const Transform& transform) const {
inline void SphereShape::computeAABB(AABB& aabb, const etk::Transform3D& transform) const {
// Get the local extents in x,y and z direction
Vector3 extents(mMargin, mMargin, mMargin);
vec3 extents(m_margin, m_margin, m_margin);
// Update the AABB with the new minimum and maximum coordinates
aabb.setMin(transform.getPosition() - extents);
@ -153,8 +151,8 @@ inline void SphereShape::computeAABB(AABB& aabb, const Transform& transform) con
}
// Return true if a point is inside the collision shape
inline bool SphereShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
return (localPoint.lengthSquare() < mMargin * mMargin);
inline bool SphereShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const {
return (localPoint.length2() < m_margin * m_margin);
}
}

34
ephysics/collision/shapes/TriangleShape.cpp
View File

@ -20,7 +20,7 @@ using namespace reactphysics3d;
* @param point3 Third point of the triangle
* @param margin The collision margin (in meters) around the collision shape
*/
TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, float margin)
TriangleShape::TriangleShape(const vec3& point1, const vec3& point2, const vec3& point3, float margin)
: ConvexShape(TRIANGLE, margin) {
mPoints[0] = point1;
mPoints[1] = point2;
@ -40,28 +40,28 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
PROFILE("TriangleShape::raycast()");
const Vector3 pq = ray.point2 - ray.point1;
const Vector3 pa = mPoints[0] - ray.point1;
const Vector3 pb = mPoints[1] - ray.point1;
const Vector3 pc = mPoints[2] - ray.point1;
const vec3 pq = ray.point2 - ray.point1;
const vec3 pa = mPoints[0] - ray.point1;
const vec3 pb = mPoints[1] - ray.point1;
const vec3 pc = mPoints[2] - ray.point1;
// Test if the line PQ is inside the eges BC, CA and AB. We use the triple
// product for this test.
const Vector3 m = pq.cross(pc);
const vec3 m = pq.cross(pc);
float u = pb.dot(m);
if (m_raycastTestType == FRONT) {
if (u < float(0.0)) return false;
if (u < 0.0f) return false;
}
else if (m_raycastTestType == BACK) {
if (u > float(0.0)) return false;
if (u > 0.0f) return false;
}
float v = -pa.dot(m);
if (m_raycastTestType == FRONT) {
if (v < float(0.0)) return false;
if (v < 0.0f) return false;
}
else if (m_raycastTestType == BACK) {
if (v > float(0.0)) return false;
if (v > 0.0f) return false;
}
else if (m_raycastTestType == FRONT_AND_BACK) {
if (!sameSign(u, v)) return false;
@ -69,10 +69,10 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
float w = pa.dot(pq.cross(pb));
if (m_raycastTestType == FRONT) {
if (w < float(0.0)) return false;
if (w < 0.0f) return false;
}
else if (m_raycastTestType == BACK) {
if (w > float(0.0)) return false;
if (w > 0.0f) return false;
}
else if (m_raycastTestType == FRONT_AND_BACK) {
if (!sameSign(u, w)) return false;
@ -83,19 +83,19 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
// Compute the barycentric coordinates (u, v, w) to determine the
// int32_tersection point R, R = u * a + v * b + w * c
float denom = float(1.0) / (u + v + w);
float denom = 1.0f / (u + v + w);
u *= denom;
v *= denom;
w *= denom;
// Compute the local hit point using the barycentric coordinates
const Vector3 localHitPoint = u * mPoints[0] + v * mPoints[1] + w * mPoints[2];
const vec3 localHitPoint = u * mPoints[0] + v * mPoints[1] + w * mPoints[2];
const float hitFraction = (localHitPoint - ray.point1).length() / pq.length();
if (hitFraction < float(0.0) || hitFraction > ray.maxFraction) return false;
if (hitFraction < 0.0f || hitFraction > ray.maxFraction) return false;
Vector3 localHitNormal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]);
if (localHitNormal.dot(pq) > float(0.0)) localHitNormal = -localHitNormal;
vec3 localHitNormal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]);
if (localHitNormal.dot(pq) > 0.0f) localHitNormal = -localHitNormal;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;

74
ephysics/collision/shapes/TriangleShape.h
View File

@ -37,7 +37,7 @@ class TriangleShape : public ConvexShape {
// -------------------- Attribute -------------------- //
/// Three points of the triangle
Vector3 mPoints[3];
vec3 mPoints[3];
/// Raycast test type for the triangle (front, back, front-back)
TriangleRaycastSide m_raycastTestType;
@ -51,11 +51,11 @@ class TriangleShape : public ConvexShape {
TriangleShape& operator=(const TriangleShape& shape);
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
@ -68,23 +68,23 @@ class TriangleShape : public ConvexShape {
// -------------------- Methods -------------------- //
/// Constructor
TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3,
TriangleShape(const vec3& point1, const vec3& point2, const vec3& point3,
float margin = OBJECT_MARGIN);
/// Destructor
virtual ~TriangleShape();
/// Return the local bounds of the shape in x, y and z directions.
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
virtual void getLocalBounds(vec3& min, vec3& max) const;
/// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
virtual void setLocalScaling(const vec3& scaling);
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const;
/// Update the AABB of a body using its collision shape
virtual void computeAABB(AABB& aabb, const Transform& transform) const;
virtual void computeAABB(AABB& aabb, const etk::Transform3D& transform) const;
/// Return the raycast test type (front, back, front-back)
TriangleRaycastSide getRaycastTestType() const;
@ -93,7 +93,7 @@ class TriangleShape : public ConvexShape {
void setRaycastTestType(TriangleRaycastSide testType);
/// Return the coordinates of a given vertex of the triangle
Vector3 getVertex(int32_t index) const;
vec3 getVertex(int32_t index) const;
// ---------- Friendship ---------- //
@ -107,9 +107,9 @@ inline size_t TriangleShape::getSizeInBytes() const {
}
// Return a local support point in a given direction without the object margin
inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
inline vec3 TriangleShape::getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const {
Vector3 dotProducts(direction.dot(mPoints[0]), direction.dot(mPoints[1]), direction.dot(mPoints[2]));
vec3 dotProducts(direction.dot(mPoints[0]), direction.dot(mPoints[1]), direction.dot(mPoints[2]));
return mPoints[dotProducts.getMaxAxis()];
}
@ -119,24 +119,24 @@ inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& d
* @param min The minimum bounds of the shape in local-space coordinates
* @param max The maximum bounds of the shape in local-space coordinates
*/
inline void TriangleShape::getLocalBounds(Vector3& min, Vector3& max) const {
inline void TriangleShape::getLocalBounds(vec3& min, vec3& max) const {
const Vector3 xAxis(mPoints[0].x, mPoints[1].x, mPoints[2].x);
const Vector3 yAxis(mPoints[0].y, mPoints[1].y, mPoints[2].y);
const Vector3 zAxis(mPoints[0].z, mPoints[1].z, mPoints[2].z);
min.setAllValues(xAxis.getMinValue(), yAxis.getMinValue(), zAxis.getMinValue());
max.setAllValues(xAxis.getMaxValue(), yAxis.getMaxValue(), zAxis.getMaxValue());
const vec3 xAxis(mPoints[0].x(), mPoints[1].x(), mPoints[2].x());
const vec3 yAxis(mPoints[0].y(), mPoints[1].y(), mPoints[2].y());
const vec3 zAxis(mPoints[0].z(), mPoints[1].z(), mPoints[2].z());
min.setValue(xAxis.getMin(), yAxis.getMin(), zAxis.getMin());
max.setValue(xAxis.getMax(), yAxis.getMax(), zAxis.getMax());
min -= Vector3(mMargin, mMargin, mMargin);
max += Vector3(mMargin, mMargin, mMargin);
min -= vec3(m_margin, m_margin, m_margin);
max += vec3(m_margin, m_margin, m_margin);
}
// Set the local scaling vector of the collision shape
inline void TriangleShape::setLocalScaling(const Vector3& scaling) {
inline void TriangleShape::setLocalScaling(const vec3& scaling) {
mPoints[0] = (mPoints[0] / mScaling) * scaling;
mPoints[1] = (mPoints[1] / mScaling) * scaling;
mPoints[2] = (mPoints[2] / mScaling) * scaling;
mPoints[0] = (mPoints[0] / m_scaling) * scaling;
mPoints[1] = (mPoints[1] / m_scaling) * scaling;
mPoints[2] = (mPoints[2] / m_scaling) * scaling;
CollisionShape::setLocalScaling(scaling);
}
@ -147,31 +147,31 @@ inline void TriangleShape::setLocalScaling(const Vector3& scaling) {
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void TriangleShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
tensor.setToZero();
inline void TriangleShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const {
tensor.setZero();
}
// Update the AABB of a body using its collision shape
/**
* @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape
* computed in world-space coordinates
* @param transform Transform used to compute the AABB of the collision shape
* @param transform etk::Transform3D used to compute the AABB of the collision shape
*/
inline void TriangleShape::computeAABB(AABB& aabb, const Transform& transform) const {
inline void TriangleShape::computeAABB(AABB& aabb, const etk::Transform3D& transform) const {
const Vector3 worldPoint1 = transform * mPoints[0];
const Vector3 worldPoint2 = transform * mPoints[1];
const Vector3 worldPoint3 = transform * mPoints[2];
const vec3 worldPoint1 = transform * mPoints[0];
const vec3 worldPoint2 = transform * mPoints[1];
const vec3 worldPoint3 = transform * mPoints[2];
const Vector3 xAxis(worldPoint1.x, worldPoint2.x, worldPoint3.x);
const Vector3 yAxis(worldPoint1.y, worldPoint2.y, worldPoint3.y);
const Vector3 zAxis(worldPoint1.z, worldPoint2.z, worldPoint3.z);
aabb.setMin(Vector3(xAxis.getMinValue(), yAxis.getMinValue(), zAxis.getMinValue()));
aabb.setMax(Vector3(xAxis.getMaxValue(), yAxis.getMaxValue(), zAxis.getMaxValue()));
const vec3 xAxis(worldPoint1.x(), worldPoint2.x(), worldPoint3.x());
const vec3 yAxis(worldPoint1.y(), worldPoint2.y(), worldPoint3.y());
const vec3 zAxis(worldPoint1.z(), worldPoint2.z(), worldPoint3.z());
aabb.setMin(vec3(xAxis.getMin(), yAxis.getMin(), zAxis.getMin()));
aabb.setMax(vec3(xAxis.getMax(), yAxis.getMax(), zAxis.getMax()));
}
// Return true if a point is inside the collision shape
inline bool TriangleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
inline bool TriangleShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const {
return false;
}
@ -192,7 +192,7 @@ inline void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) {
/**
* @param index Index (0 to 2) of a vertex of the triangle
*/
inline Vector3 TriangleShape::getVertex(int32_t index) const {
inline vec3 TriangleShape::getVertex(int32_t index) const {
assert(index >= 0 && index < 3);
return mPoints[index];
}

6
ephysics/configuration.h
View File

@ -53,10 +53,10 @@ namespace reactphysics3d {
const float DEFAULT_FRICTION_COEFFICIENT = float(0.3);
/// Default bounciness factor for a rigid body
const float DEFAULT_BOUNCINESS = float(0.5);
const float DEFAULT_BOUNCINESS = 0.5f;
/// Default rolling resistance
const float DEFAULT_ROLLING_RESISTANCE = float(0.0);
const float DEFAULT_ROLLING_RESISTANCE = 0.0f;
/// True if the spleeping technique is enabled
const bool SPLEEPING_ENABLED = true;
@ -68,7 +68,7 @@ namespace reactphysics3d {
const float PERSISTENT_CONTACT_DIST_THRESHOLD = float(0.03);
/// Velocity threshold for contact velocity restitution
const float RESTITUTION_VELOCITY_THRESHOLD = float(1.0);
const float RESTITUTION_VELOCITY_THRESHOLD = 1.0f;
/// Number of iterations when solving the velocity constraints of the Sequential Impulse technique
const uint32_t DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 10;

92
ephysics/constraint/BallAndSocketJoint.cpp
View File

@ -15,7 +15,7 @@ const float BallAndSocketJoint::BETA = float(0.2);
// Constructor
BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo)
: Joint(jointInfo), m_impulse(Vector3(0, 0, 0)) {
: Joint(jointInfo), m_impulse(vec3(0, 0, 0)) {
// Compute the local-space anchor point for each body
m_localAnchorPointBody1 = m_body1->getTransform().getInverse() * jointInfo.m_anchorPointWorldSpace;
@ -35,10 +35,10 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
m_indexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(m_body2)->second;
// Get the bodies center of mass and orientations
const Vector3& x1 = m_body1->m_centerOfMassWorld;
const Vector3& x2 = m_body2->m_centerOfMassWorld;
const Quaternion& orientationBody1 = m_body1->getTransform().getOrientation();
const Quaternion& orientationBody2 = m_body2->getTransform().getOrientation();
const vec3& x1 = m_body1->m_centerOfMassWorld;
const vec3& x2 = m_body2->m_centerOfMassWorld;
const etk::Quaternion& orientationBody1 = m_body1->getTransform().getOrientation();
const etk::Quaternion& orientationBody2 = m_body2->getTransform().getOrientation();
// Get the inertia tensor of bodies
m_i1 = m_body1->getInertiaTensorInverseWorld();
@ -49,25 +49,25 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
m_r2World = orientationBody2 * m_localAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix)
float inverseMassBodies = m_body1->m_massInverse + m_body2->m_massInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose();
// Compute the inverse mass matrix K^-1
m_inverseMassMatrix.setToZero();
m_inverseMassMatrix.setZero();
if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) {
m_inverseMassMatrix = massMatrix.getInverse();
}
// Compute the bias "b" of the constraint
m_biasVector.setToZero();
m_biasVector.setZero();
if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) {
float biasFactor = (BETA / constraintSolverData.timeStep);
m_biasVector = biasFactor * (x2 + m_r2World - x1 - m_r1World);
@ -77,7 +77,7 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
if (!constraintSolverData.isWarmStartingActive) {
// Reset the accumulated impulse
m_impulse.setToZero();
m_impulse.setZero();
}
}
@ -85,21 +85,21 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[m_indexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[m_indexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[m_indexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[m_indexBody2];
vec3& v1 = constraintSolverData.linearVelocities[m_indexBody1];
vec3& v2 = constraintSolverData.linearVelocities[m_indexBody2];
vec3& w1 = constraintSolverData.angularVelocities[m_indexBody1];
vec3& w2 = constraintSolverData.angularVelocities[m_indexBody2];
// Compute the impulse P=J^T * lambda for the body 1
const Vector3 linearImpulseBody1 = -m_impulse;
const Vector3 angularImpulseBody1 = m_impulse.cross(m_r1World);
const vec3 linearImpulseBody1 = -m_impulse;
const vec3 angularImpulseBody1 = m_impulse.cross(m_r1World);
// Apply the impulse to the body 1
v1 += m_body1->m_massInverse * linearImpulseBody1;
w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the body 2
const Vector3 angularImpulseBody2 = -m_impulse.cross(m_r2World);
const vec3 angularImpulseBody2 = -m_impulse.cross(m_r2World);
// Apply the impulse to the body to the body 2
v2 += m_body2->m_massInverse * m_impulse;
@ -110,28 +110,28 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD
void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[m_indexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[m_indexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[m_indexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[m_indexBody2];
vec3& v1 = constraintSolverData.linearVelocities[m_indexBody1];
vec3& v2 = constraintSolverData.linearVelocities[m_indexBody2];
vec3& w1 = constraintSolverData.angularVelocities[m_indexBody1];
vec3& w2 = constraintSolverData.angularVelocities[m_indexBody2];
// Compute J*v
const Vector3 Jv = v2 + w2.cross(m_r2World) - v1 - w1.cross(m_r1World);
const vec3 Jv = v2 + w2.cross(m_r2World) - v1 - w1.cross(m_r1World);
// Compute the Lagrange multiplier lambda
const Vector3 deltaLambda = m_inverseMassMatrix * (-Jv - m_biasVector);
const vec3 deltaLambda = m_inverseMassMatrix * (-Jv - m_biasVector);
m_impulse += deltaLambda;
// Compute the impulse P=J^T * lambda for the body 1
const Vector3 linearImpulseBody1 = -deltaLambda;
const Vector3 angularImpulseBody1 = deltaLambda.cross(m_r1World);
const vec3 linearImpulseBody1 = -deltaLambda;
const vec3 angularImpulseBody1 = deltaLambda.cross(m_r1World);
// Apply the impulse to the body 1
v1 += m_body1->m_massInverse * linearImpulseBody1;
w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the body 2
const Vector3 angularImpulseBody2 = -deltaLambda.cross(m_r2World);
const vec3 angularImpulseBody2 = -deltaLambda.cross(m_r2World);
// Apply the impulse to the body 2
v2 += m_body2->m_massInverse * deltaLambda;
@ -146,10 +146,10 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
if (m_positionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies center of mass and orientations
Vector3& x1 = constraintSolverData.positions[m_indexBody1];
Vector3& x2 = constraintSolverData.positions[m_indexBody2];
Quaternion& q1 = constraintSolverData.orientations[m_indexBody1];
Quaternion& q2 = constraintSolverData.orientations[m_indexBody2];
vec3& x1 = constraintSolverData.positions[m_indexBody1];
vec3& x2 = constraintSolverData.positions[m_indexBody2];
etk::Quaternion& q1 = constraintSolverData.orientations[m_indexBody1];
etk::Quaternion& q2 = constraintSolverData.orientations[m_indexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
float inverseMassBody1 = m_body1->m_massInverse;
@ -164,52 +164,52 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
m_r2World = q2 * m_localAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
// Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints
float inverseMassBodies = inverseMassBody1 + inverseMassBody2;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose();
m_inverseMassMatrix.setToZero();
m_inverseMassMatrix.setZero();
if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) {
m_inverseMassMatrix = massMatrix.getInverse();
}
// Compute the constraint error (value of the C(x) function)
const Vector3 constraintError = (x2 + m_r2World - x1 - m_r1World);
const vec3 constraintError = (x2 + m_r2World - x1 - m_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 = m_inverseMassMatrix * (-constraintError);
const vec3 lambda = m_inverseMassMatrix * (-constraintError);
// Compute the impulse of body 1
const Vector3 linearImpulseBody1 = -lambda;
const Vector3 angularImpulseBody1 = lambda.cross(m_r1World);
const vec3 linearImpulseBody1 = -lambda;
const vec3 angularImpulseBody1 = lambda.cross(m_r1World);
// Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = m_i1 * angularImpulseBody1;
const vec3 v1 = inverseMassBody1 * linearImpulseBody1;
const vec3 w1 = m_i1 * angularImpulseBody1;
// Update the body center of mass and orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * float(0.5);
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
q1.normalize();
// Compute the impulse of body 2
const Vector3 angularImpulseBody2 = -lambda.cross(m_r2World);
const vec3 angularImpulseBody2 = -lambda.cross(m_r2World);
// Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * lambda;
const Vector3 w2 = m_i2 * angularImpulseBody2;
const vec3 v2 = inverseMassBody2 * lambda;
const vec3 w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * float(0.5);
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
q2.normalize();
}

22
ephysics/constraint/BallAndSocketJoint.h
View File

@ -23,7 +23,7 @@ struct BallAndSocketJointInfo : public JointInfo {
// -------------------- Attributes -------------------- //
/// Anchor point (in world-space coordinates)
Vector3 m_anchorPointWorldSpace;
vec3 m_anchorPointWorldSpace;
/// Constructor
/**
@ -33,7 +33,7 @@ struct BallAndSocketJointInfo : public JointInfo {
* coordinates
*/
BallAndSocketJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace)
const vec3& initAnchorPointWorldSpace)
: JointInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT),
m_anchorPointWorldSpace(initAnchorPointWorldSpace) {}
};
@ -56,31 +56,31 @@ class BallAndSocketJoint : public Joint {
// -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local-space coordinates of body 1)
Vector3 m_localAnchorPointBody1;
vec3 m_localAnchorPointBody1;
/// Anchor point of body 2 (in local-space coordinates of body 2)
Vector3 m_localAnchorPointBody2;
vec3 m_localAnchorPointBody2;
/// Vector from center of body 2 to anchor point in world-space
Vector3 m_r1World;
vec3 m_r1World;
/// Vector from center of body 2 to anchor point in world-space
Vector3 m_r2World;
vec3 m_r2World;
/// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 m_i1;
etk::Matrix3x3 m_i1;
/// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 m_i2;
etk::Matrix3x3 m_i2;
/// Bias vector for the constraint
Vector3 m_biasVector;
vec3 m_biasVector;
/// Inverse mass matrix K=JM^-1J^-t of the constraint
Matrix3x3 m_inverseMassMatrix;
etk::Matrix3x3 m_inverseMassMatrix;
/// Accumulated impulse
Vector3 m_impulse;
vec3 m_impulse;
// -------------------- Methods -------------------- //

4
ephysics/constraint/ContactPoint.cpp
View File

@ -26,8 +26,8 @@ ContactPoint::ContactPoint(const ContactPointInfo& contactInfo)
contactInfo.localPoint2),
mIsRestingContact(false) {
m_frictionVectors[0] = Vector3(0, 0, 0);
m_frictionVectors[1] = Vector3(0, 0, 0);
m_frictionVectors[0] = vec3(0, 0, 0);
m_frictionVectors[1] = vec3(0, 0, 0);
assert(mPenetrationDepth > 0.0);

78
ephysics/constraint/ContactPoint.h
View File

@ -45,23 +45,23 @@ struct ContactPointInfo {
const CollisionShape* collisionShape2;
/// Normalized normal vector of the collision contact in world space
Vector3 normal;
vec3 normal;
/// Penetration depth of the contact
float penetrationDepth;
/// Contact point of body 1 in local space of body 1
Vector3 localPoint1;
vec3 localPoint1;
/// Contact point of body 2 in local space of body 2
Vector3 localPoint2;
vec3 localPoint2;
// -------------------- Methods -------------------- //
/// Constructor
ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, const CollisionShape* collShape1,
const CollisionShape* collShape2, const Vector3& normal, float penetrationDepth,
const Vector3& localPoint1, const Vector3& localPoint2)
const CollisionShape* collShape2, const vec3& normal, float penetrationDepth,
const vec3& localPoint1, const vec3& localPoint2)
: shape1(proxyShape1), shape2(proxyShape2), collisionShape1(collShape1), collisionShape2(collShape2),
normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1),
localPoint2(localPoint2) {
@ -87,28 +87,28 @@ class ContactPoint {
CollisionBody* m_body2;
/// Normalized normal vector of the contact (from body1 toward body2) in world space
const Vector3 mNormal;
const vec3 mNormal;
/// Penetration depth
float mPenetrationDepth;
/// Contact point on body 1 in local space of body 1
const Vector3 mLocalPointOnBody1;
const vec3 mLocalPointOnBody1;
/// Contact point on body 2 in local space of body 2
const Vector3 mLocalPointOnBody2;
const vec3 mLocalPointOnBody2;
/// Contact point on body 1 in world space
Vector3 m_worldPointOnBody1;
vec3 m_worldPointOnBody1;
/// Contact point on body 2 in world space
Vector3 m_worldPointOnBody2;
vec3 m_worldPointOnBody2;
/// True if the contact is a resting contact (exists for more than one time step)
bool mIsRestingContact;
/// Two orthogonal vectors that span the tangential friction plane
Vector3 m_frictionVectors[2];
vec3 m_frictionVectors[2];
/// Cached penetration impulse
float mPenetrationImpulse;
@ -120,7 +120,7 @@ class ContactPoint {
float m_frictionImpulse2;
/// Cached rolling resistance impulse
Vector3 m_rollingResistanceImpulse;
vec3 m_rollingResistanceImpulse;
// -------------------- Methods -------------------- //
@ -147,22 +147,22 @@ class ContactPoint {
CollisionBody* getBody2() const;
/// Return the normal vector of the contact
Vector3 getNormal() const;
vec3 getNormal() const;
/// Set the penetration depth of the contact
void setPenetrationDepth(float penetrationDepth);
/// Return the contact local point on body 1
Vector3 getLocalPointOnBody1() const;
vec3 getLocalPointOnBody1() const;
/// Return the contact local point on body 2
Vector3 getLocalPointOnBody2() const;
vec3 getLocalPointOnBody2() const;
/// Return the contact world point on body 1
Vector3 getWorldPointOnBody1() const;
vec3 getWorldPointOnBody1() const;
/// Return the contact world point on body 2
Vector3 getWorldPointOnBody2() const;
vec3 getWorldPointOnBody2() const;
/// Return the cached penetration impulse
float getPenetrationImpulse() const;
@ -174,7 +174,7 @@ class ContactPoint {
float getFrictionImpulse2() const;
/// Return the cached rolling resistance impulse
Vector3 getRollingResistanceImpulse() const;
vec3 getRollingResistanceImpulse() const;
/// Set the cached penetration impulse
void setPenetrationImpulse(float impulse);
@ -186,13 +186,13 @@ class ContactPoint {
void setFrictionImpulse2(float impulse);
/// Set the cached rolling resistance impulse
void setRollingResistanceImpulse(const Vector3& impulse);
void setRollingResistanceImpulse(const vec3& impulse);
/// Set the contact world point on body 1
void setWorldPointOnBody1(const Vector3& worldPoint);
void setWorldPointOnBody1(const vec3& worldPoint);
/// Set the contact world point on body 2
void setWorldPointOnBody2(const Vector3& worldPoint);
void setWorldPointOnBody2(const vec3& worldPoint);
/// Return true if the contact is a resting contact
bool getIsRestingContact() const;
@ -201,16 +201,16 @@ class ContactPoint {
void setIsRestingContact(bool isRestingContact);
/// Get the first friction vector
Vector3 getFrictionVector1() const;
vec3 getFrictionVector1() const;
/// Set the first friction vector
void setFrictionVector1(const Vector3& frictionVector1);
void setFrictionVector1(const vec3& frictionVector1);
/// Get the second friction vector
Vector3 getFrictionVector2() const;
vec3 getFrictionvec2() const;
/// Set the second friction vector
void setFrictionVector2(const Vector3& frictionVector2);
void setFrictionvec2(const vec3& frictionvec2);
/// Return the penetration depth
float getPenetrationDepth() const;
@ -230,7 +230,7 @@ inline CollisionBody* ContactPoint::getBody2() const {
}
// Return the normal vector of the contact
inline Vector3 ContactPoint::getNormal() const {
inline vec3 ContactPoint::getNormal() const {
return mNormal;
}
@ -240,22 +240,22 @@ inline void ContactPoint::setPenetrationDepth(float penetrationDepth) {
}
// Return the contact point on body 1
inline Vector3 ContactPoint::getLocalPointOnBody1() const {
inline vec3 ContactPoint::getLocalPointOnBody1() const {
return mLocalPointOnBody1;
}
// Return the contact point on body 2
inline Vector3 ContactPoint::getLocalPointOnBody2() const {
inline vec3 ContactPoint::getLocalPointOnBody2() const {
return mLocalPointOnBody2;
}
// Return the contact world point on body 1
inline Vector3 ContactPoint::getWorldPointOnBody1() const {
inline vec3 ContactPoint::getWorldPointOnBody1() const {
return m_worldPointOnBody1;
}
// Return the contact world point on body 2
inline Vector3 ContactPoint::getWorldPointOnBody2() const {
inline vec3 ContactPoint::getWorldPointOnBody2() const {
return m_worldPointOnBody2;
}
@ -275,7 +275,7 @@ inline float ContactPoint::getFrictionImpulse2() const {
}
// Return the cached rolling resistance impulse
inline Vector3 ContactPoint::getRollingResistanceImpulse() const {
inline vec3 ContactPoint::getRollingResistanceImpulse() const {
return m_rollingResistanceImpulse;
}
@ -295,17 +295,17 @@ inline void ContactPoint::setFrictionImpulse2(float impulse) {
}
// Set the cached rolling resistance impulse
inline void ContactPoint::setRollingResistanceImpulse(const Vector3& impulse) {
inline void ContactPoint::setRollingResistanceImpulse(const vec3& impulse) {
m_rollingResistanceImpulse = impulse;
}
// Set the contact world point on body 1
inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) {
inline void ContactPoint::setWorldPointOnBody1(const vec3& worldPoint) {
m_worldPointOnBody1 = worldPoint;
}
// Set the contact world point on body 2
inline void ContactPoint::setWorldPointOnBody2(const Vector3& worldPoint) {
inline void ContactPoint::setWorldPointOnBody2(const vec3& worldPoint) {
m_worldPointOnBody2 = worldPoint;
}
@ -320,23 +320,23 @@ inline void ContactPoint::setIsRestingContact(bool isRestingContact) {
}
// Get the first friction vector
inline Vector3 ContactPoint::getFrictionVector1() const {
inline vec3 ContactPoint::getFrictionVector1() const {
return m_frictionVectors[0];
}
// Set the first friction vector
inline void ContactPoint::setFrictionVector1(const Vector3& frictionVector1) {
inline void ContactPoint::setFrictionVector1(const vec3& frictionVector1) {
m_frictionVectors[0] = frictionVector1;
}
// Get the second friction vector
inline Vector3 ContactPoint::getFrictionVector2() const {
inline vec3 ContactPoint::getFrictionvec2() const {
return m_frictionVectors[1];
}
// Set the second friction vector
inline void ContactPoint::setFrictionVector2(const Vector3& frictionVector2) {
m_frictionVectors[1] = frictionVector2;
inline void ContactPoint::setFrictionvec2(const vec3& frictionvec2) {
m_frictionVectors[1] = frictionvec2;
}
// Return the penetration depth of the contact

134
ephysics/constraint/FixedJoint.cpp
View File

@ -18,8 +18,8 @@ FixedJoint::FixedJoint(const FixedJointInfo& jointInfo)
: Joint(jointInfo), m_impulseTranslation(0, 0, 0), m_impulseRotation(0, 0, 0) {
// Compute the local-space anchor point for each body
const Transform& transform1 = m_body1->getTransform();
const Transform& transform2 = m_body2->getTransform();
const etk::Transform3D& transform1 = m_body1->getTransform();
const etk::Transform3D& transform2 = m_body2->getTransform();
m_localAnchorPointBody1 = transform1.getInverse() * jointInfo.m_anchorPointWorldSpace;
m_localAnchorPointBody2 = transform2.getInverse() * jointInfo.m_anchorPointWorldSpace;
@ -43,10 +43,10 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
m_indexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(m_body2)->second;
// Get the bodies positions and orientations
const Vector3& x1 = m_body1->m_centerOfMassWorld;
const Vector3& x2 = m_body2->m_centerOfMassWorld;
const Quaternion& orientationBody1 = m_body1->getTransform().getOrientation();
const Quaternion& orientationBody2 = m_body2->getTransform().getOrientation();
const vec3& x1 = m_body1->m_centerOfMassWorld;
const vec3& x2 = m_body2->m_centerOfMassWorld;
const etk::Quaternion& orientationBody1 = m_body1->getTransform().getOrientation();
const etk::Quaternion& orientationBody2 = m_body2->getTransform().getOrientation();
// Get the inertia tensor of bodies
m_i1 = m_body1->getInertiaTensorInverseWorld();
@ -57,26 +57,26 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
m_r2World = orientationBody2 * m_localAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
float inverseMassBodies = m_body1->m_massInverse + m_body2->m_massInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose();
etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies)
+ skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose()
+ skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose();
// Compute the inverse mass matrix K^-1 for the 3 translation constraints
m_inverseMassMatrixTranslation.setToZero();
m_inverseMassMatrixTranslation.setZero();
if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) {
m_inverseMassMatrixTranslation = massMatrix.getInverse();
}
// Compute the bias "b" of the constraint for the 3 translation constraints
float biasFactor = (BETA / constraintSolverData.timeStep);
m_biasTranslation.setToZero();
m_biasTranslation.setZero();
if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) {
m_biasTranslation = biasFactor * (x2 + m_r2World - x1 - m_r1World);
}
@ -89,11 +89,11 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
}
// Compute the bias "b" for the 3 rotation constraints
m_biasRotation.setToZero();
m_biasRotation.setZero();
if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) {
Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
etk::Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * m_initOrientationDifferenceInv;
const etk::Quaternion qError = currentOrientationDifference * m_initOrientationDifferenceInv;
m_biasRotation = biasFactor * float(2.0) * qError.getVectorV();
}
@ -101,8 +101,8 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
if (!constraintSolverData.isWarmStartingActive) {
// Reset the accumulated impulses
m_impulseTranslation.setToZero();
m_impulseRotation.setToZero();
m_impulseTranslation.setZero();
m_impulseRotation.setZero();
}
}
@ -110,18 +110,18 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[m_indexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[m_indexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[m_indexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[m_indexBody2];
vec3& v1 = constraintSolverData.linearVelocities[m_indexBody1];
vec3& v2 = constraintSolverData.linearVelocities[m_indexBody2];
vec3& w1 = constraintSolverData.angularVelocities[m_indexBody1];
vec3& w2 = constraintSolverData.angularVelocities[m_indexBody2];
// Get the inverse mass of the bodies
const float inverseMassBody1 = m_body1->m_massInverse;
const float inverseMassBody2 = m_body2->m_massInverse;
// Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1
Vector3 linearImpulseBody1 = -m_impulseTranslation;
Vector3 angularImpulseBody1 = m_impulseTranslation.cross(m_r1World);
vec3 linearImpulseBody1 = -m_impulseTranslation;
vec3 angularImpulseBody1 = m_impulseTranslation.cross(m_r1World);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1
angularImpulseBody1 += -m_impulseRotation;
@ -131,7 +131,7 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 3 translation constraints for body 2
Vector3 angularImpulseBody2 = -m_impulseTranslation.cross(m_r2World);
vec3 angularImpulseBody2 = -m_impulseTranslation.cross(m_r2World);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 2
angularImpulseBody2 += m_impulseRotation;
@ -145,10 +145,10 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[m_indexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[m_indexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[m_indexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[m_indexBody2];
vec3& v1 = constraintSolverData.linearVelocities[m_indexBody1];
vec3& v2 = constraintSolverData.linearVelocities[m_indexBody2];
vec3& w1 = constraintSolverData.angularVelocities[m_indexBody1];
vec3& w2 = constraintSolverData.angularVelocities[m_indexBody2];
// Get the inverse mass of the bodies
float inverseMassBody1 = m_body1->m_massInverse;
@ -157,23 +157,23 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
// --------------- Translation Constraints --------------- //
// Compute J*v for the 3 translation constraints
const Vector3 JvTranslation = v2 + w2.cross(m_r2World) - v1 - w1.cross(m_r1World);
const vec3 JvTranslation = v2 + w2.cross(m_r2World) - v1 - w1.cross(m_r1World);
// Compute the Lagrange multiplier lambda
const Vector3 deltaLambda = m_inverseMassMatrixTranslation *
const vec3 deltaLambda = m_inverseMassMatrixTranslation *
(-JvTranslation - m_biasTranslation);
m_impulseTranslation += deltaLambda;
// Compute the impulse P=J^T * lambda for body 1
const Vector3 linearImpulseBody1 = -deltaLambda;
Vector3 angularImpulseBody1 = deltaLambda.cross(m_r1World);
const vec3 linearImpulseBody1 = -deltaLambda;
vec3 angularImpulseBody1 = deltaLambda.cross(m_r1World);
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for body 2
const Vector3 angularImpulseBody2 = -deltaLambda.cross(m_r2World);
const vec3 angularImpulseBody2 = -deltaLambda.cross(m_r2World);
// Apply the impulse to the body 2
v2 += inverseMassBody2 * deltaLambda;
@ -182,10 +182,10 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
// --------------- Rotation Constraints --------------- //
// Compute J*v for the 3 rotation constraints
const Vector3 JvRotation = w2 - w1;
const vec3 JvRotation = w2 - w1;
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 deltaLambda2 = m_inverseMassMatrixRotation * (-JvRotation - m_biasRotation);
vec3 deltaLambda2 = m_inverseMassMatrixRotation * (-JvRotation - m_biasRotation);
m_impulseRotation += deltaLambda2;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1
@ -206,10 +206,10 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
if (m_positionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
Vector3& x1 = constraintSolverData.positions[m_indexBody1];
Vector3& x2 = constraintSolverData.positions[m_indexBody2];
Quaternion& q1 = constraintSolverData.orientations[m_indexBody1];
Quaternion& q2 = constraintSolverData.orientations[m_indexBody2];
vec3& x1 = constraintSolverData.positions[m_indexBody1];
vec3& x2 = constraintSolverData.positions[m_indexBody2];
etk::Quaternion& q1 = constraintSolverData.orientations[m_indexBody1];
etk::Quaternion& q2 = constraintSolverData.orientations[m_indexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
float inverseMassBody1 = m_body1->m_massInverse;
@ -224,52 +224,52 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
m_r2World = q2 * m_localAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
// --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
float inverseMassBodies = m_body1->m_massInverse + m_body2->m_massInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose();
m_inverseMassMatrixTranslation.setToZero();
etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies)
+ skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose()
+ skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose();
m_inverseMassMatrixTranslation.setZero();
if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) {
m_inverseMassMatrixTranslation = massMatrix.getInverse();
}
// Compute position error for the 3 translation constraints
const Vector3 errorTranslation = x2 + m_r2World - x1 - m_r1World;
const vec3 errorTranslation = x2 + m_r2World - x1 - m_r1World;
// Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = m_inverseMassMatrixTranslation * (-errorTranslation);
const vec3 lambdaTranslation = m_inverseMassMatrixTranslation * (-errorTranslation);
// Compute the impulse of body 1
Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(m_r1World);
vec3 linearImpulseBody1 = -lambdaTranslation;
vec3 angularImpulseBody1 = lambdaTranslation.cross(m_r1World);
// Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
Vector3 w1 = m_i1 * angularImpulseBody1;
const vec3 v1 = inverseMassBody1 * linearImpulseBody1;
vec3 w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * float(0.5);
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
q1.normalize();
// Compute the impulse of body 2
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(m_r2World);
vec3 angularImpulseBody2 = -lambdaTranslation.cross(m_r2World);
// Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * lambdaTranslation;
Vector3 w2 = m_i2 * angularImpulseBody2;
const vec3 v2 = inverseMassBody2 * lambdaTranslation;
vec3 w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * float(0.5);
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
q2.normalize();
// --------------- Rotation Constraints --------------- //
@ -282,13 +282,13 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
}
// Compute the position error for the 3 rotation constraints
Quaternion currentOrientationDifference = q2 * q1.getInverse();
etk::Quaternion currentOrientationDifference = q2 * q1.getInverse();
currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * m_initOrientationDifferenceInv;
const Vector3 errorRotation = float(2.0) * qError.getVectorV();
const etk::Quaternion qError = currentOrientationDifference * m_initOrientationDifferenceInv;
const vec3 errorRotation = float(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = m_inverseMassMatrixRotation * (-errorRotation);
vec3 lambdaRotation = m_inverseMassMatrixRotation * (-errorRotation);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 = -lambdaRotation;
@ -297,14 +297,14 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * float(0.5);
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
q1.normalize();
// Compute the pseudo velocity of body 2
w2 = m_i2 * lambdaRotation;
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * float(0.5);
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
q2.normalize();
}

30
ephysics/constraint/FixedJoint.h
View File

@ -23,7 +23,7 @@ struct FixedJointInfo : public JointInfo {
// -------------------- Attributes -------------------- //
/// Anchor point (in world-space coordinates)
Vector3 m_anchorPointWorldSpace;
vec3 m_anchorPointWorldSpace;
/// Constructor
/**
@ -33,7 +33,7 @@ struct FixedJointInfo : public JointInfo {
* world-space coordinates
*/
FixedJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace)
const vec3& initAnchorPointWorldSpace)
: JointInfo(rigidBody1, rigidBody2, FIXEDJOINT),
m_anchorPointWorldSpace(initAnchorPointWorldSpace){}
};
@ -55,43 +55,43 @@ class FixedJoint : public Joint {
// -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local-space coordinates of body 1)
Vector3 m_localAnchorPointBody1;
vec3 m_localAnchorPointBody1;
/// Anchor point of body 2 (in local-space coordinates of body 2)
Vector3 m_localAnchorPointBody2;
vec3 m_localAnchorPointBody2;
/// Vector from center of body 2 to anchor point in world-space
Vector3 m_r1World;
vec3 m_r1World;
/// Vector from center of body 2 to anchor point in world-space
Vector3 m_r2World;
vec3 m_r2World;
/// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 m_i1;
etk::Matrix3x3 m_i1;
/// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 m_i2;
etk::Matrix3x3 m_i2;
/// Accumulated impulse for the 3 translation constraints
Vector3 m_impulseTranslation;
vec3 m_impulseTranslation;
/// Accumulate impulse for the 3 rotation constraints
Vector3 m_impulseRotation;
vec3 m_impulseRotation;
/// Inverse mass matrix K=JM^-1J^-t of the 3 translation constraints (3x3 matrix)
Matrix3x3 m_inverseMassMatrixTranslation;
etk::Matrix3x3 m_inverseMassMatrixTranslation;
/// Inverse mass matrix K=JM^-1J^-t of the 3 rotation constraints (3x3 matrix)
Matrix3x3 m_inverseMassMatrixRotation;
etk::Matrix3x3 m_inverseMassMatrixRotation;
/// Bias vector for the 3 translation constraints
Vector3 m_biasTranslation;
vec3 m_biasTranslation;
/// Bias vector for the 3 rotation constraints
Vector3 m_biasRotation;
vec3 m_biasRotation;
/// Inverse of the initial orientation difference between the two bodies
Quaternion m_initOrientationDifferenceInv;
etk::Quaternion m_initOrientationDifferenceInv;
// -------------------- Methods -------------------- //

220
ephysics/constraint/HingeJoint.cpp
View File

@ -27,8 +27,8 @@ HingeJoint::HingeJoint(const HingeJointInfo& jointInfo)
assert(mUpperLimit >= 0 && mUpperLimit <= 2.0 * PI);
// Compute the local-space anchor point for each body
Transform transform1 = m_body1->getTransform();
Transform transform2 = m_body2->getTransform();
etk::Transform3D transform1 = m_body1->getTransform();
etk::Transform3D transform2 = m_body2->getTransform();
m_localAnchorPointBody1 = transform1.getInverse() * jointInfo.m_anchorPointWorldSpace;
m_localAnchorPointBody2 = transform2.getInverse() * jointInfo.m_anchorPointWorldSpace;
@ -58,10 +58,10 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
m_indexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(m_body2)->second;
// Get the bodies positions and orientations
const Vector3& x1 = m_body1->m_centerOfMassWorld;
const Vector3& x2 = m_body2->m_centerOfMassWorld;
const Quaternion& orientationBody1 = m_body1->getTransform().getOrientation();
const Quaternion& orientationBody2 = m_body2->getTransform().getOrientation();
const vec3& x1 = m_body1->m_centerOfMassWorld;
const vec3& x2 = m_body2->m_centerOfMassWorld;
const etk::Quaternion& orientationBody1 = m_body1->getTransform().getOrientation();
const etk::Quaternion& orientationBody2 = m_body2->getTransform().getOrientation();
// Get the inertia tensor of bodies
m_i1 = m_body1->getInertiaTensorInverseWorld();
@ -90,42 +90,42 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Compute vectors needed in the Jacobian
mA1 = orientationBody1 * mHingeLocalAxisBody1;
Vector3 a2 = orientationBody2 * mHingeLocalAxisBody2;
vec3 a2 = orientationBody2 * mHingeLocalAxisBody2;
mA1.normalize();
a2.normalize();
const Vector3 b2 = a2.getOneUnitOrthogonalVector();
const Vector3 c2 = a2.cross(b2);
const vec3 b2 = a2.getOneUnitOrthogonalVector();
const vec3 c2 = a2.cross(b2);
mB2CrossA1 = b2.cross(mA1);
mC2CrossA1 = c2.cross(mA1);
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix)
float inverseMassBodies = m_body1->m_massInverse + m_body2->m_massInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
etk::Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose();
m_inverseMassMatrixTranslation.setToZero();
m_inverseMassMatrixTranslation.setZero();
if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) {
m_inverseMassMatrixTranslation = massMatrix.getInverse();
}
// Compute the bias "b" of the translation constraints
mBTranslation.setToZero();
mBTranslation.setZero();
float biasFactor = (BETA / constraintSolverData.timeStep);
if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBTranslation = biasFactor * (x2 + m_r2World - x1 - m_r1World);
}
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
Vector3 I1B2CrossA1 = m_i1 * mB2CrossA1;
Vector3 I1C2CrossA1 = m_i1 * mC2CrossA1;
Vector3 I2B2CrossA1 = m_i2 * mB2CrossA1;
Vector3 I2C2CrossA1 = m_i2 * mC2CrossA1;
vec3 I1B2CrossA1 = m_i1 * mB2CrossA1;
vec3 I1C2CrossA1 = m_i1 * mC2CrossA1;
vec3 I2B2CrossA1 = m_i2 * mB2CrossA1;
vec3 I2C2CrossA1 = m_i2 * mC2CrossA1;
const float el11 = mB2CrossA1.dot(I1B2CrossA1) +
mB2CrossA1.dot(I2B2CrossA1);
const float el12 = mB2CrossA1.dot(I1C2CrossA1) +
@ -134,24 +134,24 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
mC2CrossA1.dot(I2B2CrossA1);
const float el22 = mC2CrossA1.dot(I1C2CrossA1) +
mC2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
m_inverseMassMatrixRotation.setToZero();
const etk::Matrix2x2 matrixKRotation(el11, el12, el21, el22);
m_inverseMassMatrixRotation.setZero();
if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) {
m_inverseMassMatrixRotation = matrixKRotation.getInverse();
}
// Compute the bias "b" of the rotation constraints
mBRotation.setToZero();
mBRotation.setZero();
if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBRotation = biasFactor * Vector2(mA1.dot(b2), mA1.dot(c2));
mBRotation = biasFactor * vec2(mA1.dot(b2), mA1.dot(c2));
}
// If warm-starting is not enabled
if (!constraintSolverData.isWarmStartingActive) {
// Reset all the accumulated impulses
m_impulseTranslation.setToZero();
m_impulseRotation.setToZero();
m_impulseTranslation.setZero();
m_impulseRotation.setZero();
m_impulseLowerLimit = 0.0;
m_impulseUpperLimit = 0.0;
m_impulseMotor = 0.0;
@ -163,7 +163,7 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix)
m_inverseMassMatrixLimitMotor = mA1.dot(m_i1 * mA1) + mA1.dot(m_i2 * mA1);
m_inverseMassMatrixLimitMotor = (m_inverseMassMatrixLimitMotor > 0.0) ?
float(1.0) / m_inverseMassMatrixLimitMotor : float(0.0);
1.0f / m_inverseMassMatrixLimitMotor : 0.0f;
if (mIsLimitEnabled) {
@ -186,27 +186,27 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[m_indexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[m_indexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[m_indexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[m_indexBody2];
vec3& v1 = constraintSolverData.linearVelocities[m_indexBody1];
vec3& v2 = constraintSolverData.linearVelocities[m_indexBody2];
vec3& w1 = constraintSolverData.angularVelocities[m_indexBody1];
vec3& w2 = constraintSolverData.angularVelocities[m_indexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
const float inverseMassBody1 = m_body1->m_massInverse;
const float inverseMassBody2 = m_body2->m_massInverse;
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
Vector3 rotationImpulse = -mB2CrossA1 * m_impulseRotation.x - mC2CrossA1 * m_impulseRotation.y;
vec3 rotationImpulse = -mB2CrossA1 * m_impulseRotation.x() - mC2CrossA1 * m_impulseRotation.y();
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
const Vector3 limitsImpulse = (m_impulseUpperLimit - m_impulseLowerLimit) * mA1;
const vec3 limitsImpulse = (m_impulseUpperLimit - m_impulseLowerLimit) * mA1;
// Compute the impulse P=J^T * lambda for the motor constraint
const Vector3 motorImpulse = -m_impulseMotor * mA1;
const vec3 motorImpulse = -m_impulseMotor * mA1;
// Compute the impulse P=J^T * lambda for the 3 translation constraints of body 1
Vector3 linearImpulseBody1 = -m_impulseTranslation;
Vector3 angularImpulseBody1 = m_impulseTranslation.cross(m_r1World);
vec3 linearImpulseBody1 = -m_impulseTranslation;
vec3 angularImpulseBody1 = m_impulseTranslation.cross(m_r1World);
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1
angularImpulseBody1 += rotationImpulse;
@ -222,7 +222,7 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 3 translation constraints of body 2
Vector3 angularImpulseBody2 = -m_impulseTranslation.cross(m_r2World);
vec3 angularImpulseBody2 = -m_impulseTranslation.cross(m_r2World);
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2
angularImpulseBody2 += -rotationImpulse;
@ -242,10 +242,10 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[m_indexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[m_indexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[m_indexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[m_indexBody2];
vec3& v1 = constraintSolverData.linearVelocities[m_indexBody1];
vec3& v2 = constraintSolverData.linearVelocities[m_indexBody2];
vec3& w1 = constraintSolverData.angularVelocities[m_indexBody1];
vec3& w2 = constraintSolverData.angularVelocities[m_indexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
float inverseMassBody1 = m_body1->m_massInverse;
@ -254,23 +254,23 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
// --------------- Translation Constraints --------------- //
// Compute J*v
const Vector3 JvTranslation = v2 + w2.cross(m_r2World) - v1 - w1.cross(m_r1World);
const vec3 JvTranslation = v2 + w2.cross(m_r2World) - v1 - w1.cross(m_r1World);
// Compute the Lagrange multiplier lambda
const Vector3 deltaLambdaTranslation = m_inverseMassMatrixTranslation *
const vec3 deltaLambdaTranslation = m_inverseMassMatrixTranslation *
(-JvTranslation - mBTranslation);
m_impulseTranslation += deltaLambdaTranslation;
// Compute the impulse P=J^T * lambda of body 1
const Vector3 linearImpulseBody1 = -deltaLambdaTranslation;
Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(m_r1World);
const vec3 linearImpulseBody1 = -deltaLambdaTranslation;
vec3 angularImpulseBody1 = deltaLambdaTranslation.cross(m_r1World);
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda of body 2
Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(m_r2World);
vec3 angularImpulseBody2 = -deltaLambdaTranslation.cross(m_r2World);
// Apply the impulse to the body 2
v2 += inverseMassBody2 * deltaLambdaTranslation;
@ -279,23 +279,23 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
// --------------- Rotation Constraints --------------- //
// Compute J*v for the 2 rotation constraints
const Vector2 JvRotation(-mB2CrossA1.dot(w1) + mB2CrossA1.dot(w2),
const vec2 JvRotation(-mB2CrossA1.dot(w1) + mB2CrossA1.dot(w2),
-mC2CrossA1.dot(w1) + mC2CrossA1.dot(w2));
// Compute the Lagrange multiplier lambda for the 2 rotation constraints
Vector2 deltaLambdaRotation = m_inverseMassMatrixRotation * (-JvRotation - mBRotation);
vec2 deltaLambdaRotation = m_inverseMassMatrixRotation * (-JvRotation - mBRotation);
m_impulseRotation += deltaLambdaRotation;
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1
angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x -
mC2CrossA1 * deltaLambdaRotation.y;
angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x() -
mC2CrossA1 * deltaLambdaRotation.y();
// Apply the impulse to the body 1
w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2
angularImpulseBody2 = mB2CrossA1 * deltaLambdaRotation.x +
mC2CrossA1 * deltaLambdaRotation.y;
angularImpulseBody2 = mB2CrossA1 * deltaLambdaRotation.x() +
mC2CrossA1 * deltaLambdaRotation.y();
// Apply the impulse to the body 2
w2 += m_i2 * angularImpulseBody2;
@ -313,17 +313,17 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
// Compute the Lagrange multiplier lambda for the lower limit constraint
float deltaLambdaLower = m_inverseMassMatrixLimitMotor * (-JvLowerLimit -mBLowerLimit);
float lambdaTemp = m_impulseLowerLimit;
m_impulseLowerLimit = std::max(m_impulseLowerLimit + deltaLambdaLower, float(0.0));
m_impulseLowerLimit = std::max(m_impulseLowerLimit + deltaLambdaLower, 0.0f);
deltaLambdaLower = m_impulseLowerLimit - lambdaTemp;
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
const Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1;
const vec3 angularImpulseBody1 = -deltaLambdaLower * mA1;
// Apply the impulse to the body 1
w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
const Vector3 angularImpulseBody2 = deltaLambdaLower * mA1;
const vec3 angularImpulseBody2 = deltaLambdaLower * mA1;
// Apply the impulse to the body 2
w2 += m_i2 * angularImpulseBody2;
@ -338,17 +338,17 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
// Compute the Lagrange multiplier lambda for the upper limit constraint
float deltaLambdaUpper = m_inverseMassMatrixLimitMotor * (-JvUpperLimit -mBUpperLimit);
float lambdaTemp = m_impulseUpperLimit;
m_impulseUpperLimit = std::max(m_impulseUpperLimit + deltaLambdaUpper, float(0.0));
m_impulseUpperLimit = std::max(m_impulseUpperLimit + deltaLambdaUpper, 0.0f);
deltaLambdaUpper = m_impulseUpperLimit - lambdaTemp;
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
const Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1;
const vec3 angularImpulseBody1 = deltaLambdaUpper * mA1;
// Apply the impulse to the body 1
w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 2
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mA1;
const vec3 angularImpulseBody2 = -deltaLambdaUpper * mA1;
// Apply the impulse to the body 2
w2 += m_i2 * angularImpulseBody2;
@ -371,13 +371,13 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
deltaLambdaMotor = m_impulseMotor - lambdaTemp;
// Compute the impulse P=J^T * lambda for the motor of body 1
const Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1;
const vec3 angularImpulseBody1 = -deltaLambdaMotor * mA1;
// Apply the impulse to the body 1
w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the motor of body 2
const Vector3 angularImpulseBody2 = deltaLambdaMotor * mA1;
const vec3 angularImpulseBody2 = deltaLambdaMotor * mA1;
// Apply the impulse to the body 2
w2 += m_i2 * angularImpulseBody2;
@ -392,10 +392,10 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
if (m_positionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
Vector3& x1 = constraintSolverData.positions[m_indexBody1];
Vector3& x2 = constraintSolverData.positions[m_indexBody2];
Quaternion& q1 = constraintSolverData.orientations[m_indexBody1];
Quaternion& q2 = constraintSolverData.orientations[m_indexBody2];
vec3& x1 = constraintSolverData.positions[m_indexBody1];
vec3& x2 = constraintSolverData.positions[m_indexBody2];
etk::Quaternion& q1 = constraintSolverData.orientations[m_indexBody1];
etk::Quaternion& q2 = constraintSolverData.orientations[m_indexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
float inverseMassBody1 = m_body1->m_massInverse;
@ -420,70 +420,70 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Compute vectors needed in the Jacobian
mA1 = q1 * mHingeLocalAxisBody1;
Vector3 a2 = q2 * mHingeLocalAxisBody2;
vec3 a2 = q2 * mHingeLocalAxisBody2;
mA1.normalize();
a2.normalize();
const Vector3 b2 = a2.getOneUnitOrthogonalVector();
const Vector3 c2 = a2.cross(b2);
const vec3 b2 = a2.getOneUnitOrthogonalVector();
const vec3 c2 = a2.cross(b2);
mB2CrossA1 = b2.cross(mA1);
mC2CrossA1 = c2.cross(mA1);
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
// --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
float inverseMassBodies = m_body1->m_massInverse + m_body2->m_massInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
etk::Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose();
m_inverseMassMatrixTranslation.setToZero();
m_inverseMassMatrixTranslation.setZero();
if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) {
m_inverseMassMatrixTranslation = massMatrix.getInverse();
}
// Compute position error for the 3 translation constraints
const Vector3 errorTranslation = x2 + m_r2World - x1 - m_r1World;
const vec3 errorTranslation = x2 + m_r2World - x1 - m_r1World;
// Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = m_inverseMassMatrixTranslation * (-errorTranslation);
const vec3 lambdaTranslation = m_inverseMassMatrixTranslation * (-errorTranslation);
// Compute the impulse of body 1
Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(m_r1World);
vec3 linearImpulseBody1 = -lambdaTranslation;
vec3 angularImpulseBody1 = lambdaTranslation.cross(m_r1World);
// Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
Vector3 w1 = m_i1 * angularImpulseBody1;
const vec3 v1 = inverseMassBody1 * linearImpulseBody1;
vec3 w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * float(0.5);
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
q1.normalize();
// Compute the impulse of body 2
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(m_r2World);
vec3 angularImpulseBody2 = -lambdaTranslation.cross(m_r2World);
// Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * lambdaTranslation;
Vector3 w2 = m_i2 * angularImpulseBody2;
const vec3 v2 = inverseMassBody2 * lambdaTranslation;
vec3 w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * float(0.5);
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
q2.normalize();
// --------------- Rotation Constraints --------------- //
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
Vector3 I1B2CrossA1 = m_i1 * mB2CrossA1;
Vector3 I1C2CrossA1 = m_i1 * mC2CrossA1;
Vector3 I2B2CrossA1 = m_i2 * mB2CrossA1;
Vector3 I2C2CrossA1 = m_i2 * mC2CrossA1;
vec3 I1B2CrossA1 = m_i1 * mB2CrossA1;
vec3 I1C2CrossA1 = m_i1 * mC2CrossA1;
vec3 I2B2CrossA1 = m_i2 * mB2CrossA1;
vec3 I2C2CrossA1 = m_i2 * mC2CrossA1;
const float el11 = mB2CrossA1.dot(I1B2CrossA1) +
mB2CrossA1.dot(I2B2CrossA1);
const float el12 = mB2CrossA1.dot(I1C2CrossA1) +
@ -492,36 +492,36 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
mC2CrossA1.dot(I2B2CrossA1);
const float el22 = mC2CrossA1.dot(I1C2CrossA1) +
mC2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
m_inverseMassMatrixRotation.setToZero();
const etk::Matrix2x2 matrixKRotation(el11, el12, el21, el22);
m_inverseMassMatrixRotation.setZero();
if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) {
m_inverseMassMatrixRotation = matrixKRotation.getInverse();
}
// Compute the position error for the 3 rotation constraints
const Vector2 errorRotation = Vector2(mA1.dot(b2), mA1.dot(c2));
const vec2 errorRotation = vec2(mA1.dot(b2), mA1.dot(c2));
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector2 lambdaRotation = m_inverseMassMatrixRotation * (-errorRotation);
vec2 lambdaRotation = m_inverseMassMatrixRotation * (-errorRotation);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 = -mB2CrossA1 * lambdaRotation.x - mC2CrossA1 * lambdaRotation.y;
angularImpulseBody1 = -mB2CrossA1 * lambdaRotation.x() - mC2CrossA1 * lambdaRotation.y();
// Compute the pseudo velocity of body 1
w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * float(0.5);
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
q1.normalize();
// Compute the impulse of body 2
angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x + mC2CrossA1 * lambdaRotation.y;
angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x() + mC2CrossA1 * lambdaRotation.y();
// Compute the pseudo velocity of body 2
w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * float(0.5);
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
q2.normalize();
// --------------- Limits Constraints --------------- //
@ -533,7 +533,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
m_inverseMassMatrixLimitMotor = mA1.dot(m_i1 * mA1) + mA1.dot(m_i2 * mA1);
m_inverseMassMatrixLimitMotor = (m_inverseMassMatrixLimitMotor > 0.0) ?
float(1.0) / m_inverseMassMatrixLimitMotor : float(0.0);
1.0f / m_inverseMassMatrixLimitMotor : 0.0f;
}
// If the lower limit is violated
@ -543,23 +543,23 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
float lambdaLowerLimit = m_inverseMassMatrixLimitMotor * (-lowerLimitError );
// Compute the impulse P=J^T * lambda of body 1
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1;
const vec3 angularImpulseBody1 = -lambdaLowerLimit * mA1;
// Compute the pseudo velocity of body 1
const Vector3 w1 = m_i1 * angularImpulseBody1;
const vec3 w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * float(0.5);
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
q1.normalize();
// Compute the impulse P=J^T * lambda of body 2
const Vector3 angularImpulseBody2 = lambdaLowerLimit * mA1;
const vec3 angularImpulseBody2 = lambdaLowerLimit * mA1;
// Compute the pseudo velocity of body 2
const Vector3 w2 = m_i2 * angularImpulseBody2;
const vec3 w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * float(0.5);
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
q2.normalize();
}
@ -570,23 +570,23 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
float lambdaUpperLimit = m_inverseMassMatrixLimitMotor * (-upperLimitError);
// Compute the impulse P=J^T * lambda of body 1
const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1;
const vec3 angularImpulseBody1 = lambdaUpperLimit * mA1;
// Compute the pseudo velocity of body 1
const Vector3 w1 = m_i1 * angularImpulseBody1;
const vec3 w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * float(0.5);
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
q1.normalize();
// Compute the impulse P=J^T * lambda of body 2
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mA1;
const vec3 angularImpulseBody2 = -lambdaUpperLimit * mA1;
// Compute the pseudo velocity of body 2
const Vector3 w2 = m_i2 * angularImpulseBody2;
const vec3 w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * float(0.5);
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
q2.normalize();
}
}
@ -742,17 +742,17 @@ float HingeJoint::computeCorrespondingAngleNearLimits(float inputAngle, float lo
}
// Compute the current angle around the hinge axis
float HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1,
const Quaternion& orientationBody2) {
float HingeJoint::computeCurrentHingeAngle(const etk::Quaternion& orientationBody1,
const etk::Quaternion& orientationBody2) {
float hingeAngle;
// Compute the current orientation difference between the two bodies
Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse();
etk::Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse();
currentOrientationDiff.normalize();
// Compute the relative rotation considering the initial orientation difference
Quaternion relativeRotation = currentOrientationDiff * m_initOrientationDifferenceInv;
etk::Quaternion relativeRotation = currentOrientationDiff * m_initOrientationDifferenceInv;
relativeRotation.normalize();
// A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit
@ -769,7 +769,7 @@ float HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1,
float dotProduct = relativeRotation.getVectorV().dot(mA1);
// If the relative rotation axis and the hinge axis are pointing the same direction
if (dotProduct >= float(0.0)) {
if (dotProduct >= 0.0f) {
hingeAngle = float(2.0) * std::atan2(sinHalfAngleAbs, cosHalfAngle);
}
else {

56
ephysics/constraint/HingeJoint.h
View File

@ -23,10 +23,10 @@ struct HingeJointInfo : public JointInfo {
// -------------------- Attributes -------------------- //
/// Anchor point (in world-space coordinates)
Vector3 m_anchorPointWorldSpace;
vec3 m_anchorPointWorldSpace;
/// Hinge rotation axis (in world-space coordinates)
Vector3 rotationAxisWorld;
vec3 rotationAxisWorld;
/// True if the hinge joint limits are enabled
bool isLimitEnabled;
@ -59,8 +59,8 @@ struct HingeJointInfo : public JointInfo {
* coordinates
*/
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace,
const Vector3& initRotationAxisWorld)
const vec3& initAnchorPointWorldSpace,
const vec3& initRotationAxisWorld)
: JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
m_anchorPointWorldSpace(initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(false),
@ -77,8 +77,8 @@ struct HingeJointInfo : public JointInfo {
* @param initMaxAngleLimit The initial maximum limit angle (in radian)
*/
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace,
const Vector3& initRotationAxisWorld,
const vec3& initAnchorPointWorldSpace,
const vec3& initRotationAxisWorld,
float initMinAngleLimit, float initMaxAngleLimit)
: JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
m_anchorPointWorldSpace(initAnchorPointWorldSpace),
@ -99,8 +99,8 @@ struct HingeJointInfo : public JointInfo {
* @param initMaxMotorTorque The initial maximum motor torque (in Newtons)
*/
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace,
const Vector3& initRotationAxisWorld,
const vec3& initAnchorPointWorldSpace,
const vec3& initRotationAxisWorld,
float initMinAngleLimit, float initMaxAngleLimit,
float initMotorSpeed, float initMaxMotorTorque)
: JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
@ -129,43 +129,43 @@ class HingeJoint : public Joint {
// -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local-space coordinates of body 1)
Vector3 m_localAnchorPointBody1;
vec3 m_localAnchorPointBody1;
/// Anchor point of body 2 (in local-space coordinates of body 2)
Vector3 m_localAnchorPointBody2;
vec3 m_localAnchorPointBody2;
/// Hinge rotation axis (in local-space coordinates of body 1)
Vector3 mHingeLocalAxisBody1;
vec3 mHingeLocalAxisBody1;
/// Hinge rotation axis (in local-space coordiantes of body 2)
Vector3 mHingeLocalAxisBody2;
vec3 mHingeLocalAxisBody2;
/// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 m_i1;
etk::Matrix3x3 m_i1;
/// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 m_i2;
etk::Matrix3x3 m_i2;
/// Hinge rotation axis (in world-space coordinates) computed from body 1
Vector3 mA1;
vec3 mA1;
/// Vector from center of body 2 to anchor point in world-space
Vector3 m_r1World;
vec3 m_r1World;
/// Vector from center of body 2 to anchor point in world-space
Vector3 m_r2World;
vec3 m_r2World;
/// Cross product of vector b2 and a1
Vector3 mB2CrossA1;
vec3 mB2CrossA1;
/// Cross product of vector c2 and a1;
Vector3 mC2CrossA1;
vec3 mC2CrossA1;
/// Impulse for the 3 translation constraints
Vector3 m_impulseTranslation;
vec3 m_impulseTranslation;
/// Impulse for the 2 rotation constraints
Vector2 m_impulseRotation;
vec2 m_impulseRotation;
/// Accumulated impulse for the lower limit constraint
float m_impulseLowerLimit;
@ -177,10 +177,10 @@ class HingeJoint : public Joint {
float m_impulseMotor;
/// Inverse mass matrix K=JM^-1J^t for the 3 translation constraints
Matrix3x3 m_inverseMassMatrixTranslation;
etk::Matrix3x3 m_inverseMassMatrixTranslation;
/// Inverse mass matrix K=JM^-1J^t for the 2 rotation constraints
Matrix2x2 m_inverseMassMatrixRotation;
etk::Matrix2x2 m_inverseMassMatrixRotation;
/// Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix)
float m_inverseMassMatrixLimitMotor;
@ -189,10 +189,10 @@ class HingeJoint : public Joint {
float m_inverseMassMatrixMotor;
/// Bias vector for the error correction for the translation constraints
Vector3 mBTranslation;
vec3 mBTranslation;
/// Bias vector for the error correction for the rotation constraints
Vector2 mBRotation;
vec2 mBRotation;
/// Bias of the lower limit constraint
float mBLowerLimit;
@ -201,7 +201,7 @@ class HingeJoint : public Joint {
float mBUpperLimit;
/// Inverse of the initial orientation difference between the bodies
Quaternion m_initOrientationDifferenceInv;
etk::Quaternion m_initOrientationDifferenceInv;
/// True if the joint limits are enabled
bool mIsLimitEnabled;
@ -249,8 +249,8 @@ class HingeJoint : public Joint {
float upperLimitAngle) const;
/// Compute the current angle around the hinge axis
float computeCurrentHingeAngle(const Quaternion& orientationBody1,
const Quaternion& orientationBody2);
float computeCurrentHingeAngle(const etk::Quaternion& orientationBody1,
const etk::Quaternion& orientationBody2);
/// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const;

244
ephysics/constraint/SliderJoint.cpp
View File

@ -27,8 +27,8 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
assert(mMaxMotorForce >= 0.0);
// Compute the local-space anchor point for each body
const Transform& transform1 = m_body1->getTransform();
const Transform& transform2 = m_body2->getTransform();
const etk::Transform3D& transform1 = m_body1->getTransform();
const etk::Transform3D& transform2 = m_body2->getTransform();
m_localAnchorPointBody1 = transform1.getInverse() * jointInfo.m_anchorPointWorldSpace;
m_localAnchorPointBody2 = transform2.getInverse() * jointInfo.m_anchorPointWorldSpace;
@ -57,10 +57,10 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
m_indexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(m_body2)->second;
// Get the bodies positions and orientations
const Vector3& x1 = m_body1->m_centerOfMassWorld;
const Vector3& x2 = m_body2->m_centerOfMassWorld;
const Quaternion& orientationBody1 = m_body1->getTransform().getOrientation();
const Quaternion& orientationBody2 = m_body2->getTransform().getOrientation();
const vec3& x1 = m_body1->m_centerOfMassWorld;
const vec3& x2 = m_body2->m_centerOfMassWorld;
const etk::Quaternion& orientationBody1 = m_body1->getTransform().getOrientation();
const etk::Quaternion& orientationBody2 = m_body2->getTransform().getOrientation();
// Get the inertia tensor of bodies
m_i1 = m_body1->getInertiaTensorInverseWorld();
@ -71,7 +71,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
mR2 = orientationBody2 * m_localAnchorPointBody2;
// Compute the vector u (difference between anchor points)
const Vector3 u = x2 + mR2 - x1 - mR1;
const vec3 u = x2 + mR2 - x1 - mR1;
// Compute the two orthogonal vectors to the slider axis in world-space
mSliderAxisWorld = orientationBody1 * mSliderAxisBody1;
@ -98,7 +98,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
mR2CrossN1 = mR2.cross(mN1);
mR2CrossN2 = mR2.cross(mN2);
mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld);
const Vector3 r1PlusU = mR1 + u;
const vec3 r1PlusU = mR1 + u;
mR1PlusUCrossN1 = (r1PlusU).cross(mN1);
mR1PlusUCrossN2 = (r1PlusU).cross(mN2);
mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld);
@ -106,10 +106,10 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
// constraints (2x2 matrix)
float sumInverseMass = m_body1->m_massInverse + m_body2->m_massInverse;
Vector3 I1R1PlusUCrossN1 = m_i1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = m_i1 * mR1PlusUCrossN2;
Vector3 I2R2CrossN1 = m_i2 * mR2CrossN1;
Vector3 I2R2CrossN2 = m_i2 * mR2CrossN2;
vec3 I1R1PlusUCrossN1 = m_i1 * mR1PlusUCrossN1;
vec3 I1R1PlusUCrossN2 = m_i1 * mR1PlusUCrossN2;
vec3 I2R2CrossN1 = m_i2 * mR2CrossN1;
vec3 I2R2CrossN2 = m_i2 * mR2CrossN2;
const float el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1);
const float el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
@ -118,18 +118,18 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
mR2CrossN2.dot(I2R2CrossN1);
const float el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
mR2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
m_inverseMassMatrixTranslationConstraint.setToZero();
etk::Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
m_inverseMassMatrixTranslationConstraint.setZero();
if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) {
m_inverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
}
// Compute the bias "b" of the translation constraint
mBTranslation.setToZero();
mBTranslation.setZero();
float biasFactor = (BETA / constraintSolverData.timeStep);
if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBTranslation.x = u.dot(mN1);
mBTranslation.y = u.dot(mN2);
mBTranslation.x() = u.dot(mN1);
mBTranslation.y() = u.dot(mN2);
mBTranslation *= biasFactor;
}
@ -141,11 +141,11 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
}
// Compute the bias "b" of the rotation constraint
mBRotation.setToZero();
mBRotation.setZero();
if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) {
Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
etk::Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * m_initOrientationDifferenceInv;
const etk::Quaternion qError = currentOrientationDifference * m_initOrientationDifferenceInv;
mBRotation = biasFactor * float(2.0) * qError.getVectorV();
}
@ -157,7 +157,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
mR1PlusUCrossSliderAxis.dot(m_i1 * mR1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(m_i2 * mR2CrossSliderAxis);
m_inverseMassMatrixLimit = (m_inverseMassMatrixLimit > 0.0) ?
float(1.0) / m_inverseMassMatrixLimit : float(0.0);
1.0f / m_inverseMassMatrixLimit : 0.0f;
// Compute the bias "b" of the lower limit constraint
mBLowerLimit = 0.0;
@ -178,15 +178,15 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix)
m_inverseMassMatrixMotor = m_body1->m_massInverse + m_body2->m_massInverse;
m_inverseMassMatrixMotor = (m_inverseMassMatrixMotor > 0.0) ?
float(1.0) / m_inverseMassMatrixMotor : float(0.0);
1.0f / m_inverseMassMatrixMotor : 0.0f;
}
// If warm-starting is not enabled
if (!constraintSolverData.isWarmStartingActive) {
// Reset all the accumulated impulses
m_impulseTranslation.setToZero();
m_impulseRotation.setToZero();
m_impulseTranslation.setZero();
m_impulseRotation.setZero();
m_impulseLowerLimit = 0.0;
m_impulseUpperLimit = 0.0;
m_impulseMotor = 0.0;
@ -197,10 +197,10 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[m_indexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[m_indexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[m_indexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[m_indexBody2];
vec3& v1 = constraintSolverData.linearVelocities[m_indexBody1];
vec3& v2 = constraintSolverData.linearVelocities[m_indexBody2];
vec3& w1 = constraintSolverData.angularVelocities[m_indexBody1];
vec3& w2 = constraintSolverData.angularVelocities[m_indexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
const float inverseMassBody1 = m_body1->m_massInverse;
@ -208,15 +208,15 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
float impulseLimits = m_impulseUpperLimit - m_impulseLowerLimit;
Vector3 linearImpulseLimits = impulseLimits * mSliderAxisWorld;
vec3 linearImpulseLimits = impulseLimits * mSliderAxisWorld;
// Compute the impulse P=J^T * lambda for the motor constraint of body 1
Vector3 impulseMotor = m_impulseMotor * mSliderAxisWorld;
vec3 impulseMotor = m_impulseMotor * mSliderAxisWorld;
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
Vector3 linearImpulseBody1 = -mN1 * m_impulseTranslation.x - mN2 * m_impulseTranslation.y;
Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * m_impulseTranslation.x -
mR1PlusUCrossN2 * m_impulseTranslation.y;
vec3 linearImpulseBody1 = -mN1 * m_impulseTranslation.x() - mN2 * m_impulseTranslation.y();
vec3 angularImpulseBody1 = -mR1PlusUCrossN1 * m_impulseTranslation.x() -
mR1PlusUCrossN2 * m_impulseTranslation.y();
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 += -m_impulseRotation;
@ -233,9 +233,9 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
Vector3 linearImpulseBody2 = mN1 * m_impulseTranslation.x + mN2 * m_impulseTranslation.y;
Vector3 angularImpulseBody2 = mR2CrossN1 * m_impulseTranslation.x +
mR2CrossN2 * m_impulseTranslation.y;
vec3 linearImpulseBody2 = mN1 * m_impulseTranslation.x() + mN2 * m_impulseTranslation.y();
vec3 angularImpulseBody2 = mR2CrossN1 * m_impulseTranslation.x() +
mR2CrossN2 * m_impulseTranslation.y();
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
angularImpulseBody2 += m_impulseRotation;
@ -256,10 +256,10 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[m_indexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[m_indexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[m_indexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[m_indexBody2];
vec3& v1 = constraintSolverData.linearVelocities[m_indexBody1];
vec3& v2 = constraintSolverData.linearVelocities[m_indexBody2];
vec3& w1 = constraintSolverData.angularVelocities[m_indexBody1];
vec3& w2 = constraintSolverData.angularVelocities[m_indexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
float inverseMassBody1 = m_body1->m_massInverse;
@ -272,24 +272,24 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
mN1.dot(v2) + w2.dot(mR2CrossN1);
const float el2 = -mN2.dot(v1) - w1.dot(mR1PlusUCrossN2) +
mN2.dot(v2) + w2.dot(mR2CrossN2);
const Vector2 JvTranslation(el1, el2);
const vec2 JvTranslation(el1, el2);
// Compute the Lagrange multiplier lambda for the 2 translation constraints
Vector2 deltaLambda = m_inverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation);
vec2 deltaLambda = m_inverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation);
m_impulseTranslation += deltaLambda;
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
const Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y;
Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x -
mR1PlusUCrossN2 * deltaLambda.y;
const vec3 linearImpulseBody1 = -mN1 * deltaLambda.x() - mN2 * deltaLambda.y();
vec3 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x() -
mR1PlusUCrossN2 * deltaLambda.y();
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
const Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y;
Vector3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x + mR2CrossN2 * deltaLambda.y;
const vec3 linearImpulseBody2 = mN1 * deltaLambda.x() + mN2 * deltaLambda.y();
vec3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x() + mR2CrossN2 * deltaLambda.y();
// Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2;
@ -298,10 +298,10 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// --------------- Rotation Constraints --------------- //
// Compute J*v for the 3 rotation constraints
const Vector3 JvRotation = w2 - w1;
const vec3 JvRotation = w2 - w1;
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 deltaLambda2 = m_inverseMassMatrixRotationConstraint * (-JvRotation - mBRotation);
vec3 deltaLambda2 = m_inverseMassMatrixRotationConstraint * (-JvRotation - mBRotation);
m_impulseRotation += deltaLambda2;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
@ -330,20 +330,20 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// Compute the Lagrange multiplier lambda for the lower limit constraint
float deltaLambdaLower = m_inverseMassMatrixLimit * (-JvLowerLimit -mBLowerLimit);
float lambdaTemp = m_impulseLowerLimit;
m_impulseLowerLimit = std::max(m_impulseLowerLimit + deltaLambdaLower, float(0.0));
m_impulseLowerLimit = std::max(m_impulseLowerLimit + deltaLambdaLower, 0.0f);
deltaLambdaLower = m_impulseLowerLimit - lambdaTemp;
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
const Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis;
const vec3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld;
const vec3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis;
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
const Vector3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis;
const vec3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld;
const vec3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis;
// Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2;
@ -360,20 +360,20 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// Compute the Lagrange multiplier lambda for the upper limit constraint
float deltaLambdaUpper = m_inverseMassMatrixLimit * (-JvUpperLimit -mBUpperLimit);
float lambdaTemp = m_impulseUpperLimit;
m_impulseUpperLimit = std::max(m_impulseUpperLimit + deltaLambdaUpper, float(0.0));
m_impulseUpperLimit = std::max(m_impulseUpperLimit + deltaLambdaUpper, 0.0f);
deltaLambdaUpper = m_impulseUpperLimit - lambdaTemp;
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
const Vector3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = deltaLambdaUpper * mR1PlusUCrossSliderAxis;
const vec3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld;
const vec3 angularImpulseBody1 = deltaLambdaUpper * mR1PlusUCrossSliderAxis;
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 2
const Vector3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mR2CrossSliderAxis;
const vec3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld;
const vec3 angularImpulseBody2 = -deltaLambdaUpper * mR2CrossSliderAxis;
// Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2;
@ -396,13 +396,13 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
deltaLambdaMotor = m_impulseMotor - lambdaTemp;
// Compute the impulse P=J^T * lambda for the motor of body 1
const Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld;
const vec3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld;
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
// Compute the impulse P=J^T * lambda for the motor of body 2
const Vector3 linearImpulseBody2 = -deltaLambdaMotor * mSliderAxisWorld;
const vec3 linearImpulseBody2 = -deltaLambdaMotor * mSliderAxisWorld;
// Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2;
@ -417,10 +417,10 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
if (m_positionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
Vector3& x1 = constraintSolverData.positions[m_indexBody1];
Vector3& x2 = constraintSolverData.positions[m_indexBody2];
Quaternion& q1 = constraintSolverData.orientations[m_indexBody1];
Quaternion& q2 = constraintSolverData.orientations[m_indexBody2];
vec3& x1 = constraintSolverData.positions[m_indexBody1];
vec3& x2 = constraintSolverData.positions[m_indexBody2];
etk::Quaternion& q1 = constraintSolverData.orientations[m_indexBody1];
etk::Quaternion& q2 = constraintSolverData.orientations[m_indexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
float inverseMassBody1 = m_body1->m_massInverse;
@ -435,7 +435,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
mR2 = q2 * m_localAnchorPointBody2;
// Compute the vector u (difference between anchor points)
const Vector3 u = x2 + mR2 - x1 - mR1;
const vec3 u = x2 + mR2 - x1 - mR1;
// Compute the two orthogonal vectors to the slider axis in world-space
mSliderAxisWorld = q1 * mSliderAxisBody1;
@ -454,7 +454,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
mR2CrossN1 = mR2.cross(mN1);
mR2CrossN2 = mR2.cross(mN2);
mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld);
const Vector3 r1PlusU = mR1 + u;
const vec3 r1PlusU = mR1 + u;
mR1PlusUCrossN1 = (r1PlusU).cross(mN1);
mR1PlusUCrossN2 = (r1PlusU).cross(mN2);
mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld);
@ -464,10 +464,10 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
// constraints (2x2 matrix)
float sumInverseMass = m_body1->m_massInverse + m_body2->m_massInverse;
Vector3 I1R1PlusUCrossN1 = m_i1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = m_i1 * mR1PlusUCrossN2;
Vector3 I2R2CrossN1 = m_i2 * mR2CrossN1;
Vector3 I2R2CrossN2 = m_i2 * mR2CrossN2;
vec3 I1R1PlusUCrossN1 = m_i1 * mR1PlusUCrossN1;
vec3 I1R1PlusUCrossN2 = m_i1 * mR1PlusUCrossN2;
vec3 I2R2CrossN1 = m_i2 * mR2CrossN1;
vec3 I2R2CrossN2 = m_i2 * mR2CrossN2;
const float el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1);
const float el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
@ -476,44 +476,44 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
mR2CrossN2.dot(I2R2CrossN1);
const float el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
mR2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
m_inverseMassMatrixTranslationConstraint.setToZero();
etk::Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
m_inverseMassMatrixTranslationConstraint.setZero();
if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) {
m_inverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
}
// Compute the position error for the 2 translation constraints
const Vector2 translationError(u.dot(mN1), u.dot(mN2));
const vec2 translationError(u.dot(mN1), u.dot(mN2));
// Compute the Lagrange multiplier lambda for the 2 translation constraints
Vector2 lambdaTranslation = m_inverseMassMatrixTranslationConstraint * (-translationError);
vec2 lambdaTranslation = m_inverseMassMatrixTranslationConstraint * (-translationError);
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
const Vector3 linearImpulseBody1 = -mN1 * lambdaTranslation.x - mN2 * lambdaTranslation.y;
Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * lambdaTranslation.x -
mR1PlusUCrossN2 * lambdaTranslation.y;
const vec3 linearImpulseBody1 = -mN1 * lambdaTranslation.x() - mN2 * lambdaTranslation.y();
vec3 angularImpulseBody1 = -mR1PlusUCrossN1 * lambdaTranslation.x() -
mR1PlusUCrossN2 * lambdaTranslation.y();
// Apply the impulse to the body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
Vector3 w1 = m_i1 * angularImpulseBody1;
const vec3 v1 = inverseMassBody1 * linearImpulseBody1;
vec3 w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * float(0.5);
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
q1.normalize();
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
const Vector3 linearImpulseBody2 = mN1 * lambdaTranslation.x + mN2 * lambdaTranslation.y;
Vector3 angularImpulseBody2 = mR2CrossN1 * lambdaTranslation.x +
mR2CrossN2 * lambdaTranslation.y;
const vec3 linearImpulseBody2 = mN1 * lambdaTranslation.x() + mN2 * lambdaTranslation.y();
vec3 angularImpulseBody2 = mR2CrossN1 * lambdaTranslation.x() +
mR2CrossN2 * lambdaTranslation.y();
// Apply the impulse to the body 2
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
Vector3 w2 = m_i2 * angularImpulseBody2;
const vec3 v2 = inverseMassBody2 * linearImpulseBody2;
vec3 w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * float(0.5);
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
q2.normalize();
// --------------- Rotation Constraints --------------- //
@ -526,13 +526,13 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
}
// Compute the position error for the 3 rotation constraints
Quaternion currentOrientationDifference = q2 * q1.getInverse();
etk::Quaternion currentOrientationDifference = q2 * q1.getInverse();
currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * m_initOrientationDifferenceInv;
const Vector3 errorRotation = float(2.0) * qError.getVectorV();
const etk::Quaternion qError = currentOrientationDifference * m_initOrientationDifferenceInv;
const vec3 errorRotation = float(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = m_inverseMassMatrixRotationConstraint * (-errorRotation);
vec3 lambdaRotation = m_inverseMassMatrixRotationConstraint * (-errorRotation);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 = -lambdaRotation;
@ -541,7 +541,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * float(0.5);
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
q1.normalize();
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
@ -551,7 +551,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * float(0.5);
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
q2.normalize();
// --------------- Limits Constraints --------------- //
@ -565,7 +565,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
mR1PlusUCrossSliderAxis.dot(m_i1 * mR1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(m_i2 * mR2CrossSliderAxis);
m_inverseMassMatrixLimit = (m_inverseMassMatrixLimit > 0.0) ?
float(1.0) / m_inverseMassMatrixLimit : float(0.0);
1.0f / m_inverseMassMatrixLimit : 0.0f;
}
// If the lower limit is violated
@ -575,29 +575,29 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
float lambdaLowerLimit = m_inverseMassMatrixLimit * (-lowerLimitError);
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mR1PlusUCrossSliderAxis;
const vec3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld;
const vec3 angularImpulseBody1 = -lambdaLowerLimit * mR1PlusUCrossSliderAxis;
// Apply the impulse to the body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = m_i1 * angularImpulseBody1;
const vec3 v1 = inverseMassBody1 * linearImpulseBody1;
const vec3 w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * float(0.5);
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
q1.normalize();
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
const Vector3 linearImpulseBody2 = lambdaLowerLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = lambdaLowerLimit * mR2CrossSliderAxis;
const vec3 linearImpulseBody2 = lambdaLowerLimit * mSliderAxisWorld;
const vec3 angularImpulseBody2 = lambdaLowerLimit * mR2CrossSliderAxis;
// Apply the impulse to the body 2
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = m_i2 * angularImpulseBody2;
const vec3 v2 = inverseMassBody2 * linearImpulseBody2;
const vec3 w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * float(0.5);
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
q2.normalize();
}
@ -608,29 +608,29 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
float lambdaUpperLimit = m_inverseMassMatrixLimit * (-upperLimitError);
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = lambdaUpperLimit * mR1PlusUCrossSliderAxis;
const vec3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld;
const vec3 angularImpulseBody1 = lambdaUpperLimit * mR1PlusUCrossSliderAxis;
// Apply the impulse to the body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = m_i1 * angularImpulseBody1;
const vec3 v1 = inverseMassBody1 * linearImpulseBody1;
const vec3 w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * float(0.5);
q1 += etk::Quaternion(0, w1) * q1 * 0.5f;
q1.normalize();
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 2
const Vector3 linearImpulseBody2 = -lambdaUpperLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mR2CrossSliderAxis;
const vec3 linearImpulseBody2 = -lambdaUpperLimit * mSliderAxisWorld;
const vec3 angularImpulseBody2 = -lambdaUpperLimit * mR2CrossSliderAxis;
// Apply the impulse to the body 2
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = m_i2 * angularImpulseBody2;
const vec3 v2 = inverseMassBody2 * linearImpulseBody2;
const vec3 w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * float(0.5);
q2 += etk::Quaternion(0, w2) * q2 * 0.5f;
q2.normalize();
}
}
@ -676,20 +676,20 @@ float SliderJoint::getTranslation() const {
// TODO : Check if we need to compare rigid body position or center of mass here
// Get the bodies positions and orientations
const Vector3& x1 = m_body1->getTransform().getPosition();
const Vector3& x2 = m_body2->getTransform().getPosition();
const Quaternion& q1 = m_body1->getTransform().getOrientation();
const Quaternion& q2 = m_body2->getTransform().getOrientation();
const vec3& x1 = m_body1->getTransform().getPosition();
const vec3& x2 = m_body2->getTransform().getPosition();
const etk::Quaternion& q1 = m_body1->getTransform().getOrientation();
const etk::Quaternion& q2 = m_body2->getTransform().getOrientation();
// Compute the two anchor points in world-space coordinates
const Vector3 anchorBody1 = x1 + q1 * m_localAnchorPointBody1;
const Vector3 anchorBody2 = x2 + q2 * m_localAnchorPointBody2;
const vec3 anchorBody1 = x1 + q1 * m_localAnchorPointBody1;
const vec3 anchorBody2 = x2 + q2 * m_localAnchorPointBody2;
// Compute the vector u (difference between anchor points)
const Vector3 u = anchorBody2 - anchorBody1;
const vec3 u = anchorBody2 - anchorBody1;
// Compute the slider axis in world-space
Vector3 sliderAxisWorld = q1 * mSliderAxisBody1;
vec3 sliderAxisWorld = q1 * mSliderAxisBody1;
sliderAxisWorld.normalize();
// Compute and return the translation value

62
ephysics/constraint/SliderJoint.h
View File

@ -23,10 +23,10 @@ struct SliderJointInfo : public JointInfo {
// -------------------- Attributes -------------------- //
/// Anchor point (in world-space coordinates)
Vector3 m_anchorPointWorldSpace;
vec3 m_anchorPointWorldSpace;
/// Slider axis (in world-space coordinates)
Vector3 sliderAxisWorldSpace;
vec3 sliderAxisWorldSpace;
/// True if the slider limits are enabled
bool isLimitEnabled;
@ -54,8 +54,8 @@ struct SliderJointInfo : public JointInfo {
* @param initSliderAxisWorldSpace The initial slider axis in world-space
*/
SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace,
const Vector3& initSliderAxisWorldSpace)
const vec3& initAnchorPointWorldSpace,
const vec3& initSliderAxisWorldSpace)
: JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
m_anchorPointWorldSpace(initAnchorPointWorldSpace),
sliderAxisWorldSpace(initSliderAxisWorldSpace),
@ -72,8 +72,8 @@ struct SliderJointInfo : public JointInfo {
* @param initMaxTranslationLimit The initial maximum translation limit (in meters)
*/
SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace,
const Vector3& initSliderAxisWorldSpace,
const vec3& initAnchorPointWorldSpace,
const vec3& initSliderAxisWorldSpace,
float initMinTranslationLimit, float initMaxTranslationLimit)
: JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
m_anchorPointWorldSpace(initAnchorPointWorldSpace),
@ -95,8 +95,8 @@ struct SliderJointInfo : public JointInfo {
* @param initMaxMotorForce The initial maximum motor force of the joint (in Newtons x meters)
*/
SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace,
const Vector3& initSliderAxisWorldSpace,
const vec3& initAnchorPointWorldSpace,
const vec3& initSliderAxisWorldSpace,
float initMinTranslationLimit, float initMaxTranslationLimit,
float initMotorSpeed, float initMaxMotorForce)
: JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
@ -126,58 +126,58 @@ class SliderJoint : public Joint {
// -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local-space coordinates of body 1)
Vector3 m_localAnchorPointBody1;
vec3 m_localAnchorPointBody1;
/// Anchor point of body 2 (in local-space coordinates of body 2)
Vector3 m_localAnchorPointBody2;
vec3 m_localAnchorPointBody2;
/// Slider axis (in local-space coordinates of body 1)
Vector3 mSliderAxisBody1;
vec3 mSliderAxisBody1;
/// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 m_i1;
etk::Matrix3x3 m_i1;
/// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 m_i2;
etk::Matrix3x3 m_i2;
/// Inverse of the initial orientation difference between the two bodies
Quaternion m_initOrientationDifferenceInv;
etk::Quaternion m_initOrientationDifferenceInv;
/// First vector orthogonal to the slider axis local-space of body 1
Vector3 mN1;
vec3 mN1;
/// Second vector orthogonal to the slider axis and mN1 in local-space of body 1
Vector3 mN2;
vec3 mN2;
/// Vector r1 in world-space coordinates
Vector3 mR1;
vec3 mR1;
/// Vector r2 in world-space coordinates
Vector3 mR2;
vec3 mR2;
/// Cross product of r2 and n1
Vector3 mR2CrossN1;
vec3 mR2CrossN1;
/// Cross product of r2 and n2
Vector3 mR2CrossN2;
vec3 mR2CrossN2;
/// Cross product of r2 and the slider axis
Vector3 mR2CrossSliderAxis;
vec3 mR2CrossSliderAxis;
/// Cross product of vector (r1 + u) and n1
Vector3 mR1PlusUCrossN1;
vec3 mR1PlusUCrossN1;
/// Cross product of vector (r1 + u) and n2
Vector3 mR1PlusUCrossN2;
vec3 mR1PlusUCrossN2;
/// Cross product of vector (r1 + u) and the slider axis
Vector3 mR1PlusUCrossSliderAxis;
vec3 mR1PlusUCrossSliderAxis;
/// Bias of the 2 translation constraints
Vector2 mBTranslation;
vec2 mBTranslation;
/// Bias of the 3 rotation constraints
Vector3 mBRotation;
vec3 mBRotation;
/// Bias of the lower limit constraint
float mBLowerLimit;
@ -186,10 +186,10 @@ class SliderJoint : public Joint {
float mBUpperLimit;
/// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix)
Matrix2x2 m_inverseMassMatrixTranslationConstraint;
etk::Matrix2x2 m_inverseMassMatrixTranslationConstraint;
/// Inverse of mass matrix K=JM^-1J^t for the rotation constraint (3x3 matrix)
Matrix3x3 m_inverseMassMatrixRotationConstraint;
etk::Matrix3x3 m_inverseMassMatrixRotationConstraint;
/// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix)
float m_inverseMassMatrixLimit;
@ -198,10 +198,10 @@ class SliderJoint : public Joint {
float m_inverseMassMatrixMotor;
/// Accumulated impulse for the 2 translation constraints
Vector2 m_impulseTranslation;
vec2 m_impulseTranslation;
/// Accumulated impulse for the 3 rotation constraints
Vector3 m_impulseRotation;
vec3 m_impulseRotation;
/// Accumulated impulse for the lower limit constraint
float m_impulseLowerLimit;
@ -219,7 +219,7 @@ class SliderJoint : public Joint {
bool mIsMotorEnabled;
/// Slider axis in world-space coordinates
Vector3 mSliderAxisWorld;
vec3 mSliderAxisWorld;
/// Lower limit (minimum translation distance)
float mLowerLimit;

4
ephysics/engine/CollisionWorld.cpp
View File

@ -35,10 +35,10 @@ CollisionWorld::~CollisionWorld() {
// Create a collision body and add it to the world
/**
* @param transform Transformation mapping the local-space of the body to world-space
* @param transform etk::Transform3Dation mapping the local-space of the body to world-space
* @return A pointer to the body that has been created in the world
*/
CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) {
CollisionBody* CollisionWorld::createCollisionBody(const etk::Transform3D& transform) {
// Get the next available body ID
bodyindex bodyID = computeNextAvailableBodyID();

2
ephysics/engine/CollisionWorld.h
View File

@ -88,7 +88,7 @@ class CollisionWorld {
std::set<CollisionBody*>::iterator getBodiesEndIterator();
/// Create a collision body
CollisionBody* createCollisionBody(const Transform& transform);
CollisionBody* createCollisionBody(const etk::Transform3D& transform);
/// Destroy a collision body
void destroyCollisionBody(CollisionBody* collisionBody);

24
ephysics/engine/ConstraintSolver.h
View File

@ -28,16 +28,16 @@ struct ConstraintSolverData {
float timeStep;
/// Array with the bodies linear velocities
Vector3* linearVelocities;
vec3* linearVelocities;
/// Array with the bodies angular velocities
Vector3* angularVelocities;
vec3* angularVelocities;
/// Reference to the bodies positions
Vector3* positions;
vec3* positions;
/// Reference to the bodies orientations
Quaternion* orientations;
etk::Quaternion* orientations;
/// Reference to the map that associates rigid body to their index
/// in the constrained velocities array
@ -170,17 +170,17 @@ class ConstraintSolver {
void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive);
/// Set the constrained velocities arrays
void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities);
void setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities,
vec3* constrainedAngularVelocities);
/// Set the constrained positions/orientations arrays
void setConstrainedPositionsArrays(Vector3* constrainedPositions,
Quaternion* constrainedOrientations);
void setConstrainedPositionsArrays(vec3* constrainedPositions,
etk::Quaternion* constrainedOrientations);
};
// Set the constrained velocities arrays
inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities) {
inline void ConstraintSolver::setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities,
vec3* constrainedAngularVelocities) {
assert(constrainedLinearVelocities != NULL);
assert(constrainedAngularVelocities != NULL);
m_constraintSolverData.linearVelocities = constrainedLinearVelocities;
@ -188,8 +188,8 @@ inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constraine
}
// Set the constrained positions/orientations arrays
inline void ConstraintSolver::setConstrainedPositionsArrays(Vector3* constrainedPositions,
Quaternion* constrainedOrientations) {
inline void ConstraintSolver::setConstrainedPositionsArrays(vec3* constrainedPositions,
etk::Quaternion* constrainedOrientations) {
assert(constrainedPositions != NULL);
assert(constrainedOrientations != NULL);
m_constraintSolverData.positions = constrainedPositions;

206
ephysics/engine/ContactSolver.cpp
View File

@ -70,8 +70,8 @@ void ContactSolver::initializeForIsland(float dt, Island* island) {
assert(body2 != NULL);
// Get the position of the two bodies
const Vector3& x1 = body1->m_centerOfMassWorld;
const Vector3& x2 = body2->m_centerOfMassWorld;
const vec3& x1 = body1->m_centerOfMassWorld;
const vec3& x2 = body2->m_centerOfMassWorld;
// Initialize the int32_ternal contact manifold structure using the external
// contact manifold
@ -91,8 +91,8 @@ void ContactSolver::initializeForIsland(float dt, Island* island) {
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
int32_ternalManifold.frictionPointBody1 = Vector3::zero();
int32_ternalManifold.frictionPointBody2 = Vector3::zero();
int32_ternalManifold.frictionPointBody1 = vec3::zero();
int32_ternalManifold.frictionPointBody2 = vec3::zero();
}
// For each contact point of the contact manifold
@ -104,8 +104,8 @@ void ContactSolver::initializeForIsland(float dt, Island* island) {
ContactPoint* externalContact = externalManifold->getContactPoint(c);
// Get the contact point on the two bodies
Vector3 p1 = externalContact->getWorldPointOnBody1();
Vector3 p2 = externalContact->getWorldPointOnBody2();
vec3 p1 = externalContact->getWorldPointOnBody1();
vec3 p2 = externalContact->getWorldPointOnBody2();
contactPoint.externalContact = externalContact;
contactPoint.normal = externalContact->getNormal();
@ -115,11 +115,11 @@ void ContactSolver::initializeForIsland(float dt, Island* island) {
contactPoint.isRestingContact = externalContact->getIsRestingContact();
externalContact->setIsRestingContact(true);
contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1();
contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2();
contactPoint.oldFrictionvec2 = externalContact->getFrictionvec2();
contactPoint.penetrationImpulse = 0.0;
contactPoint.friction1Impulse = 0.0;
contactPoint.friction2Impulse = 0.0;
contactPoint.rollingResistanceImpulse = Vector3::zero();
contactPoint.rollingResistanceImpulse = vec3::zero();
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
@ -136,7 +136,7 @@ void ContactSolver::initializeForIsland(float dt, Island* island) {
int32_ternalManifold.r1Friction = int32_ternalManifold.frictionPointBody1 - x1;
int32_ternalManifold.r2Friction = int32_ternalManifold.frictionPointBody2 - x2;
int32_ternalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1();
int32_ternalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2();
int32_ternalManifold.oldFrictionvec2 = externalManifold->getFrictionvec2();
// If warm starting is active
if (mIsWarmStartingActive) {
@ -152,7 +152,7 @@ void ContactSolver::initializeForIsland(float dt, Island* island) {
int32_ternalManifold.friction1Impulse = 0.0;
int32_ternalManifold.friction2Impulse = 0.0;
int32_ternalManifold.frictionTwistImpulse = 0.0;
int32_ternalManifold.rollingResistanceImpulse = Vector3(0, 0, 0);
int32_ternalManifold.rollingResistanceImpulse = vec3(0, 0, 0);
}
}
}
@ -170,19 +170,19 @@ void ContactSolver::initializeContactConstraints() {
ContactManifoldSolver& manifold = mContactConstraints[c];
// Get the inertia tensors of both bodies
Matrix3x3& I1 = manifold.inverseInertiaTensorBody1;
Matrix3x3& I2 = manifold.inverseInertiaTensorBody2;
etk::Matrix3x3& I1 = manifold.inverseInertiaTensorBody1;
etk::Matrix3x3& I2 = manifold.inverseInertiaTensorBody2;
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
manifold.normal = Vector3(0.0, 0.0, 0.0);
manifold.normal = vec3(0.0, 0.0, 0.0);
}
// Get the velocities of the bodies
const Vector3& v1 = mLinearVelocities[manifold.indexBody1];
const Vector3& w1 = mAngularVelocities[manifold.indexBody1];
const Vector3& v2 = mLinearVelocities[manifold.indexBody2];
const Vector3& w2 = mAngularVelocities[manifold.indexBody2];
const vec3& v1 = mLinearVelocities[manifold.indexBody1];
const vec3& w1 = mAngularVelocities[manifold.indexBody1];
const vec3& v2 = mLinearVelocities[manifold.indexBody2];
const vec3& w2 = mAngularVelocities[manifold.indexBody2];
// For each contact point constraint
for (uint32_t i=0; i<manifold.nbContacts; i++) {
@ -191,7 +191,7 @@ void ContactSolver::initializeContactConstraints() {
ContactPoint* externalContact = contactPoint.externalContact;
// Compute the velocity difference
Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
vec3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal);
contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal);
@ -200,9 +200,9 @@ void ContactSolver::initializeContactConstraints() {
float massPenetration = manifold.massInverseBody1 + manifold.massInverseBody2 +
((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) +
((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal);
massPenetration > 0.0 ? contactPoint.inversePenetrationMass = float(1.0) /
massPenetration > 0.0 ? contactPoint.inversePenetrationMass = 1.0f /
massPenetration :
float(0.0);
0.0f;
// If we do not solve the friction constraints at the center of the contact manifold
if (!mIsSolveFrictionAtContactManifoldCenterActive) {
@ -211,9 +211,9 @@ void ContactSolver::initializeContactConstraints() {
computeFrictionVectors(deltaV, contactPoint);
contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1);
contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionVector2);
contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionvec2);
contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1);
contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionVector2);
contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionvec2);
// Compute the inverse mass matrix K for the friction
// constraints at each contact point
@ -224,15 +224,15 @@ void ContactSolver::initializeContactConstraints() {
contactPoint.frictionVector1);
float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot(
contactPoint.frictionVector2) +
contactPoint.frictionvec2) +
((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot(
contactPoint.frictionVector2);
friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = float(1.0) /
contactPoint.frictionvec2);
friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = 1.0f /
friction1Mass :
float(0.0);
friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = float(1.0) /
0.0f;
friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = 1.0f /
friction2Mass :
float(0.0);
0.0f;
}
// Compute the restitution velocity bias "b". We compute this here instead
@ -265,7 +265,7 @@ void ContactSolver::initializeContactConstraints() {
}
// Compute the inverse K matrix for the rolling resistance constraint
manifold.inverseRollingResistance.setToZero();
manifold.inverseRollingResistance.setZero();
if (manifold.rollingResistanceFactor > 0 && (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) {
manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1 + manifold.inverseInertiaTensorBody2;
manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse();
@ -276,7 +276,7 @@ void ContactSolver::initializeContactConstraints() {
manifold.normal.normalize();
Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) -
vec3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) -
v1 - w1.cross(manifold.r1Friction);
// Compute the friction vectors
@ -285,9 +285,9 @@ void ContactSolver::initializeContactConstraints() {
// Compute the inverse mass matrix K for the friction constraints at the center of
// the contact manifold
manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1);
manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2);
manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionvec2);
manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1);
manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2);
manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionvec2);
float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot(
manifold.frictionVector1) +
@ -295,20 +295,20 @@ void ContactSolver::initializeContactConstraints() {
manifold.frictionVector1);
float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot(
manifold.frictionVector2) +
manifold.frictionvec2) +
((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot(
manifold.frictionVector2);
manifold.frictionvec2);
float frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 *
manifold.normal) +
manifold.normal.dot(manifold.inverseInertiaTensorBody2 *
manifold.normal);
friction1Mass > 0.0 ? manifold.inverseFriction1Mass = float(1.0)/friction1Mass
: float(0.0);
friction2Mass > 0.0 ? manifold.inverseFriction2Mass = float(1.0)/friction2Mass
: float(0.0);
frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = float(1.0) /
friction1Mass > 0.0 ? manifold.inverseFriction1Mass = 1.0f/friction1Mass
: 0.0f;
friction2Mass > 0.0 ? manifold.inverseFriction2Mass = 1.0f/friction2Mass
: 0.0f;
frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = 1.0f /
frictionTwistMass :
float(0.0);
0.0f;
}
}
}
@ -352,14 +352,14 @@ void ContactSolver::warmStart() {
// Project the old friction impulses (with old friction vectors) int32_to
// the new friction vectors to get the new friction impulses
Vector3 oldFrictionImpulse = contactPoint.friction1Impulse *
vec3 oldFrictionImpulse = contactPoint.friction1Impulse *
contactPoint.oldFrictionVector1 +
contactPoint.friction2Impulse *
contactPoint.oldFrictionVector2;
contactPoint.oldFrictionvec2;
contactPoint.friction1Impulse = oldFrictionImpulse.dot(
contactPoint.frictionVector1);
contactPoint.friction2Impulse = oldFrictionImpulse.dot(
contactPoint.frictionVector2);
contactPoint.frictionvec2);
// --------- Friction 1 --------- //
@ -384,8 +384,8 @@ void ContactSolver::warmStart() {
if (contactManifold.rollingResistanceFactor > 0) {
// Compute the impulse P = J^T * lambda
const Impulse impulseRollingResistance(Vector3::zero(), -contactPoint.rollingResistanceImpulse,
Vector3::zero(), contactPoint.rollingResistanceImpulse);
const Impulse impulseRollingResistance(vec3::zero(), -contactPoint.rollingResistanceImpulse,
vec3::zero(), contactPoint.rollingResistanceImpulse);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseRollingResistance, contactManifold);
@ -398,7 +398,7 @@ void ContactSolver::warmStart() {
contactPoint.penetrationImpulse = 0.0;
contactPoint.friction1Impulse = 0.0;
contactPoint.friction2Impulse = 0.0;
contactPoint.rollingResistanceImpulse = Vector3::zero();
contactPoint.rollingResistanceImpulse = vec3::zero();
}
}
@ -408,25 +408,25 @@ void ContactSolver::warmStart() {
// Project the old friction impulses (with old friction vectors) int32_to the new friction
// vectors to get the new friction impulses
Vector3 oldFrictionImpulse = contactManifold.friction1Impulse *
vec3 oldFrictionImpulse = contactManifold.friction1Impulse *
contactManifold.oldFrictionVector1 +
contactManifold.friction2Impulse *
contactManifold.oldFrictionVector2;
contactManifold.oldFrictionvec2;
contactManifold.friction1Impulse = oldFrictionImpulse.dot(
contactManifold.frictionVector1);
contactManifold.friction2Impulse = oldFrictionImpulse.dot(
contactManifold.frictionVector2);
contactManifold.frictionvec2);
// ------ First friction constraint at the center of the contact manifold ------ //
// Compute the impulse P = J^T * lambda
Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 *
vec3 linearImpulseBody1 = -contactManifold.frictionVector1 *
contactManifold.friction1Impulse;
Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 *
vec3 angularImpulseBody1 = -contactManifold.r1CrossT1 *
contactManifold.friction1Impulse;
Vector3 linearImpulseBody2 = contactManifold.frictionVector1 *
vec3 linearImpulseBody2 = contactManifold.frictionVector1 *
contactManifold.friction1Impulse;
Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 *
vec3 angularImpulseBody2 = contactManifold.r2CrossT1 *
contactManifold.friction1Impulse;
const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
@ -437,11 +437,11 @@ void ContactSolver::warmStart() {
// ------ Second friction constraint at the center of the contact manifold ----- //
// Compute the impulse P = J^T * lambda
linearImpulseBody1 = -contactManifold.frictionVector2 *
linearImpulseBody1 = -contactManifold.frictionvec2 *
contactManifold.friction2Impulse;
angularImpulseBody1 = -contactManifold.r1CrossT2 *
contactManifold.friction2Impulse;
linearImpulseBody2 = contactManifold.frictionVector2 *
linearImpulseBody2 = contactManifold.frictionvec2 *
contactManifold.friction2Impulse;
angularImpulseBody2 = contactManifold.r2CrossT2 *
contactManifold.friction2Impulse;
@ -454,9 +454,9 @@ void ContactSolver::warmStart() {
// ------ Twist friction constraint at the center of the contact manifold ------ //
// Compute the impulse P = J^T * lambda
linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
linearImpulseBody1 = vec3(0.0, 0.0, 0.0);
angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse;
linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);
linearImpulseBody2 = vec3(0.0, 0.0, 0.0);
angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse;
const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
@ -469,8 +469,8 @@ void ContactSolver::warmStart() {
// Compute the impulse P = J^T * lambda
angularImpulseBody1 = -contactManifold.rollingResistanceImpulse;
angularImpulseBody2 = contactManifold.rollingResistanceImpulse;
const Impulse impulseRollingResistance(Vector3::zero(), angularImpulseBody1,
Vector3::zero(), angularImpulseBody2);
const Impulse impulseRollingResistance(vec3::zero(), angularImpulseBody1,
vec3::zero(), angularImpulseBody2);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseRollingResistance, contactManifold);
@ -481,7 +481,7 @@ void ContactSolver::warmStart() {
contactManifold.friction1Impulse = 0.0;
contactManifold.friction2Impulse = 0.0;
contactManifold.frictionTwistImpulse = 0.0;
contactManifold.rollingResistanceImpulse = Vector3::zero();
contactManifold.rollingResistanceImpulse = vec3::zero();
}
}
}
@ -502,10 +502,10 @@ void ContactSolver::solve() {
float sumPenetrationImpulse = 0.0;
// Get the constrained velocities
const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1];
const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2];
const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2];
const vec3& v1 = mLinearVelocities[contactManifold.indexBody1];
const vec3& w1 = mAngularVelocities[contactManifold.indexBody1];
const vec3& v2 = mLinearVelocities[contactManifold.indexBody2];
const vec3& w2 = mAngularVelocities[contactManifold.indexBody2];
for (uint32_t i=0; i<contactManifold.nbContacts; i++) {
@ -514,7 +514,7 @@ void ContactSolver::solve() {
// --------- Penetration --------- //
// Compute J*v
Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
vec3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
float deltaVDotN = deltaV.dot(contactPoint.normal);
float Jv = deltaVDotN;
@ -535,7 +535,7 @@ void ContactSolver::solve() {
}
lambdaTemp = contactPoint.penetrationImpulse;
contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse +
deltaLambda, float(0.0));
deltaLambda, 0.0f);
deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
@ -551,11 +551,11 @@ void ContactSolver::solve() {
if (mIsSplitImpulseActive) {
// Split impulse (position correction)
const Vector3& v1Split = m_splitLinearVelocities[contactManifold.indexBody1];
const Vector3& w1Split = m_splitAngularVelocities[contactManifold.indexBody1];
const Vector3& v2Split = m_splitLinearVelocities[contactManifold.indexBody2];
const Vector3& w2Split = m_splitAngularVelocities[contactManifold.indexBody2];
Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) -
const vec3& v1Split = m_splitLinearVelocities[contactManifold.indexBody1];
const vec3& w1Split = m_splitAngularVelocities[contactManifold.indexBody1];
const vec3& v2Split = m_splitLinearVelocities[contactManifold.indexBody2];
const vec3& w2Split = m_splitAngularVelocities[contactManifold.indexBody2];
vec3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) -
v1Split - w1Split.cross(contactPoint.r1);
float JvSplit = deltaVSplit.dot(contactPoint.normal);
float deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
@ -563,7 +563,7 @@ void ContactSolver::solve() {
float lambdaTempSplit = contactPoint.penetrationSplitImpulse;
contactPoint.penetrationSplitImpulse = std::max(
contactPoint.penetrationSplitImpulse +
deltaLambdaSplit, float(0.0));
deltaLambdaSplit, 0.0f);
deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
// Compute the impulse P=J^T * lambda
@ -604,7 +604,7 @@ void ContactSolver::solve() {
// Compute J*v
deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
Jv = deltaV.dot(contactPoint.frictionVector2);
Jv = deltaV.dot(contactPoint.frictionvec2);
// Compute the Lagrange multiplier lambda
deltaLambda = -Jv;
@ -629,19 +629,19 @@ void ContactSolver::solve() {
if (contactManifold.rollingResistanceFactor > 0) {
// Compute J*v
const Vector3 JvRolling = w2 - w1;
const vec3 JvRolling = w2 - w1;
// Compute the Lagrange multiplier lambda
Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
vec3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
float rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse;
Vector3 lambdaTempRolling = contactPoint.rollingResistanceImpulse;
vec3 lambdaTempRolling = contactPoint.rollingResistanceImpulse;
contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse +
deltaLambdaRolling, rollingLimit);
deltaLambdaRolling = contactPoint.rollingResistanceImpulse - lambdaTempRolling;
// Compute the impulse P=J^T * lambda
const Impulse impulseRolling(Vector3::zero(), -deltaLambdaRolling,
Vector3::zero(), deltaLambdaRolling);
const Impulse impulseRolling(vec3::zero(), -deltaLambdaRolling,
vec3::zero(), deltaLambdaRolling);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseRolling, contactManifold);
@ -655,7 +655,7 @@ void ContactSolver::solve() {
// ------ First friction constraint at the center of the contact manifol ------ //
// Compute J*v
Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
vec3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
- v1 - w1.cross(contactManifold.r1Friction);
float Jv = deltaV.dot(contactManifold.frictionVector1);
@ -669,10 +669,10 @@ void ContactSolver::solve() {
deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda;
Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
vec3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda;
vec3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
vec3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
vec3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
@ -684,7 +684,7 @@ void ContactSolver::solve() {
// Compute J*v
deltaV = v2 + w2.cross(contactManifold.r2Friction)
- v1 - w1.cross(contactManifold.r1Friction);
Jv = deltaV.dot(contactManifold.frictionVector2);
Jv = deltaV.dot(contactManifold.frictionvec2);
// Compute the Lagrange multiplier lambda
deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
@ -696,9 +696,9 @@ void ContactSolver::solve() {
deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda;
linearImpulseBody1 = -contactManifold.frictionvec2 * deltaLambda;
angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda;
linearImpulseBody2 = contactManifold.frictionvec2 * deltaLambda;
angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
@ -721,9 +721,9 @@ void ContactSolver::solve() {
deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
linearImpulseBody1 = vec3(0.0, 0.0, 0.0);
angularImpulseBody1 = -contactManifold.normal * deltaLambda;
linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);;
linearImpulseBody2 = vec3(0.0, 0.0, 0.0);;
angularImpulseBody2 = contactManifold.normal * deltaLambda;
const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
@ -736,12 +736,12 @@ void ContactSolver::solve() {
if (contactManifold.rollingResistanceFactor > 0) {
// Compute J*v
const Vector3 JvRolling = w2 - w1;
const vec3 JvRolling = w2 - w1;
// Compute the Lagrange multiplier lambda
Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
vec3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
float rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse;
Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse;
vec3 lambdaTempRolling = contactManifold.rollingResistanceImpulse;
contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse +
deltaLambdaRolling, rollingLimit);
deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling;
@ -749,8 +749,8 @@ void ContactSolver::solve() {
// Compute the impulse P=J^T * lambda
angularImpulseBody1 = -deltaLambdaRolling;
angularImpulseBody2 = deltaLambdaRolling;
const Impulse impulseRolling(Vector3::zero(), angularImpulseBody1,
Vector3::zero(), angularImpulseBody2);
const Impulse impulseRolling(vec3::zero(), angularImpulseBody1,
vec3::zero(), angularImpulseBody2);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseRolling, contactManifold);
@ -778,7 +778,7 @@ void ContactSolver::storeImpulses() {
contactPoint.externalContact->setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse);
contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1);
contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2);
contactPoint.externalContact->setFrictionvec2(contactPoint.frictionvec2);
}
manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse);
@ -786,7 +786,7 @@ void ContactSolver::storeImpulses() {
manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse);
manifold.externalContactManifold->setRollingResistanceImpulse(manifold.rollingResistanceImpulse);
manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1);
manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2);
manifold.externalContactManifold->setFrictionvec2(manifold.frictionvec2);
}
}
@ -826,14 +826,14 @@ void ContactSolver::applySplitImpulse(const Impulse& impulse,
// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
// for a contact point. The two vectors have to be such that : t1 x t2 = contactNormal.
void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
void ContactSolver::computeFrictionVectors(const vec3& deltaVelocity,
ContactPointSolver& contactPoint) const {
assert(contactPoint.normal.length() > 0.0);
// Compute the velocity difference vector in the tangential plane
Vector3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal;
Vector3 tangentVelocity = deltaVelocity - normalVelocity;
vec3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal;
vec3 tangentVelocity = deltaVelocity - normalVelocity;
// If the velocty difference in the tangential plane is not zero
float lengthTangenVelocity = tangentVelocity.length();
@ -851,19 +851,19 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
// The second friction vector is computed by the cross product of the firs
// friction vector and the contact normal
contactPoint.frictionVector2 =contactPoint.normal.cross(contactPoint.frictionVector1).getUnit();
contactPoint.frictionvec2 =contactPoint.normal.cross(contactPoint.frictionVector1).safeNormalized();
}
// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
// for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal.
void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
void ContactSolver::computeFrictionVectors(const vec3& deltaVelocity,
ContactManifoldSolver& contact) const {
assert(contact.normal.length() > 0.0);
// Compute the velocity difference vector in the tangential plane
Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal;
Vector3 tangentVelocity = deltaVelocity - normalVelocity;
vec3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal;
vec3 tangentVelocity = deltaVelocity - normalVelocity;
// If the velocty difference in the tangential plane is not zero
float lengthTangenVelocity = tangentVelocity.length();
@ -881,7 +881,7 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
// The second friction vector is computed by the cross product of the firs
// friction vector and the contact normal
contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit();
contact.frictionvec2 = contact.normal.cross(contact.frictionVector1).safeNormalized();
}
// Clean up the constraint solver

94
ephysics/engine/ContactSolver.h
View File

@ -112,46 +112,46 @@ class ContactSolver {
float penetrationSplitImpulse;
/// Accumulated rolling resistance impulse
Vector3 rollingResistanceImpulse;
vec3 rollingResistanceImpulse;
/// Normal vector of the contact
Vector3 normal;
vec3 normal;
/// First friction vector in the tangent plane
Vector3 frictionVector1;
vec3 frictionVector1;
/// Second friction vector in the tangent plane
Vector3 frictionVector2;
vec3 frictionvec2;
/// Old first friction vector in the tangent plane
Vector3 oldFrictionVector1;
vec3 oldFrictionVector1;
/// Old second friction vector in the tangent plane
Vector3 oldFrictionVector2;
vec3 oldFrictionvec2;
/// Vector from the body 1 center to the contact point
Vector3 r1;
vec3 r1;
/// Vector from the body 2 center to the contact point
Vector3 r2;
vec3 r2;
/// Cross product of r1 with 1st friction vector
Vector3 r1CrossT1;
vec3 r1CrossT1;
/// Cross product of r1 with 2nd friction vector
Vector3 r1CrossT2;
vec3 r1CrossT2;
/// Cross product of r2 with 1st friction vector
Vector3 r2CrossT1;
vec3 r2CrossT1;
/// Cross product of r2 with 2nd friction vector
Vector3 r2CrossT2;
vec3 r2CrossT2;
/// Cross product of r1 with the contact normal
Vector3 r1CrossN;
vec3 r1CrossN;
/// Cross product of r2 with the contact normal
Vector3 r2CrossN;
vec3 r2CrossN;
/// Penetration depth
float penetrationDepth;
@ -195,10 +195,10 @@ class ContactSolver {
float massInverseBody2;
/// Inverse inertia tensor of body 1
Matrix3x3 inverseInertiaTensorBody1;
etk::Matrix3x3 inverseInertiaTensorBody1;
/// Inverse inertia tensor of body 2
Matrix3x3 inverseInertiaTensorBody2;
etk::Matrix3x3 inverseInertiaTensorBody2;
/// Contact point constraints
ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];
@ -227,31 +227,31 @@ class ContactSolver {
// - Variables used when friction constraints are apply at the center of the manifold-//
/// Average normal vector of the contact manifold
Vector3 normal;
vec3 normal;
/// Point on body 1 where to apply the friction constraints
Vector3 frictionPointBody1;
vec3 frictionPointBody1;
/// Point on body 2 where to apply the friction constraints
Vector3 frictionPointBody2;
vec3 frictionPointBody2;
/// R1 vector for the friction constraints
Vector3 r1Friction;
vec3 r1Friction;
/// R2 vector for the friction constraints
Vector3 r2Friction;
vec3 r2Friction;
/// Cross product of r1 with 1st friction vector
Vector3 r1CrossT1;
vec3 r1CrossT1;
/// Cross product of r1 with 2nd friction vector
Vector3 r1CrossT2;
vec3 r1CrossT2;
/// Cross product of r2 with 1st friction vector
Vector3 r2CrossT1;
vec3 r2CrossT1;
/// Cross product of r2 with 2nd friction vector
Vector3 r2CrossT2;
vec3 r2CrossT2;
/// Matrix K for the first friction constraint
float inverseFriction1Mass;
@ -263,19 +263,19 @@ class ContactSolver {
float inverseTwistFrictionMass;
/// Matrix K for the rolling resistance constraint
Matrix3x3 inverseRollingResistance;
etk::Matrix3x3 inverseRollingResistance;
/// First friction direction at contact manifold center
Vector3 frictionVector1;
vec3 frictionVector1;
/// Second friction direction at contact manifold center
Vector3 frictionVector2;
vec3 frictionvec2;
/// Old 1st friction direction at contact manifold center
Vector3 oldFrictionVector1;
vec3 oldFrictionVector1;
/// Old 2nd friction direction at contact manifold center
Vector3 oldFrictionVector2;
vec3 oldFrictionvec2;
/// First friction direction impulse at manifold center
float friction1Impulse;
@ -287,7 +287,7 @@ class ContactSolver {
float frictionTwistImpulse;
/// Rolling resistance impulse
Vector3 rollingResistanceImpulse;
vec3 rollingResistanceImpulse;
};
// -------------------- Constants --------------------- //
@ -304,10 +304,10 @@ class ContactSolver {
// -------------------- Attributes -------------------- //
/// Split linear velocities for the position contact solver (split impulse)
Vector3* m_splitLinearVelocities;
vec3* m_splitLinearVelocities;
/// Split angular velocities for the position contact solver (split impulse)
Vector3* m_splitAngularVelocities;
vec3* m_splitAngularVelocities;
/// Current time step
float m_timeStep;
@ -319,10 +319,10 @@ class ContactSolver {
uint32_t m_numberContactManifolds;
/// Array of linear velocities
Vector3* mLinearVelocities;
vec3* mLinearVelocities;
/// Array of angular velocities
Vector3* mAngularVelocities;
vec3* mAngularVelocities;
/// Reference to the map of rigid body to their index in the constrained velocities array
const std::map<RigidBody*, uint32_t>& m_mapBodyToConstrainedVelocityIndex;
@ -363,13 +363,13 @@ class ContactSolver {
/// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
/// plane for a contact point. The two vectors have to be
/// such that : t1 x t2 = contactNormal.
void computeFrictionVectors(const Vector3& deltaVelocity,
void computeFrictionVectors(const vec3& deltaVelocity,
ContactPointSolver &contactPoint) const;
/// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
/// plane for a contact manifold. The two vectors have to be
/// such that : t1 x t2 = contactNormal.
void computeFrictionVectors(const Vector3& deltaVelocity,
void computeFrictionVectors(const vec3& deltaVelocity,
ContactManifoldSolver& contactPoint) const;
/// Compute a penetration constraint impulse
@ -398,12 +398,12 @@ class ContactSolver {
void initializeForIsland(float dt, Island* island);
/// Set the split velocities arrays
void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
Vector3* splitAngularVelocities);
void setSplitVelocitiesArrays(vec3* splitLinearVelocities,
vec3* splitAngularVelocities);
/// Set the constrained velocities arrays
void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities);
void setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities,
vec3* constrainedAngularVelocities);
/// Warm start the solver.
void warmStart();
@ -430,8 +430,8 @@ class ContactSolver {
};
// Set the split velocities arrays
inline void ContactSolver::setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
Vector3* splitAngularVelocities) {
inline void ContactSolver::setSplitVelocitiesArrays(vec3* splitLinearVelocities,
vec3* splitAngularVelocities) {
assert(splitLinearVelocities != NULL);
assert(splitAngularVelocities != NULL);
m_splitLinearVelocities = splitLinearVelocities;
@ -439,8 +439,8 @@ inline void ContactSolver::setSplitVelocitiesArrays(Vector3* splitLinearVelociti
}
// Set the constrained velocities arrays
inline void ContactSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities) {
inline void ContactSolver::setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities,
vec3* constrainedAngularVelocities) {
assert(constrainedLinearVelocities != NULL);
assert(constrainedAngularVelocities != NULL);
mLinearVelocities = constrainedLinearVelocities;
@ -509,9 +509,9 @@ inline const Impulse ContactSolver::computeFriction1Impulse(float deltaLambda,
inline const Impulse ContactSolver::computeFriction2Impulse(float deltaLambda,
const ContactPointSolver& contactPoint)
const {
return Impulse(-contactPoint.frictionVector2 * deltaLambda,
return Impulse(-contactPoint.frictionvec2 * deltaLambda,
-contactPoint.r1CrossT2 * deltaLambda,
contactPoint.frictionVector2 * deltaLambda,
contactPoint.frictionvec2 * deltaLambda,
contactPoint.r2CrossT2 * deltaLambda);
}

52
ephysics/engine/DynamicsWorld.cpp
View File

@ -19,7 +19,7 @@ using namespace std;
/**
* @param gravity Gravity vector in the world (in meters per second squared)
*/
DynamicsWorld::DynamicsWorld(const Vector3 &gravity)
DynamicsWorld::DynamicsWorld(const vec3 &gravity)
: CollisionWorld(),
m_contactSolver(m_mapBodyToConstrainedVelocityIndex),
m_constraintSolver(m_mapBodyToConstrainedVelocityIndex),
@ -168,8 +168,8 @@ void DynamicsWorld::int32_tegrateRigidBodiesPositions() {
// Get the constrained velocity
uint32_t indexArray = m_mapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
Vector3 newLinVelocity = m_constrainedLinearVelocities[indexArray];
Vector3 newAngVelocity = m_constrainedAngularVelocities[indexArray];
vec3 newLinVelocity = m_constrainedLinearVelocities[indexArray];
vec3 newAngVelocity = m_constrainedAngularVelocities[indexArray];
// Add the split impulse velocity from Contact Solver (only used
// to update the position)
@ -180,14 +180,14 @@ void DynamicsWorld::int32_tegrateRigidBodiesPositions() {
}
// Get current position and orientation of the body
const Vector3& currentPosition = bodies[b]->m_centerOfMassWorld;
const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation();
const vec3& currentPosition = bodies[b]->m_centerOfMassWorld;
const etk::Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation();
// Update the new constrained position and orientation of the body
m_constrainedPositions[indexArray] = currentPosition + newLinVelocity * m_timeStep;
m_constrainedOrientations[indexArray] = currentOrientation +
Quaternion(0, newAngVelocity) *
currentOrientation * float(0.5) * m_timeStep;
etk::Quaternion(0, newAngVelocity) *
currentOrientation * 0.5f * m_timeStep;
}
}
}
@ -215,7 +215,7 @@ void DynamicsWorld::updateBodiesState() {
bodies[b]->m_centerOfMassWorld = m_constrainedPositions[index];
// Update the orientation of the body
bodies[b]->m_transform.setOrientation(m_constrainedOrientations[index].getUnit());
bodies[b]->m_transform.setOrientation(m_constrainedOrientations[index].safeNormalized());
// Update the transform of the body (using the new center of mass and new orientation)
bodies[b]->updateTransformWithCenterOfMass();
@ -238,12 +238,12 @@ void DynamicsWorld::initVelocityArrays() {
}
m_numberBodiesCapacity = nbBodies;
// TODO : Use better memory allocation here
m_splitLinearVelocities = new Vector3[m_numberBodiesCapacity];
m_splitAngularVelocities = new Vector3[m_numberBodiesCapacity];
m_constrainedLinearVelocities = new Vector3[m_numberBodiesCapacity];
m_constrainedAngularVelocities = new Vector3[m_numberBodiesCapacity];
m_constrainedPositions = new Vector3[m_numberBodiesCapacity];
m_constrainedOrientations = new Quaternion[m_numberBodiesCapacity];
m_splitLinearVelocities = new vec3[m_numberBodiesCapacity];
m_splitAngularVelocities = new vec3[m_numberBodiesCapacity];
m_constrainedLinearVelocities = new vec3[m_numberBodiesCapacity];
m_constrainedAngularVelocities = new vec3[m_numberBodiesCapacity];
m_constrainedPositions = new vec3[m_numberBodiesCapacity];
m_constrainedOrientations = new etk::Quaternion[m_numberBodiesCapacity];
assert(m_splitLinearVelocities != NULL);
assert(m_splitAngularVelocities != NULL);
assert(m_constrainedLinearVelocities != NULL);
@ -254,8 +254,8 @@ void DynamicsWorld::initVelocityArrays() {
// Reset the velocities arrays
for (uint32_t i=0; i<m_numberBodiesCapacity; i++) {
m_splitLinearVelocities[i].setToZero();
m_splitAngularVelocities[i].setToZero();
m_splitLinearVelocities[i].setZero();
m_splitAngularVelocities[i].setZero();
}
// Initialize the map of body indexes in the velocity arrays
@ -293,8 +293,8 @@ void DynamicsWorld::int32_tegrateRigidBodiesVelocities() {
// Insert the body int32_to the map of constrained velocities
uint32_t indexBody = m_mapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
assert(m_splitLinearVelocities[indexBody] == Vector3(0, 0, 0));
assert(m_splitAngularVelocities[indexBody] == Vector3(0, 0, 0));
assert(m_splitLinearVelocities[indexBody] == vec3(0, 0, 0));
assert(m_splitAngularVelocities[indexBody] == vec3(0, 0, 0));
// Integrate the external force to get the new velocity of the body
m_constrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() +
@ -326,8 +326,8 @@ void DynamicsWorld::int32_tegrateRigidBodiesVelocities() {
// => v2 = v1 * (1 - c * dt)
float linDampingFactor = bodies[b]->getLinearDamping();
float angDampingFactor = bodies[b]->getAngularDamping();
float linearDamping = pow(float(1.0) - linDampingFactor, m_timeStep);
float angularDamping = pow(float(1.0) - angDampingFactor, m_timeStep);
float linearDamping = pow(1.0f - linDampingFactor, m_timeStep);
float angularDamping = pow(1.0f - angDampingFactor, m_timeStep);
m_constrainedLinearVelocities[indexBody] *= linearDamping;
m_constrainedAngularVelocities[indexBody] *= angularDamping;
@ -422,10 +422,10 @@ void DynamicsWorld::solvePositionCorrection() {
// Create a rigid body int32_to the physics world
/**
* @param transform Transformation from body local-space to world-space
* @param transform etk::Transform3Dation from body local-space to world-space
* @return A pointer to the body that has been created in the world
*/
RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
RigidBody* DynamicsWorld::createRigidBody(const etk::Transform3D& transform) {
// Compute the body ID
bodyindex bodyID = computeNextAvailableBodyID();
@ -794,13 +794,13 @@ void DynamicsWorld::updateSleepingBodies() {
if (bodies[b]->getType() == STATIC) continue;
// If the body is velocity is large enough to stay awake
if (bodies[b]->getLinearVelocity().lengthSquare() > sleepLinearVelocitySquare ||
bodies[b]->getAngularVelocity().lengthSquare() > sleepAngularVelocitySquare ||
if (bodies[b]->getLinearVelocity().length2() > sleepLinearVelocitySquare ||
bodies[b]->getAngularVelocity().length2() > sleepAngularVelocitySquare ||
!bodies[b]->isAllowedToSleep()) {
// Reset the sleep time of the body
bodies[b]->m_sleepTime = float(0.0);
minSleepTime = float(0.0);
bodies[b]->m_sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else { // If the body velocity is bellow the sleeping velocity threshold

40
ephysics/engine/DynamicsWorld.h
View File

@ -51,7 +51,7 @@ class DynamicsWorld : public CollisionWorld {
std::set<Joint*> m_joints;
/// Gravity vector of the world
Vector3 m_gravity;
vec3 m_gravity;
/// Current frame time step (in seconds)
float m_timeStep;
@ -61,23 +61,23 @@ class DynamicsWorld : public CollisionWorld {
/// Array of constrained linear velocities (state of the linear velocities
/// after solving the constraints)
Vector3* m_constrainedLinearVelocities;
vec3* m_constrainedLinearVelocities;
/// Array of constrained angular velocities (state of the angular velocities
/// after solving the constraints)
Vector3* m_constrainedAngularVelocities;
vec3* m_constrainedAngularVelocities;
/// Split linear velocities for the position contact solver (split impulse)
Vector3* m_splitLinearVelocities;
vec3* m_splitLinearVelocities;
/// Split angular velocities for the position contact solver (split impulse)
Vector3* m_splitAngularVelocities;
vec3* m_splitAngularVelocities;
/// Array of constrained rigid bodies position (for position error correction)
Vector3* m_constrainedPositions;
vec3* m_constrainedPositions;
/// Array of constrained rigid bodies orientation (for position error correction)
Quaternion* m_constrainedOrientations;
etk::Quaternion* m_constrainedOrientations;
/// Map body to their index in the constrained velocities array
std::map<RigidBody*, uint32_t> m_mapBodyToConstrainedVelocityIndex;
@ -122,8 +122,8 @@ class DynamicsWorld : public CollisionWorld {
void resetBodiesForceAndTorque();
/// Update the position and orientation of a body
void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
Vector3 newAngVelocity);
void updatePositionAndOrientationOfBody(RigidBody* body, vec3 newLinVelocity,
vec3 newAngVelocity);
/// Compute and set the int32_terpolation factor to all bodies
void setInterpolationFactorToAllBodies();
@ -160,7 +160,7 @@ class DynamicsWorld : public CollisionWorld {
// -------------------- Methods -------------------- //
/// Constructor
DynamicsWorld(const Vector3& m_gravity);
DynamicsWorld(const vec3& m_gravity);
/// Destructor
virtual ~DynamicsWorld();
@ -191,7 +191,7 @@ class DynamicsWorld : public CollisionWorld {
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
/// Create a rigid body int32_to the physics world.
RigidBody* createRigidBody(const Transform& transform);
RigidBody* createRigidBody(const etk::Transform3D& transform);
/// Destroy a rigid body and all the joints which it belongs
void destroyRigidBody(RigidBody* rigidBody);
@ -203,10 +203,10 @@ class DynamicsWorld : public CollisionWorld {
void destroyJoint(Joint* joint);
/// Return the gravity vector of the world
Vector3 getGravity() const;
vec3 getGravity() const;
/// Set the gravity vector of the world
void setGravity(Vector3& gravity);
void setGravity(vec3& gravity);
/// Return if the gravity is on
bool isGravityEnabled() const;
@ -290,8 +290,8 @@ inline void DynamicsWorld::resetBodiesForceAndTorque() {
// For each body of the world
std::set<RigidBody*>::iterator it;
for (it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) {
(*it)->m_externalForce.setToZero();
(*it)->m_externalTorque.setToZero();
(*it)->m_externalForce.setZero();
(*it)->m_externalTorque.setZero();
}
}
@ -363,7 +363,7 @@ inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool
/**
* @return The current gravity vector (in meter per seconds squared)
*/
inline Vector3 DynamicsWorld::getGravity() const {
inline vec3 DynamicsWorld::getGravity() const {
return m_gravity;
}
@ -371,7 +371,7 @@ inline Vector3 DynamicsWorld::getGravity() const {
/**
* @param gravity The gravity vector (in meter per seconds squared)
*/
inline void DynamicsWorld::setGravity(Vector3& gravity) {
inline void DynamicsWorld::setGravity(vec3& gravity) {
m_gravity = gravity;
}
@ -448,7 +448,7 @@ inline float DynamicsWorld::getSleepLinearVelocity() const {
* @param sleepLinearVelocity The sleep linear velocity (in meters per second)
*/
inline void DynamicsWorld::setSleepLinearVelocity(float sleepLinearVelocity) {
assert(sleepLinearVelocity >= float(0.0));
assert(sleepLinearVelocity >= 0.0f);
m_sleepLinearVelocity = sleepLinearVelocity;
}
@ -468,7 +468,7 @@ inline float DynamicsWorld::getSleepAngularVelocity() const {
* @param sleepAngularVelocity The sleep angular velocity (in radian per second)
*/
inline void DynamicsWorld::setSleepAngularVelocity(float sleepAngularVelocity) {
assert(sleepAngularVelocity >= float(0.0));
assert(sleepAngularVelocity >= 0.0f);
m_sleepAngularVelocity = sleepAngularVelocity;
}
@ -486,7 +486,7 @@ inline float DynamicsWorld::getTimeBeforeSleep() const {
* @param timeBeforeSleep Time a body is required to stay still before sleeping (in seconds)
*/
inline void DynamicsWorld::setTimeBeforeSleep(float timeBeforeSleep) {
assert(timeBeforeSleep >= float(0.0));
assert(timeBeforeSleep >= 0.0f);
m_timeBeforeSleep = timeBeforeSleep;
}

12
ephysics/engine/Impulse.h
View File

@ -28,22 +28,22 @@ struct Impulse {
// -------------------- Attributes -------------------- //
/// Linear impulse applied to the first body
const Vector3 linearImpulseBody1;
const vec3 linearImpulseBody1;
/// Angular impulse applied to the first body
const Vector3 angularImpulseBody1;
const vec3 angularImpulseBody1;
/// Linear impulse applied to the second body
const Vector3 linearImpulseBody2;
const vec3 linearImpulseBody2;
/// Angular impulse applied to the second body
const Vector3 angularImpulseBody2;
const vec3 angularImpulseBody2;
// -------------------- Methods -------------------- //
/// Constructor
Impulse(const Vector3& initLinearImpulseBody1, const Vector3& initAngularImpulseBody1,
const Vector3& initLinearImpulseBody2, const Vector3& initAngularImpulseBody2)
Impulse(const vec3& initLinearImpulseBody1, const vec3& initAngularImpulseBody1,
const vec3& initLinearImpulseBody2, const vec3& initAngularImpulseBody2)
: linearImpulseBody1(initLinearImpulseBody1),
angularImpulseBody1(initAngularImpulseBody1),
linearImpulseBody2(initLinearImpulseBody2),

4
ephysics/engine/Material.h
View File

@ -82,7 +82,7 @@ inline float Material::getBounciness() const {
* @param bounciness Bounciness factor (between 0 and 1) where 1 is very bouncy
*/
inline void Material::setBounciness(float bounciness) {
assert(bounciness >= float(0.0) && bounciness <= float(1.0));
assert(bounciness >= 0.0f && bounciness <= 1.0f);
m_bounciness = bounciness;
}
@ -101,7 +101,7 @@ inline float Material::getFrictionCoefficient() const {
* @param frictionCoefficient Friction coefficient (positive value)
*/
inline void Material::setFrictionCoefficient(float frictionCoefficient) {
assert(frictionCoefficient >= float(0.0));
assert(frictionCoefficient >= 0.0f);
m_frictionCoefficient = frictionCoefficient;
}

10
ephysics/engine/OverlappingPair.h
View File

@ -34,7 +34,7 @@ class OverlappingPair {
ContactManifoldSet m_contactManifoldSet;
/// Cached previous separating axis
Vector3 m_cachedSeparatingAxis;
vec3 m_cachedSeparatingAxis;
// -------------------- Methods -------------------- //
@ -68,10 +68,10 @@ class OverlappingPair {
void update();
/// Return the cached separating axis
Vector3 getCachedSeparatingAxis() const;
vec3 getCachedSeparatingAxis() const;
/// Set the cached separating axis
void setCachedSeparatingAxis(const Vector3& axis);
void setCachedSeparatingAxis(const vec3& axis);
/// Return the number of contacts in the cache
uint32_t getNbContactPoints() const;
@ -114,12 +114,12 @@ inline void OverlappingPair::update() {
}
// Return the cached separating axis
inline Vector3 OverlappingPair::getCachedSeparatingAxis() const {
inline vec3 OverlappingPair::getCachedSeparatingAxis() const {
return m_cachedSeparatingAxis;
}
// Set the cached separating axis
inline void OverlappingPair::setCachedSeparatingAxis(const Vector3& axis) {
inline void OverlappingPair::setCachedSeparatingAxis(const vec3& axis) {
m_cachedSeparatingAxis = axis;
}

68
ephysics/mathematics/Matrix2x2.cpp
View File

@ -1,68 +0,0 @@
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/mathematics/Matrix2x2.h>
using namespace reactphysics3d;
// Constructor of the class Matrix2x2
Matrix2x2::Matrix2x2() {
// Initialize all values in the matrix to zero
setAllValues(0.0, 0.0, 0.0, 0.0);
}
// Constructor
Matrix2x2::Matrix2x2(float value) {
setAllValues(value, value, value, value);
}
// Constructor with arguments
Matrix2x2::Matrix2x2(float a1, float a2, float b1, float b2) {
// Initialize the matrix with the values
setAllValues(a1, a2, b1, b2);
}
// Destructor
Matrix2x2::~Matrix2x2() {
}
// Copy-constructor
Matrix2x2::Matrix2x2(const Matrix2x2& matrix) {
setAllValues(matrix.mRows[0][0], matrix.mRows[0][1],
matrix.mRows[1][0], matrix.mRows[1][1]);
}
// Assignment operator
Matrix2x2& Matrix2x2::operator=(const Matrix2x2& matrix) {
// Check for self-assignment
if (&matrix != this) {
setAllValues(matrix.mRows[0][0], matrix.mRows[0][1],
matrix.mRows[1][0], matrix.mRows[1][1]);
}
return *this;
}
// Return the inverse matrix
Matrix2x2 Matrix2x2::getInverse() const {
// Compute the determinant of the matrix
float determinant = getDeterminant();
// Check if the determinant is equal to zero
assert(std::abs(determinant) > MACHINE_EPSILON);
float invDeterminant = float(1.0) / determinant;
Matrix2x2 tempMatrix(mRows[1][1], -mRows[0][1], -mRows[1][0], mRows[0][0]);
// Return the inverse matrix
return (invDeterminant * tempMatrix);
}

297
ephysics/mathematics/Matrix2x2.h
View File

@ -1,297 +0,0 @@
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <cassert>
#include <ephysics/mathematics/Vector2.h>
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Class Matrix2x2
/**
* This class represents a 2x2 matrix.
*/
class Matrix2x2 {
private :
// -------------------- Attributes -------------------- //
/// Rows of the matrix;
Vector2 mRows[2];
public :
// -------------------- Methods -------------------- //
/// Constructor
Matrix2x2();
/// Constructor
Matrix2x2(float value);
/// Constructor
Matrix2x2(float a1, float a2, float b1, float b2);
/// Destructor
~Matrix2x2();
/// Copy-constructor
Matrix2x2(const Matrix2x2& matrix);
/// Assignment operator
Matrix2x2& operator=(const Matrix2x2& matrix);
/// Set all the values in the matrix
void setAllValues(float a1, float a2, float b1, float b2);
/// Set the matrix to zero
void setToZero();
/// Return a column
Vector2 getColumn(int32_t i) const;
/// Return a row
Vector2 getRow(int32_t i) const;
/// Return the transpose matrix
Matrix2x2 getTranspose() const;
/// Return the determinant of the matrix
float getDeterminant() const;
/// Return the trace of the matrix
float getTrace() const;
/// Return the inverse matrix
Matrix2x2 getInverse() const;
/// Return the matrix with absolute values
Matrix2x2 getAbsoluteMatrix() const;
/// Set the matrix to the identity matrix
void setToIdentity();
/// Return the 2x2 identity matrix
static Matrix2x2 identity();
/// Return the 2x2 zero matrix
static Matrix2x2 zero();
/// Overloaded operator for addition
friend Matrix2x2 operator+(const Matrix2x2& matrix1, const Matrix2x2& matrix2);
/// Overloaded operator for substraction
friend Matrix2x2 operator-(const Matrix2x2& matrix1, const Matrix2x2& matrix2);
/// Overloaded operator for the negative of the matrix
friend Matrix2x2 operator-(const Matrix2x2& matrix);
/// Overloaded operator for multiplication with a number
friend Matrix2x2 operator*(float nb, const Matrix2x2& matrix);
/// Overloaded operator for multiplication with a matrix
friend Matrix2x2 operator*(const Matrix2x2& matrix, float nb);
/// Overloaded operator for matrix multiplication
friend Matrix2x2 operator*(const Matrix2x2& matrix1, const Matrix2x2& matrix2);
/// Overloaded operator for multiplication with a vector
friend Vector2 operator*(const Matrix2x2& matrix, const Vector2& vector);
/// Overloaded operator for equality condition
bool operator==(const Matrix2x2& matrix) const;
/// Overloaded operator for the is different condition
bool operator!= (const Matrix2x2& matrix) const;
/// Overloaded operator for addition with assignment
Matrix2x2& operator+=(const Matrix2x2& matrix);
/// Overloaded operator for substraction with assignment
Matrix2x2& operator-=(const Matrix2x2& matrix);
/// Overloaded operator for multiplication with a number with assignment
Matrix2x2& operator*=(float nb);
/// Overloaded operator to read element of the matrix.
const Vector2& operator[](int32_t row) const;
/// Overloaded operator to read/write element of the matrix.
Vector2& operator[](int32_t row);
};
// Method to set all the values in the matrix
inline void Matrix2x2::setAllValues(float a1, float a2,
float b1, float b2) {
mRows[0][0] = a1; mRows[0][1] = a2;
mRows[1][0] = b1; mRows[1][1] = b2;
}
// Set the matrix to zero
inline void Matrix2x2::setToZero() {
mRows[0].setToZero();
mRows[1].setToZero();
}
// Return a column
inline Vector2 Matrix2x2::getColumn(int32_t i) const {
assert(i>= 0 && i<2);
return Vector2(mRows[0][i], mRows[1][i]);
}
// Return a row
inline Vector2 Matrix2x2::getRow(int32_t i) const {
assert(i>= 0 && i<2);
return mRows[i];
}
// Return the transpose matrix
inline Matrix2x2 Matrix2x2::getTranspose() const {
// Return the transpose matrix
return Matrix2x2(mRows[0][0], mRows[1][0],
mRows[0][1], mRows[1][1]);
}
// Return the determinant of the matrix
inline float Matrix2x2::getDeterminant() const {
// Compute and return the determinant of the matrix
return mRows[0][0] * mRows[1][1] - mRows[1][0] * mRows[0][1];
}
// Return the trace of the matrix
inline float Matrix2x2::getTrace() const {
// Compute and return the trace
return (mRows[0][0] + mRows[1][1]);
}
// Set the matrix to the identity matrix
inline void Matrix2x2::setToIdentity() {
mRows[0][0] = 1.0; mRows[0][1] = 0.0;
mRows[1][0] = 0.0; mRows[1][1] = 1.0;
}
// Return the 2x2 identity matrix
inline Matrix2x2 Matrix2x2::identity() {
// Return the isdentity matrix
return Matrix2x2(1.0, 0.0, 0.0, 1.0);
}
// Return the 2x2 zero matrix
inline Matrix2x2 Matrix2x2::zero() {
return Matrix2x2(0.0, 0.0, 0.0, 0.0);
}
// Return the matrix with absolute values
inline Matrix2x2 Matrix2x2::getAbsoluteMatrix() const {
return Matrix2x2(fabs(mRows[0][0]), fabs(mRows[0][1]),
fabs(mRows[1][0]), fabs(mRows[1][1]));
}
// Overloaded operator for addition
inline Matrix2x2 operator+(const Matrix2x2& matrix1, const Matrix2x2& matrix2) {
return Matrix2x2(matrix1.mRows[0][0] + matrix2.mRows[0][0],
matrix1.mRows[0][1] + matrix2.mRows[0][1],
matrix1.mRows[1][0] + matrix2.mRows[1][0],
matrix1.mRows[1][1] + matrix2.mRows[1][1]);
}
// Overloaded operator for substraction
inline Matrix2x2 operator-(const Matrix2x2& matrix1, const Matrix2x2& matrix2) {
return Matrix2x2(matrix1.mRows[0][0] - matrix2.mRows[0][0],
matrix1.mRows[0][1] - matrix2.mRows[0][1],
matrix1.mRows[1][0] - matrix2.mRows[1][0],
matrix1.mRows[1][1] - matrix2.mRows[1][1]);
}
// Overloaded operator for the negative of the matrix
inline Matrix2x2 operator-(const Matrix2x2& matrix) {
return Matrix2x2(-matrix.mRows[0][0], -matrix.mRows[0][1],
-matrix.mRows[1][0], -matrix.mRows[1][1]);
}
// Overloaded operator for multiplication with a number
inline Matrix2x2 operator*(float nb, const Matrix2x2& matrix) {
return Matrix2x2(matrix.mRows[0][0] * nb, matrix.mRows[0][1] * nb,
matrix.mRows[1][0] * nb, matrix.mRows[1][1] * nb);
}
// Overloaded operator for multiplication with a matrix
inline Matrix2x2 operator*(const Matrix2x2& matrix, float nb) {
return nb * matrix;
}
// Overloaded operator for matrix multiplication
inline Matrix2x2 operator*(const Matrix2x2& matrix1, const Matrix2x2& matrix2) {
return Matrix2x2(matrix1.mRows[0][0] * matrix2.mRows[0][0] + matrix1.mRows[0][1] *
matrix2.mRows[1][0],
matrix1.mRows[0][0] * matrix2.mRows[0][1] + matrix1.mRows[0][1] *
matrix2.mRows[1][1],
matrix1.mRows[1][0] * matrix2.mRows[0][0] + matrix1.mRows[1][1] *
matrix2.mRows[1][0],
matrix1.mRows[1][0] * matrix2.mRows[0][1] + matrix1.mRows[1][1] *
matrix2.mRows[1][1]);
}
// Overloaded operator for multiplication with a vector
inline Vector2 operator*(const Matrix2x2& matrix, const Vector2& vector) {
return Vector2(matrix.mRows[0][0]*vector.x + matrix.mRows[0][1]*vector.y,
matrix.mRows[1][0]*vector.x + matrix.mRows[1][1]*vector.y);
}
// Overloaded operator for equality condition
inline bool Matrix2x2::operator==(const Matrix2x2& matrix) const {
return (mRows[0][0] == matrix.mRows[0][0] && mRows[0][1] == matrix.mRows[0][1] &&
mRows[1][0] == matrix.mRows[1][0] && mRows[1][1] == matrix.mRows[1][1]);
}
// Overloaded operator for the is different condition
inline bool Matrix2x2::operator!= (const Matrix2x2& matrix) const {
return !(*this == matrix);
}
// Overloaded operator for addition with assignment
inline Matrix2x2& Matrix2x2::operator+=(const Matrix2x2& matrix) {
mRows[0][0] += matrix.mRows[0][0]; mRows[0][1] += matrix.mRows[0][1];
mRows[1][0] += matrix.mRows[1][0]; mRows[1][1] += matrix.mRows[1][1];
return *this;
}
// Overloaded operator for substraction with assignment
inline Matrix2x2& Matrix2x2::operator-=(const Matrix2x2& matrix) {
mRows[0][0] -= matrix.mRows[0][0]; mRows[0][1] -= matrix.mRows[0][1];
mRows[1][0] -= matrix.mRows[1][0]; mRows[1][1] -= matrix.mRows[1][1];
return *this;
}
// Overloaded operator for multiplication with a number with assignment
inline Matrix2x2& Matrix2x2::operator*=(float nb) {
mRows[0][0] *= nb; mRows[0][1] *= nb;
mRows[1][0] *= nb; mRows[1][1] *= nb;
return *this;
}
// Overloaded operator to return a row of the matrix.
/// This operator is also used to access a matrix value using the syntax
/// matrix[row][col].
inline const Vector2& Matrix2x2::operator[](int32_t row) const {
return mRows[row];
}
// Overloaded operator to return a row of the matrix.
/// This operator is also used to access a matrix value using the syntax
/// matrix[row][col].
inline Vector2& Matrix2x2::operator[](int32_t row) {
return mRows[row];
}
}

83
ephysics/mathematics/Matrix3x3.cpp
View File

@ -1,83 +0,0 @@
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <iostream>
#include <ephysics/mathematics/Matrix3x3.h>
// Namespaces
using namespace reactphysics3d;
// Constructor of the class Matrix3x3
Matrix3x3::Matrix3x3() {
// Initialize all values in the matrix to zero
setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
}
// Constructor
Matrix3x3::Matrix3x3(float value) {
setAllValues(value, value, value, value, value, value, value, value, value);
}
// Constructor with arguments
Matrix3x3::Matrix3x3(float a1, float a2, float a3,
float b1, float b2, float b3,
float c1, float c2, float c3) {
// Initialize the matrix with the values
setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3);
}
// Destructor
Matrix3x3::~Matrix3x3() {
}
// Copy-constructor
Matrix3x3::Matrix3x3(const Matrix3x3& matrix) {
setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[0][2],
matrix.mRows[1][0], matrix.mRows[1][1], matrix.mRows[1][2],
matrix.mRows[2][0], matrix.mRows[2][1], matrix.mRows[2][2]);
}
// Assignment operator
Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix) {
// Check for self-assignment
if (&matrix != this) {
setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[0][2],
matrix.mRows[1][0], matrix.mRows[1][1], matrix.mRows[1][2],
matrix.mRows[2][0], matrix.mRows[2][1], matrix.mRows[2][2]);
}
return *this;
}
// Return the inverse matrix
Matrix3x3 Matrix3x3::getInverse() const {
// Compute the determinant of the matrix
float determinant = getDeterminant();
// Check if the determinant is equal to zero
assert(std::abs(determinant) > MACHINE_EPSILON);
float invDeterminant = float(1.0) / determinant;
Matrix3x3 tempMatrix((mRows[1][1]*mRows[2][2]-mRows[2][1]*mRows[1][2]),
-(mRows[0][1]*mRows[2][2]-mRows[2][1]*mRows[0][2]),
(mRows[0][1]*mRows[1][2]-mRows[0][2]*mRows[1][1]),
-(mRows[1][0]*mRows[2][2]-mRows[2][0]*mRows[1][2]),
(mRows[0][0]*mRows[2][2]-mRows[2][0]*mRows[0][2]),
-(mRows[0][0]*mRows[1][2]-mRows[1][0]*mRows[0][2]),
(mRows[1][0]*mRows[2][1]-mRows[2][0]*mRows[1][1]),
-(mRows[0][0]*mRows[2][1]-mRows[2][0]*mRows[0][1]),
(mRows[0][0]*mRows[1][1]-mRows[0][1]*mRows[1][0]));
// Return the inverse matrix
return (invDeterminant * tempMatrix);
}

347
ephysics/mathematics/Matrix3x3.h
View File

@ -1,347 +0,0 @@
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <cassert>
#include <ephysics/mathematics/Vector3.h>
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Class Matrix3x3
/**
* This class represents a 3x3 matrix.
*/
class Matrix3x3 {
private :
// -------------------- Attributes -------------------- //
/// Rows of the matrix;
Vector3 mRows[3];
public :
// -------------------- Methods -------------------- //
/// Constructor
Matrix3x3();
/// Constructor
Matrix3x3(float value);
/// Constructor
Matrix3x3(float a1, float a2, float a3, float b1, float b2, float b3,
float c1, float c2, float c3);
/// Destructor
virtual ~Matrix3x3();
/// Copy-constructor
Matrix3x3(const Matrix3x3& matrix);
/// Assignment operator
Matrix3x3& operator=(const Matrix3x3& matrix);
/// Set all the values in the matrix
void setAllValues(float a1, float a2, float a3, float b1, float b2, float b3,
float c1, float c2, float c3);
/// Set the matrix to zero
void setToZero();
/// Return a column
Vector3 getColumn(int32_t i) const;
/// Return a row
Vector3 getRow(int32_t i) const;
/// Return the transpose matrix
Matrix3x3 getTranspose() const;
/// Return the determinant of the matrix
float getDeterminant() const;
/// Return the trace of the matrix
float getTrace() const;
/// Return the inverse matrix
Matrix3x3 getInverse() const;
/// Return the matrix with absolute values
Matrix3x3 getAbsoluteMatrix() const;
/// Set the matrix to the identity matrix
void setToIdentity();
/// Return the 3x3 identity matrix
static Matrix3x3 identity();
/// Return the 3x3 zero matrix
static Matrix3x3 zero();
/// Return a skew-symmetric matrix using a given vector that can be used
/// to compute cross product with another vector using matrix multiplication
static Matrix3x3 computeSkewSymmetricMatrixForCrossProduct(const Vector3& vector);
/// Overloaded operator for addition
friend Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2);
/// Overloaded operator for substraction
friend Matrix3x3 operator-(const Matrix3x3& matrix1, const Matrix3x3& matrix2);
/// Overloaded operator for the negative of the matrix
friend Matrix3x3 operator-(const Matrix3x3& matrix);
/// Overloaded operator for multiplication with a number
friend Matrix3x3 operator*(float nb, const Matrix3x3& matrix);
/// Overloaded operator for multiplication with a matrix
friend Matrix3x3 operator*(const Matrix3x3& matrix, float nb);
/// Overloaded operator for matrix multiplication
friend Matrix3x3 operator*(const Matrix3x3& matrix1, const Matrix3x3& matrix2);
/// Overloaded operator for multiplication with a vector
friend Vector3 operator*(const Matrix3x3& matrix, const Vector3& vector);
/// Overloaded operator for equality condition
bool operator==(const Matrix3x3& matrix) const;
/// Overloaded operator for the is different condition
bool operator!= (const Matrix3x3& matrix) const;
/// Overloaded operator for addition with assignment
Matrix3x3& operator+=(const Matrix3x3& matrix);
/// Overloaded operator for substraction with assignment
Matrix3x3& operator-=(const Matrix3x3& matrix);
/// Overloaded operator for multiplication with a number with assignment
Matrix3x3& operator*=(float nb);
/// Overloaded operator to read element of the matrix.
const Vector3& operator[](int32_t row) const;
/// Overloaded operator to read/write element of the matrix.
Vector3& operator[](int32_t row);
};
// Method to set all the values in the matrix
inline void Matrix3x3::setAllValues(float a1, float a2, float a3,
float b1, float b2, float b3,
float c1, float c2, float c3) {
mRows[0][0] = a1; mRows[0][1] = a2; mRows[0][2] = a3;
mRows[1][0] = b1; mRows[1][1] = b2; mRows[1][2] = b3;
mRows[2][0] = c1; mRows[2][1] = c2; mRows[2][2] = c3;
}
// Set the matrix to zero
inline void Matrix3x3::setToZero() {
mRows[0].setToZero();
mRows[1].setToZero();
mRows[2].setToZero();
}
// Return a column
inline Vector3 Matrix3x3::getColumn(int32_t i) const {
assert(i>= 0 && i<3);
return Vector3(mRows[0][i], mRows[1][i], mRows[2][i]);
}
// Return a row
inline Vector3 Matrix3x3::getRow(int32_t i) const {
assert(i>= 0 && i<3);
return mRows[i];
}
// Return the transpose matrix
inline Matrix3x3 Matrix3x3::getTranspose() const {
// Return the transpose matrix
return Matrix3x3(mRows[0][0], mRows[1][0], mRows[2][0],
mRows[0][1], mRows[1][1], mRows[2][1],
mRows[0][2], mRows[1][2], mRows[2][2]);
}
// Return the determinant of the matrix
inline float Matrix3x3::getDeterminant() const {
// Compute and return the determinant of the matrix
return (mRows[0][0]*(mRows[1][1]*mRows[2][2]-mRows[2][1]*mRows[1][2]) -
mRows[0][1]*(mRows[1][0]*mRows[2][2]-mRows[2][0]*mRows[1][2]) +
mRows[0][2]*(mRows[1][0]*mRows[2][1]-mRows[2][0]*mRows[1][1]));
}
// Return the trace of the matrix
inline float Matrix3x3::getTrace() const {
// Compute and return the trace
return (mRows[0][0] + mRows[1][1] + mRows[2][2]);
}
// Set the matrix to the identity matrix
inline void Matrix3x3::setToIdentity() {
mRows[0][0] = 1.0; mRows[0][1] = 0.0; mRows[0][2] = 0.0;
mRows[1][0] = 0.0; mRows[1][1] = 1.0; mRows[1][2] = 0.0;
mRows[2][0] = 0.0; mRows[2][1] = 0.0; mRows[2][2] = 1.0;
}
// Return the 3x3 identity matrix
inline Matrix3x3 Matrix3x3::identity() {
return Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
}
// Return the 3x3 zero matrix
inline Matrix3x3 Matrix3x3::zero() {
return Matrix3x3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
}
// Return a skew-symmetric matrix using a given vector that can be used
// to compute cross product with another vector using matrix multiplication
inline Matrix3x3 Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(const Vector3& vector) {
return Matrix3x3(0, -vector.z, vector.y, vector.z, 0, -vector.x, -vector.y, vector.x, 0);
}
// Return the matrix with absolute values
inline Matrix3x3 Matrix3x3::getAbsoluteMatrix() const {
return Matrix3x3(fabs(mRows[0][0]), fabs(mRows[0][1]), fabs(mRows[0][2]),
fabs(mRows[1][0]), fabs(mRows[1][1]), fabs(mRows[1][2]),
fabs(mRows[2][0]), fabs(mRows[2][1]), fabs(mRows[2][2]));
}
// Overloaded operator for addition
inline Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2) {
return Matrix3x3(matrix1.mRows[0][0] + matrix2.mRows[0][0], matrix1.mRows[0][1] +
matrix2.mRows[0][1], matrix1.mRows[0][2] + matrix2.mRows[0][2],
matrix1.mRows[1][0] + matrix2.mRows[1][0], matrix1.mRows[1][1] +
matrix2.mRows[1][1], matrix1.mRows[1][2] + matrix2.mRows[1][2],
matrix1.mRows[2][0] + matrix2.mRows[2][0], matrix1.mRows[2][1] +
matrix2.mRows[2][1], matrix1.mRows[2][2] + matrix2.mRows[2][2]);
}
// Overloaded operator for substraction
inline Matrix3x3 operator-(const Matrix3x3& matrix1, const Matrix3x3& matrix2) {
return Matrix3x3(matrix1.mRows[0][0] - matrix2.mRows[0][0], matrix1.mRows[0][1] -
matrix2.mRows[0][1], matrix1.mRows[0][2] - matrix2.mRows[0][2],
matrix1.mRows[1][0] - matrix2.mRows[1][0], matrix1.mRows[1][1] -
matrix2.mRows[1][1], matrix1.mRows[1][2] - matrix2.mRows[1][2],
matrix1.mRows[2][0] - matrix2.mRows[2][0], matrix1.mRows[2][1] -
matrix2.mRows[2][1], matrix1.mRows[2][2] - matrix2.mRows[2][2]);
}
// Overloaded operator for the negative of the matrix
inline Matrix3x3 operator-(const Matrix3x3& matrix) {
return Matrix3x3(-matrix.mRows[0][0], -matrix.mRows[0][1], -matrix.mRows[0][2],
-matrix.mRows[1][0], -matrix.mRows[1][1], -matrix.mRows[1][2],
-matrix.mRows[2][0], -matrix.mRows[2][1], -matrix.mRows[2][2]);
}
// Overloaded operator for multiplication with a number
inline Matrix3x3 operator*(float nb, const Matrix3x3& matrix) {
return Matrix3x3(matrix.mRows[0][0] * nb, matrix.mRows[0][1] * nb, matrix.mRows[0][2] * nb,
matrix.mRows[1][0] * nb, matrix.mRows[1][1] * nb, matrix.mRows[1][2] * nb,
matrix.mRows[2][0] * nb, matrix.mRows[2][1] * nb, matrix.mRows[2][2] * nb);
}
// Overloaded operator for multiplication with a matrix
inline Matrix3x3 operator*(const Matrix3x3& matrix, float nb) {
return nb * matrix;
}
// Overloaded operator for matrix multiplication
inline Matrix3x3 operator*(const Matrix3x3& matrix1, const Matrix3x3& matrix2) {
return Matrix3x3(matrix1.mRows[0][0]*matrix2.mRows[0][0] + matrix1.mRows[0][1] *
matrix2.mRows[1][0] + matrix1.mRows[0][2]*matrix2.mRows[2][0],
matrix1.mRows[0][0]*matrix2.mRows[0][1] + matrix1.mRows[0][1] *
matrix2.mRows[1][1] + matrix1.mRows[0][2]*matrix2.mRows[2][1],
matrix1.mRows[0][0]*matrix2.mRows[0][2] + matrix1.mRows[0][1] *
matrix2.mRows[1][2] + matrix1.mRows[0][2]*matrix2.mRows[2][2],
matrix1.mRows[1][0]*matrix2.mRows[0][0] + matrix1.mRows[1][1] *
matrix2.mRows[1][0] + matrix1.mRows[1][2]*matrix2.mRows[2][0],
matrix1.mRows[1][0]*matrix2.mRows[0][1] + matrix1.mRows[1][1] *
matrix2.mRows[1][1] + matrix1.mRows[1][2]*matrix2.mRows[2][1],
matrix1.mRows[1][0]*matrix2.mRows[0][2] + matrix1.mRows[1][1] *
matrix2.mRows[1][2] + matrix1.mRows[1][2]*matrix2.mRows[2][2],
matrix1.mRows[2][0]*matrix2.mRows[0][0] + matrix1.mRows[2][1] *
matrix2.mRows[1][0] + matrix1.mRows[2][2]*matrix2.mRows[2][0],
matrix1.mRows[2][0]*matrix2.mRows[0][1] + matrix1.mRows[2][1] *
matrix2.mRows[1][1] + matrix1.mRows[2][2]*matrix2.mRows[2][1],
matrix1.mRows[2][0]*matrix2.mRows[0][2] + matrix1.mRows[2][1] *
matrix2.mRows[1][2] + matrix1.mRows[2][2]*matrix2.mRows[2][2]);
}
// Overloaded operator for multiplication with a vector
inline Vector3 operator*(const Matrix3x3& matrix, const Vector3& vector) {
return Vector3(matrix.mRows[0][0]*vector.x + matrix.mRows[0][1]*vector.y +
matrix.mRows[0][2]*vector.z,
matrix.mRows[1][0]*vector.x + matrix.mRows[1][1]*vector.y +
matrix.mRows[1][2]*vector.z,
matrix.mRows[2][0]*vector.x + matrix.mRows[2][1]*vector.y +
matrix.mRows[2][2]*vector.z);
}
// Overloaded operator for equality condition
inline bool Matrix3x3::operator==(const Matrix3x3& matrix) const {
return (mRows[0][0] == matrix.mRows[0][0] && mRows[0][1] == matrix.mRows[0][1] &&
mRows[0][2] == matrix.mRows[0][2] &&
mRows[1][0] == matrix.mRows[1][0] && mRows[1][1] == matrix.mRows[1][1] &&
mRows[1][2] == matrix.mRows[1][2] &&
mRows[2][0] == matrix.mRows[2][0] && mRows[2][1] == matrix.mRows[2][1] &&
mRows[2][2] == matrix.mRows[2][2]);
}
// Overloaded operator for the is different condition
inline bool Matrix3x3::operator!= (const Matrix3x3& matrix) const {
return !(*this == matrix);
}
// Overloaded operator for addition with assignment
inline Matrix3x3& Matrix3x3::operator+=(const Matrix3x3& matrix) {
mRows[0][0] += matrix.mRows[0][0]; mRows[0][1] += matrix.mRows[0][1];
mRows[0][2] += matrix.mRows[0][2]; mRows[1][0] += matrix.mRows[1][0];
mRows[1][1] += matrix.mRows[1][1]; mRows[1][2] += matrix.mRows[1][2];
mRows[2][0] += matrix.mRows[2][0]; mRows[2][1] += matrix.mRows[2][1];
mRows[2][2] += matrix.mRows[2][2];
return *this;
}
// Overloaded operator for substraction with assignment
inline Matrix3x3& Matrix3x3::operator-=(const Matrix3x3& matrix) {
mRows[0][0] -= matrix.mRows[0][0]; mRows[0][1] -= matrix.mRows[0][1];
mRows[0][2] -= matrix.mRows[0][2]; mRows[1][0] -= matrix.mRows[1][0];
mRows[1][1] -= matrix.mRows[1][1]; mRows[1][2] -= matrix.mRows[1][2];
mRows[2][0] -= matrix.mRows[2][0]; mRows[2][1] -= matrix.mRows[2][1];
mRows[2][2] -= matrix.mRows[2][2];
return *this;
}
// Overloaded operator for multiplication with a number with assignment
inline Matrix3x3& Matrix3x3::operator*=(float nb) {
mRows[0][0] *= nb; mRows[0][1] *= nb; mRows[0][2] *= nb;
mRows[1][0] *= nb; mRows[1][1] *= nb; mRows[1][2] *= nb;
mRows[2][0] *= nb; mRows[2][1] *= nb; mRows[2][2] *= nb;
return *this;
}
// Overloaded operator to return a row of the matrix.
/// This operator is also used to access a matrix value using the syntax
/// matrix[row][col].
inline const Vector3& Matrix3x3::operator[](int32_t row) const {
return mRows[row];
}
// Overloaded operator to return a row of the matrix.
/// This operator is also used to access a matrix value using the syntax
/// matrix[row][col].
inline Vector3& Matrix3x3::operator[](int32_t row) {
return mRows[row];
}
}

241
ephysics/mathematics/Quaternion.cpp
View File

@ -1,241 +0,0 @@
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/mathematics/Quaternion.h>
#include <ephysics/mathematics/Vector3.h>
#include <cassert>
// Namespace
using namespace reactphysics3d;
// Constructor of the class
Quaternion::Quaternion() : x(0.0), y(0.0), z(0.0), w(0.0) {
}
// Constructor with arguments
Quaternion::Quaternion(float newX, float newY, float newZ, float newW)
:x(newX), y(newY), z(newZ), w(newW) {
}
// Constructor with the component w and the vector v=(x y z)
Quaternion::Quaternion(float newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), w(newW) {
}
// Constructor which convert Euler angles (in radians) to a quaternion
Quaternion::Quaternion(float angleX, float angleY, float angleZ) {
initWithEulerAngles(angleX, angleY, angleZ);
}
// Constructor which convert Euler angles (in radians) to a quaternion
Quaternion::Quaternion(const Vector3& eulerAngles) {
initWithEulerAngles(eulerAngles.x, eulerAngles.y, eulerAngles.z);
}
// Copy-constructor
Quaternion::Quaternion(const Quaternion& quaternion)
:x(quaternion.x), y(quaternion.y), z(quaternion.z), w(quaternion.w) {
}
// Create a unit quaternion from a rotation matrix
Quaternion::Quaternion(const Matrix3x3& matrix) {
// Get the trace of the matrix
float trace = matrix.getTrace();
float r;
float s;
if (trace < 0.0) {
if (matrix[1][1] > matrix[0][0]) {
if(matrix[2][2] > matrix[1][1]) {
r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + float(1.0));
s = float(0.5) / r;
// Compute the quaternion
x = (matrix[2][0] + matrix[0][2]) * s;
y = (matrix[1][2] + matrix[2][1]) * s;
z = float(0.5) * r;
w = (matrix[1][0] - matrix[0][1]) * s;
}
else {
r = sqrt(matrix[1][1] - matrix[2][2] - matrix[0][0] + float(1.0));
s = float(0.5) / r;
// Compute the quaternion
x = (matrix[0][1] + matrix[1][0]) * s;
y = float(0.5) * r;
z = (matrix[1][2] + matrix[2][1]) * s;
w = (matrix[0][2] - matrix[2][0]) * s;
}
}
else if (matrix[2][2] > matrix[0][0]) {
r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + float(1.0));
s = float(0.5) / r;
// Compute the quaternion
x = (matrix[2][0] + matrix[0][2]) * s;
y = (matrix[1][2] + matrix[2][1]) * s;
z = float(0.5) * r;
w = (matrix[1][0] - matrix[0][1]) * s;
}
else {
r = sqrt(matrix[0][0] - matrix[1][1] - matrix[2][2] + float(1.0));
s = float(0.5) / r;
// Compute the quaternion
x = float(0.5) * r;
y = (matrix[0][1] + matrix[1][0]) * s;
z = (matrix[2][0] - matrix[0][2]) * s;
w = (matrix[2][1] - matrix[1][2]) * s;
}
}
else {
r = sqrt(trace + float(1.0));
s = float(0.5) / r;
// Compute the quaternion
x = (matrix[2][1] - matrix[1][2]) * s;
y = (matrix[0][2] - matrix[2][0]) * s;
z = (matrix[1][0] - matrix[0][1]) * s;
w = float(0.5) * r;
}
}
// Destructor
Quaternion::~Quaternion() {
}
// Compute the rotation angle (in radians) and the rotation axis
/// This method is used to get the rotation angle (in radian) and the unit
/// rotation axis of an orientation quaternion.
void Quaternion::getRotationAngleAxis(float& angle, Vector3& axis) const {
Quaternion quaternion;
// If the quaternion is unit
if (length() == 1.0) {
quaternion = *this;
}
else {
// We compute the unit quaternion
quaternion = getUnit();
}
// Compute the roation angle
angle = acos(quaternion.w) * float(2.0);
// Compute the 3D rotation axis
Vector3 rotationAxis(quaternion.x, quaternion.y, quaternion.z);
// Normalize the rotation axis
rotationAxis = rotationAxis.getUnit();
// Set the rotation axis values
axis.setAllValues(rotationAxis.x, rotationAxis.y, rotationAxis.z);
}
// Return the orientation matrix corresponding to this quaternion
Matrix3x3 Quaternion::getMatrix() const {
float nQ = x*x + y*y + z*z + w*w;
float s = 0.0;
if (nQ > 0.0) {
s = float(2.0) / nQ;
}
// Computations used for optimization (less multiplications)
float xs = x*s;
float ys = y*s;
float zs = z*s;
float wxs = w*xs;
float wys = w*ys;
float wzs = w*zs;
float xxs = x*xs;
float xys = x*ys;
float xzs = x*zs;
float yys = y*ys;
float yzs = y*zs;
float zzs = z*zs;
// Create the matrix corresponding to the quaternion
return Matrix3x3(float(1.0) - yys - zzs, xys-wzs, xzs + wys,
xys + wzs, float(1.0) - xxs - zzs, yzs-wxs,
xzs-wys, yzs + wxs, float(1.0) - xxs - yys);
}
// Compute the spherical linear int32_terpolation between two quaternions.
/// The t argument has to be such that 0 <= t <= 1. This method is static.
Quaternion Quaternion::slerp(const Quaternion& quaternion1,
const Quaternion& quaternion2, float t) {
assert(t >= 0.0 && t <= 1.0);
float invert = 1.0;
// Compute cos(theta) using the quaternion scalar product
float cosineTheta = quaternion1.dot(quaternion2);
// Take care of the sign of cosineTheta
if (cosineTheta < 0.0) {
cosineTheta = -cosineTheta;
invert = -1.0;
}
// Because of precision, if cos(theta) is nearly 1,
// therefore theta is nearly 0 and we can write
// sin((1-t)*theta) as (1-t) and sin(t*theta) as t
const float epsilon = float(0.00001);
if(1-cosineTheta < epsilon) {
return quaternion1 * (float(1.0)-t) + quaternion2 * (t * invert);
}
// Compute the theta angle
float theta = acos(cosineTheta);
// Compute sin(theta)
float sineTheta = sin(theta);
// Compute the two coefficients that are in the spherical linear int32_terpolation formula
float coeff1 = sin((float(1.0)-t)*theta) / sineTheta;
float coeff2 = sin(t*theta) / sineTheta * invert;
// Compute and return the int32_terpolated quaternion
return quaternion1 * coeff1 + quaternion2 * coeff2;
}
// Initialize the quaternion using Euler angles
void Quaternion::initWithEulerAngles(float angleX, float angleY, float angleZ) {
float angle = angleX * float(0.5);
const float sinX = std::sin(angle);
const float cosX = std::cos(angle);
angle = angleY * float(0.5);
const float sinY = std::sin(angle);
const float cosY = std::cos(angle);
angle = angleZ * float(0.5);
const float sinZ = std::sin(angle);
const float cosZ = std::cos(angle);
const float cosYcosZ = cosY * cosZ;
const float sinYcosZ = sinY * cosZ;
const float cosYsinZ = cosY * sinZ;
const float sinYsinZ = sinY * sinZ;
x = sinX * cosYcosZ - cosX * sinYsinZ;
y = cosX * sinYcosZ + sinX * cosYsinZ;
z = cosX * cosYsinZ - sinX * sinYcosZ;
w = cosX * cosYcosZ + sinX * sinYsinZ;
// Normalize the quaternion
normalize();
}

330
ephysics/mathematics/Quaternion.h
View File

@ -1,330 +0,0 @@
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <cmath>
#include <ephysics/mathematics/Vector3.h>
#include <ephysics/mathematics/Matrix3x3.h>
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Class Quaternion
/**
* This class represents a quaternion. We use the notation :
* q = (x*i, y*j, z*k, w) to represent a quaternion.
*/
struct Quaternion {
public :
// -------------------- Attributes -------------------- //
/// Component x
float x;
/// Component y
float y;
/// Component z
float z;
/// Component w
float w;
// -------------------- Methods -------------------- //
/// Constructor
Quaternion();
/// Constructor with arguments
Quaternion(float newX, float newY, float newZ, float newW);
/// Constructor with the component w and the vector v=(x y z)
Quaternion(float newW, const Vector3& v);
/// Constructor which convert Euler angles (in radians) to a quaternion
Quaternion(float angleX, float angleY, float angleZ);
/// Constructor which convert Euler angles (in radians) to a quaternion
Quaternion(const Vector3& eulerAngles);
/// Copy-constructor
Quaternion(const Quaternion& quaternion);
/// Create a unit quaternion from a rotation matrix
Quaternion(const Matrix3x3& matrix);
/// Destructor
~Quaternion();
/// Set all the values
void setAllValues(float newX, float newY, float newZ, float newW);
/// Set the quaternion to zero
void setToZero();
/// Set to the identity quaternion
void setToIdentity();
/// Return the vector v=(x y z) of the quaternion
Vector3 getVectorV() const;
/// Return the length of the quaternion
float length() const;
/// Return the square of the length of the quaternion
float lengthSquare() const;
/// Normalize the quaternion
void normalize();
/// Inverse the quaternion
void inverse();
/// Return the unit quaternion
Quaternion getUnit() const;
/// Return the conjugate quaternion
Quaternion getConjugate() const;
/// Return the inverse of the quaternion
Quaternion getInverse() const;
/// Return the orientation matrix corresponding to this quaternion
Matrix3x3 getMatrix() const;
/// Return the identity quaternion
static Quaternion identity();
/// Dot product between two quaternions
float dot(const Quaternion& quaternion) const;
/// Compute the rotation angle (in radians) and the rotation axis
void getRotationAngleAxis(float& angle, Vector3& axis) const;
/// Compute the spherical linear int32_terpolation between two quaternions
static Quaternion slerp(const Quaternion& quaternion1, const Quaternion& quaternion2,
float t);
/// Overloaded operator for the addition
Quaternion operator+(const Quaternion& quaternion) const;
/// Overloaded operator for the substraction
Quaternion operator-(const Quaternion& quaternion) const;
/// Overloaded operator for addition with assignment
Quaternion& operator+=(const Quaternion& quaternion);
/// Overloaded operator for substraction with assignment
Quaternion& operator-=(const Quaternion& quaternion);
/// Overloaded operator for the multiplication with a constant
Quaternion operator*(float nb) const;
/// Overloaded operator for the multiplication
Quaternion operator*(const Quaternion& quaternion) const;
/// Overloaded operator for the multiplication with a vector
Vector3 operator*(const Vector3& point) const;
/// Overloaded operator for assignment
Quaternion& operator=(const Quaternion& quaternion);
/// Overloaded operator for equality condition
bool operator==(const Quaternion& quaternion) const;
private:
/// Initialize the quaternion using Euler angles
void initWithEulerAngles(float angleX, float angleY, float angleZ);
};
/// Set all the values
inline void Quaternion::setAllValues(float newX, float newY, float newZ, float newW) {
x = newX;
y = newY;
z = newZ;
w = newW;
}
/// Set the quaternion to zero
inline void Quaternion::setToZero() {
x = 0;
y = 0;
z = 0;
w = 0;
}
// Set to the identity quaternion
inline void Quaternion::setToIdentity() {
x = 0;
y = 0;
z = 0;
w = 1;
}
// Return the vector v=(x y z) of the quaternion
inline Vector3 Quaternion::getVectorV() const {
// Return the vector v
return Vector3(x, y, z);
}
// Return the length of the quaternion (inline)
inline float Quaternion::length() const {
return sqrt(x*x + y*y + z*z + w*w);
}
// Return the square of the length of the quaternion
inline float Quaternion::lengthSquare() const {
return x*x + y*y + z*z + w*w;
}
// Normalize the quaternion
inline void Quaternion::normalize() {
float l = length();
// Check if the length is not equal to zero
assert (l > MACHINE_EPSILON);
x /= l;
y /= l;
z /= l;
w /= l;
}
// Inverse the quaternion
inline void Quaternion::inverse() {
// Get the square length of the quaternion
float lengthSquareQuaternion = lengthSquare();
assert (lengthSquareQuaternion > MACHINE_EPSILON);
// Compute and return the inverse quaternion
x /= -lengthSquareQuaternion;
y /= -lengthSquareQuaternion;
z /= -lengthSquareQuaternion;
w /= lengthSquareQuaternion;
}
// Return the unit quaternion
inline Quaternion Quaternion::getUnit() const {
float lengthQuaternion = length();
// Check if the length is not equal to zero
assert (lengthQuaternion > MACHINE_EPSILON);
// Compute and return the unit quaternion
return Quaternion(x / lengthQuaternion, y / lengthQuaternion,
z / lengthQuaternion, w / lengthQuaternion);
}
// Return the identity quaternion
inline Quaternion Quaternion::identity() {
return Quaternion(0.0, 0.0, 0.0, 1.0);
}
// Return the conjugate of the quaternion (inline)
inline Quaternion Quaternion::getConjugate() const {
return Quaternion(-x, -y, -z, w);
}
// Return the inverse of the quaternion (inline)
inline Quaternion Quaternion::getInverse() const {
float lengthSquareQuaternion = lengthSquare();
assert (lengthSquareQuaternion > MACHINE_EPSILON);
// Compute and return the inverse quaternion
return Quaternion(-x / lengthSquareQuaternion, -y / lengthSquareQuaternion,
-z / lengthSquareQuaternion, w / lengthSquareQuaternion);
}
// Scalar product between two quaternions
inline float Quaternion::dot(const Quaternion& quaternion) const {
return (x*quaternion.x + y*quaternion.y + z*quaternion.z + w*quaternion.w);
}
// Overloaded operator for the addition of two quaternions
inline Quaternion Quaternion::operator+(const Quaternion& quaternion) const {
// Return the result quaternion
return Quaternion(x + quaternion.x, y + quaternion.y, z + quaternion.z, w + quaternion.w);
}
// Overloaded operator for the substraction of two quaternions
inline Quaternion Quaternion::operator-(const Quaternion& quaternion) const {
// Return the result of the substraction
return Quaternion(x - quaternion.x, y - quaternion.y, z - quaternion.z, w - quaternion.w);
}
// Overloaded operator for addition with assignment
inline Quaternion& Quaternion::operator+=(const Quaternion& quaternion) {
x += quaternion.x;
y += quaternion.y;
z += quaternion.z;
w += quaternion.w;
return *this;
}
// Overloaded operator for substraction with assignment
inline Quaternion& Quaternion::operator-=(const Quaternion& quaternion) {
x -= quaternion.x;
y -= quaternion.y;
z -= quaternion.z;
w -= quaternion.w;
return *this;
}
// Overloaded operator for the multiplication with a constant
inline Quaternion Quaternion::operator*(float nb) const {
return Quaternion(nb * x, nb * y, nb * z, nb * w);
}
// Overloaded operator for the multiplication of two quaternions
inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const {
return Quaternion(w * quaternion.w - getVectorV().dot(quaternion.getVectorV()),
w * quaternion.getVectorV() + quaternion.w * getVectorV() +
getVectorV().cross(quaternion.getVectorV()));
}
// Overloaded operator for the multiplication with a vector.
/// This methods rotates a point given the rotation of a quaternion.
inline Vector3 Quaternion::operator*(const Vector3& point) const {
Quaternion p(point.x, point.y, point.z, 0.0);
return (((*this) * p) * getConjugate()).getVectorV();
}
// Overloaded operator for the assignment
inline Quaternion& Quaternion::operator=(const Quaternion& quaternion) {
// Check for self-assignment
if (this != &quaternion) {
x = quaternion.x;
y = quaternion.y;
z = quaternion.z;
w = quaternion.w;
}
// Return this quaternion
return *this;
}
// Overloaded operator for equality condition
inline bool Quaternion::operator==(const Quaternion& quaternion) const {
return (x == quaternion.x && y == quaternion.y &&
z == quaternion.z && w == quaternion.w);
}
}

8
ephysics/mathematics/Ray.h
View File

@ -6,7 +6,7 @@
#pragma once
// Libraries
#include <ephysics/mathematics/Vector3.h>
#include <etk/math/Vector3D.hpp>
/// ReactPhysics3D namespace
namespace reactphysics3d {
@ -24,10 +24,10 @@ struct Ray {
// -------------------- Attributes -------------------- //
/// First point of the ray (origin)
Vector3 point1;
vec3 point1;
/// Second point of the ray
Vector3 point2;
vec3 point2;
/// Maximum fraction value
float maxFraction;
@ -35,7 +35,7 @@ struct Ray {
// -------------------- Methods -------------------- //
/// Constructor with arguments
Ray(const Vector3& p1, const Vector3& p2, float maxFrac = float(1.0))
Ray(const vec3& p1, const vec3& p2, float maxFrac = 1.0f)
: point1(p1), point2(p2), maxFraction(maxFrac) {
}

39
ephysics/mathematics/Transform.cpp
View File

@ -1,39 +0,0 @@
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/mathematics/Transform.h>
// Namespaces
using namespace reactphysics3d;
// Constructor
Transform::Transform() : m_position(Vector3(0.0, 0.0, 0.0)), m_orientation(Quaternion::identity()) {
}
// Constructor
Transform::Transform(const Vector3& position, const Matrix3x3& orientation)
: m_position(position), m_orientation(Quaternion(orientation)) {
}
// Constructor
Transform::Transform(const Vector3& position, const Quaternion& orientation)
: m_position(position), m_orientation(orientation) {
}
// Copy-constructor
Transform::Transform(const Transform& transform)
: m_position(transform.m_position), m_orientation(transform.m_orientation) {
}
// Destructor
Transform::~Transform() {
}

205
ephysics/mathematics/Transform.h
View File

@ -1,205 +0,0 @@
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/mathematics/Matrix3x3.h>
#include <ephysics/mathematics/Vector3.h>
#include <ephysics/mathematics/Quaternion.h>
// ReactPhysiscs3D namespace
namespace reactphysics3d {
// Class Transform
/**
* This class represents a position and an orientation in 3D. It can
* also be seen as representing a translation and a rotation.
*/
class Transform {
private :
// -------------------- Attributes -------------------- //
/// Position
Vector3 m_position;
/// Orientation
Quaternion m_orientation;
public :
// -------------------- Methods -------------------- //
/// Constructor
Transform();
/// Constructor
Transform(const Vector3& position, const Matrix3x3& orientation);
/// Constructor
Transform(const Vector3& position, const Quaternion& orientation);
/// Destructor
~Transform();
/// Copy-constructor
Transform(const Transform& transform);
/// Return the origin of the transform
const Vector3& getPosition() const;
/// Set the origin of the transform
void setPosition(const Vector3& position);
/// Return the orientation quaternion
const Quaternion& getOrientation() const;
/// Set the rotation quaternion
void setOrientation(const Quaternion& orientation);
/// Set the transform to the identity transform
void setToIdentity();
/// Set the transform from an OpenGL transform matrix
void setFromOpenGL(float* openglMatrix);
/// Get the OpenGL matrix of the transform
void getOpenGLMatrix(float* openglMatrix) const;
/// Return the inverse of the transform
Transform getInverse() const;
/// Return an int32_terpolated transform
static Transform int32_terpolateTransforms(const Transform& oldTransform,
const Transform& newTransform,
float int32_terpolationFactor);
/// Return the identity transform
static Transform identity();
/// Return the transformed vector
Vector3 operator*(const Vector3& vector) const;
/// Operator of multiplication of a transform with another one
Transform operator*(const Transform& transform2) const;
/// Return true if the two transforms are equal
bool operator==(const Transform& transform2) const;
/// Return true if the two transforms are different
bool operator!=(const Transform& transform2) const;
/// Assignment operator
Transform& operator=(const Transform& transform);
};
// Return the position of the transform
inline const Vector3& Transform::getPosition() const {
return m_position;
}
// Set the origin of the transform
inline void Transform::setPosition(const Vector3& position) {
m_position = position;
}
// Return the rotation matrix
inline const Quaternion& Transform::getOrientation() const {
return m_orientation;
}
// Set the rotation matrix of the transform
inline void Transform::setOrientation(const Quaternion& orientation) {
m_orientation = orientation;
}
// Set the transform to the identity transform
inline void Transform::setToIdentity() {
m_position = Vector3(0.0, 0.0, 0.0);
m_orientation = Quaternion::identity();
}
// Set the transform from an OpenGL transform matrix
inline void Transform::setFromOpenGL(float* openglMatrix) {
Matrix3x3 matrix(openglMatrix[0], openglMatrix[4], openglMatrix[8],
openglMatrix[1], openglMatrix[5], openglMatrix[9],
openglMatrix[2], openglMatrix[6], openglMatrix[10]);
m_orientation = Quaternion(matrix);
m_position.setAllValues(openglMatrix[12], openglMatrix[13], openglMatrix[14]);
}
// Get the OpenGL matrix of the transform
inline void Transform::getOpenGLMatrix(float* openglMatrix) const {
const Matrix3x3& matrix = m_orientation.getMatrix();
openglMatrix[0] = matrix[0][0]; openglMatrix[1] = matrix[1][0];
openglMatrix[2] = matrix[2][0]; openglMatrix[3] = 0.0;
openglMatrix[4] = matrix[0][1]; openglMatrix[5] = matrix[1][1];
openglMatrix[6] = matrix[2][1]; openglMatrix[7] = 0.0;
openglMatrix[8] = matrix[0][2]; openglMatrix[9] = matrix[1][2];
openglMatrix[10] = matrix[2][2]; openglMatrix[11] = 0.0;
openglMatrix[12] = m_position.x; openglMatrix[13] = m_position.y;
openglMatrix[14] = m_position.z; openglMatrix[15] = 1.0;
}
// Return the inverse of the transform
inline Transform Transform::getInverse() const {
const Quaternion& invQuaternion = m_orientation.getInverse();
Matrix3x3 invMatrix = invQuaternion.getMatrix();
return Transform(invMatrix * (-m_position), invQuaternion);
}
// Return an int32_terpolated transform
inline Transform Transform::int32_terpolateTransforms(const Transform& oldTransform,
const Transform& newTransform,
float int32_terpolationFactor) {
Vector3 int32_terPosition = oldTransform.m_position * (float(1.0) - int32_terpolationFactor) +
newTransform.m_position * int32_terpolationFactor;
Quaternion int32_terOrientation = Quaternion::slerp(oldTransform.m_orientation,
newTransform.m_orientation,
int32_terpolationFactor);
return Transform(int32_terPosition, int32_terOrientation);
}
// Return the identity transform
inline Transform Transform::identity() {
return Transform(Vector3(0, 0, 0), Quaternion::identity());
}
// Return the transformed vector
inline Vector3 Transform::operator*(const Vector3& vector) const {
return (m_orientation.getMatrix() * vector) + m_position;
}
// Operator of multiplication of a transform with another one
inline Transform Transform::operator*(const Transform& transform2) const {
return Transform(m_position + m_orientation.getMatrix() * transform2.m_position,
m_orientation * transform2.m_orientation);
}
// Return true if the two transforms are equal
inline bool Transform::operator==(const Transform& transform2) const {
return (m_position == transform2.m_position) && (m_orientation == transform2.m_orientation);
}
// Return true if the two transforms are different
inline bool Transform::operator!=(const Transform& transform2) const {
return !(*this == transform2);
}
// Assignment operator
inline Transform& Transform::operator=(const Transform& transform) {
if (&transform != this) {
m_position = transform.m_position;
m_orientation = transform.m_orientation;
}
return *this;
}
}

54
ephysics/mathematics/Vector2.cpp
View File

@ -1,54 +0,0 @@
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/mathematics/Vector2.h>
#include <vector>
// Namespaces
using namespace reactphysics3d;
// Constructor
Vector2::Vector2() : x(0.0), y(0.0) {
}
// Constructor with arguments
Vector2::Vector2(float newX, float newY) : x(newX), y(newY) {
}
// Copy-constructor
Vector2::Vector2(const Vector2& vector) : x(vector.x), y(vector.y) {
}
// Destructor
Vector2::~Vector2() {
}
// Return the corresponding unit vector
Vector2 Vector2::getUnit() const {
float lengthVector = length();
if (lengthVector < MACHINE_EPSILON) {
return *this;
}
// Compute and return the unit vector
float lengthInv = float(1.0) / lengthVector;
return Vector2(x * lengthInv, y * lengthInv);
}
// Return one unit orthogonal vector of the current vector
Vector2 Vector2::getOneUnitOrthogonalVector() const {
float l = length();
assert(l > MACHINE_EPSILON);
return Vector2(-y / l, x / l);
}

323
ephysics/mathematics/Vector2.h
View File

@ -1,323 +0,0 @@
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <cmath>
#include <cassert>
#include <ephysics/mathematics/mathematics_functions.h>
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Class Vector2
/**
* This class represents a 2D vector.
*/
struct Vector2 {
public:
// -------------------- Attributes -------------------- //
/// Component x
float x;
/// Component y
float y;
// -------------------- Methods -------------------- //
/// Constructor of the class Vector3D
Vector2();
/// Constructor with arguments
Vector2(float newX, float newY);
/// Copy-constructor
Vector2(const Vector2& vector);
/// Destructor
~Vector2();
/// Set all the values of the vector
void setAllValues(float newX, float newY);
/// Set the vector to zero
void setToZero();
/// Return the length of the vector
float length() const;
/// Return the square of the length of the vector
float lengthSquare() const;
/// Return the corresponding unit vector
Vector2 getUnit() const;
/// Return one unit orthogonal vector of the current vector
Vector2 getOneUnitOrthogonalVector() const;
/// Return true if the vector is unit and false otherwise
bool isUnit() const;
/// Return true if the current vector is the zero vector
bool isZero() const;
/// Dot product of two vectors
float dot(const Vector2& vector) const;
/// Normalize the vector
void normalize();
/// Return the corresponding absolute value vector
Vector2 getAbsoluteVector() const;
/// Return the axis with the minimal value
int32_t getMinAxis() const;
/// Return the axis with the maximal value
int32_t getMaxAxis() const;
/// Overloaded operator for the equality condition
bool operator== (const Vector2& vector) const;
/// Overloaded operator for the is different condition
bool operator!= (const Vector2& vector) const;
/// Overloaded operator for addition with assignment
Vector2& operator+=(const Vector2& vector);
/// Overloaded operator for substraction with assignment
Vector2& operator-=(const Vector2& vector);
/// Overloaded operator for multiplication with a number with assignment
Vector2& operator*=(float number);
/// Overloaded operator for division by a number with assignment
Vector2& operator/=(float number);
/// Overloaded operator for value access
float& operator[] (int32_t index);
/// Overloaded operator for value access
const float& operator[] (int32_t index) const;
/// Overloaded operator
Vector2& operator=(const Vector2& vector);
/// Overloaded less than operator for ordering to be used inside std::set for instance
bool operator<(const Vector2& vector) const;
/// Return a vector taking the minimum components of two vectors
static Vector2 min(const Vector2& vector1, const Vector2& vector2);
/// Return a vector taking the maximum components of two vectors
static Vector2 max(const Vector2& vector1, const Vector2& vector2);
/// Return the zero vector
static Vector2 zero();
// -------------------- Friends -------------------- //
friend Vector2 operator+(const Vector2& vector1, const Vector2& vector2);
friend Vector2 operator-(const Vector2& vector1, const Vector2& vector2);
friend Vector2 operator-(const Vector2& vector);
friend Vector2 operator*(const Vector2& vector, float number);
friend Vector2 operator*(float number, const Vector2& vector);
friend Vector2 operator*(const Vector2& vector1, const Vector2& vector2);
friend Vector2 operator/(const Vector2& vector, float number);
friend Vector2 operator/(const Vector2& vector1, const Vector2& vector2);
};
// Set the vector to zero
inline void Vector2::setToZero() {
x = 0;
y = 0;
}
// Set all the values of the vector
inline void Vector2::setAllValues(float newX, float newY) {
x = newX;
y = newY;
}
// Return the length of the vector
inline float Vector2::length() const {
return sqrt(x*x + y*y);
}
// Return the square of the length of the vector
inline float Vector2::lengthSquare() const {
return x*x + y*y;
}
// Scalar product of two vectors (inline)
inline float Vector2::dot(const Vector2& vector) const {
return (x*vector.x + y*vector.y);
}
// Normalize the vector
inline void Vector2::normalize() {
float l = length();
if (l < MACHINE_EPSILON) {
return;
}
x /= l;
y /= l;
}
// Return the corresponding absolute value vector
inline Vector2 Vector2::getAbsoluteVector() const {
return Vector2(std::abs(x), std::abs(y));
}
// Return the axis with the minimal value
inline int32_t Vector2::getMinAxis() const {
return (x < y ? 0 : 1);
}
// Return the axis with the maximal value
inline int32_t Vector2::getMaxAxis() const {
return (x < y ? 1 : 0);
}
// Return true if the vector is unit and false otherwise
inline bool Vector2::isUnit() const {
return approxEqual(lengthSquare(), 1.0);
}
// Return true if the vector is the zero vector
inline bool Vector2::isZero() const {
return approxEqual(lengthSquare(), 0.0);
}
// Overloaded operator for the equality condition
inline bool Vector2::operator== (const Vector2& vector) const {
return (x == vector.x && y == vector.y);
}
// Overloaded operator for the is different condition
inline bool Vector2::operator!= (const Vector2& vector) const {
return !(*this == vector);
}
// Overloaded operator for addition with assignment
inline Vector2& Vector2::operator+=(const Vector2& vector) {
x += vector.x;
y += vector.y;
return *this;
}
// Overloaded operator for substraction with assignment
inline Vector2& Vector2::operator-=(const Vector2& vector) {
x -= vector.x;
y -= vector.y;
return *this;
}
// Overloaded operator for multiplication with a number with assignment
inline Vector2& Vector2::operator*=(float number) {
x *= number;
y *= number;
return *this;
}
// Overloaded operator for division by a number with assignment
inline Vector2& Vector2::operator/=(float number) {
assert(number > std::numeric_limits<float>::epsilon());
x /= number;
y /= number;
return *this;
}
// Overloaded operator for value access
inline float& Vector2::operator[] (int32_t index) {
return (&x)[index];
}
// Overloaded operator for value access
inline const float& Vector2::operator[] (int32_t index) const {
return (&x)[index];
}
// Overloaded operator for addition
inline Vector2 operator+(const Vector2& vector1, const Vector2& vector2) {
return Vector2(vector1.x + vector2.x, vector1.y + vector2.y);
}
// Overloaded operator for substraction
inline Vector2 operator-(const Vector2& vector1, const Vector2& vector2) {
return Vector2(vector1.x - vector2.x, vector1.y - vector2.y);
}
// Overloaded operator for the negative of a vector
inline Vector2 operator-(const Vector2& vector) {
return Vector2(-vector.x, -vector.y);
}
// Overloaded operator for multiplication with a number
inline Vector2 operator*(const Vector2& vector, float number) {
return Vector2(number * vector.x, number * vector.y);
}
// Overloaded operator for multiplication of two vectors
inline Vector2 operator*(const Vector2& vector1, const Vector2& vector2) {
return Vector2(vector1.x * vector2.x, vector1.y * vector2.y);
}
// Overloaded operator for division by a number
inline Vector2 operator/(const Vector2& vector, float number) {
assert(number > MACHINE_EPSILON);
return Vector2(vector.x / number, vector.y / number);
}
// Overload operator for division between two vectors
inline Vector2 operator/(const Vector2& vector1, const Vector2& vector2) {
assert(vector2.x > MACHINE_EPSILON);
assert(vector2.y > MACHINE_EPSILON);
return Vector2(vector1.x / vector2.x, vector1.y / vector2.y);
}
// Overloaded operator for multiplication with a number
inline Vector2 operator*(float number, const Vector2& vector) {
return vector * number;
}
// Assignment operator
inline Vector2& Vector2::operator=(const Vector2& vector) {
if (&vector != this) {
x = vector.x;
y = vector.y;
}
return *this;
}
// Overloaded less than operator for ordering to be used inside std::set for instance
inline bool Vector2::operator<(const Vector2& vector) const {
return (x == vector.x ? y < vector.y : x < vector.x);
}
// Return a vector taking the minimum components of two vectors
inline Vector2 Vector2::min(const Vector2& vector1, const Vector2& vector2) {
return Vector2(std::min(vector1.x, vector2.x),
std::min(vector1.y, vector2.y));
}
// Return a vector taking the maximum components of two vectors
inline Vector2 Vector2::max(const Vector2& vector1, const Vector2& vector2) {
return Vector2(std::max(vector1.x, vector2.x),
std::max(vector1.y, vector2.y));
}
// Return the zero vector
inline Vector2 Vector2::zero() {
return Vector2(0, 0);
}
}

67
ephysics/mathematics/Vector3.cpp
View File

@ -1,67 +0,0 @@
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/mathematics/Vector3.h>
#include <iostream>
#include <vector>
// Namespaces
using namespace reactphysics3d;
// Constructor of the class Vector3D
Vector3::Vector3() : x(0.0), y(0.0), z(0.0) {
}
// Constructor with arguments
Vector3::Vector3(float newX, float newY, float newZ) : x(newX), y(newY), z(newZ) {
}
// Copy-constructor
Vector3::Vector3(const Vector3& vector) : x(vector.x), y(vector.y), z(vector.z) {
}
// Destructor
Vector3::~Vector3() {
}
// Return the corresponding unit vector
Vector3 Vector3::getUnit() const {
float lengthVector = length();
if (lengthVector < MACHINE_EPSILON) {
return *this;
}
// Compute and return the unit vector
float lengthInv = float(1.0) / lengthVector;
return Vector3(x * lengthInv, y * lengthInv, z * lengthInv);
}
// Return one unit orthogonal vector of the current vector
Vector3 Vector3::getOneUnitOrthogonalVector() const {
assert(length() > MACHINE_EPSILON);
// Get the minimum element of the vector
Vector3 vectorAbs(fabs(x), fabs(y), fabs(z));
int32_t minElement = vectorAbs.getMinAxis();
if (minElement == 0) {
return Vector3(0.0, -z, y) / sqrt(y*y + z*z);
}
else if (minElement == 1) {
return Vector3(-z, 0.0, x) / sqrt(x*x + z*z);
}
else {
return Vector3(-y, x, 0.0) / sqrt(x*x + y*y);
}
}

363
ephysics/mathematics/Vector3.h
View File

@ -1,363 +0,0 @@
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <cmath>
#include <cassert>
#include <ephysics/mathematics/mathematics_functions.h>
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Class Vector3
/**
* This class represents a 3D vector.
*/
struct Vector3 {
public:
// -------------------- Attributes -------------------- //
/// Component x
float x;
/// Component y
float y;
/// Component z
float z;
// -------------------- Methods -------------------- //
/// Constructor of the class Vector3D
Vector3();
/// Constructor with arguments
Vector3(float newX, float newY, float newZ);
/// Copy-constructor
Vector3(const Vector3& vector);
/// Destructor
~Vector3();
/// Set all the values of the vector
void setAllValues(float newX, float newY, float newZ);
/// Set the vector to zero
void setToZero();
/// Return the length of the vector
float length() const;
/// Return the square of the length of the vector
float lengthSquare() const;
/// Return the corresponding unit vector
Vector3 getUnit() const;
/// Return one unit orthogonal vector of the current vector
Vector3 getOneUnitOrthogonalVector() const;
/// Return true if the vector is unit and false otherwise
bool isUnit() const;
/// Return true if the current vector is the zero vector
bool isZero() const;
/// Dot product of two vectors
float dot(const Vector3& vector) const;
/// Cross product of two vectors
Vector3 cross(const Vector3& vector) const;
/// Normalize the vector
void normalize();
/// Return the corresponding absolute value vector
Vector3 getAbsoluteVector() const;
/// Return the axis with the minimal value
int32_t getMinAxis() const;
/// Return the axis with the maximal value
int32_t getMaxAxis() const;
/// Return the minimum value among the three components of a vector
float getMinValue() const;
/// Return the maximum value among the three components of a vector
float getMaxValue() const;
/// Overloaded operator for the equality condition
bool operator== (const Vector3& vector) const;
/// Overloaded operator for the is different condition
bool operator!= (const Vector3& vector) const;
/// Overloaded operator for addition with assignment
Vector3& operator+=(const Vector3& vector);
/// Overloaded operator for substraction with assignment
Vector3& operator-=(const Vector3& vector);
/// Overloaded operator for multiplication with a number with assignment
Vector3& operator*=(float number);
/// Overloaded operator for division by a number with assignment
Vector3& operator/=(float number);
/// Overloaded operator for value access
float& operator[] (int32_t index);
/// Overloaded operator for value access
const float& operator[] (int32_t index) const;
/// Overloaded operator
Vector3& operator=(const Vector3& vector);
/// Overloaded less than operator for ordering to be used inside std::set for instance
bool operator<(const Vector3& vector) const;
/// Return a vector taking the minimum components of two vectors
static Vector3 min(const Vector3& vector1, const Vector3& vector2);
/// Return a vector taking the maximum components of two vectors
static Vector3 max(const Vector3& vector1, const Vector3& vector2);
/// Return the zero vector
static Vector3 zero();
// -------------------- Friends -------------------- //
friend Vector3 operator+(const Vector3& vector1, const Vector3& vector2);
friend Vector3 operator-(const Vector3& vector1, const Vector3& vector2);
friend Vector3 operator-(const Vector3& vector);
friend Vector3 operator*(const Vector3& vector, float number);
friend Vector3 operator*(float number, const Vector3& vector);
friend Vector3 operator*(const Vector3& vector1, const Vector3& vector2);
friend Vector3 operator/(const Vector3& vector, float number);
friend Vector3 operator/(const Vector3& vector1, const Vector3& vector2);
};
// Set the vector to zero
inline void Vector3::setToZero() {
x = 0;
y = 0;
z = 0;
}
// Set all the values of the vector
inline void Vector3::setAllValues(float newX, float newY, float newZ) {
x = newX;
y = newY;
z = newZ;
}
// Return the length of the vector
inline float Vector3::length() const {
return sqrt(x*x + y*y + z*z);
}
// Return the square of the length of the vector
inline float Vector3::lengthSquare() const {
return x*x + y*y + z*z;
}
// Scalar product of two vectors (inline)
inline float Vector3::dot(const Vector3& vector) const {
return (x*vector.x + y*vector.y + z*vector.z);
}
// Cross product of two vectors (inline)
inline Vector3 Vector3::cross(const Vector3& vector) const {
return Vector3(y * vector.z - z * vector.y,
z * vector.x - x * vector.z,
x * vector.y - y * vector.x);
}
// Normalize the vector
inline void Vector3::normalize() {
float l = length();
if (l < MACHINE_EPSILON) {
return;
}
x /= l;
y /= l;
z /= l;
}
// Return the corresponding absolute value vector
inline Vector3 Vector3::getAbsoluteVector() const {
return Vector3(std::abs(x), std::abs(y), std::abs(z));
}
// Return the axis with the minimal value
inline int32_t Vector3::getMinAxis() const {
return (x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2));
}
// Return the axis with the maximal value
inline int32_t Vector3::getMaxAxis() const {
return (x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0));
}
// Return true if the vector is unit and false otherwise
inline bool Vector3::isUnit() const {
return approxEqual(lengthSquare(), 1.0);
}
// Return true if the vector is the zero vector
inline bool Vector3::isZero() const {
return approxEqual(lengthSquare(), 0.0);
}
// Overloaded operator for the equality condition
inline bool Vector3::operator== (const Vector3& vector) const {
return (x == vector.x && y == vector.y && z == vector.z);
}
// Overloaded operator for the is different condition
inline bool Vector3::operator!= (const Vector3& vector) const {
return !(*this == vector);
}
// Overloaded operator for addition with assignment
inline Vector3& Vector3::operator+=(const Vector3& vector) {
x += vector.x;
y += vector.y;
z += vector.z;
return *this;
}
// Overloaded operator for substraction with assignment
inline Vector3& Vector3::operator-=(const Vector3& vector) {
x -= vector.x;
y -= vector.y;
z -= vector.z;
return *this;
}
// Overloaded operator for multiplication with a number with assignment
inline Vector3& Vector3::operator*=(float number) {
x *= number;
y *= number;
z *= number;
return *this;
}
// Overloaded operator for division by a number with assignment
inline Vector3& Vector3::operator/=(float number) {
assert(number > std::numeric_limits<float>::epsilon());
x /= number;
y /= number;
z /= number;
return *this;
}
// Overloaded operator for value access
inline float& Vector3::operator[] (int32_t index) {
return (&x)[index];
}
// Overloaded operator for value access
inline const float& Vector3::operator[] (int32_t index) const {
return (&x)[index];
}
// Overloaded operator for addition
inline Vector3 operator+(const Vector3& vector1, const Vector3& vector2) {
return Vector3(vector1.x + vector2.x, vector1.y + vector2.y, vector1.z + vector2.z);
}
// Overloaded operator for substraction
inline Vector3 operator-(const Vector3& vector1, const Vector3& vector2) {
return Vector3(vector1.x - vector2.x, vector1.y - vector2.y, vector1.z - vector2.z);
}
// Overloaded operator for the negative of a vector
inline Vector3 operator-(const Vector3& vector) {
return Vector3(-vector.x, -vector.y, -vector.z);
}
// Overloaded operator for multiplication with a number
inline Vector3 operator*(const Vector3& vector, float number) {
return Vector3(number * vector.x, number * vector.y, number * vector.z);
}
// Overloaded operator for division by a number
inline Vector3 operator/(const Vector3& vector, float number) {
assert(number > MACHINE_EPSILON);
return Vector3(vector.x / number, vector.y / number, vector.z / number);
}
// Overload operator for division between two vectors
inline Vector3 operator/(const Vector3& vector1, const Vector3& vector2) {
assert(vector2.x > MACHINE_EPSILON);
assert(vector2.y > MACHINE_EPSILON);
assert(vector2.z > MACHINE_EPSILON);
return Vector3(vector1.x / vector2.x, vector1.y / vector2.y, vector1.z / vector2.z);
}
// Overloaded operator for multiplication with a number
inline Vector3 operator*(float number, const Vector3& vector) {
return vector * number;
}
// Overload operator for multiplication between two vectors
inline Vector3 operator*(const Vector3& vector1, const Vector3& vector2) {
return Vector3(vector1.x * vector2.x, vector1.y * vector2.y, vector1.z * vector2.z);
}
// Assignment operator
inline Vector3& Vector3::operator=(const Vector3& vector) {
if (&vector != this) {
x = vector.x;
y = vector.y;
z = vector.z;
}
return *this;
}
// Overloaded less than operator for ordering to be used inside std::set for instance
inline bool Vector3::operator<(const Vector3& vector) const {
return (x == vector.x ? (y == vector.y ? z < vector.z : y < vector.y) : x < vector.x);
}
// Return a vector taking the minimum components of two vectors
inline Vector3 Vector3::min(const Vector3& vector1, const Vector3& vector2) {
return Vector3(std::min(vector1.x, vector2.x),
std::min(vector1.y, vector2.y),
std::min(vector1.z, vector2.z));
}
// Return a vector taking the maximum components of two vectors
inline Vector3 Vector3::max(const Vector3& vector1, const Vector3& vector2) {
return Vector3(std::max(vector1.x, vector2.x),
std::max(vector1.y, vector2.y),
std::max(vector1.z, vector2.z));
}
// Return the minimum value among the three components of a vector
inline float Vector3::getMinValue() const {
return std::min(std::min(x, y), z);
}
// Return the maximum value among the three components of a vector
inline float Vector3::getMaxValue() const {
return std::max(std::max(x, y), z);
}
// Return the zero vector
inline Vector3 Vector3::zero() {
return Vector3(0, 0, 0);
}
}

47
ephysics/mathematics/mathematics.h
View File

@ -1,38 +1,17 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 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_MATHEMATICS_H
#define REACTPHYSICS3D_MATHEMATICS_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/mathematics/Matrix3x3.h>
#include <ephysics/mathematics/Matrix2x2.h>
#include <ephysics/mathematics/Quaternion.h>
#include <ephysics/mathematics/Vector3.h>
#include <ephysics/mathematics/Vector2.h>
#include <ephysics/mathematics/Transform.h>
#include <etk/math/Matrix3x3.hpp>
#include <etk/math/Matrix2x2.hpp>
#include <etk/math/Quaternion.hpp>
#include <etk/math/Vector3D.hpp>
#include <etk/math/Vector2D.hpp>
#include <etk/math/Transform3D.hpp>
#include <ephysics/mathematics/Ray.h>
#include <ephysics/configuration.h>
#include <ephysics/mathematics/mathematics_functions.h>
@ -40,5 +19,3 @@
#include <cstdio>
#include <cassert>
#include <cmath>
#endif

20
ephysics/mathematics/mathematics_functions.cpp
View File

@ -6,18 +6,18 @@
// Libraries
#include <ephysics/mathematics/mathematics_functions.h>
#include <ephysics/mathematics/Vector3.h>
#include <etk/math/Vector3D.hpp>
using namespace reactphysics3d;
/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c)
/// This method uses the technique described in the book Real-Time collision detection by
/// Christer Ericson.
void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c,
const Vector3& p, float& u, float& v, float& w) {
const Vector3 v0 = b - a;
const Vector3 v1 = c - a;
const Vector3 v2 = p - a;
void reactphysics3d::computeBarycentricCoordinatesInTriangle(const vec3& a, const vec3& b, const vec3& c,
const vec3& p, float& u, float& v, float& w) {
const vec3 v0 = b - a;
const vec3 v1 = c - a;
const vec3 v2 = p - a;
float d00 = v0.dot(v0);
float d01 = v0.dot(v1);
@ -28,13 +28,13 @@ void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, c
float denom = d00 * d11 - d01 * d01;
v = (d11 * d20 - d01 * d21) / denom;
w = (d00 * d21 - d01 * d20) / denom;
u = float(1.0) - v - w;
u = 1.0f - v - w;
}
// Clamp a vector such that it is no longer than a given maximum length
Vector3 reactphysics3d::clamp(const Vector3& vector, float maxLength) {
if (vector.lengthSquare() > maxLength * maxLength) {
return vector.getUnit() * maxLength;
vec3 reactphysics3d::clamp(const vec3& vector, float maxLength) {
if (vector.length2() > maxLength * maxLength) {
return vector.safeNormalized() * maxLength;
}
return vector;
}

10
ephysics/mathematics/mathematics_functions.h
View File

@ -15,8 +15,6 @@
/// ReactPhysics3D namespace
namespace reactphysics3d {
struct Vector3;
// ---------- Mathematics functions ---------- //
/// Function to test if two real numbers are (almost) equal
@ -51,14 +49,14 @@ inline float max3(float a, float b, float c) {
/// Return true if two values have the same sign
inline bool sameSign(float a, float b) {
return a * b >= float(0.0);
return a * b >= 0.0f;
}
/// Clamp a vector such that it is no longer than a given maximum length
Vector3 clamp(const Vector3& vector, float maxLength);
vec3 clamp(const vec3& vector, float maxLength);
/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c)
void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c,
const Vector3& p, float& u, float& v, float& w);
void computeBarycentricCoordinatesInTriangle(const vec3& a, const vec3& b, const vec3& c,
const vec3& p, float& u, float& v, float& w);
}

12
lutin_ephysics.py
View File

@ -70,13 +70,7 @@ def configure(target, my_module):
'ephysics/body/RigidBody.cpp',
'ephysics/body/Body.cpp',
'ephysics/body/CollisionBody.cpp',
'ephysics/mathematics/Vector2.cpp',
'ephysics/mathematics/Vector3.cpp',
'ephysics/mathematics/Transform.cpp',
'ephysics/mathematics/Matrix2x2.cpp',
'ephysics/mathematics/mathematics_functions.cpp',
'ephysics/mathematics/Matrix3x3.cpp',
'ephysics/mathematics/Quaternion.cpp',
'ephysics/engine/CollisionWorld.cpp',
'ephysics/engine/OverlappingPair.cpp',
'ephysics/engine/Material.cpp',
@ -138,13 +132,7 @@ def configure(target, my_module):
'ephysics/body/CollisionBody.h',
'ephysics/mathematics/mathematics.h',
'ephysics/mathematics/Ray.h',
'ephysics/mathematics/Matrix2x2.h',
'ephysics/mathematics/Vector2.h',
'ephysics/mathematics/mathematics_functions.h',
'ephysics/mathematics/Vector3.h',
'ephysics/mathematics/Quaternion.h',
'ephysics/mathematics/Transform.h',
'ephysics/mathematics/Matrix3x3.h',
'ephysics/engine/CollisionWorld.h',
'ephysics/engine/DynamicsWorld.h',
'ephysics/engine/ConstraintSolver.h',

24
test/main.cpp
View File

@ -25,12 +25,12 @@
// Libraries
#include <test/TestSuite.h>
#include <test/tests/mathematics/TestVector2.h>
#include <test/tests/mathematics/TestVector3.h>
#include <test/tests/mathematics/TestTransform.h>
#include <test/tests/mathematics/TestQuaternion.h>
#include <test/tests/mathematics/TestMatrix2x2.h>
#include <test/tests/mathematics/TestMatrix3x3.h>
#include <test/tests/mathematics/Testvec2.h>
#include <test/tests/mathematics/Testvec3.h>
#include <test/tests/mathematics/Testetk::Transform3D.h>
#include <test/tests/mathematics/Testetk::Quaternion.h>
#include <test/tests/mathematics/Testetk::Matrix2x2.h>
#include <test/tests/mathematics/Testetk::Matrix3x3.h>
#include <test/tests/mathematics/TestMathematicsFunctions.h>
#include <test/tests/collision/TestPointInside.h>
#include <test/tests/collision/TestRaycast.h>
@ -46,12 +46,12 @@ int32_t main() {
// ---------- Mathematics tests ---------- //
testSuite.addTest(new TestVector2("Vector2"));
testSuite.addTest(new TestVector3("Vector3"));
testSuite.addTest(new TestTransform("Transform"));
testSuite.addTest(new TestQuaternion("Quaternion"));
testSuite.addTest(new TestMatrix3x3("Matrix3x3"));
testSuite.addTest(new TestMatrix2x2("Matrix2x2"));
testSuite.addTest(new Testvec2("vec2"));
testSuite.addTest(new Testvec3("vec3"));
testSuite.addTest(new Testetk::Transform3D("Transform"));
testSuite.addTest(new Testetk::Quaternion("Quaternion"));
testSuite.addTest(new Testetk::Matrix3x3("Matrix3x3"));
testSuite.addTest(new Testetk::Matrix2x2("Matrix2x2"));
testSuite.addTest(new TestMathematicsFunctions("Maths Functions"));
// ---------- Collision Detection tests ---------- //

280
test/tests/collision/TestAABB.h
View File

@ -43,10 +43,10 @@ class TestAABB : public Test {
// ---------- Atributes ---------- //
AABB mAABB1;
AABB mAABB2;
AABB mAABB3;
AABB mAABB4;
AABB m_AABB1;
AABB m_AABB2;
AABB m_AABB3;
AABB m_AABB4;
public :
@ -55,20 +55,20 @@ class TestAABB : public Test {
/// Constructor
TestAABB(const std::string& name) : Test(name) {
mAABB1.setMin(Vector3(-10, -10, -10));
mAABB1.setMax(Vector3(10, 10, 10));
m_AABB1.setMin(vec3(-10, -10, -10));
m_AABB1.setMax(vec3(10, 10, 10));
// AABB2 int32_tersect with AABB1
mAABB2.setMin(Vector3(-5, 4, -30));
mAABB2.setMax(Vector3(-2, 20, 30));
m_AABB2.setMin(vec3(-5, 4, -30));
m_AABB2.setMax(vec3(-2, 20, 30));
// AABB3 contains AABB1
mAABB3.setMin(Vector3(-25, -25, -25));
mAABB3.setMax(Vector3(25, 25, 25));
m_AABB3.setMin(vec3(-25, -25, -25));
m_AABB3.setMax(vec3(25, 25, 25));
// AABB4 does not collide with AABB1
mAABB4.setMin(Vector3(-40, -40, -40));
mAABB4.setMax(Vector3(-15, -25, -12));
m_AABB4.setMin(vec3(-40, -40, -40));
m_AABB4.setMax(vec3(-15, -25, -12));
}
/// Destructor
@ -87,201 +87,201 @@ class TestAABB : public Test {
// -------- Test constructors -------- //
AABB aabb1;
AABB aabb2(Vector3(-3, -5, -8), Vector3(65, -1, 56));
Vector3 trianglePoints[] = {
Vector3(-5, 7, 23), Vector3(45, -34, -73), Vector3(-12, 98, 76)
AABB aabb2(vec3(-3, -5, -8), vec3(65, -1, 56));
vec3 trianglePoints[] = {
vec3(-5, 7, 23), vec3(45, -34, -73), vec3(-12, 98, 76)
};
AABB aabb3 = AABB::createAABBForTriangle(trianglePoints);
test(aabb1.getMin().x == 0);
test(aabb1.getMin().y == 0);
test(aabb1.getMin().z == 0);
test(aabb1.getMax().x == 0);
test(aabb1.getMax().y == 0);
test(aabb1.getMax().z == 0);
test(aabb1.getMin().x() == 0);
test(aabb1.getMin().y() == 0);
test(aabb1.getMin().z() == 0);
test(aabb1.getMax().x() == 0);
test(aabb1.getMax().y() == 0);
test(aabb1.getMax().z() == 0);
test(aabb2.getMin().x == -3);
test(aabb2.getMin().y == -5);
test(aabb2.getMin().z == -8);
test(aabb2.getMax().x == 65);
test(aabb2.getMax().y == -1);
test(aabb2.getMax().z == 56);
test(aabb2.getMin().x() == -3);
test(aabb2.getMin().y() == -5);
test(aabb2.getMin().z() == -8);
test(aabb2.getMax().x() == 65);
test(aabb2.getMax().y() == -1);
test(aabb2.getMax().z() == 56);
test(aabb3.getMin().x == -12);
test(aabb3.getMin().y == -34);
test(aabb3.getMin().z == -73);
test(aabb3.getMax().x == 45);
test(aabb3.getMax().y == 98);
test(aabb3.getMax().z == 76);
test(aabb3.getMin().x() == -12);
test(aabb3.getMin().y() == -34);
test(aabb3.getMin().z() == -73);
test(aabb3.getMax().x() == 45);
test(aabb3.getMax().y() == 98);
test(aabb3.getMax().z() == 76);
// -------- Test inflate() -------- //
AABB aabbInflate(Vector3(-3, 4, 8), Vector3(-1, 6, 32));
AABB aabbInflate(vec3(-3, 4, 8), vec3(-1, 6, 32));
aabbInflate.inflate(1, 2, 3);
test(approxEqual(aabbInflate.getMin().x, -4, 0.00001));
test(approxEqual(aabbInflate.getMin().y, 2, 0.00001));
test(approxEqual(aabbInflate.getMin().z, 5, 0.00001));
test(approxEqual(aabbInflate.getMax().x, 0, 0.00001));
test(approxEqual(aabbInflate.getMax().y, 8, 0.00001));
test(approxEqual(aabbInflate.getMax().z, 35, 0.00001));
test(approxEqual(aabbInflate.getMin().x(), -4, 0.00001));
test(approxEqual(aabbInflate.getMin().y(), 2, 0.00001));
test(approxEqual(aabbInflate.getMin().z(), 5, 0.00001));
test(approxEqual(aabbInflate.getMax().x(), 0, 0.00001));
test(approxEqual(aabbInflate.getMax().y(), 8, 0.00001));
test(approxEqual(aabbInflate.getMax().z(), 35, 0.00001));
// -------- Test getExtent() --------- //
test(approxEqual(mAABB1.getExtent().x, 20));
test(approxEqual(mAABB1.getExtent().y, 20));
test(approxEqual(mAABB1.getExtent().z, 20));
test(approxEqual(m_AABB1.getExtent().x(), 20));
test(approxEqual(m_AABB1.getExtent().y(), 20));
test(approxEqual(m_AABB1.getExtent().z(), 20));
test(approxEqual(mAABB2.getExtent().x, 3));
test(approxEqual(mAABB2.getExtent().y, 16));
test(approxEqual(mAABB2.getExtent().z, 60));
test(approxEqual(m_AABB2.getExtent().x(), 3));
test(approxEqual(m_AABB2.getExtent().y(), 16));
test(approxEqual(m_AABB2.getExtent().z(), 60));
test(approxEqual(mAABB3.getExtent().x, 50));
test(approxEqual(mAABB3.getExtent().y, 50));
test(approxEqual(mAABB3.getExtent().z, 50));
test(approxEqual(m_AABB3.getExtent().x(), 50));
test(approxEqual(m_AABB3.getExtent().y(), 50));
test(approxEqual(m_AABB3.getExtent().z(), 50));
// -------- Test getCenter() -------- //
test(mAABB1.getCenter().x == 0);
test(mAABB1.getCenter().y == 0);
test(mAABB1.getCenter().z == 0);
test(m_AABB1.getCenter().x() == 0);
test(m_AABB1.getCenter().y() == 0);
test(m_AABB1.getCenter().z() == 0);
test(approxEqual(mAABB2.getCenter().x, -3.5));
test(approxEqual(mAABB2.getCenter().y, 12));
test(approxEqual(mAABB2.getCenter().z, 0));
test(approxEqual(m_AABB2.getCenter().x(), -3.5));
test(approxEqual(m_AABB2.getCenter().y(), 12));
test(approxEqual(m_AABB2.getCenter().z(), 0));
// -------- Test setMin(), setMax(), getMin(), getMax() -------- //
AABB aabb5;
aabb5.setMin(Vector3(-12, 34, 6));
aabb5.setMax(Vector3(-3, 56, 20));
aabb5.setMin(vec3(-12, 34, 6));
aabb5.setMax(vec3(-3, 56, 20));
test(aabb5.getMin().x == -12);
test(aabb5.getMin().y == 34);
test(aabb5.getMin().z == 6);
test(aabb5.getMax().x == -3);
test(aabb5.getMax().y == 56);
test(aabb5.getMax().z == 20);
test(aabb5.getMin().x() == -12);
test(aabb5.getMin().y() == 34);
test(aabb5.getMin().z() == 6);
test(aabb5.getMax().x() == -3);
test(aabb5.getMax().y() == 56);
test(aabb5.getMax().z() == 20);
// -------- Test assignment operator -------- //
AABB aabb6;
aabb6 = aabb2;
test(aabb6.getMin().x == -3);
test(aabb6.getMin().y == -5);
test(aabb6.getMin().z == -8);
test(aabb6.getMax().x == 65);
test(aabb6.getMax().y == -1);
test(aabb6.getMax().z == 56);
test(aabb6.getMin().x() == -3);
test(aabb6.getMin().y() == -5);
test(aabb6.getMin().z() == -8);
test(aabb6.getMax().x() == 65);
test(aabb6.getMax().y() == -1);
test(aabb6.getMax().z() == 56);
// -------- Test getVolume() -------- //
test(approxEqual(mAABB1.getVolume(), 8000));
test(approxEqual(mAABB2.getVolume(), 2880));
test(approxEqual(m_AABB1.getVolume(), 8000));
test(approxEqual(m_AABB2.getVolume(), 2880));
}
void testMergeMethods() {
AABB aabb1(Vector3(-45, 7, -2), Vector3(23, 8, 1));
AABB aabb2(Vector3(-15, 6, 23), Vector3(-5, 9, 45));
AABB aabb1(vec3(-45, 7, -2), vec3(23, 8, 1));
AABB aabb2(vec3(-15, 6, 23), vec3(-5, 9, 45));
// -------- Test mergeTwoAABBs() -------- //
AABB aabb3;
aabb3.mergeTwoAABBs(aabb1, mAABB1);
aabb3.mergeTwoAABBs(aabb1, m_AABB1);
test(aabb3.getMin().x == -45);
test(aabb3.getMin().y == -10);
test(aabb3.getMin().z == -10);
test(aabb3.getMax().x == 23);
test(aabb3.getMax().y == 10);
test(aabb3.getMax().z == 10);
test(aabb3.getMin().x() == -45);
test(aabb3.getMin().y() == -10);
test(aabb3.getMin().z() == -10);
test(aabb3.getMax().x() == 23);
test(aabb3.getMax().y() == 10);
test(aabb3.getMax().z() == 10);
AABB aabb4;
aabb4.mergeTwoAABBs(aabb1, aabb2);
test(aabb4.getMin().x == -45);
test(aabb4.getMin().y == 6);
test(aabb4.getMin().z == -2);
test(aabb4.getMax().x == 23);
test(aabb4.getMax().y == 9);
test(aabb4.getMax().z == 45);
test(aabb4.getMin().x() == -45);
test(aabb4.getMin().y() == 6);
test(aabb4.getMin().z() == -2);
test(aabb4.getMax().x() == 23);
test(aabb4.getMax().y() == 9);
test(aabb4.getMax().z() == 45);
// -------- Test mergeWithAABB() -------- //
aabb1.mergeWithAABB(mAABB1);
aabb1.mergeWithAABB(m_AABB1);
test(aabb1.getMin().x == -45);
test(aabb1.getMin().y == -10);
test(aabb1.getMin().z == -10);
test(aabb1.getMax().x == 23);
test(aabb1.getMax().y == 10);
test(aabb1.getMax().z == 10);
test(aabb1.getMin().x() == -45);
test(aabb1.getMin().y() == -10);
test(aabb1.getMin().z() == -10);
test(aabb1.getMax().x() == 23);
test(aabb1.getMax().y() == 10);
test(aabb1.getMax().z() == 10);
aabb2.mergeWithAABB(mAABB1);
aabb2.mergeWithAABB(m_AABB1);
test(aabb2.getMin().x == -15);
test(aabb2.getMin().y == -10);
test(aabb2.getMin().z == -10);
test(aabb2.getMax().x == 10);
test(aabb2.getMax().y == 10);
test(aabb2.getMax().z == 45);
test(aabb2.getMin().x() == -15);
test(aabb2.getMin().y() == -10);
test(aabb2.getMin().z() == -10);
test(aabb2.getMax().x() == 10);
test(aabb2.getMax().y() == 10);
test(aabb2.getMax().z() == 45);
}
void testIntersection() {
// -------- Test contains(AABB) -------- //
test(!mAABB1.contains(mAABB2));
test(mAABB3.contains(mAABB1));
test(!mAABB1.contains(mAABB3));
test(!mAABB1.contains(mAABB4));
test(!mAABB4.contains(mAABB1));
test(!m_AABB1.contains(m_AABB2));
test(m_AABB3.contains(m_AABB1));
test(!m_AABB1.contains(m_AABB3));
test(!m_AABB1.contains(m_AABB4));
test(!m_AABB4.contains(m_AABB1));
// -------- Test contains(Vector3) -------- //
test(mAABB1.contains(Vector3(0, 0, 0)));
test(mAABB1.contains(Vector3(-5, 6, 9)));
test(mAABB1.contains(Vector3(-9, -4, -9)));
test(mAABB1.contains(Vector3(9, 4, 7)));
test(!mAABB1.contains(Vector3(-11, -4, -9)));
test(!mAABB1.contains(Vector3(1, 12, -9)));
test(!mAABB1.contains(Vector3(1, 8, -13)));
test(!mAABB1.contains(Vector3(-14, 82, -13)));
// -------- Test contains(vec3) -------- //
test(m_AABB1.contains(vec3(0, 0, 0)));
test(m_AABB1.contains(vec3(-5, 6, 9)));
test(m_AABB1.contains(vec3(-9, -4, -9)));
test(m_AABB1.contains(vec3(9, 4, 7)));
test(!m_AABB1.contains(vec3(-11, -4, -9)));
test(!m_AABB1.contains(vec3(1, 12, -9)));
test(!m_AABB1.contains(vec3(1, 8, -13)));
test(!m_AABB1.contains(vec3(-14, 82, -13)));
// -------- Test testCollision() -------- //
test(mAABB1.testCollision(mAABB2));
test(mAABB2.testCollision(mAABB1));
test(mAABB1.testCollision(mAABB3));
test(mAABB3.testCollision(mAABB1));
test(!mAABB1.testCollision(mAABB4));
test(!mAABB4.testCollision(mAABB1));
test(m_AABB1.testCollision(m_AABB2));
test(m_AABB2.testCollision(m_AABB1));
test(m_AABB1.testCollision(m_AABB3));
test(m_AABB3.testCollision(m_AABB1));
test(!m_AABB1.testCollision(m_AABB4));
test(!m_AABB4.testCollision(m_AABB1));
// -------- Test testCollisionTriangleAABB() -------- //
AABB aabb(Vector3(100, 100, 100), Vector3(200, 200, 200));
Vector3 trianglePoints[] = {
Vector3(-2, 4, 6), Vector3(20, -34, -73), Vector3(-12, 98, 76)
AABB aabb(vec3(100, 100, 100), vec3(200, 200, 200));
vec3 trianglePoints[] = {
vec3(-2, 4, 6), vec3(20, -34, -73), vec3(-12, 98, 76)
};
test(mAABB1.testCollisionTriangleAABB(trianglePoints));
test(m_AABB1.testCollisionTriangleAABB(trianglePoints));
test(!aabb.testCollisionTriangleAABB(trianglePoints));
// -------- Test testRayIntersect() -------- //
Ray ray1(Vector3(-20, 4, -7), Vector3(20, 4, -7));
Ray ray2(Vector3(-20, 11, -7), Vector3(20, 11, -7));
Ray ray3(Vector3(0, 15, 0), Vector3(0, -15, 0));
Ray ray4(Vector3(0, -15, 0), Vector3(0, 15, 0));
Ray ray5(Vector3(-3, 4, 8), Vector3(-7, 9, 4));
Ray ray6(Vector3(-4, 6, -100), Vector3(-4, 6, -9));
Ray ray7(Vector3(-4, 6, -100), Vector3(-4, 6, -11), 0.6);
Ray ray8(Vector3(-403, -432, -100), Vector3(134, 643, 23));
Ray ray1(vec3(-20, 4, -7), vec3(20, 4, -7));
Ray ray2(vec3(-20, 11, -7), vec3(20, 11, -7));
Ray ray3(vec3(0, 15, 0), vec3(0, -15, 0));
Ray ray4(vec3(0, -15, 0), vec3(0, 15, 0));
Ray ray5(vec3(-3, 4, 8), vec3(-7, 9, 4));
Ray ray6(vec3(-4, 6, -100), vec3(-4, 6, -9));
Ray ray7(vec3(-4, 6, -100), vec3(-4, 6, -11), 0.6);
Ray ray8(vec3(-403, -432, -100), vec3(134, 643, 23));
test(mAABB1.testRayIntersect(ray1));
test(!mAABB1.testRayIntersect(ray2));
test(mAABB1.testRayIntersect(ray3));
test(mAABB1.testRayIntersect(ray4));
test(mAABB1.testRayIntersect(ray5));
test(mAABB1.testRayIntersect(ray6));
test(!mAABB1.testRayIntersect(ray7));
test(!mAABB1.testRayIntersect(ray8));
test(m_AABB1.testRayIntersect(ray1));
test(!m_AABB1.testRayIntersect(ray2));
test(m_AABB1.testRayIntersect(ray3));
test(m_AABB1.testRayIntersect(ray4));
test(m_AABB1.testRayIntersect(ray5));
test(m_AABB1.testRayIntersect(ray6));
test(!m_AABB1.testRayIntersect(ray7));
test(!m_AABB1.testRayIntersect(ray8));
}
};

34
test/tests/collision/TestCollisionWorld.h
View File

@ -139,24 +139,24 @@ class TestCollisionWorld : public Test {
m_world = new CollisionWorld();
// Create the bodies
Transform boxTransform(Vector3(10, 0, 0), Quaternion::identity());
mBoxBody = m_world->createCollisionBody(boxTransform);
mBoxShape = new BoxShape(Vector3(3, 3, 3));
mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, Transform::identity());
etk::Transform3D boxTransform(vec3(10, 0, 0), etk::Quaternion::identity());
mBoxBody = m_world->createCollisionBody(boxetk::Transform3D);
mBoxShape = new BoxShape(vec3(3, 3, 3));
mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, etk::Transform3D::identity());
mSphereShape = new SphereShape(3.0);
Transform sphere1Transform(Vector3(10,5, 0), Quaternion::identity());
mSphere1Body = m_world->createCollisionBody(sphere1Transform);
mSphere1ProxyShape = mSphere1Body->addCollisionShape(mSphereShape, Transform::identity());
etk::Transform3D sphere1Transform(vec3(10,5, 0), etk::Quaternion::identity());
mSphere1Body = m_world->createCollisionBody(sphere1etk::Transform3D);
mSphere1ProxyShape = mSphere1Body->addCollisionShape(mSphereShape, etk::Transform3D::identity());
Transform sphere2Transform(Vector3(30, 10, 10), Quaternion::identity());
mSphere2Body = m_world->createCollisionBody(sphere2Transform);
mSphere2ProxyShape = mSphere2Body->addCollisionShape(mSphereShape, Transform::identity());
etk::Transform3D sphere2Transform(vec3(30, 10, 10), etk::Quaternion::identity());
mSphere2Body = m_world->createCollisionBody(sphere2etk::Transform3D);
mSphere2ProxyShape = mSphere2Body->addCollisionShape(mSphereShape, etk::Transform3D::identity());
Transform cylinderTransform(Vector3(10, -5, 0), Quaternion::identity());
mCylinderBody = m_world->createCollisionBody(cylinderTransform);
etk::Transform3D cylinderTransform(vec3(10, -5, 0), etk::Quaternion::identity());
mCylinderBody = m_world->createCollisionBody(cylinderetk::Transform3D);
mCylinderShape = new CylinderShape(2, 5);
mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, Transform::identity());
mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, etk::Transform3D::identity());
// Assign collision categories to proxy shapes
mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1);
@ -238,7 +238,7 @@ class TestCollisionWorld : public Test {
test(!m_collisionCallback.sphere1CollideWithSphere2);
// Move sphere 1 to collide with sphere 2
mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity()));
mSphere1Body->setTransform(Transform(vec3(30, 15, 10), etk::Quaternion::identity()));
m_collisionCallback.reset();
m_world->testCollision(&m_collisionCallback);
@ -262,7 +262,7 @@ class TestCollisionWorld : public Test {
test(!m_collisionCallback.sphere1CollideWithSphere2);
// Move sphere 1 to collide with box
mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity()));
mSphere1Body->setTransform(Transform(vec3(10, 5, 0), etk::Quaternion::identity()));
// --------- Test collision with inactive bodies --------- //
@ -307,7 +307,7 @@ class TestCollisionWorld : public Test {
test(!m_collisionCallback.sphere1CollideWithSphere2);
// Move sphere 1 to collide with sphere 2
mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity()));
mSphere1Body->setTransform(Transform(vec3(30, 15, 10), etk::Quaternion::identity()));
m_collisionCallback.reset();
m_world->testCollision(&m_collisionCallback);
@ -329,7 +329,7 @@ class TestCollisionWorld : public Test {
test(!m_collisionCallback.sphere1CollideWithSphere2);
// Move sphere 1 to collide with box
mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity()));
mSphere1Body->setTransform(Transform(vec3(10, 5, 0), etk::Quaternion::identity()));
mBoxProxyShape->setCollideWithMaskBits(0xFFFF);
mSphere1ProxyShape->setCollideWithMaskBits(0xFFFF);

130
test/tests/collision/TestDynamicAABBTree.h
View File

@ -123,31 +123,31 @@ class TestDynamicAABBTree : public Test {
int32_t object4Data = 7;
// First object
AABB aabb1 = AABB(Vector3(-6, 4, -3), Vector3(4, 8, 3));
AABB aabb1 = AABB(vec3(-6, 4, -3), vec3(4, 8, 3));
int32_t object1Id = tree.addObject(aabb1, &object1Data);
// Second object
AABB aabb2 = AABB(Vector3(5, 2, -3), Vector3(10, 7, 3));
AABB aabb2 = AABB(vec3(5, 2, -3), vec3(10, 7, 3));
int32_t object2Id = tree.addObject(aabb2, &object2Data);
// Third object
AABB aabb3 = AABB(Vector3(-5, 1, -3), Vector3(-2, 3, 3));
AABB aabb3 = AABB(vec3(-5, 1, -3), vec3(-2, 3, 3));
int32_t object3Id = tree.addObject(aabb3, &object3Data);
// Fourth object
AABB aabb4 = AABB(Vector3(0, -4, -3), Vector3(3, -2, 3));
AABB aabb4 = AABB(vec3(0, -4, -3), vec3(3, -2, 3));
int32_t object4Id = tree.addObject(aabb4, &object4Data);
// ----------- Tests ----------- //
// Test root AABB
AABB rootAABB = tree.getRootAABB();
test(rootAABB.getMin().x == -6);
test(rootAABB.getMin().y == -4);
test(rootAABB.getMin().z == -3);
test(rootAABB.getMax().x == 10);
test(rootAABB.getMax().y == 8);
test(rootAABB.getMax().z == 3);
test(rootAABB.getMin().x() == -6);
test(rootAABB.getMin().y() == -4);
test(rootAABB.getMin().z() == -3);
test(rootAABB.getMax().x() == 10);
test(rootAABB.getMax().y() == 8);
test(rootAABB.getMax().z() == 3);
// Test data stored at the nodes of the tree
test(*(int32_t*)(tree.getNodeDataPointer(object1Id)) == object1Data);
@ -169,26 +169,26 @@ class TestDynamicAABBTree : public Test {
int32_t object4Data = 7;
// First object
AABB aabb1 = AABB(Vector3(-6, 4, -3), Vector3(4, 8, 3));
AABB aabb1 = AABB(vec3(-6, 4, -3), vec3(4, 8, 3));
int32_t object1Id = tree.addObject(aabb1, &object1Data);
// Second object
AABB aabb2 = AABB(Vector3(5, 2, -3), Vector3(10, 7, 3));
AABB aabb2 = AABB(vec3(5, 2, -3), vec3(10, 7, 3));
int32_t object2Id = tree.addObject(aabb2, &object2Data);
// Third object
AABB aabb3 = AABB(Vector3(-5, 1, -3), Vector3(-2, 3, 3));
AABB aabb3 = AABB(vec3(-5, 1, -3), vec3(-2, 3, 3));
int32_t object3Id = tree.addObject(aabb3, &object3Data);
// Fourth object
AABB aabb4 = AABB(Vector3(0, -4, -3), Vector3(3, -2, 3));
AABB aabb4 = AABB(vec3(0, -4, -3), vec3(3, -2, 3));
int32_t object4Id = tree.addObject(aabb4, &object4Data);
// ---------- Tests ---------- //
// AABB overlapping nothing
mOverlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-10, 12, -4), Vector3(10, 50, 4)), mOverlapCallback);
tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-10, 12, -4), vec3(10, 50, 4)), mOverlapCallback);
test(!mOverlapCallback.isOverlapping(object1Id));
test(!mOverlapCallback.isOverlapping(object2Id));
test(!mOverlapCallback.isOverlapping(object3Id));
@ -196,7 +196,7 @@ class TestDynamicAABBTree : public Test {
// AABB overlapping everything
mOverlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-15, -15, -4), Vector3(15, 15, 4)), mOverlapCallback);
tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-15, -15, -4), vec3(15, 15, 4)), mOverlapCallback);
test(mOverlapCallback.isOverlapping(object1Id));
test(mOverlapCallback.isOverlapping(object2Id));
test(mOverlapCallback.isOverlapping(object3Id));
@ -204,7 +204,7 @@ class TestDynamicAABBTree : public Test {
// AABB overlapping object 1 and 3
mOverlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-4, 2, -4), Vector3(-1, 7, 4)), mOverlapCallback);
tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-4, 2, -4), vec3(-1, 7, 4)), mOverlapCallback);
test(mOverlapCallback.isOverlapping(object1Id));
test(!mOverlapCallback.isOverlapping(object2Id));
test(mOverlapCallback.isOverlapping(object3Id));
@ -212,7 +212,7 @@ class TestDynamicAABBTree : public Test {
// AABB overlapping object 3 and 4
mOverlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-6, -5, -2), Vector3(2, 2, 0)), mOverlapCallback);
tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-6, -5, -2), vec3(2, 2, 0)), mOverlapCallback);
test(!mOverlapCallback.isOverlapping(object1Id));
test(!mOverlapCallback.isOverlapping(object2Id));
test(mOverlapCallback.isOverlapping(object3Id));
@ -220,7 +220,7 @@ class TestDynamicAABBTree : public Test {
// AABB overlapping object 2
mOverlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(5, -10, -2), Vector3(7, 10, 9)), mOverlapCallback);
tree.reportAllShapesOverlappingWithAABB(AABB(vec3(5, -10, -2), vec3(7, 10, 9)), mOverlapCallback);
test(!mOverlapCallback.isOverlapping(object1Id));
test(mOverlapCallback.isOverlapping(object2Id));
test(!mOverlapCallback.isOverlapping(object3Id));
@ -228,14 +228,14 @@ class TestDynamicAABBTree : public Test {
// ---- Update the object AABBs with the initial AABBs (no reinsertion) ----- //
tree.updateObject(object1Id, aabb1, Vector3::zero(), false);
tree.updateObject(object2Id, aabb2, Vector3::zero(), false);
tree.updateObject(object3Id, aabb3, Vector3::zero(), false);
tree.updateObject(object4Id, aabb4, Vector3::zero(), false);
tree.updateObject(object1Id, aabb1, vec3::zero(), false);
tree.updateObject(object2Id, aabb2, vec3::zero(), false);
tree.updateObject(object3Id, aabb3, vec3::zero(), false);
tree.updateObject(object4Id, aabb4, vec3::zero(), false);
// AABB overlapping nothing
mOverlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-10, 12, -4), Vector3(10, 50, 4)), mOverlapCallback);
tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-10, 12, -4), vec3(10, 50, 4)), mOverlapCallback);
test(!mOverlapCallback.isOverlapping(object1Id));
test(!mOverlapCallback.isOverlapping(object2Id));
test(!mOverlapCallback.isOverlapping(object3Id));
@ -243,7 +243,7 @@ class TestDynamicAABBTree : public Test {
// AABB overlapping everything
mOverlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-15, -15, -4), Vector3(15, 15, 4)), mOverlapCallback);
tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-15, -15, -4), vec3(15, 15, 4)), mOverlapCallback);
test(mOverlapCallback.isOverlapping(object1Id));
test(mOverlapCallback.isOverlapping(object2Id));
test(mOverlapCallback.isOverlapping(object3Id));
@ -251,7 +251,7 @@ class TestDynamicAABBTree : public Test {
// AABB overlapping object 1 and 3
mOverlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-4, 2, -4), Vector3(-1, 7, 4)), mOverlapCallback);
tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-4, 2, -4), vec3(-1, 7, 4)), mOverlapCallback);
test(mOverlapCallback.isOverlapping(object1Id));
test(!mOverlapCallback.isOverlapping(object2Id));
test(mOverlapCallback.isOverlapping(object3Id));
@ -259,7 +259,7 @@ class TestDynamicAABBTree : public Test {
// AABB overlapping object 3 and 4
mOverlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-6, -5, -2), Vector3(2, 2, 0)), mOverlapCallback);
tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-6, -5, -2), vec3(2, 2, 0)), mOverlapCallback);
test(!mOverlapCallback.isOverlapping(object1Id));
test(!mOverlapCallback.isOverlapping(object2Id));
test(mOverlapCallback.isOverlapping(object3Id));
@ -267,7 +267,7 @@ class TestDynamicAABBTree : public Test {
// AABB overlapping object 2
mOverlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(5, -10, -2), Vector3(7, 10, 9)), mOverlapCallback);
tree.reportAllShapesOverlappingWithAABB(AABB(vec3(5, -10, -2), vec3(7, 10, 9)), mOverlapCallback);
test(!mOverlapCallback.isOverlapping(object1Id));
test(mOverlapCallback.isOverlapping(object2Id));
test(!mOverlapCallback.isOverlapping(object3Id));
@ -275,14 +275,14 @@ class TestDynamicAABBTree : public Test {
// ---- Update the object AABBs with the initial AABBs (with reinsertion) ----- //
tree.updateObject(object1Id, aabb1, Vector3::zero(), true);
tree.updateObject(object2Id, aabb2, Vector3::zero(), true);
tree.updateObject(object3Id, aabb3, Vector3::zero(), true);
tree.updateObject(object4Id, aabb4, Vector3::zero(), true);
tree.updateObject(object1Id, aabb1, vec3::zero(), true);
tree.updateObject(object2Id, aabb2, vec3::zero(), true);
tree.updateObject(object3Id, aabb3, vec3::zero(), true);
tree.updateObject(object4Id, aabb4, vec3::zero(), true);
// AABB overlapping nothing
mOverlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-10, 12, -4), Vector3(10, 50, 4)), mOverlapCallback);
tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-10, 12, -4), vec3(10, 50, 4)), mOverlapCallback);
test(!mOverlapCallback.isOverlapping(object1Id));
test(!mOverlapCallback.isOverlapping(object2Id));
test(!mOverlapCallback.isOverlapping(object3Id));
@ -290,7 +290,7 @@ class TestDynamicAABBTree : public Test {
// AABB overlapping everything
mOverlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-15, -15, -4), Vector3(15, 15, 4)), mOverlapCallback);
tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-15, -15, -4), vec3(15, 15, 4)), mOverlapCallback);
test(mOverlapCallback.isOverlapping(object1Id));
test(mOverlapCallback.isOverlapping(object2Id));
test(mOverlapCallback.isOverlapping(object3Id));
@ -298,7 +298,7 @@ class TestDynamicAABBTree : public Test {
// AABB overlapping object 1 and 3
mOverlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-4, 2, -4), Vector3(-1, 7, 4)), mOverlapCallback);
tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-4, 2, -4), vec3(-1, 7, 4)), mOverlapCallback);
test(mOverlapCallback.isOverlapping(object1Id));
test(!mOverlapCallback.isOverlapping(object2Id));
test(mOverlapCallback.isOverlapping(object3Id));
@ -306,7 +306,7 @@ class TestDynamicAABBTree : public Test {
// AABB overlapping object 3 and 4
mOverlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-6, -5, -2), Vector3(2, 2, 0)), mOverlapCallback);
tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-6, -5, -2), vec3(2, 2, 0)), mOverlapCallback);
test(!mOverlapCallback.isOverlapping(object1Id));
test(!mOverlapCallback.isOverlapping(object2Id));
test(mOverlapCallback.isOverlapping(object3Id));
@ -314,7 +314,7 @@ class TestDynamicAABBTree : public Test {
// AABB overlapping object 2
mOverlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(5, -10, -2), Vector3(7, 10, 9)), mOverlapCallback);
tree.reportAllShapesOverlappingWithAABB(AABB(vec3(5, -10, -2), vec3(7, 10, 9)), mOverlapCallback);
test(!mOverlapCallback.isOverlapping(object1Id));
test(mOverlapCallback.isOverlapping(object2Id));
test(!mOverlapCallback.isOverlapping(object3Id));
@ -322,15 +322,15 @@ class TestDynamicAABBTree : public Test {
// ---- Move objects 2 and 3 ----- //
AABB newAABB2(Vector3(-7, 10, -3), Vector3(1, 13, 3));
tree.updateObject(object2Id, newAABB2, Vector3::zero());
AABB newAABB2(vec3(-7, 10, -3), vec3(1, 13, 3));
tree.updateObject(object2Id, newAABB2, vec3::zero());
AABB newAABB3(Vector3(7, -6, -3), Vector3(9, 1, 3));
tree.updateObject(object3Id, newAABB3, Vector3::zero());
AABB newAABB3(vec3(7, -6, -3), vec3(9, 1, 3));
tree.updateObject(object3Id, newAABB3, vec3::zero());
// AABB overlapping object 3
mOverlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(6, -10, -2), Vector3(8, 5, 3)), mOverlapCallback);
tree.reportAllShapesOverlappingWithAABB(AABB(vec3(6, -10, -2), vec3(8, 5, 3)), mOverlapCallback);
test(!mOverlapCallback.isOverlapping(object1Id));
test(!mOverlapCallback.isOverlapping(object2Id));
test(mOverlapCallback.isOverlapping(object3Id));
@ -338,7 +338,7 @@ class TestDynamicAABBTree : public Test {
// AABB overlapping objects 1, 2
mOverlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-8, 5, -3), Vector3(-2, 11, 3)), mOverlapCallback);
tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-8, 5, -3), vec3(-2, 11, 3)), mOverlapCallback);
test(mOverlapCallback.isOverlapping(object1Id));
test(mOverlapCallback.isOverlapping(object2Id));
test(!mOverlapCallback.isOverlapping(object3Id));
@ -359,26 +359,26 @@ class TestDynamicAABBTree : public Test {
int32_t object4Data = 7;
// First object
AABB aabb1 = AABB(Vector3(-6, 4, -3), Vector3(4, 8, 3));
AABB aabb1 = AABB(vec3(-6, 4, -3), vec3(4, 8, 3));
int32_t object1Id = tree.addObject(aabb1, &object1Data);
// Second object
AABB aabb2 = AABB(Vector3(5, 2, -3), Vector3(10, 7, 3));
AABB aabb2 = AABB(vec3(5, 2, -3), vec3(10, 7, 3));
int32_t object2Id = tree.addObject(aabb2, &object2Data);
// Third object
AABB aabb3 = AABB(Vector3(-5, 1, -3), Vector3(-2, 3, 3));
AABB aabb3 = AABB(vec3(-5, 1, -3), vec3(-2, 3, 3));
int32_t object3Id = tree.addObject(aabb3, &object3Data);
// Fourth object
AABB aabb4 = AABB(Vector3(0, -4, -3), Vector3(3, -2, 3));
AABB aabb4 = AABB(vec3(0, -4, -3), vec3(3, -2, 3));
int32_t object4Id = tree.addObject(aabb4, &object4Data);
// ---------- Tests ---------- //
// Ray with no hits
m_raycastCallback.reset();
Ray ray1(Vector3(4.5, -10, -5), Vector3(4.5, 10, -5));
Ray ray1(vec3(4.5, -10, -5), vec3(4.5, 10, -5));
tree.raycast(ray1, m_raycastCallback);
test(!m_raycastCallback.isHit(object1Id));
test(!m_raycastCallback.isHit(object2Id));
@ -387,7 +387,7 @@ class TestDynamicAABBTree : public Test {
// Ray that hits object 1
m_raycastCallback.reset();
Ray ray2(Vector3(-1, -20, -2), Vector3(-1, 20, -2));
Ray ray2(vec3(-1, -20, -2), vec3(-1, 20, -2));
tree.raycast(ray2, m_raycastCallback);
test(m_raycastCallback.isHit(object1Id));
test(!m_raycastCallback.isHit(object2Id));
@ -396,7 +396,7 @@ class TestDynamicAABBTree : public Test {
// Ray that hits object 1 and 2
m_raycastCallback.reset();
Ray ray3(Vector3(-7, 6, -2), Vector3(8, 6, -2));
Ray ray3(vec3(-7, 6, -2), vec3(8, 6, -2));
tree.raycast(ray3, m_raycastCallback);
test(m_raycastCallback.isHit(object1Id));
test(m_raycastCallback.isHit(object2Id));
@ -405,7 +405,7 @@ class TestDynamicAABBTree : public Test {
// Ray that hits object 3
m_raycastCallback.reset();
Ray ray4(Vector3(-7, 2, 0), Vector3(-1, 2, 0));
Ray ray4(vec3(-7, 2, 0), vec3(-1, 2, 0));
tree.raycast(ray4, m_raycastCallback);
test(!m_raycastCallback.isHit(object1Id));
test(!m_raycastCallback.isHit(object2Id));
@ -414,10 +414,10 @@ class TestDynamicAABBTree : public Test {
// ---- Update the object AABBs with the initial AABBs (no reinsertion) ----- //
tree.updateObject(object1Id, aabb1, Vector3::zero(), false);
tree.updateObject(object2Id, aabb2, Vector3::zero(), false);
tree.updateObject(object3Id, aabb3, Vector3::zero(), false);
tree.updateObject(object4Id, aabb4, Vector3::zero(), false);
tree.updateObject(object1Id, aabb1, vec3::zero(), false);
tree.updateObject(object2Id, aabb2, vec3::zero(), false);
tree.updateObject(object3Id, aabb3, vec3::zero(), false);
tree.updateObject(object4Id, aabb4, vec3::zero(), false);
// Ray with no hits
m_raycastCallback.reset();
@ -453,10 +453,10 @@ class TestDynamicAABBTree : public Test {
// ---- Update the object AABBs with the initial AABBs (with reinsertion) ----- //
tree.updateObject(object1Id, aabb1, Vector3::zero(), true);
tree.updateObject(object2Id, aabb2, Vector3::zero(), true);
tree.updateObject(object3Id, aabb3, Vector3::zero(), true);
tree.updateObject(object4Id, aabb4, Vector3::zero(), true);
tree.updateObject(object1Id, aabb1, vec3::zero(), true);
tree.updateObject(object2Id, aabb2, vec3::zero(), true);
tree.updateObject(object3Id, aabb3, vec3::zero(), true);
tree.updateObject(object4Id, aabb4, vec3::zero(), true);
// Ray with no hits
m_raycastCallback.reset();
@ -492,14 +492,14 @@ class TestDynamicAABBTree : public Test {
// ---- Move objects 2 and 3 ----- //
AABB newAABB2(Vector3(-7, 10, -3), Vector3(1, 13, 3));
tree.updateObject(object2Id, newAABB2, Vector3::zero());
AABB newAABB2(vec3(-7, 10, -3), vec3(1, 13, 3));
tree.updateObject(object2Id, newAABB2, vec3::zero());
AABB newAABB3(Vector3(7, -6, -3), Vector3(9, 1, 3));
tree.updateObject(object3Id, newAABB3, Vector3::zero());
AABB newAABB3(vec3(7, -6, -3), vec3(9, 1, 3));
tree.updateObject(object3Id, newAABB3, vec3::zero());
// Ray that hits object 1, 2
Ray ray5(Vector3(-4, -5, 0), Vector3(-4, 12, 0));
Ray ray5(vec3(-4, -5, 0), vec3(-4, 12, 0));
m_raycastCallback.reset();
tree.raycast(ray5, m_raycastCallback);
test(m_raycastCallback.isHit(object1Id));
@ -508,7 +508,7 @@ class TestDynamicAABBTree : public Test {
test(!m_raycastCallback.isHit(object4Id));
// Ray that hits object 3 and 4
Ray ray6(Vector3(11, -3, 1), Vector3(-2, -3, 1));
Ray ray6(vec3(11, -3, 1), vec3(-2, -3, 1));
m_raycastCallback.reset();
tree.raycast(ray6, m_raycastCallback);
test(!m_raycastCallback.isHit(object1Id));

1026
test/tests/collision/TestPointInside.h
View File

File diff suppressed because it is too large Load Diff

Some files were not shown because too many files have changed in this diff Show More