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

[DEV] continue rework

Browse Source
This commit is contained in:
Edouard DUPIN 2017-06-08 22:35:22 +02:00
parent 562af6c0ae
commit 0f6adcf289
224 changed files with 28906 additions and 31578 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
ephysics/body/Body.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/body/Body.h> #include <ephysics/body/Body.h>

41
ephysics/body/Body.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_BODY_H
#define REACTPHYSICS3D_BODY_H
// Libraries // Libraries
#include <stdexcept> #include <stdexcept>
@ -70,7 +49,7 @@ class Body {
bool mIsSleeping; bool mIsSleeping;
/// Elapsed time since the body velocity was bellow the sleep velocity /// Elapsed time since the body velocity was bellow the sleep velocity
decimal mSleepTime; float mSleepTime;
/// Pointer that can be used to attach user data to the body /// Pointer that can be used to attach user data to the body
void* mUserData; void* mUserData;
@ -191,11 +170,11 @@ inline void Body::setIsActive(bool isActive) {
inline void Body::setIsSleeping(bool isSleeping) { inline void Body::setIsSleeping(bool isSleeping) {
if (isSleeping) { if (isSleeping) {
mSleepTime = decimal(0.0); mSleepTime = float(0.0);
} }
else { else {
if (mIsSleeping) { if (mIsSleeping) {
mSleepTime = decimal(0.0); mSleepTime = float(0.0);
} }
} }
@ -239,5 +218,3 @@ inline bool Body::operator!=(const Body& body2) const {
} }
} }
#endif

37
ephysics/body/CollisionBody.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/body/CollisionBody.h> #include <ephysics/body/CollisionBody.h>
@ -62,7 +43,7 @@ CollisionBody::~CollisionBody() {
/** /**
* @param collisionShape A pointer to the collision shape you want to add to the body * @param collisionShape A pointer to the collision shape you want to add to the body
* @param transform The transformation of the collision shape that transforms the * @param transform The transformation of the collision shape that transforms the
* local-space of the collision shape into the local-space of the body * local-space of the collision shape int32_to the local-space of the body
* @return A pointer to the proxy shape that has been created to link the body to * @return A pointer to the proxy shape that has been created to link the body to
* the new collision shape you have added. * the new collision shape you have added.
*/ */
@ -72,7 +53,7 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape,
// Create a new proxy collision shape to attach the collision shape to the body // Create a new proxy collision shape to attach the collision shape to the body
ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate( ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate(
sizeof(ProxyShape))) ProxyShape(this, collisionShape, sizeof(ProxyShape))) ProxyShape(this, collisionShape,
transform, decimal(1)); transform, float(1));
// Add it to the list of proxy collision shapes of the body // Add it to the list of proxy collision shapes of the body
if (mProxyCollisionShapes == NULL) { if (mProxyCollisionShapes == NULL) {
@ -262,11 +243,11 @@ void CollisionBody::askForBroadPhaseCollisionCheck() const {
// Reset the mIsAlreadyInIsland variable of the body and contact manifolds. // Reset the mIsAlreadyInIsland variable of the body and contact manifolds.
/// This method also returns the number of contact manifolds of the body. /// This method also returns the number of contact manifolds of the body.
int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() { int32_t CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
mIsAlreadyInIsland = false; mIsAlreadyInIsland = false;
int nbManifolds = 0; int32_t nbManifolds = 0;
// Reset the mIsAlreadyInIsland variable of the contact manifolds for // Reset the mIsAlreadyInIsland variable of the contact manifolds for
// this body // this body

43
ephysics/body/CollisionBody.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_COLLISION_BODY_H
#define REACTPHYSICS3D_COLLISION_BODY_H
// Libraries // Libraries
#include <stdexcept> #include <stdexcept>
@ -77,7 +56,7 @@ class CollisionBody : public Body {
ProxyShape* mProxyCollisionShapes; ProxyShape* mProxyCollisionShapes;
/// Number of collision shapes /// Number of collision shapes
uint mNbCollisionShapes; uint32_t mNbCollisionShapes;
/// First element of the linked list of contact manifolds involving this body /// First element of the linked list of contact manifolds involving this body
ContactManifoldListElement* mContactManifoldsList; ContactManifoldListElement* mContactManifoldsList;
@ -110,7 +89,7 @@ class CollisionBody : public Body {
void askForBroadPhaseCollisionCheck() const; void askForBroadPhaseCollisionCheck() const;
/// Reset the mIsAlreadyInIsland variable of the body and contact manifolds /// Reset the mIsAlreadyInIsland variable of the body and contact manifolds
int resetIsAlreadyInIslandAndCountManifolds(); int32_t resetIsAlreadyInIslandAndCountManifolds();
public : public :
@ -218,7 +197,7 @@ inline void CollisionBody::setType(BodyType type) {
// Return the current position and orientation // Return the current position and orientation
/** /**
* @return The current transformation of the body that transforms the local-space * @return The current transformation of the body that transforms the local-space
* of the body into world-space * of the body int32_to world-space
*/ */
inline const Transform& CollisionBody::getTransform() const { inline const Transform& CollisionBody::getTransform() const {
return mTransform; return mTransform;
@ -227,7 +206,7 @@ inline const Transform& CollisionBody::getTransform() const {
// Set the current position and orientation // Set the current position and orientation
/** /**
* @param transform The transformation of the body that transforms the local-space * @param transform The transformation of the body that transforms the local-space
* of the body into world-space * of the body int32_to world-space
*/ */
inline void CollisionBody::setTransform(const Transform& transform) { inline void CollisionBody::setTransform(const Transform& transform) {
@ -302,5 +281,3 @@ inline Vector3 CollisionBody::getLocalVector(const Vector3& worldVector) const {
} }
} }
#endif

89
ephysics/body/RigidBody.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/body/RigidBody.h> #include <ephysics/body/RigidBody.h>
@ -39,13 +20,13 @@ using namespace reactphysics3d;
* @param id The ID of the body * @param id The ID of the body
*/ */
RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id) RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id)
: CollisionBody(transform, world, id), mInitMass(decimal(1.0)), : CollisionBody(transform, world, id), mInitMass(float(1.0)),
mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()), mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()),
mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), mIsGravityEnabled(true), mLinearDamping(float(0.0)), mAngularDamping(float(0.0)),
mJointsList(NULL) { mJointsList(NULL) {
// Compute the inverse mass // Compute the inverse mass
mMassInverse = decimal(1.0) / mInitMass; mMassInverse = float(1.0) / mInitMass;
} }
// Destructor // Destructor
@ -87,13 +68,13 @@ void RigidBody::setType(BodyType type) {
if (mType == STATIC || mType == KINEMATIC) { if (mType == STATIC || mType == KINEMATIC) {
// Reset the inverse mass and inverse inertia tensor to zero // Reset the inverse mass and inverse inertia tensor to zero
mMassInverse = decimal(0.0); mMassInverse = float(0.0);
mInertiaTensorLocal.setToZero(); mInertiaTensorLocal.setToZero();
mInertiaTensorLocalInverse.setToZero(); mInertiaTensorLocalInverse.setToZero();
} }
else { // If it is a dynamic body else { // If it is a dynamic body
mMassInverse = decimal(1.0) / mInitMass; mMassInverse = float(1.0) / mInitMass;
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
} }
@ -150,18 +131,18 @@ void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) {
/** /**
* @param mass The mass (in kilograms) of the body * @param mass The mass (in kilograms) of the body
*/ */
void RigidBody::setMass(decimal mass) { void RigidBody::setMass(float mass) {
if (mType != DYNAMIC) return; if (mType != DYNAMIC) return;
mInitMass = mass; mInitMass = mass;
if (mInitMass > decimal(0.0)) { if (mInitMass > float(0.0)) {
mMassInverse = decimal(1.0) / mInitMass; mMassInverse = float(1.0) / mInitMass;
} }
else { else {
mInitMass = decimal(1.0); mInitMass = float(1.0);
mMassInverse = decimal(1.0); mMassInverse = float(1.0);
} }
} }
@ -194,8 +175,8 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons
} }
// Add a collision shape to the body. // Add a collision shape to the body.
/// When you add a collision shape to the body, an internal copy of this /// When you add a collision shape to the body, an int32_ternal copy of this
/// collision shape will be created internally. Therefore, you can delete it /// collision shape will be created int32_ternally. Therefore, you can delete it
/// right after calling this method or use it later to add it to another body. /// right after calling this method or use it later to add it to another body.
/// This method will return a pointer to a new proxy shape. A proxy shape is /// This method will return a pointer to a new proxy shape. A proxy shape is
/// an object that links a collision shape and a given body. You can use the /// an object that links a collision shape and a given body. You can use the
@ -204,16 +185,16 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons
/** /**
* @param collisionShape The collision shape you want to add to the body * @param collisionShape The collision shape you want to add to the body
* @param transform The transformation of the collision shape that transforms the * @param transform The transformation of the collision shape that transforms the
* local-space of the collision shape into the local-space of the body * local-space of the collision shape int32_to the local-space of the body
* @param mass Mass (in kilograms) of the collision shape you want to add * @param mass Mass (in kilograms) of the collision shape you want to add
* @return A pointer to the proxy shape that has been created to link the body to * @return A pointer to the proxy shape that has been created to link the body to
* the new collision shape you have added. * the new collision shape you have added.
*/ */
ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape, ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
const Transform& transform, const Transform& transform,
decimal mass) { float mass) {
assert(mass > decimal(0.0)); assert(mass > float(0.0));
// Create a new proxy collision shape to attach the collision shape to the body // Create a new proxy collision shape to attach the collision shape to the body
ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate( ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate(
@ -275,7 +256,7 @@ void RigidBody::setLinearVelocity(const Vector3& linearVelocity) {
mLinearVelocity = linearVelocity; mLinearVelocity = linearVelocity;
// If the linear velocity is not zero, awake the body // If the linear velocity is not zero, awake the body
if (mLinearVelocity.lengthSquare() > decimal(0.0)) { if (mLinearVelocity.lengthSquare() > float(0.0)) {
setIsSleeping(false); setIsSleeping(false);
} }
} }
@ -293,7 +274,7 @@ void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
mAngularVelocity = angularVelocity; mAngularVelocity = angularVelocity;
// If the velocity is not zero, awake the body // If the velocity is not zero, awake the body
if (mAngularVelocity.lengthSquare() > decimal(0.0)) { if (mAngularVelocity.lengthSquare() > float(0.0)) {
setIsSleeping(false); setIsSleeping(false);
} }
} }
@ -301,7 +282,7 @@ void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
// Set the current position and orientation // Set the current position and orientation
/** /**
* @param transform The transformation of the body that transforms the local-space * @param transform The transformation of the body that transforms the local-space
* of the body into world-space * of the body int32_to world-space
*/ */
void RigidBody::setTransform(const Transform& transform) { void RigidBody::setTransform(const Transform& transform) {
@ -324,8 +305,8 @@ void RigidBody::setTransform(const Transform& transform) {
// the collision shapes attached to the body. // the collision shapes attached to the body.
void RigidBody::recomputeMassInformation() { void RigidBody::recomputeMassInformation() {
mInitMass = decimal(0.0); mInitMass = float(0.0);
mMassInverse = decimal(0.0); mMassInverse = float(0.0);
mInertiaTensorLocal.setToZero(); mInertiaTensorLocal.setToZero();
mInertiaTensorLocalInverse.setToZero(); mInertiaTensorLocalInverse.setToZero();
mCenterOfMassLocal.setToZero(); mCenterOfMassLocal.setToZero();
@ -344,12 +325,12 @@ void RigidBody::recomputeMassInformation() {
mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass(); mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass();
} }
if (mInitMass > decimal(0.0)) { if (mInitMass > float(0.0)) {
mMassInverse = decimal(1.0) / mInitMass; mMassInverse = float(1.0) / mInitMass;
} }
else { else {
mInitMass = decimal(1.0); mInitMass = float(1.0);
mMassInverse = decimal(1.0); mMassInverse = float(1.0);
} }
// Compute the center of mass // Compute the center of mass
@ -364,19 +345,19 @@ void RigidBody::recomputeMassInformation() {
Matrix3x3 inertiaTensor; Matrix3x3 inertiaTensor;
shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass()); shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass());
// Convert the collision shape inertia tensor into the local-space of the body // Convert the collision shape inertia tensor int32_to the local-space of the body
const Transform& shapeTransform = shape->getLocalToBodyTransform(); const Transform& shapeTransform = shape->getLocalToBodyTransform();
Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix(); Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix();
inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose(); inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose();
// Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape // Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape
// center into a inertia tensor w.r.t to the body origin. // center int32_to a inertia tensor w.r.t to the body origin.
Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal; Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal;
decimal offsetSquare = offset.lengthSquare(); float offsetSquare = offset.lengthSquare();
Matrix3x3 offsetMatrix; Matrix3x3 offsetMatrix;
offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0)); offsetMatrix[0].setAllValues(offsetSquare, float(0.0), float(0.0));
offsetMatrix[1].setAllValues(decimal(0.0), offsetSquare, decimal(0.0)); offsetMatrix[1].setAllValues(float(0.0), offsetSquare, float(0.0));
offsetMatrix[2].setAllValues(decimal(0.0), decimal(0.0), offsetSquare); offsetMatrix[2].setAllValues(float(0.0), float(0.0), offsetSquare);
offsetMatrix[0] += offset * (-offset.x); offsetMatrix[0] += offset * (-offset.x);
offsetMatrix[1] += offset * (-offset.y); offsetMatrix[1] += offset * (-offset.y);
offsetMatrix[2] += offset * (-offset.z); offsetMatrix[2] += offset * (-offset.z);

70
ephysics/body/RigidBody.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_RIGID_BODY_H
#define REACTPHYSICS3D_RIGID_BODY_H
// Libraries // Libraries
#include <cassert> #include <cassert>
@ -55,7 +34,7 @@ class RigidBody : public CollisionBody {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Intial mass of the body /// Intial mass of the body
decimal mInitMass; float mInitMass;
/// Center of mass of the body in local-space coordinates. /// Center of mass of the body in local-space coordinates.
/// The center of mass can therefore be different from the body origin /// The center of mass can therefore be different from the body origin
@ -84,7 +63,7 @@ class RigidBody : public CollisionBody {
Matrix3x3 mInertiaTensorLocalInverse; Matrix3x3 mInertiaTensorLocalInverse;
/// Inverse of the mass of the body /// Inverse of the mass of the body
decimal mMassInverse; float mMassInverse;
/// True if the gravity needs to be applied to this rigid body /// True if the gravity needs to be applied to this rigid body
bool mIsGravityEnabled; bool mIsGravityEnabled;
@ -93,10 +72,10 @@ class RigidBody : public CollisionBody {
Material mMaterial; Material mMaterial;
/// Linear velocity damping factor /// Linear velocity damping factor
decimal mLinearDamping; float mLinearDamping;
/// Angular velocity damping factor /// Angular velocity damping factor
decimal mAngularDamping; float mAngularDamping;
/// First element of the linked list of joints involving this body /// First element of the linked list of joints involving this body
JointListElement* mJointsList; JointListElement* mJointsList;
@ -135,7 +114,7 @@ class RigidBody : public CollisionBody {
virtual void setTransform(const Transform& transform); virtual void setTransform(const Transform& transform);
/// Return the mass of the body /// Return the mass of the body
decimal getMass() const; float getMass() const;
/// Return the linear velocity /// Return the linear velocity
Vector3 getLinearVelocity() const; Vector3 getLinearVelocity() const;
@ -162,7 +141,7 @@ class RigidBody : public CollisionBody {
void setCenterOfMassLocal(const Vector3& centerOfMassLocal); void setCenterOfMassLocal(const Vector3& centerOfMassLocal);
/// Set the mass of the rigid body /// Set the mass of the rigid body
void setMass(decimal mass); void setMass(float mass);
/// Return the inertia tensor in world coordinates. /// Return the inertia tensor in world coordinates.
Matrix3x3 getInertiaTensorWorld() const; Matrix3x3 getInertiaTensorWorld() const;
@ -183,16 +162,16 @@ class RigidBody : public CollisionBody {
void setMaterial(const Material& material); void setMaterial(const Material& material);
/// Return the linear velocity damping factor /// Return the linear velocity damping factor
decimal getLinearDamping() const; float getLinearDamping() const;
/// Set the linear damping factor /// Set the linear damping factor
void setLinearDamping(decimal linearDamping); void setLinearDamping(float linearDamping);
/// Return the angular velocity damping factor /// Return the angular velocity damping factor
decimal getAngularDamping() const; float getAngularDamping() const;
/// Set the angular damping factor /// Set the angular damping factor
void setAngularDamping(decimal angularDamping); void setAngularDamping(float angularDamping);
/// Return the first element of the linked list of joints involving this body /// Return the first element of the linked list of joints involving this body
const JointListElement* getJointsList() const; const JointListElement* getJointsList() const;
@ -212,7 +191,7 @@ class RigidBody : public CollisionBody {
/// Add a collision shape to the body. /// Add a collision shape to the body.
virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape, virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape,
const Transform& transform, const Transform& transform,
decimal mass); float mass);
/// Remove a collision shape from the body /// Remove a collision shape from the body
virtual void removeCollisionShape(const ProxyShape* proxyShape); virtual void removeCollisionShape(const ProxyShape* proxyShape);
@ -235,7 +214,7 @@ class RigidBody : public CollisionBody {
/** /**
* @return The mass (in kilograms) of the body * @return The mass (in kilograms) of the body
*/ */
inline decimal RigidBody::getMass() const { inline float RigidBody::getMass() const {
return mInitMass; return mInitMass;
} }
@ -335,7 +314,7 @@ inline void RigidBody::setMaterial(const Material& material) {
/** /**
* @return The linear damping factor of this body * @return The linear damping factor of this body
*/ */
inline decimal RigidBody::getLinearDamping() const { inline float RigidBody::getLinearDamping() const {
return mLinearDamping; return mLinearDamping;
} }
@ -344,8 +323,8 @@ inline decimal RigidBody::getLinearDamping() const {
/** /**
* @param linearDamping The linear damping factor of this body * @param linearDamping The linear damping factor of this body
*/ */
inline void RigidBody::setLinearDamping(decimal linearDamping) { inline void RigidBody::setLinearDamping(float linearDamping) {
assert(linearDamping >= decimal(0.0)); assert(linearDamping >= float(0.0));
mLinearDamping = linearDamping; mLinearDamping = linearDamping;
} }
@ -353,7 +332,7 @@ inline void RigidBody::setLinearDamping(decimal linearDamping) {
/** /**
* @return The angular damping factor of this body * @return The angular damping factor of this body
*/ */
inline decimal RigidBody::getAngularDamping() const { inline float RigidBody::getAngularDamping() const {
return mAngularDamping; return mAngularDamping;
} }
@ -362,8 +341,8 @@ inline decimal RigidBody::getAngularDamping() const {
/** /**
* @param angularDamping The angular damping factor of this body * @param angularDamping The angular damping factor of this body
*/ */
inline void RigidBody::setAngularDamping(decimal angularDamping) { inline void RigidBody::setAngularDamping(float angularDamping) {
assert(angularDamping >= decimal(0.0)); assert(angularDamping >= float(0.0));
mAngularDamping = angularDamping; mAngularDamping = angularDamping;
} }
@ -475,4 +454,3 @@ inline void RigidBody::updateTransformWithCenterOfMass() {
} }
#endif

59
ephysics/collision/CollisionDetection.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/CollisionDetection.h> #include <ephysics/collision/CollisionDetection.h>
@ -72,8 +53,8 @@ void CollisionDetection::computeCollisionDetection() {
// Compute the collision detection // Compute the collision detection
void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback, void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1, const std::set<uint32_t>& shapes1,
const std::set<uint>& shapes2) { const std::set<uint32_t>& shapes2) {
// Compute the broad-phase collision detection // Compute the broad-phase collision detection
computeBroadPhase(); computeBroadPhase();
@ -87,8 +68,8 @@ void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback,
// Report collision between two sets of shapes // Report collision between two sets of shapes
void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback, void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1, const std::set<uint32_t>& shapes1,
const std::set<uint>& shapes2) { const std::set<uint32_t>& shapes2) {
// For each possible collision pair of bodies // For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it; map<overlappingpairid, OverlappingPair*>::iterator it;
@ -121,12 +102,12 @@ void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callbac
// For each contact manifold set of the overlapping pair // For each contact manifold set of the overlapping pair
const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
for (int j=0; j<manifoldSet.getNbContactManifolds(); j++) { for (int32_t j=0; j<manifoldSet.getNbContactManifolds(); j++) {
const ContactManifold* manifold = manifoldSet.getContactManifold(j); const ContactManifold* manifold = manifoldSet.getContactManifold(j);
// For each contact manifold of the manifold set // For each contact manifold of the manifold set
for (uint i=0; i<manifold->getNbContactPoints(); i++) { for (uint32_t i=0; i<manifold->getNbContactPoints(); i++) {
ContactPoint* contactPoint = manifold->getContactPoint(i); ContactPoint* contactPoint = manifold->getContactPoint(i);
@ -246,8 +227,8 @@ void CollisionDetection::computeNarrowPhase() {
// Compute the narrow-phase collision detection // Compute the narrow-phase collision detection
void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback, void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1, const std::set<uint32_t>& shapes1,
const std::set<uint>& shapes2) { const std::set<uint32_t>& shapes2) {
mContactOverlappingPairs.clear(); mContactOverlappingPairs.clear();
@ -367,10 +348,10 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return; if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return;
// Compute the maximum number of contact manifolds for this pair // Compute the maximum number of contact manifolds for this pair
int nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(), int32_t nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(),
shape2->getCollisionShape()->getType()); shape2->getCollisionShape()->getType());
// Create the overlapping pair and add it into the set of overlapping pairs // Create the overlapping pair and add it int32_to the set of overlapping pairs
OverlappingPair* newPair = new (mWorld->mMemoryAllocator.allocate(sizeof(OverlappingPair))) OverlappingPair* newPair = new (mWorld->mMemoryAllocator.allocate(sizeof(OverlappingPair)))
OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mMemoryAllocator); OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mMemoryAllocator);
assert(newPair != NULL); assert(newPair != NULL);
@ -441,7 +422,7 @@ void CollisionDetection::createContact(OverlappingPair* overlappingPair,
// Add the contact to the contact manifold set of the corresponding overlapping pair // Add the contact to the contact manifold set of the corresponding overlapping pair
overlappingPair->addContact(contact); overlappingPair->addContact(contact);
// Add the overlapping pair into the set of pairs in contact during narrow-phase // Add the overlapping pair int32_to the set of pairs in contact during narrow-phase
overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(), overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(),
overlappingPair->getShape2()); overlappingPair->getShape2());
mContactOverlappingPairs[pairId] = overlappingPair; mContactOverlappingPairs[pairId] = overlappingPair;
@ -453,7 +434,7 @@ void CollisionDetection::addAllContactManifoldsToBodies() {
std::map<overlappingpairid, OverlappingPair*>::iterator it; std::map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) { for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) {
// Add all the contact manifolds of the pair into the list of contact manifolds // Add all the contact manifolds of the pair int32_to the list of contact manifolds
// of the two bodies involved in the contact // of the two bodies involved in the contact
addContactManifoldToBody(it->second); addContactManifoldToBody(it->second);
} }
@ -470,7 +451,7 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
// For each contact manifold in the set of manifolds in the pair // For each contact manifold in the set of manifolds in the pair
for (int i=0; i<manifoldSet.getNbContactManifolds(); i++) { for (int32_t i=0; i<manifoldSet.getNbContactManifolds(); i++) {
ContactManifold* contactManifold = manifoldSet.getContactManifold(i); ContactManifold* contactManifold = manifoldSet.getContactManifold(i);
@ -508,8 +489,8 @@ void CollisionDetection::clearContactPoints() {
void CollisionDetection::fillInCollisionMatrix() { void CollisionDetection::fillInCollisionMatrix() {
// For each possible type of collision shape // For each possible type of collision shape
for (int i=0; i<NB_COLLISION_SHAPE_TYPES; i++) { for (int32_t i=0; i<NB_COLLISION_SHAPE_TYPES; i++) {
for (int j=0; j<NB_COLLISION_SHAPE_TYPES; j++) { for (int32_t j=0; j<NB_COLLISION_SHAPE_TYPES; j++) {
mCollisionMatrix[i][j] = mCollisionDispatch->selectAlgorithm(i, j); mCollisionMatrix[i][j] = mCollisionDispatch->selectAlgorithm(i, j);
} }
} }

47
ephysics/collision/CollisionDetection.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_COLLISION_DETECTION_H
#define REACTPHYSICS3D_COLLISION_DETECTION_H
// Libraries // Libraries
#include <ephysics/body/CollisionBody.h> #include <ephysics/body/CollisionBody.h>
@ -182,13 +161,13 @@ class CollisionDetection : public NarrowPhaseCallback {
/// Compute the collision detection /// Compute the collision detection
void testCollisionBetweenShapes(CollisionCallback* callback, void testCollisionBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1, const std::set<uint32_t>& shapes1,
const std::set<uint>& shapes2); const std::set<uint32_t>& shapes2);
/// Report collision between two sets of shapes /// Report collision between two sets of shapes
void reportCollisionBetweenShapes(CollisionCallback* callback, void reportCollisionBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1, const std::set<uint32_t>& shapes1,
const std::set<uint>& shapes2) ; const std::set<uint32_t>& shapes2) ;
/// Ray casting method /// Ray casting method
void raycast(RaycastCallback* raycastCallback, const Ray& ray, void raycast(RaycastCallback* raycastCallback, const Ray& ray,
@ -207,8 +186,8 @@ class CollisionDetection : public NarrowPhaseCallback {
/// Compute the narrow-phase collision detection /// Compute the narrow-phase collision detection
void computeNarrowPhaseBetweenShapes(CollisionCallback* callback, void computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1, const std::set<uint32_t>& shapes1,
const std::set<uint>& shapes2); const std::set<uint32_t>& shapes2);
/// Return a pointer to the world /// Return a pointer to the world
CollisionWorld* getWorld(); CollisionWorld* getWorld();
@ -314,5 +293,3 @@ inline CollisionWorld* CollisionDetection::getWorld() {
} }
} }
#endif

34
ephysics/collision/CollisionShapeInfo.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_COLLISION_SHAPE_INFO_H
#define REACTPHYSICS3D_COLLISION_SHAPE_INFO_H
// Libraries // Libraries
#include <ephysics/collision/shapes/CollisionShape.h> #include <ephysics/collision/shapes/CollisionShape.h>
@ -71,5 +50,4 @@ struct CollisionShapeInfo {
} }
#endif

72
ephysics/collision/ContactManifold.cpp
View File

@ -1,27 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <iostream> #include <iostream>
@ -48,11 +30,11 @@ ContactManifold::~ContactManifold() {
void ContactManifold::addContactPoint(ContactPoint* contact) { void ContactManifold::addContactPoint(ContactPoint* contact) {
// For contact already in the manifold // For contact already in the manifold
for (uint i=0; i<mNbContactPoints; i++) { for (uint32_t i=0; i<mNbContactPoints; i++) {
// Check if the new point point does not correspond to a same contact point // Check if the new point point does not correspond to a same contact point
// already in the manifold. // already in the manifold.
decimal distance = (mContactPoints[i]->getWorldPointOnBody1() - float distance = (mContactPoints[i]->getWorldPointOnBody1() -
contact->getWorldPointOnBody1()).lengthSquare(); contact->getWorldPointOnBody1()).lengthSquare();
if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) { if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) {
@ -68,8 +50,8 @@ void ContactManifold::addContactPoint(ContactPoint* contact) {
// If the contact manifold is full // If the contact manifold is full
if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) { if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) {
int indexMaxPenetration = getIndexOfDeepestPenetration(contact); int32_t indexMaxPenetration = getIndexOfDeepestPenetration(contact);
int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1()); int32_t indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
removeContactPoint(indexToRemove); removeContactPoint(indexToRemove);
} }
@ -81,7 +63,7 @@ void ContactManifold::addContactPoint(ContactPoint* contact) {
} }
// Remove a contact point from the manifold // Remove a contact point from the manifold
void ContactManifold::removeContactPoint(uint index) { void ContactManifold::removeContactPoint(uint32_t index) {
assert(index < mNbContactPoints); assert(index < mNbContactPoints);
assert(mNbContactPoints > 0); assert(mNbContactPoints > 0);
@ -109,7 +91,7 @@ void ContactManifold::update(const Transform& transform1, const Transform& trans
if (mNbContactPoints == 0) return; if (mNbContactPoints == 0) return;
// Update the world coordinates and penetration depth of the contact points in the manifold // Update the world coordinates and penetration depth of the contact points in the manifold
for (uint i=0; i<mNbContactPoints; i++) { for (uint32_t i=0; i<mNbContactPoints; i++) {
mContactPoints[i]->setWorldPointOnBody1(transform1 * mContactPoints[i]->setWorldPointOnBody1(transform1 *
mContactPoints[i]->getLocalPointOnBody1()); mContactPoints[i]->getLocalPointOnBody1());
mContactPoints[i]->setWorldPointOnBody2(transform2 * mContactPoints[i]->setWorldPointOnBody2(transform2 *
@ -118,15 +100,15 @@ void ContactManifold::update(const Transform& transform1, const Transform& trans
mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal())); mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal()));
} }
const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD * const float squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD *
PERSISTENT_CONTACT_DIST_THRESHOLD; PERSISTENT_CONTACT_DIST_THRESHOLD;
// Remove the contact points that don't represent very well the contact manifold // Remove the contact points that don't represent very well the contact manifold
for (int i=static_cast<int>(mNbContactPoints)-1; i>=0; i--) { for (int32_t i=static_cast<int32_t>(mNbContactPoints)-1; i>=0; i--) {
assert(i < static_cast<int>(mNbContactPoints)); assert(i < static_cast<int32_t>(mNbContactPoints));
// Compute the distance between contact points in the normal direction // Compute the distance between contact points in the normal direction
decimal distanceNormal = -mContactPoints[i]->getPenetrationDepth(); float distanceNormal = -mContactPoints[i]->getPenetrationDepth();
// If the contacts points are too far from each other in the normal direction // If the contacts points are too far from each other in the normal direction
if (distanceNormal > squarePersistentContactThreshold) { if (distanceNormal > squarePersistentContactThreshold) {
@ -151,13 +133,13 @@ void ContactManifold::update(const Transform& transform1, const Transform& trans
// Return the index of the contact point with the larger penetration depth. // Return the index of the contact point with the larger penetration depth.
/// This corresponding contact will be kept in the cache. The method returns -1 is /// This corresponding contact will be kept in the cache. The method returns -1 is
/// the new contact is the deepest. /// the new contact is the deepest.
int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const { int32_t ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const {
assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
int indexMaxPenetrationDepth = -1; int32_t indexMaxPenetrationDepth = -1;
decimal maxPenetrationDepth = newContact->getPenetrationDepth(); float maxPenetrationDepth = newContact->getPenetrationDepth();
// For each contact in the cache // For each contact in the cache
for (uint i=0; i<mNbContactPoints; i++) { for (uint32_t i=0; i<mNbContactPoints; i++) {
// If the current contact has a larger penetration depth // If the current contact has a larger penetration depth
if (mContactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) { if (mContactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) {
@ -180,14 +162,14 @@ int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) cons
/// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore, /// 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 /// 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). /// by Erwin Coumans (http://wwww.bulletphysics.org).
int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const { int32_t ContactManifold::getIndexToRemove(int32_t indexMaxPenetration, const Vector3& newPoint) const {
assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
decimal area0 = 0.0; // Area with contact 1,2,3 and newPoint float area0 = 0.0; // Area with contact 1,2,3 and newPoint
decimal area1 = 0.0; // Area with contact 0,2,3 and newPoint float area1 = 0.0; // Area with contact 0,2,3 and newPoint
decimal area2 = 0.0; // Area with contact 0,1,3 and newPoint float area2 = 0.0; // Area with contact 0,1,3 and newPoint
decimal area3 = 0.0; // Area with contact 0,1,2 and newPoint float area3 = 0.0; // Area with contact 0,1,2 and newPoint
if (indexMaxPenetration != 0) { if (indexMaxPenetration != 0) {
// Compute the area // Compute the area
@ -227,7 +209,7 @@ int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& ne
} }
// Return the index of maximum area // Return the index of maximum area
int ContactManifold::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const { int32_t ContactManifold::getMaxArea(float area0, float area1, float area2, float area3) const {
if (area0 < area1) { if (area0 < area1) {
if (area1 < area2) { if (area1 < area2) {
if (area2 < area3) return 3; if (area2 < area3) return 3;
@ -252,7 +234,7 @@ int ContactManifold::getMaxArea(decimal area0, decimal area1, decimal area2, dec
// Clear the contact manifold // Clear the contact manifold
void ContactManifold::clear() { void ContactManifold::clear() {
for (uint i=0; i<mNbContactPoints; i++) { for (uint32_t i=0; i<mNbContactPoints; i++) {
// Call the destructor explicitly and tell the memory allocator that // Call the destructor explicitly and tell the memory allocator that
// the corresponding memory block is now free // the corresponding memory block is now free

111
ephysics/collision/ContactManifold.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CONTACT_MANIFOLD_H
#define REACTPHYSICS3D_CONTACT_MANIFOLD_H
// Libraries // Libraries
#include <vector> #include <vector>
@ -37,7 +16,7 @@
namespace reactphysics3d { namespace reactphysics3d {
// Constants // Constants
const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold const uint32_t MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold
// Class declarations // Class declarations
class ContactManifold; class ContactManifold;
@ -99,10 +78,10 @@ class ContactManifold {
ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD]; ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD];
/// Normal direction Id (Unique Id representing the normal direction) /// Normal direction Id (Unique Id representing the normal direction)
short int mNormalDirectionId; int16_t mNormalDirectionId;
/// Number of contacts in the cache /// Number of contacts in the cache
uint mNbContactPoints; uint32_t mNbContactPoints;
/// First friction vector of the contact manifold /// First friction vector of the contact manifold
Vector3 mFrictionVector1; Vector3 mFrictionVector1;
@ -111,18 +90,18 @@ class ContactManifold {
Vector3 mFrictionVector2; Vector3 mFrictionVector2;
/// First friction constraint accumulated impulse /// First friction constraint accumulated impulse
decimal mFrictionImpulse1; float mFrictionImpulse1;
/// Second friction constraint accumulated impulse /// Second friction constraint accumulated impulse
decimal mFrictionImpulse2; float mFrictionImpulse2;
/// Twist friction constraint accumulated impulse /// Twist friction constraint accumulated impulse
decimal mFrictionTwistImpulse; float mFrictionTwistImpulse;
/// Accumulated rolling resistance impulse /// Accumulated rolling resistance impulse
Vector3 mRollingResistanceImpulse; Vector3 mRollingResistanceImpulse;
/// True if the contact manifold has already been added into an island /// True if the contact manifold has already been added int32_to an island
bool mIsAlreadyInIsland; bool mIsAlreadyInIsland;
/// Reference to the memory allocator /// Reference to the memory allocator
@ -137,18 +116,18 @@ class ContactManifold {
ContactManifold& operator=(const ContactManifold& contactManifold); ContactManifold& operator=(const ContactManifold& contactManifold);
/// Return the index of maximum area /// Return the index of maximum area
int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const; int32_t getMaxArea(float area0, float area1, float area2, float area3) const;
/// Return the index of the contact with the larger penetration depth. /// Return the index of the contact with the larger penetration depth.
int getIndexOfDeepestPenetration(ContactPoint* newContact) const; int32_t getIndexOfDeepestPenetration(ContactPoint* newContact) const;
/// Return the index that will be removed. /// Return the index that will be removed.
int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const; int32_t getIndexToRemove(int32_t indexMaxPenetration, const Vector3& newPoint) const;
/// Remove a contact point from the manifold /// Remove a contact point from the manifold
void removeContactPoint(uint index); void removeContactPoint(uint32_t index);
/// Return true if the contact manifold has already been added into an island /// Return true if the contact manifold has already been added int32_to an island
bool isAlreadyInIsland() const; bool isAlreadyInIsland() const;
public: public:
@ -157,7 +136,7 @@ class ContactManifold {
/// Constructor /// Constructor
ContactManifold(ProxyShape* shape1, ProxyShape* shape2, ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, short int normalDirectionId); MemoryAllocator& memoryAllocator, int16_t normalDirectionId);
/// Destructor /// Destructor
~ContactManifold(); ~ContactManifold();
@ -175,7 +154,7 @@ class ContactManifold {
CollisionBody* getBody2() const; CollisionBody* getBody2() const;
/// Return the normal direction Id /// Return the normal direction Id
short int getNormalDirectionId() const; int16_t getNormalDirectionId() const;
/// Add a contact point to the manifold /// Add a contact point to the manifold
void addContactPoint(ContactPoint* contact); void addContactPoint(ContactPoint* contact);
@ -187,7 +166,7 @@ class ContactManifold {
void clear(); void clear();
/// Return the number of contact points in the manifold /// Return the number of contact points in the manifold
uint getNbContactPoints() const; uint32_t getNbContactPoints() const;
/// Return the first friction vector at the center of the contact manifold /// Return the first friction vector at the center of the contact manifold
const Vector3& getFrictionVector1() const; const Vector3& getFrictionVector1() const;
@ -202,34 +181,34 @@ class ContactManifold {
void setFrictionVector2(const Vector3& mFrictionVector2); void setFrictionVector2(const Vector3& mFrictionVector2);
/// Return the first friction accumulated impulse /// Return the first friction accumulated impulse
decimal getFrictionImpulse1() const; float getFrictionImpulse1() const;
/// Set the first friction accumulated impulse /// Set the first friction accumulated impulse
void setFrictionImpulse1(decimal frictionImpulse1); void setFrictionImpulse1(float frictionImpulse1);
/// Return the second friction accumulated impulse /// Return the second friction accumulated impulse
decimal getFrictionImpulse2() const; float getFrictionImpulse2() const;
/// Set the second friction accumulated impulse /// Set the second friction accumulated impulse
void setFrictionImpulse2(decimal frictionImpulse2); void setFrictionImpulse2(float frictionImpulse2);
/// Return the friction twist accumulated impulse /// Return the friction twist accumulated impulse
decimal getFrictionTwistImpulse() const; float getFrictionTwistImpulse() const;
/// Set the friction twist accumulated impulse /// Set the friction twist accumulated impulse
void setFrictionTwistImpulse(decimal frictionTwistImpulse); void setFrictionTwistImpulse(float frictionTwistImpulse);
/// Set the accumulated rolling resistance impulse /// Set the accumulated rolling resistance impulse
void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse); void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse);
/// Return a contact point of the manifold /// Return a contact point of the manifold
ContactPoint* getContactPoint(uint index) const; ContactPoint* getContactPoint(uint32_t index) const;
/// Return the normalized averaged normal vector /// Return the normalized averaged normal vector
Vector3 getAverageContactNormal() const; Vector3 getAverageContactNormal() const;
/// Return the largest depth of all the contact points /// Return the largest depth of all the contact points
decimal getLargestContactDepth() const; float getLargestContactDepth() const;
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
@ -259,12 +238,12 @@ inline CollisionBody* ContactManifold::getBody2() const {
} }
// Return the normal direction Id // Return the normal direction Id
inline short int ContactManifold::getNormalDirectionId() const { inline int16_t ContactManifold::getNormalDirectionId() const {
return mNormalDirectionId; return mNormalDirectionId;
} }
// Return the number of contact points in the manifold // Return the number of contact points in the manifold
inline uint ContactManifold::getNbContactPoints() const { inline uint32_t ContactManifold::getNbContactPoints() const {
return mNbContactPoints; return mNbContactPoints;
} }
@ -289,32 +268,32 @@ inline void ContactManifold::setFrictionVector2(const Vector3& frictionVector2)
} }
// Return the first friction accumulated impulse // Return the first friction accumulated impulse
inline decimal ContactManifold::getFrictionImpulse1() const { inline float ContactManifold::getFrictionImpulse1() const {
return mFrictionImpulse1; return mFrictionImpulse1;
} }
// Set the first friction accumulated impulse // Set the first friction accumulated impulse
inline void ContactManifold::setFrictionImpulse1(decimal frictionImpulse1) { inline void ContactManifold::setFrictionImpulse1(float frictionImpulse1) {
mFrictionImpulse1 = frictionImpulse1; mFrictionImpulse1 = frictionImpulse1;
} }
// Return the second friction accumulated impulse // Return the second friction accumulated impulse
inline decimal ContactManifold::getFrictionImpulse2() const { inline float ContactManifold::getFrictionImpulse2() const {
return mFrictionImpulse2; return mFrictionImpulse2;
} }
// Set the second friction accumulated impulse // Set the second friction accumulated impulse
inline void ContactManifold::setFrictionImpulse2(decimal frictionImpulse2) { inline void ContactManifold::setFrictionImpulse2(float frictionImpulse2) {
mFrictionImpulse2 = frictionImpulse2; mFrictionImpulse2 = frictionImpulse2;
} }
// Return the friction twist accumulated impulse // Return the friction twist accumulated impulse
inline decimal ContactManifold::getFrictionTwistImpulse() const { inline float ContactManifold::getFrictionTwistImpulse() const {
return mFrictionTwistImpulse; return mFrictionTwistImpulse;
} }
// Set the friction twist accumulated impulse // Set the friction twist accumulated impulse
inline void ContactManifold::setFrictionTwistImpulse(decimal frictionTwistImpulse) { inline void ContactManifold::setFrictionTwistImpulse(float frictionTwistImpulse) {
mFrictionTwistImpulse = frictionTwistImpulse; mFrictionTwistImpulse = frictionTwistImpulse;
} }
@ -324,12 +303,12 @@ inline void ContactManifold::setRollingResistanceImpulse(const Vector3& rollingR
} }
// Return a contact point of the manifold // Return a contact point of the manifold
inline ContactPoint* ContactManifold::getContactPoint(uint index) const { inline ContactPoint* ContactManifold::getContactPoint(uint32_t index) const {
assert(index < mNbContactPoints); assert(index < mNbContactPoints);
return mContactPoints[index]; return mContactPoints[index];
} }
// Return true if the contact manifold has already been added into an island // Return true if the contact manifold has already been added int32_to an island
inline bool ContactManifold::isAlreadyInIsland() const { inline bool ContactManifold::isAlreadyInIsland() const {
return mIsAlreadyInIsland; return mIsAlreadyInIsland;
} }
@ -338,7 +317,7 @@ inline bool ContactManifold::isAlreadyInIsland() const {
inline Vector3 ContactManifold::getAverageContactNormal() const { inline Vector3 ContactManifold::getAverageContactNormal() const {
Vector3 averageNormal; Vector3 averageNormal;
for (uint i=0; i<mNbContactPoints; i++) { for (uint32_t i=0; i<mNbContactPoints; i++) {
averageNormal += mContactPoints[i]->getNormal(); averageNormal += mContactPoints[i]->getNormal();
} }
@ -346,11 +325,11 @@ inline Vector3 ContactManifold::getAverageContactNormal() const {
} }
// Return the largest depth of all the contact points // Return the largest depth of all the contact points
inline decimal ContactManifold::getLargestContactDepth() const { inline float ContactManifold::getLargestContactDepth() const {
decimal largestDepth = 0.0f; float largestDepth = 0.0f;
for (uint i=0; i<mNbContactPoints; i++) { for (uint32_t i=0; i<mNbContactPoints; i++) {
decimal depth = mContactPoints[i]->getPenetrationDepth(); float depth = mContactPoints[i]->getPenetrationDepth();
if (depth > largestDepth) { if (depth > largestDepth) {
largestDepth = depth; largestDepth = depth;
} }
@ -360,5 +339,5 @@ inline decimal ContactManifold::getLargestContactDepth() const {
} }
} }
#endif

83
ephysics/collision/ContactManifoldSet.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/ContactManifoldSet.h> #include <ephysics/collision/ContactManifoldSet.h>
@ -30,7 +11,7 @@ using namespace reactphysics3d;
// Constructor // Constructor
ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, int nbMaxManifolds) MemoryAllocator& memoryAllocator, int32_t nbMaxManifolds)
: mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1), : mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1),
mShape2(shape2), mMemoryAllocator(memoryAllocator) { mShape2(shape2), mMemoryAllocator(memoryAllocator) {
assert(nbMaxManifolds >= 1); assert(nbMaxManifolds >= 1);
@ -47,7 +28,7 @@ ContactManifoldSet::~ContactManifoldSet() {
void ContactManifoldSet::addContactPoint(ContactPoint* contact) { void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
// Compute an Id corresponding to the normal direction (using a cubemap) // Compute an Id corresponding to the normal direction (using a cubemap)
short int normalDirectionId = computeCubemapNormalId(contact->getNormal()); int16_t normalDirectionId = computeCubemapNormalId(contact->getNormal());
// If there is no contact manifold yet // If there is no contact manifold yet
if (mNbManifolds == 0) { if (mNbManifolds == 0) {
@ -55,7 +36,7 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
createManifold(normalDirectionId); createManifold(normalDirectionId);
mManifolds[0]->addContactPoint(contact); mManifolds[0]->addContactPoint(contact);
assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0); assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0);
for (int i=0; i<mNbManifolds; i++) { for (int32_t i=0; i<mNbManifolds; i++) {
assert(mManifolds[i]->getNbContactPoints() > 0); assert(mManifolds[i]->getNbContactPoints() > 0);
} }
@ -63,7 +44,7 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
} }
// Select the manifold with the most similar normal (if exists) // Select the manifold with the most similar normal (if exists)
int similarManifoldIndex = 0; int32_t similarManifoldIndex = 0;
if (mNbMaxManifolds > 1) { if (mNbMaxManifolds > 1) {
similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId); similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId);
} }
@ -84,7 +65,7 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
// Create a new manifold for the contact point // Create a new manifold for the contact point
createManifold(normalDirectionId); createManifold(normalDirectionId);
mManifolds[mNbManifolds-1]->addContactPoint(contact); mManifolds[mNbManifolds-1]->addContactPoint(contact);
for (int i=0; i<mNbManifolds; i++) { for (int32_t i=0; i<mNbManifolds; i++) {
assert(mManifolds[i]->getNbContactPoints() > 0); assert(mManifolds[i]->getNbContactPoints() > 0);
} }
@ -94,11 +75,11 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
// The contact point will be in a new contact manifold, we now have too much // The contact point will be in a new contact manifold, we now have too much
// manifolds condidates. We need to remove one. We choose to keep the manifolds // manifolds condidates. We need to remove one. We choose to keep the manifolds
// with the largest contact depth among their points // with the largest contact depth among their points
int smallestDepthIndex = -1; int32_t smallestDepthIndex = -1;
decimal minDepth = contact->getPenetrationDepth(); float minDepth = contact->getPenetrationDepth();
assert(mNbManifolds == mNbMaxManifolds); assert(mNbManifolds == mNbMaxManifolds);
for (int i=0; i<mNbManifolds; i++) { for (int32_t i=0; i<mNbManifolds; i++) {
decimal depth = mManifolds[i]->getLargestContactDepth(); float depth = mManifolds[i]->getLargestContactDepth();
if (depth < minDepth) { if (depth < minDepth) {
minDepth = depth; minDepth = depth;
smallestDepthIndex = i; smallestDepthIndex = i;
@ -124,7 +105,7 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
createManifold(normalDirectionId); createManifold(normalDirectionId);
mManifolds[mNbManifolds-1]->addContactPoint(contact); mManifolds[mNbManifolds-1]->addContactPoint(contact);
assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0); assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0);
for (int i=0; i<mNbManifolds; i++) { for (int32_t i=0; i<mNbManifolds; i++) {
assert(mManifolds[i]->getNbContactPoints() > 0); assert(mManifolds[i]->getNbContactPoints() > 0);
} }
@ -133,10 +114,10 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
// Return the index of the contact manifold with a similar average normal. // Return the index of the contact manifold with a similar average normal.
// If no manifold has close enough average normal, it returns -1 // If no manifold has close enough average normal, it returns -1
int ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const { int32_t ContactManifoldSet::selectManifoldWithSimilarNormal(int16_t normalDirectionId) const {
// Return the Id of the manifold with the same normal direction id (if exists) // Return the Id of the manifold with the same normal direction id (if exists)
for (int i=0; i<mNbManifolds; i++) { for (int32_t i=0; i<mNbManifolds; i++) {
if (normalDirectionId == mManifolds[i]->getNormalDirectionId()) { if (normalDirectionId == mManifolds[i]->getNormalDirectionId()) {
return i; return i;
} }
@ -145,16 +126,16 @@ int ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectio
return -1; return -1;
} }
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) // Map the normal vector int32_to a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided into 4x4 buckets. This method maps the // Each face of the cube is divided int32_to 4x4 buckets. This method maps the
// normal vector into of the of the bucket and returns a unique Id for the bucket // normal vector int32_to of the of the bucket and returns a unique Id for the bucket
short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) const { int16_t ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) const {
assert(normal.lengthSquare() > MACHINE_EPSILON); assert(normal.lengthSquare() > MACHINE_EPSILON);
int faceNo; int32_t faceNo;
decimal u, v; float u, v;
decimal max = max3(fabs(normal.x), fabs(normal.y), fabs(normal.z)); float max = max3(fabs(normal.x), fabs(normal.y), fabs(normal.z));
Vector3 normalScaled = normal / max; Vector3 normalScaled = normal / max;
if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) { if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) {
@ -173,19 +154,19 @@ short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) cons
v = normalScaled.y; v = normalScaled.y;
} }
int indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); int32_t indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
int indexV = floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); int32_t indexV = floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexU--; if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexU--;
if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexV--; if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexV--;
const int nbSubDivInFace = CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS; const int32_t nbSubDivInFace = CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS;
return faceNo * 200 + indexU * nbSubDivInFace + indexV; return faceNo * 200 + indexU * nbSubDivInFace + indexV;
} }
// Update the contact manifolds // Update the contact manifolds
void ContactManifoldSet::update() { void ContactManifoldSet::update() {
for (int i=mNbManifolds-1; i>=0; i--) { for (int32_t i=mNbManifolds-1; i>=0; i--) {
// Update the contact manifold // Update the contact manifold
mManifolds[i]->update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(), mManifolds[i]->update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(),
@ -202,7 +183,7 @@ void ContactManifoldSet::update() {
void ContactManifoldSet::clear() { void ContactManifoldSet::clear() {
// Destroy all the contact manifolds // Destroy all the contact manifolds
for (int i=mNbManifolds-1; i>=0; i--) { for (int32_t i=mNbManifolds-1; i>=0; i--) {
removeManifold(i); removeManifold(i);
} }
@ -210,7 +191,7 @@ void ContactManifoldSet::clear() {
} }
// Create a new contact manifold and add it to the set // Create a new contact manifold and add it to the set
void ContactManifoldSet::createManifold(short int normalDirectionId) { void ContactManifoldSet::createManifold(int16_t normalDirectionId) {
assert(mNbManifolds < mNbMaxManifolds); assert(mNbManifolds < mNbMaxManifolds);
mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold))) mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
@ -219,7 +200,7 @@ void ContactManifoldSet::createManifold(short int normalDirectionId) {
} }
// Remove a contact manifold from the set // Remove a contact manifold from the set
void ContactManifoldSet::removeManifold(int index) { void ContactManifoldSet::removeManifold(int32_t index) {
assert(mNbManifolds > 0); assert(mNbManifolds > 0);
assert(index >= 0 && index < mNbManifolds); assert(index >= 0 && index < mNbManifolds);
@ -228,7 +209,7 @@ void ContactManifoldSet::removeManifold(int index) {
mManifolds[index]->~ContactManifold(); mManifolds[index]->~ContactManifold();
mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold)); mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold));
for (int i=index; (i+1) < mNbManifolds; i++) { for (int32_t i=index; (i+1) < mNbManifolds; i++) {
mManifolds[i] = mManifolds[i+1]; mManifolds[i] = mManifolds[i+1];
} }

73
ephysics/collision/ContactManifoldSet.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CONTACT_MANIFOLD_SET_H
#define REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H
// Libraries // Libraries
#include <ephysics/collision/ContactManifold.h> #include <ephysics/collision/ContactManifold.h>
@ -32,8 +11,8 @@
namespace reactphysics3d { namespace reactphysics3d {
// Constants // Constants
const int MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET = 3; // Maximum number of contact manifolds in the set const int32_t MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET = 3; // Maximum number of contact manifolds in the set
const int CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS = 3; // N Number for the N x N subdivisions of the cubemap const int32_t CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS = 3; // N Number for the N x N subdivisions of the cubemap
// Class ContactManifoldSet // Class ContactManifoldSet
/** /**
@ -49,10 +28,10 @@ class ContactManifoldSet {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Maximum number of contact manifolds in the set /// Maximum number of contact manifolds in the set
int mNbMaxManifolds; int32_t mNbMaxManifolds;
/// Current number of contact manifolds in the set /// Current number of contact manifolds in the set
int mNbManifolds; int32_t mNbManifolds;
/// Pointer to the first proxy shape of the contact /// Pointer to the first proxy shape of the contact
ProxyShape* mShape1; ProxyShape* mShape1;
@ -72,15 +51,15 @@ class ContactManifoldSet {
void createManifold(short normalDirectionId); void createManifold(short normalDirectionId);
/// Remove a contact manifold from the set /// Remove a contact manifold from the set
void removeManifold(int index); void removeManifold(int32_t index);
// Return the index of the contact manifold with a similar average normal. // Return the index of the contact manifold with a similar average normal.
int selectManifoldWithSimilarNormal(short int normalDirectionId) const; int32_t selectManifoldWithSimilarNormal(int16_t normalDirectionId) const;
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) // Map the normal vector int32_to a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided into 4x4 buckets. This method maps the // Each face of the cube is divided int32_to 4x4 buckets. This method maps the
// normal vector into of the of the bucket and returns a unique Id for the bucket // normal vector int32_to of the of the bucket and returns a unique Id for the bucket
short int computeCubemapNormalId(const Vector3& normal) const; int16_t computeCubemapNormalId(const Vector3& normal) const;
public: public:
@ -88,7 +67,7 @@ class ContactManifoldSet {
/// Constructor /// Constructor
ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, int nbMaxManifolds); MemoryAllocator& memoryAllocator, int32_t nbMaxManifolds);
/// Destructor /// Destructor
~ContactManifoldSet(); ~ContactManifoldSet();
@ -109,13 +88,13 @@ class ContactManifoldSet {
void clear(); void clear();
/// Return the number of manifolds in the set /// Return the number of manifolds in the set
int getNbContactManifolds() const; int32_t getNbContactManifolds() const;
/// Return a given contact manifold /// Return a given contact manifold
ContactManifold* getContactManifold(int index) const; ContactManifold* getContactManifold(int32_t index) const;
/// Return the total number of contact points in the set of manifolds /// Return the total number of contact points in the set of manifolds
int getTotalNbContactPoints() const; int32_t getTotalNbContactPoints() const;
}; };
// Return the first proxy shape // Return the first proxy shape
@ -129,20 +108,20 @@ inline ProxyShape* ContactManifoldSet::getShape2() const {
} }
// Return the number of manifolds in the set // Return the number of manifolds in the set
inline int ContactManifoldSet::getNbContactManifolds() const { inline int32_t ContactManifoldSet::getNbContactManifolds() const {
return mNbManifolds; return mNbManifolds;
} }
// Return a given contact manifold // Return a given contact manifold
inline ContactManifold* ContactManifoldSet::getContactManifold(int index) const { inline ContactManifold* ContactManifoldSet::getContactManifold(int32_t index) const {
assert(index >= 0 && index < mNbManifolds); assert(index >= 0 && index < mNbManifolds);
return mManifolds[index]; return mManifolds[index];
} }
// Return the total number of contact points in the set of manifolds // Return the total number of contact points in the set of manifolds
inline int ContactManifoldSet::getTotalNbContactPoints() const { inline int32_t ContactManifoldSet::getTotalNbContactPoints() const {
int nbPoints = 0; int32_t nbPoints = 0;
for (int i=0; i<mNbManifolds; i++) { for (int32_t i=0; i<mNbManifolds; i++) {
nbPoints += mManifolds[i]->getNbContactPoints(); nbPoints += mManifolds[i]->getNbContactPoints();
} }
return nbPoints; return nbPoints;
@ -150,5 +129,3 @@ inline int ContactManifoldSet::getTotalNbContactPoints() const {
} }
#endif

35
ephysics/collision/ProxyShape.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/ProxyShape.h> #include <ephysics/collision/ProxyShape.h>
@ -35,7 +16,7 @@ using namespace reactphysics3d;
* @param transform Transformation from collision shape local-space to body local-space * @param transform Transformation from collision shape local-space to body local-space
* @param mass Mass of the collision shape (in kilograms) * @param mass Mass of the collision shape (in kilograms)
*/ */
ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass) ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, float mass)
:mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass), :mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass),
mNext(NULL), mBroadPhaseID(-1), mCachedCollisionData(NULL), mUserData(NULL), mNext(NULL), mBroadPhaseID(-1), mCachedCollisionData(NULL), mUserData(NULL),
mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) { mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) {
@ -74,7 +55,7 @@ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
// If the corresponding body is not active, it cannot be hit by rays // If the corresponding body is not active, it cannot be hit by rays
if (!mBody->isActive()) return false; if (!mBody->isActive()) return false;
// Convert the ray into the local-space of the collision shape // Convert the ray int32_to the local-space of the collision shape
const Transform localToWorldTransform = getLocalToWorldTransform(); const Transform localToWorldTransform = getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse(); const Transform worldToLocalTransform = localToWorldTransform.getInverse();
Ray rayLocal(worldToLocalTransform * ray.point1, Ray rayLocal(worldToLocalTransform * ray.point1,
@ -83,7 +64,7 @@ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this); bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this);
// Convert the raycast info into world-space // Convert the raycast info int32_to world-space
raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint; raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint;
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * raycastInfo.worldNormal; raycastInfo.worldNormal = localToWorldTransform.getOrientation() * raycastInfo.worldNormal;
raycastInfo.worldNormal.normalize(); raycastInfo.worldNormal.normalize();

48
ephysics/collision/ProxyShape.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_PROXY_SHAPE_H
#define REACTPHYSICS3D_PROXY_SHAPE_H
// Libraries // Libraries
#include <ephysics/body/CollisionBody.h> #include <ephysics/body/CollisionBody.h>
@ -58,13 +37,13 @@ class ProxyShape {
Transform mLocalToBodyTransform; Transform mLocalToBodyTransform;
/// Mass (in kilogramms) of the corresponding collision shape /// Mass (in kilogramms) of the corresponding collision shape
decimal mMass; float mMass;
/// Pointer to the next proxy shape of the body (linked list) /// Pointer to the next proxy shape of the body (linked list)
ProxyShape* mNext; ProxyShape* mNext;
/// Broad-phase ID (node ID in the dynamic AABB tree) /// Broad-phase ID (node ID in the dynamic AABB tree)
int mBroadPhaseID; int32_t mBroadPhaseID;
/// Cached collision data /// Cached collision data
void* mCachedCollisionData; void* mCachedCollisionData;
@ -99,7 +78,7 @@ class ProxyShape {
/// Constructor /// Constructor
ProxyShape(CollisionBody* body, CollisionShape* shape, ProxyShape(CollisionBody* body, CollisionShape* shape,
const Transform& transform, decimal mass); const Transform& transform, float mass);
/// Destructor /// Destructor
virtual ~ProxyShape(); virtual ~ProxyShape();
@ -111,7 +90,7 @@ class ProxyShape {
CollisionBody* getBody() const; CollisionBody* getBody() const;
/// Return the mass of the collision shape /// Return the mass of the collision shape
decimal getMass() const; float getMass() const;
/// Return a pointer to the user data attached to this body /// Return a pointer to the user data attached to this body
void* getUserData() const; void* getUserData() const;
@ -184,7 +163,7 @@ inline void** ProxyShape::getCachedCollisionData() {
// Return the collision shape // Return the collision shape
/** /**
* @return Pointer to the internal collision shape * @return Pointer to the int32_ternal collision shape
*/ */
inline const CollisionShape* ProxyShape::getCollisionShape() const { inline const CollisionShape* ProxyShape::getCollisionShape() const {
return mCollisionShape; return mCollisionShape;
@ -202,13 +181,13 @@ inline CollisionBody* ProxyShape::getBody() const {
/** /**
* @return Mass of the collision shape (in kilograms) * @return Mass of the collision shape (in kilograms)
*/ */
inline decimal ProxyShape::getMass() const { inline float ProxyShape::getMass() const {
return mMass; return mMass;
} }
// Return a pointer to the user data attached to this body // Return a pointer to the user data attached to this body
/** /**
* @return A pointer to the user data stored into the proxy shape * @return A pointer to the user data stored int32_to the proxy shape
*/ */
inline void* ProxyShape::getUserData() const { inline void* ProxyShape::getUserData() const {
return mUserData; return mUserData;
@ -324,4 +303,3 @@ inline void ProxyShape::setLocalScaling(const Vector3& scaling) {
} }
#endif

33
ephysics/collision/RaycastInfo.cpp
View File

@ -1,37 +1,18 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/decimal.h>
#include <ephysics/collision/RaycastInfo.h> #include <ephysics/collision/RaycastInfo.h>
#include <ephysics/collision/ProxyShape.h> #include <ephysics/collision/ProxyShape.h>
using namespace reactphysics3d; using namespace reactphysics3d;
// Ray cast test against a proxy shape // Ray cast test against a proxy shape
decimal RaycastTest::raycastAgainstShape(ProxyShape* shape, const Ray& ray) { float RaycastTest::raycastAgainstShape(ProxyShape* shape, const Ray& ray) {
// Ray casting test against the collision shape // Ray casting test against the collision shape
RaycastInfo raycastInfo; RaycastInfo raycastInfo;

44
ephysics/collision/RaycastInfo.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_RAYCAST_INFO_H
#define REACTPHYSICS3D_RAYCAST_INFO_H
// Libraries // Libraries
#include <ephysics/mathematics/Vector3.h> #include <ephysics/mathematics/Vector3.h>
@ -66,13 +45,13 @@ struct RaycastInfo {
/// Fraction distance of the hit point between point1 and point2 of the ray /// 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) /// The hit point "p" is such that p = point1 + hitFraction * (point2 - point1)
decimal hitFraction; float hitFraction;
/// Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise) /// Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise)
int meshSubpart; int32_t meshSubpart;
/// Hit triangle index (only used for triangles mesh and -1 otherwise) /// Hit triangle index (only used for triangles mesh and -1 otherwise)
int triangleIndex; int32_t triangleIndex;
/// Pointer to the hit collision body /// Pointer to the hit collision body
CollisionBody* body; CollisionBody* body;
@ -126,7 +105,7 @@ class RaycastCallback {
* @param raycastInfo Information about the raycast hit * @param raycastInfo Information about the raycast hit
* @return Value that controls the continuation of the ray after a hit * @return Value that controls the continuation of the ray after a hit
*/ */
virtual decimal notifyRaycastHit(const RaycastInfo& raycastInfo)=0; virtual float notifyRaycastHit(const RaycastInfo& raycastInfo)=0;
}; };
@ -144,9 +123,8 @@ struct RaycastTest {
} }
/// Ray cast test against a proxy shape /// Ray cast test against a proxy shape
decimal raycastAgainstShape(ProxyShape* shape, const Ray& ray); float raycastAgainstShape(ProxyShape* shape, const Ray& ray);
}; };
} }
#endif

29
ephysics/collision/TriangleMesh.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/TriangleMesh.h> #include <ephysics/collision/TriangleMesh.h>

42
ephysics/collision/TriangleMesh.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_TRIANGLE_MESH_H
#define REACTPHYSICS3D_TRIANGLE_MESH_H
// Libraries // Libraries
#include <vector> #include <vector>
@ -60,10 +39,10 @@ class TriangleMesh {
void addSubpart(TriangleVertexArray* triangleVertexArray); void addSubpart(TriangleVertexArray* triangleVertexArray);
/// Return a pointer to a given subpart (triangle vertex array) of the mesh /// Return a pointer to a given subpart (triangle vertex array) of the mesh
TriangleVertexArray* getSubpart(uint indexSubpart) const; TriangleVertexArray* getSubpart(uint32_t indexSubpart) const;
/// Return the number of subparts of the mesh /// Return the number of subparts of the mesh
uint getNbSubparts() const; uint32_t getNbSubparts() const;
}; };
// Add a subpart of the mesh // Add a subpart of the mesh
@ -72,17 +51,16 @@ inline void TriangleMesh::addSubpart(TriangleVertexArray* triangleVertexArray) {
} }
// Return a pointer to a given subpart (triangle vertex array) of the mesh // Return a pointer to a given subpart (triangle vertex array) of the mesh
inline TriangleVertexArray* TriangleMesh::getSubpart(uint indexSubpart) const { inline TriangleVertexArray* TriangleMesh::getSubpart(uint32_t indexSubpart) const {
assert(indexSubpart < mTriangleArrays.size()); assert(indexSubpart < mTriangleArrays.size());
return mTriangleArrays[indexSubpart]; return mTriangleArrays[indexSubpart];
} }
// Return the number of subparts of the mesh // Return the number of subparts of the mesh
inline uint TriangleMesh::getNbSubparts() const { inline uint32_t TriangleMesh::getNbSubparts() const {
return mTriangleArrays.size(); return mTriangleArrays.size();
} }
} }
#endif

37
ephysics/collision/TriangleVertexArray.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/TriangleVertexArray.h> #include <ephysics/collision/TriangleVertexArray.h>
@ -29,7 +10,7 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
/// Note that your data will not be copied into the TriangleVertexArray and /// Note that your data will not be copied int32_to the TriangleVertexArray and
/// therefore, you need to make sure that those data are always valid during /// therefore, you need to make sure that those data are always valid during
/// the lifetime of the TriangleVertexArray. /// the lifetime of the TriangleVertexArray.
/** /**
@ -40,10 +21,10 @@ using namespace reactphysics3d;
* @param indexesStart Pointer to the first triangle index * @param indexesStart Pointer to the first triangle index
* @param indexesStride Number of bytes between the beginning of two consecutive triangle indices * @param indexesStride Number of bytes between the beginning of two consecutive triangle indices
* @param vertexDataType Type of data for the vertices (float, double) * @param vertexDataType Type of data for the vertices (float, double)
* @param indexDataType Type of data for the indices (short, int) * @param indexDataType Type of data for the indices (short, int32_t)
*/ */
TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride, TriangleVertexArray::TriangleVertexArray(uint32_t nbVertices, void* verticesStart, int32_t verticesStride,
uint nbTriangles, void* indexesStart, int indexesStride, uint32_t nbTriangles, void* indexesStart, int32_t indexesStride,
VertexDataType vertexDataType, IndexDataType indexDataType) { VertexDataType vertexDataType, IndexDataType indexDataType) {
m_numberVertices = nbVertices; m_numberVertices = nbVertices;
m_verticesStart = reinterpret_cast<unsigned char*>(verticesStart); m_verticesStart = reinterpret_cast<unsigned char*>(verticesStart);

64
ephysics/collision/TriangleVertexArray.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_TRIANGLE_VERTEX_ARRAY_H
#define REACTPHYSICS3D_TRIANGLE_VERTEX_ARRAY_H
// Libraries // Libraries
#include <ephysics/configuration.h> #include <ephysics/configuration.h>
@ -36,7 +15,7 @@ namespace reactphysics3d {
* This class is used to describe the vertices and faces of a triangular mesh. * This class is used to describe the vertices and faces of a triangular mesh.
* A TriangleVertexArray represents a continuous array of vertices and indexes * A TriangleVertexArray represents a continuous array of vertices and indexes
* of a triangular mesh. When you create a TriangleVertexArray, no data is copied * of a triangular mesh. When you create a TriangleVertexArray, no data is copied
* into the array. It only stores pointer to the data. The purpose is to allow * int32_to the array. It only stores pointer to the data. The purpose is to allow
* the user to share vertices data between the physics engine and the rendering * the user to share vertices data between the physics engine and the rendering
* part. Therefore, make sure that the data pointed by a TriangleVertexArray * part. Therefore, make sure that the data pointed by a TriangleVertexArray
* remains valid during the TriangleVertexArray life. * remains valid during the TriangleVertexArray life.
@ -54,24 +33,24 @@ class TriangleVertexArray {
protected: protected:
/// Number of vertices in the array /// Number of vertices in the array
uint m_numberVertices; uint32_t m_numberVertices;
/// Pointer to the first vertex value in the array /// Pointer to the first vertex value in the array
unsigned char* m_verticesStart; unsigned char* m_verticesStart;
/// Stride (number of bytes) between the beginning of two vertices /// Stride (number of bytes) between the beginning of two vertices
/// values in the array /// values in the array
int m_verticesStride; int32_t m_verticesStride;
/// Number of triangles in the array /// Number of triangles in the array
uint mNbTriangles; uint32_t mNbTriangles;
/// Pointer to the first vertex index of the array /// Pointer to the first vertex index of the array
unsigned char* mIndicesStart; unsigned char* mIndicesStart;
/// Stride (number of bytes) between the beginning of two indices in /// Stride (number of bytes) between the beginning of two indices in
/// the array /// the array
int mIndicesStride; int32_t mIndicesStride;
/// Data type of the vertices in the array /// Data type of the vertices in the array
VertexDataType mVertexDataType; VertexDataType mVertexDataType;
@ -82,8 +61,8 @@ class TriangleVertexArray {
public: public:
/// Constructor /// Constructor
TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride, TriangleVertexArray(uint32_t nbVertices, void* verticesStart, int32_t verticesStride,
uint nbTriangles, void* indexesStart, int indexesStride, uint32_t nbTriangles, void* indexesStart, int32_t indexesStride,
VertexDataType vertexDataType, IndexDataType indexDataType); VertexDataType vertexDataType, IndexDataType indexDataType);
/// Destructor /// Destructor
@ -96,16 +75,16 @@ class TriangleVertexArray {
IndexDataType getIndexDataType() const; IndexDataType getIndexDataType() const;
/// Return the number of vertices /// Return the number of vertices
uint getNbVertices() const; uint32_t getNbVertices() const;
/// Return the number of triangles /// Return the number of triangles
uint getNbTriangles() const; uint32_t getNbTriangles() const;
/// Return the vertices stride (number of bytes) /// Return the vertices stride (number of bytes)
int getVerticesStride() const; int32_t getVerticesStride() const;
/// Return the indices stride (number of bytes) /// Return the indices stride (number of bytes)
int getIndicesStride() const; int32_t getIndicesStride() const;
/// Return the pointer to the start of the vertices array /// Return the pointer to the start of the vertices array
unsigned char* getVerticesStart() const; unsigned char* getVerticesStart() const;
@ -125,22 +104,22 @@ inline TriangleVertexArray::IndexDataType TriangleVertexArray::getIndexDataType(
} }
// Return the number of vertices // Return the number of vertices
inline uint TriangleVertexArray::getNbVertices() const { inline uint32_t TriangleVertexArray::getNbVertices() const {
return m_numberVertices; return m_numberVertices;
} }
// Return the number of triangles // Return the number of triangles
inline uint TriangleVertexArray::getNbTriangles() const { inline uint32_t TriangleVertexArray::getNbTriangles() const {
return mNbTriangles; return mNbTriangles;
} }
// Return the vertices stride (number of bytes) // Return the vertices stride (number of bytes)
inline int TriangleVertexArray::getVerticesStride() const { inline int32_t TriangleVertexArray::getVerticesStride() const {
return m_verticesStride; return m_verticesStride;
} }
// Return the indices stride (number of bytes) // Return the indices stride (number of bytes)
inline int TriangleVertexArray::getIndicesStride() const { inline int32_t TriangleVertexArray::getIndicesStride() const {
return mIndicesStride; return mIndicesStride;
} }
@ -156,5 +135,4 @@ inline unsigned char* TriangleVertexArray::getIndicesStart() const {
} }
#endif

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

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/broadphase/BroadPhaseAlgorithm.h> #include <ephysics/collision/broadphase/BroadPhaseAlgorithm.h>
@ -38,7 +19,7 @@ BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
mCollisionDetection(collisionDetection) { mCollisionDetection(collisionDetection) {
// Allocate memory for the array of non-static proxy shapes IDs // Allocate memory for the array of non-static proxy shapes IDs
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int)); mMovedShapes = (int32_t*) malloc(mNbAllocatedMovedShapes * sizeof(int32_t));
assert(mMovedShapes != NULL); assert(mMovedShapes != NULL);
// Allocate memory for the array of potential overlapping pairs // Allocate memory for the array of potential overlapping pairs
@ -58,19 +39,19 @@ BroadPhaseAlgorithm::~BroadPhaseAlgorithm() {
// Add a collision shape in the array of shapes that have moved in the last simulation step // 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. // and that need to be tested again for broad-phase overlapping.
void BroadPhaseAlgorithm::addMovedCollisionShape(int broadPhaseID) { void BroadPhaseAlgorithm::addMovedCollisionShape(int32_t broadPhaseID) {
// Allocate more elements in the array of shapes that have moved if necessary // Allocate more elements in the array of shapes that have moved if necessary
if (mNbAllocatedMovedShapes == mNbMovedShapes) { if (mNbAllocatedMovedShapes == mNbMovedShapes) {
mNbAllocatedMovedShapes *= 2; mNbAllocatedMovedShapes *= 2;
int* oldArray = mMovedShapes; int32_t* oldArray = mMovedShapes;
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int)); mMovedShapes = (int32_t*) malloc(mNbAllocatedMovedShapes * sizeof(int32_t));
assert(mMovedShapes != NULL); assert(mMovedShapes != NULL);
memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int)); memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int32_t));
free(oldArray); free(oldArray);
} }
// Store the broad-phase ID into the array of shapes that have moved // Store the broad-phase ID int32_to the array of shapes that have moved
assert(mNbMovedShapes < mNbAllocatedMovedShapes); assert(mNbMovedShapes < mNbAllocatedMovedShapes);
assert(mMovedShapes != NULL); assert(mMovedShapes != NULL);
mMovedShapes[mNbMovedShapes] = broadPhaseID; mMovedShapes[mNbMovedShapes] = broadPhaseID;
@ -79,7 +60,7 @@ void BroadPhaseAlgorithm::addMovedCollisionShape(int broadPhaseID) {
// Remove a collision shape from the array of shapes that have moved in the last simulation step // Remove a collision shape from the array of shapes that have moved in the last simulation step
// and that need to be tested again for broad-phase overlapping. // and that need to be tested again for broad-phase overlapping.
void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) { void BroadPhaseAlgorithm::removeMovedCollisionShape(int32_t broadPhaseID) {
assert(mNbNonUsedMovedShapes <= mNbMovedShapes); assert(mNbNonUsedMovedShapes <= mNbMovedShapes);
@ -89,11 +70,11 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
mNbAllocatedMovedShapes > 8) { mNbAllocatedMovedShapes > 8) {
mNbAllocatedMovedShapes /= 2; mNbAllocatedMovedShapes /= 2;
int* oldArray = mMovedShapes; int32_t* oldArray = mMovedShapes;
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int)); mMovedShapes = (int32_t*) malloc(mNbAllocatedMovedShapes * sizeof(int32_t));
assert(mMovedShapes != NULL); assert(mMovedShapes != NULL);
uint nbElements = 0; uint32_t nbElements = 0;
for (uint i=0; i<mNbMovedShapes; i++) { for (uint32_t i=0; i<mNbMovedShapes; i++) {
if (oldArray[i] != -1) { if (oldArray[i] != -1) {
mMovedShapes[nbElements] = oldArray[i]; mMovedShapes[nbElements] = oldArray[i];
nbElements++; nbElements++;
@ -105,7 +86,7 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
} }
// Remove the broad-phase ID from the array // Remove the broad-phase ID from the array
for (uint i=0; i<mNbMovedShapes; i++) { for (uint32_t i=0; i<mNbMovedShapes; i++) {
if (mMovedShapes[i] == broadPhaseID) { if (mMovedShapes[i] == broadPhaseID) {
mMovedShapes[i] = -1; mMovedShapes[i] = -1;
mNbNonUsedMovedShapes++; mNbNonUsedMovedShapes++;
@ -114,16 +95,16 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
} }
} }
// Add a proxy collision shape into the broad-phase collision detection // Add a proxy collision shape int32_to the broad-phase collision detection
void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) { void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) {
// Add the collision shape into the dynamic AABB tree and get its broad-phase ID // Add the collision shape int32_to the dynamic AABB tree and get its broad-phase ID
int nodeId = m_dynamicAABBTree.addObject(aabb, proxyShape); int32_t nodeId = m_dynamicAABBTree.addObject(aabb, proxyShape);
// Set the broad-phase ID of the proxy shape // Set the broad-phase ID of the proxy shape
proxyShape->mBroadPhaseID = nodeId; proxyShape->mBroadPhaseID = nodeId;
// Add the collision shape into the array of bodies that have moved (or have been created) // Add the collision shape int32_to the array of bodies that have moved (or have been created)
// during the last simulation step // during the last simulation step
addMovedCollisionShape(proxyShape->mBroadPhaseID); addMovedCollisionShape(proxyShape->mBroadPhaseID);
} }
@ -131,12 +112,12 @@ void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const A
// Remove a proxy collision shape from the broad-phase collision detection // Remove a proxy collision shape from the broad-phase collision detection
void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) { void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
int broadPhaseID = proxyShape->mBroadPhaseID; int32_t broadPhaseID = proxyShape->mBroadPhaseID;
// Remove the collision shape from the dynamic AABB tree // Remove the collision shape from the dynamic AABB tree
m_dynamicAABBTree.removeObject(broadPhaseID); m_dynamicAABBTree.removeObject(broadPhaseID);
// Remove the collision shape into the array of shapes that have moved (or have been created) // Remove the collision shape int32_to the array of shapes that have moved (or have been created)
// during the last simulation step // during the last simulation step
removeMovedCollisionShape(broadPhaseID); removeMovedCollisionShape(broadPhaseID);
} }
@ -145,7 +126,7 @@ void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb, void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb,
const Vector3& displacement, bool forceReinsert) { const Vector3& displacement, bool forceReinsert) {
int broadPhaseID = proxyShape->mBroadPhaseID; int32_t broadPhaseID = proxyShape->mBroadPhaseID;
assert(broadPhaseID >= 0); assert(broadPhaseID >= 0);
@ -153,10 +134,10 @@ void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, cons
bool hasBeenReInserted = m_dynamicAABBTree.updateObject(broadPhaseID, aabb, displacement, forceReinsert); bool hasBeenReInserted = m_dynamicAABBTree.updateObject(broadPhaseID, aabb, displacement, forceReinsert);
// If the collision shape has moved out of its fat AABB (and therefore has been reinserted // If the collision shape has moved out of its fat AABB (and therefore has been reinserted
// into the tree). // int32_to the tree).
if (hasBeenReInserted) { if (hasBeenReInserted) {
// Add the collision shape into the array of shapes that have moved (or have been created) // Add the collision shape int32_to the array of shapes that have moved (or have been created)
// during the last simulation step // during the last simulation step
addMovedCollisionShape(broadPhaseID); addMovedCollisionShape(broadPhaseID);
} }
@ -170,8 +151,8 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
// For all collision shapes that have moved (or have been created) during the // For all collision shapes that have moved (or have been created) during the
// last simulation step // last simulation step
for (uint i=0; i<mNbMovedShapes; i++) { for (uint32_t i=0; i<mNbMovedShapes; i++) {
int shapeID = mMovedShapes[i]; int32_t shapeID = mMovedShapes[i];
if (shapeID == -1) continue; if (shapeID == -1) continue;
@ -195,7 +176,7 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
// Check all the potential overlapping pairs avoiding duplicates to report unique // Check all the potential overlapping pairs avoiding duplicates to report unique
// overlapping pairs // overlapping pairs
uint i=0; uint32_t i=0;
while (i < mNbPotentialPairs) { while (i < mNbPotentialPairs) {
// Get a potential overlapping pair // Get a potential overlapping pair
@ -241,7 +222,7 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
} }
// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree // Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
void BroadPhaseAlgorithm::notifyOverlappingNodes(int node1ID, int node2ID) { void BroadPhaseAlgorithm::notifyOverlappingNodes(int32_t node1ID, int32_t node2ID) {
// If both the nodes are the same, we do not create store the overlapping pair // If both the nodes are the same, we do not create store the overlapping pair
if (node1ID == node2ID) return; if (node1ID == node2ID) return;
@ -258,7 +239,7 @@ void BroadPhaseAlgorithm::notifyOverlappingNodes(int node1ID, int node2ID) {
free(oldPairs); free(oldPairs);
} }
// Add the new potential pair into the array of potential overlapping pairs // Add the new potential pair int32_to the array of potential overlapping pairs
mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(node1ID, node2ID); mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(node1ID, node2ID);
mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID); mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID);
mNbPotentialPairs++; mNbPotentialPairs++;
@ -266,15 +247,15 @@ void BroadPhaseAlgorithm::notifyOverlappingNodes(int node1ID, int node2ID) {
// Called when a overlapping node has been found during the call to // Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB() // DynamicAABBTree:reportAllShapesOverlappingWithAABB()
void AABBOverlapCallback::notifyOverlappingNode(int nodeId) { void AABBOverlapCallback::notifyOverlappingNode(int32_t nodeId) {
mBroadPhaseAlgorithm.notifyOverlappingNodes(mReferenceNodeId, nodeId); mBroadPhaseAlgorithm.notifyOverlappingNodes(mReferenceNodeId, nodeId);
} }
// Called for a broad-phase shape that has to be tested for raycast // Called for a broad-phase shape that has to be tested for raycast
decimal BroadPhaseRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) { float BroadPhaseRaycastCallback::raycastBroadPhaseShape(int32_t nodeId, const Ray& ray) {
decimal hitFraction = decimal(-1.0); float hitFraction = float(-1.0);
// Get the proxy shape from the node // Get the proxy shape from the node
ProxyShape* proxyShape = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(nodeId)); ProxyShape* proxyShape = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(nodeId));

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_BROAD_PHASE_ALGORITHM_H
#define REACTPHYSICS3D_BROAD_PHASE_ALGORITHM_H
// Libraries // Libraries
#include <vector> #include <vector>
@ -50,10 +29,10 @@ struct BroadPhasePair {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Broad-phase ID of the first collision shape /// Broad-phase ID of the first collision shape
int collisionShape1ID; int32_t collisionShape1ID;
/// Broad-phase ID of the second collision shape /// Broad-phase ID of the second collision shape
int collisionShape2ID; int32_t collisionShape2ID;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -68,19 +47,19 @@ class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
BroadPhaseAlgorithm& mBroadPhaseAlgorithm; BroadPhaseAlgorithm& mBroadPhaseAlgorithm;
int mReferenceNodeId; int32_t mReferenceNodeId;
public: public:
// Constructor // Constructor
AABBOverlapCallback(BroadPhaseAlgorithm& broadPhaseAlgo, int referenceNodeId) AABBOverlapCallback(BroadPhaseAlgorithm& broadPhaseAlgo, int32_t referenceNodeId)
: mBroadPhaseAlgorithm(broadPhaseAlgo), mReferenceNodeId(referenceNodeId) { : mBroadPhaseAlgorithm(broadPhaseAlgo), mReferenceNodeId(referenceNodeId) {
} }
// Called when a overlapping node has been found during the call to // Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB() // DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int nodeId); virtual void notifyOverlappingNode(int32_t nodeId);
}; };
@ -110,7 +89,7 @@ class BroadPhaseRaycastCallback : public DynamicAABBTreeRaycastCallback {
} }
// Called for a broad-phase shape that has to be tested for raycast // Called for a broad-phase shape that has to be tested for raycast
virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray); virtual float raycastBroadPhaseShape(int32_t nodeId, const Ray& ray);
}; };
@ -134,28 +113,28 @@ class BroadPhaseAlgorithm {
/// Array with the broad-phase IDs of all collision shapes that have moved (or have been /// Array with the broad-phase IDs of all collision shapes that have moved (or have been
/// created) during the last simulation step. Those are the shapes that need to be tested /// created) during the last simulation step. Those are the shapes that need to be tested
/// for overlapping in the next simulation step. /// for overlapping in the next simulation step.
int* mMovedShapes; int32_t* mMovedShapes;
/// Number of collision shapes in the array of shapes that have moved during the last /// Number of collision shapes in the array of shapes that have moved during the last
/// simulation step. /// simulation step.
uint mNbMovedShapes; uint32_t mNbMovedShapes;
/// Number of allocated elements for the array of shapes that have moved during the last /// Number of allocated elements for the array of shapes that have moved during the last
/// simulation step. /// simulation step.
uint mNbAllocatedMovedShapes; uint32_t mNbAllocatedMovedShapes;
/// Number of non-used elements in the array of shapes that have moved during the last /// Number of non-used elements in the array of shapes that have moved during the last
/// simulation step. /// simulation step.
uint mNbNonUsedMovedShapes; uint32_t mNbNonUsedMovedShapes;
/// Temporary array of potential overlapping pairs (with potential duplicates) /// Temporary array of potential overlapping pairs (with potential duplicates)
BroadPhasePair* mPotentialPairs; BroadPhasePair* mPotentialPairs;
/// Number of potential overlapping pairs /// Number of potential overlapping pairs
uint mNbPotentialPairs; uint32_t mNbPotentialPairs;
/// Number of allocated elements for the array of potential overlapping pairs /// Number of allocated elements for the array of potential overlapping pairs
uint mNbAllocatedPotentialPairs; uint32_t mNbAllocatedPotentialPairs;
/// Reference to the collision detection object /// Reference to the collision detection object
CollisionDetection& mCollisionDetection; CollisionDetection& mCollisionDetection;
@ -178,7 +157,7 @@ class BroadPhaseAlgorithm {
/// Destructor /// Destructor
virtual ~BroadPhaseAlgorithm(); virtual ~BroadPhaseAlgorithm();
/// Add a proxy collision shape into the broad-phase collision detection /// Add a proxy collision shape int32_to the broad-phase collision detection
void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb); void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb);
/// Remove a proxy collision shape from the broad-phase collision detection /// Remove a proxy collision shape from the broad-phase collision detection
@ -190,14 +169,14 @@ class BroadPhaseAlgorithm {
/// Add a collision shape in the array of shapes that have moved in the last simulation step /// 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. /// and that need to be tested again for broad-phase overlapping.
void addMovedCollisionShape(int broadPhaseID); void addMovedCollisionShape(int32_t broadPhaseID);
/// Remove a collision shape from the array of shapes that have moved in the last simulation /// Remove a collision shape from the array of shapes that have moved in the last simulation
/// step and that need to be tested again for broad-phase overlapping. /// step and that need to be tested again for broad-phase overlapping.
void removeMovedCollisionShape(int broadPhaseID); void removeMovedCollisionShape(int32_t broadPhaseID);
/// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree /// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
void notifyOverlappingNodes(int broadPhaseId1, int broadPhaseId2); void notifyOverlappingNodes(int32_t broadPhaseId1, int32_t broadPhaseId2);
/// Compute all the overlapping pairs of collision shapes /// Compute all the overlapping pairs of collision shapes
void computeOverlappingPairs(); void computeOverlappingPairs();
@ -244,5 +223,5 @@ inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTes
} }
#endif

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

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/broadphase/DynamicAABBTree.h> #include <ephysics/collision/broadphase/DynamicAABBTree.h>
@ -32,10 +13,10 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Initialization of static variables // Initialization of static variables
const int TreeNode::NULL_TREE_NODE = -1; const int32_t TreeNode::NULL_TREE_NODE = -1;
// Constructor // Constructor
DynamicAABBTree::DynamicAABBTree(decimal extraAABBGap) : mExtraAABBGap(extraAABBGap) { DynamicAABBTree::DynamicAABBTree(float extraAABBGap) : mExtraAABBGap(extraAABBGap) {
init(); init();
} }
@ -60,7 +41,7 @@ void DynamicAABBTree::init() {
memset(mNodes, 0, mNbAllocatedNodes * sizeof(TreeNode)); memset(mNodes, 0, mNbAllocatedNodes * sizeof(TreeNode));
// Initialize the allocated nodes // Initialize the allocated nodes
for (int i=0; i<mNbAllocatedNodes - 1; i++) { for (int32_t i=0; i<mNbAllocatedNodes - 1; i++) {
mNodes[i].nextNodeID = i + 1; mNodes[i].nextNodeID = i + 1;
mNodes[i].height = -1; mNodes[i].height = -1;
} }
@ -80,7 +61,7 @@ void DynamicAABBTree::reset() {
} }
// Allocate and return a new node in the tree // Allocate and return a new node in the tree
int DynamicAABBTree::allocateNode() { int32_t DynamicAABBTree::allocateNode() {
// If there is no more allocated node to use // If there is no more allocated node to use
if (mFreeNodeID == TreeNode::NULL_TREE_NODE) { if (mFreeNodeID == TreeNode::NULL_TREE_NODE) {
@ -96,7 +77,7 @@ int DynamicAABBTree::allocateNode() {
free(oldNodes); free(oldNodes);
// Initialize the allocated nodes // Initialize the allocated nodes
for (int i=mNbNodes; i<mNbAllocatedNodes - 1; i++) { for (int32_t i=mNbNodes; i<mNbAllocatedNodes - 1; i++) {
mNodes[i].nextNodeID = i + 1; mNodes[i].nextNodeID = i + 1;
mNodes[i].height = -1; mNodes[i].height = -1;
} }
@ -106,7 +87,7 @@ int DynamicAABBTree::allocateNode() {
} }
// Get the next free node // Get the next free node
int freeNodeID = mFreeNodeID; int32_t freeNodeID = mFreeNodeID;
mFreeNodeID = mNodes[freeNodeID].nextNodeID; mFreeNodeID = mNodes[freeNodeID].nextNodeID;
mNodes[freeNodeID].parentID = TreeNode::NULL_TREE_NODE; mNodes[freeNodeID].parentID = TreeNode::NULL_TREE_NODE;
mNodes[freeNodeID].height = 0; mNodes[freeNodeID].height = 0;
@ -116,7 +97,7 @@ int DynamicAABBTree::allocateNode() {
} }
// Release a node // Release a node
void DynamicAABBTree::releaseNode(int nodeID) { void DynamicAABBTree::releaseNode(int32_t nodeID) {
assert(mNbNodes > 0); assert(mNbNodes > 0);
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
@ -127,11 +108,11 @@ void DynamicAABBTree::releaseNode(int nodeID) {
mNbNodes--; mNbNodes--;
} }
// Internally add an object into the tree // Internally add an object int32_to the tree
int DynamicAABBTree::addObjectInternal(const AABB& aabb) { int32_t DynamicAABBTree::addObjectInternal(const AABB& aabb) {
// Get the next available node (or allocate new ones if necessary) // Get the next available node (or allocate new ones if necessary)
int nodeID = allocateNode(); int32_t nodeID = allocateNode();
// Create the fat aabb to use in the tree // Create the fat aabb to use in the tree
const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap); const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap);
@ -152,7 +133,7 @@ int DynamicAABBTree::addObjectInternal(const AABB& aabb) {
} }
// Remove an object from the tree // Remove an object from the tree
void DynamicAABBTree::removeObject(int nodeID) { void DynamicAABBTree::removeObject(int32_t nodeID) {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf()); assert(mNodes[nodeID].isLeaf());
@ -164,12 +145,12 @@ void DynamicAABBTree::removeObject(int nodeID) {
// Update the dynamic tree after an object has moved. // Update the dynamic tree after an object has moved.
/// If the new AABB of the object that has moved is still inside its fat AABB, then /// If the new AABB of the object that has moved is still inside its fat AABB, then
/// nothing is done. Otherwise, the corresponding node is removed and reinserted into the tree. /// nothing is done. Otherwise, the corresponding node is removed and reinserted int32_to the tree.
/// The method returns true if the object has been reinserted into the tree. The "displacement" /// The method returns true if the object has been reinserted int32_to the tree. The "displacement"
/// argument is the linear velocity of the AABB multiplied by the elapsed time between two /// 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 /// 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). /// (this can be useful if the shape AABB has become much smaller than the previous one for instance).
bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert) { bool DynamicAABBTree::updateObject(int32_t nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert) {
PROFILE("DynamicAABBTree::updateObject()"); PROFILE("DynamicAABBTree::updateObject()");
@ -192,19 +173,19 @@ bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector
mNodes[nodeID].aabb.mMaxCoordinates += gap; mNodes[nodeID].aabb.mMaxCoordinates += gap;
// Inflate the fat AABB in direction of the linear motion of the AABB // Inflate the fat AABB in direction of the linear motion of the AABB
if (displacement.x < decimal(0.0)) { if (displacement.x < float(0.0)) {
mNodes[nodeID].aabb.mMinCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x; mNodes[nodeID].aabb.mMinCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x;
} }
else { else {
mNodes[nodeID].aabb.mMaxCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x; mNodes[nodeID].aabb.mMaxCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x;
} }
if (displacement.y < decimal(0.0)) { if (displacement.y < float(0.0)) {
mNodes[nodeID].aabb.mMinCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y; mNodes[nodeID].aabb.mMinCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y;
} }
else { else {
mNodes[nodeID].aabb.mMaxCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y; mNodes[nodeID].aabb.mMaxCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y;
} }
if (displacement.z < decimal(0.0)) { if (displacement.z < float(0.0)) {
mNodes[nodeID].aabb.mMinCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z; mNodes[nodeID].aabb.mMinCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z;
} }
else { else {
@ -213,7 +194,7 @@ bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector
assert(mNodes[nodeID].aabb.contains(newAABB)); assert(mNodes[nodeID].aabb.contains(newAABB));
// Reinsert the node into the tree // Reinsert the node int32_to the tree
insertLeafNode(nodeID); insertLeafNode(nodeID);
return true; return true;
@ -222,7 +203,7 @@ bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector
// Insert a leaf node in the tree. The process of inserting a new leaf node // Insert a leaf node in the tree. The process of inserting a new leaf node
// in the dynamic tree is described in the book "Introduction to Game Physics // in the dynamic tree is described in the book "Introduction to Game Physics
// with Box2D" by Ian Parberry. // with Box2D" by Ian Parberry.
void DynamicAABBTree::insertLeafNode(int nodeID) { void DynamicAABBTree::insertLeafNode(int32_t nodeID) {
// If the tree is empty // If the tree is empty
if (mRootNodeID == TreeNode::NULL_TREE_NODE) { if (mRootNodeID == TreeNode::NULL_TREE_NODE) {
@ -235,53 +216,53 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
// Find the best sibling node for the new node // Find the best sibling node for the new node
AABB newNodeAABB = mNodes[nodeID].aabb; AABB newNodeAABB = mNodes[nodeID].aabb;
int currentNodeID = mRootNodeID; int32_t currentNodeID = mRootNodeID;
while (!mNodes[currentNodeID].isLeaf()) { while (!mNodes[currentNodeID].isLeaf()) {
int leftChild = mNodes[currentNodeID].children[0]; int32_t leftChild = mNodes[currentNodeID].children[0];
int rightChild = mNodes[currentNodeID].children[1]; int32_t rightChild = mNodes[currentNodeID].children[1];
// Compute the merged AABB // Compute the merged AABB
decimal volumeAABB = mNodes[currentNodeID].aabb.getVolume(); float volumeAABB = mNodes[currentNodeID].aabb.getVolume();
AABB mergedAABBs; AABB mergedAABBs;
mergedAABBs.mergeTwoAABBs(mNodes[currentNodeID].aabb, newNodeAABB); mergedAABBs.mergeTwoAABBs(mNodes[currentNodeID].aabb, newNodeAABB);
decimal mergedVolume = mergedAABBs.getVolume(); float mergedVolume = mergedAABBs.getVolume();
// Compute the cost of making the current node the sibbling of the new node // Compute the cost of making the current node the sibbling of the new node
decimal costS = decimal(2.0) * mergedVolume; float costS = float(2.0) * mergedVolume;
// Compute the minimum cost of pushing the new node further down the tree (inheritance cost) // Compute the minimum cost of pushing the new node further down the tree (inheritance cost)
decimal costI = decimal(2.0) * (mergedVolume - volumeAABB); float costI = float(2.0) * (mergedVolume - volumeAABB);
// Compute the cost of descending into the left child // Compute the cost of descending int32_to the left child
decimal costLeft; float costLeft;
AABB currentAndLeftAABB; AABB currentAndLeftAABB;
currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, mNodes[leftChild].aabb); currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, mNodes[leftChild].aabb);
if (mNodes[leftChild].isLeaf()) { // If the left child is a leaf if (mNodes[leftChild].isLeaf()) { // If the left child is a leaf
costLeft = currentAndLeftAABB.getVolume() + costI; costLeft = currentAndLeftAABB.getVolume() + costI;
} }
else { else {
decimal leftChildVolume = mNodes[leftChild].aabb.getVolume(); float leftChildVolume = mNodes[leftChild].aabb.getVolume();
costLeft = costI + currentAndLeftAABB.getVolume() - leftChildVolume; costLeft = costI + currentAndLeftAABB.getVolume() - leftChildVolume;
} }
// Compute the cost of descending into the right child // Compute the cost of descending int32_to the right child
decimal costRight; float costRight;
AABB currentAndRightAABB; AABB currentAndRightAABB;
currentAndRightAABB.mergeTwoAABBs(newNodeAABB, mNodes[rightChild].aabb); currentAndRightAABB.mergeTwoAABBs(newNodeAABB, mNodes[rightChild].aabb);
if (mNodes[rightChild].isLeaf()) { // If the right child is a leaf if (mNodes[rightChild].isLeaf()) { // If the right child is a leaf
costRight = currentAndRightAABB.getVolume() + costI; costRight = currentAndRightAABB.getVolume() + costI;
} }
else { else {
decimal rightChildVolume = mNodes[rightChild].aabb.getVolume(); float rightChildVolume = mNodes[rightChild].aabb.getVolume();
costRight = costI + currentAndRightAABB.getVolume() - rightChildVolume; costRight = costI + currentAndRightAABB.getVolume() - rightChildVolume;
} }
// If the cost of making the current node a sibbling of the new node is smaller than // If the cost of making the current node a sibbling of the new node is smaller than
// the cost of going down into the left or right child // the cost of going down int32_to the left or right child
if (costS < costLeft && costS < costRight) break; if (costS < costLeft && costS < costRight) break;
// It is cheaper to go down into a child of the current node, choose the best child // It is cheaper to go down int32_to a child of the current node, choose the best child
if (costLeft < costRight) { if (costLeft < costRight) {
currentNodeID = leftChild; currentNodeID = leftChild;
} }
@ -290,11 +271,11 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
} }
} }
int siblingNode = currentNodeID; int32_t siblingNode = currentNodeID;
// Create a new parent for the new node and the sibling node // Create a new parent for the new node and the sibling node
int oldParentNode = mNodes[siblingNode].parentID; int32_t oldParentNode = mNodes[siblingNode].parentID;
int newParentNode = allocateNode(); int32_t newParentNode = allocateNode();
mNodes[newParentNode].parentID = oldParentNode; mNodes[newParentNode].parentID = oldParentNode;
mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB); mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB);
mNodes[newParentNode].height = mNodes[siblingNode].height + 1; mNodes[newParentNode].height = mNodes[siblingNode].height + 1;
@ -332,8 +313,8 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
assert(mNodes[nodeID].isLeaf()); assert(mNodes[nodeID].isLeaf());
assert(!mNodes[currentNodeID].isLeaf()); assert(!mNodes[currentNodeID].isLeaf());
int leftChild = mNodes[currentNodeID].children[0]; int32_t leftChild = mNodes[currentNodeID].children[0];
int rightChild = mNodes[currentNodeID].children[1]; int32_t rightChild = mNodes[currentNodeID].children[1];
assert(leftChild != TreeNode::NULL_TREE_NODE); assert(leftChild != TreeNode::NULL_TREE_NODE);
assert(rightChild != TreeNode::NULL_TREE_NODE); assert(rightChild != TreeNode::NULL_TREE_NODE);
@ -352,7 +333,7 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
} }
// Remove a leaf node from the tree // Remove a leaf node from the tree
void DynamicAABBTree::removeLeafNode(int nodeID) { void DynamicAABBTree::removeLeafNode(int32_t nodeID) {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf()); assert(mNodes[nodeID].isLeaf());
@ -363,9 +344,9 @@ void DynamicAABBTree::removeLeafNode(int nodeID) {
return; return;
} }
int parentNodeID = mNodes[nodeID].parentID; int32_t parentNodeID = mNodes[nodeID].parentID;
int grandParentNodeID = mNodes[parentNodeID].parentID; int32_t grandParentNodeID = mNodes[parentNodeID].parentID;
int siblingNodeID; int32_t siblingNodeID;
if (mNodes[parentNodeID].children[0] == nodeID) { if (mNodes[parentNodeID].children[0] == nodeID) {
siblingNodeID = mNodes[parentNodeID].children[1]; siblingNodeID = mNodes[parentNodeID].children[1];
} }
@ -389,7 +370,7 @@ void DynamicAABBTree::removeLeafNode(int nodeID) {
// Now, we need to recompute the AABBs of the node on the path back to the root // Now, we need to recompute the AABBs of the node on the path back to the root
// and make sure that the tree is still balanced // and make sure that the tree is still balanced
int currentNodeID = grandParentNodeID; int32_t currentNodeID = grandParentNodeID;
while(currentNodeID != TreeNode::NULL_TREE_NODE) { while(currentNodeID != TreeNode::NULL_TREE_NODE) {
// Balance the current sub-tree if necessary // Balance the current sub-tree if necessary
@ -398,8 +379,8 @@ void DynamicAABBTree::removeLeafNode(int nodeID) {
assert(!mNodes[currentNodeID].isLeaf()); assert(!mNodes[currentNodeID].isLeaf());
// Get the two children of the current node // Get the two children of the current node
int leftChildID = mNodes[currentNodeID].children[0]; int32_t leftChildID = mNodes[currentNodeID].children[0];
int rightChildID = mNodes[currentNodeID].children[1]; int32_t rightChildID = mNodes[currentNodeID].children[1];
// Recompute the AABB and the height of the current node // Recompute the AABB and the height of the current node
mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChildID].aabb, mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChildID].aabb,
@ -423,7 +404,7 @@ void DynamicAABBTree::removeLeafNode(int nodeID) {
// Balance the sub-tree of a given node using left or right rotations. // Balance the sub-tree of a given node using left or right rotations.
/// The rotation schemes are described in the book "Introduction to Game Physics /// The rotation schemes are described in the book "Introduction to Game Physics
/// with Box2D" by Ian Parberry. This method returns the new root node ID. /// with Box2D" by Ian Parberry. This method returns the new root node ID.
int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) { int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) {
assert(nodeID != TreeNode::NULL_TREE_NODE); assert(nodeID != TreeNode::NULL_TREE_NODE);
@ -437,23 +418,23 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
} }
// Get the two children nodes // Get the two children nodes
int nodeBID = nodeA->children[0]; int32_t nodeBID = nodeA->children[0];
int nodeCID = nodeA->children[1]; int32_t nodeCID = nodeA->children[1];
assert(nodeBID >= 0 && nodeBID < mNbAllocatedNodes); assert(nodeBID >= 0 && nodeBID < mNbAllocatedNodes);
assert(nodeCID >= 0 && nodeCID < mNbAllocatedNodes); assert(nodeCID >= 0 && nodeCID < mNbAllocatedNodes);
TreeNode* nodeB = mNodes + nodeBID; TreeNode* nodeB = mNodes + nodeBID;
TreeNode* nodeC = mNodes + nodeCID; TreeNode* nodeC = mNodes + nodeCID;
// Compute the factor of the left and right sub-trees // Compute the factor of the left and right sub-trees
int balanceFactor = nodeC->height - nodeB->height; int32_t balanceFactor = nodeC->height - nodeB->height;
// If the right node C is 2 higher than left node B // If the right node C is 2 higher than left node B
if (balanceFactor > 1) { if (balanceFactor > 1) {
assert(!nodeC->isLeaf()); assert(!nodeC->isLeaf());
int nodeFID = nodeC->children[0]; int32_t nodeFID = nodeC->children[0];
int nodeGID = nodeC->children[1]; int32_t nodeGID = nodeC->children[1];
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes); assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes); assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
TreeNode* nodeF = mNodes + nodeFID; TreeNode* nodeF = mNodes + nodeFID;
@ -522,8 +503,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
assert(!nodeB->isLeaf()); assert(!nodeB->isLeaf());
int nodeFID = nodeB->children[0]; int32_t nodeFID = nodeB->children[0];
int nodeGID = nodeB->children[1]; int32_t nodeGID = nodeB->children[1];
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes); assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes); assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
TreeNode* nodeF = mNodes + nodeFID; TreeNode* nodeF = mNodes + nodeFID;
@ -596,14 +577,14 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb,
DynamicAABBTreeOverlapCallback& callback) const { DynamicAABBTreeOverlapCallback& callback) const {
// Create a stack with the nodes to visit // Create a stack with the nodes to visit
Stack<int, 64> stack; Stack<int32_t, 64> stack;
stack.push(mRootNodeID); stack.push(mRootNodeID);
// While there are still nodes to visit // While there are still nodes to visit
while(stack.getNbElements() > 0) { while(stack.getNbElements() > 0) {
// Get the next node ID to visit // Get the next node ID to visit
int nodeIDToVisit = stack.pop(); int32_t nodeIDToVisit = stack.pop();
// Skip it if it is a null node // Skip it if it is a null node
if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) continue; if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) continue;
@ -635,9 +616,9 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &ca
PROFILE("DynamicAABBTree::raycast()"); PROFILE("DynamicAABBTree::raycast()");
decimal maxFraction = ray.maxFraction; float maxFraction = ray.maxFraction;
Stack<int, 128> stack; Stack<int32_t, 128> stack;
stack.push(mRootNodeID); stack.push(mRootNodeID);
// Walk through the tree from the root looking for proxy shapes // Walk through the tree from the root looking for proxy shapes
@ -645,7 +626,7 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &ca
while (stack.getNbElements() > 0) { while (stack.getNbElements() > 0) {
// Get the next node in the stack // Get the next node in the stack
int nodeID = stack.pop(); int32_t nodeID = stack.pop();
// If it is a null node, skip it // If it is a null node, skip it
if (nodeID == TreeNode::NULL_TREE_NODE) continue; if (nodeID == TreeNode::NULL_TREE_NODE) continue;
@ -655,23 +636,23 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &ca
Ray rayTemp(ray.point1, ray.point2, maxFraction); Ray rayTemp(ray.point1, ray.point2, maxFraction);
// Test if the ray intersects with the current node AABB // Test if the ray int32_tersects with the current node AABB
if (!node->aabb.testRayIntersect(rayTemp)) continue; if (!node->aabb.testRayIntersect(rayTemp)) continue;
// If the node is a leaf of the tree // If the node is a leaf of the tree
if (node->isLeaf()) { if (node->isLeaf()) {
// Call the callback that will raycast again the broad-phase shape // Call the callback that will raycast again the broad-phase shape
decimal hitFraction = callback.raycastBroadPhaseShape(nodeID, rayTemp); float hitFraction = callback.raycastBroadPhaseShape(nodeID, rayTemp);
// If the user returned a hitFraction of zero, it means that // If the user returned a hitFraction of zero, it means that
// the raycasting should stop here // the raycasting should stop here
if (hitFraction == decimal(0.0)) { if (hitFraction == float(0.0)) {
return; return;
} }
// If the user returned a positive fraction // If the user returned a positive fraction
if (hitFraction > decimal(0.0)) { if (hitFraction > float(0.0)) {
// We update the maxFraction value and the ray // We update the maxFraction value and the ray
// AABB using the new maximum fraction // AABB using the new maximum fraction
@ -700,8 +681,8 @@ void DynamicAABBTree::check() const {
// Recursively check each node // Recursively check each node
checkNode(mRootNodeID); checkNode(mRootNodeID);
int nbFreeNodes = 0; int32_t nbFreeNodes = 0;
int freeNodeID = mFreeNodeID; int32_t freeNodeID = mFreeNodeID;
// Check the free nodes // Check the free nodes
while(freeNodeID != TreeNode::NULL_TREE_NODE) { while(freeNodeID != TreeNode::NULL_TREE_NODE) {
@ -714,7 +695,7 @@ void DynamicAABBTree::check() const {
} }
// Check if the node structure is valid (for debugging purpose) // Check if the node structure is valid (for debugging purpose)
void DynamicAABBTree::checkNode(int nodeID) const { void DynamicAABBTree::checkNode(int32_t nodeID) const {
if (nodeID == TreeNode::NULL_TREE_NODE) return; if (nodeID == TreeNode::NULL_TREE_NODE) return;
@ -726,8 +707,8 @@ void DynamicAABBTree::checkNode(int nodeID) const {
// Get the children nodes // Get the children nodes
TreeNode* pNode = mNodes + nodeID; TreeNode* pNode = mNodes + nodeID;
assert(!pNode->isLeaf()); assert(!pNode->isLeaf());
int leftChild = pNode->children[0]; int32_t leftChild = pNode->children[0];
int rightChild = pNode->children[1]; int32_t rightChild = pNode->children[1];
assert(pNode->height >= 0); assert(pNode->height >= 0);
assert(pNode->aabb.getVolume() > 0); assert(pNode->aabb.getVolume() > 0);
@ -751,7 +732,7 @@ void DynamicAABBTree::checkNode(int nodeID) const {
assert(mNodes[rightChild].parentID == nodeID); assert(mNodes[rightChild].parentID == nodeID);
// Check the height of node // Check the height of node
int height = 1 + std::max(mNodes[leftChild].height, mNodes[rightChild].height); int32_t height = 1 + std::max(mNodes[leftChild].height, mNodes[rightChild].height);
assert(mNodes[nodeID].height == height); assert(mNodes[nodeID].height == height);
// Check the AABB of the node // Check the AABB of the node
@ -767,12 +748,12 @@ void DynamicAABBTree::checkNode(int nodeID) const {
} }
// Compute the height of the tree // Compute the height of the tree
int DynamicAABBTree::computeHeight() { int32_t DynamicAABBTree::computeHeight() {
return computeHeight(mRootNodeID); return computeHeight(mRootNodeID);
} }
// Compute the height of a given node in the tree // Compute the height of a given node in the tree
int DynamicAABBTree::computeHeight(int nodeID) { int32_t DynamicAABBTree::computeHeight(int32_t nodeID) {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
TreeNode* node = mNodes + nodeID; TreeNode* node = mNodes + nodeID;
@ -782,8 +763,8 @@ int DynamicAABBTree::computeHeight(int nodeID) {
} }
// Compute the height of the left and right sub-tree // Compute the height of the left and right sub-tree
int leftHeight = computeHeight(node->children[0]); int32_t leftHeight = computeHeight(node->children[0]);
int rightHeight = computeHeight(node->children[1]); int32_t rightHeight = computeHeight(node->children[1]);
// Return the height of the node // Return the height of the node
return 1 + std::max(leftHeight, rightHeight); return 1 + std::max(leftHeight, rightHeight);

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_DYNAMIC_AABB_TREE_H
#define REACTPHYSICS3D_DYNAMIC_AABB_TREE_H
// Libraries // Libraries
#include <ephysics/configuration.h> #include <ephysics/configuration.h>
@ -49,7 +28,7 @@ struct TreeNode {
// -------------------- Constants -------------------- // // -------------------- Constants -------------------- //
/// Null tree node constant /// Null tree node constant
const static int NULL_TREE_NODE; const static int32_t NULL_TREE_NODE;
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
@ -58,27 +37,27 @@ struct TreeNode {
union { union {
/// Parent node ID /// Parent node ID
int32 parentID; int32_t parentID;
/// Next allocated node ID /// Next allocated node ID
int32 nextNodeID; int32_t nextNodeID;
}; };
// A node is either a leaf (has data) or is an internal node (has children) // A node is either a leaf (has data) or is an int32_ternal node (has children)
union { union {
/// Left and right child of the node (children[0] = left child) /// Left and right child of the node (children[0] = left child)
int32 children[2]; int32_t children[2];
/// Two pieces of data stored at that node (in case the node is a leaf) /// Two pieces of data stored at that node (in case the node is a leaf)
union { union {
int32 dataInt[2]; int32_t dataInt[2];
void* dataPointer; void* dataPointer;
}; };
}; };
/// Height of the node in the tree /// Height of the node in the tree
int16 height; int16_t height;
/// Fat axis aligned bounding box (AABB) corresponding to the node /// Fat axis aligned bounding box (AABB) corresponding to the node
AABB aabb; AABB aabb;
@ -101,7 +80,7 @@ class DynamicAABBTreeOverlapCallback {
// Called when a overlapping node has been found during the call to // Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB() // DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int nodeId)=0; virtual void notifyOverlappingNode(int32_t nodeId)=0;
}; };
// Class DynamicAABBTreeRaycastCallback // Class DynamicAABBTreeRaycastCallback
@ -115,7 +94,7 @@ class DynamicAABBTreeRaycastCallback {
virtual ~DynamicAABBTreeRaycastCallback() = default; virtual ~DynamicAABBTreeRaycastCallback() = default;
// Called when the AABB of a leaf node is hit by a ray // Called when the AABB of a leaf node is hit by a ray
virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray)=0; virtual float raycastBroadPhaseShape(int32_t nodeId, const Ray& ray)=0;
}; };
@ -137,43 +116,43 @@ class DynamicAABBTree {
TreeNode* mNodes; TreeNode* mNodes;
/// ID of the root node of the tree /// ID of the root node of the tree
int mRootNodeID; int32_t mRootNodeID;
/// ID of the first node of the list of free (allocated) nodes in the tree that we can use /// ID of the first node of the list of free (allocated) nodes in the tree that we can use
int mFreeNodeID; int32_t mFreeNodeID;
/// Number of allocated nodes in the tree /// Number of allocated nodes in the tree
int mNbAllocatedNodes; int32_t mNbAllocatedNodes;
/// Number of nodes in the tree /// Number of nodes in the tree
int mNbNodes; int32_t mNbNodes;
/// Extra AABB Gap used to allow the collision shape to move a little bit /// 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 /// without triggering a large modification of the tree which can be costly
decimal mExtraAABBGap; float mExtraAABBGap;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Allocate and return a node to use in the tree /// Allocate and return a node to use in the tree
int allocateNode(); int32_t allocateNode();
/// Release a node /// Release a node
void releaseNode(int nodeID); void releaseNode(int32_t nodeID);
/// Insert a leaf node in the tree /// Insert a leaf node in the tree
void insertLeafNode(int nodeID); void insertLeafNode(int32_t nodeID);
/// Remove a leaf node from the tree /// Remove a leaf node from the tree
void removeLeafNode(int nodeID); void removeLeafNode(int32_t nodeID);
/// Balance the sub-tree of a given node using left or right rotations. /// Balance the sub-tree of a given node using left or right rotations.
int balanceSubTreeAtNode(int nodeID); int32_t balanceSubTreeAtNode(int32_t nodeID);
/// Compute the height of a given node in the tree /// Compute the height of a given node in the tree
int computeHeight(int nodeID); int32_t computeHeight(int32_t nodeID);
/// Internally add an object into the tree /// Internally add an object int32_to the tree
int addObjectInternal(const AABB& aabb); int32_t addObjectInternal(const AABB& aabb);
/// Initialize the tree /// Initialize the tree
void init(); void init();
@ -184,7 +163,7 @@ class DynamicAABBTree {
void check() const; void check() const;
/// Check if the node structure is valid (for debugging purpose) /// Check if the node structure is valid (for debugging purpose)
void checkNode(int nodeID) const; void checkNode(int32_t nodeID) const;
#endif #endif
@ -193,31 +172,31 @@ class DynamicAABBTree {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
DynamicAABBTree(decimal extraAABBGap = decimal(0.0)); DynamicAABBTree(float extraAABBGap = float(0.0));
/// Destructor /// Destructor
virtual ~DynamicAABBTree(); virtual ~DynamicAABBTree();
/// Add an object into the tree (where node data are two integers) /// Add an object int32_to the tree (where node data are two int32_tegers)
int addObject(const AABB& aabb, int32 data1, int32 data2); int32_t addObject(const AABB& aabb, int32_t data1, int32_t data2);
/// Add an object into the tree (where node data is a pointer) /// Add an object int32_to the tree (where node data is a pointer)
int addObject(const AABB& aabb, void* data); int32_t addObject(const AABB& aabb, void* data);
/// Remove an object from the tree /// Remove an object from the tree
void removeObject(int nodeID); void removeObject(int32_t nodeID);
/// Update the dynamic tree after an object has moved. /// Update the dynamic tree after an object has moved.
bool updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert = false); bool updateObject(int32_t nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert = false);
/// Return the fat AABB corresponding to a given node ID /// Return the fat AABB corresponding to a given node ID
const AABB& getFatAABB(int nodeID) const; const AABB& getFatAABB(int32_t nodeID) const;
/// Return the pointer to the data array of a given leaf node of the tree /// Return the pointer to the data array of a given leaf node of the tree
int32* getNodeDataInt(int nodeID) const; int32_t* getNodeDataInt(int32_t nodeID) const;
/// Return the data pointer of a given leaf node of the tree /// Return the data pointer of a given leaf node of the tree
void* getNodeDataPointer(int nodeID) const; void* getNodeDataPointer(int32_t nodeID) const;
/// Report all shapes overlapping with the AABB given in parameter. /// Report all shapes overlapping with the AABB given in parameter.
void reportAllShapesOverlappingWithAABB(const AABB& aabb, void reportAllShapesOverlappingWithAABB(const AABB& aabb,
@ -227,7 +206,7 @@ class DynamicAABBTree {
void raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& callback) const; void raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& callback) const;
/// Compute the height of the tree /// Compute the height of the tree
int computeHeight(); int32_t computeHeight();
/// Return the root AABB of the tree /// Return the root AABB of the tree
AABB getRootAABB() const; AABB getRootAABB() const;
@ -242,20 +221,20 @@ inline bool TreeNode::isLeaf() const {
} }
// Return the fat AABB corresponding to a given node ID // Return the fat AABB corresponding to a given node ID
inline const AABB& DynamicAABBTree::getFatAABB(int nodeID) const { inline const AABB& DynamicAABBTree::getFatAABB(int32_t nodeID) const {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
return mNodes[nodeID].aabb; return mNodes[nodeID].aabb;
} }
// Return the pointer to the data array of a given leaf node of the tree // Return the pointer to the data array of a given leaf node of the tree
inline int32* DynamicAABBTree::getNodeDataInt(int nodeID) const { inline int32_t* DynamicAABBTree::getNodeDataInt(int32_t nodeID) const {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf()); assert(mNodes[nodeID].isLeaf());
return mNodes[nodeID].dataInt; return mNodes[nodeID].dataInt;
} }
// Return the pointer to the data pointer of a given leaf node of the tree // Return the pointer to the data pointer of a given leaf node of the tree
inline void* DynamicAABBTree::getNodeDataPointer(int nodeID) const { inline void* DynamicAABBTree::getNodeDataPointer(int32_t nodeID) const {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf()); assert(mNodes[nodeID].isLeaf());
return mNodes[nodeID].dataPointer; return mNodes[nodeID].dataPointer;
@ -266,11 +245,11 @@ inline AABB DynamicAABBTree::getRootAABB() const {
return getFatAABB(mRootNodeID); return getFatAABB(mRootNodeID);
} }
// Add an object into the tree. This method creates a new leaf node in the tree and // Add an object int32_to the tree. This method creates a new leaf node in the tree and
// returns the ID of the corresponding node. // returns the ID of the corresponding node.
inline int DynamicAABBTree::addObject(const AABB& aabb, int32 data1, int32 data2) { inline int32_t DynamicAABBTree::addObject(const AABB& aabb, int32_t data1, int32_t data2) {
int nodeId = addObjectInternal(aabb); int32_t nodeId = addObjectInternal(aabb);
mNodes[nodeId].dataInt[0] = data1; mNodes[nodeId].dataInt[0] = data1;
mNodes[nodeId].dataInt[1] = data2; mNodes[nodeId].dataInt[1] = data2;
@ -278,11 +257,11 @@ inline int DynamicAABBTree::addObject(const AABB& aabb, int32 data1, int32 data2
return nodeId; return nodeId;
} }
// Add an object into the tree. This method creates a new leaf node in the tree and // Add an object int32_to the tree. This method creates a new leaf node in the tree and
// returns the ID of the corresponding node. // returns the ID of the corresponding node.
inline int DynamicAABBTree::addObject(const AABB& aabb, void* data) { inline int32_t DynamicAABBTree::addObject(const AABB& aabb, void* data) {
int nodeId = addObjectInternal(aabb); int32_t nodeId = addObjectInternal(aabb);
mNodes[nodeId].dataPointer = data; mNodes[nodeId].dataPointer = data;
@ -290,5 +269,3 @@ inline int DynamicAABBTree::addObject(const AABB& aabb, void* data) {
} }
} }
#endif

39
ephysics/collision/narrowphase/CollisionDispatch.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_COLLISION_DISPATCH_H
#define REACTPHYSICS3D_COLLISION_DISPATCH_H
// Libraries // Libraries
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h> #include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h>
@ -52,16 +31,14 @@ class CollisionDispatch {
/// Initialize the collision dispatch configuration /// Initialize the collision dispatch configuration
virtual void init(CollisionDetection* collisionDetection, virtual void init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator) { MemoryAllocator* memoryAllocator) {
} }
/// Select and return the narrow-phase collision detection algorithm to /// Select and return the narrow-phase collision detection algorithm to
/// use between two types of collision shapes. /// use between two types of collision shapes.
virtual NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type, virtual NarrowPhaseAlgorithm* selectAlgorithm(int32_t shape1Type,
int shape2Type)=0; int32_t shape2Type)=0;
}; };
} }
#endif

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

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/shapes/ConcaveShape.h> #include <ephysics/collision/shapes/ConcaveShape.h>
@ -107,7 +88,7 @@ void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Inf
void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) { void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) {
// Create a triangle collision shape // Create a triangle collision shape
decimal margin = mConcaveShape->getTriangleMargin(); float margin = mConcaveShape->getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
// Select the collision algorithm to use between the triangle and the convex shape // Select the collision algorithm to use between the triangle and the convex shape
@ -133,13 +114,13 @@ void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) {
// Process the concave triangle mesh collision using the smooth mesh collision algorithm described // Process the concave triangle mesh collision using the smooth mesh collision algorithm described
// by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision // by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision
// issue with some internal edges. // issue with some int32_ternal edges.
void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair, void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair,
std::vector<SmoothMeshContactInfo> contactPoints, std::vector<SmoothMeshContactInfo> contactPoints,
NarrowPhaseCallback* narrowPhaseCallback) { NarrowPhaseCallback* narrowPhaseCallback) {
// Set with the triangle vertices already processed to void further contacts with same triangle // Set with the triangle vertices already processed to void further contacts with same triangle
std::unordered_multimap<int, Vector3> processTriangleVertices; std::unordered_multimap<int32_t, Vector3> processTriangleVertices;
// Sort the list of narrow-phase contacts according to their penetration depth // Sort the list of narrow-phase contacts according to their penetration depth
std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare()); std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare());
@ -152,12 +133,12 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
// Compute the barycentric coordinates of the point in the triangle // Compute the barycentric coordinates of the point in the triangle
decimal u, v, w; float u, v, w;
computeBarycentricCoordinatesInTriangle(info.triangleVertices[0], computeBarycentricCoordinatesInTriangle(info.triangleVertices[0],
info.triangleVertices[1], info.triangleVertices[1],
info.triangleVertices[2], info.triangleVertices[2],
contactPoint, u, v, w); contactPoint, u, v, w);
int nbZeros = 0; int32_t nbZeros = 0;
bool isUZero = approxEqual(u, 0, 0.0001); bool isUZero = approxEqual(u, 0, 0.0001);
bool isVZero = approxEqual(v, 0, 0.0001); bool isVZero = approxEqual(v, 0, 0.0001);
bool isWZero = approxEqual(w, 0, 0.0001); bool isWZero = approxEqual(w, 0, 0.0001);
@ -244,9 +225,9 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
} }
// Return true if the vertex is in the set of already processed vertices // Return true if the vertex is in the set of already processed vertices
bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap<int, Vector3>& processTriangleVertices, const Vector3& vertex) const { bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap<int32_t, Vector3>& processTriangleVertices, const Vector3& vertex) const {
int key = int(vertex.x * vertex.y * vertex.z); int32_t key = int32_t(vertex.x * vertex.y * vertex.z);
auto range = processTriangleVertices.equal_range(key); auto range = processTriangleVertices.equal_range(key);
for (auto it = range.first; it != range.second; ++it) { for (auto it = range.first; it != range.second; ++it) {
@ -285,7 +266,7 @@ void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlapp
} }
SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]); SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]);
// Add the narrow-phase contact into the list of contact to process for // Add the narrow-phase contact int32_to the list of contact to process for
// smooth mesh collision // smooth mesh collision
mContactPoints.push_back(smoothContactInfo); mContactPoints.push_back(smoothContactInfo);
} }

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CONCAVE_VS_CONVEX_ALGORITHM_H
#define REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H
// Libraries // Libraries
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h> #include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h>
@ -199,12 +178,12 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
std::vector<SmoothMeshContactInfo> contactPoints, std::vector<SmoothMeshContactInfo> contactPoints,
NarrowPhaseCallback* narrowPhaseCallback); NarrowPhaseCallback* narrowPhaseCallback);
/// Add a triangle vertex into the set of processed triangles /// Add a triangle vertex int32_to the set of processed triangles
void addProcessedVertex(std::unordered_multimap<int, Vector3>& processTriangleVertices, void addProcessedVertex(std::unordered_multimap<int32_t, Vector3>& processTriangleVertices,
const Vector3& vertex); const Vector3& vertex);
/// Return true if the vertex is in the set of already processed vertices /// Return true if the vertex is in the set of already processed vertices
bool hasVertexBeenProcessed(const std::unordered_multimap<int, Vector3>& processTriangleVertices, bool hasVertexBeenProcessed(const std::unordered_multimap<int32_t, Vector3>& processTriangleVertices,
const Vector3& vertex) const; const Vector3& vertex) const;
public : public :
@ -223,12 +202,10 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
NarrowPhaseCallback* narrowPhaseCallback); NarrowPhaseCallback* narrowPhaseCallback);
}; };
// Add a triangle vertex into the set of processed triangles // Add a triangle vertex int32_to the set of processed triangles
inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap<int, Vector3>& processTriangleVertices, const Vector3& vertex) { inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap<int32_t, Vector3>& processTriangleVertices, const Vector3& vertex) {
processTriangleVertices.insert(std::make_pair(int(vertex.x * vertex.y * vertex.z), vertex)); processTriangleVertices.insert(std::make_pair(int32_t(vertex.x * vertex.y * vertex.z), vertex));
} }
} }
#endif

46
ephysics/collision/narrowphase/DefaultCollisionDispatch.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/narrowphase/DefaultCollisionDispatch.h> #include <ephysics/collision/narrowphase/DefaultCollisionDispatch.h>
@ -51,25 +32,20 @@ void DefaultCollisionDispatch::init(CollisionDetection* collisionDetection,
// Select and return the narrow-phase collision detection algorithm to // Select and return the narrow-phase collision detection algorithm to
// use between two types of collision shapes. // use between two types of collision shapes.
NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int type2) { NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int32_t type1, int32_t type2) {
CollisionShapeType shape1Type = static_cast<CollisionShapeType>(type1); CollisionShapeType shape1Type = static_cast<CollisionShapeType>(type1);
CollisionShapeType shape2Type = static_cast<CollisionShapeType>(type2); CollisionShapeType shape2Type = static_cast<CollisionShapeType>(type2);
// Sphere vs Sphere algorithm // Sphere vs Sphere algorithm
if (shape1Type == SPHERE && shape2Type == SPHERE) { if (shape1Type == SPHERE && shape2Type == SPHERE) {
return &mSphereVsSphereAlgorithm; return &mSphereVsSphereAlgorithm;
} } else if ( (!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))
|| (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
// Concave vs Convex algorithm // Concave vs Convex algorithm
else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) ||
(!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
return &mConcaveVsConvexAlgorithm; return &mConcaveVsConvexAlgorithm;
} } else if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) {
// Convex vs Convex algorithm (GJK algorithm) // Convex vs Convex algorithm (GJK algorithm)
else if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) {
return &mGJKAlgorithm; return &mGJKAlgorithm;
} } else {
else { return nullptr;
return NULL;
} }
} }

38
ephysics/collision/narrowphase/DefaultCollisionDispatch.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_DEFAULT_COLLISION_DISPATCH_H
#define REACTPHYSICS3D_DEFAULT_COLLISION_DISPATCH_H
// Libraries // Libraries
#include <ephysics/collision/narrowphase/CollisionDispatch.h> #include <ephysics/collision/narrowphase/CollisionDispatch.h>
@ -67,12 +46,9 @@ class DefaultCollisionDispatch : public CollisionDispatch {
/// Select and return the narrow-phase collision detection algorithm to /// Select and return the narrow-phase collision detection algorithm to
/// use between two types of collision shapes. /// use between two types of collision shapes.
virtual NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2); virtual NarrowPhaseAlgorithm* selectAlgorithm(int32_t type1, int32_t type2);
}; };
} }
#endif

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

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/narrowphase/EPA/EPAAlgorithm.h> #include <ephysics/collision/narrowphase/EPA/EPAAlgorithm.h>
@ -45,7 +26,7 @@ EPAAlgorithm::~EPAAlgorithm() {
// Decide if the origin is in the tetrahedron. // 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 /// 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 /// the vertex that is wrong if the origin is not in the tetrahedron
int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, int32_t EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
const Vector3& p3, const Vector3& p4) const { const Vector3& p3, const Vector3& p4) const {
// Check vertex 1 // Check vertex 1
@ -79,7 +60,7 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
// Compute the penetration depth with the EPA algorithm. // Compute the penetration depth with the EPA algorithm.
/// This method computes the penetration depth and contact points between two /// This method computes the penetration depth and contact points between two
/// enlarged objects (with margin) where the original objects (without margin) /// enlarged objects (with margin) where the original objects (without margin)
/// intersect. An initial simplex that contains origin has been computed with /// int32_tersect. An initial simplex that contains origin has been computed with
/// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find /// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
/// the correct penetration depth /// the correct penetration depth
void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex, void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex,
@ -113,18 +94,18 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
Transform body2Tobody1 = transform1.getInverse() * transform2; Transform body2Tobody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local // Matrix that transform a direction from local
// space of body 1 into local space of body 2 // space of body 1 int32_to local space of body 2
Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * Quaternion rotateToBody2 = transform2.getOrientation().getInverse() *
transform1.getOrientation(); transform1.getOrientation();
// Get the simplex computed previously by the GJK algorithm // Get the simplex computed previously by the GJK algorithm
unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points); uint32_t nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
// Compute the tolerance // Compute the tolerance
decimal tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint(); float tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint();
// Number of triangles in the polytope // Number of triangles in the polytope
unsigned int nbTriangles = 0; uint32_t nbTriangles = 0;
// Clear the storing of triangles // Clear the storing of triangles
triangleStore.clear(); triangleStore.clear();
@ -152,10 +133,10 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
Vector3 d = (points[1] - points[0]).getUnit(); Vector3 d = (points[1] - points[0]).getUnit();
// Choose the coordinate axis from the minimal absolute component of the vector d // Choose the coordinate axis from the minimal absolute component of the vector d
int minAxis = d.getAbsoluteVector().getMinAxis(); int32_t minAxis = d.getAbsoluteVector().getMinAxis();
// Compute sin(60) // Compute sin(60)
const decimal sin60 = decimal(sqrt(3.0)) * decimal(0.5); const float sin60 = float(sqrt(3.0)) * float(0.5);
// Create a rotation quaternion to rotate the vector v1 to get the vectors // Create a rotation quaternion to rotate the vector v1 to get the vectors
// v2 and v3 // v2 and v3
@ -217,7 +198,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// where the GJK algorithm compute a simplex of three vertices. // where the GJK algorithm compute a simplex of three vertices.
// Check if the tetrahedron contains the origin (or wich is the wrong vertex otherwise) // Check if the tetrahedron contains the origin (or wich is the wrong vertex otherwise)
int badVertex = isOriginInTetrahedron(points[0], points[1], points[2], points[3]); int32_t badVertex = isOriginInTetrahedron(points[0], points[1], points[2], points[3]);
// If the origin is in the tetrahedron // If the origin is in the tetrahedron
if (badVertex == 0) { if (badVertex == 0) {
@ -357,7 +338,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
} }
TriangleEPA* triangle = 0; TriangleEPA* triangle = 0;
decimal upperBoundSquarePenDepth = DECIMAL_LARGEST; float upperBoundSquarePenDepth = DECIMAL_LARGEST;
do { do {
triangle = triangleHeap[0]; triangle = triangleHeap[0];
@ -383,19 +364,19 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
(-triangle->getClosestPoint()), shape2CachedCollisionData); (-triangle->getClosestPoint()), shape2CachedCollisionData);
points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices]; points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
int indexNewVertex = nbVertices; int32_t indexNewVertex = nbVertices;
nbVertices++; nbVertices++;
// Update the upper bound of the penetration depth // Update the upper bound of the penetration depth
decimal wDotv = points[indexNewVertex].dot(triangle->getClosestPoint()); float wDotv = points[indexNewVertex].dot(triangle->getClosestPoint());
assert(wDotv > 0.0); assert(wDotv > 0.0);
decimal wDotVSquare = wDotv * wDotv / triangle->getDistSquare(); float wDotVSquare = wDotv * wDotv / triangle->getDistSquare();
if (wDotVSquare < upperBoundSquarePenDepth) { if (wDotVSquare < upperBoundSquarePenDepth) {
upperBoundSquarePenDepth = wDotVSquare; upperBoundSquarePenDepth = wDotVSquare;
} }
// Compute the error // Compute the error
decimal error = wDotv - triangle->getDistSquare(); float error = wDotv - triangle->getDistSquare();
if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) || if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) ||
points[indexNewVertex] == points[(*triangle)[0]] || points[indexNewVertex] == points[(*triangle)[0]] ||
points[indexNewVertex] == points[(*triangle)[1]] || points[indexNewVertex] == points[(*triangle)[1]] ||
@ -406,7 +387,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// Now, we compute the silhouette cast by the new vertex. The current triangle // Now, we compute the silhouette cast by the new vertex. The current triangle
// face will not be in the convex hull. We start the local recursive silhouette // face will not be in the convex hull. We start the local recursive silhouette
// algorithm from the current triangle face. // algorithm from the current triangle face.
int i = triangleStore.getNbTriangles(); int32_t i = triangleStore.getNbTriangles();
if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) { if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) {
break; break;
} }
@ -426,7 +407,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA); Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB); Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
Vector3 normal = v.getUnit(); Vector3 normal = v.getUnit();
decimal penetrationDepth = v.length(); float penetrationDepth = v.length();
assert(penetrationDepth > 0.0); assert(penetrationDepth > 0.0);
if (normal.lengthSquare() < MACHINE_EPSILON) return; if (normal.lengthSquare() < MACHINE_EPSILON) return;

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_EPA_ALGORITHM_H
#define REACTPHYSICS3D_EPA_ALGORITHM_H
// Libraries // Libraries
#include <ephysics/collision/narrowphase/GJK/Simplex.h> #include <ephysics/collision/narrowphase/GJK/Simplex.h>
@ -43,10 +22,10 @@ namespace reactphysics3d {
// ---------- Constants ---------- // // ---------- Constants ---------- //
/// Maximum number of support points of the polytope /// Maximum number of support points of the polytope
const unsigned int MAX_SUPPORT_POINTS = 100; const uint32_t MAX_SUPPORT_POINTS = 100;
/// Maximum number of facets of the polytope /// Maximum number of facets of the polytope
const unsigned int MAX_FACETS = 200; const uint32_t MAX_FACETS = 200;
// Class TriangleComparison // Class TriangleComparison
@ -72,7 +51,7 @@ class TriangleComparison {
* This class is the implementation of the Expanding Polytope Algorithm (EPA). * This class is the implementation of the Expanding Polytope Algorithm (EPA).
* The EPA algorithm computes the penetration depth and contact points between * The EPA algorithm computes the penetration depth and contact points between
* two enlarged objects (with margin) where the original objects (without margin) * two enlarged objects (with margin) where the original objects (without margin)
* intersect. The penetration depth of a pair of intersecting objects A and B is * int32_tersect. The penetration depth of a pair of int32_tersecting objects A and B is
* the length of a point on the boundary of the Minkowski sum (A-B) closest to the * the length of a point on the boundary of the Minkowski sum (A-B) closest to the
* origin. The goal of the EPA algorithm is to start with an initial simplex polytope * origin. The goal of the EPA algorithm is to start with an initial simplex polytope
* that contains the origin and expend it in order to find the point on the boundary * that contains the origin and expend it in order to find the point on the boundary
@ -102,11 +81,11 @@ class EPAAlgorithm {
EPAAlgorithm& operator=(const EPAAlgorithm& algorithm); EPAAlgorithm& operator=(const EPAAlgorithm& algorithm);
/// Add a triangle face in the candidate triangle heap /// Add a triangle face in the candidate triangle heap
void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, uint& nbTriangles, void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, uint32_t& nbTriangles,
decimal upperBoundSquarePenDepth); float upperBoundSquarePenDepth);
/// Decide if the origin is in the tetrahedron. /// Decide if the origin is in the tetrahedron.
int isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, int32_t isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
const Vector3& p3, const Vector3& p4) const; const Vector3& p3, const Vector3& p4) const;
public: public:
@ -134,10 +113,10 @@ class EPAAlgorithm {
// Add a triangle face in the candidate triangle heap in the EPA algorithm // Add a triangle face in the candidate triangle heap in the EPA algorithm
inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap,
uint& nbTriangles, decimal upperBoundSquarePenDepth) { uint32_t& nbTriangles, float upperBoundSquarePenDepth) {
// If the closest point of the affine hull of triangle // If the closest point of the affine hull of triangle
// points is internal to the triangle and if the distance // points is int32_ternal to the triangle and if the distance
// of the closest point from the origin is at most the // of the closest point from the origin is at most the
// penetration depth upper bound // penetration depth upper bound
if (triangle->isClosestPointInternalToTriangle() && if (triangle->isClosestPointInternalToTriangle() &&
@ -157,5 +136,3 @@ inline void EPAAlgorithm::init(MemoryAllocator* memoryAllocator) {
} }
#endif

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

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/narrowphase/EPA/EdgeEPA.h> #include <ephysics/collision/narrowphase/EPA/EdgeEPA.h>
@ -39,7 +20,7 @@ EdgeEPA::EdgeEPA() {
} }
// Constructor // Constructor
EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int index) EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int32_t index)
: mOwnerTriangle(ownerTriangle), mIndex(index) { : mOwnerTriangle(ownerTriangle), mIndex(index) {
assert(index >= 0 && index < 3); assert(index >= 0 && index < 3);
} }
@ -56,17 +37,17 @@ EdgeEPA::~EdgeEPA() {
} }
// Return the index of the source vertex of the edge (vertex starting the edge) // Return the index of the source vertex of the edge (vertex starting the edge)
uint EdgeEPA::getSourceVertexIndex() const { uint32_t EdgeEPA::getSourceVertexIndex() const {
return (*mOwnerTriangle)[mIndex]; return (*mOwnerTriangle)[mIndex];
} }
// Return the index of the target vertex of the edge (vertex ending the edge) // Return the index of the target vertex of the edge (vertex ending the edge)
uint EdgeEPA::getTargetVertexIndex() const { uint32_t EdgeEPA::getTargetVertexIndex() const {
return (*mOwnerTriangle)[indexOfNextCounterClockwiseEdge(mIndex)]; return (*mOwnerTriangle)[indexOfNextCounterClockwiseEdge(mIndex)];
} }
// Execute the recursive silhouette algorithm from this edge // Execute the recursive silhouette algorithm from this edge
bool EdgeEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex, bool EdgeEPA::computeSilhouette(const Vector3* vertices, uint32_t indexNewVertex,
TrianglesStore& triangleStore) { TrianglesStore& triangleStore) {
// If the edge has not already been visited // If the edge has not already been visited
if (!mOwnerTriangle->getIsObsolete()) { if (!mOwnerTriangle->getIsObsolete()) {
@ -90,7 +71,7 @@ bool EdgeEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex,
// The current triangle is visible and therefore obsolete // The current triangle is visible and therefore obsolete
mOwnerTriangle->setIsObsolete(true); mOwnerTriangle->setIsObsolete(true);
int backup = triangleStore.getNbTriangles(); int32_t backup = triangleStore.getNbTriangles();
if(!mOwnerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge( if(!mOwnerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(
this->mIndex)).computeSilhouette(vertices, this->mIndex)).computeSilhouette(vertices,

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_EDGE_EPA_H
#define REACTPHYSICS3D_EDGE_EPA_H
// Libraries // Libraries
@ -52,7 +31,7 @@ class EdgeEPA {
/// Index of the edge in the triangle (between 0 and 2). /// Index of the edge in the triangle (between 0 and 2).
/// The edge with index i connect triangle vertices i and (i+1 % 3) /// The edge with index i connect triangle vertices i and (i+1 % 3)
int mIndex; int32_t mIndex;
public: public:
@ -62,7 +41,7 @@ class EdgeEPA {
EdgeEPA(); EdgeEPA();
/// Constructor /// Constructor
EdgeEPA(TriangleEPA* ownerTriangle, int index); EdgeEPA(TriangleEPA* ownerTriangle, int32_t index);
/// Copy-constructor /// Copy-constructor
EdgeEPA(const EdgeEPA& edge); EdgeEPA(const EdgeEPA& edge);
@ -74,16 +53,16 @@ class EdgeEPA {
TriangleEPA* getOwnerTriangle() const; TriangleEPA* getOwnerTriangle() const;
/// Return the index of the edge in the triangle /// Return the index of the edge in the triangle
int getIndex() const; int32_t getIndex() const;
/// Return index of the source vertex of the edge /// Return index of the source vertex of the edge
uint getSourceVertexIndex() const; uint32_t getSourceVertexIndex() const;
/// Return the index of the target vertex of the edge /// Return the index of the target vertex of the edge
uint getTargetVertexIndex() const; uint32_t getTargetVertexIndex() const;
/// Execute the recursive silhouette algorithm from this edge /// Execute the recursive silhouette algorithm from this edge
bool computeSilhouette(const Vector3* vertices, uint index, TrianglesStore& triangleStore); bool computeSilhouette(const Vector3* vertices, uint32_t index, TrianglesStore& triangleStore);
/// Assignment operator /// Assignment operator
EdgeEPA& operator=(const EdgeEPA& edge); EdgeEPA& operator=(const EdgeEPA& edge);
@ -95,7 +74,7 @@ inline TriangleEPA* EdgeEPA::getOwnerTriangle() const {
} }
// Return the edge index // Return the edge index
inline int EdgeEPA::getIndex() const { inline int32_t EdgeEPA::getIndex() const {
return mIndex; return mIndex;
} }
@ -107,16 +86,15 @@ inline EdgeEPA& EdgeEPA::operator=(const EdgeEPA& edge) {
} }
// Return the index of the next counter-clockwise edge of the ownver triangle // Return the index of the next counter-clockwise edge of the ownver triangle
inline int indexOfNextCounterClockwiseEdge(int i) { inline int32_t indexOfNextCounterClockwiseEdge(int32_t i) {
return (i + 1) % 3; return (i + 1) % 3;
} }
// Return the index of the previous counter-clockwise edge of the ownver triangle // Return the index of the previous counter-clockwise edge of the ownver triangle
inline int indexOfPreviousCounterClockwiseEdge(int i) { inline int32_t indexOfPreviousCounterClockwiseEdge(int32_t i) {
return (i + 2) % 3; return (i + 2) % 3;
} }
} }
#endif

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

@ -1,28 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/narrowphase/EPA/TriangleEPA.h> #include <ephysics/collision/narrowphase/EPA/TriangleEPA.h>
@ -38,7 +18,7 @@ TriangleEPA::TriangleEPA() {
} }
// Constructor // Constructor
TriangleEPA::TriangleEPA(uint indexVertex1, uint indexVertex2, uint indexVertex3) TriangleEPA::TriangleEPA(uint32_t indexVertex1, uint32_t indexVertex2, uint32_t indexVertex3)
: mIsObsolete(false) { : mIsObsolete(false) {
mIndicesVertices[0] = indexVertex1; mIndicesVertices[0] = indexVertex1;
mIndicesVertices[1] = indexVertex2; mIndicesVertices[1] = indexVertex2;
@ -56,11 +36,11 @@ bool TriangleEPA::computeClosestPoint(const Vector3* vertices) {
Vector3 v1 = vertices[mIndicesVertices[1]] - p0; Vector3 v1 = vertices[mIndicesVertices[1]] - p0;
Vector3 v2 = vertices[mIndicesVertices[2]] - p0; Vector3 v2 = vertices[mIndicesVertices[2]] - p0;
decimal v1Dotv1 = v1.dot(v1); float v1Dotv1 = v1.dot(v1);
decimal v1Dotv2 = v1.dot(v2); float v1Dotv2 = v1.dot(v2);
decimal v2Dotv2 = v2.dot(v2); float v2Dotv2 = v2.dot(v2);
decimal p0Dotv1 = p0.dot(v1); float p0Dotv1 = p0.dot(v1);
decimal p0Dotv2 = p0.dot(v2); float p0Dotv2 = p0.dot(v2);
// Compute determinant // Compute determinant
mDet = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2; mDet = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2;
@ -72,7 +52,7 @@ bool TriangleEPA::computeClosestPoint(const Vector3* vertices) {
// If the determinant is positive // If the determinant is positive
if (mDet > 0.0) { if (mDet > 0.0) {
// Compute the closest point v // Compute the closest point v
mClosestPoint = p0 + decimal(1.0) / mDet * (mLambda1 * v1 + mLambda2 * v2); mClosestPoint = p0 + float(1.0) / mDet * (mLambda1 * v1 + mLambda2 * v2);
// Compute the square distance of closest point to the origin // Compute the square distance of closest point to the origin
mDistSquare = mClosestPoint.dot(mClosestPoint); mDistSquare = mClosestPoint.dot(mClosestPoint);
@ -121,10 +101,10 @@ 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 /// 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 /// 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. /// obselete and will not be considered as being a candidate face in the future.
bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex, bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint32_t indexNewVertex,
TrianglesStore& triangleStore) { TrianglesStore& triangleStore) {
uint first = triangleStore.getNbTriangles(); uint32_t first = triangleStore.getNbTriangles();
// Mark the current triangle as obsolete because it // Mark the current triangle as obsolete because it
setIsObsolete(true); setIsObsolete(true);
@ -136,7 +116,7 @@ bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex
mAdjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore); mAdjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore);
if (result) { if (result) {
int i,j; int32_t i,j;
// For each triangle face that contains the new vertex and an edge of the silhouette // For each triangle face that contains the new vertex and an edge of the silhouette
for (i=first, j=triangleStore.getNbTriangles()-1; for (i=first, j=triangleStore.getNbTriangles()-1;

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_TRIANGLE_EPA_H
#define REACTPHYSICS3D_TRIANGLE_EPA_H
// Libraries // Libraries
#include <ephysics/mathematics/mathematics.h> #include <ephysics/mathematics/mathematics.h>
@ -51,7 +30,7 @@ class TriangleEPA {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Indices of the vertices y_i of the triangle /// Indices of the vertices y_i of the triangle
uint mIndicesVertices[3]; uint32_t mIndicesVertices[3];
/// Three adjacent edges of the triangle (edges of other triangles) /// Three adjacent edges of the triangle (edges of other triangles)
EdgeEPA mAdjacentEdges[3]; EdgeEPA mAdjacentEdges[3];
@ -60,19 +39,19 @@ class TriangleEPA {
bool mIsObsolete; bool mIsObsolete;
/// Determinant /// Determinant
decimal mDet; float mDet;
/// Point v closest to the origin on the affine hull of the triangle /// Point v closest to the origin on the affine hull of the triangle
Vector3 mClosestPoint; Vector3 mClosestPoint;
/// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 /// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
decimal mLambda1; float mLambda1;
/// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 /// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
decimal mLambda2; float mLambda2;
/// Square distance of the point closest point v to the origin /// Square distance of the point closest point v to the origin
decimal mDistSquare; float mDistSquare;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -90,19 +69,19 @@ class TriangleEPA {
TriangleEPA(); TriangleEPA();
/// Constructor /// Constructor
TriangleEPA(uint v1, uint v2, uint v3); TriangleEPA(uint32_t v1, uint32_t v2, uint32_t v3);
/// Destructor /// Destructor
~TriangleEPA(); ~TriangleEPA();
/// Return an adjacent edge of the triangle /// Return an adjacent edge of the triangle
EdgeEPA& getAdjacentEdge(int index); EdgeEPA& getAdjacentEdge(int32_t index);
/// Set an adjacent edge of the triangle /// Set an adjacent edge of the triangle
void setAdjacentEdge(int index, EdgeEPA& edge); void setAdjacentEdge(int32_t index, EdgeEPA& edge);
/// Return the square distance of the closest point to origin /// Return the square distance of the closest point to origin
decimal getDistSquare() const; float getDistSquare() const;
/// Set the isObsolete value /// Set the isObsolete value
void setIsObsolete(bool isObsolete); void setIsObsolete(bool isObsolete);
@ -117,7 +96,7 @@ class TriangleEPA {
bool isClosestPointInternalToTriangle() const; bool isClosestPointInternalToTriangle() const;
/// Return true if the triangle is visible from a given vertex /// Return true if the triangle is visible from a given vertex
bool isVisibleFromVertex(const Vector3* vertices, uint index) const; bool isVisibleFromVertex(const Vector3* vertices, uint32_t index) const;
/// Compute the point v closest to the origin of this triangle /// Compute the point v closest to the origin of this triangle
bool computeClosestPoint(const Vector3* vertices); bool computeClosestPoint(const Vector3* vertices);
@ -126,10 +105,10 @@ class TriangleEPA {
Vector3 computeClosestPointOfObject(const Vector3* supportPointsOfObject) const; Vector3 computeClosestPointOfObject(const Vector3* supportPointsOfObject) const;
/// Execute the recursive silhouette algorithm from this triangle face. /// Execute the recursive silhouette algorithm from this triangle face.
bool computeSilhouette(const Vector3* vertices, uint index, TrianglesStore& triangleStore); bool computeSilhouette(const Vector3* vertices, uint32_t index, TrianglesStore& triangleStore);
/// Access operator /// Access operator
uint operator[](int i) const; uint32_t operator[](int32_t i) const;
/// Associate two edges /// Associate two edges
friend bool link(const EdgeEPA& edge0, const EdgeEPA& edge1); friend bool link(const EdgeEPA& edge0, const EdgeEPA& edge1);
@ -139,19 +118,19 @@ class TriangleEPA {
}; };
// Return an edge of the triangle // Return an edge of the triangle
inline EdgeEPA& TriangleEPA::getAdjacentEdge(int index) { inline EdgeEPA& TriangleEPA::getAdjacentEdge(int32_t index) {
assert(index >= 0 && index < 3); assert(index >= 0 && index < 3);
return mAdjacentEdges[index]; return mAdjacentEdges[index];
} }
// Set an adjacent edge of the triangle // Set an adjacent edge of the triangle
inline void TriangleEPA::setAdjacentEdge(int index, EdgeEPA& edge) { inline void TriangleEPA::setAdjacentEdge(int32_t index, EdgeEPA& edge) {
assert(index >=0 && index < 3); assert(index >=0 && index < 3);
mAdjacentEdges[index] = edge; mAdjacentEdges[index] = edge;
} }
// Return the square distance of the closest point to origin // Return the square distance of the closest point to origin
inline decimal TriangleEPA::getDistSquare() const { inline float TriangleEPA::getDistSquare() const {
return mDistSquare; return mDistSquare;
} }
@ -176,7 +155,7 @@ inline bool TriangleEPA::isClosestPointInternalToTriangle() const {
} }
// Return true if the triangle is visible from a given vertex // Return true if the triangle is visible from a given vertex
inline bool TriangleEPA::isVisibleFromVertex(const Vector3* vertices, uint index) const { inline bool TriangleEPA::isVisibleFromVertex(const Vector3* vertices, uint32_t index) const {
Vector3 closestToVert = vertices[index] - mClosestPoint; Vector3 closestToVert = vertices[index] - mClosestPoint;
return (mClosestPoint.dot(closestToVert) > 0.0); return (mClosestPoint.dot(closestToVert) > 0.0);
} }
@ -184,16 +163,14 @@ inline bool TriangleEPA::isVisibleFromVertex(const Vector3* vertices, uint index
// Compute the point of an object closest to the origin // Compute the point of an object closest to the origin
inline Vector3 TriangleEPA::computeClosestPointOfObject(const Vector3* supportPointsOfObject) const{ inline Vector3 TriangleEPA::computeClosestPointOfObject(const Vector3* supportPointsOfObject) const{
const Vector3& p0 = supportPointsOfObject[mIndicesVertices[0]]; const Vector3& p0 = supportPointsOfObject[mIndicesVertices[0]];
return p0 + decimal(1.0)/mDet * (mLambda1 * (supportPointsOfObject[mIndicesVertices[1]] - p0) + return p0 + float(1.0)/mDet * (mLambda1 * (supportPointsOfObject[mIndicesVertices[1]] - p0) +
mLambda2 * (supportPointsOfObject[mIndicesVertices[2]] - p0)); mLambda2 * (supportPointsOfObject[mIndicesVertices[2]] - p0));
} }
// Access operator // Access operator
inline uint TriangleEPA::operator[](int i) const { inline uint32_t TriangleEPA::operator[](int32_t i) const {
assert(i >= 0 && i <3); assert(i >= 0 && i <3);
return mIndicesVertices[i]; return mIndicesVertices[i];
} }
} }
#endif

29
ephysics/collision/narrowphase/EPA/TrianglesStore.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/narrowphase/EPA/TrianglesStore.h> #include <ephysics/collision/narrowphase/EPA/TrianglesStore.h>

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_TRIANGLES_STORE_H
#define REACTPHYSICS3D_TRIANGLES_STORE_H
#include <ephysics/collision/narrowphase/EPA/TriangleEPA.h> #include <ephysics/collision/narrowphase/EPA/TriangleEPA.h>
@ -36,7 +15,7 @@
namespace reactphysics3d { namespace reactphysics3d {
// Constants // Constants
const unsigned int MAX_TRIANGLES = 200; // Maximum number of triangles const uint32_t MAX_TRIANGLES = 200; // Maximum number of triangles
// Class TriangleStore // Class TriangleStore
/** /**
@ -52,7 +31,7 @@ class TrianglesStore {
TriangleEPA mTriangles[MAX_TRIANGLES]; TriangleEPA mTriangles[MAX_TRIANGLES];
/// Number of triangles /// Number of triangles
int mNbTriangles; int32_t mNbTriangles;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -76,19 +55,19 @@ class TrianglesStore {
void clear(); void clear();
/// Return the number of triangles /// Return the number of triangles
int getNbTriangles() const; int32_t getNbTriangles() const;
/// Set the number of triangles /// Set the number of triangles
void setNbTriangles(int backup); void setNbTriangles(int32_t backup);
/// Return the last triangle /// Return the last triangle
TriangleEPA& last(); TriangleEPA& last();
/// Create a new triangle /// Create a new triangle
TriangleEPA* newTriangle(const Vector3* vertices, uint v0, uint v1, uint v2); TriangleEPA* newTriangle(const Vector3* vertices, uint32_t v0, uint32_t v1, uint32_t v2);
/// Access operator /// Access operator
TriangleEPA& operator[](int i); TriangleEPA& operator[](int32_t i);
}; };
// Clear all the storage // Clear all the storage
@ -97,12 +76,12 @@ inline void TrianglesStore::clear() {
} }
// Return the number of triangles // Return the number of triangles
inline int TrianglesStore::getNbTriangles() const { inline int32_t TrianglesStore::getNbTriangles() const {
return mNbTriangles; return mNbTriangles;
} }
inline void TrianglesStore::setNbTriangles(int backup) { inline void TrianglesStore::setNbTriangles(int32_t backup) {
mNbTriangles = backup; mNbTriangles = backup;
} }
@ -114,7 +93,7 @@ inline TriangleEPA& TrianglesStore::last() {
// Create a new triangle // Create a new triangle
inline TriangleEPA* TrianglesStore::newTriangle(const Vector3* vertices, inline TriangleEPA* TrianglesStore::newTriangle(const Vector3* vertices,
uint v0,uint v1, uint v2) { uint32_t v0,uint32_t v1, uint32_t v2) {
TriangleEPA* newTriangle = NULL; TriangleEPA* newTriangle = NULL;
// If we have not reached the maximum number of triangles // If we have not reached the maximum number of triangles
@ -132,10 +111,9 @@ inline TriangleEPA* TrianglesStore::newTriangle(const Vector3* vertices,
} }
// Access operator // Access operator
inline TriangleEPA& TrianglesStore::operator[](int i) { inline TriangleEPA& TrianglesStore::operator[](int32_t i) {
return mTriangles[i]; return mTriangles[i];
} }
} }
#endif

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

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.h> #include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.h>
@ -49,9 +30,9 @@ GJKAlgorithm::~GJKAlgorithm() {
// Compute a contact info if the two collision shapes collide. // Compute a contact info if the two collision shapes collide.
/// This method implements the Hybrid Technique for computing the penetration depth by /// This method implements the Hybrid Technique for computing the penetration depth by
/// running the GJK algorithm on original objects (without margin). If the shapes intersect /// running the GJK algorithm on original objects (without margin). If the shapes int32_tersect
/// only in the margins, the method compute the penetration depth and contact points /// only in the margins, the method compute the penetration depth and contact points
/// (of enlarged objects). If the original objects (without margin) intersect, we /// (of enlarged objects). If the original objects (without margin) int32_tersect, we
/// call the computePenetrationDepthForEnlargedObjects() method that run the GJK /// call the computePenetrationDepthForEnlargedObjects() method that run the GJK
/// algorithm on the enlarged object to obtain a simplex polytope that contains the /// algorithm on the enlarged object to obtain a simplex polytope that contains the
/// origin, they we give that simplex polytope to the EPA algorithm which will compute /// origin, they we give that simplex polytope to the EPA algorithm which will compute
@ -67,8 +48,8 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
Vector3 w; // Support point of Minkowski difference A-B Vector3 w; // Support point of Minkowski difference A-B
Vector3 pA; // Closest point of object A Vector3 pA; // Closest point of object A
Vector3 pB; // Closest point of object B Vector3 pB; // Closest point of object B
decimal vDotw; float vDotw;
decimal prevDistSquare; float prevDistSquare;
assert(shape1Info.collisionShape->isConvex()); assert(shape1Info.collisionShape->isConvex());
assert(shape2Info.collisionShape->isConvex()); assert(shape2Info.collisionShape->isConvex());
@ -88,13 +69,13 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
Transform body2Tobody1 = transform1.getInverse() * transform2; Transform body2Tobody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local // Matrix that transform a direction from local
// space of body 1 into local space of body 2 // space of body 1 int32_to local space of body 2
Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
transform1.getOrientation().getMatrix(); transform1.getOrientation().getMatrix();
// Initialize the margin (sum of margins of both objects) // Initialize the margin (sum of margins of both objects)
decimal margin = shape1->getMargin() + shape2->getMargin(); float margin = shape1->getMargin() + shape2->getMargin();
decimal marginSquare = margin * margin; float marginSquare = margin * margin;
assert(margin > 0.0); assert(margin > 0.0);
// Create a simplex set // Create a simplex set
@ -104,7 +85,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis(); Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis();
// Initialize the upper bound for the square distance // Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_LARGEST; float distSquare = DECIMAL_LARGEST;
do { do {
@ -118,17 +99,17 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
vDotw = v.dot(w); vDotw = v.dot(w);
// If the enlarge objects (with margins) do not intersect // If the enlarge objects (with margins) do not int32_tersect
if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) { if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
// Cache the current separating axis for frame coherence // Cache the current separating axis for frame coherence
mCurrentOverlappingPair->setCachedSeparatingAxis(v); mCurrentOverlappingPair->setCachedSeparatingAxis(v);
// No intersection, we return // No int32_tersection, we return
return; return;
} }
// If the objects intersect only in the margins // If the objects int32_tersect only in the margins
if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) { if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) {
// Compute the closet points of both objects (without the margins) // Compute the closet points of both objects (without the margins)
@ -136,14 +117,14 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
// Project those two points on the margins to have the closest points of both // Project those two points on the margins to have the closest points of both
// object with the margins // object with the margins
decimal dist = sqrt(distSquare); float dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v); pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info // Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit()); Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist; float penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors) // Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return; if (penetrationDepth <= 0.0) return;
@ -154,7 +135,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return // There is an int32_tersection, therefore we return
return; return;
} }
@ -169,14 +150,14 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
// Project those two points on the margins to have the closest points of both // Project those two points on the margins to have the closest points of both
// object with the margins // object with the margins
decimal dist = sqrt(distSquare); float dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v); pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info // Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit()); Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist; float penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors) // Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return; if (penetrationDepth <= 0.0) return;
@ -187,7 +168,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return // There is an int32_tersection, therefore we return
return; return;
} }
@ -200,14 +181,14 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
// Project those two points on the margins to have the closest points of both // Project those two points on the margins to have the closest points of both
// object with the margins // object with the margins
decimal dist = sqrt(distSquare); float dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v); pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info // Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit()); Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist; float penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors) // Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return; if (penetrationDepth <= 0.0) return;
@ -218,7 +199,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return // There is an int32_tersection, therefore we return
return; return;
} }
@ -238,14 +219,14 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
// Project those two points on the margins to have the closest points of both // Project those two points on the margins to have the closest points of both
// object with the margins // object with the margins
decimal dist = sqrt(distSquare); float dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v); pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info // Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit()); Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist; float penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors) // Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return; if (penetrationDepth <= 0.0) return;
@ -256,13 +237,13 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return // There is an int32_tersection, therefore we return
return; return;
} }
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON * } while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
simplex.getMaxLengthSquareOfAPoint()); simplex.getMaxLengthSquareOfAPoint());
// The objects (without margins) intersect. Therefore, we run the GJK algorithm // The objects (without margins) int32_tersect. Therefore, we run the GJK algorithm
// again but on the enlarged objects to compute a simplex polytope that contains // again but on the enlarged objects to compute a simplex polytope that contains
// the origin. Then, we give that simplex polytope to the EPA algorithm to compute // the origin. Then, we give that simplex polytope to the EPA algorithm to compute
// the correct penetration depth and contact points between the enlarged objects. // the correct penetration depth and contact points between the enlarged objects.
@ -272,7 +253,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
/// This method runs the GJK algorithm on the two enlarged objects (with margin) /// This method runs the GJK algorithm on the two enlarged objects (with margin)
/// to compute a simplex polytope that contains the origin. The two objects are /// to compute a simplex polytope that contains the origin. The two objects are
/// assumed to intersect in the original objects (without margin). Therefore such /// assumed to int32_tersect in the original objects (without margin). Therefore such
/// a polytope must exist. Then, we give that polytope to the EPA algorithm to /// 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. /// compute the correct penetration depth and contact points of the enlarged objects.
void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info, void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
@ -287,9 +268,9 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
Vector3 suppA; Vector3 suppA;
Vector3 suppB; Vector3 suppB;
Vector3 w; Vector3 w;
decimal vDotw; float vDotw;
decimal distSquare = DECIMAL_LARGEST; float distSquare = DECIMAL_LARGEST;
decimal prevDistSquare; float prevDistSquare;
assert(shape1Info.collisionShape->isConvex()); assert(shape1Info.collisionShape->isConvex());
assert(shape2Info.collisionShape->isConvex()); assert(shape2Info.collisionShape->isConvex());
@ -304,7 +285,7 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
// of body 1 (the GJK algorithm is done in local space of body 1) // of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2ToBody1 = transform1.getInverse() * transform2; Transform body2ToBody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local space of body 1 into local space of body 2 // Matrix that transform a direction from local space of body 1 int32_to local space of body 2
Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
transform1.getOrientation().getMatrix(); transform1.getOrientation().getMatrix();
@ -318,10 +299,10 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
vDotw = v.dot(w); vDotw = v.dot(w);
// If the enlarge objects do not intersect // If the enlarge objects do not int32_tersect
if (vDotw > 0.0) { if (vDotw > 0.0) {
// No intersection, we return // No int32_tersection, we return
return; return;
} }
@ -360,7 +341,7 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
Vector3 suppA; // Support point of object A Vector3 suppA; // Support point of object A
Vector3 w; // Support point of Minkowski difference A-B Vector3 w; // Support point of Minkowski difference A-B
decimal prevDistSquare; float prevDistSquare;
assert(proxyShape->getCollisionShape()->isConvex()); assert(proxyShape->getCollisionShape()->isConvex());
@ -378,7 +359,7 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
Vector3 v(1, 1, 1); Vector3 v(1, 1, 1);
// Initialize the upper bound for the square distance // Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_LARGEST; float distSquare = DECIMAL_LARGEST;
do { do {
@ -434,10 +415,10 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
Vector3 suppA; // Current lower bound point on the ray (starting at ray's origin) Vector3 suppA; // Current lower bound point on the ray (starting at ray's origin)
Vector3 suppB; // Support point on the collision shape Vector3 suppB; // Support point on the collision shape
const decimal machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON; const float machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON;
const decimal epsilon = decimal(0.0001); const float epsilon = float(0.0001);
// Convert the ray origin and direction into the local-space of the collision shape // Convert the ray origin and direction int32_to the local-space of the collision shape
Vector3 rayDirection = ray.point2 - ray.point1; Vector3 rayDirection = ray.point2 - ray.point1;
// If the points of the segment are two close, return no hit // If the points of the segment are two close, return no hit
@ -448,14 +429,14 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
// Create a simplex set // Create a simplex set
Simplex simplex; Simplex simplex;
Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0)); Vector3 n(float(0.0), float(0.0), float(0.0));
decimal lambda = decimal(0.0); float lambda = float(0.0);
suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin) suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin)
suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData); suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData);
Vector3 v = suppA - suppB; Vector3 v = suppA - suppB;
decimal vDotW, vDotR; float vDotW, vDotR;
decimal distSquare = v.lengthSquare(); float distSquare = v.lengthSquare();
int nbIterations = 0; int32_t nbIterations = 0;
// GJK Algorithm loop // GJK Algorithm loop
while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) { while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) {
@ -466,7 +447,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
vDotW = v.dot(w); vDotW = v.dot(w);
if (vDotW > decimal(0)) { if (vDotW > float(0)) {
vDotR = v.dot(rayDirection); vDotR = v.dot(rayDirection);
@ -494,7 +475,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
distSquare = v.lengthSquare(); distSquare = v.lengthSquare();
} }
else { else {
distSquare = decimal(0.0); distSquare = float(0.0);
} }
// If the current lower bound distance is larger than the maximum raycasting distance // If the current lower bound distance is larger than the maximum raycasting distance
@ -521,7 +502,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
raycastInfo.worldNormal = n; raycastInfo.worldNormal = n;
} }
else { // Degenerated normal vector, we return a zero normal vector else { // Degenerated normal vector, we return a zero normal vector
raycastInfo.worldNormal = Vector3(decimal(0), decimal(0), decimal(0)); raycastInfo.worldNormal = Vector3(float(0), float(0), float(0));
} }
return true; return true;

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_GJK_ALGORITHM_H
#define REACTPHYSICS3D_GJK_ALGORITHM_H
// Libraries // Libraries
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h> #include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h>
@ -37,9 +16,9 @@
namespace reactphysics3d { namespace reactphysics3d {
// Constants // Constants
const decimal REL_ERROR = decimal(1.0e-3); const float REL_ERROR = float(1.0e-3);
const decimal REL_ERROR_SQUARE = REL_ERROR * REL_ERROR; const float REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
const int MAX_ITERATIONS_GJK_RAYCAST = 32; const int32_t MAX_ITERATIONS_GJK_RAYCAST = 32;
// Class GJKAlgorithm // Class GJKAlgorithm
/** /**
@ -49,9 +28,9 @@ const int MAX_ITERATIONS_GJK_RAYCAST = 32;
* "Collision Detection in Interactive 3D Environments" by Gino van den Bergen. * "Collision Detection in Interactive 3D Environments" by Gino van den Bergen.
* This method implements the Hybrid Technique for calculating the * This method implements the Hybrid Technique for calculating the
* penetration depth. The two objects are enlarged with a small margin. If * penetration depth. The two objects are enlarged with a small margin. If
* the object intersects in their margins, the penetration depth is quickly * the object int32_tersects in their margins, the penetration depth is quickly
* computed using the GJK algorithm on the original objects (without margin). * computed using the GJK algorithm on the original objects (without margin).
* If the original objects (without margin) intersect, we run again the GJK * If the original objects (without margin) int32_tersect, we run again the GJK
* algorithm on the enlarged objects (with margin) to compute simplex * algorithm on the enlarged objects (with margin) to compute simplex
* polytope that contains the origin and give it to the EPA (Expanding * polytope that contains the origin and give it to the EPA (Expanding
* Polytope Algorithm) to compute the correct penetration depth between the * Polytope Algorithm) to compute the correct penetration depth between the
@ -117,4 +96,3 @@ inline void GJKAlgorithm::init(CollisionDetection* collisionDetection,
} }
#endif

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

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/narrowphase/GJK/Simplex.h> #include <ephysics/collision/narrowphase/GJK/Simplex.h>
@ -40,7 +21,7 @@ Simplex::~Simplex() {
} }
// Add a new support point of (A-B) into the simplex // Add a new support point of (A-B) int32_to the simplex
/// suppPointA : support point of object A in a direction -v /// suppPointA : support point of object A in a direction -v
/// suppPointB : support point of object B in a direction v /// suppPointB : support point of object B in a direction v
/// point : support point of object (A-B) => point = suppPointA - suppPointB /// point : support point of object (A-B) => point = suppPointA - suppPointB
@ -59,7 +40,7 @@ void Simplex::addPoint(const Vector3& point, const Vector3& suppPointA, const Ve
assert(mLastFound < 4); assert(mLastFound < 4);
// Add the point into the simplex // Add the point int32_to the simplex
mPoints[mLastFound] = point; mPoints[mLastFound] = point;
mPointsLengthSquare[mLastFound] = point.dot(point); mPointsLengthSquare[mLastFound] = point.dot(point);
mAllBits = mBitsCurrentSimplex | mLastFoundBit; mAllBits = mBitsCurrentSimplex | mLastFoundBit;
@ -77,7 +58,7 @@ void Simplex::addPoint(const Vector3& point, const Vector3& suppPointA, const Ve
// Return true if the point is in the simplex // Return true if the point is in the simplex
bool Simplex::isPointInSimplex(const Vector3& point) const { bool Simplex::isPointInSimplex(const Vector3& point) const {
int i; int32_t i;
Bits bit; Bits bit;
// For each four possible points in the simplex // For each four possible points in the simplex
@ -91,7 +72,7 @@ bool Simplex::isPointInSimplex(const Vector3& point) const {
// Update the cached values used during the GJK algorithm // Update the cached values used during the GJK algorithm
void Simplex::updateCache() { void Simplex::updateCache() {
int i; int32_t i;
Bits bit; Bits bit;
// For each of the four possible points of the simplex // For each of the four possible points of the simplex
@ -112,10 +93,10 @@ void Simplex::updateCache() {
} }
// Return the points of the simplex // Return the points of the simplex
unsigned int Simplex::getSimplex(Vector3* suppPointsA, Vector3* suppPointsB, uint32_t Simplex::getSimplex(Vector3* suppPointsA, Vector3* suppPointsB,
Vector3* points) const { Vector3* points) const {
unsigned int nbVertices = 0; uint32_t nbVertices = 0;
int i; int32_t i;
Bits bit; Bits bit;
// For each four point in the possible simplex // For each four point in the possible simplex
@ -143,7 +124,7 @@ void Simplex::computeDeterminants() {
// If the current simplex is not empty // If the current simplex is not empty
if (!isEmpty()) { if (!isEmpty()) {
int i; int32_t i;
Bits bitI; Bits bitI;
// For each possible four points in the simplex set // For each possible four points in the simplex set
@ -157,12 +138,12 @@ void Simplex::computeDeterminants() {
mDet[bit2][mLastFound] = mDiffLength[i][mLastFound].dot(mPoints[i]); mDet[bit2][mLastFound] = mDiffLength[i][mLastFound].dot(mPoints[i]);
int j; int32_t j;
Bits bitJ; Bits bitJ;
for (j=0, bitJ = 0x1; j<i; j++, bitJ <<= 1) { for (j=0, bitJ = 0x1; j<i; j++, bitJ <<= 1) {
if (overlap(mBitsCurrentSimplex, bitJ)) { if (overlap(mBitsCurrentSimplex, bitJ)) {
int k; int32_t k;
Bits bit3 = bitJ | bit2; Bits bit3 = bitJ | bit2;
k = mNormSquare[i][j] < mNormSquare[mLastFound][j] ? i : mLastFound; k = mNormSquare[i][j] < mNormSquare[mLastFound][j] ? i : mLastFound;
@ -187,7 +168,7 @@ void Simplex::computeDeterminants() {
} }
if (mAllBits == 0xf) { if (mAllBits == 0xf) {
int k; int32_t k;
k = mNormSquare[1][0] < mNormSquare[2][0] ? k = mNormSquare[1][0] < mNormSquare[2][0] ?
(mNormSquare[1][0] < mNormSquare[3][0] ? 1 : 3) : (mNormSquare[1][0] < mNormSquare[3][0] ? 1 : 3) :
@ -224,7 +205,7 @@ void Simplex::computeDeterminants() {
/// A proper subset X is a subset where for all point "y_i" in X, we have /// A proper subset X is a subset where for all point "y_i" in X, we have
/// detX_i value bigger than zero /// detX_i value bigger than zero
bool Simplex::isProperSubset(Bits subset) const { bool Simplex::isProperSubset(Bits subset) const {
int i; int32_t i;
Bits bit; Bits bit;
// For each four point of the possible simplex set // For each four point of the possible simplex set
@ -241,8 +222,8 @@ bool Simplex::isProperSubset(Bits subset) const {
/// A set if affinely dependent if a point of the set /// A set if affinely dependent if a point of the set
/// is an affine combination of other points in the set /// is an affine combination of other points in the set
bool Simplex::isAffinelyDependent() const { bool Simplex::isAffinelyDependent() const {
decimal sum = 0.0; float sum = 0.0;
int i; int32_t i;
Bits bit; Bits bit;
// For each four point of the possible simplex set // For each four point of the possible simplex set
@ -260,7 +241,7 @@ bool Simplex::isAffinelyDependent() const {
/// 1. delta(X)_i > 0 for each "i" in I_x and /// 1. delta(X)_i > 0 for each "i" in I_x and
/// 2. delta(X U {y_j})_j <= 0 for each "j" not in I_x_ /// 2. delta(X U {y_j})_j <= 0 for each "j" not in I_x_
bool Simplex::isValidSubset(Bits subset) const { bool Simplex::isValidSubset(Bits subset) const {
int i; int32_t i;
Bits bit; Bits bit;
// For each four point in the possible simplex set // For each four point in the possible simplex set
@ -292,10 +273,10 @@ bool Simplex::isValidSubset(Bits subset) const {
/// pB = sum(lambda_i * b_i) where "b_i" are the support points of object B /// pB = sum(lambda_i * b_i) where "b_i" are the support points of object B
/// with lambda_i = deltaX_i / deltaX /// with lambda_i = deltaX_i / deltaX
void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const { void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {
decimal deltaX = 0.0; float deltaX = 0.0;
pA.setAllValues(0.0, 0.0, 0.0); pA.setAllValues(0.0, 0.0, 0.0);
pB.setAllValues(0.0, 0.0, 0.0); pB.setAllValues(0.0, 0.0, 0.0);
int i; int32_t i;
Bits bit; Bits bit;
// For each four points in the possible simplex set // For each four points in the possible simplex set
@ -309,7 +290,7 @@ void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {
} }
assert(deltaX > 0.0); assert(deltaX > 0.0);
decimal factor = decimal(1.0) / deltaX; float factor = float(1.0) / deltaX;
pA *= factor; pA *= factor;
pB *= factor; pB *= factor;
} }
@ -346,13 +327,13 @@ bool Simplex::computeClosestPoint(Vector3& v) {
// Backup the closest point // Backup the closest point
void Simplex::backupClosestPointInSimplex(Vector3& v) { void Simplex::backupClosestPointInSimplex(Vector3& v) {
decimal minDistSquare = DECIMAL_LARGEST; float minDistSquare = DECIMAL_LARGEST;
Bits bit; Bits bit;
for (bit = mAllBits; bit != 0x0; bit--) { for (bit = mAllBits; bit != 0x0; bit--) {
if (isSubset(bit, mAllBits) && isProperSubset(bit)) { if (isSubset(bit, mAllBits) && isProperSubset(bit)) {
Vector3 u = computeClosestPointForSubset(bit); Vector3 u = computeClosestPointForSubset(bit);
decimal distSquare = u.dot(u); float distSquare = u.dot(u);
if (distSquare < minDistSquare) { if (distSquare < minDistSquare) {
minDistSquare = distSquare; minDistSquare = distSquare;
mBitsCurrentSimplex = bit; mBitsCurrentSimplex = bit;
@ -367,8 +348,8 @@ void Simplex::backupClosestPointInSimplex(Vector3& v) {
Vector3 Simplex::computeClosestPointForSubset(Bits subset) { Vector3 Simplex::computeClosestPointForSubset(Bits subset) {
Vector3 v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i]) Vector3 v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i])
mMaxLengthSquare = 0.0; mMaxLengthSquare = 0.0;
decimal deltaX = 0.0; // deltaX = sum of all det[subset][i] float deltaX = 0.0; // deltaX = sum of all det[subset][i]
int i; int32_t i;
Bits bit; Bits bit;
// For each four point in the possible simplex set // For each four point in the possible simplex set
@ -390,5 +371,5 @@ Vector3 Simplex::computeClosestPointForSubset(Bits subset) {
assert(deltaX > 0.0); assert(deltaX > 0.0);
// Return the closet point "v" in the convex hull for the given subset // Return the closet point "v" in the convex hull for the given subset
return (decimal(1.0) / deltaX) * v; return (float(1.0) / deltaX) * v;
} }

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_SIMPLEX_H
#define REACTPHYSICS3D_SIMPLEX_H
// Libraries // Libraries
#include <ephysics/mathematics/mathematics.h> #include <ephysics/mathematics/mathematics.h>
@ -34,7 +13,7 @@
namespace reactphysics3d { namespace reactphysics3d {
// Type definitions // Type definitions
typedef unsigned int Bits; typedef uint32_t Bits;
// Class Simplex // Class Simplex
/** /**
@ -55,10 +34,10 @@ class Simplex {
Vector3 mPoints[4]; Vector3 mPoints[4];
/// pointsLengthSquare[i] = (points[i].length)^2 /// pointsLengthSquare[i] = (points[i].length)^2
decimal mPointsLengthSquare[4]; float mPointsLengthSquare[4];
/// Maximum length of pointsLengthSquare[i] /// Maximum length of pointsLengthSquare[i]
decimal mMaxLengthSquare; float mMaxLengthSquare;
/// Support points of object A in local coordinates /// Support points of object A in local coordinates
Vector3 mSuppPointsA[4]; Vector3 mSuppPointsA[4];
@ -70,10 +49,10 @@ class Simplex {
Vector3 mDiffLength[4][4]; Vector3 mDiffLength[4][4];
/// Cached determinant values /// Cached determinant values
decimal mDet[16][4]; float mDet[16][4];
/// norm[i][j] = (diff[i][j].length())^2 /// norm[i][j] = (diff[i][j].length())^2
decimal mNormSquare[4][4]; float mNormSquare[4][4];
/// 4 bits that identify the current points of the simplex /// 4 bits that identify the current points of the simplex
/// For instance, 0101 means that points[1] and points[3] are in the simplex /// For instance, 0101 means that points[1] and points[3] are in the simplex
@ -134,13 +113,13 @@ class Simplex {
bool isEmpty() const; bool isEmpty() const;
/// Return the points of the simplex /// Return the points of the simplex
unsigned int getSimplex(Vector3* mSuppPointsA, Vector3* mSuppPointsB, uint32_t getSimplex(Vector3* mSuppPointsA, Vector3* mSuppPointsB,
Vector3* mPoints) const; Vector3* mPoints) const;
/// Return the maximum squared length of a point /// Return the maximum squared length of a point
decimal getMaxLengthSquareOfAPoint() const; float getMaxLengthSquareOfAPoint() const;
/// Add a new support point of (A-B) into the simplex. /// 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 Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB);
/// Return true if the point is in the simplex /// Return true if the point is in the simplex
@ -180,10 +159,9 @@ inline bool Simplex::isEmpty() const {
} }
// Return the maximum squared length of a point // Return the maximum squared length of a point
inline decimal Simplex::getMaxLengthSquareOfAPoint() const { inline float Simplex::getMaxLengthSquareOfAPoint() const {
return mMaxLengthSquare; return mMaxLengthSquare;
} }
} }
#endif

29
ephysics/collision/narrowphase/NarrowPhaseAlgorithm.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h> #include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h>

35
ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_NARROW_PHASE_ALGORITHM_H
#define REACTPHYSICS3D_NARROW_PHASE_ALGORITHM_H
// Libraries // Libraries
#include <ephysics/body/Body.h> #include <ephysics/body/Body.h>
@ -112,6 +91,4 @@ inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* ove
} }
#endif

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

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/narrowphase/SphereVsSphereAlgorithm.h> #include <ephysics/collision/narrowphase/SphereVsSphereAlgorithm.h>
@ -31,7 +12,8 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
SphereVsSphereAlgorithm::SphereVsSphereAlgorithm() : NarrowPhaseAlgorithm() { SphereVsSphereAlgorithm::SphereVsSphereAlgorithm() :
NarrowPhaseAlgorithm() {
} }
@ -43,37 +25,31 @@ SphereVsSphereAlgorithm::~SphereVsSphereAlgorithm() {
void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info, const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) { NarrowPhaseCallback* narrowPhaseCallback) {
// Get the sphere collision shapes // Get the sphere collision shapes
const SphereShape* sphereShape1 = static_cast<const SphereShape*>(shape1Info.collisionShape); const SphereShape* sphereShape1 = static_cast<const SphereShape*>(shape1Info.collisionShape);
const SphereShape* sphereShape2 = static_cast<const SphereShape*>(shape2Info.collisionShape); const SphereShape* sphereShape2 = static_cast<const SphereShape*>(shape2Info.collisionShape);
// Get the local-space to world-space transforms // Get the local-space to world-space transforms
const Transform& transform1 = shape1Info.shapeToWorldTransform; const Transform& transform1 = shape1Info.shapeToWorldTransform;
const Transform& transform2 = shape2Info.shapeToWorldTransform; const Transform& transform2 = shape2Info.shapeToWorldTransform;
// Compute the distance between the centers // Compute the distance between the centers
Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare(); float squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
// Compute the sum of the radius // Compute the sum of the radius
decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius(); float sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius();
// If the sphere collision shapes int32_tersect
// If the sphere collision shapes intersect
if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) { if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
Vector3 intersectionOnBody1 = sphereShape1->getRadius() * Vector3 int32_tersectionOnBody1 = sphereShape1->getRadius() *
centerSphere2InBody1LocalSpace.getUnit(); centerSphere2InBody1LocalSpace.getUnit();
Vector3 intersectionOnBody2 = sphereShape2->getRadius() * Vector3 int32_tersectionOnBody2 = sphereShape2->getRadius() *
centerSphere1InBody2LocalSpace.getUnit(); centerSphere1InBody2LocalSpace.getUnit();
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); float penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
// Create the contact info object // Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, vectorBetweenCenters.getUnit(), penetrationDepth, shape2Info.collisionShape, vectorBetweenCenters.getUnit(), penetrationDepth,
intersectionOnBody1, intersectionOnBody2); int32_tersectionOnBody1, int32_tersectionOnBody2);
// Notify about the new contact // Notify about the new contact
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
} }

34
ephysics/collision/narrowphase/SphereVsSphereAlgorithm.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_SPHERE_VS_SPHERE_ALGORITHM_H
#define REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H
// Libraries // Libraries
#include <ephysics/body/Body.h> #include <ephysics/body/Body.h>
@ -70,5 +49,4 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
} }
#endif

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

@ -1,27 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/shapes/AABB.h> #include <ephysics/collision/shapes/AABB.h>
@ -114,7 +96,7 @@ AABB AABB::createAABBForTriangle(const Vector3* trianglePoints) {
return AABB(minCoords, maxCoords); return AABB(minCoords, maxCoords);
} }
// Return true if the ray intersects the AABB // Return true if the ray int32_tersects the AABB
/// This method use the line vs AABB raycasting technique described in /// This method use the line vs AABB raycasting technique described in
/// Real-time Collision Detection by Christer Ericson. /// Real-time Collision Detection by Christer Ericson.
bool AABB::testRayIntersect(const Ray& ray) const { bool AABB::testRayIntersect(const Ray& ray) const {
@ -125,16 +107,16 @@ bool AABB::testRayIntersect(const Ray& ray) const {
const Vector3 m = ray.point1 + point2 - mMinCoordinates - mMaxCoordinates; const Vector3 m = ray.point1 + point2 - mMinCoordinates - mMaxCoordinates;
// Test if the AABB face normals are separating axis // Test if the AABB face normals are separating axis
decimal adx = std::abs(d.x); float adx = std::abs(d.x);
if (std::abs(m.x) > e.x + adx) return false; if (std::abs(m.x) > e.x + adx) return false;
decimal ady = std::abs(d.y); float ady = std::abs(d.y);
if (std::abs(m.y) > e.y + ady) return false; if (std::abs(m.y) > e.y + ady) return false;
decimal adz = std::abs(d.z); float adz = std::abs(d.z);
if (std::abs(m.z) > e.z + adz) return false; if (std::abs(m.z) > e.z + adz) return false;
// Add in an epsilon term to counteract arithmetic errors when segment is // Add in an epsilon term to counteract arithmetic errors when segment is
// (near) parallel to a coordinate axis (see text for detail) // (near) parallel to a coordinate axis (see text for detail)
const decimal epsilon = 0.00001; const float epsilon = 0.00001;
adx += epsilon; adx += epsilon;
ady += epsilon; ady += epsilon;
adz += epsilon; adz += epsilon;

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_AABB_H
#define REACTPHYSICS3D_AABB_H
// Libraries // Libraries
#include <ephysics/mathematics/mathematics.h> #include <ephysics/mathematics/mathematics.h>
@ -86,13 +65,13 @@ class AABB {
Vector3 getExtent() const; Vector3 getExtent() const;
/// Inflate each side of the AABB by a given size /// Inflate each side of the AABB by a given size
void inflate(decimal dx, decimal dy, decimal dz); void inflate(float dx, float dy, float dz);
/// Return true if the current AABB is overlapping with the AABB in argument /// Return true if the current AABB is overlapping with the AABB in argument
bool testCollision(const AABB& aabb) const; bool testCollision(const AABB& aabb) const;
/// Return the volume of the AABB /// Return the volume of the AABB
decimal getVolume() const; float getVolume() const;
/// Merge the AABB in parameter with the current one /// Merge the AABB in parameter with the current one
void mergeWithAABB(const AABB& aabb); void mergeWithAABB(const AABB& aabb);
@ -106,10 +85,10 @@ class AABB {
/// Return true if a point is inside the AABB /// Return true if a point is inside the AABB
bool contains(const Vector3& point) const; bool contains(const Vector3& point) const;
/// Return true if the AABB of a triangle intersects the AABB /// Return true if the AABB of a triangle int32_tersects the AABB
bool testCollisionTriangleAABB(const Vector3* trianglePoints) const; bool testCollisionTriangleAABB(const Vector3* trianglePoints) const;
/// Return true if the ray intersects the AABB /// Return true if the ray int32_tersects the AABB
bool testRayIntersect(const Ray& ray) const; bool testRayIntersect(const Ray& ray) const;
/// Create and return an AABB for a triangle /// Create and return an AABB for a triangle
@ -125,7 +104,7 @@ class AABB {
// Return the center point of the AABB in world coordinates // Return the center point of the AABB in world coordinates
inline Vector3 AABB::getCenter() const { inline Vector3 AABB::getCenter() const {
return (mMinCoordinates + mMaxCoordinates) * decimal(0.5); return (mMinCoordinates + mMaxCoordinates) * float(0.5);
} }
// Return the minimum coordinates of the AABB // Return the minimum coordinates of the AABB
@ -154,7 +133,7 @@ inline Vector3 AABB::getExtent() const {
} }
// Inflate each side of the AABB by a given size // Inflate each side of the AABB by a given size
inline void AABB::inflate(decimal dx, decimal dy, decimal dz) { inline void AABB::inflate(float dx, float dy, float dz) {
mMaxCoordinates += Vector3(dx, dy, dz); mMaxCoordinates += Vector3(dx, dy, dz);
mMinCoordinates -= Vector3(dx, dy, dz); mMinCoordinates -= Vector3(dx, dy, dz);
} }
@ -172,12 +151,12 @@ inline bool AABB::testCollision(const AABB& aabb) const {
} }
// Return the volume of the AABB // Return the volume of the AABB
inline decimal AABB::getVolume() const { inline float AABB::getVolume() const {
const Vector3 diff = mMaxCoordinates - mMinCoordinates; const Vector3 diff = mMaxCoordinates - mMinCoordinates;
return (diff.x * diff.y * diff.z); return (diff.x * diff.y * diff.z);
} }
// Return true if the AABB of a triangle intersects the AABB // 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 Vector3* trianglePoints) const {
if (min3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) > mMaxCoordinates.x) return false; if (min3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) > mMaxCoordinates.x) return false;
@ -209,5 +188,3 @@ inline AABB& AABB::operator=(const AABB& aabb) {
} }
} }
#endif

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

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2015 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/shapes/BoxShape.h> #include <ephysics/collision/shapes/BoxShape.h>
@ -37,11 +18,11 @@ using namespace reactphysics3d;
* @param extent The vector with the three extents of the box (in meters) * @param extent The vector with the three extents of the box (in meters)
* @param margin The collision margin (in meters) around the collision shape * @param margin The collision margin (in meters) around the collision shape
*/ */
BoxShape::BoxShape(const Vector3& extent, decimal margin) BoxShape::BoxShape(const Vector3& extent, float margin)
: ConvexShape(BOX, margin), mExtent(extent - Vector3(margin, margin, margin)) { : ConvexShape(BOX, margin), mExtent(extent - Vector3(margin, margin, margin)) {
assert(extent.x > decimal(0.0) && extent.x > margin); assert(extent.x > float(0.0) && extent.x > margin);
assert(extent.y > decimal(0.0) && extent.y > margin); assert(extent.y > float(0.0) && extent.y > margin);
assert(extent.z > decimal(0.0) && extent.z > margin); assert(extent.z > float(0.0) && extent.z > margin);
} }
// Destructor // Destructor
@ -55,12 +36,12 @@ BoxShape::~BoxShape() {
* coordinates * coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape * @param mass Mass to use to compute the inertia tensor of the collision shape
*/ */
void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
decimal factor = (decimal(1.0) / decimal(3.0)) * mass; float factor = (float(1.0) / float(3.0)) * mass;
Vector3 realExtent = mExtent + Vector3(mMargin, mMargin, mMargin); Vector3 realExtent = mExtent + Vector3(mMargin, mMargin, mMargin);
decimal xSquare = realExtent.x * realExtent.x; float xSquare = realExtent.x * realExtent.x;
decimal ySquare = realExtent.y * realExtent.y; float ySquare = realExtent.y * realExtent.y;
decimal zSquare = realExtent.z * realExtent.z; float zSquare = realExtent.z * realExtent.z;
tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0, tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0,
0.0, factor * (xSquare + zSquare), 0.0, 0.0, factor * (xSquare + zSquare), 0.0,
0.0, 0.0, factor * (xSquare + ySquare)); 0.0, 0.0, factor * (xSquare + ySquare));
@ -70,13 +51,13 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const
bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
Vector3 rayDirection = ray.point2 - ray.point1; Vector3 rayDirection = ray.point2 - ray.point1;
decimal tMin = DECIMAL_SMALLEST; float tMin = DECIMAL_SMALLEST;
decimal tMax = DECIMAL_LARGEST; float tMax = DECIMAL_LARGEST;
Vector3 normalDirection(decimal(0), decimal(0), decimal(0)); Vector3 normalDirection(float(0), float(0), float(0));
Vector3 currentNormal; Vector3 currentNormal;
// For each of the three slabs // For each of the three slabs
for (int i=0; i<3; i++) { for (int32_t i=0; i<3; i++) {
// If ray is parallel to the slab // If ray is parallel to the slab
if (std::abs(rayDirection[i]) < MACHINE_EPSILON) { if (std::abs(rayDirection[i]) < MACHINE_EPSILON) {
@ -86,22 +67,22 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro
} }
else { else {
// Compute the intersection of the ray with the near and far plane of the slab // Compute the int32_tersection of the ray with the near and far plane of the slab
decimal oneOverD = decimal(1.0) / rayDirection[i]; float oneOverD = float(1.0) / rayDirection[i];
decimal t1 = (-mExtent[i] - ray.point1[i]) * oneOverD; float t1 = (-mExtent[i] - ray.point1[i]) * oneOverD;
decimal t2 = (mExtent[i] - ray.point1[i]) * oneOverD; float t2 = (mExtent[i] - ray.point1[i]) * oneOverD;
currentNormal[0] = (i == 0) ? -mExtent[i] : decimal(0.0); currentNormal[0] = (i == 0) ? -mExtent[i] : float(0.0);
currentNormal[1] = (i == 1) ? -mExtent[i] : decimal(0.0); currentNormal[1] = (i == 1) ? -mExtent[i] : float(0.0);
currentNormal[2] = (i == 2) ? -mExtent[i] : decimal(0.0); currentNormal[2] = (i == 2) ? -mExtent[i] : float(0.0);
// Swap t1 and t2 if need so that t1 is intersection with near plane and // Swap t1 and t2 if need so that t1 is int32_tersection with near plane and
// t2 with far plane // t2 with far plane
if (t1 > t2) { if (t1 > t2) {
std::swap(t1, t2); std::swap(t1, t2);
currentNormal = -currentNormal; currentNormal = -currentNormal;
} }
// Compute the intersection of the of slab intersection interval with previous slabs // Compute the int32_tersection of the of slab int32_tersection int32_terval with previous slabs
if (t1 > tMin) { if (t1 > tMin) {
tMin = t1; tMin = t1;
normalDirection = currentNormal; normalDirection = currentNormal;
@ -111,15 +92,15 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro
// If tMin is larger than the maximum raycasting fraction, we return no hit // If tMin is larger than the maximum raycasting fraction, we return no hit
if (tMin > ray.maxFraction) return false; if (tMin > ray.maxFraction) return false;
// If the slabs intersection is empty, there is no hit // If the slabs int32_tersection is empty, there is no hit
if (tMin > tMax) return false; if (tMin > tMax) return false;
} }
} }
// If tMin is negative, we return no hit // If tMin is negative, we return no hit
if (tMin < decimal(0.0) || tMin > ray.maxFraction) return false; if (tMin < float(0.0) || tMin > ray.maxFraction) return false;
// The ray intersects the three slabs, we compute the hit point // The ray int32_tersects the three slabs, we compute the hit point
Vector3 localHitPoint = ray.point1 + tMin * rayDirection; Vector3 localHitPoint = ray.point1 + tMin * rayDirection;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_BOX_SHAPE_H
#define REACTPHYSICS3D_BOX_SHAPE_H
// Libraries // Libraries
#include <cfloat> #include <cfloat>
@ -85,7 +64,7 @@ class BoxShape : public ConvexShape {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
BoxShape(const Vector3& extent, decimal margin = OBJECT_MARGIN); BoxShape(const Vector3& extent, float margin = OBJECT_MARGIN);
/// Destructor /// Destructor
virtual ~BoxShape(); virtual ~BoxShape();
@ -100,7 +79,7 @@ class BoxShape : public ConvexShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const; virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// Return the local inertia tensor of the collision shape /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
}; };
// Return the extents of the box // Return the extents of the box
@ -156,5 +135,3 @@ inline bool BoxShape::testPointInside(const Vector3& localPoint, ProxyShape* pro
} }
} }
#endif

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

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/shapes/CapsuleShape.h> #include <ephysics/collision/shapes/CapsuleShape.h>
@ -36,10 +17,10 @@ using namespace reactphysics3d;
* @param radius The radius of the capsule (in meters) * @param radius The radius of the capsule (in meters)
* @param height The height of the capsule (in meters) * @param height The height of the capsule (in meters)
*/ */
CapsuleShape::CapsuleShape(decimal radius, decimal height) CapsuleShape::CapsuleShape(float radius, float height)
: ConvexShape(CAPSULE, radius), mHalfHeight(height * decimal(0.5)) { : ConvexShape(CAPSULE, radius), mHalfHeight(height * float(0.5)) {
assert(radius > decimal(0.0)); assert(radius > float(0.0));
assert(height > decimal(0.0)); assert(height > float(0.0));
} }
// Destructor // Destructor
@ -53,21 +34,21 @@ CapsuleShape::~CapsuleShape() {
* coordinates * coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape * @param mass Mass to use to compute the inertia tensor of the collision shape
*/ */
void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
// The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1 // The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1
decimal height = mHalfHeight + mHalfHeight; float height = mHalfHeight + mHalfHeight;
decimal radiusSquare = mMargin * mMargin; float radiusSquare = mMargin * mMargin;
decimal heightSquare = height * height; float heightSquare = height * height;
decimal radiusSquareDouble = radiusSquare + radiusSquare; float radiusSquareDouble = radiusSquare + radiusSquare;
decimal factor1 = decimal(2.0) * mMargin / (decimal(4.0) * mMargin + decimal(3.0) * height); float factor1 = float(2.0) * mMargin / (float(4.0) * mMargin + float(3.0) * height);
decimal factor2 = decimal(3.0) * height / (decimal(4.0) * mMargin + decimal(3.0) * height); float factor2 = float(3.0) * height / (float(4.0) * mMargin + float(3.0) * height);
decimal sum1 = decimal(0.4) * radiusSquareDouble; float sum1 = float(0.4) * radiusSquareDouble;
decimal sum2 = decimal(0.75) * height * mMargin + decimal(0.5) * heightSquare; float sum2 = float(0.75) * height * mMargin + float(0.5) * heightSquare;
decimal sum3 = decimal(0.25) * radiusSquare + decimal(1.0 / 12.0) * heightSquare; float sum3 = float(0.25) * radiusSquare + float(1.0 / 12.0) * heightSquare;
decimal IxxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3; float IxxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3;
decimal Iyy = factor1 * mass * sum1 + factor2 * mass * decimal(0.25) * radiusSquareDouble; float Iyy = factor1 * mass * sum1 + factor2 * mass * float(0.25) * radiusSquareDouble;
tensor.setAllValues(IxxAndzz, 0.0, 0.0, tensor.setAllValues(IxxAndzz, 0.0, 0.0,
0.0, Iyy, 0.0, 0.0, Iyy, 0.0,
0.0, 0.0, IxxAndzz); 0.0, 0.0, IxxAndzz);
@ -76,11 +57,11 @@ void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) co
// Return true if a point is inside the collision shape // Return true if a point is inside the collision shape
bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
const decimal diffYCenterSphere1 = localPoint.y - mHalfHeight; const float diffYCenterSphere1 = localPoint.y - mHalfHeight;
const decimal diffYCenterSphere2 = localPoint.y + mHalfHeight; const float diffYCenterSphere2 = localPoint.y + mHalfHeight;
const decimal xSquare = localPoint.x * localPoint.x; const float xSquare = localPoint.x * localPoint.x;
const decimal zSquare = localPoint.z * localPoint.z; const float zSquare = localPoint.z * localPoint.z;
const decimal squareRadius = mMargin * mMargin; const float squareRadius = mMargin * mMargin;
// Return true if the point is inside the cylinder or one of the two spheres of the capsule // Return true if the point is inside the cylinder or one of the two spheres of the capsule
return ((xSquare + zSquare) < squareRadius && return ((xSquare + zSquare) < squareRadius &&
@ -94,44 +75,44 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
const Vector3 n = ray.point2 - ray.point1; const Vector3 n = ray.point2 - ray.point1;
const decimal epsilon = decimal(0.01); const float epsilon = float(0.01);
Vector3 p(decimal(0), -mHalfHeight, decimal(0)); Vector3 p(float(0), -mHalfHeight, float(0));
Vector3 q(decimal(0), mHalfHeight, decimal(0)); Vector3 q(float(0), mHalfHeight, float(0));
Vector3 d = q - p; Vector3 d = q - p;
Vector3 m = ray.point1 - p; Vector3 m = ray.point1 - p;
decimal t; float t;
decimal mDotD = m.dot(d); float mDotD = m.dot(d);
decimal nDotD = n.dot(d); float nDotD = n.dot(d);
decimal dDotD = d.dot(d); float dDotD = d.dot(d);
// Test if the segment is outside the cylinder // Test if the segment is outside the cylinder
decimal vec1DotD = (ray.point1 - Vector3(decimal(0.0), -mHalfHeight - mMargin, decimal(0.0))).dot(d); float vec1DotD = (ray.point1 - Vector3(float(0.0), -mHalfHeight - mMargin, float(0.0))).dot(d);
if (vec1DotD < decimal(0.0) && vec1DotD + nDotD < decimal(0.0)) return false; if (vec1DotD < float(0.0) && vec1DotD + nDotD < float(0.0)) return false;
decimal ddotDExtraCaps = decimal(2.0) * mMargin * d.y; float ddotDExtraCaps = float(2.0) * mMargin * d.y;
if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) return false; if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) return false;
decimal nDotN = n.dot(n); float nDotN = n.dot(n);
decimal mDotN = m.dot(n); float mDotN = m.dot(n);
decimal a = dDotD * nDotN - nDotD * nDotD; float a = dDotD * nDotN - nDotD * nDotD;
decimal k = m.dot(m) - mMargin * mMargin; float k = m.dot(m) - mMargin * mMargin;
decimal c = dDotD * k - mDotD * mDotD; float c = dDotD * k - mDotD * mDotD;
// If the ray is parallel to the capsule axis // If the ray is parallel to the capsule axis
if (std::abs(a) < epsilon) { if (std::abs(a) < epsilon) {
// If the origin is outside the surface of the capusle's cylinder, we return no hit // If the origin is outside the surface of the capusle's cylinder, we return no hit
if (c > decimal(0.0)) return false; if (c > float(0.0)) return false;
// Here we know that the segment intersect an endcap of the capsule // Here we know that the segment int32_tersect an endcap of the capsule
// If the ray intersects with the "p" endcap of the capsule // If the ray int32_tersects with the "p" endcap of the capsule
if (mDotD < decimal(0.0)) { if (mDotD < float(0.0)) {
// Check intersection between the ray and the "p" sphere endcap of the capsule // Check int32_tersection between the ray and the "p" sphere endcap of the capsule
Vector3 hitLocalPoint; Vector3 hitLocalPoint;
decimal hitFraction; float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
@ -145,11 +126,11 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
return false; return false;
} }
else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder else if (mDotD > dDotD) { // If the ray int32_tersects with the "q" endcap of the cylinder
// Check intersection between the ray and the "q" sphere endcap of the capsule // Check int32_tersection between the ray and the "q" sphere endcap of the capsule
Vector3 hitLocalPoint; Vector3 hitLocalPoint;
decimal hitFraction; float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
@ -167,22 +148,22 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
return false; return false;
} }
} }
decimal b = dDotD * mDotN - nDotD * mDotD; float b = dDotD * mDotN - nDotD * mDotD;
decimal discriminant = b * b - a * c; float discriminant = b * b - a * c;
// If the discriminant is negative, no real roots and therfore, no hit // If the discriminant is negative, no real roots and therfore, no hit
if (discriminant < decimal(0.0)) return false; if (discriminant < float(0.0)) return false;
// Compute the smallest root (first intersection along the ray) // Compute the smallest root (first int32_tersection along the ray)
decimal t0 = t = (-b - std::sqrt(discriminant)) / a; float t0 = t = (-b - std::sqrt(discriminant)) / a;
// If the intersection is outside the finite cylinder of the capsule on "p" endcap side // If the int32_tersection is outside the finite cylinder of the capsule on "p" endcap side
decimal value = mDotD + t * nDotD; float value = mDotD + t * nDotD;
if (value < decimal(0.0)) { if (value < float(0.0)) {
// Check intersection between the ray and the "p" sphere endcap of the capsule // Check int32_tersection between the ray and the "p" sphere endcap of the capsule
Vector3 hitLocalPoint; Vector3 hitLocalPoint;
decimal hitFraction; float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
@ -196,11 +177,11 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
return false; return false;
} }
else if (value > dDotD) { // If the intersection is outside the finite cylinder on the "q" side else if (value > dDotD) { // If the int32_tersection is outside the finite cylinder on the "q" side
// Check intersection between the ray and the "q" sphere endcap of the capsule // Check int32_tersection between the ray and the "q" sphere endcap of the capsule
Vector3 hitLocalPoint; Vector3 hitLocalPoint;
decimal hitFraction; float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
@ -217,9 +198,9 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
t = t0; t = t0;
// If the intersection is behind the origin of the ray or beyond the maximum // If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit // raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false; if (t < float(0.0) || t > ray.maxFraction) return false;
// Compute the hit information // Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n; Vector3 localHitPoint = ray.point1 + t * n;
@ -237,39 +218,39 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
// Raycasting method between a ray one of the two spheres end cap of the capsule // Raycasting method between a ray one of the two spheres end cap of the capsule
bool CapsuleShape::raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2, bool CapsuleShape::raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,
const Vector3& sphereCenter, decimal maxFraction, const Vector3& sphereCenter, float maxFraction,
Vector3& hitLocalPoint, decimal& hitFraction) const { Vector3& hitLocalPoint, float& hitFraction) const {
const Vector3 m = point1 - sphereCenter; const Vector3 m = point1 - sphereCenter;
decimal c = m.dot(m) - mMargin * mMargin; float c = m.dot(m) - mMargin * mMargin;
// If the origin of the ray is inside the sphere, we return no intersection // If the origin of the ray is inside the sphere, we return no int32_tersection
if (c < decimal(0.0)) return false; if (c < float(0.0)) return false;
const Vector3 rayDirection = point2 - point1; const Vector3 rayDirection = point2 - point1;
decimal b = m.dot(rayDirection); float b = m.dot(rayDirection);
// If the origin of the ray is outside the sphere and the ray // If the origin of the ray is outside the sphere and the ray
// is pointing away from the sphere, there is no intersection // is pointing away from the sphere, there is no int32_tersection
if (b > decimal(0.0)) return false; if (b > float(0.0)) return false;
decimal raySquareLength = rayDirection.lengthSquare(); float raySquareLength = rayDirection.lengthSquare();
// Compute the discriminant of the quadratic equation // Compute the discriminant of the quadratic equation
decimal discriminant = b * b - raySquareLength * c; float discriminant = b * b - raySquareLength * c;
// If the discriminant is negative or the ray length is very small, there is no intersection // If the discriminant is negative or the ray length is very small, there is no int32_tersection
if (discriminant < decimal(0.0) || raySquareLength < MACHINE_EPSILON) return false; if (discriminant < float(0.0) || raySquareLength < MACHINE_EPSILON) return false;
// Compute the solution "t" closest to the origin // Compute the solution "t" closest to the origin
decimal t = -b - std::sqrt(discriminant); float t = -b - std::sqrt(discriminant);
assert(t >= decimal(0.0)); assert(t >= float(0.0));
// If the hit point is withing the segment ray fraction // If the hit point is withing the segment ray fraction
if (t < maxFraction * raySquareLength) { if (t < maxFraction * raySquareLength) {
// Compute the intersection information // Compute the int32_tersection information
t /= raySquareLength; t /= raySquareLength;
hitFraction = t; hitFraction = t;
hitLocalPoint = point1 + t * rayDirection; hitLocalPoint = point1 + t * rayDirection;

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CAPSULE_SHAPE_H
#define REACTPHYSICS3D_CAPSULE_SHAPE_H
// Libraries // Libraries
#include <ephysics/collision/shapes/ConvexShape.h> #include <ephysics/collision/shapes/ConvexShape.h>
@ -51,7 +30,7 @@ class CapsuleShape : public ConvexShape {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Half height of the capsule (height = distance between the centers of the two spheres) /// Half height of the capsule (height = distance between the centers of the two spheres)
decimal mHalfHeight; float mHalfHeight;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -73,8 +52,8 @@ class CapsuleShape : public ConvexShape {
/// Raycasting method between a ray one of the two spheres end cap of the capsule /// Raycasting method between a ray one of the two spheres end cap of the capsule
bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2, bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,
const Vector3& sphereCenter, decimal maxFraction, const Vector3& sphereCenter, float maxFraction,
Vector3& hitLocalPoint, decimal& hitFraction) const; Vector3& hitLocalPoint, float& hitFraction) const;
/// Return the number of bytes used by the collision shape /// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
@ -84,16 +63,16 @@ class CapsuleShape : public ConvexShape {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
CapsuleShape(decimal radius, decimal height); CapsuleShape(float radius, float height);
/// Destructor /// Destructor
virtual ~CapsuleShape(); virtual ~CapsuleShape();
/// Return the radius of the capsule /// Return the radius of the capsule
decimal getRadius() const; float getRadius() const;
/// Return the height of the capsule /// Return the height of the capsule
decimal getHeight() const; float getHeight() const;
/// Set the scaling vector of the collision shape /// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling); virtual void setLocalScaling(const Vector3& scaling);
@ -102,14 +81,14 @@ class CapsuleShape : public ConvexShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const; virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// Return the local inertia tensor of the collision shape /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
}; };
// Get the radius of the capsule // Get the radius of the capsule
/** /**
* @return The radius of the capsule shape (in meters) * @return The radius of the capsule shape (in meters)
*/ */
inline decimal CapsuleShape::getRadius() const { inline float CapsuleShape::getRadius() const {
return mMargin; return mMargin;
} }
@ -117,7 +96,7 @@ inline decimal CapsuleShape::getRadius() const {
/** /**
* @return The height of the capsule shape (in meters) * @return The height of the capsule shape (in meters)
*/ */
inline decimal CapsuleShape::getHeight() const { inline float CapsuleShape::getHeight() const {
return mHalfHeight + mHalfHeight; return mHalfHeight + mHalfHeight;
} }
@ -165,10 +144,10 @@ inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& di
void** cachedCollisionData) const { void** cachedCollisionData) const {
// Support point top sphere // Support point top sphere
decimal dotProductTop = mHalfHeight * direction.y; float dotProductTop = mHalfHeight * direction.y;
// Support point bottom sphere // Support point bottom sphere
decimal dotProductBottom = -mHalfHeight * direction.y; float dotProductBottom = -mHalfHeight * direction.y;
// Return the point with the maximum dot product // Return the point with the maximum dot product
if (dotProductTop > dotProductBottom) { if (dotProductTop > dotProductBottom) {
@ -180,5 +159,3 @@ inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& di
} }
} }
#endif

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

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/shapes/CollisionShape.h> #include <ephysics/collision/shapes/CollisionShape.h>

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_COLLISION_SHAPE_H
#define REACTPHYSICS3D_COLLISION_SHAPE_H
// Libraries // Libraries
#include <cassert> #include <cassert>
@ -42,7 +21,7 @@ namespace reactphysics3d {
/// Type of the collision shape /// Type of the collision shape
enum CollisionShapeType {TRIANGLE, BOX, SPHERE, CONE, CYLINDER, enum CollisionShapeType {TRIANGLE, BOX, SPHERE, CONE, CYLINDER,
CAPSULE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD}; CAPSULE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD};
const int NB_COLLISION_SHAPE_TYPES = 9; const int32_t NB_COLLISION_SHAPE_TYPES = 9;
// Declarations // Declarations
class ProxyShape; class ProxyShape;
@ -108,7 +87,7 @@ class CollisionShape {
virtual void setLocalScaling(const Vector3& scaling); virtual void setLocalScaling(const Vector3& scaling);
/// Return the local inertia tensor of the collision shapes /// Return the local inertia tensor of the collision shapes
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const=0;
/// Compute the world-space AABB of the collision shape given a transform /// 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 Transform& transform) const;
@ -117,7 +96,7 @@ class CollisionShape {
static bool isConvex(CollisionShapeType shapeType); static bool isConvex(CollisionShapeType shapeType);
/// Return the maximum number of contact manifolds in an overlapping pair given two shape types /// Return the maximum number of contact manifolds in an overlapping pair given two shape types
static int computeNbMaxContactManifolds(CollisionShapeType shapeType1, static int32_t computeNbMaxContactManifolds(CollisionShapeType shapeType1,
CollisionShapeType shapeType2); CollisionShapeType shapeType2);
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
@ -151,7 +130,7 @@ inline void CollisionShape::setLocalScaling(const Vector3& scaling) {
// Return the maximum number of contact manifolds allowed in an overlapping // Return the maximum number of contact manifolds allowed in an overlapping
// pair wit the given two collision shape types // pair wit the given two collision shape types
inline int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1, inline int32_t CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1,
CollisionShapeType shapeType2) { CollisionShapeType shapeType2) {
// If both shapes are convex // If both shapes are convex
if (isConvex(shapeType1) && isConvex(shapeType2)) { if (isConvex(shapeType1) && isConvex(shapeType2)) {
@ -164,4 +143,3 @@ inline int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shape
} }
#endif

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

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/shapes/ConcaveMeshShape.h> #include <ephysics/collision/shapes/ConcaveMeshShape.h>
@ -34,7 +15,7 @@ ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh):
ConcaveShape(CONCAVE_MESH) { ConcaveShape(CONCAVE_MESH) {
m_triangleMesh = triangleMesh; m_triangleMesh = triangleMesh;
m_raycastTestType = FRONT; m_raycastTestType = FRONT;
// Insert all the triangles into the dynamic AABB tree // Insert all the triangles int32_to the dynamic AABB tree
initBVHTree(); initBVHTree();
} }
@ -43,29 +24,29 @@ ConcaveMeshShape::~ConcaveMeshShape() {
} }
// Insert all the triangles into the dynamic AABB tree // Insert all the triangles int32_to the dynamic AABB tree
void ConcaveMeshShape::initBVHTree() { void ConcaveMeshShape::initBVHTree() {
// TODO : Try to randomly add the triangles into the tree to obtain a better tree // TODO : Try to randomly add the triangles int32_to the tree to obtain a better tree
// For each sub-part of the mesh // For each sub-part of the mesh
for (uint subPart=0; subPart<m_triangleMesh->getNbSubparts(); subPart++) { for (uint32_t subPart=0; subPart<m_triangleMesh->getNbSubparts(); subPart++) {
// Get the triangle vertex array of the current sub-part // Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(subPart); TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(subPart);
TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType(); TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
unsigned char* verticesStart = triangleVertexArray->getVerticesStart(); unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
unsigned char* indicesStart = triangleVertexArray->getIndicesStart(); unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
int vertexStride = triangleVertexArray->getVerticesStride(); int32_t vertexStride = triangleVertexArray->getVerticesStride();
int indexStride = triangleVertexArray->getIndicesStride(); int32_t indexStride = triangleVertexArray->getIndicesStride();
// For each triangle of the concave mesh // For each triangle of the concave mesh
for (uint triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) { for (uint32_t triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) {
void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride); void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
Vector3 trianglePoints[3]; Vector3 trianglePoints[3];
// For each vertex of the triangle // For each vertex of the triangle
for (int k=0; k < 3; k++) { for (int32_t k=0; k < 3; k++) {
// Get the index of the current vertex in the triangle // Get the index of the current vertex in the triangle
int vertexIndex = 0; int32_t vertexIndex = 0;
if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) { if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
vertexIndex = ((uint*)vertexIndexPointer)[k]; vertexIndex = ((uint32_t*)vertexIndexPointer)[k];
} else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) { } else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
vertexIndex = ((unsigned short*)vertexIndexPointer)[k]; vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
} else { } else {
@ -74,14 +55,14 @@ void ConcaveMeshShape::initBVHTree() {
// Get the vertices components of the triangle // Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) { if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride); const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x; trianglePoints[k][0] = float(vertices[0]) * mScaling.x;
trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y; trianglePoints[k][1] = float(vertices[1]) * mScaling.y;
trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z; trianglePoints[k][2] = float(vertices[2]) * mScaling.z;
} else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) { } else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride); const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x; trianglePoints[k][0] = float(vertices[0]) * mScaling.x;
trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y; trianglePoints[k][1] = float(vertices[1]) * mScaling.y;
trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z; trianglePoints[k][2] = float(vertices[2]) * mScaling.z;
} else { } else {
assert(false); assert(false);
} }
@ -89,7 +70,7 @@ void ConcaveMeshShape::initBVHTree() {
// Create the AABB for the triangle // Create the AABB for the triangle
AABB aabb = AABB::createAABBForTriangle(trianglePoints); AABB aabb = AABB::createAABBForTriangle(trianglePoints);
aabb.inflate(m_triangleMargin, m_triangleMargin, m_triangleMargin); aabb.inflate(m_triangleMargin, m_triangleMargin, m_triangleMargin);
// Add the AABB with the index of the triangle into the dynamic AABB tree // Add the AABB with the index of the triangle int32_to the dynamic AABB tree
m_dynamicAABBTree.addObject(aabb, subPart, triangleIndex); m_dynamicAABBTree.addObject(aabb, subPart, triangleIndex);
} }
} }
@ -97,7 +78,7 @@ void ConcaveMeshShape::initBVHTree() {
// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle // Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
// given the start vertex index pointer of the triangle // given the start vertex index pointer of the triangle
void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex, Vector3* outTriangleVertices) const { void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32_t subPart, int32_t triangleIndex, Vector3* outTriangleVertices) const {
// Get the triangle vertex array of the current sub-part // Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(subPart); TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(subPart);
if (triangleVertexArray == nullptr) { if (triangleVertexArray == nullptr) {
@ -107,15 +88,15 @@ void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32
TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
unsigned char* verticesStart = triangleVertexArray->getVerticesStart(); unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
unsigned char* indicesStart = triangleVertexArray->getIndicesStart(); unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
int vertexStride = triangleVertexArray->getVerticesStride(); int32_t vertexStride = triangleVertexArray->getVerticesStride();
int indexStride = triangleVertexArray->getIndicesStride(); int32_t indexStride = triangleVertexArray->getIndicesStride();
void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride); void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
// For each vertex of the triangle // For each vertex of the triangle
for (int k=0; k < 3; k++) { for (int32_t k=0; k < 3; k++) {
// Get the index of the current vertex in the triangle // Get the index of the current vertex in the triangle
int vertexIndex = 0; int32_t vertexIndex = 0;
if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) { if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
vertexIndex = ((uint*)vertexIndexPointer)[k]; vertexIndex = ((uint32_t*)vertexIndexPointer)[k];
} else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) { } else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
vertexIndex = ((unsigned short*)vertexIndexPointer)[k]; vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
} else { } else {
@ -125,14 +106,14 @@ void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32
// Get the vertices components of the triangle // Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) { if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride); const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x; outTriangleVertices[k][0] = float(vertices[0]) * mScaling.x;
outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y; outTriangleVertices[k][1] = float(vertices[1]) * mScaling.y;
outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z; outTriangleVertices[k][2] = float(vertices[2]) * mScaling.z;
} else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) { } else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride); const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x; outTriangleVertices[k][0] = float(vertices[0]) * mScaling.x;
outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y; outTriangleVertices[k][1] = float(vertices[1]) * mScaling.y;
outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z; outTriangleVertices[k][2] = float(vertices[2]) * mScaling.z;
} else { } else {
assert(false); assert(false);
} }
@ -163,24 +144,24 @@ bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh
} }
// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree // Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) { float ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32_t nodeId, const Ray& ray) {
// Add the id of the hit AABB node into // Add the id of the hit AABB node int32_to
m_hitAABBNodes.push_back(nodeId); m_hitAABBNodes.push_back(nodeId);
return ray.maxFraction; return ray.maxFraction;
} }
// Raycast all collision shapes that have been collected // Raycast all collision shapes that have been collected
void ConcaveMeshRaycastCallback::raycastTriangles() { void ConcaveMeshRaycastCallback::raycastTriangles() {
std::vector<int>::const_iterator it; std::vector<int32_t>::const_iterator it;
decimal smallestHitFraction = m_ray.maxFraction; float smallestHitFraction = m_ray.maxFraction;
for (it = m_hitAABBNodes.begin(); it != m_hitAABBNodes.end(); ++it) { for (it = m_hitAABBNodes.begin(); it != m_hitAABBNodes.end(); ++it) {
// Get the node data (triangle index and mesh subpart index) // Get the node data (triangle index and mesh subpart index)
int32* data = m_dynamicAABBTree.getNodeDataInt(*it); int32_t* data = m_dynamicAABBTree.getNodeDataInt(*it);
// Get the triangle vertices for this node from the concave mesh shape // Get the triangle vertices for this node from the concave mesh shape
Vector3 trianglePoints[3]; Vector3 trianglePoints[3];
m_concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints); m_concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
// Create a triangle collision shape // Create a triangle collision shape
decimal margin = m_concaveMeshShape.getTriangleMargin(); float margin = m_concaveMeshShape.getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
triangleShape.setRaycastTestType(m_concaveMeshShape.getRaycastTestType()); triangleShape.setRaycastTestType(m_concaveMeshShape.getRaycastTestType());
// Ray casting test against the collision shape // Ray casting test against the collision shape
@ -188,7 +169,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
bool isTriangleHit = triangleShape.raycast(m_ray, raycastInfo, m_proxyShape); bool isTriangleHit = triangleShape.raycast(m_ray, raycastInfo, m_proxyShape);
// If the ray hit the collision shape // If the ray hit the collision shape
if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) { if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) {
assert(raycastInfo.hitFraction >= decimal(0.0)); assert(raycastInfo.hitFraction >= float(0.0));
m_raycastInfo.body = raycastInfo.body; m_raycastInfo.body = raycastInfo.body;
m_raycastInfo.proxyShape = raycastInfo.proxyShape; m_raycastInfo.proxyShape = raycastInfo.proxyShape;
m_raycastInfo.hitFraction = raycastInfo.hitFraction; m_raycastInfo.hitFraction = raycastInfo.hitFraction;

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CONCAVE_MESH_SHAPE_H
#define REACTPHYSICS3D_CONCAVE_MESH_SHAPE_H
// Libraries // Libraries
#include <ephysics/collision/shapes/ConcaveShape.h> #include <ephysics/collision/shapes/ConcaveShape.h>
@ -61,7 +40,7 @@ class ConvexTriangleAABBOverlapCallback : public DynamicAABBTreeOverlapCallback
// Called when a overlapping node has been found during the call to // Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB() // DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int nodeId); virtual void notifyOverlappingNode(int32_t nodeId);
}; };
@ -70,7 +49,7 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
private : private :
std::vector<int32> m_hitAABBNodes; std::vector<int32_t> m_hitAABBNodes;
const DynamicAABBTree& m_dynamicAABBTree; const DynamicAABBTree& m_dynamicAABBTree;
const ConcaveMeshShape& m_concaveMeshShape; const ConcaveMeshShape& m_concaveMeshShape;
ProxyShape* m_proxyShape; ProxyShape* m_proxyShape;
@ -89,7 +68,7 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
} }
/// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree /// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray); virtual float raycastBroadPhaseShape(int32_t nodeId, const Ray& ray);
/// Raycast all collision shapes that have been collected /// Raycast all collision shapes that have been collected
void raycastTriangles(); void raycastTriangles();
@ -132,12 +111,12 @@ class ConcaveMeshShape : public ConcaveShape {
/// Return the number of bytes used by the collision shape /// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
/// Insert all the triangles into the dynamic AABB tree /// Insert all the triangles int32_to the dynamic AABB tree
void initBVHTree(); void initBVHTree();
/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
/// given the start vertex index pointer of the triangle. /// given the start vertex index pointer of the triangle.
void getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex, void getTriangleVerticesWithIndexPointer(int32_t subPart, int32_t triangleIndex,
Vector3* outTriangleVertices) const; Vector3* outTriangleVertices) const;
public: public:
@ -155,7 +134,7 @@ class ConcaveMeshShape : public ConcaveShape {
virtual void setLocalScaling(const Vector3& scaling); virtual void setLocalScaling(const Vector3& scaling);
/// Return the local inertia tensor of the collision shape /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
/// Use a callback method on all triangles of the concave shape inside a given AABB /// Use a callback method on all triangles of the concave shape inside a given AABB
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const; virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const;
@ -204,7 +183,7 @@ inline void ConcaveMeshShape::setLocalScaling(const Vector3& scaling) {
* coordinates * coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape * @param mass Mass to use to compute the inertia tensor of the collision shape
*/ */
inline void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { inline void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
// Default inertia tensor // Default inertia tensor
// Note that this is not very realistic for a concave triangle mesh. // Note that this is not very realistic for a concave triangle mesh.
@ -217,10 +196,10 @@ inline void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decim
// Called when a overlapping node has been found during the call to // Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB() // DynamicAABBTree:reportAllShapesOverlappingWithAABB()
inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId) { inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int32_t nodeId) {
// Get the node data (triangle index and mesh subpart index) // Get the node data (triangle index and mesh subpart index)
int32* data = m_dynamicAABBTree.getNodeDataInt(nodeId); int32_t* data = m_dynamicAABBTree.getNodeDataInt(nodeId);
// Get the triangle vertices for this node from the concave mesh shape // Get the triangle vertices for this node from the concave mesh shape
Vector3 trianglePoints[3]; Vector3 trianglePoints[3];
@ -231,5 +210,3 @@ inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId)
} }
} }
#endif

29
ephysics/collision/shapes/ConcaveShape.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/shapes/ConcaveShape.h> #include <ephysics/collision/shapes/ConcaveShape.h>

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CONCAVE_SHAPE_H
#define REACTPHYSICS3D_CONCAVE_SHAPE_H
// Libraries // Libraries
#include <ephysics/collision/shapes/CollisionShape.h> #include <ephysics/collision/shapes/CollisionShape.h>
@ -64,7 +43,7 @@ class ConcaveShape : public CollisionShape {
bool m_isSmoothMeshCollisionEnabled; bool m_isSmoothMeshCollisionEnabled;
// Margin use for collision detection for each triangle // Margin use for collision detection for each triangle
decimal m_triangleMargin; float m_triangleMargin;
/// Raycast test type for the triangle (front, back, front-back) /// Raycast test type for the triangle (front, back, front-back)
TriangleRaycastSide m_raycastTestType; TriangleRaycastSide m_raycastTestType;
@ -91,7 +70,7 @@ class ConcaveShape : public CollisionShape {
virtual ~ConcaveShape(); virtual ~ConcaveShape();
/// Return the triangle margin /// Return the triangle margin
decimal getTriangleMargin() const; float getTriangleMargin() const;
/// Return the raycast test type (front, back, front-back) /// Return the raycast test type (front, back, front-back)
TriangleRaycastSide getRaycastTestType() const; TriangleRaycastSide getRaycastTestType() const;
@ -113,7 +92,7 @@ class ConcaveShape : public CollisionShape {
}; };
// Return the triangle margin // Return the triangle margin
inline decimal ConcaveShape::getTriangleMargin() const { inline float ConcaveShape::getTriangleMargin() const {
return m_triangleMargin; return m_triangleMargin;
} }
@ -133,7 +112,7 @@ inline bool ConcaveShape::getIsSmoothMeshCollisionEnabled() const {
} }
// Enable/disable the smooth mesh collision algorithm // Enable/disable the smooth mesh collision algorithm
/// Smooth mesh collision is used to avoid collisions against some internal edges /// Smooth mesh collision is used to avoid collisions against some int32_ternal edges
/// of the triangle mesh. If it is enabled, collsions with the mesh will be smoother /// of the triangle mesh. If it is enabled, collsions with the mesh will be smoother
/// but collisions computation is a bit more expensive. /// but collisions computation is a bit more expensive.
inline void ConcaveShape::setIsSmoothMeshCollisionEnabled(bool isEnabled) { inline void ConcaveShape::setIsSmoothMeshCollisionEnabled(bool isEnabled) {
@ -155,5 +134,4 @@ inline void ConcaveShape::setRaycastTestType(TriangleRaycastSide testType) {
} }
#endif

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

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <complex> #include <complex>
@ -37,10 +18,10 @@ using namespace reactphysics3d;
* @param height Height of the cone (in meters) * @param height Height of the cone (in meters)
* @param margin Collision margin (in meters) around the collision shape * @param margin Collision margin (in meters) around the collision shape
*/ */
ConeShape::ConeShape(decimal radius, decimal height, decimal margin) ConeShape::ConeShape(float radius, float height, float margin)
: ConvexShape(CONE, margin), mRadius(radius), mHalfHeight(height * decimal(0.5)) { : ConvexShape(CONE, margin), mRadius(radius), mHalfHeight(height * float(0.5)) {
assert(mRadius > decimal(0.0)); assert(mRadius > float(0.0));
assert(mHalfHeight > decimal(0.0)); assert(mHalfHeight > float(0.0));
// Compute the sine of the semi-angle at the apex point // Compute the sine of the semi-angle at the apex point
mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height)); mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height));
@ -56,16 +37,16 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const { void** cachedCollisionData) const {
const Vector3& v = direction; const Vector3& v = direction;
decimal sinThetaTimesLengthV = mSinTheta * v.length(); float sinThetaTimesLengthV = mSinTheta * v.length();
Vector3 supportPoint; Vector3 supportPoint;
if (v.y > sinThetaTimesLengthV) { if (v.y > sinThetaTimesLengthV) {
supportPoint = Vector3(0.0, mHalfHeight, 0.0); supportPoint = Vector3(0.0, mHalfHeight, 0.0);
} }
else { else {
decimal 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) { if (projectedLength > MACHINE_EPSILON) {
decimal d = mRadius / projectedLength; float d = mRadius / projectedLength;
supportPoint = Vector3(v.x * d, -mHalfHeight, v.z * d); supportPoint = Vector3(v.x * d, -mHalfHeight, v.z * d);
} }
else { else {
@ -84,40 +65,40 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
const Vector3 r = ray.point2 - ray.point1; const Vector3 r = ray.point2 - ray.point1;
const decimal epsilon = decimal(0.00001); const float epsilon = float(0.00001);
Vector3 V(0, mHalfHeight, 0); Vector3 V(0, mHalfHeight, 0);
Vector3 centerBase(0, -mHalfHeight, 0); Vector3 centerBase(0, -mHalfHeight, 0);
Vector3 axis(0, decimal(-1.0), 0); Vector3 axis(0, float(-1.0), 0);
decimal heightSquare = decimal(4.0) * mHalfHeight * mHalfHeight; float heightSquare = float(4.0) * mHalfHeight * mHalfHeight;
decimal cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius); float cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius);
decimal factor = decimal(1.0) - cosThetaSquare; float factor = float(1.0) - cosThetaSquare;
Vector3 delta = ray.point1 - V; Vector3 delta = ray.point1 - V;
decimal c0 = -cosThetaSquare * delta.x * delta.x + factor * delta.y * delta.y - float c0 = -cosThetaSquare * delta.x * delta.x + factor * delta.y * delta.y -
cosThetaSquare * delta.z * delta.z; cosThetaSquare * delta.z * delta.z;
decimal c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z; float c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z;
decimal c2 = -cosThetaSquare * r.x * r.x + factor * r.y * r.y - cosThetaSquare * r.z * r.z; float c2 = -cosThetaSquare * r.x * r.x + factor * r.y * r.y - cosThetaSquare * r.z * r.z;
decimal tHit[] = {decimal(-1.0), decimal(-1.0), decimal(-1.0)}; float tHit[] = {float(-1.0), float(-1.0), float(-1.0)};
Vector3 localHitPoint[3]; Vector3 localHitPoint[3];
Vector3 localNormal[3]; Vector3 localNormal[3];
// If c2 is different from zero // If c2 is different from zero
if (std::abs(c2) > MACHINE_EPSILON) { if (std::abs(c2) > MACHINE_EPSILON) {
decimal gamma = c1 * c1 - c0 * c2; float gamma = c1 * c1 - c0 * c2;
// If there is no real roots in the quadratic equation // If there is no real roots in the quadratic equation
if (gamma < decimal(0.0)) { if (gamma < float(0.0)) {
return false; return false;
} }
else if (gamma > decimal(0.0)) { // The equation has two real roots else if (gamma > float(0.0)) { // The equation has two real roots
// Compute two intersections // Compute two int32_tersections
decimal sqrRoot = std::sqrt(gamma); float sqrRoot = std::sqrt(gamma);
tHit[0] = (-c1 - sqrRoot) / c2; tHit[0] = (-c1 - sqrRoot) / c2;
tHit[1] = (-c1 + sqrRoot) / c2; tHit[1] = (-c1 + sqrRoot) / c2;
} }
else { // If the equation has a single real root else { // If the equation has a single real root
// Compute the intersection // Compute the int32_tersection
tHit[0] = -c1 / c2; tHit[0] = -c1 / c2;
} }
} }
@ -125,7 +106,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
// If c2 = 0 and c1 != 0 // If c2 = 0 and c1 != 0
if (std::abs(c1) > MACHINE_EPSILON) { if (std::abs(c1) > MACHINE_EPSILON) {
tHit[0] = -c0 / (decimal(2.0) * c1); tHit[0] = -c0 / (float(2.0) * c1);
} }
else { // If c2 = c1 = 0 else { // If c2 = c1 = 0
@ -142,33 +123,33 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
localHitPoint[0] = ray.point1 + tHit[0] * r; localHitPoint[0] = ray.point1 + tHit[0] * r;
localHitPoint[1] = ray.point1 + tHit[1] * r; localHitPoint[1] = ray.point1 + tHit[1] * r;
// Only keep hit points in one side of the double cone (the cone we are interested in) // Only keep hit points in one side of the double cone (the cone we are int32_terested in)
if (axis.dot(localHitPoint[0] - V) < decimal(0.0)) { if (axis.dot(localHitPoint[0] - V) < float(0.0)) {
tHit[0] = decimal(-1.0); tHit[0] = float(-1.0);
} }
if (axis.dot(localHitPoint[1] - V) < decimal(0.0)) { if (axis.dot(localHitPoint[1] - V) < float(0.0)) {
tHit[1] = decimal(-1.0); tHit[1] = float(-1.0);
} }
// Only keep hit points that are within the correct height of the cone // Only keep hit points that are within the correct height of the cone
if (localHitPoint[0].y < decimal(-mHalfHeight)) { if (localHitPoint[0].y < float(-mHalfHeight)) {
tHit[0] = decimal(-1.0); tHit[0] = float(-1.0);
} }
if (localHitPoint[1].y < decimal(-mHalfHeight)) { if (localHitPoint[1].y < float(-mHalfHeight)) {
tHit[1] = decimal(-1.0); tHit[1] = float(-1.0);
} }
// If the ray is in direction of the base plane of the cone // If the ray is in direction of the base plane of the cone
if (r.y > epsilon) { if (r.y > epsilon) {
// Compute the intersection with the base plane of the cone // Compute the int32_tersection with the base plane of the cone
tHit[2] = (-ray.point1.y - mHalfHeight) / (r.y); tHit[2] = (-ray.point1.y - mHalfHeight) / (r.y);
// Only keep this intersection if it is inside the cone radius // Only keep this int32_tersection if it is inside the cone radius
localHitPoint[2] = ray.point1 + tHit[2] * r; localHitPoint[2] = ray.point1 + tHit[2] * r;
if ((localHitPoint[2] - centerBase).lengthSquare() > mRadius * mRadius) { if ((localHitPoint[2] - centerBase).lengthSquare() > mRadius * mRadius) {
tHit[2] = decimal(-1.0); tHit[2] = float(-1.0);
} }
// Compute the normal direction // Compute the normal direction
@ -176,10 +157,10 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
} }
// Find the smallest positive t value // Find the smallest positive t value
int hitIndex = -1; int32_t hitIndex = -1;
decimal t = DECIMAL_LARGEST; float t = DECIMAL_LARGEST;
for (int i=0; i<3; i++) { for (int32_t i=0; i<3; i++) {
if (tHit[i] < decimal(0.0)) continue; if (tHit[i] < float(0.0)) continue;
if (tHit[i] < t) { if (tHit[i] < t) {
hitIndex = i; hitIndex = i;
t = tHit[hitIndex]; t = tHit[hitIndex];
@ -191,14 +172,14 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
// Compute the normal direction for hit against side of the cone // Compute the normal direction for hit against side of the cone
if (hitIndex != 2) { if (hitIndex != 2) {
decimal h = decimal(2.0) * mHalfHeight; float h = float(2.0) * mHalfHeight;
decimal value1 = (localHitPoint[hitIndex].x * localHitPoint[hitIndex].x + float value1 = (localHitPoint[hitIndex].x * localHitPoint[hitIndex].x +
localHitPoint[hitIndex].z * localHitPoint[hitIndex].z); localHitPoint[hitIndex].z * localHitPoint[hitIndex].z);
decimal rOverH = mRadius / h; float rOverH = mRadius / h;
decimal value2 = decimal(1.0) + rOverH * rOverH; float value2 = float(1.0) + rOverH * rOverH;
decimal factor = decimal(1.0) / std::sqrt(value1 * value2); float factor = float(1.0) / std::sqrt(value1 * value2);
decimal x = localHitPoint[hitIndex].x * factor; float x = localHitPoint[hitIndex].x * factor;
decimal z = localHitPoint[hitIndex].z * factor; float z = localHitPoint[hitIndex].z * factor;
localNormal[hitIndex].x = x; localNormal[hitIndex].x = x;
localNormal[hitIndex].y = std::sqrt(x * x + z * z) * rOverH; localNormal[hitIndex].y = std::sqrt(x * x + z * z) * rOverH;
localNormal[hitIndex].z = z; localNormal[hitIndex].z = z;

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CONE_SHAPE_H
#define REACTPHYSICS3D_CONE_SHAPE_H
// Libraries // Libraries
#include <ephysics/collision/shapes/ConvexShape.h> #include <ephysics/collision/shapes/ConvexShape.h>
@ -56,13 +35,13 @@ class ConeShape : public ConvexShape {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Radius of the base /// Radius of the base
decimal mRadius; float mRadius;
/// Half height of the cone /// Half height of the cone
decimal mHalfHeight; float mHalfHeight;
/// sine of the semi angle at the apex point /// sine of the semi angle at the apex point
decimal mSinTheta; float mSinTheta;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -90,16 +69,16 @@ class ConeShape : public ConvexShape {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ConeShape(decimal mRadius, decimal height, decimal margin = OBJECT_MARGIN); ConeShape(float mRadius, float height, float margin = OBJECT_MARGIN);
/// Destructor /// Destructor
virtual ~ConeShape(); virtual ~ConeShape();
/// Return the radius /// Return the radius
decimal getRadius() const; float getRadius() const;
/// Return the height /// Return the height
decimal getHeight() const; float getHeight() const;
/// Set the scaling vector of the collision shape /// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling); virtual void setLocalScaling(const Vector3& scaling);
@ -108,14 +87,14 @@ class ConeShape : public ConvexShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const; virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// Return the local inertia tensor of the collision shape /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
}; };
// Return the radius // Return the radius
/** /**
* @return Radius of the cone (in meters) * @return Radius of the cone (in meters)
*/ */
inline decimal ConeShape::getRadius() const { inline float ConeShape::getRadius() const {
return mRadius; return mRadius;
} }
@ -123,8 +102,8 @@ inline decimal ConeShape::getRadius() const {
/** /**
* @return Height of the cone (in meters) * @return Height of the cone (in meters)
*/ */
inline decimal ConeShape::getHeight() const { inline float ConeShape::getHeight() const {
return decimal(2.0) * mHalfHeight; return float(2.0) * mHalfHeight;
} }
// Set the scaling vector of the collision shape // Set the scaling vector of the collision shape
@ -165,22 +144,20 @@ inline void ConeShape::getLocalBounds(Vector3& min, Vector3& max) const {
* coordinates * coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape * @param mass Mass to use to compute the inertia tensor of the collision shape
*/ */
inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
decimal rSquare = mRadius * mRadius; float rSquare = mRadius * mRadius;
decimal diagXZ = decimal(0.15) * mass * (rSquare + mHalfHeight); float diagXZ = float(0.15) * mass * (rSquare + mHalfHeight);
tensor.setAllValues(diagXZ, 0.0, 0.0, tensor.setAllValues(diagXZ, 0.0, 0.0,
0.0, decimal(0.3) * mass * rSquare, 0.0, float(0.3) * mass * rSquare,
0.0, 0.0, 0.0, diagXZ); 0.0, 0.0, 0.0, diagXZ);
} }
// Return true if a point is inside the collision shape // Return true if a point is inside the collision shape
inline bool ConeShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { inline bool ConeShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
const decimal radiusHeight = mRadius * (-localPoint.y + mHalfHeight) / const float radiusHeight = mRadius * (-localPoint.y + mHalfHeight) /
(mHalfHeight * decimal(2.0)); (mHalfHeight * float(2.0));
return (localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) && return (localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) &&
(localPoint.x * localPoint.x + localPoint.z * localPoint.z < radiusHeight *radiusHeight); (localPoint.x * localPoint.x + localPoint.z * localPoint.z < radiusHeight *radiusHeight);
} }
} }
#endif

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

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <complex> #include <complex>
@ -31,14 +12,14 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor to initialize with an array of 3D vertices. // Constructor to initialize with an array of 3D vertices.
/// This method creates an internal copy of the input vertices. /// This method creates an int32_ternal copy of the input vertices.
/** /**
* @param arrayVertices Array with the vertices of the convex mesh * @param arrayVertices Array with the vertices of the convex mesh
* @param nbVertices Number of vertices in the convex mesh * @param nbVertices Number of vertices in the convex mesh
* @param stride Stride between the beginning of two elements in the vertices array * @param stride Stride between the beginning of two elements in the vertices array
* @param margin Collision margin (in meters) around the collision shape * @param margin Collision margin (in meters) around the collision shape
*/ */
ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride, decimal margin) ConvexMeshShape::ConvexMeshShape(const float* arrayVertices, uint32_t nbVertices, int32_t stride, float margin)
: ConvexShape(CONVEX_MESH, margin), m_numberVertices(nbVertices), m_minBounds(0, 0, 0), : ConvexShape(CONVEX_MESH, margin), m_numberVertices(nbVertices), m_minBounds(0, 0, 0),
m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(false) { m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(false) {
assert(nbVertices > 0); assert(nbVertices > 0);
@ -46,9 +27,9 @@ ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices,
const unsigned char* vertexPointer = (const unsigned char*) arrayVertices; const unsigned char* vertexPointer = (const unsigned char*) arrayVertices;
// Copy all the vertices into the internal array // Copy all the vertices int32_to the int32_ternal array
for (uint i=0; i<m_numberVertices; i++) { for (uint32_t i=0; i<m_numberVertices; i++) {
const decimal* newPoint = (const decimal*) vertexPointer; const float* newPoint = (const float*) vertexPointer;
m_vertices.push_back(Vector3(newPoint[0], newPoint[1], newPoint[2])); m_vertices.push_back(Vector3(newPoint[0], newPoint[1], newPoint[2]));
vertexPointer += stride; vertexPointer += stride;
} }
@ -58,13 +39,13 @@ ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices,
} }
// Constructor to initialize with a triangle mesh // Constructor to initialize with a triangle mesh
/// This method creates an internal copy of the input vertices. /// This method creates an int32_ternal copy of the input vertices.
/** /**
* @param triangleVertexArray Array with the vertices and indices of the vertices and triangles of the mesh * @param triangleVertexArray Array with the vertices and indices of the vertices and triangles of the mesh
* @param isEdgesInformationUsed True if you want to use edges information for collision detection (faster but requires more memory) * @param isEdgesInformationUsed True if you want to use edges information for collision detection (faster but requires more memory)
* @param margin Collision margin (in meters) around the collision shape * @param margin Collision margin (in meters) around the collision shape
*/ */
ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed, decimal margin) ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed, float margin)
: ConvexShape(CONVEX_MESH, margin), m_minBounds(0, 0, 0), : ConvexShape(CONVEX_MESH, margin), m_minBounds(0, 0, 0),
m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(isEdgesInformationUsed) { m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(isEdgesInformationUsed) {
@ -72,11 +53,11 @@ ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool
TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
unsigned char* verticesStart = triangleVertexArray->getVerticesStart(); unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
unsigned char* indicesStart = triangleVertexArray->getIndicesStart(); unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
int vertexStride = triangleVertexArray->getVerticesStride(); int32_t vertexStride = triangleVertexArray->getVerticesStride();
int indexStride = triangleVertexArray->getIndicesStride(); int32_t indexStride = triangleVertexArray->getIndicesStride();
// For each vertex of the mesh // For each vertex of the mesh
for (uint v = 0; v < triangleVertexArray->getNbVertices(); v++) { for (uint32_t v = 0; v < triangleVertexArray->getNbVertices(); v++) {
// Get the vertices components of the triangle // Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) { if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
@ -99,18 +80,18 @@ ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool
if (m_isEdgesInformationUsed) { if (m_isEdgesInformationUsed) {
// For each triangle of the mesh // For each triangle of the mesh
for (uint triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) { for (uint32_t triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) {
void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride); void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
uint vertexIndex[3] = {0, 0, 0}; uint32_t vertexIndex[3] = {0, 0, 0};
// For each vertex of the triangle // For each vertex of the triangle
for (int k=0; k < 3; k++) { for (int32_t k=0; k < 3; k++) {
// Get the index of the current vertex in the triangle // Get the index of the current vertex in the triangle
if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) { if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
vertexIndex[k] = ((uint*)vertexIndexPointer)[k]; vertexIndex[k] = ((uint32_t*)vertexIndexPointer)[k];
} }
else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) { else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
vertexIndex[k] = ((unsigned short*)vertexIndexPointer)[k]; vertexIndex[k] = ((unsigned short*)vertexIndexPointer)[k];
@ -134,7 +115,7 @@ ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool
// Constructor. // Constructor.
/// If you use this constructor, you will need to set the vertices manually one by one using /// If you use this constructor, you will need to set the vertices manually one by one using
/// the addVertex() method. /// the addVertex() method.
ConvexMeshShape::ConvexMeshShape(decimal margin) ConvexMeshShape::ConvexMeshShape(float margin)
: ConvexShape(CONVEX_MESH, margin), m_numberVertices(0), m_minBounds(0, 0, 0), : ConvexShape(CONVEX_MESH, margin), m_numberVertices(0), m_minBounds(0, 0, 0),
m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(false) { m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(false) {
@ -161,8 +142,8 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct
// Allocate memory for the cached collision data if not allocated yet // Allocate memory for the cached collision data if not allocated yet
if ((*cachedCollisionData) == NULL) { if ((*cachedCollisionData) == NULL) {
*cachedCollisionData = (int*) malloc(sizeof(int)); *cachedCollisionData = (int32_t*) malloc(sizeof(int32_t));
*((int*)(*cachedCollisionData)) = 0; *((int32_t*)(*cachedCollisionData)) = 0;
} }
// If the edges information is used to speed up the collision detection // If the edges information is used to speed up the collision detection
@ -170,8 +151,8 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct
assert(m_edgesAdjacencyList.size() == m_numberVertices); assert(m_edgesAdjacencyList.size() == m_numberVertices);
uint maxVertex = *((int*)(*cachedCollisionData)); uint32_t maxVertex = *((int32_t*)(*cachedCollisionData));
decimal maxDotProduct = direction.dot(m_vertices[maxVertex]); float maxDotProduct = direction.dot(m_vertices[maxVertex]);
bool isOptimal; bool isOptimal;
// Perform hill-climbing (local search) // Perform hill-climbing (local search)
@ -181,13 +162,13 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct
assert(m_edgesAdjacencyList.at(maxVertex).size() > 0); assert(m_edgesAdjacencyList.at(maxVertex).size() > 0);
// For all neighbors of the current vertex // For all neighbors of the current vertex
std::set<uint>::const_iterator it; std::set<uint32_t>::const_iterator it;
std::set<uint>::const_iterator itBegin = m_edgesAdjacencyList.at(maxVertex).begin(); std::set<uint32_t>::const_iterator itBegin = m_edgesAdjacencyList.at(maxVertex).begin();
std::set<uint>::const_iterator itEnd = m_edgesAdjacencyList.at(maxVertex).end(); std::set<uint32_t>::const_iterator itEnd = m_edgesAdjacencyList.at(maxVertex).end();
for (it = itBegin; it != itEnd; ++it) { for (it = itBegin; it != itEnd; ++it) {
// Compute the dot product // Compute the dot product
decimal dotProduct = direction.dot(m_vertices[*it]); float dotProduct = direction.dot(m_vertices[*it]);
// If the current vertex is a better vertex (larger dot product) // If the current vertex is a better vertex (larger dot product)
if (dotProduct > maxDotProduct) { if (dotProduct > maxDotProduct) {
@ -200,7 +181,7 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct
} while(!isOptimal); } while(!isOptimal);
// Cache the support vertex // Cache the support vertex
*((int*)(*cachedCollisionData)) = maxVertex; *((int32_t*)(*cachedCollisionData)) = maxVertex;
// Return the support vertex // Return the support vertex
return m_vertices[maxVertex] * mScaling; return m_vertices[maxVertex] * mScaling;
@ -208,10 +189,10 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct
else { // If the edges information is not used else { // If the edges information is not used
double maxDotProduct = DECIMAL_SMALLEST; double maxDotProduct = DECIMAL_SMALLEST;
uint indexMaxDotProduct = 0; uint32_t indexMaxDotProduct = 0;
// For each vertex of the mesh // For each vertex of the mesh
for (uint i=0; i<m_numberVertices; i++) { for (uint32_t i=0; i<m_numberVertices; i++) {
// Compute the dot product of the current vertex // Compute the dot product of the current vertex
double dotProduct = direction.dot(m_vertices[i]); double dotProduct = direction.dot(m_vertices[i]);
@ -223,7 +204,7 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct
} }
} }
assert(maxDotProduct >= decimal(0.0)); assert(maxDotProduct >= float(0.0));
// Return the vertex with the largest dot product in the support direction // Return the vertex with the largest dot product in the support direction
return m_vertices[indexMaxDotProduct] * mScaling; return m_vertices[indexMaxDotProduct] * mScaling;
@ -240,7 +221,7 @@ void ConvexMeshShape::recalculateBounds() {
m_maxBounds.setToZero(); m_maxBounds.setToZero();
// For each vertex of the mesh // For each vertex of the mesh
for (uint i=0; i<m_numberVertices; i++) { 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_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_minBounds.x) m_minBounds.x = m_vertices[i].x;

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CONVEX_MESH_SHAPE_H
#define REACTPHYSICS3D_CONVEX_MESH_SHAPE_H
// Libraries // Libraries
#include <ephysics/collision/shapes/ConvexShape.h> #include <ephysics/collision/shapes/ConvexShape.h>
@ -68,7 +47,7 @@ class ConvexMeshShape : public ConvexShape {
std::vector<Vector3> m_vertices; std::vector<Vector3> m_vertices;
/// Number of vertices in the mesh /// Number of vertices in the mesh
uint m_numberVertices; uint32_t m_numberVertices;
/// Mesh minimum bounds in the three local x, y and z directions /// Mesh minimum bounds in the three local x, y and z directions
Vector3 m_minBounds; Vector3 m_minBounds;
@ -81,7 +60,7 @@ class ConvexMeshShape : public ConvexShape {
bool m_isEdgesInformationUsed; bool m_isEdgesInformationUsed;
/// Adjacency list representing the edges of the mesh /// Adjacency list representing the edges of the mesh
std::map<uint, std::set<uint> > m_edgesAdjacencyList; std::map<uint32_t, std::set<uint32_t> > m_edgesAdjacencyList;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -115,15 +94,15 @@ class ConvexMeshShape : public ConvexShape {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor to initialize with an array of 3D vertices. /// Constructor to initialize with an array of 3D vertices.
ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride, ConvexMeshShape(const float* arrayVertices, uint32_t nbVertices, int32_t stride,
decimal margin = OBJECT_MARGIN); float margin = OBJECT_MARGIN);
/// Constructor to initialize with a triangle vertex array /// Constructor to initialize with a triangle vertex array
ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed = true, ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed = true,
decimal margin = OBJECT_MARGIN); float margin = OBJECT_MARGIN);
/// Constructor. /// Constructor.
ConvexMeshShape(decimal margin = OBJECT_MARGIN); ConvexMeshShape(float margin = OBJECT_MARGIN);
/// Destructor /// Destructor
virtual ~ConvexMeshShape(); virtual ~ConvexMeshShape();
@ -132,13 +111,13 @@ class ConvexMeshShape : public ConvexShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const; virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// Return the local inertia tensor of the collision shape. /// Return the local inertia tensor of the collision shape.
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
/// Add a vertex into the convex mesh /// Add a vertex int32_to the convex mesh
void addVertex(const Vector3& vertex); void addVertex(const Vector3& vertex);
/// Add an edge into the convex mesh by specifying the two vertex indices of the edge. /// Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge.
void addEdge(uint v1, uint v2); void addEdge(uint32_t v1, uint32_t v2);
/// Return true if the edges information is used to speed up the collision detection /// Return true if the edges information is used to speed up the collision detection
bool isEdgesInformationUsed() const; bool isEdgesInformationUsed() const;
@ -177,19 +156,19 @@ inline void ConvexMeshShape::getLocalBounds(Vector3& min, Vector3& max) const {
* coordinates * coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape * @param mass Mass to use to compute the inertia tensor of the collision shape
*/ */
inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
decimal factor = (decimal(1.0) / decimal(3.0)) * mass; float factor = (float(1.0) / float(3.0)) * mass;
Vector3 realExtent = decimal(0.5) * (m_maxBounds - m_minBounds); Vector3 realExtent = float(0.5) * (m_maxBounds - m_minBounds);
assert(realExtent.x > 0 && realExtent.y > 0 && realExtent.z > 0); assert(realExtent.x > 0 && realExtent.y > 0 && realExtent.z > 0);
decimal xSquare = realExtent.x * realExtent.x; float xSquare = realExtent.x * realExtent.x;
decimal ySquare = realExtent.y * realExtent.y; float ySquare = realExtent.y * realExtent.y;
decimal zSquare = realExtent.z * realExtent.z; float zSquare = realExtent.z * realExtent.z;
tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0, tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0,
0.0, factor * (xSquare + zSquare), 0.0, 0.0, factor * (xSquare + zSquare), 0.0,
0.0, 0.0, factor * (xSquare + ySquare)); 0.0, 0.0, factor * (xSquare + ySquare));
} }
// Add a vertex into the convex mesh // Add a vertex int32_to the convex mesh
/** /**
* @param vertex Vertex to be added * @param vertex Vertex to be added
*/ */
@ -208,24 +187,24 @@ inline void ConvexMeshShape::addVertex(const Vector3& vertex) {
if (vertex.z * mScaling.z < m_minBounds.z) m_minBounds.z = vertex.z * mScaling.z; if (vertex.z * mScaling.z < m_minBounds.z) m_minBounds.z = vertex.z * mScaling.z;
} }
// Add an edge into the convex mesh by specifying the two vertex indices of the edge. // Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge.
/// Note that the vertex indices start at zero and need to correspond to the order of /// Note that the vertex indices start at zero and need to correspond to the order of
/// the vertices in the vertices array in the constructor or the order of the calls /// the vertices in the vertices array in the constructor or the order of the calls
/// of the addVertex() methods that you use to add vertices into the convex mesh. /// of the addVertex() methods that you use to add vertices int32_to the convex mesh.
/** /**
* @param v1 Index of the first vertex of the edge to add * @param v1 Index of the first vertex of the edge to add
* @param v2 Index of the second vertex of the edge to add * @param v2 Index of the second vertex of the edge to add
*/ */
inline void ConvexMeshShape::addEdge(uint v1, uint v2) { inline void ConvexMeshShape::addEdge(uint32_t v1, uint32_t v2) {
// If the entry for vertex v1 does not exist in the adjacency list // If the entry for vertex v1 does not exist in the adjacency list
if (m_edgesAdjacencyList.count(v1) == 0) { if (m_edgesAdjacencyList.count(v1) == 0) {
m_edgesAdjacencyList.insert(std::make_pair(v1, std::set<uint>())); m_edgesAdjacencyList.insert(std::make_pair(v1, std::set<uint32_t>()));
} }
// If the entry for vertex v2 does not exist in the adjacency list // If the entry for vertex v2 does not exist in the adjacency list
if (m_edgesAdjacencyList.count(v2) == 0) { if (m_edgesAdjacencyList.count(v2) == 0) {
m_edgesAdjacencyList.insert(std::make_pair(v2, std::set<uint>())); m_edgesAdjacencyList.insert(std::make_pair(v2, std::set<uint32_t>()));
} }
// Add the edge in the adjacency list // Add the edge in the adjacency list
@ -261,5 +240,3 @@ inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint,
} }
} }
#endif

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

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/shapes/ConvexShape.h> #include <ephysics/collision/shapes/ConvexShape.h>
@ -31,7 +12,7 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
ConvexShape::ConvexShape(CollisionShapeType type, decimal margin) ConvexShape::ConvexShape(CollisionShapeType type, float margin)
: CollisionShape(type), mMargin(margin) { : CollisionShape(type), mMargin(margin) {
} }
@ -48,7 +29,7 @@ Vector3 ConvexShape::getLocalSupportPointWithMargin(const Vector3& direction,
// Get the support point without margin // Get the support point without margin
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData); Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
if (mMargin != decimal(0.0)) { if (mMargin != float(0.0)) {
// Add the margin to the support point // Add the margin to the support point
Vector3 unitVec(0.0, -1.0, 0.0); Vector3 unitVec(0.0, -1.0, 0.0);

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CONVEX_SHAPE_H
#define REACTPHYSICS3D_CONVEX_SHAPE_H
// Libraries // Libraries
#include <ephysics/collision/shapes/CollisionShape.h> #include <ephysics/collision/shapes/CollisionShape.h>
@ -44,7 +23,7 @@ class ConvexShape : public CollisionShape {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Margin used for the GJK collision detection algorithm /// Margin used for the GJK collision detection algorithm
decimal mMargin; float mMargin;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -70,13 +49,13 @@ class ConvexShape : public CollisionShape {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ConvexShape(CollisionShapeType type, decimal margin); ConvexShape(CollisionShapeType type, float margin);
/// Destructor /// Destructor
virtual ~ConvexShape(); virtual ~ConvexShape();
/// Return the current object margin /// Return the current object margin
decimal getMargin() const; float getMargin() const;
/// Return true if the collision shape is convex, false if it is concave /// Return true if the collision shape is convex, false if it is concave
virtual bool isConvex() const; virtual bool isConvex() const;
@ -96,11 +75,10 @@ inline bool ConvexShape::isConvex() const {
/** /**
* @return The margin (in meters) around the collision shape * @return The margin (in meters) around the collision shape
*/ */
inline decimal ConvexShape::getMargin() const { inline float ConvexShape::getMargin() const {
return mMargin; return mMargin;
} }
} }
#endif

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

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/shapes/CylinderShape.h> #include <ephysics/collision/shapes/CylinderShape.h>
@ -36,11 +17,11 @@ using namespace reactphysics3d;
* @param height Height of the cylinder (in meters) * @param height Height of the cylinder (in meters)
* @param margin Collision margin (in meters) around the collision shape * @param margin Collision margin (in meters) around the collision shape
*/ */
CylinderShape::CylinderShape(decimal radius, decimal height, decimal margin) CylinderShape::CylinderShape(float radius, float height, float margin)
: ConvexShape(CYLINDER, margin), mRadius(radius), : ConvexShape(CYLINDER, margin), mRadius(radius),
mHalfHeight(height/decimal(2.0)) { mHalfHeight(height/float(2.0)) {
assert(radius > decimal(0.0)); assert(radius > float(0.0));
assert(height > decimal(0.0)); assert(height > float(0.0));
} }
// Destructor // Destructor
@ -53,9 +34,9 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio
void** cachedCollisionData) const { void** cachedCollisionData) const {
Vector3 supportPoint(0.0, 0.0, 0.0); Vector3 supportPoint(0.0, 0.0, 0.0);
decimal uDotv = direction.y; float uDotv = direction.y;
Vector3 w(direction.x, 0.0, direction.z); Vector3 w(direction.x, 0.0, direction.z);
decimal lengthW = sqrt(direction.x * direction.x + direction.z * direction.z); float lengthW = sqrt(direction.x * direction.x + direction.z * direction.z);
if (lengthW > MACHINE_EPSILON) { if (lengthW > MACHINE_EPSILON) {
if (uDotv < 0.0) supportPoint.y = -mHalfHeight; if (uDotv < 0.0) supportPoint.y = -mHalfHeight;
@ -77,44 +58,44 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
const Vector3 n = ray.point2 - ray.point1; const Vector3 n = ray.point2 - ray.point1;
const decimal epsilon = decimal(0.01); const float epsilon = float(0.01);
Vector3 p(decimal(0), -mHalfHeight, decimal(0)); Vector3 p(float(0), -mHalfHeight, float(0));
Vector3 q(decimal(0), mHalfHeight, decimal(0)); Vector3 q(float(0), mHalfHeight, float(0));
Vector3 d = q - p; Vector3 d = q - p;
Vector3 m = ray.point1 - p; Vector3 m = ray.point1 - p;
decimal t; float t;
decimal mDotD = m.dot(d); float mDotD = m.dot(d);
decimal nDotD = n.dot(d); float nDotD = n.dot(d);
decimal dDotD = d.dot(d); float dDotD = d.dot(d);
// Test if the segment is outside the cylinder // Test if the segment is outside the cylinder
if (mDotD < decimal(0.0) && mDotD + nDotD < decimal(0.0)) return false; if (mDotD < float(0.0) && mDotD + nDotD < float(0.0)) return false;
if (mDotD > dDotD && mDotD + nDotD > dDotD) return false; if (mDotD > dDotD && mDotD + nDotD > dDotD) return false;
decimal nDotN = n.dot(n); float nDotN = n.dot(n);
decimal mDotN = m.dot(n); float mDotN = m.dot(n);
decimal a = dDotD * nDotN - nDotD * nDotD; float a = dDotD * nDotN - nDotD * nDotD;
decimal k = m.dot(m) - mRadius * mRadius; float k = m.dot(m) - mRadius * mRadius;
decimal c = dDotD * k - mDotD * mDotD; float c = dDotD * k - mDotD * mDotD;
// If the ray is parallel to the cylinder axis // If the ray is parallel to the cylinder axis
if (std::abs(a) < epsilon) { if (std::abs(a) < epsilon) {
// If the origin is outside the surface of the cylinder, we return no hit // If the origin is outside the surface of the cylinder, we return no hit
if (c > decimal(0.0)) return false; if (c > float(0.0)) return false;
// Here we know that the segment intersect an endcap of the cylinder // Here we know that the segment int32_tersect an endcap of the cylinder
// If the ray intersects with the "p" endcap of the cylinder // If the ray int32_tersects with the "p" endcap of the cylinder
if (mDotD < decimal(0.0)) { if (mDotD < float(0.0)) {
t = -mDotN / nDotN; t = -mDotN / nDotN;
// If the intersection is behind the origin of the ray or beyond the maximum // If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit // raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false; if (t < float(0.0) || t > ray.maxFraction) return false;
// Compute the hit information // Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n; Vector3 localHitPoint = ray.point1 + t * n;
@ -122,18 +103,18 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(-1), 0); Vector3 normalDirection(0, float(-1), 0);
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder else if (mDotD > dDotD) { // If the ray int32_tersects with the "q" endcap of the cylinder
t = (nDotD - mDotN) / nDotN; t = (nDotD - mDotN) / nDotN;
// If the intersection is behind the origin of the ray or beyond the maximum // If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit // raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false; if (t < float(0.0) || t > ray.maxFraction) return false;
// Compute the hit information // Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n; Vector3 localHitPoint = ray.point1 + t * n;
@ -141,7 +122,7 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(1.0), 0); Vector3 normalDirection(0, float(1.0), 0);
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
@ -150,31 +131,31 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
return false; return false;
} }
} }
decimal b = dDotD * mDotN - nDotD * mDotD; float b = dDotD * mDotN - nDotD * mDotD;
decimal discriminant = b * b - a * c; float discriminant = b * b - a * c;
// If the discriminant is negative, no real roots and therfore, no hit // If the discriminant is negative, no real roots and therfore, no hit
if (discriminant < decimal(0.0)) return false; if (discriminant < float(0.0)) return false;
// Compute the smallest root (first intersection along the ray) // Compute the smallest root (first int32_tersection along the ray)
decimal t0 = t = (-b - std::sqrt(discriminant)) / a; float t0 = t = (-b - std::sqrt(discriminant)) / a;
// If the intersection is outside the cylinder on "p" endcap side // If the int32_tersection is outside the cylinder on "p" endcap side
decimal value = mDotD + t * nDotD; float value = mDotD + t * nDotD;
if (value < decimal(0.0)) { if (value < float(0.0)) {
// If the ray is pointing away from the "p" endcap, we return no hit // If the ray is pointing away from the "p" endcap, we return no hit
if (nDotD <= decimal(0.0)) return false; if (nDotD <= float(0.0)) return false;
// Compute the intersection against the "p" endcap (intersection agains whole plane) // Compute the int32_tersection against the "p" endcap (int32_tersection agains whole plane)
t = -mDotD / nDotD; t = -mDotD / nDotD;
// Keep the intersection if the it is inside the cylinder radius // Keep the int32_tersection if the it is inside the cylinder radius
if (k + t * (decimal(2.0) * mDotN + t) > decimal(0.0)) return false; if (k + t * (float(2.0) * mDotN + t) > float(0.0)) return false;
// If the intersection is behind the origin of the ray or beyond the maximum // If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit // raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false; if (t < float(0.0) || t > ray.maxFraction) return false;
// Compute the hit information // Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n; Vector3 localHitPoint = ray.point1 + t * n;
@ -182,26 +163,26 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(-1.0), 0); Vector3 normalDirection(0, float(-1.0), 0);
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
else if (value > dDotD) { // If the intersection is outside the cylinder on the "q" side 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 the ray is pointing away from the "q" endcap, we return no hit
if (nDotD >= decimal(0.0)) return false; if (nDotD >= float(0.0)) return false;
// Compute the intersection against the "q" endcap (intersection against whole plane) // Compute the int32_tersection against the "q" endcap (int32_tersection against whole plane)
t = (dDotD - mDotD) / nDotD; t = (dDotD - mDotD) / nDotD;
// Keep the intersection if it is inside the cylinder radius // Keep the int32_tersection if it is inside the cylinder radius
if (k + dDotD - decimal(2.0) * mDotD + t * (decimal(2.0) * (mDotN - nDotD) + t) > if (k + dDotD - float(2.0) * mDotD + t * (float(2.0) * (mDotN - nDotD) + t) >
decimal(0.0)) return false; float(0.0)) return false;
// If the intersection is behind the origin of the ray or beyond the maximum // If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit // raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false; if (t < float(0.0) || t > ray.maxFraction) return false;
// Compute the hit information // Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n; Vector3 localHitPoint = ray.point1 + t * n;
@ -209,7 +190,7 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(1.0), 0); Vector3 normalDirection(0, float(1.0), 0);
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
@ -217,9 +198,9 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
t = t0; t = t0;
// If the intersection is behind the origin of the ray or beyond the maximum // If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit // raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false; if (t < float(0.0) || t > ray.maxFraction) return false;
// Compute the hit information // Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n; Vector3 localHitPoint = ray.point1 + t * n;

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CYLINDER_SHAPE_H
#define REACTPHYSICS3D_CYLINDER_SHAPE_H
// Libraries // Libraries
#include <ephysics/collision/shapes/ConvexShape.h> #include <ephysics/collision/shapes/ConvexShape.h>
@ -56,10 +35,10 @@ class CylinderShape : public ConvexShape {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Radius of the base /// Radius of the base
decimal mRadius; float mRadius;
/// Half height of the cylinder /// Half height of the cylinder
decimal mHalfHeight; float mHalfHeight;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -87,16 +66,16 @@ class CylinderShape : public ConvexShape {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
CylinderShape(decimal radius, decimal height, decimal margin = OBJECT_MARGIN); CylinderShape(float radius, float height, float margin = OBJECT_MARGIN);
/// Destructor /// Destructor
virtual ~CylinderShape(); virtual ~CylinderShape();
/// Return the radius /// Return the radius
decimal getRadius() const; float getRadius() const;
/// Return the height /// Return the height
decimal getHeight() const; float getHeight() const;
/// Set the scaling vector of the collision shape /// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling); virtual void setLocalScaling(const Vector3& scaling);
@ -105,14 +84,14 @@ class CylinderShape : public ConvexShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const; virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// Return the local inertia tensor of the collision shape /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
}; };
// Return the radius // Return the radius
/** /**
* @return Radius of the cylinder (in meters) * @return Radius of the cylinder (in meters)
*/ */
inline decimal CylinderShape::getRadius() const { inline float CylinderShape::getRadius() const {
return mRadius; return mRadius;
} }
@ -120,7 +99,7 @@ inline decimal CylinderShape::getRadius() const {
/** /**
* @return Height of the cylinder (in meters) * @return Height of the cylinder (in meters)
*/ */
inline decimal CylinderShape::getHeight() const { inline float CylinderShape::getHeight() const {
return mHalfHeight + mHalfHeight; return mHalfHeight + mHalfHeight;
} }
@ -162,11 +141,11 @@ inline void CylinderShape::getLocalBounds(Vector3& min, Vector3& max) const {
* coordinates * coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape * @param mass Mass to use to compute the inertia tensor of the collision shape
*/ */
inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
decimal height = decimal(2.0) * mHalfHeight; float height = float(2.0) * mHalfHeight;
decimal diag = (decimal(1.0) / decimal(12.0)) * mass * (3 * mRadius * mRadius + height * height); float diag = (float(1.0) / float(12.0)) * mass * (3 * mRadius * mRadius + height * height);
tensor.setAllValues(diag, 0.0, 0.0, 0.0, tensor.setAllValues(diag, 0.0, 0.0, 0.0,
decimal(0.5) * mass * mRadius * mRadius, 0.0, float(0.5) * mass * mRadius * mRadius, 0.0,
0.0, 0.0, diag); 0.0, 0.0, diag);
} }
@ -178,5 +157,4 @@ inline bool CylinderShape::testPointInside(const Vector3& localPoint, ProxyShape
} }
#endif

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

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/shapes/HeightFieldShape.h> #include <ephysics/collision/shapes/HeightFieldShape.h>
@ -35,16 +16,16 @@ using namespace reactphysics3d;
* @param minHeight Minimum height value of the height field * @param minHeight Minimum height value of the height field
* @param maxHeight Maximum height value of the height field * @param maxHeight Maximum height value of the height field
* @param heightFieldData Pointer to the first height value data (note that values are shared and not copied) * @param heightFieldData Pointer to the first height value data (note that values are shared and not copied)
* @param dataType Data type for the height values (int, float, double) * @param dataType Data type for the height values (int32_t, float, double)
* @param upAxis Integer representing the up axis direction (0 for x, 1 for y and 2 for z) * @param upAxis Integer representing the up axis direction (0 for x, 1 for y and 2 for z)
* @param integerHeightScale Scaling factor used to scale the height values (only when height values type is integer) * @param int32_tegerHeightScale Scaling factor used to scale the height values (only when height values type is int32_teger)
*/ */
HeightFieldShape::HeightFieldShape(int nbGridColumns, int nbGridRows, decimal minHeight, decimal maxHeight, HeightFieldShape::HeightFieldShape(int32_t nbGridColumns, int32_t nbGridRows, float minHeight, float maxHeight,
const void* heightFieldData, HeightDataType dataType, int upAxis, const void* heightFieldData, HeightDataType dataType, int32_t upAxis,
decimal integerHeightScale) float int32_tegerHeightScale)
: ConcaveShape(HEIGHTFIELD), mNbColumns(nbGridColumns), mNbRows(nbGridRows), : ConcaveShape(HEIGHTFIELD), mNbColumns(nbGridColumns), mNbRows(nbGridRows),
mWidth(nbGridColumns - 1), mLength(nbGridRows - 1), mMinHeight(minHeight), mWidth(nbGridColumns - 1), mLength(nbGridRows - 1), mMinHeight(minHeight),
mMaxHeight(maxHeight), mUpAxis(upAxis), mIntegerHeightScale(integerHeightScale), mMaxHeight(maxHeight), mUpAxis(upAxis), mIntegerHeightScale(int32_tegerHeightScale),
mHeightDataType(dataType) { mHeightDataType(dataType) {
assert(nbGridColumns >= 2); assert(nbGridColumns >= 2);
@ -56,21 +37,21 @@ HeightFieldShape::HeightFieldShape(int nbGridColumns, int nbGridRows, decimal mi
mHeightFieldData = heightFieldData; mHeightFieldData = heightFieldData;
decimal halfHeight = (mMaxHeight - mMinHeight) * decimal(0.5); float halfHeight = (mMaxHeight - mMinHeight) * float(0.5);
assert(halfHeight >= 0); assert(halfHeight >= 0);
// Compute the local AABB of the height field // Compute the local AABB of the height field
if (mUpAxis == 0) { if (mUpAxis == 0) {
mAABB.setMin(Vector3(-halfHeight, -mWidth * decimal(0.5), -mLength * decimal(0.5))); mAABB.setMin(Vector3(-halfHeight, -mWidth * float(0.5), -mLength * float(0.5)));
mAABB.setMax(Vector3(halfHeight, mWidth * decimal(0.5), mLength* decimal(0.5))); mAABB.setMax(Vector3(halfHeight, mWidth * float(0.5), mLength* float(0.5)));
} }
else if (mUpAxis == 1) { else if (mUpAxis == 1) {
mAABB.setMin(Vector3(-mWidth * decimal(0.5), -halfHeight, -mLength * decimal(0.5))); mAABB.setMin(Vector3(-mWidth * float(0.5), -halfHeight, -mLength * float(0.5)));
mAABB.setMax(Vector3(mWidth * decimal(0.5), halfHeight, mLength * decimal(0.5))); mAABB.setMax(Vector3(mWidth * float(0.5), halfHeight, mLength * float(0.5)));
} }
else if (mUpAxis == 2) { else if (mUpAxis == 2) {
mAABB.setMin(Vector3(-mWidth * decimal(0.5), -mLength * decimal(0.5), -halfHeight)); mAABB.setMin(Vector3(-mWidth * float(0.5), -mLength * float(0.5), -halfHeight));
mAABB.setMax(Vector3(mWidth * decimal(0.5), mLength * decimal(0.5), halfHeight)); mAABB.setMax(Vector3(mWidth * float(0.5), mLength * float(0.5), halfHeight));
} }
} }
@ -97,19 +78,19 @@ void HeightFieldShape::getLocalBounds(Vector3& min, Vector3& max) const {
void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const { void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const {
// Compute the non-scaled AABB // Compute the non-scaled AABB
Vector3 inverseScaling(decimal(1.0) / mScaling.x, decimal(1.0) / mScaling.y, decimal(1.0) / mScaling.z); Vector3 inverseScaling(float(1.0) / mScaling.x, float(1.0) / mScaling.y, float(1.0) / mScaling.z);
AABB aabb(localAABB.getMin() * inverseScaling, localAABB.getMax() * inverseScaling); AABB aabb(localAABB.getMin() * inverseScaling, localAABB.getMax() * inverseScaling);
// Compute the integer grid coordinates inside the area we need to test for collision // Compute the int32_teger grid coordinates inside the area we need to test for collision
int minGridCoords[3]; int32_t minGridCoords[3];
int maxGridCoords[3]; int32_t maxGridCoords[3];
computeMinMaxGridCoordinates(minGridCoords, maxGridCoords, aabb); computeMinMaxGridCoordinates(minGridCoords, maxGridCoords, aabb);
// Compute the starting and ending coords of the sub-grid according to the up axis // Compute the starting and ending coords of the sub-grid according to the up axis
int iMin = 0; int32_t iMin = 0;
int iMax = 0; int32_t iMax = 0;
int jMin = 0; int32_t jMin = 0;
int jMax = 0; int32_t jMax = 0;
switch(mUpAxis) { switch(mUpAxis) {
case 0 : iMin = clamp(minGridCoords[1], 0, mNbColumns - 1); case 0 : iMin = clamp(minGridCoords[1], 0, mNbColumns - 1);
iMax = clamp(maxGridCoords[1], 0, mNbColumns - 1); iMax = clamp(maxGridCoords[1], 0, mNbColumns - 1);
@ -134,8 +115,8 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB&
assert(jMax >= 0 && jMax < mNbRows); assert(jMax >= 0 && jMax < mNbRows);
// For each sub-grid points (except the last ones one each dimension) // For each sub-grid points (except the last ones one each dimension)
for (int i = iMin; i < iMax; i++) { for (int32_t i = iMin; i < iMax; i++) {
for (int j = jMin; j < jMax; j++) { for (int32_t j = jMin; j < jMax; j++) {
// Compute the four point of the current quad // Compute the four point of the current quad
Vector3 p1 = getVertexAt(i, j); Vector3 p1 = getVertexAt(i, j);
@ -160,9 +141,9 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB&
} }
} }
// Compute the min/max grid coords corresponding to the intersection of the AABB of the height field and // Compute the min/max grid coords corresponding to the int32_tersection of the AABB of the height field and
// the AABB to collide // the AABB to collide
void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, const AABB& aabbToCollide) const { 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 // Clamp the min/max coords of the AABB to collide inside the height field AABB
Vector3 minPoint = Vector3::max(aabbToCollide.getMin(), mAABB.getMin()); Vector3 minPoint = Vector3::max(aabbToCollide.getMin(), mAABB.getMin());
@ -174,11 +155,11 @@ void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoor
// Translate the min/max points such that the we compute grid points from [0 ... mNbWidthGridPoints] // 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 from [0 ... mNbLengthGridPoints] because the AABB coordinates range are [-mWdith/2 ... mWidth/2]
// and [-mLength/2 ... mLength/2] // and [-mLength/2 ... mLength/2]
const Vector3 translateVec = mAABB.getExtent() * decimal(0.5); const Vector3 translateVec = mAABB.getExtent() * float(0.5);
minPoint += translateVec; minPoint += translateVec;
maxPoint += translateVec; maxPoint += translateVec;
// Convert the floating min/max coords of the AABB into closest integer // 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 // grid values (note that we use the closest grid coordinate that is out
// of the AABB) // of the AABB)
minCoords[0] = computeIntegerGridValue(minPoint.x) - 1; minCoords[0] = computeIntegerGridValue(minPoint.x) - 1;
@ -212,21 +193,21 @@ bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh
} }
// Return the vertex (local-coordinates) of the height field at a given (x,y) position // Return the vertex (local-coordinates) of the height field at a given (x,y) position
Vector3 HeightFieldShape::getVertexAt(int x, int y) const { Vector3 HeightFieldShape::getVertexAt(int32_t x, int32_t y) const {
// Get the height value // Get the height value
const decimal height = getHeightAt(x, y); const float height = getHeightAt(x, y);
// Height values origin // Height values origin
const decimal heightOrigin = -(mMaxHeight - mMinHeight) * decimal(0.5) - mMinHeight; const float heightOrigin = -(mMaxHeight - mMinHeight) * float(0.5) - mMinHeight;
Vector3 vertex; Vector3 vertex;
switch (mUpAxis) { switch (mUpAxis) {
case 0: vertex = Vector3(heightOrigin + height, -mWidth * decimal(0.5) + x, -mLength * decimal(0.5) + y); case 0: vertex = Vector3(heightOrigin + height, -mWidth * float(0.5) + x, -mLength * float(0.5) + y);
break; break;
case 1: vertex = Vector3(-mWidth * decimal(0.5) + x, heightOrigin + height, -mLength * decimal(0.5) + y); case 1: vertex = Vector3(-mWidth * float(0.5) + x, heightOrigin + height, -mLength * float(0.5) + y);
break; break;
case 2: vertex = Vector3(-mWidth * decimal(0.5) + x, -mLength * decimal(0.5) + y, heightOrigin + height); case 2: vertex = Vector3(-mWidth * float(0.5) + x, -mLength * float(0.5) + y, heightOrigin + height);
break; break;
default: assert(false); default: assert(false);
} }
@ -240,7 +221,7 @@ Vector3 HeightFieldShape::getVertexAt(int x, int y) const {
void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) { void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) {
// Create a triangle collision shape // Create a triangle collision shape
decimal margin = mHeightFieldShape.getTriangleMargin(); float margin = mHeightFieldShape.getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType()); triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType());
@ -251,7 +232,7 @@ void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) {
// If the ray hit the collision shape // If the ray hit the collision shape
if (isTriangleHit && raycastInfo.hitFraction <= mSmallestHitFraction) { if (isTriangleHit && raycastInfo.hitFraction <= mSmallestHitFraction) {
assert(raycastInfo.hitFraction >= decimal(0.0)); assert(raycastInfo.hitFraction >= float(0.0));
m_raycastInfo.body = raycastInfo.body; m_raycastInfo.body = raycastInfo.body;
m_raycastInfo.proxyShape = raycastInfo.proxyShape; m_raycastInfo.proxyShape = raycastInfo.proxyShape;

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_HEIGHTFIELD_SHAPE_H
#define REACTPHYSICS3D_HEIGHTFIELD_SHAPE_H
// Libraries // Libraries
#include <ephysics/collision/shapes/ConcaveShape.h> #include <ephysics/collision/shapes/ConcaveShape.h>
@ -47,7 +26,7 @@ class TriangleOverlapCallback : public TriangleCallback {
ProxyShape* m_proxyShape; ProxyShape* m_proxyShape;
RaycastInfo& m_raycastInfo; RaycastInfo& m_raycastInfo;
bool mIsHit; bool mIsHit;
decimal mSmallestHitFraction; float mSmallestHitFraction;
const HeightFieldShape& mHeightFieldShape; const HeightFieldShape& mHeightFieldShape;
public: public:
@ -72,8 +51,8 @@ class TriangleOverlapCallback : public TriangleCallback {
/** /**
* This class represents a static height field that can be used to represent * 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 * 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 into the shape * 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 integer, float or double. * but are shared instead. The height values can be of type int32_teger, float or double.
* When creating a HeightFieldShape, you need to specify the minimum and maximum height value of * When creating a HeightFieldShape, you need to specify the minimum and maximum height value of
* your height field. Note that the HeightFieldShape will be re-centered based on its AABB. It means * your height field. Note that the HeightFieldShape will be re-centered based on its AABB. It means
* that for instance, if the minimum height value is -200 and the maximum value is 400, the final * that for instance, if the minimum height value is -200 and the maximum value is 400, the final
@ -91,28 +70,28 @@ class HeightFieldShape : public ConcaveShape {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Number of columns in the grid of the height field /// Number of columns in the grid of the height field
int mNbColumns; int32_t mNbColumns;
/// Number of rows in the grid of the height field /// Number of rows in the grid of the height field
int mNbRows; int32_t mNbRows;
/// Height field width /// Height field width
decimal mWidth; float mWidth;
/// Height field length /// Height field length
decimal mLength; float mLength;
/// Minimum height of the height field /// Minimum height of the height field
decimal mMinHeight; float mMinHeight;
/// Maximum height of the height field /// Maximum height of the height field
decimal mMaxHeight; float mMaxHeight;
/// Up axis direction (0 => x, 1 => y, 2 => z) /// Up axis direction (0 => x, 1 => y, 2 => z)
int mUpAxis; int32_t mUpAxis;
/// Height values scale for height field with integer height values /// Height values scale for height field with int32_teger height values
decimal mIntegerHeightScale; float mIntegerHeightScale;
/// Data type of the height values /// Data type of the height values
HeightDataType mHeightDataType; HeightDataType mHeightDataType;
@ -137,41 +116,41 @@ class HeightFieldShape : public ConcaveShape {
/// Return the number of bytes used by the collision shape /// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
/// Insert all the triangles into the dynamic AABB tree /// Insert all the triangles int32_to the dynamic AABB tree
void initBVHTree(); void initBVHTree();
/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
/// given the start vertex index pointer of the triangle. /// given the start vertex index pointer of the triangle.
void getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex, void getTriangleVerticesWithIndexPointer(int32_t subPart, int32_t triangleIndex,
Vector3* outTriangleVertices) const; Vector3* outTriangleVertices) const;
/// Return the vertex (local-coordinates) of the height field at a given (x,y) position /// Return the vertex (local-coordinates) of the height field at a given (x,y) position
Vector3 getVertexAt(int x, int y) const; Vector3 getVertexAt(int32_t x, int32_t y) const;
/// Return the height of a given (x,y) point in the height field /// Return the height of a given (x,y) point in the height field
decimal getHeightAt(int x, int y) const; float getHeightAt(int32_t x, int32_t y) const;
/// Return the closest inside integer grid value of a given floating grid value /// Return the closest inside int32_teger grid value of a given floating grid value
int computeIntegerGridValue(decimal value) const; int32_t computeIntegerGridValue(float value) const;
/// Compute the min/max grid coords corresponding to the intersection of the AABB of the height field and the AABB to collide /// Compute the min/max grid coords corresponding to the int32_tersection of the AABB of the height field and the AABB to collide
void computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, const AABB& aabbToCollide) const; void computeMinMaxGridCoordinates(int32_t* minCoords, int32_t* maxCoords, const AABB& aabbToCollide) const;
public: public:
/// Constructor /// Constructor
HeightFieldShape(int nbGridColumns, int nbGridRows, decimal minHeight, decimal maxHeight, HeightFieldShape(int32_t nbGridColumns, int32_t nbGridRows, float minHeight, float maxHeight,
const void* heightFieldData, HeightDataType dataType, const void* heightFieldData, HeightDataType dataType,
int upAxis = 1, decimal integerHeightScale = 1.0f); int32_t upAxis = 1, float int32_tegerHeightScale = 1.0f);
/// Destructor /// Destructor
~HeightFieldShape(); ~HeightFieldShape();
/// Return the number of rows in the height field /// Return the number of rows in the height field
int getNbRows() const; int32_t getNbRows() const;
/// Return the number of columns in the height field /// Return the number of columns in the height field
int getNbColumns() const; int32_t getNbColumns() const;
/// Return the type of height value in the height field /// Return the type of height value in the height field
HeightDataType getHeightDataType() const; HeightDataType getHeightDataType() const;
@ -183,7 +162,7 @@ class HeightFieldShape : public ConcaveShape {
virtual void setLocalScaling(const Vector3& scaling); virtual void setLocalScaling(const Vector3& scaling);
/// Return the local inertia tensor of the collision shape /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
/// Use a callback method on all triangles of the concave shape inside a given AABB /// Use a callback method on all triangles of the concave shape inside a given AABB
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const; virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const;
@ -195,12 +174,12 @@ class HeightFieldShape : public ConcaveShape {
}; };
// Return the number of rows in the height field // Return the number of rows in the height field
inline int HeightFieldShape::getNbRows() const { inline int32_t HeightFieldShape::getNbRows() const {
return mNbRows; return mNbRows;
} }
// Return the number of columns in the height field // Return the number of columns in the height field
inline int HeightFieldShape::getNbColumns() const { inline int32_t HeightFieldShape::getNbColumns() const {
return mNbColumns; return mNbColumns;
} }
@ -220,19 +199,19 @@ inline void HeightFieldShape::setLocalScaling(const Vector3& scaling) {
} }
// Return the height of a given (x,y) point in the height field // Return the height of a given (x,y) point in the height field
inline decimal HeightFieldShape::getHeightAt(int x, int y) const { inline float HeightFieldShape::getHeightAt(int32_t x, int32_t y) const {
switch(mHeightDataType) { switch(mHeightDataType) {
case HEIGHT_FLOAT_TYPE : return ((float*)mHeightFieldData)[y * mNbColumns + x]; case HEIGHT_FLOAT_TYPE : return ((float*)mHeightFieldData)[y * mNbColumns + x];
case HEIGHT_DOUBLE_TYPE : return ((double*)mHeightFieldData)[y * mNbColumns + x]; case HEIGHT_DOUBLE_TYPE : return ((double*)mHeightFieldData)[y * mNbColumns + x];
case HEIGHT_INT_TYPE : return ((int*)mHeightFieldData)[y * mNbColumns + x] * mIntegerHeightScale; case HEIGHT_INT_TYPE : return ((int32_t*)mHeightFieldData)[y * mNbColumns + x] * mIntegerHeightScale;
default: assert(false); return 0; default: assert(false); return 0;
} }
} }
// Return the closest inside integer grid value of a given floating grid value // Return the closest inside int32_teger grid value of a given floating grid value
inline int HeightFieldShape::computeIntegerGridValue(decimal value) const { inline int32_t HeightFieldShape::computeIntegerGridValue(float value) const {
return (value < decimal(0.0)) ? value - decimal(0.5) : value + decimal(0.5); return (value < float(0.0)) ? value - float(0.5) : value + float(0.5);
} }
// Return the local inertia tensor // Return the local inertia tensor
@ -241,7 +220,7 @@ inline int HeightFieldShape::computeIntegerGridValue(decimal value) const {
* coordinates * coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape * @param mass Mass to use to compute the inertia tensor of the collision shape
*/ */
inline void HeightFieldShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { inline void HeightFieldShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
// Default inertia tensor // Default inertia tensor
// Note that this is not very realistic for a concave triangle mesh. // Note that this is not very realistic for a concave triangle mesh.
@ -253,5 +232,4 @@ inline void HeightFieldShape::computeLocalInertiaTensor(Matrix3x3& tensor, decim
} }
} }
#endif

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

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/shapes/SphereShape.h> #include <ephysics/collision/shapes/SphereShape.h>
@ -35,8 +16,8 @@ using namespace reactphysics3d;
/** /**
* @param radius Radius of the sphere (in meters) * @param radius Radius of the sphere (in meters)
*/ */
SphereShape::SphereShape(decimal radius) : ConvexShape(SPHERE, radius) { SphereShape::SphereShape(float radius) : ConvexShape(SPHERE, radius) {
assert(radius > decimal(0.0)); assert(radius > float(0.0));
} }
// Destructor // Destructor
@ -48,35 +29,35 @@ SphereShape::~SphereShape() {
bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const Vector3 m = ray.point1; const Vector3 m = ray.point1;
decimal c = m.dot(m) - mMargin * mMargin; float c = m.dot(m) - mMargin * mMargin;
// If the origin of the ray is inside the sphere, we return no intersection // If the origin of the ray is inside the sphere, we return no int32_tersection
if (c < decimal(0.0)) return false; if (c < float(0.0)) return false;
const Vector3 rayDirection = ray.point2 - ray.point1; const Vector3 rayDirection = ray.point2 - ray.point1;
decimal b = m.dot(rayDirection); float b = m.dot(rayDirection);
// If the origin of the ray is outside the sphere and the ray // If the origin of the ray is outside the sphere and the ray
// is pointing away from the sphere, there is no intersection // is pointing away from the sphere, there is no int32_tersection
if (b > decimal(0.0)) return false; if (b > float(0.0)) return false;
decimal raySquareLength = rayDirection.lengthSquare(); float raySquareLength = rayDirection.lengthSquare();
// Compute the discriminant of the quadratic equation // Compute the discriminant of the quadratic equation
decimal discriminant = b * b - raySquareLength * c; float discriminant = b * b - raySquareLength * c;
// If the discriminant is negative or the ray length is very small, there is no intersection // If the discriminant is negative or the ray length is very small, there is no int32_tersection
if (discriminant < decimal(0.0) || raySquareLength < MACHINE_EPSILON) return false; if (discriminant < float(0.0) || raySquareLength < MACHINE_EPSILON) return false;
// Compute the solution "t" closest to the origin // Compute the solution "t" closest to the origin
decimal t = -b - std::sqrt(discriminant); float t = -b - std::sqrt(discriminant);
assert(t >= decimal(0.0)); assert(t >= float(0.0));
// If the hit point is withing the segment ray fraction // If the hit point is withing the segment ray fraction
if (t < ray.maxFraction * raySquareLength) { if (t < ray.maxFraction * raySquareLength) {
// Compute the intersection information // Compute the int32_tersection information
t /= raySquareLength; t /= raySquareLength;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_SPHERE_SHAPE_H
#define REACTPHYSICS3D_SPHERE_SHAPE_H
// Libraries // Libraries
#include <ephysics/collision/shapes/ConvexShape.h> #include <ephysics/collision/shapes/ConvexShape.h>
@ -75,13 +54,13 @@ class SphereShape : public ConvexShape {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
SphereShape(decimal radius); SphereShape(float radius);
/// Destructor /// Destructor
virtual ~SphereShape(); virtual ~SphereShape();
/// Return the radius of the sphere /// Return the radius of the sphere
decimal getRadius() const; float getRadius() const;
/// Set the scaling vector of the collision shape /// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling); virtual void setLocalScaling(const Vector3& scaling);
@ -90,7 +69,7 @@ class SphereShape : public ConvexShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const; virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// Return the local inertia tensor of the collision shape /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
/// Update the AABB of a body using its collision shape /// 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 Transform& transform) const;
@ -100,7 +79,7 @@ class SphereShape : public ConvexShape {
/** /**
* @return Radius of the sphere (in meters) * @return Radius of the sphere (in meters)
*/ */
inline decimal SphereShape::getRadius() const { inline float SphereShape::getRadius() const {
return mMargin; return mMargin;
} }
@ -121,7 +100,7 @@ inline size_t SphereShape::getSizeInBytes() const {
inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction, inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const { void** cachedCollisionData) const {
// Return the center of the sphere (the radius is taken into account in the object margin) // 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 Vector3(0.0, 0.0, 0.0);
} }
@ -150,8 +129,8 @@ inline void SphereShape::getLocalBounds(Vector3& min, Vector3& max) const {
* coordinates * coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape * @param mass Mass to use to compute the inertia tensor of the collision shape
*/ */
inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
decimal diag = decimal(0.4) * mass * mMargin * mMargin; float diag = float(0.4) * mass * mMargin * mMargin;
tensor.setAllValues(diag, 0.0, 0.0, tensor.setAllValues(diag, 0.0, 0.0,
0.0, diag, 0.0, 0.0, diag, 0.0,
0.0, 0.0, diag); 0.0, 0.0, diag);
@ -179,5 +158,3 @@ inline bool SphereShape::testPointInside(const Vector3& localPoint, ProxyShape*
} }
} }
#endif

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

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/collision/shapes/TriangleShape.h> #include <ephysics/collision/shapes/TriangleShape.h>
@ -39,7 +20,7 @@ using namespace reactphysics3d;
* @param point3 Third point of the triangle * @param point3 Third point of the triangle
* @param margin The collision margin (in meters) around the collision shape * @param margin The collision margin (in meters) around the collision shape
*/ */
TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, decimal margin) TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, float margin)
: ConvexShape(TRIANGLE, margin) { : ConvexShape(TRIANGLE, margin) {
mPoints[0] = point1; mPoints[0] = point1;
mPoints[1] = point2; mPoints[1] = point2;
@ -67,31 +48,31 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
// Test if the line PQ is inside the eges BC, CA and AB. We use the triple // Test if the line PQ is inside the eges BC, CA and AB. We use the triple
// product for this test. // product for this test.
const Vector3 m = pq.cross(pc); const Vector3 m = pq.cross(pc);
decimal u = pb.dot(m); float u = pb.dot(m);
if (m_raycastTestType == FRONT) { if (m_raycastTestType == FRONT) {
if (u < decimal(0.0)) return false; if (u < float(0.0)) return false;
} }
else if (m_raycastTestType == BACK) { else if (m_raycastTestType == BACK) {
if (u > decimal(0.0)) return false; if (u > float(0.0)) return false;
} }
decimal v = -pa.dot(m); float v = -pa.dot(m);
if (m_raycastTestType == FRONT) { if (m_raycastTestType == FRONT) {
if (v < decimal(0.0)) return false; if (v < float(0.0)) return false;
} }
else if (m_raycastTestType == BACK) { else if (m_raycastTestType == BACK) {
if (v > decimal(0.0)) return false; if (v > float(0.0)) return false;
} }
else if (m_raycastTestType == FRONT_AND_BACK) { else if (m_raycastTestType == FRONT_AND_BACK) {
if (!sameSign(u, v)) return false; if (!sameSign(u, v)) return false;
} }
decimal w = pa.dot(pq.cross(pb)); float w = pa.dot(pq.cross(pb));
if (m_raycastTestType == FRONT) { if (m_raycastTestType == FRONT) {
if (w < decimal(0.0)) return false; if (w < float(0.0)) return false;
} }
else if (m_raycastTestType == BACK) { else if (m_raycastTestType == BACK) {
if (w > decimal(0.0)) return false; if (w > float(0.0)) return false;
} }
else if (m_raycastTestType == FRONT_AND_BACK) { else if (m_raycastTestType == FRONT_AND_BACK) {
if (!sameSign(u, w)) return false; if (!sameSign(u, w)) return false;
@ -101,20 +82,20 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
if (approxEqual(u, 0) && approxEqual(v, 0) && approxEqual(w, 0)) return false; if (approxEqual(u, 0) && approxEqual(v, 0) && approxEqual(w, 0)) return false;
// Compute the barycentric coordinates (u, v, w) to determine the // Compute the barycentric coordinates (u, v, w) to determine the
// intersection point R, R = u * a + v * b + w * c // int32_tersection point R, R = u * a + v * b + w * c
decimal denom = decimal(1.0) / (u + v + w); float denom = float(1.0) / (u + v + w);
u *= denom; u *= denom;
v *= denom; v *= denom;
w *= denom; w *= denom;
// Compute the local hit point using the barycentric coordinates // Compute the local hit point using the barycentric coordinates
const Vector3 localHitPoint = u * mPoints[0] + v * mPoints[1] + w * mPoints[2]; const Vector3 localHitPoint = u * mPoints[0] + v * mPoints[1] + w * mPoints[2];
const decimal hitFraction = (localHitPoint - ray.point1).length() / pq.length(); const float hitFraction = (localHitPoint - ray.point1).length() / pq.length();
if (hitFraction < decimal(0.0) || hitFraction > ray.maxFraction) return false; if (hitFraction < float(0.0) || hitFraction > ray.maxFraction) return false;
Vector3 localHitNormal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]); Vector3 localHitNormal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]);
if (localHitNormal.dot(pq) > decimal(0.0)) localHitNormal = -localHitNormal; if (localHitNormal.dot(pq) > float(0.0)) localHitNormal = -localHitNormal;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;

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

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_TRIANGLE_SHAPE_H
#define REACTPHYSICS3D_TRIANGLE_SHAPE_H
// Libraries // Libraries
#include <ephysics/mathematics/mathematics.h> #include <ephysics/mathematics/mathematics.h>
@ -90,7 +69,7 @@ class TriangleShape : public ConvexShape {
/// Constructor /// Constructor
TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3,
decimal margin = OBJECT_MARGIN); float margin = OBJECT_MARGIN);
/// Destructor /// Destructor
virtual ~TriangleShape(); virtual ~TriangleShape();
@ -102,7 +81,7 @@ class TriangleShape : public ConvexShape {
virtual void setLocalScaling(const Vector3& scaling); virtual void setLocalScaling(const Vector3& scaling);
/// Return the local inertia tensor of the collision shape /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const;
/// Update the AABB of a body using its collision shape /// 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 Transform& transform) const;
@ -114,7 +93,7 @@ class TriangleShape : public ConvexShape {
void setRaycastTestType(TriangleRaycastSide testType); void setRaycastTestType(TriangleRaycastSide testType);
/// Return the coordinates of a given vertex of the triangle /// Return the coordinates of a given vertex of the triangle
Vector3 getVertex(int index) const; Vector3 getVertex(int32_t index) const;
// ---------- Friendship ---------- // // ---------- Friendship ---------- //
@ -168,7 +147,7 @@ inline void TriangleShape::setLocalScaling(const Vector3& scaling) {
* coordinates * coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape * @param mass Mass to use to compute the inertia tensor of the collision shape
*/ */
inline void TriangleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { inline void TriangleShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
tensor.setToZero(); tensor.setToZero();
} }
@ -213,12 +192,10 @@ inline void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) {
/** /**
* @param index Index (0 to 2) of a vertex of the triangle * @param index Index (0 to 2) of a vertex of the triangle
*/ */
inline Vector3 TriangleShape::getVertex(int index) const { inline Vector3 TriangleShape::getVertex(int32_t index) const {
assert(index >= 0 && index < 3); assert(index >= 0 && index < 3);
return mPoints[index]; return mPoints[index];
} }
} }
#endif

187
ephysics/configuration.h
View File

@ -1,149 +1,108 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CONFIGURATION_H
#define REACTPHYSICS3D_CONFIGURATION_H
// Libraries // Libraries
#include <limits> #include <limits>
#include <cfloat> #include <cfloat>
#include <utility> #include <utility>
#include <ephysics/decimal.h> #include <cstdint>
// Windows platform
#if defined(WIN32) ||defined(_WIN32) || defined(_WIN64) ||defined(__WIN32__) || defined(__WINDOWS__)
#define WINDOWS_OS
#elif defined(__APPLE__) // Apple platform
#define APPLE_OS
#elif defined(__linux__) || defined(linux) || defined(__linux) // Linux platform
#define LINUX_OS
#endif
/// Namespace reactphysics3d /// Namespace reactphysics3d
namespace reactphysics3d { namespace reactphysics3d {
// ------------------- Type definitions ------------------- //
typedef uint64_t bodyindex;
typedef std::pair<bodyindex, bodyindex> bodyindexpair;
// ------------------- Type definitions ------------------- // // ------------------- Enumerations ------------------- //
typedef unsigned int uint; /// Position correction technique used in the constraint solver (for joints).
typedef long unsigned int luint; /// BAUMGARTE_JOINTS : Faster but can be innacurate in some situations.
typedef luint bodyindex; /// NON_LINEAR_GAUSS_SEIDEL : Slower but more precise. This is the option used by default.
typedef std::pair<bodyindex, bodyindex> bodyindexpair; enum JointsPositionCorrectionTechnique {BAUMGARTE_JOINTS, NON_LINEAR_GAUSS_SEIDEL};
typedef signed short int16; /// Position correction technique used in the contact solver (for contacts)
typedef signed int int32; /// BAUMGARTE_CONTACTS : Faster but can be innacurate and can lead to unexpected bounciness
typedef unsigned short uint16; /// in some situations (due to error correction factor being added to
typedef unsigned int uint32; /// the bodies momentum).
/// SPLIT_IMPULSES : A bit slower but the error correction factor is not added to the
/// bodies momentum. This is the option used by default.
enum ContactsPositionCorrectionTechnique {BAUMGARTE_CONTACTS, SPLIT_IMPULSES};
// ------------------- Enumerations ------------------- // // ------------------- Constants ------------------- //
/// Position correction technique used in the constraint solver (for joints). /// Smallest float value (negative)
/// BAUMGARTE_JOINTS : Faster but can be innacurate in some situations. const float DECIMAL_SMALLEST = - std::numeric_limits<float>::max();
/// NON_LINEAR_GAUSS_SEIDEL : Slower but more precise. This is the option used by default.
enum JointsPositionCorrectionTechnique {BAUMGARTE_JOINTS, NON_LINEAR_GAUSS_SEIDEL};
/// Position correction technique used in the contact solver (for contacts) /// Maximum float value
/// BAUMGARTE_CONTACTS : Faster but can be innacurate and can lead to unexpected bounciness const float DECIMAL_LARGEST = std::numeric_limits<float>::max();
/// in some situations (due to error correction factor being added to
/// the bodies momentum).
/// SPLIT_IMPULSES : A bit slower but the error correction factor is not added to the
/// bodies momentum. This is the option used by default.
enum ContactsPositionCorrectionTechnique {BAUMGARTE_CONTACTS, SPLIT_IMPULSES};
// ------------------- Constants ------------------- // /// Machine epsilon
const float MACHINE_EPSILON = std::numeric_limits<float>::epsilon();
/// Smallest decimal value (negative) /// Pi constant
const decimal DECIMAL_SMALLEST = - std::numeric_limits<decimal>::max(); const float PI = float(3.14159265);
/// Maximum decimal value /// 2*Pi constant
const decimal DECIMAL_LARGEST = std::numeric_limits<decimal>::max(); const float PI_TIMES_2 = float(6.28318530);
/// Machine epsilon /// Default friction coefficient for a rigid body
const decimal MACHINE_EPSILON = std::numeric_limits<decimal>::epsilon(); const float DEFAULT_FRICTION_COEFFICIENT = float(0.3);
/// Pi constant /// Default bounciness factor for a rigid body
const decimal PI = decimal(3.14159265); const float DEFAULT_BOUNCINESS = float(0.5);
/// 2*Pi constant /// Default rolling resistance
const decimal PI_TIMES_2 = decimal(6.28318530); const float DEFAULT_ROLLING_RESISTANCE = float(0.0);
/// Default friction coefficient for a rigid body /// True if the spleeping technique is enabled
const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3); const bool SPLEEPING_ENABLED = true;
/// Default bounciness factor for a rigid body /// Object margin for collision detection in meters (for the GJK-EPA Algorithm)
const decimal DEFAULT_BOUNCINESS = decimal(0.5); const float OBJECT_MARGIN = float(0.04);
/// Default rolling resistance /// Distance threshold for two contact points for a valid persistent contact (in meters)
const decimal DEFAULT_ROLLING_RESISTANCE = decimal(0.0); const float PERSISTENT_CONTACT_DIST_THRESHOLD = float(0.03);
/// True if the spleeping technique is enabled /// Velocity threshold for contact velocity restitution
const bool SPLEEPING_ENABLED = true; const float RESTITUTION_VELOCITY_THRESHOLD = float(1.0);
/// Object margin for collision detection in meters (for the GJK-EPA Algorithm) /// Number of iterations when solving the velocity constraints of the Sequential Impulse technique
const decimal OBJECT_MARGIN = decimal(0.04); const uint32_t DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 10;
/// Distance threshold for two contact points for a valid persistent contact (in meters) /// Number of iterations when solving the position constraints of the Sequential Impulse technique
const decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03); const uint32_t DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5;
/// Velocity threshold for contact velocity restitution /// Time (in seconds) that a body must stay still to be considered sleeping
const decimal RESTITUTION_VELOCITY_THRESHOLD = decimal(1.0); const float DEFAULT_TIME_BEFORE_SLEEP = 1.0f;
/// Number of iterations when solving the velocity constraints of the Sequential Impulse technique /// A body with a linear velocity smaller than the sleep linear velocity (in m/s)
const uint DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 10; /// might enter sleeping mode.
const float DEFAULT_SLEEP_LINEAR_VELOCITY = float(0.02);
/// Number of iterations when solving the position constraints of the Sequential Impulse technique /// A body with angular velocity smaller than the sleep angular velocity (in rad/s)
const uint DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5; /// might enter sleeping mode
const float DEFAULT_SLEEP_ANGULAR_VELOCITY = float(3.0 * (PI / 180.0));
/// Time (in seconds) that a body must stay still to be considered sleeping /// In the broad-phase collision detection (dynamic AABB tree), the AABBs are
const float DEFAULT_TIME_BEFORE_SLEEP = 1.0f; /// inflated with a constant gap to allow the collision shape to move a little bit
/// without triggering a large modification of the tree which can be costly
const float DYNAMIC_TREE_AABB_GAP = float(0.1);
/// A body with a linear velocity smaller than the sleep linear velocity (in m/s) /// In the broad-phase collision detection (dynamic AABB tree), the AABBs are
/// might enter sleeping mode. /// also inflated in direction of the linear motion of the body by mutliplying the
const decimal DEFAULT_SLEEP_LINEAR_VELOCITY = decimal(0.02); /// followin constant with the linear velocity and the elapsed time between two frames.
const float DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = float(1.7);
/// A body with angular velocity smaller than the sleep angular velocity (in rad/s) /// Maximum number of contact manifolds in an overlapping pair that involves two
/// might enter sleeping mode /// convex collision shapes.
const decimal DEFAULT_SLEEP_ANGULAR_VELOCITY = decimal(3.0 * (PI / 180.0)); const int32_t NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1;
/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are /// Maximum number of contact manifolds in an overlapping pair that involves at
/// inflated with a constant gap to allow the collision shape to move a little bit /// least one concave collision shape.
/// without triggering a large modification of the tree which can be costly const int32_t NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3;
const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1);
/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are
/// also inflated in direction of the linear motion of the body by mutliplying the
/// followin constant with the linear velocity and the elapsed time between two frames.
const decimal DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = decimal(1.7);
/// Maximum number of contact manifolds in an overlapping pair that involves two
/// convex collision shapes.
const int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1;
/// Maximum number of contact manifolds in an overlapping pair that involves at
/// least one concave collision shape.
const int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3;
} }
#endif

45
ephysics/constraint/BallAndSocketJoint.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/constraint/BallAndSocketJoint.h> #include <ephysics/constraint/BallAndSocketJoint.h>
@ -30,7 +11,7 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Static variables definition // Static variables definition
const decimal BallAndSocketJoint::BETA = decimal(0.2); const float BallAndSocketJoint::BETA = float(0.2);
// Constructor // Constructor
BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo) BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo)
@ -72,7 +53,7 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix) // Compute the matrix K=JM^-1J^t (3x3 matrix)
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) + 0, 0, inverseMassBodies) +
@ -88,7 +69,7 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
// Compute the bias "b" of the constraint // Compute the bias "b" of the constraint
mBiasVector.setToZero(); mBiasVector.setToZero();
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
decimal biasFactor = (BETA / constraintSolverData.timeStep); float biasFactor = (BETA / constraintSolverData.timeStep);
mBiasVector = biasFactor * (x2 + mR2World - x1 - mR1World); mBiasVector = biasFactor * (x2 + mR2World - x1 - mR1World);
} }
@ -171,8 +152,8 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse; float inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse; float inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inverse inertia tensors // Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld(); mI1 = mBody1->getInertiaTensorInverseWorld();
@ -187,7 +168,7 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints // Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints
decimal inverseMassBodies = inverseMassBody1 + inverseMassBody2; float inverseMassBodies = inverseMassBody1 + inverseMassBody2;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) + 0, 0, inverseMassBodies) +
@ -216,7 +197,7 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
// Update the body center of mass and orientation of body 1 // Update the body center of mass and orientation of body 1
x1 += v1; x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize(); q1.normalize();
// Compute the impulse of body 2 // Compute the impulse of body 2
@ -228,7 +209,7 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize(); q2.normalize();
} }

37
ephysics/constraint/BallAndSocketJoint.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_BALL_AND_SOCKET_JOINT_H
#define REACTPHYSICS3D_BALL_AND_SOCKET_JOINT_H
// Libraries // Libraries
#include <ephysics/constraint/Joint.h> #include <ephysics/constraint/Joint.h>
@ -72,7 +51,7 @@ class BallAndSocketJoint : public Joint {
// -------------------- Constants -------------------- // // -------------------- Constants -------------------- //
// Beta value for the bias factor of position correction // Beta value for the bias factor of position correction
static const decimal BETA; static const float BETA;
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
@ -143,5 +122,3 @@ inline size_t BallAndSocketJoint::getSizeInBytes() const {
} }
} }
#endif

29
ephysics/constraint/ContactPoint.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/constraint/ContactPoint.h> #include <ephysics/constraint/ContactPoint.h>

79
ephysics/constraint/ContactPoint.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CONTACT_POINT_H
#define REACTPHYSICS3D_CONTACT_POINT_H
// Libraries // Libraries
#include <ephysics/body/CollisionBody.h> #include <ephysics/body/CollisionBody.h>
@ -69,7 +48,7 @@ struct ContactPointInfo {
Vector3 normal; Vector3 normal;
/// Penetration depth of the contact /// Penetration depth of the contact
decimal penetrationDepth; float penetrationDepth;
/// Contact point of body 1 in local space of body 1 /// Contact point of body 1 in local space of body 1
Vector3 localPoint1; Vector3 localPoint1;
@ -81,7 +60,7 @@ struct ContactPointInfo {
/// Constructor /// Constructor
ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, const CollisionShape* collShape1, ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, const CollisionShape* collShape1,
const CollisionShape* collShape2, const Vector3& normal, decimal penetrationDepth, const CollisionShape* collShape2, const Vector3& normal, float penetrationDepth,
const Vector3& localPoint1, const Vector3& localPoint2) const Vector3& localPoint1, const Vector3& localPoint2)
: shape1(proxyShape1), shape2(proxyShape2), collisionShape1(collShape1), collisionShape2(collShape2), : shape1(proxyShape1), shape2(proxyShape2), collisionShape1(collShape1), collisionShape2(collShape2),
normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1), normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1),
@ -111,7 +90,7 @@ class ContactPoint {
const Vector3 mNormal; const Vector3 mNormal;
/// Penetration depth /// Penetration depth
decimal mPenetrationDepth; float mPenetrationDepth;
/// Contact point on body 1 in local space of body 1 /// Contact point on body 1 in local space of body 1
const Vector3 mLocalPointOnBody1; const Vector3 mLocalPointOnBody1;
@ -132,13 +111,13 @@ class ContactPoint {
Vector3 mFrictionVectors[2]; Vector3 mFrictionVectors[2];
/// Cached penetration impulse /// Cached penetration impulse
decimal mPenetrationImpulse; float mPenetrationImpulse;
/// Cached first friction impulse /// Cached first friction impulse
decimal mFrictionImpulse1; float mFrictionImpulse1;
/// Cached second friction impulse /// Cached second friction impulse
decimal mFrictionImpulse2; float mFrictionImpulse2;
/// Cached rolling resistance impulse /// Cached rolling resistance impulse
Vector3 mRollingResistanceImpulse; Vector3 mRollingResistanceImpulse;
@ -171,7 +150,7 @@ class ContactPoint {
Vector3 getNormal() const; Vector3 getNormal() const;
/// Set the penetration depth of the contact /// Set the penetration depth of the contact
void setPenetrationDepth(decimal penetrationDepth); void setPenetrationDepth(float penetrationDepth);
/// Return the contact local point on body 1 /// Return the contact local point on body 1
Vector3 getLocalPointOnBody1() const; Vector3 getLocalPointOnBody1() const;
@ -186,25 +165,25 @@ class ContactPoint {
Vector3 getWorldPointOnBody2() const; Vector3 getWorldPointOnBody2() const;
/// Return the cached penetration impulse /// Return the cached penetration impulse
decimal getPenetrationImpulse() const; float getPenetrationImpulse() const;
/// Return the cached first friction impulse /// Return the cached first friction impulse
decimal getFrictionImpulse1() const; float getFrictionImpulse1() const;
/// Return the cached second friction impulse /// Return the cached second friction impulse
decimal getFrictionImpulse2() const; float getFrictionImpulse2() const;
/// Return the cached rolling resistance impulse /// Return the cached rolling resistance impulse
Vector3 getRollingResistanceImpulse() const; Vector3 getRollingResistanceImpulse() const;
/// Set the cached penetration impulse /// Set the cached penetration impulse
void setPenetrationImpulse(decimal impulse); void setPenetrationImpulse(float impulse);
/// Set the first cached friction impulse /// Set the first cached friction impulse
void setFrictionImpulse1(decimal impulse); void setFrictionImpulse1(float impulse);
/// Set the second cached friction impulse /// Set the second cached friction impulse
void setFrictionImpulse2(decimal impulse); void setFrictionImpulse2(float impulse);
/// Set the cached rolling resistance impulse /// Set the cached rolling resistance impulse
void setRollingResistanceImpulse(const Vector3& impulse); void setRollingResistanceImpulse(const Vector3& impulse);
@ -234,7 +213,7 @@ class ContactPoint {
void setFrictionVector2(const Vector3& frictionVector2); void setFrictionVector2(const Vector3& frictionVector2);
/// Return the penetration depth /// Return the penetration depth
decimal getPenetrationDepth() const; float getPenetrationDepth() const;
/// Return the number of bytes used by the contact point /// Return the number of bytes used by the contact point
size_t getSizeInBytes() const; size_t getSizeInBytes() const;
@ -256,7 +235,7 @@ inline Vector3 ContactPoint::getNormal() const {
} }
// Set the penetration depth of the contact // Set the penetration depth of the contact
inline void ContactPoint::setPenetrationDepth(decimal penetrationDepth) { inline void ContactPoint::setPenetrationDepth(float penetrationDepth) {
this->mPenetrationDepth = penetrationDepth; this->mPenetrationDepth = penetrationDepth;
} }
@ -281,17 +260,17 @@ inline Vector3 ContactPoint::getWorldPointOnBody2() const {
} }
// Return the cached penetration impulse // Return the cached penetration impulse
inline decimal ContactPoint::getPenetrationImpulse() const { inline float ContactPoint::getPenetrationImpulse() const {
return mPenetrationImpulse; return mPenetrationImpulse;
} }
// Return the cached first friction impulse // Return the cached first friction impulse
inline decimal ContactPoint::getFrictionImpulse1() const { inline float ContactPoint::getFrictionImpulse1() const {
return mFrictionImpulse1; return mFrictionImpulse1;
} }
// Return the cached second friction impulse // Return the cached second friction impulse
inline decimal ContactPoint::getFrictionImpulse2() const { inline float ContactPoint::getFrictionImpulse2() const {
return mFrictionImpulse2; return mFrictionImpulse2;
} }
@ -301,17 +280,17 @@ inline Vector3 ContactPoint::getRollingResistanceImpulse() const {
} }
// Set the cached penetration impulse // Set the cached penetration impulse
inline void ContactPoint::setPenetrationImpulse(decimal impulse) { inline void ContactPoint::setPenetrationImpulse(float impulse) {
mPenetrationImpulse = impulse; mPenetrationImpulse = impulse;
} }
// Set the first cached friction impulse // Set the first cached friction impulse
inline void ContactPoint::setFrictionImpulse1(decimal impulse) { inline void ContactPoint::setFrictionImpulse1(float impulse) {
mFrictionImpulse1 = impulse; mFrictionImpulse1 = impulse;
} }
// Set the second cached friction impulse // Set the second cached friction impulse
inline void ContactPoint::setFrictionImpulse2(decimal impulse) { inline void ContactPoint::setFrictionImpulse2(float impulse) {
mFrictionImpulse2 = impulse; mFrictionImpulse2 = impulse;
} }
@ -361,7 +340,7 @@ inline void ContactPoint::setFrictionVector2(const Vector3& frictionVector2) {
} }
// Return the penetration depth of the contact // Return the penetration depth of the contact
inline decimal ContactPoint::getPenetrationDepth() const { inline float ContactPoint::getPenetrationDepth() const {
return mPenetrationDepth; return mPenetrationDepth;
} }
@ -371,5 +350,3 @@ inline size_t ContactPoint::getSizeInBytes() const {
} }
} }
#endif

61
ephysics/constraint/FixedJoint.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/constraint/FixedJoint.h> #include <ephysics/constraint/FixedJoint.h>
@ -30,7 +11,7 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Static variables definition // Static variables definition
const decimal FixedJoint::BETA = decimal(0.2); const float FixedJoint::BETA = float(0.2);
// Constructor // Constructor
FixedJoint::FixedJoint(const FixedJointInfo& jointInfo) FixedJoint::FixedJoint(const FixedJointInfo& jointInfo)
@ -80,7 +61,7 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) + 0, 0, inverseMassBodies) +
@ -94,7 +75,7 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
} }
// Compute the bias "b" of the constraint for the 3 translation constraints // Compute the bias "b" of the constraint for the 3 translation constraints
decimal biasFactor = (BETA / constraintSolverData.timeStep); float biasFactor = (BETA / constraintSolverData.timeStep);
mBiasTranslation.setToZero(); mBiasTranslation.setToZero();
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBiasTranslation = biasFactor * (x2 + mR2World - x1 - mR1World); mBiasTranslation = biasFactor * (x2 + mR2World - x1 - mR1World);
@ -113,7 +94,7 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
currentOrientationDifference.normalize(); currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
mBiasRotation = biasFactor * decimal(2.0) * qError.getVectorV(); mBiasRotation = biasFactor * float(2.0) * qError.getVectorV();
} }
// If warm-starting is not enabled // If warm-starting is not enabled
@ -135,8 +116,8 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass of the bodies // Get the inverse mass of the bodies
const decimal inverseMassBody1 = mBody1->mMassInverse; const float inverseMassBody1 = mBody1->mMassInverse;
const decimal inverseMassBody2 = mBody2->mMassInverse; const float inverseMassBody2 = mBody2->mMassInverse;
// Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1 // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1
Vector3 linearImpulseBody1 = -mImpulseTranslation; Vector3 linearImpulseBody1 = -mImpulseTranslation;
@ -170,8 +151,8 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass of the bodies // Get the inverse mass of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse; float inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse; float inverseMassBody2 = mBody2->mMassInverse;
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
@ -231,8 +212,8 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse; float inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse; float inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inverse inertia tensors // Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld(); mI1 = mBody1->getInertiaTensorInverseWorld();
@ -249,7 +230,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) + 0, 0, inverseMassBodies) +
@ -276,7 +257,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
x1 += v1; x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize(); q1.normalize();
// Compute the impulse of body 2 // Compute the impulse of body 2
@ -288,7 +269,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize(); q2.normalize();
// --------------- Rotation Constraints --------------- // // --------------- Rotation Constraints --------------- //
@ -304,7 +285,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
Quaternion currentOrientationDifference = q2 * q1.getInverse(); Quaternion currentOrientationDifference = q2 * q1.getInverse();
currentOrientationDifference.normalize(); currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
const Vector3 errorRotation = decimal(2.0) * qError.getVectorV(); const Vector3 errorRotation = float(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation constraints // Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation); Vector3 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation);
@ -316,14 +297,14 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
w1 = mI1 * angularImpulseBody1; w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize(); q1.normalize();
// Compute the pseudo velocity of body 2 // Compute the pseudo velocity of body 2
w2 = mI2 * lambdaRotation; w2 = mI2 * lambdaRotation;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize(); q2.normalize();
} }

37
ephysics/constraint/FixedJoint.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_FIXED_JOINT_H
#define REACTPHYSICS3D_FIXED_JOINT_H
// Libraries // Libraries
#include <ephysics/constraint/Joint.h> #include <ephysics/constraint/Joint.h>
@ -71,7 +50,7 @@ class FixedJoint : public Joint {
// -------------------- Constants -------------------- // // -------------------- Constants -------------------- //
// Beta value for the bias factor of position correction // Beta value for the bias factor of position correction
static const decimal BETA; static const float BETA;
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
@ -154,5 +133,3 @@ inline size_t FixedJoint::getSizeInBytes() const {
} }
} }
#endif

169
ephysics/constraint/HingeJoint.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/constraint/HingeJoint.h> #include <ephysics/constraint/HingeJoint.h>
@ -31,7 +12,7 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Static variables definition // Static variables definition
const decimal HingeJoint::BETA = decimal(0.2); const float HingeJoint::BETA = float(0.2);
// Constructor // Constructor
HingeJoint::HingeJoint(const HingeJointInfo& jointInfo) HingeJoint::HingeJoint(const HingeJointInfo& jointInfo)
@ -91,11 +72,11 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
mR2World = orientationBody2 * mLocalAnchorPointBody2; mR2World = orientationBody2 * mLocalAnchorPointBody2;
// Compute the current angle around the hinge axis // Compute the current angle around the hinge axis
decimal hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2); float hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2);
// Check if the limit constraints are violated or not // Check if the limit constraints are violated or not
decimal lowerLimitError = hingeAngle - mLowerLimit; float lowerLimitError = hingeAngle - mLowerLimit;
decimal upperLimitError = mUpperLimit - hingeAngle; float upperLimitError = mUpperLimit - hingeAngle;
bool oldIsLowerLimitViolated = mIsLowerLimitViolated; bool oldIsLowerLimitViolated = mIsLowerLimitViolated;
mIsLowerLimitViolated = lowerLimitError <= 0; mIsLowerLimitViolated = lowerLimitError <= 0;
if (mIsLowerLimitViolated != oldIsLowerLimitViolated) { if (mIsLowerLimitViolated != oldIsLowerLimitViolated) {
@ -122,7 +103,7 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix) // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix)
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) + 0, 0, inverseMassBodies) +
@ -135,7 +116,7 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Compute the bias "b" of the translation constraints // Compute the bias "b" of the translation constraints
mBTranslation.setToZero(); mBTranslation.setToZero();
decimal biasFactor = (BETA / constraintSolverData.timeStep); float biasFactor = (BETA / constraintSolverData.timeStep);
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBTranslation = biasFactor * (x2 + mR2World - x1 - mR1World); mBTranslation = biasFactor * (x2 + mR2World - x1 - mR1World);
} }
@ -145,13 +126,13 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
Vector3 I1C2CrossA1 = mI1 * mC2CrossA1; Vector3 I1C2CrossA1 = mI1 * mC2CrossA1;
Vector3 I2B2CrossA1 = mI2 * mB2CrossA1; Vector3 I2B2CrossA1 = mI2 * mB2CrossA1;
Vector3 I2C2CrossA1 = mI2 * mC2CrossA1; Vector3 I2C2CrossA1 = mI2 * mC2CrossA1;
const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) + const float el11 = mB2CrossA1.dot(I1B2CrossA1) +
mB2CrossA1.dot(I2B2CrossA1); mB2CrossA1.dot(I2B2CrossA1);
const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) + const float el12 = mB2CrossA1.dot(I1C2CrossA1) +
mB2CrossA1.dot(I2C2CrossA1); mB2CrossA1.dot(I2C2CrossA1);
const decimal el21 = mC2CrossA1.dot(I1B2CrossA1) + const float el21 = mC2CrossA1.dot(I1B2CrossA1) +
mC2CrossA1.dot(I2B2CrossA1); mC2CrossA1.dot(I2B2CrossA1);
const decimal el22 = mC2CrossA1.dot(I1C2CrossA1) + const float el22 = mC2CrossA1.dot(I1C2CrossA1) +
mC2CrossA1.dot(I2C2CrossA1); mC2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22); const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mInverseMassMatrixRotation.setToZero(); mInverseMassMatrixRotation.setToZero();
@ -182,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) // Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix)
mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1); mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1);
mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0); float(1.0) / mInverseMassMatrixLimitMotor : float(0.0);
if (mIsLimitEnabled) { if (mIsLimitEnabled) {
@ -211,8 +192,8 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = mBody1->mMassInverse; const float inverseMassBody1 = mBody1->mMassInverse;
const decimal inverseMassBody2 = mBody2->mMassInverse; const float inverseMassBody2 = mBody2->mMassInverse;
// Compute the impulse P=J^T * lambda for the 2 rotation constraints // Compute the impulse P=J^T * lambda for the 2 rotation constraints
Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y; Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y;
@ -267,8 +248,8 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse; float inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse; float inverseMassBody2 = mBody2->mMassInverse;
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
@ -327,12 +308,12 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
if (mIsLowerLimitViolated) { if (mIsLowerLimitViolated) {
// Compute J*v for the lower limit constraint // Compute J*v for the lower limit constraint
const decimal JvLowerLimit = (w2 - w1).dot(mA1); const float JvLowerLimit = (w2 - w1).dot(mA1);
// Compute the Lagrange multiplier lambda for the lower limit constraint // Compute the Lagrange multiplier lambda for the lower limit constraint
decimal deltaLambdaLower = mInverseMassMatrixLimitMotor * (-JvLowerLimit -mBLowerLimit); float deltaLambdaLower = mInverseMassMatrixLimitMotor * (-JvLowerLimit -mBLowerLimit);
decimal lambdaTemp = mImpulseLowerLimit; float lambdaTemp = mImpulseLowerLimit;
mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0)); mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, float(0.0));
deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; deltaLambdaLower = mImpulseLowerLimit - lambdaTemp;
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
@ -352,12 +333,12 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
if (mIsUpperLimitViolated) { if (mIsUpperLimitViolated) {
// Compute J*v for the upper limit constraint // Compute J*v for the upper limit constraint
const decimal JvUpperLimit = -(w2 - w1).dot(mA1); const float JvUpperLimit = -(w2 - w1).dot(mA1);
// Compute the Lagrange multiplier lambda for the upper limit constraint // Compute the Lagrange multiplier lambda for the upper limit constraint
decimal deltaLambdaUpper = mInverseMassMatrixLimitMotor * (-JvUpperLimit -mBUpperLimit); float deltaLambdaUpper = mInverseMassMatrixLimitMotor * (-JvUpperLimit -mBUpperLimit);
decimal lambdaTemp = mImpulseUpperLimit; float lambdaTemp = mImpulseUpperLimit;
mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0)); mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, float(0.0));
deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp; deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp;
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
@ -380,12 +361,12 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
if (mIsMotorEnabled) { if (mIsMotorEnabled) {
// Compute J*v for the motor // Compute J*v for the motor
const decimal JvMotor = mA1.dot(w1 - w2); const float JvMotor = mA1.dot(w1 - w2);
// Compute the Lagrange multiplier lambda for the motor // Compute the Lagrange multiplier lambda for the motor
const decimal maxMotorImpulse = mMaxMotorTorque * constraintSolverData.timeStep; const float maxMotorImpulse = mMaxMotorTorque * constraintSolverData.timeStep;
decimal deltaLambdaMotor = mInverseMassMatrixLimitMotor * (-JvMotor - mMotorSpeed); float deltaLambdaMotor = mInverseMassMatrixLimitMotor * (-JvMotor - mMotorSpeed);
decimal lambdaTemp = mImpulseMotor; float lambdaTemp = mImpulseMotor;
mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
deltaLambdaMotor = mImpulseMotor - lambdaTemp; deltaLambdaMotor = mImpulseMotor - lambdaTemp;
@ -417,8 +398,8 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse; float inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse; float inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inverse inertia tensors // Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld(); mI1 = mBody1->getInertiaTensorInverseWorld();
@ -429,11 +410,11 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
mR2World = q2 * mLocalAnchorPointBody2; mR2World = q2 * mLocalAnchorPointBody2;
// Compute the current angle around the hinge axis // Compute the current angle around the hinge axis
decimal hingeAngle = computeCurrentHingeAngle(q1, q2); float hingeAngle = computeCurrentHingeAngle(q1, q2);
// Check if the limit constraints are violated or not // Check if the limit constraints are violated or not
decimal lowerLimitError = hingeAngle - mLowerLimit; float lowerLimitError = hingeAngle - mLowerLimit;
decimal upperLimitError = mUpperLimit - hingeAngle; float upperLimitError = mUpperLimit - hingeAngle;
mIsLowerLimitViolated = lowerLimitError <= 0; mIsLowerLimitViolated = lowerLimitError <= 0;
mIsUpperLimitViolated = upperLimitError <= 0; mIsUpperLimitViolated = upperLimitError <= 0;
@ -454,7 +435,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) + 0, 0, inverseMassBodies) +
@ -481,7 +462,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
x1 += v1; x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize(); q1.normalize();
// Compute the impulse of body 2 // Compute the impulse of body 2
@ -493,7 +474,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize(); q2.normalize();
// --------------- Rotation Constraints --------------- // // --------------- Rotation Constraints --------------- //
@ -503,13 +484,13 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
Vector3 I1C2CrossA1 = mI1 * mC2CrossA1; Vector3 I1C2CrossA1 = mI1 * mC2CrossA1;
Vector3 I2B2CrossA1 = mI2 * mB2CrossA1; Vector3 I2B2CrossA1 = mI2 * mB2CrossA1;
Vector3 I2C2CrossA1 = mI2 * mC2CrossA1; Vector3 I2C2CrossA1 = mI2 * mC2CrossA1;
const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) + const float el11 = mB2CrossA1.dot(I1B2CrossA1) +
mB2CrossA1.dot(I2B2CrossA1); mB2CrossA1.dot(I2B2CrossA1);
const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) + const float el12 = mB2CrossA1.dot(I1C2CrossA1) +
mB2CrossA1.dot(I2C2CrossA1); mB2CrossA1.dot(I2C2CrossA1);
const decimal el21 = mC2CrossA1.dot(I1B2CrossA1) + const float el21 = mC2CrossA1.dot(I1B2CrossA1) +
mC2CrossA1.dot(I2B2CrossA1); mC2CrossA1.dot(I2B2CrossA1);
const decimal el22 = mC2CrossA1.dot(I1C2CrossA1) + const float el22 = mC2CrossA1.dot(I1C2CrossA1) +
mC2CrossA1.dot(I2C2CrossA1); mC2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22); const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mInverseMassMatrixRotation.setToZero(); mInverseMassMatrixRotation.setToZero();
@ -530,7 +511,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
w1 = mI1 * angularImpulseBody1; w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize(); q1.normalize();
// Compute the impulse of body 2 // Compute the impulse of body 2
@ -540,7 +521,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
w2 = mI2 * angularImpulseBody2; w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize(); q2.normalize();
// --------------- Limits Constraints --------------- // // --------------- Limits Constraints --------------- //
@ -552,14 +533,14 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1); mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1);
mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0); float(1.0) / mInverseMassMatrixLimitMotor : float(0.0);
} }
// If the lower limit is violated // If the lower limit is violated
if (mIsLowerLimitViolated) { if (mIsLowerLimitViolated) {
// Compute the Lagrange multiplier lambda for the lower limit constraint // Compute the Lagrange multiplier lambda for the lower limit constraint
decimal lambdaLowerLimit = mInverseMassMatrixLimitMotor * (-lowerLimitError ); float lambdaLowerLimit = mInverseMassMatrixLimitMotor * (-lowerLimitError );
// Compute the impulse P=J^T * lambda of body 1 // Compute the impulse P=J^T * lambda of body 1
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1; const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1;
@ -568,7 +549,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
const Vector3 w1 = mI1 * angularImpulseBody1; const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize(); q1.normalize();
// Compute the impulse P=J^T * lambda of body 2 // Compute the impulse P=J^T * lambda of body 2
@ -578,7 +559,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
const Vector3 w2 = mI2 * angularImpulseBody2; const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize(); q2.normalize();
} }
@ -586,7 +567,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
if (mIsUpperLimitViolated) { if (mIsUpperLimitViolated) {
// Compute the Lagrange multiplier lambda for the upper limit constraint // Compute the Lagrange multiplier lambda for the upper limit constraint
decimal lambdaUpperLimit = mInverseMassMatrixLimitMotor * (-upperLimitError); float lambdaUpperLimit = mInverseMassMatrixLimitMotor * (-upperLimitError);
// Compute the impulse P=J^T * lambda of body 1 // Compute the impulse P=J^T * lambda of body 1
const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1; const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1;
@ -595,7 +576,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
const Vector3 w1 = mI1 * angularImpulseBody1; const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize(); q1.normalize();
// Compute the impulse P=J^T * lambda of body 2 // Compute the impulse P=J^T * lambda of body 2
@ -605,7 +586,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
const Vector3 w2 = mI2 * angularImpulseBody2; const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize(); q2.normalize();
} }
} }
@ -647,7 +628,7 @@ void HingeJoint::enableMotor(bool isMotorEnabled) {
/** /**
* @param lowerLimit The minimum limit angle of the joint (in radian) * @param lowerLimit The minimum limit angle of the joint (in radian)
*/ */
void HingeJoint::setMinAngleLimit(decimal lowerLimit) { void HingeJoint::setMinAngleLimit(float lowerLimit) {
assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI); assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI);
@ -664,7 +645,7 @@ void HingeJoint::setMinAngleLimit(decimal lowerLimit) {
/** /**
* @param upperLimit The maximum limit angle of the joint (in radian) * @param upperLimit The maximum limit angle of the joint (in radian)
*/ */
void HingeJoint::setMaxAngleLimit(decimal upperLimit) { void HingeJoint::setMaxAngleLimit(float upperLimit) {
assert(upperLimit >= 0 && upperLimit <= 2.0 * PI); assert(upperLimit >= 0 && upperLimit <= 2.0 * PI);
@ -690,7 +671,7 @@ void HingeJoint::resetLimits() {
} }
// Set the motor speed // Set the motor speed
void HingeJoint::setMotorSpeed(decimal motorSpeed) { void HingeJoint::setMotorSpeed(float motorSpeed) {
if (motorSpeed != mMotorSpeed) { if (motorSpeed != mMotorSpeed) {
@ -706,7 +687,7 @@ void HingeJoint::setMotorSpeed(decimal motorSpeed) {
/** /**
* @param maxMotorTorque The maximum torque (in Newtons) of the joint motor * @param maxMotorTorque The maximum torque (in Newtons) of the joint motor
*/ */
void HingeJoint::setMaxMotorTorque(decimal maxMotorTorque) { void HingeJoint::setMaxMotorTorque(float maxMotorTorque) {
if (maxMotorTorque != mMaxMotorTorque) { if (maxMotorTorque != mMaxMotorTorque) {
@ -720,12 +701,12 @@ void HingeJoint::setMaxMotorTorque(decimal maxMotorTorque) {
} }
// Given an angle in radian, this method returns the corresponding angle in the range [-pi; pi] // Given an angle in radian, this method returns the corresponding angle in the range [-pi; pi]
decimal HingeJoint::computeNormalizedAngle(decimal angle) const { float HingeJoint::computeNormalizedAngle(float angle) const {
// Convert it into the range [-2*pi; 2*pi] // Convert it int32_to the range [-2*pi; 2*pi]
angle = fmod(angle, PI_TIMES_2); angle = fmod(angle, PI_TIMES_2);
// Convert it into the range [-pi; pi] // Convert it int32_to the range [-pi; pi]
if (angle < -PI) { if (angle < -PI) {
return angle + PI_TIMES_2; return angle + PI_TIMES_2;
} }
@ -740,19 +721,19 @@ decimal HingeJoint::computeNormalizedAngle(decimal angle) const {
// Given an "inputAngle" in the range [-pi, pi], this method returns an // Given an "inputAngle" in the range [-pi, pi], this method returns an
// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the // angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the
// two angle limits in arguments. // two angle limits in arguments.
decimal HingeJoint::computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle, float HingeJoint::computeCorrespondingAngleNearLimits(float inputAngle, float lowerLimitAngle,
decimal upperLimitAngle) const { float upperLimitAngle) const {
if (upperLimitAngle <= lowerLimitAngle) { if (upperLimitAngle <= lowerLimitAngle) {
return inputAngle; return inputAngle;
} }
else if (inputAngle > upperLimitAngle) { else if (inputAngle > upperLimitAngle) {
decimal diffToUpperLimit = fabs(computeNormalizedAngle(inputAngle - upperLimitAngle)); float diffToUpperLimit = fabs(computeNormalizedAngle(inputAngle - upperLimitAngle));
decimal diffToLowerLimit = fabs(computeNormalizedAngle(inputAngle - lowerLimitAngle)); float diffToLowerLimit = fabs(computeNormalizedAngle(inputAngle - lowerLimitAngle));
return (diffToUpperLimit > diffToLowerLimit) ? (inputAngle - PI_TIMES_2) : inputAngle; return (diffToUpperLimit > diffToLowerLimit) ? (inputAngle - PI_TIMES_2) : inputAngle;
} }
else if (inputAngle < lowerLimitAngle) { else if (inputAngle < lowerLimitAngle) {
decimal diffToUpperLimit = fabs(computeNormalizedAngle(upperLimitAngle - inputAngle)); float diffToUpperLimit = fabs(computeNormalizedAngle(upperLimitAngle - inputAngle));
decimal diffToLowerLimit = fabs(computeNormalizedAngle(lowerLimitAngle - inputAngle)); float diffToLowerLimit = fabs(computeNormalizedAngle(lowerLimitAngle - inputAngle));
return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + PI_TIMES_2); return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + PI_TIMES_2);
} }
else { else {
@ -761,10 +742,10 @@ decimal HingeJoint::computeCorrespondingAngleNearLimits(decimal inputAngle, deci
} }
// Compute the current angle around the hinge axis // Compute the current angle around the hinge axis
decimal HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1, float HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1,
const Quaternion& orientationBody2) { const Quaternion& orientationBody2) {
decimal hingeAngle; float hingeAngle;
// Compute the current orientation difference between the two bodies // Compute the current orientation difference between the two bodies
Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse(); Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse();
@ -781,21 +762,21 @@ decimal HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1,
// axis is not pointing in the same direction as the hinge axis, we use the rotation -q which // axis is not pointing in the same direction as the hinge axis, we use the rotation -q which
// has the same |sin(theta/2)| value but the value cos(theta/2) is sign inverted. Some details // has the same |sin(theta/2)| value but the value cos(theta/2) is sign inverted. Some details
// about this trick is explained in the source code of OpenTissue (http://www.opentissue.org). // about this trick is explained in the source code of OpenTissue (http://www.opentissue.org).
decimal cosHalfAngle = relativeRotation.w; float cosHalfAngle = relativeRotation.w;
decimal sinHalfAngleAbs = relativeRotation.getVectorV().length(); float sinHalfAngleAbs = relativeRotation.getVectorV().length();
// Compute the dot product of the relative rotation axis and the hinge axis // Compute the dot product of the relative rotation axis and the hinge axis
decimal dotProduct = relativeRotation.getVectorV().dot(mA1); float dotProduct = relativeRotation.getVectorV().dot(mA1);
// If the relative rotation axis and the hinge axis are pointing the same direction // If the relative rotation axis and the hinge axis are pointing the same direction
if (dotProduct >= decimal(0.0)) { if (dotProduct >= float(0.0)) {
hingeAngle = decimal(2.0) * std::atan2(sinHalfAngleAbs, cosHalfAngle); hingeAngle = float(2.0) * std::atan2(sinHalfAngleAbs, cosHalfAngle);
} }
else { else {
hingeAngle = decimal(2.0) * std::atan2(sinHalfAngleAbs, -cosHalfAngle); hingeAngle = float(2.0) * std::atan2(sinHalfAngleAbs, -cosHalfAngle);
} }
// Convert the angle from range [-2*pi; 2*pi] into the range [-pi; pi] // Convert the angle from range [-2*pi; 2*pi] int32_to the range [-pi; pi]
hingeAngle = computeNormalizedAngle(hingeAngle); hingeAngle = computeNormalizedAngle(hingeAngle);
// Compute and return the corresponding angle near one the two limits // Compute and return the corresponding angle near one the two limits

117
ephysics/constraint/HingeJoint.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_HINGE_JOINT_H
#define REACTPHYSICS3D_HINGE_JOINT_H
// Libraries // Libraries
#include <ephysics/constraint/Joint.h> #include <ephysics/constraint/Joint.h>
@ -57,18 +36,18 @@ struct HingeJointInfo : public JointInfo {
/// Minimum allowed rotation angle (in radian) if limits are enabled. /// Minimum allowed rotation angle (in radian) if limits are enabled.
/// The angle must be in the range [-2*pi, 0] /// The angle must be in the range [-2*pi, 0]
decimal minAngleLimit; float minAngleLimit;
/// Maximum allowed rotation angle (in radian) if limits are enabled. /// Maximum allowed rotation angle (in radian) if limits are enabled.
/// The angle must be in the range [0, 2*pi] /// The angle must be in the range [0, 2*pi]
decimal maxAngleLimit; float maxAngleLimit;
/// Motor speed (in radian/second) /// Motor speed (in radian/second)
decimal motorSpeed; float motorSpeed;
/// Maximum motor torque (in Newtons * meters) that can be applied to reach /// Maximum motor torque (in Newtons * meters) that can be applied to reach
/// to desired motor speed /// to desired motor speed
decimal maxMotorTorque; float maxMotorTorque;
/// Constructor without limits and without motor /// Constructor without limits and without motor
/** /**
@ -93,14 +72,14 @@ struct HingeJointInfo : public JointInfo {
* @param rigidBody1 The first body of the joint * @param rigidBody1 The first body of the joint
* @param rigidBody2 The second body of the joint * @param rigidBody2 The second body of the joint
* @param initAnchorPointWorldSpace The initial anchor point in world-space coordinates * @param initAnchorPointWorldSpace The initial anchor point in world-space coordinates
* @param initRotationAxisWorld The intial rotation axis in world-space coordinates * @param initRotationAxisWorld The int32_tial rotation axis in world-space coordinates
* @param initMinAngleLimit The initial minimum limit angle (in radian) * @param initMinAngleLimit The initial minimum limit angle (in radian)
* @param initMaxAngleLimit The initial maximum limit angle (in radian) * @param initMaxAngleLimit The initial maximum limit angle (in radian)
*/ */
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace, const Vector3& initAnchorPointWorldSpace,
const Vector3& initRotationAxisWorld, const Vector3& initRotationAxisWorld,
decimal initMinAngleLimit, decimal initMaxAngleLimit) float initMinAngleLimit, float initMaxAngleLimit)
: JointInfo(rigidBody1, rigidBody2, HINGEJOINT), : JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace), anchorPointWorldSpace(initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true),
@ -122,8 +101,8 @@ struct HingeJointInfo : public JointInfo {
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace, const Vector3& initAnchorPointWorldSpace,
const Vector3& initRotationAxisWorld, const Vector3& initRotationAxisWorld,
decimal initMinAngleLimit, decimal initMaxAngleLimit, float initMinAngleLimit, float initMaxAngleLimit,
decimal initMotorSpeed, decimal initMaxMotorTorque) float initMotorSpeed, float initMaxMotorTorque)
: JointInfo(rigidBody1, rigidBody2, HINGEJOINT), : JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace), anchorPointWorldSpace(initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true),
@ -145,7 +124,7 @@ class HingeJoint : public Joint {
// -------------------- Constants -------------------- // // -------------------- Constants -------------------- //
// Beta value for the bias factor of position correction // Beta value for the bias factor of position correction
static const decimal BETA; static const float BETA;
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
@ -189,13 +168,13 @@ class HingeJoint : public Joint {
Vector2 mImpulseRotation; Vector2 mImpulseRotation;
/// Accumulated impulse for the lower limit constraint /// Accumulated impulse for the lower limit constraint
decimal mImpulseLowerLimit; float mImpulseLowerLimit;
/// Accumulated impulse for the upper limit constraint /// Accumulated impulse for the upper limit constraint
decimal mImpulseUpperLimit; float mImpulseUpperLimit;
/// Accumulated impulse for the motor constraint; /// Accumulated impulse for the motor constraint;
decimal mImpulseMotor; float mImpulseMotor;
/// Inverse mass matrix K=JM^-1J^t for the 3 translation constraints /// Inverse mass matrix K=JM^-1J^t for the 3 translation constraints
Matrix3x3 mInverseMassMatrixTranslation; Matrix3x3 mInverseMassMatrixTranslation;
@ -204,10 +183,10 @@ class HingeJoint : public Joint {
Matrix2x2 mInverseMassMatrixRotation; Matrix2x2 mInverseMassMatrixRotation;
/// Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) /// Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix)
decimal mInverseMassMatrixLimitMotor; float mInverseMassMatrixLimitMotor;
/// Inverse of mass matrix K=JM^-1J^t for the motor /// Inverse of mass matrix K=JM^-1J^t for the motor
decimal mInverseMassMatrixMotor; float mInverseMassMatrixMotor;
/// Bias vector for the error correction for the translation constraints /// Bias vector for the error correction for the translation constraints
Vector3 mBTranslation; Vector3 mBTranslation;
@ -216,10 +195,10 @@ class HingeJoint : public Joint {
Vector2 mBRotation; Vector2 mBRotation;
/// Bias of the lower limit constraint /// Bias of the lower limit constraint
decimal mBLowerLimit; float mBLowerLimit;
/// Bias of the upper limit constraint /// Bias of the upper limit constraint
decimal mBUpperLimit; float mBUpperLimit;
/// Inverse of the initial orientation difference between the bodies /// Inverse of the initial orientation difference between the bodies
Quaternion mInitOrientationDifferenceInv; Quaternion mInitOrientationDifferenceInv;
@ -231,10 +210,10 @@ class HingeJoint : public Joint {
bool mIsMotorEnabled; bool mIsMotorEnabled;
/// Lower limit (minimum allowed rotation angle in radian) /// Lower limit (minimum allowed rotation angle in radian)
decimal mLowerLimit; float mLowerLimit;
/// Upper limit (maximum translation distance) /// Upper limit (maximum translation distance)
decimal mUpperLimit; float mUpperLimit;
/// True if the lower limit is violated /// True if the lower limit is violated
bool mIsLowerLimitViolated; bool mIsLowerLimitViolated;
@ -243,10 +222,10 @@ class HingeJoint : public Joint {
bool mIsUpperLimitViolated; bool mIsUpperLimitViolated;
/// Motor speed (in rad/s) /// Motor speed (in rad/s)
decimal mMotorSpeed; float mMotorSpeed;
/// Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed /// Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed
decimal mMaxMotorTorque; float mMaxMotorTorque;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -261,16 +240,16 @@ class HingeJoint : public Joint {
/// Given an angle in radian, this method returns the corresponding /// Given an angle in radian, this method returns the corresponding
/// angle in the range [-pi; pi] /// angle in the range [-pi; pi]
decimal computeNormalizedAngle(decimal angle) const; float computeNormalizedAngle(float angle) const;
/// Given an "inputAngle" in the range [-pi, pi], this method returns an /// Given an "inputAngle" in the range [-pi, pi], this method returns an
/// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the /// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the
/// two angle limits in arguments. /// two angle limits in arguments.
decimal computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle, float computeCorrespondingAngleNearLimits(float inputAngle, float lowerLimitAngle,
decimal upperLimitAngle) const; float upperLimitAngle) const;
/// Compute the current angle around the hinge axis /// Compute the current angle around the hinge axis
decimal computeCurrentHingeAngle(const Quaternion& orientationBody1, float computeCurrentHingeAngle(const Quaternion& orientationBody1,
const Quaternion& orientationBody2); const Quaternion& orientationBody2);
/// Return the number of bytes used by the joint /// Return the number of bytes used by the joint
@ -311,31 +290,31 @@ class HingeJoint : public Joint {
void enableMotor(bool isMotorEnabled); void enableMotor(bool isMotorEnabled);
/// Return the minimum angle limit /// Return the minimum angle limit
decimal getMinAngleLimit() const; float getMinAngleLimit() const;
/// Set the minimum angle limit /// Set the minimum angle limit
void setMinAngleLimit(decimal lowerLimit); void setMinAngleLimit(float lowerLimit);
/// Return the maximum angle limit /// Return the maximum angle limit
decimal getMaxAngleLimit() const; float getMaxAngleLimit() const;
/// Set the maximum angle limit /// Set the maximum angle limit
void setMaxAngleLimit(decimal upperLimit); void setMaxAngleLimit(float upperLimit);
/// Return the motor speed /// Return the motor speed
decimal getMotorSpeed() const; float getMotorSpeed() const;
/// Set the motor speed /// Set the motor speed
void setMotorSpeed(decimal motorSpeed); void setMotorSpeed(float motorSpeed);
/// Return the maximum motor torque /// Return the maximum motor torque
decimal getMaxMotorTorque() const; float getMaxMotorTorque() const;
/// Set the maximum motor torque /// Set the maximum motor torque
void setMaxMotorTorque(decimal maxMotorTorque); void setMaxMotorTorque(float maxMotorTorque);
/// Return the intensity of the current torque applied for the joint motor /// Return the int32_tensity of the current torque applied for the joint motor
decimal getMotorTorque(decimal timeStep) const; float getMotorTorque(float timeStep) const;
}; };
// Return true if the limits of the joint are enabled // Return true if the limits of the joint are enabled
@ -358,7 +337,7 @@ inline bool HingeJoint::isMotorEnabled() const {
/** /**
* @return The minimum limit angle of the joint (in radian) * @return The minimum limit angle of the joint (in radian)
*/ */
inline decimal HingeJoint::getMinAngleLimit() const { inline float HingeJoint::getMinAngleLimit() const {
return mLowerLimit; return mLowerLimit;
} }
@ -366,7 +345,7 @@ inline decimal HingeJoint::getMinAngleLimit() const {
/** /**
* @return The maximum limit angle of the joint (in radian) * @return The maximum limit angle of the joint (in radian)
*/ */
inline decimal HingeJoint::getMaxAngleLimit() const { inline float HingeJoint::getMaxAngleLimit() const {
return mUpperLimit; return mUpperLimit;
} }
@ -374,7 +353,7 @@ inline decimal HingeJoint::getMaxAngleLimit() const {
/** /**
* @return The current speed of the joint motor (in radian per second) * @return The current speed of the joint motor (in radian per second)
*/ */
inline decimal HingeJoint::getMotorSpeed() const { inline float HingeJoint::getMotorSpeed() const {
return mMotorSpeed; return mMotorSpeed;
} }
@ -382,16 +361,16 @@ inline decimal HingeJoint::getMotorSpeed() const {
/** /**
* @return The maximum torque of the joint motor (in Newtons) * @return The maximum torque of the joint motor (in Newtons)
*/ */
inline decimal HingeJoint::getMaxMotorTorque() const { inline float HingeJoint::getMaxMotorTorque() const {
return mMaxMotorTorque; return mMaxMotorTorque;
} }
// Return the intensity of the current torque applied for the joint motor // Return the int32_tensity of the current torque applied for the joint motor
/** /**
* @param timeStep The current time step (in seconds) * @param timeStep The current time step (in seconds)
* @return The intensity of the current torque (in Newtons) of the joint motor * @return The int32_tensity of the current torque (in Newtons) of the joint motor
*/ */
inline decimal HingeJoint::getMotorTorque(decimal timeStep) const { inline float HingeJoint::getMotorTorque(float timeStep) const {
return mImpulseMotor / timeStep; return mImpulseMotor / timeStep;
} }
@ -402,5 +381,3 @@ inline size_t HingeJoint::getSizeInBytes() const {
} }
#endif

30
ephysics/constraint/Joint.cpp
View File

@ -1,27 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/constraint/Joint.h> #include <ephysics/constraint/Joint.h>

45
ephysics/constraint/Joint.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CONSTRAINT_H
#define REACTPHYSICS3D_CONSTRAINT_H
// Libraries // Libraries
#include <ephysics/configuration.h> #include <ephysics/configuration.h>
@ -130,10 +109,10 @@ class Joint {
const JointType mType; const JointType mType;
/// Body 1 index in the velocity array to solve the constraint /// Body 1 index in the velocity array to solve the constraint
uint mIndexBody1; uint32_t mIndexBody1;
/// Body 2 index in the velocity array to solve the constraint /// Body 2 index in the velocity array to solve the constraint
uint mIndexBody2; uint32_t mIndexBody2;
/// Position correction technique used for the constraint (used for joints) /// Position correction technique used for the constraint (used for joints)
JointsPositionCorrectionTechnique mPositionCorrectionTechnique; JointsPositionCorrectionTechnique mPositionCorrectionTechnique;
@ -141,7 +120,7 @@ class Joint {
/// True if the two bodies of the constraint are allowed to collide with each other /// True if the two bodies of the constraint are allowed to collide with each other
bool mIsCollisionEnabled; bool mIsCollisionEnabled;
/// True if the joint has already been added into an island /// True if the joint has already been added int32_to an island
bool mIsAlreadyInIsland; bool mIsAlreadyInIsland;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -152,7 +131,7 @@ class Joint {
/// Private assignment operator /// Private assignment operator
Joint& operator=(const Joint& constraint); Joint& operator=(const Joint& constraint);
/// Return true if the joint has already been added into an island /// Return true if the joint has already been added int32_to an island
bool isAlreadyInIsland() const; bool isAlreadyInIsland() const;
/// Return the number of bytes used by the joint /// Return the number of bytes used by the joint
@ -243,11 +222,9 @@ inline bool Joint::isCollisionEnabled() const {
return mIsCollisionEnabled; return mIsCollisionEnabled;
} }
// Return true if the joint has already been added into an island // Return true if the joint has already been added int32_to an island
inline bool Joint::isAlreadyInIsland() const { inline bool Joint::isAlreadyInIsland() const {
return mIsAlreadyInIsland; return mIsAlreadyInIsland;
} }
} }
#endif

147
ephysics/constraint/SliderJoint.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/constraint/SliderJoint.h> #include <ephysics/constraint/SliderJoint.h>
@ -29,7 +10,7 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Static variables definition // Static variables definition
const decimal SliderJoint::BETA = decimal(0.2); const float SliderJoint::BETA = float(0.2);
// Constructor // Constructor
SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
@ -99,9 +80,9 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
mN2 = mSliderAxisWorld.cross(mN1); mN2 = mSliderAxisWorld.cross(mN1);
// Check if the limit constraints are violated or not // Check if the limit constraints are violated or not
decimal uDotSliderAxis = u.dot(mSliderAxisWorld); float uDotSliderAxis = u.dot(mSliderAxisWorld);
decimal lowerLimitError = uDotSliderAxis - mLowerLimit; float lowerLimitError = uDotSliderAxis - mLowerLimit;
decimal upperLimitError = mUpperLimit - uDotSliderAxis; float upperLimitError = mUpperLimit - uDotSliderAxis;
bool oldIsLowerLimitViolated = mIsLowerLimitViolated; bool oldIsLowerLimitViolated = mIsLowerLimitViolated;
mIsLowerLimitViolated = lowerLimitError <= 0; mIsLowerLimitViolated = lowerLimitError <= 0;
if (mIsLowerLimitViolated != oldIsLowerLimitViolated) { if (mIsLowerLimitViolated != oldIsLowerLimitViolated) {
@ -124,18 +105,18 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
// constraints (2x2 matrix) // constraints (2x2 matrix)
decimal sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse; float sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse;
Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
Vector3 I2R2CrossN1 = mI2 * mR2CrossN1; Vector3 I2R2CrossN1 = mI2 * mR2CrossN1;
Vector3 I2R2CrossN2 = mI2 * mR2CrossN2; Vector3 I2R2CrossN2 = mI2 * mR2CrossN2;
const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + const float el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1); mR2CrossN1.dot(I2R2CrossN1);
const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + const float el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
mR2CrossN1.dot(I2R2CrossN2); mR2CrossN1.dot(I2R2CrossN2);
const decimal el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + const float el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
mR2CrossN2.dot(I2R2CrossN1); mR2CrossN2.dot(I2R2CrossN1);
const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + const float el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
mR2CrossN2.dot(I2R2CrossN2); mR2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22); Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
mInverseMassMatrixTranslationConstraint.setToZero(); mInverseMassMatrixTranslationConstraint.setToZero();
@ -145,7 +126,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Compute the bias "b" of the translation constraint // Compute the bias "b" of the translation constraint
mBTranslation.setToZero(); mBTranslation.setToZero();
decimal biasFactor = (BETA / constraintSolverData.timeStep); float biasFactor = (BETA / constraintSolverData.timeStep);
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBTranslation.x = u.dot(mN1); mBTranslation.x = u.dot(mN1);
mBTranslation.y = u.dot(mN2); mBTranslation.y = u.dot(mN2);
@ -165,7 +146,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
currentOrientationDifference.normalize(); currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
mBRotation = biasFactor * decimal(2.0) * qError.getVectorV(); mBRotation = biasFactor * float(2.0) * qError.getVectorV();
} }
// If the limits are enabled // If the limits are enabled
@ -176,7 +157,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) + mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0); float(1.0) / mInverseMassMatrixLimit : float(0.0);
// Compute the bias "b" of the lower limit constraint // Compute the bias "b" of the lower limit constraint
mBLowerLimit = 0.0; mBLowerLimit = 0.0;
@ -197,7 +178,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix) // Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix)
mInverseMassMatrixMotor = mBody1->mMassInverse + mBody2->mMassInverse; mInverseMassMatrixMotor = mBody1->mMassInverse + mBody2->mMassInverse;
mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ? mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ?
decimal(1.0) / mInverseMassMatrixMotor : decimal(0.0); float(1.0) / mInverseMassMatrixMotor : float(0.0);
} }
// If warm-starting is not enabled // If warm-starting is not enabled
@ -222,11 +203,11 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = mBody1->mMassInverse; const float inverseMassBody1 = mBody1->mMassInverse;
const decimal inverseMassBody2 = mBody2->mMassInverse; const float inverseMassBody2 = mBody2->mMassInverse;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1 // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
decimal impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit; float impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit;
Vector3 linearImpulseLimits = impulseLimits * mSliderAxisWorld; Vector3 linearImpulseLimits = impulseLimits * mSliderAxisWorld;
// Compute the impulse P=J^T * lambda for the motor constraint of body 1 // Compute the impulse P=J^T * lambda for the motor constraint of body 1
@ -281,15 +262,15 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse; float inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse; float inverseMassBody2 = mBody2->mMassInverse;
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
// Compute J*v for the 2 translation constraints // Compute J*v for the 2 translation constraints
const decimal el1 = -mN1.dot(v1) - w1.dot(mR1PlusUCrossN1) + const float el1 = -mN1.dot(v1) - w1.dot(mR1PlusUCrossN1) +
mN1.dot(v2) + w2.dot(mR2CrossN1); mN1.dot(v2) + w2.dot(mR2CrossN1);
const decimal el2 = -mN2.dot(v1) - w1.dot(mR1PlusUCrossN2) + const float el2 = -mN2.dot(v1) - w1.dot(mR1PlusUCrossN2) +
mN2.dot(v2) + w2.dot(mR2CrossN2); mN2.dot(v2) + w2.dot(mR2CrossN2);
const Vector2 JvTranslation(el1, el2); const Vector2 JvTranslation(el1, el2);
@ -343,13 +324,13 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
if (mIsLowerLimitViolated) { if (mIsLowerLimitViolated) {
// Compute J*v for the lower limit constraint // Compute J*v for the lower limit constraint
const decimal JvLowerLimit = mSliderAxisWorld.dot(v2) + mR2CrossSliderAxis.dot(w2) - const float JvLowerLimit = mSliderAxisWorld.dot(v2) + mR2CrossSliderAxis.dot(w2) -
mSliderAxisWorld.dot(v1) - mR1PlusUCrossSliderAxis.dot(w1); mSliderAxisWorld.dot(v1) - mR1PlusUCrossSliderAxis.dot(w1);
// Compute the Lagrange multiplier lambda for the lower limit constraint // Compute the Lagrange multiplier lambda for the lower limit constraint
decimal deltaLambdaLower = mInverseMassMatrixLimit * (-JvLowerLimit -mBLowerLimit); float deltaLambdaLower = mInverseMassMatrixLimit * (-JvLowerLimit -mBLowerLimit);
decimal lambdaTemp = mImpulseLowerLimit; float lambdaTemp = mImpulseLowerLimit;
mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0)); mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, float(0.0));
deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; deltaLambdaLower = mImpulseLowerLimit - lambdaTemp;
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
@ -373,13 +354,13 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
if (mIsUpperLimitViolated) { if (mIsUpperLimitViolated) {
// Compute J*v for the upper limit constraint // Compute J*v for the upper limit constraint
const decimal JvUpperLimit = mSliderAxisWorld.dot(v1) + mR1PlusUCrossSliderAxis.dot(w1) const float JvUpperLimit = mSliderAxisWorld.dot(v1) + mR1PlusUCrossSliderAxis.dot(w1)
- mSliderAxisWorld.dot(v2) - mR2CrossSliderAxis.dot(w2); - mSliderAxisWorld.dot(v2) - mR2CrossSliderAxis.dot(w2);
// Compute the Lagrange multiplier lambda for the upper limit constraint // Compute the Lagrange multiplier lambda for the upper limit constraint
decimal deltaLambdaUpper = mInverseMassMatrixLimit * (-JvUpperLimit -mBUpperLimit); float deltaLambdaUpper = mInverseMassMatrixLimit * (-JvUpperLimit -mBUpperLimit);
decimal lambdaTemp = mImpulseUpperLimit; float lambdaTemp = mImpulseUpperLimit;
mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0)); mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, float(0.0));
deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp; deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp;
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
@ -405,12 +386,12 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
if (mIsMotorEnabled) { if (mIsMotorEnabled) {
// Compute J*v for the motor // Compute J*v for the motor
const decimal JvMotor = mSliderAxisWorld.dot(v1) - mSliderAxisWorld.dot(v2); const float JvMotor = mSliderAxisWorld.dot(v1) - mSliderAxisWorld.dot(v2);
// Compute the Lagrange multiplier lambda for the motor // Compute the Lagrange multiplier lambda for the motor
const decimal maxMotorImpulse = mMaxMotorForce * constraintSolverData.timeStep; const float maxMotorImpulse = mMaxMotorForce * constraintSolverData.timeStep;
decimal deltaLambdaMotor = mInverseMassMatrixMotor * (-JvMotor - mMotorSpeed); float deltaLambdaMotor = mInverseMassMatrixMotor * (-JvMotor - mMotorSpeed);
decimal lambdaTemp = mImpulseMotor; float lambdaTemp = mImpulseMotor;
mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
deltaLambdaMotor = mImpulseMotor - lambdaTemp; deltaLambdaMotor = mImpulseMotor - lambdaTemp;
@ -442,8 +423,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse; float inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse; float inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inertia tensor of bodies // Recompute the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld(); mI1 = mBody1->getInertiaTensorInverseWorld();
@ -463,9 +444,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
mN2 = mSliderAxisWorld.cross(mN1); mN2 = mSliderAxisWorld.cross(mN1);
// Check if the limit constraints are violated or not // Check if the limit constraints are violated or not
decimal uDotSliderAxis = u.dot(mSliderAxisWorld); float uDotSliderAxis = u.dot(mSliderAxisWorld);
decimal lowerLimitError = uDotSliderAxis - mLowerLimit; float lowerLimitError = uDotSliderAxis - mLowerLimit;
decimal upperLimitError = mUpperLimit - uDotSliderAxis; float upperLimitError = mUpperLimit - uDotSliderAxis;
mIsLowerLimitViolated = lowerLimitError <= 0; mIsLowerLimitViolated = lowerLimitError <= 0;
mIsUpperLimitViolated = upperLimitError <= 0; mIsUpperLimitViolated = upperLimitError <= 0;
@ -482,18 +463,18 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
// constraints (2x2 matrix) // constraints (2x2 matrix)
decimal sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse; float sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse;
Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
Vector3 I2R2CrossN1 = mI2 * mR2CrossN1; Vector3 I2R2CrossN1 = mI2 * mR2CrossN1;
Vector3 I2R2CrossN2 = mI2 * mR2CrossN2; Vector3 I2R2CrossN2 = mI2 * mR2CrossN2;
const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + const float el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1); mR2CrossN1.dot(I2R2CrossN1);
const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + const float el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
mR2CrossN1.dot(I2R2CrossN2); mR2CrossN1.dot(I2R2CrossN2);
const decimal el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + const float el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
mR2CrossN2.dot(I2R2CrossN1); mR2CrossN2.dot(I2R2CrossN1);
const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + const float el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
mR2CrossN2.dot(I2R2CrossN2); mR2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22); Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
mInverseMassMatrixTranslationConstraint.setToZero(); mInverseMassMatrixTranslationConstraint.setToZero();
@ -518,7 +499,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
x1 += v1; x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize(); q1.normalize();
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
@ -532,7 +513,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize(); q2.normalize();
// --------------- Rotation Constraints --------------- // // --------------- Rotation Constraints --------------- //
@ -548,7 +529,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
Quaternion currentOrientationDifference = q2 * q1.getInverse(); Quaternion currentOrientationDifference = q2 * q1.getInverse();
currentOrientationDifference.normalize(); currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
const Vector3 errorRotation = decimal(2.0) * qError.getVectorV(); const Vector3 errorRotation = float(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation constraints // Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = mInverseMassMatrixRotationConstraint * (-errorRotation); Vector3 lambdaRotation = mInverseMassMatrixRotationConstraint * (-errorRotation);
@ -560,7 +541,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
w1 = mI1 * angularImpulseBody1; w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize(); q1.normalize();
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
@ -570,7 +551,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
w2 = mI2 * angularImpulseBody2; w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize(); q2.normalize();
// --------------- Limits Constraints --------------- // // --------------- Limits Constraints --------------- //
@ -584,14 +565,14 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) + mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0); float(1.0) / mInverseMassMatrixLimit : float(0.0);
} }
// If the lower limit is violated // If the lower limit is violated
if (mIsLowerLimitViolated) { if (mIsLowerLimitViolated) {
// Compute the Lagrange multiplier lambda for the lower limit constraint // Compute the Lagrange multiplier lambda for the lower limit constraint
decimal lambdaLowerLimit = mInverseMassMatrixLimit * (-lowerLimitError); float lambdaLowerLimit = mInverseMassMatrixLimit * (-lowerLimitError);
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld; const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld;
@ -603,7 +584,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
x1 += v1; x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize(); q1.normalize();
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
@ -616,7 +597,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize(); q2.normalize();
} }
@ -624,7 +605,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
if (mIsUpperLimitViolated) { if (mIsUpperLimitViolated) {
// Compute the Lagrange multiplier lambda for the upper limit constraint // Compute the Lagrange multiplier lambda for the upper limit constraint
decimal lambdaUpperLimit = mInverseMassMatrixLimit * (-upperLimitError); float lambdaUpperLimit = mInverseMassMatrixLimit * (-upperLimitError);
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld; const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld;
@ -636,7 +617,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
x1 += v1; x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize(); q1.normalize();
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2
@ -649,7 +630,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize(); q2.normalize();
} }
} }
@ -690,7 +671,7 @@ void SliderJoint::enableMotor(bool isMotorEnabled) {
/** /**
* @return The current translation distance of the joint (in meters) * @return The current translation distance of the joint (in meters)
*/ */
decimal SliderJoint::getTranslation() const { float SliderJoint::getTranslation() const {
// TODO : Check if we need to compare rigid body position or center of mass here // TODO : Check if we need to compare rigid body position or center of mass here
@ -719,7 +700,7 @@ decimal SliderJoint::getTranslation() const {
/** /**
* @param lowerLimit The minimum translation limit of the joint (in meters) * @param lowerLimit The minimum translation limit of the joint (in meters)
*/ */
void SliderJoint::setMinTranslationLimit(decimal lowerLimit) { void SliderJoint::setMinTranslationLimit(float lowerLimit) {
assert(lowerLimit <= mUpperLimit); assert(lowerLimit <= mUpperLimit);
@ -736,7 +717,7 @@ void SliderJoint::setMinTranslationLimit(decimal lowerLimit) {
/** /**
* @param lowerLimit The maximum translation limit of the joint (in meters) * @param lowerLimit The maximum translation limit of the joint (in meters)
*/ */
void SliderJoint::setMaxTranslationLimit(decimal upperLimit) { void SliderJoint::setMaxTranslationLimit(float upperLimit) {
assert(mLowerLimit <= upperLimit); assert(mLowerLimit <= upperLimit);
@ -765,7 +746,7 @@ void SliderJoint::resetLimits() {
/** /**
* @param motorSpeed The speed of the joint motor (in meters per second) * @param motorSpeed The speed of the joint motor (in meters per second)
*/ */
void SliderJoint::setMotorSpeed(decimal motorSpeed) { void SliderJoint::setMotorSpeed(float motorSpeed) {
if (motorSpeed != mMotorSpeed) { if (motorSpeed != mMotorSpeed) {
@ -781,7 +762,7 @@ void SliderJoint::setMotorSpeed(decimal motorSpeed) {
/** /**
* @param maxMotorForce The maximum force of the joint motor (in Newton x meters) * @param maxMotorForce The maximum force of the joint motor (in Newton x meters)
*/ */
void SliderJoint::setMaxMotorForce(decimal maxMotorForce) { void SliderJoint::setMaxMotorForce(float maxMotorForce) {
if (maxMotorForce != mMaxMotorForce) { if (maxMotorForce != mMaxMotorForce) {

107
ephysics/constraint/SliderJoint.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_SLIDER_JOINT_H
#define REACTPHYSICS3D_SLIDER_JOINT_H
// Libraries // Libraries
#include <ephysics/mathematics/mathematics.h> #include <ephysics/mathematics/mathematics.h>
@ -56,16 +35,16 @@ struct SliderJointInfo : public JointInfo {
bool isMotorEnabled; bool isMotorEnabled;
/// Mininum allowed translation if limits are enabled /// Mininum allowed translation if limits are enabled
decimal minTranslationLimit; float minTranslationLimit;
/// Maximum allowed translation if limits are enabled /// Maximum allowed translation if limits are enabled
decimal maxTranslationLimit; float maxTranslationLimit;
/// Motor speed /// Motor speed
decimal motorSpeed; float motorSpeed;
/// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed /// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
decimal maxMotorForce; float maxMotorForce;
/// Constructor without limits and without motor /// Constructor without limits and without motor
/** /**
@ -95,7 +74,7 @@ struct SliderJointInfo : public JointInfo {
SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace, const Vector3& initAnchorPointWorldSpace,
const Vector3& initSliderAxisWorldSpace, const Vector3& initSliderAxisWorldSpace,
decimal initMinTranslationLimit, decimal initMaxTranslationLimit) float initMinTranslationLimit, float initMaxTranslationLimit)
: JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace), anchorPointWorldSpace(initAnchorPointWorldSpace),
sliderAxisWorldSpace(initSliderAxisWorldSpace), sliderAxisWorldSpace(initSliderAxisWorldSpace),
@ -118,8 +97,8 @@ struct SliderJointInfo : public JointInfo {
SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace, const Vector3& initAnchorPointWorldSpace,
const Vector3& initSliderAxisWorldSpace, const Vector3& initSliderAxisWorldSpace,
decimal initMinTranslationLimit, decimal initMaxTranslationLimit, float initMinTranslationLimit, float initMaxTranslationLimit,
decimal initMotorSpeed, decimal initMaxMotorForce) float initMotorSpeed, float initMaxMotorForce)
: JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace), anchorPointWorldSpace(initAnchorPointWorldSpace),
sliderAxisWorldSpace(initSliderAxisWorldSpace), sliderAxisWorldSpace(initSliderAxisWorldSpace),
@ -142,7 +121,7 @@ class SliderJoint : public Joint {
// -------------------- Constants -------------------- // // -------------------- Constants -------------------- //
// Beta value for the position correction bias factor // Beta value for the position correction bias factor
static const decimal BETA; static const float BETA;
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
@ -201,10 +180,10 @@ class SliderJoint : public Joint {
Vector3 mBRotation; Vector3 mBRotation;
/// Bias of the lower limit constraint /// Bias of the lower limit constraint
decimal mBLowerLimit; float mBLowerLimit;
/// Bias of the upper limit constraint /// Bias of the upper limit constraint
decimal mBUpperLimit; float mBUpperLimit;
/// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix) /// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix)
Matrix2x2 mInverseMassMatrixTranslationConstraint; Matrix2x2 mInverseMassMatrixTranslationConstraint;
@ -213,10 +192,10 @@ class SliderJoint : public Joint {
Matrix3x3 mInverseMassMatrixRotationConstraint; Matrix3x3 mInverseMassMatrixRotationConstraint;
/// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix) /// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix)
decimal mInverseMassMatrixLimit; float mInverseMassMatrixLimit;
/// Inverse of mass matrix K=JM^-1J^t for the motor /// Inverse of mass matrix K=JM^-1J^t for the motor
decimal mInverseMassMatrixMotor; float mInverseMassMatrixMotor;
/// Accumulated impulse for the 2 translation constraints /// Accumulated impulse for the 2 translation constraints
Vector2 mImpulseTranslation; Vector2 mImpulseTranslation;
@ -225,13 +204,13 @@ class SliderJoint : public Joint {
Vector3 mImpulseRotation; Vector3 mImpulseRotation;
/// Accumulated impulse for the lower limit constraint /// Accumulated impulse for the lower limit constraint
decimal mImpulseLowerLimit; float mImpulseLowerLimit;
/// Accumulated impulse for the upper limit constraint /// Accumulated impulse for the upper limit constraint
decimal mImpulseUpperLimit; float mImpulseUpperLimit;
/// Accumulated impulse for the motor /// Accumulated impulse for the motor
decimal mImpulseMotor; float mImpulseMotor;
/// True if the slider limits are enabled /// True if the slider limits are enabled
bool mIsLimitEnabled; bool mIsLimitEnabled;
@ -243,10 +222,10 @@ class SliderJoint : public Joint {
Vector3 mSliderAxisWorld; Vector3 mSliderAxisWorld;
/// Lower limit (minimum translation distance) /// Lower limit (minimum translation distance)
decimal mLowerLimit; float mLowerLimit;
/// Upper limit (maximum translation distance) /// Upper limit (maximum translation distance)
decimal mUpperLimit; float mUpperLimit;
/// True if the lower limit is violated /// True if the lower limit is violated
bool mIsLowerLimitViolated; bool mIsLowerLimitViolated;
@ -255,10 +234,10 @@ class SliderJoint : public Joint {
bool mIsUpperLimitViolated; bool mIsUpperLimitViolated;
/// Motor speed (in m/s) /// Motor speed (in m/s)
decimal mMotorSpeed; float mMotorSpeed;
/// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed /// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
decimal mMaxMotorForce; float mMaxMotorForce;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -309,34 +288,34 @@ class SliderJoint : public Joint {
void enableMotor(bool isMotorEnabled); void enableMotor(bool isMotorEnabled);
/// Return the current translation value of the joint /// Return the current translation value of the joint
decimal getTranslation() const; float getTranslation() const;
/// Return the minimum translation limit /// Return the minimum translation limit
decimal getMinTranslationLimit() const; float getMinTranslationLimit() const;
/// Set the minimum translation limit /// Set the minimum translation limit
void setMinTranslationLimit(decimal lowerLimit); void setMinTranslationLimit(float lowerLimit);
/// Return the maximum translation limit /// Return the maximum translation limit
decimal getMaxTranslationLimit() const; float getMaxTranslationLimit() const;
/// Set the maximum translation limit /// Set the maximum translation limit
void setMaxTranslationLimit(decimal upperLimit); void setMaxTranslationLimit(float upperLimit);
/// Return the motor speed /// Return the motor speed
decimal getMotorSpeed() const; float getMotorSpeed() const;
/// Set the motor speed /// Set the motor speed
void setMotorSpeed(decimal motorSpeed); void setMotorSpeed(float motorSpeed);
/// Return the maximum motor force /// Return the maximum motor force
decimal getMaxMotorForce() const; float getMaxMotorForce() const;
/// Set the maximum motor force /// Set the maximum motor force
void setMaxMotorForce(decimal maxMotorForce); void setMaxMotorForce(float maxMotorForce);
/// Return the intensity of the current force applied for the joint motor /// Return the int32_tensity of the current force applied for the joint motor
decimal getMotorForce(decimal timeStep) const; float getMotorForce(float timeStep) const;
}; };
// Return true if the limits or the joint are enabled // Return true if the limits or the joint are enabled
@ -359,7 +338,7 @@ inline bool SliderJoint::isMotorEnabled() const {
/** /**
* @return The minimum translation limit of the joint (in meters) * @return The minimum translation limit of the joint (in meters)
*/ */
inline decimal SliderJoint::getMinTranslationLimit() const { inline float SliderJoint::getMinTranslationLimit() const {
return mLowerLimit; return mLowerLimit;
} }
@ -367,7 +346,7 @@ inline decimal SliderJoint::getMinTranslationLimit() const {
/** /**
* @return The maximum translation limit of the joint (in meters) * @return The maximum translation limit of the joint (in meters)
*/ */
inline decimal SliderJoint::getMaxTranslationLimit() const { inline float SliderJoint::getMaxTranslationLimit() const {
return mUpperLimit; return mUpperLimit;
} }
@ -375,7 +354,7 @@ inline decimal SliderJoint::getMaxTranslationLimit() const {
/** /**
* @return The current motor speed of the joint (in meters per second) * @return The current motor speed of the joint (in meters per second)
*/ */
inline decimal SliderJoint::getMotorSpeed() const { inline float SliderJoint::getMotorSpeed() const {
return mMotorSpeed; return mMotorSpeed;
} }
@ -383,16 +362,16 @@ inline decimal SliderJoint::getMotorSpeed() const {
/** /**
* @return The maximum force of the joint motor (in Newton x meters) * @return The maximum force of the joint motor (in Newton x meters)
*/ */
inline decimal SliderJoint::getMaxMotorForce() const { inline float SliderJoint::getMaxMotorForce() const {
return mMaxMotorForce; return mMaxMotorForce;
} }
// Return the intensity of the current force applied for the joint motor // Return the int32_tensity of the current force applied for the joint motor
/** /**
* @param timeStep Time step (in seconds) * @param timeStep Time step (in seconds)
* @return The current force of the joint motor (in Newton x meters) * @return The current force of the joint motor (in Newton x meters)
*/ */
inline decimal SliderJoint::getMotorForce(decimal timeStep) const { inline float SliderJoint::getMotorForce(float timeStep) const {
return mImpulseMotor / timeStep; return mImpulseMotor / timeStep;
} }
@ -402,5 +381,3 @@ inline size_t SliderJoint::getSizeInBytes() const {
} }
} }
#endif

35
ephysics/decimal.h
View File

@ -1,35 +0,0 @@
/********************************************************************************
* 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_DECIMAL_H
#define REACTPHYSICS3D_DECIMAL_H
/// ReactPhysiscs3D namespace
namespace reactphysics3d {
typedef float decimal;
}
#endif

47
ephysics/engine/CollisionWorld.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/engine/CollisionWorld.h> #include <ephysics/engine/CollisionWorld.h>
@ -161,9 +142,9 @@ void CollisionWorld::testCollision(const ProxyShape* shape,
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
// Create the sets of shapes // Create the sets of shapes
std::set<uint> shapes; std::set<uint32_t> shapes;
shapes.insert(shape->mBroadPhaseID); shapes.insert(shape->mBroadPhaseID);
std::set<uint> emptySet; std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet); mCollisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet);
@ -183,9 +164,9 @@ void CollisionWorld::testCollision(const ProxyShape* shape1,
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
// Create the sets of shapes // Create the sets of shapes
std::set<uint> shapes1; std::set<uint32_t> shapes1;
shapes1.insert(shape1->mBroadPhaseID); shapes1.insert(shape1->mBroadPhaseID);
std::set<uint> shapes2; std::set<uint32_t> shapes2;
shapes2.insert(shape2->mBroadPhaseID); shapes2.insert(shape2->mBroadPhaseID);
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
@ -205,7 +186,7 @@ void CollisionWorld::testCollision(const CollisionBody* body,
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
// Create the sets of shapes // Create the sets of shapes
std::set<uint> shapes1; std::set<uint32_t> shapes1;
// For each shape of the body // For each shape of the body
for (const ProxyShape* shape=body->getProxyShapesList(); shape != NULL; for (const ProxyShape* shape=body->getProxyShapesList(); shape != NULL;
@ -213,7 +194,7 @@ void CollisionWorld::testCollision(const CollisionBody* body,
shapes1.insert(shape->mBroadPhaseID); shapes1.insert(shape->mBroadPhaseID);
} }
std::set<uint> emptySet; std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet); mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet);
@ -233,13 +214,13 @@ void CollisionWorld::testCollision(const CollisionBody* body1,
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
// Create the sets of shapes // Create the sets of shapes
std::set<uint> shapes1; std::set<uint32_t> shapes1;
for (const ProxyShape* shape=body1->getProxyShapesList(); shape != NULL; for (const ProxyShape* shape=body1->getProxyShapesList(); shape != NULL;
shape = shape->getNext()) { shape = shape->getNext()) {
shapes1.insert(shape->mBroadPhaseID); shapes1.insert(shape->mBroadPhaseID);
} }
std::set<uint> shapes2; std::set<uint32_t> shapes2;
for (const ProxyShape* shape=body2->getProxyShapesList(); shape != NULL; for (const ProxyShape* shape=body2->getProxyShapesList(); shape != NULL;
shape = shape->getNext()) { shape = shape->getNext()) {
shapes2.insert(shape->mBroadPhaseID); shapes2.insert(shape->mBroadPhaseID);
@ -258,7 +239,7 @@ void CollisionWorld::testCollision(CollisionCallback* callback) {
// Reset all the contact manifolds lists of each body // Reset all the contact manifolds lists of each body
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
std::set<uint> emptySet; std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet); mCollisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet);

37
ephysics/engine/CollisionWorld.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_COLLISION_WORLD_H
#define REACTPHYSICS3D_COLLISION_WORLD_H
// Libraries // Libraries
#include <vector> #include <vector>
@ -70,7 +49,7 @@ class CollisionWorld {
bodyindex mCurrentBodyID; bodyindex mCurrentBodyID;
/// List of free ID for rigid bodies /// List of free ID for rigid bodies
std::vector<luint> mFreeBodiesIDs; std::vector<uint64_t> mFreeBodiesIDs;
/// Memory allocator /// Memory allocator
MemoryAllocator mMemoryAllocator; MemoryAllocator mMemoryAllocator;
@ -233,5 +212,3 @@ class CollisionCallback {
}; };
} }
#endif

39
ephysics/engine/ConstraintSolver.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/engine/ConstraintSolver.h> #include <ephysics/engine/ConstraintSolver.h>
@ -30,7 +11,7 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
ConstraintSolver::ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex) ConstraintSolver::ConstraintSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex)
: mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), : mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mConstraintSolverData(mapBodyToVelocityIndex) { mIsWarmStartingActive(true), mConstraintSolverData(mapBodyToVelocityIndex) {
@ -42,7 +23,7 @@ ConstraintSolver::~ConstraintSolver() {
} }
// Initialize the constraint solver for a given island // Initialize the constraint solver for a given island
void ConstraintSolver::initializeForIsland(decimal dt, Island* island) { void ConstraintSolver::initializeForIsland(float dt, Island* island) {
PROFILE("ConstraintSolver::initializeForIsland()"); PROFILE("ConstraintSolver::initializeForIsland()");
@ -59,7 +40,7 @@ void ConstraintSolver::initializeForIsland(decimal dt, Island* island) {
// For each joint of the island // For each joint of the island
Joint** joints = island->getJoints(); Joint** joints = island->getJoints();
for (uint i=0; i<island->getNbJoints(); i++) { for (uint32_t i=0; i<island->getNbJoints(); i++) {
// Initialize the constraint before solving it // Initialize the constraint before solving it
joints[i]->initBeforeSolve(mConstraintSolverData); joints[i]->initBeforeSolve(mConstraintSolverData);
@ -81,7 +62,7 @@ void ConstraintSolver::solveVelocityConstraints(Island* island) {
// For each joint of the island // For each joint of the island
Joint** joints = island->getJoints(); Joint** joints = island->getJoints();
for (uint i=0; i<island->getNbJoints(); i++) { for (uint32_t i=0; i<island->getNbJoints(); i++) {
// Solve the constraint // Solve the constraint
joints[i]->solveVelocityConstraint(mConstraintSolverData); joints[i]->solveVelocityConstraint(mConstraintSolverData);
@ -98,7 +79,7 @@ void ConstraintSolver::solvePositionConstraints(Island* island) {
// For each joint of the island // For each joint of the island
Joint** joints = island->getJoints(); Joint** joints = island->getJoints();
for (uint i=0; i < island->getNbJoints(); i++) { for (uint32_t i=0; i < island->getNbJoints(); i++) {
// Solve the constraint // Solve the constraint
joints[i]->solvePositionConstraint(mConstraintSolverData); joints[i]->solvePositionConstraint(mConstraintSolverData);

53
ephysics/engine/ConstraintSolver.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CONSTRAINT_SOLVER_H
#define REACTPHYSICS3D_CONSTRAINT_SOLVER_H
// Libraries // Libraries
#include <ephysics/configuration.h> #include <ephysics/configuration.h>
@ -46,7 +25,7 @@ struct ConstraintSolverData {
public : public :
/// Current time step of the simulation /// Current time step of the simulation
decimal timeStep; float timeStep;
/// Array with the bodies linear velocities /// Array with the bodies linear velocities
Vector3* linearVelocities; Vector3* linearVelocities;
@ -62,13 +41,13 @@ struct ConstraintSolverData {
/// Reference to the map that associates rigid body to their index /// Reference to the map that associates rigid body to their index
/// in the constrained velocities array /// in the constrained velocities array
const std::map<RigidBody*, uint>& mapBodyToConstrainedVelocityIndex; const std::map<RigidBody*, uint32_t>& mapBodyToConstrainedVelocityIndex;
/// True if warm starting of the solver is active /// True if warm starting of the solver is active
bool isWarmStartingActive; bool isWarmStartingActive;
/// Constructor /// Constructor
ConstraintSolverData(const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex) ConstraintSolverData(const std::map<RigidBody*, uint32_t>& refMapBodyToConstrainedVelocityIndex)
:linearVelocities(NULL), angularVelocities(NULL), :linearVelocities(NULL), angularVelocities(NULL),
positions(NULL), orientations(NULL), positions(NULL), orientations(NULL),
mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){ mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
@ -99,7 +78,7 @@ struct ConstraintSolverData {
* *
* --- Step 1 --- * --- Step 1 ---
* *
* First, we integrate the applied force F_a acting of each rigid body (like gravity, ...) and * First, we int32_tegrate the applied force F_a acting of each rigid body (like gravity, ...) and
* we obtain some new velocities v2' that tends to violate the constraints. * we obtain some new velocities v2' that tends to violate the constraints.
* *
* v2' = v1 + dt * M^-1 * F_a * v2' = v1 + dt * M^-1 * F_a
@ -120,7 +99,7 @@ struct ConstraintSolverData {
* *
* --- Step 3 --- * --- Step 3 ---
* *
* In the third step, we integrate the new position x2 of the bodies using the new velocities * In the third step, we int32_tegrate the new position x2 of the bodies using the new velocities
* v2 computed in the second step with : x2 = x1 + dt * v2. * v2 computed in the second step with : x2 = x1 + dt * v2.
* *
* Note that in the following code (as it is also explained in the slides from Erin Catto), * Note that in the following code (as it is also explained in the slides from Erin Catto),
@ -154,10 +133,10 @@ class ConstraintSolver {
/// Reference to the map that associates rigid body to their index in /// Reference to the map that associates rigid body to their index in
/// the constrained velocities array /// the constrained velocities array
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex; const std::map<RigidBody*, uint32_t>& mMapBodyToConstrainedVelocityIndex;
/// Current time step /// Current time step
decimal mTimeStep; float mTimeStep;
/// True if the warm starting of the solver is active /// True if the warm starting of the solver is active
bool mIsWarmStartingActive; bool mIsWarmStartingActive;
@ -170,13 +149,13 @@ class ConstraintSolver {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex); ConstraintSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex);
/// Destructor /// Destructor
~ConstraintSolver(); ~ConstraintSolver();
/// Initialize the constraint solver for a given island /// Initialize the constraint solver for a given island
void initializeForIsland(decimal dt, Island* island); void initializeForIsland(float dt, Island* island);
/// Solve the constraints /// Solve the constraints
void solveVelocityConstraints(Island* island); void solveVelocityConstraints(Island* island);
@ -218,5 +197,3 @@ inline void ConstraintSolver::setConstrainedPositionsArrays(Vector3* constrained
} }
} }
#endif

209
ephysics/engine/ContactSolver.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/engine/ContactSolver.h> #include <ephysics/engine/ContactSolver.h>
@ -34,12 +15,12 @@ using namespace reactphysics3d;
using namespace std; using namespace std;
// Constants initialization // Constants initialization
const decimal ContactSolver::BETA = decimal(0.2); const float ContactSolver::BETA = float(0.2);
const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); const float ContactSolver::BETA_SPLIT_IMPULSE = float(0.2);
const decimal ContactSolver::SLOP= decimal(0.01); const float ContactSolver::SLOP= float(0.01);
// Constructor // Constructor
ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex) ContactSolver::ContactSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex)
:mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL), :mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
mContactConstraints(NULL), mLinearVelocities(NULL), mAngularVelocities(NULL), mContactConstraints(NULL), mLinearVelocities(NULL), mAngularVelocities(NULL),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
@ -54,7 +35,7 @@ ContactSolver::~ContactSolver() {
} }
// Initialize the constraint solver for a given island // Initialize the constraint solver for a given island
void ContactSolver::initializeForIsland(decimal dt, Island* island) { void ContactSolver::initializeForIsland(float dt, Island* island) {
PROFILE("ContactSolver::initializeForIsland()"); PROFILE("ContactSolver::initializeForIsland()");
@ -74,11 +55,11 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
// For each contact manifold of the island // For each contact manifold of the island
ContactManifold** contactManifolds = island->getContactManifold(); ContactManifold** contactManifolds = island->getContactManifold();
for (uint i=0; i<mNbContactManifolds; i++) { for (uint32_t i=0; i<mNbContactManifolds; i++) {
ContactManifold* externalManifold = contactManifolds[i]; ContactManifold* externalManifold = contactManifolds[i];
ContactManifoldSolver& internalManifold = mContactConstraints[i]; ContactManifoldSolver& int32_ternalManifold = mContactConstraints[i];
assert(externalManifold->getNbContactPoints() > 0); assert(externalManifold->getNbContactPoints() > 0);
@ -92,32 +73,32 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
const Vector3& x1 = body1->mCenterOfMassWorld; const Vector3& x1 = body1->mCenterOfMassWorld;
const Vector3& x2 = body2->mCenterOfMassWorld; const Vector3& x2 = body2->mCenterOfMassWorld;
// Initialize the internal contact manifold structure using the external // Initialize the int32_ternal contact manifold structure using the external
// contact manifold // contact manifold
internalManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; int32_ternalManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; int32_ternalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); int32_ternalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); int32_ternalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
internalManifold.massInverseBody1 = body1->mMassInverse; int32_ternalManifold.massInverseBody1 = body1->mMassInverse;
internalManifold.massInverseBody2 = body2->mMassInverse; int32_ternalManifold.massInverseBody2 = body2->mMassInverse;
internalManifold.nbContacts = externalManifold->getNbContactPoints(); int32_ternalManifold.nbContacts = externalManifold->getNbContactPoints();
internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2); int32_ternalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2);
internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); int32_ternalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
internalManifold.rollingResistanceFactor = computeMixedRollingResistance(body1, body2); int32_ternalManifold.rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
internalManifold.externalContactManifold = externalManifold; int32_ternalManifold.externalContactManifold = externalManifold;
internalManifold.isBody1DynamicType = body1->getType() == DYNAMIC; int32_ternalManifold.isBody1DynamicType = body1->getType() == DYNAMIC;
internalManifold.isBody2DynamicType = body2->getType() == DYNAMIC; int32_ternalManifold.isBody2DynamicType = body2->getType() == DYNAMIC;
// If we solve the friction constraints at the center of the contact manifold // If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) { if (mIsSolveFrictionAtContactManifoldCenterActive) {
internalManifold.frictionPointBody1 = Vector3::zero(); int32_ternalManifold.frictionPointBody1 = Vector3::zero();
internalManifold.frictionPointBody2 = Vector3::zero(); int32_ternalManifold.frictionPointBody2 = Vector3::zero();
} }
// For each contact point of the contact manifold // For each contact point of the contact manifold
for (uint c=0; c<externalManifold->getNbContactPoints(); c++) { for (uint32_t c=0; c<externalManifold->getNbContactPoints(); c++) {
ContactPointSolver& contactPoint = internalManifold.contacts[c]; ContactPointSolver& contactPoint = int32_ternalManifold.contacts[c];
// Get a contact point // Get a contact point
ContactPoint* externalContact = externalManifold->getContactPoint(c); ContactPoint* externalContact = externalManifold->getContactPoint(c);
@ -142,36 +123,36 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
// If we solve the friction constraints at the center of the contact manifold // If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) { if (mIsSolveFrictionAtContactManifoldCenterActive) {
internalManifold.frictionPointBody1 += p1; int32_ternalManifold.frictionPointBody1 += p1;
internalManifold.frictionPointBody2 += p2; int32_ternalManifold.frictionPointBody2 += p2;
} }
} }
// If we solve the friction constraints at the center of the contact manifold // If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) { if (mIsSolveFrictionAtContactManifoldCenterActive) {
internalManifold.frictionPointBody1 /=static_cast<decimal>(internalManifold.nbContacts); int32_ternalManifold.frictionPointBody1 /=static_cast<float>(int32_ternalManifold.nbContacts);
internalManifold.frictionPointBody2 /=static_cast<decimal>(internalManifold.nbContacts); int32_ternalManifold.frictionPointBody2 /=static_cast<float>(int32_ternalManifold.nbContacts);
internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1; int32_ternalManifold.r1Friction = int32_ternalManifold.frictionPointBody1 - x1;
internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2; int32_ternalManifold.r2Friction = int32_ternalManifold.frictionPointBody2 - x2;
internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1(); int32_ternalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1();
internalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2(); int32_ternalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2();
// If warm starting is active // If warm starting is active
if (mIsWarmStartingActive) { if (mIsWarmStartingActive) {
// Initialize the accumulated impulses with the previous step accumulated impulses // Initialize the accumulated impulses with the previous step accumulated impulses
internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1(); int32_ternalManifold.friction1Impulse = externalManifold->getFrictionImpulse1();
internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2(); int32_ternalManifold.friction2Impulse = externalManifold->getFrictionImpulse2();
internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); int32_ternalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
} }
else { else {
// Initialize the accumulated impulses to zero // Initialize the accumulated impulses to zero
internalManifold.friction1Impulse = 0.0; int32_ternalManifold.friction1Impulse = 0.0;
internalManifold.friction2Impulse = 0.0; int32_ternalManifold.friction2Impulse = 0.0;
internalManifold.frictionTwistImpulse = 0.0; int32_ternalManifold.frictionTwistImpulse = 0.0;
internalManifold.rollingResistanceImpulse = Vector3(0, 0, 0); int32_ternalManifold.rollingResistanceImpulse = Vector3(0, 0, 0);
} }
} }
} }
@ -184,7 +165,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
void ContactSolver::initializeContactConstraints() { void ContactSolver::initializeContactConstraints() {
// For each contact constraint // For each contact constraint
for (uint c=0; c<mNbContactManifolds; c++) { for (uint32_t c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& manifold = mContactConstraints[c]; ContactManifoldSolver& manifold = mContactConstraints[c];
@ -204,7 +185,7 @@ void ContactSolver::initializeContactConstraints() {
const Vector3& w2 = mAngularVelocities[manifold.indexBody2]; const Vector3& w2 = mAngularVelocities[manifold.indexBody2];
// For each contact point constraint // For each contact point constraint
for (uint i=0; i<manifold.nbContacts; i++) { for (uint32_t i=0; i<manifold.nbContacts; i++) {
ContactPointSolver& contactPoint = manifold.contacts[i]; ContactPointSolver& contactPoint = manifold.contacts[i];
ContactPoint* externalContact = contactPoint.externalContact; ContactPoint* externalContact = contactPoint.externalContact;
@ -216,12 +197,12 @@ void ContactSolver::initializeContactConstraints() {
contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal); contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal);
// Compute the inverse mass matrix K for the penetration constraint // Compute the inverse mass matrix K for the penetration constraint
decimal massPenetration = manifold.massInverseBody1 + manifold.massInverseBody2 + float massPenetration = manifold.massInverseBody1 + manifold.massInverseBody2 +
((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) + ((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) +
((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal); ((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal);
massPenetration > 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) / massPenetration > 0.0 ? contactPoint.inversePenetrationMass = float(1.0) /
massPenetration : massPenetration :
decimal(0.0); float(0.0);
// If we do not solve the friction constraints at the center of the contact manifold // If we do not solve the friction constraints at the center of the contact manifold
if (!mIsSolveFrictionAtContactManifoldCenterActive) { if (!mIsSolveFrictionAtContactManifoldCenterActive) {
@ -236,22 +217,22 @@ void ContactSolver::initializeContactConstraints() {
// Compute the inverse mass matrix K for the friction // Compute the inverse mass matrix K for the friction
// constraints at each contact point // constraints at each contact point
decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot( ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot(
contactPoint.frictionVector1) + contactPoint.frictionVector1) +
((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot( ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot(
contactPoint.frictionVector1); contactPoint.frictionVector1);
decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot( ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot(
contactPoint.frictionVector2) + contactPoint.frictionVector2) +
((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot( ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot(
contactPoint.frictionVector2); contactPoint.frictionVector2);
friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) / friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = float(1.0) /
friction1Mass : friction1Mass :
decimal(0.0); float(0.0);
friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = decimal(1.0) / friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = float(1.0) /
friction2Mass : friction2Mass :
decimal(0.0); float(0.0);
} }
// Compute the restitution velocity bias "b". We compute this here instead // Compute the restitution velocity bias "b". We compute this here instead
@ -259,7 +240,7 @@ void ContactSolver::initializeContactConstraints() {
// at the beginning of the contact. Note that if it is a resting contact (normal // at the beginning of the contact. Note that if it is a resting contact (normal
// velocity bellow a given threshold), we do not add a restitution velocity bias // velocity bellow a given threshold), we do not add a restitution velocity bias
contactPoint.restitutionBias = 0.0; contactPoint.restitutionBias = 0.0;
decimal deltaVDotN = deltaV.dot(contactPoint.normal); float deltaVDotN = deltaV.dot(contactPoint.normal);
if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) {
contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN; contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN;
} }
@ -307,27 +288,27 @@ void ContactSolver::initializeContactConstraints() {
manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2); manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2);
manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1); manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1);
manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2); manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2);
decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot( ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot(
manifold.frictionVector1) + manifold.frictionVector1) +
((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot( ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot(
manifold.frictionVector1); manifold.frictionVector1);
decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot( ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot(
manifold.frictionVector2) + manifold.frictionVector2) +
((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot( ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot(
manifold.frictionVector2); manifold.frictionVector2);
decimal frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 * float frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 *
manifold.normal) + manifold.normal) +
manifold.normal.dot(manifold.inverseInertiaTensorBody2 * manifold.normal.dot(manifold.inverseInertiaTensorBody2 *
manifold.normal); manifold.normal);
friction1Mass > 0.0 ? manifold.inverseFriction1Mass = decimal(1.0)/friction1Mass friction1Mass > 0.0 ? manifold.inverseFriction1Mass = float(1.0)/friction1Mass
: decimal(0.0); : float(0.0);
friction2Mass > 0.0 ? manifold.inverseFriction2Mass = decimal(1.0)/friction2Mass friction2Mass > 0.0 ? manifold.inverseFriction2Mass = float(1.0)/friction2Mass
: decimal(0.0); : float(0.0);
frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = decimal(1.0) / frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = float(1.0) /
frictionTwistMass : frictionTwistMass :
decimal(0.0); float(0.0);
} }
} }
} }
@ -342,13 +323,13 @@ void ContactSolver::warmStart() {
if (!mIsWarmStartingActive) return; if (!mIsWarmStartingActive) return;
// For each constraint // For each constraint
for (uint c=0; c<mNbContactManifolds; c++) { for (uint32_t c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c]; ContactManifoldSolver& contactManifold = mContactConstraints[c];
bool atLeastOneRestingContactPoint = false; bool atLeastOneRestingContactPoint = false;
for (uint i=0; i<contactManifold.nbContacts; i++) { for (uint32_t i=0; i<contactManifold.nbContacts; i++) {
ContactPointSolver& contactPoint = contactManifold.contacts[i]; ContactPointSolver& contactPoint = contactManifold.contacts[i];
@ -369,7 +350,7 @@ void ContactSolver::warmStart() {
// If we do not solve the friction constraints at the center of the contact manifold // If we do not solve the friction constraints at the center of the contact manifold
if (!mIsSolveFrictionAtContactManifoldCenterActive) { if (!mIsSolveFrictionAtContactManifoldCenterActive) {
// Project the old friction impulses (with old friction vectors) into // Project the old friction impulses (with old friction vectors) int32_to
// the new friction vectors to get the new friction impulses // the new friction vectors to get the new friction impulses
Vector3 oldFrictionImpulse = contactPoint.friction1Impulse * Vector3 oldFrictionImpulse = contactPoint.friction1Impulse *
contactPoint.oldFrictionVector1 + contactPoint.oldFrictionVector1 +
@ -425,7 +406,7 @@ void ContactSolver::warmStart() {
// at least one resting contact point in the contact manifold // at least one resting contact point in the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) { if (mIsSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) {
// Project the old friction impulses (with old friction vectors) into the new friction // Project the old friction impulses (with old friction vectors) int32_to the new friction
// vectors to get the new friction impulses // vectors to get the new friction impulses
Vector3 oldFrictionImpulse = contactManifold.friction1Impulse * Vector3 oldFrictionImpulse = contactManifold.friction1Impulse *
contactManifold.oldFrictionVector1 + contactManifold.oldFrictionVector1 +
@ -510,15 +491,15 @@ void ContactSolver::solve() {
PROFILE("ContactSolver::solve()"); PROFILE("ContactSolver::solve()");
decimal deltaLambda; float deltaLambda;
decimal lambdaTemp; float lambdaTemp;
// For each contact manifold // For each contact manifold
for (uint c=0; c<mNbContactManifolds; c++) { for (uint32_t c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c]; ContactManifoldSolver& contactManifold = mContactConstraints[c];
decimal sumPenetrationImpulse = 0.0; float sumPenetrationImpulse = 0.0;
// Get the constrained velocities // Get the constrained velocities
const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1]; const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
@ -526,7 +507,7 @@ void ContactSolver::solve() {
const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2]; const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2];
const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2]; const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2];
for (uint i=0; i<contactManifold.nbContacts; i++) { for (uint32_t i=0; i<contactManifold.nbContacts; i++) {
ContactPointSolver& contactPoint = contactManifold.contacts[i]; ContactPointSolver& contactPoint = contactManifold.contacts[i];
@ -534,15 +515,15 @@ void ContactSolver::solve() {
// Compute J*v // Compute J*v
Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
decimal deltaVDotN = deltaV.dot(contactPoint.normal); float deltaVDotN = deltaV.dot(contactPoint.normal);
decimal Jv = deltaVDotN; float Jv = deltaVDotN;
// Compute the bias "b" of the constraint // Compute the bias "b" of the constraint
decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA; float beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
decimal biasPenetrationDepth = 0.0; float biasPenetrationDepth = 0.0;
if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) * if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
max(0.0f, float(contactPoint.penetrationDepth - SLOP)); max(0.0f, float(contactPoint.penetrationDepth - SLOP));
decimal b = biasPenetrationDepth + contactPoint.restitutionBias; float b = biasPenetrationDepth + contactPoint.restitutionBias;
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
if (mIsSplitImpulseActive) { if (mIsSplitImpulseActive) {
@ -554,7 +535,7 @@ void ContactSolver::solve() {
} }
lambdaTemp = contactPoint.penetrationImpulse; lambdaTemp = contactPoint.penetrationImpulse;
contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse + contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse +
deltaLambda, decimal(0.0)); deltaLambda, float(0.0));
deltaLambda = contactPoint.penetrationImpulse - lambdaTemp; deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
@ -576,13 +557,13 @@ void ContactSolver::solve() {
const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2]; const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2];
Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) -
v1Split - w1Split.cross(contactPoint.r1); v1Split - w1Split.cross(contactPoint.r1);
decimal JvSplit = deltaVSplit.dot(contactPoint.normal); float JvSplit = deltaVSplit.dot(contactPoint.normal);
decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * float deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
contactPoint.inversePenetrationMass; contactPoint.inversePenetrationMass;
decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse; float lambdaTempSplit = contactPoint.penetrationSplitImpulse;
contactPoint.penetrationSplitImpulse = std::max( contactPoint.penetrationSplitImpulse = std::max(
contactPoint.penetrationSplitImpulse + contactPoint.penetrationSplitImpulse +
deltaLambdaSplit, decimal(0.0)); deltaLambdaSplit, float(0.0));
deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit; deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
@ -604,7 +585,7 @@ void ContactSolver::solve() {
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
deltaLambda = -Jv; deltaLambda = -Jv;
deltaLambda *= contactPoint.inverseFriction1Mass; deltaLambda *= contactPoint.inverseFriction1Mass;
decimal frictionLimit = contactManifold.frictionCoefficient * float frictionLimit = contactManifold.frictionCoefficient *
contactPoint.penetrationImpulse; contactPoint.penetrationImpulse;
lambdaTemp = contactPoint.friction1Impulse; lambdaTemp = contactPoint.friction1Impulse;
contactPoint.friction1Impulse = std::max(-frictionLimit, contactPoint.friction1Impulse = std::max(-frictionLimit,
@ -652,7 +633,7 @@ void ContactSolver::solve() {
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
decimal rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse; float rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse;
Vector3 lambdaTempRolling = contactPoint.rollingResistanceImpulse; Vector3 lambdaTempRolling = contactPoint.rollingResistanceImpulse;
contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse + contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse +
deltaLambdaRolling, rollingLimit); deltaLambdaRolling, rollingLimit);
@ -676,11 +657,11 @@ void ContactSolver::solve() {
// Compute J*v // Compute J*v
Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction) Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
- v1 - w1.cross(contactManifold.r1Friction); - v1 - w1.cross(contactManifold.r1Friction);
decimal Jv = deltaV.dot(contactManifold.frictionVector1); float Jv = deltaV.dot(contactManifold.frictionVector1);
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass; float deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; float frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.friction1Impulse; lambdaTemp = contactManifold.friction1Impulse;
contactManifold.friction1Impulse = std::max(-frictionLimit, contactManifold.friction1Impulse = std::max(-frictionLimit,
std::min(contactManifold.friction1Impulse + std::min(contactManifold.friction1Impulse +
@ -759,7 +740,7 @@ void ContactSolver::solve() {
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
decimal rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse; float rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse;
Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse; Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse;
contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse + contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse +
deltaLambdaRolling, rollingLimit); deltaLambdaRolling, rollingLimit);
@ -783,11 +764,11 @@ void ContactSolver::solve() {
void ContactSolver::storeImpulses() { void ContactSolver::storeImpulses() {
// For each contact manifold // For each contact manifold
for (uint c=0; c<mNbContactManifolds; c++) { for (uint32_t c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& manifold = mContactConstraints[c]; ContactManifoldSolver& manifold = mContactConstraints[c];
for (uint i=0; i<manifold.nbContacts; i++) { for (uint32_t i=0; i<manifold.nbContacts; i++) {
ContactPointSolver& contactPoint = manifold.contacts[i]; ContactPointSolver& contactPoint = manifold.contacts[i];
@ -855,7 +836,7 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
Vector3 tangentVelocity = deltaVelocity - normalVelocity; Vector3 tangentVelocity = deltaVelocity - normalVelocity;
// If the velocty difference in the tangential plane is not zero // If the velocty difference in the tangential plane is not zero
decimal lengthTangenVelocity = tangentVelocity.length(); float lengthTangenVelocity = tangentVelocity.length();
if (lengthTangenVelocity > MACHINE_EPSILON) { if (lengthTangenVelocity > MACHINE_EPSILON) {
// Compute the first friction vector in the direction of the tangent // Compute the first friction vector in the direction of the tangent
@ -885,7 +866,7 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
Vector3 tangentVelocity = deltaVelocity - normalVelocity; Vector3 tangentVelocity = deltaVelocity - normalVelocity;
// If the velocty difference in the tangential plane is not zero // If the velocty difference in the tangential plane is not zero
decimal lengthTangenVelocity = tangentVelocity.length(); float lengthTangenVelocity = tangentVelocity.length();
if (lengthTangenVelocity > MACHINE_EPSILON) { if (lengthTangenVelocity > MACHINE_EPSILON) {
// Compute the first friction vector in the direction of the tangent // Compute the first friction vector in the direction of the tangent

135
ephysics/engine/ContactSolver.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_CONTACT_SOLVER_H
#define REACTPHYSICS3D_CONTACT_SOLVER_H
// Libraries // Libraries
#include <ephysics/constraint/ContactPoint.h> #include <ephysics/constraint/ContactPoint.h>
@ -62,7 +41,7 @@ namespace reactphysics3d {
* *
* --- Step 1 --- * --- Step 1 ---
* *
* First, we integrate the applied force F_a acting of each rigid body (like gravity, ...) and * First, we int32_tegrate the applied force F_a acting of each rigid body (like gravity, ...) and
* we obtain some new velocities v2' that tends to violate the constraints. * we obtain some new velocities v2' that tends to violate the constraints.
* *
* v2' = v1 + dt * M^-1 * F_a * v2' = v1 + dt * M^-1 * F_a
@ -83,7 +62,7 @@ namespace reactphysics3d {
* *
* --- Step 3 --- * --- Step 3 ---
* *
* In the third step, we integrate the new position x2 of the bodies using the new velocities * In the third step, we int32_tegrate the new position x2 of the bodies using the new velocities
* v2 computed in the second step with : x2 = x1 + dt * v2. * v2 computed in the second step with : x2 = x1 + dt * v2.
* *
* Note that in the following code (as it is also explained in the slides from Erin Catto), * Note that in the following code (as it is also explained in the slides from Erin Catto),
@ -115,22 +94,22 @@ class ContactSolver {
// Structure ContactPointSolver // Structure ContactPointSolver
/** /**
* Contact solver internal data structure that to store all the * Contact solver int32_ternal data structure that to store all the
* information relative to a contact point * information relative to a contact point
*/ */
struct ContactPointSolver { struct ContactPointSolver {
/// Accumulated normal impulse /// Accumulated normal impulse
decimal penetrationImpulse; float penetrationImpulse;
/// Accumulated impulse in the 1st friction direction /// Accumulated impulse in the 1st friction direction
decimal friction1Impulse; float friction1Impulse;
/// Accumulated impulse in the 2nd friction direction /// Accumulated impulse in the 2nd friction direction
decimal friction2Impulse; float friction2Impulse;
/// Accumulated split impulse for penetration correction /// Accumulated split impulse for penetration correction
decimal penetrationSplitImpulse; float penetrationSplitImpulse;
/// Accumulated rolling resistance impulse /// Accumulated rolling resistance impulse
Vector3 rollingResistanceImpulse; Vector3 rollingResistanceImpulse;
@ -175,19 +154,19 @@ class ContactSolver {
Vector3 r2CrossN; Vector3 r2CrossN;
/// Penetration depth /// Penetration depth
decimal penetrationDepth; float penetrationDepth;
/// Velocity restitution bias /// Velocity restitution bias
decimal restitutionBias; float restitutionBias;
/// Inverse of the matrix K for the penenetration /// Inverse of the matrix K for the penenetration
decimal inversePenetrationMass; float inversePenetrationMass;
/// Inverse of the matrix K for the 1st friction /// Inverse of the matrix K for the 1st friction
decimal inverseFriction1Mass; float inverseFriction1Mass;
/// Inverse of the matrix K for the 2nd friction /// Inverse of the matrix K for the 2nd friction
decimal inverseFriction2Mass; float inverseFriction2Mass;
/// True if the contact was existing last time step /// True if the contact was existing last time step
bool isRestingContact; bool isRestingContact;
@ -198,22 +177,22 @@ class ContactSolver {
// Structure ContactManifoldSolver // Structure ContactManifoldSolver
/** /**
* Contact solver internal data structure to store all the * Contact solver int32_ternal data structure to store all the
* information relative to a contact manifold. * information relative to a contact manifold.
*/ */
struct ContactManifoldSolver { struct ContactManifoldSolver {
/// Index of body 1 in the constraint solver /// Index of body 1 in the constraint solver
uint indexBody1; uint32_t indexBody1;
/// Index of body 2 in the constraint solver /// Index of body 2 in the constraint solver
uint indexBody2; uint32_t indexBody2;
/// Inverse of the mass of body 1 /// Inverse of the mass of body 1
decimal massInverseBody1; float massInverseBody1;
// Inverse of the mass of body 2 // Inverse of the mass of body 2
decimal massInverseBody2; float massInverseBody2;
/// Inverse inertia tensor of body 1 /// Inverse inertia tensor of body 1
Matrix3x3 inverseInertiaTensorBody1; Matrix3x3 inverseInertiaTensorBody1;
@ -225,7 +204,7 @@ class ContactSolver {
ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD]; ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];
/// Number of contact points /// Number of contact points
uint nbContacts; uint32_t nbContacts;
/// True if the body 1 is of type dynamic /// True if the body 1 is of type dynamic
bool isBody1DynamicType; bool isBody1DynamicType;
@ -234,13 +213,13 @@ class ContactSolver {
bool isBody2DynamicType; bool isBody2DynamicType;
/// Mix of the restitution factor for two bodies /// Mix of the restitution factor for two bodies
decimal restitutionFactor; float restitutionFactor;
/// Mix friction coefficient for the two bodies /// Mix friction coefficient for the two bodies
decimal frictionCoefficient; float frictionCoefficient;
/// Rolling resistance factor between the two bodies /// Rolling resistance factor between the two bodies
decimal rollingResistanceFactor; float rollingResistanceFactor;
/// Pointer to the external contact manifold /// Pointer to the external contact manifold
ContactManifold* externalContactManifold; ContactManifold* externalContactManifold;
@ -275,13 +254,13 @@ class ContactSolver {
Vector3 r2CrossT2; Vector3 r2CrossT2;
/// Matrix K for the first friction constraint /// Matrix K for the first friction constraint
decimal inverseFriction1Mass; float inverseFriction1Mass;
/// Matrix K for the second friction constraint /// Matrix K for the second friction constraint
decimal inverseFriction2Mass; float inverseFriction2Mass;
/// Matrix K for the twist friction constraint /// Matrix K for the twist friction constraint
decimal inverseTwistFrictionMass; float inverseTwistFrictionMass;
/// Matrix K for the rolling resistance constraint /// Matrix K for the rolling resistance constraint
Matrix3x3 inverseRollingResistance; Matrix3x3 inverseRollingResistance;
@ -299,13 +278,13 @@ class ContactSolver {
Vector3 oldFrictionVector2; Vector3 oldFrictionVector2;
/// First friction direction impulse at manifold center /// First friction direction impulse at manifold center
decimal friction1Impulse; float friction1Impulse;
/// Second friction direction impulse at manifold center /// Second friction direction impulse at manifold center
decimal friction2Impulse; float friction2Impulse;
/// Twist friction impulse at contact manifold center /// Twist friction impulse at contact manifold center
decimal frictionTwistImpulse; float frictionTwistImpulse;
/// Rolling resistance impulse /// Rolling resistance impulse
Vector3 rollingResistanceImpulse; Vector3 rollingResistanceImpulse;
@ -314,13 +293,13 @@ class ContactSolver {
// -------------------- Constants --------------------- // // -------------------- Constants --------------------- //
/// Beta value for the penetration depth position correction without split impulses /// Beta value for the penetration depth position correction without split impulses
static const decimal BETA; static const float BETA;
/// Beta value for the penetration depth position correction with split impulses /// Beta value for the penetration depth position correction with split impulses
static const decimal BETA_SPLIT_IMPULSE; static const float BETA_SPLIT_IMPULSE;
/// Slop distance (allowed penetration distance between bodies) /// Slop distance (allowed penetration distance between bodies)
static const decimal SLOP; static const float SLOP;
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
@ -331,13 +310,13 @@ class ContactSolver {
Vector3* mSplitAngularVelocities; Vector3* mSplitAngularVelocities;
/// Current time step /// Current time step
decimal mTimeStep; float mTimeStep;
/// Contact constraints /// Contact constraints
ContactManifoldSolver* mContactConstraints; ContactManifoldSolver* mContactConstraints;
/// Number of contact constraints /// Number of contact constraints
uint mNbContactManifolds; uint32_t mNbContactManifolds;
/// Array of linear velocities /// Array of linear velocities
Vector3* mLinearVelocities; Vector3* mLinearVelocities;
@ -346,7 +325,7 @@ class ContactSolver {
Vector3* mAngularVelocities; Vector3* mAngularVelocities;
/// Reference to the map of rigid body to their index in the constrained velocities array /// Reference to the map of rigid body to their index in the constrained velocities array
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex; const std::map<RigidBody*, uint32_t>& mMapBodyToConstrainedVelocityIndex;
/// True if the warm starting of the solver is active /// True if the warm starting of the solver is active
bool mIsWarmStartingActive; bool mIsWarmStartingActive;
@ -371,15 +350,15 @@ class ContactSolver {
const ContactManifoldSolver& manifold); const ContactManifoldSolver& manifold);
/// Compute the collision restitution factor from the restitution factor of each body /// Compute the collision restitution factor from the restitution factor of each body
decimal computeMixedRestitutionFactor(RigidBody *body1, float computeMixedRestitutionFactor(RigidBody *body1,
RigidBody *body2) const; RigidBody *body2) const;
/// Compute the mixed friction coefficient from the friction coefficient of each body /// Compute the mixed friction coefficient from the friction coefficient of each body
decimal computeMixedFrictionCoefficient(RigidBody* body1, float computeMixedFrictionCoefficient(RigidBody* body1,
RigidBody* body2) const; RigidBody* body2) const;
/// Compute th mixed rolling resistance factor between two bodies /// Compute th mixed rolling resistance factor between two bodies
decimal computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const; float computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const;
/// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction /// 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 /// plane for a contact point. The two vectors have to be
@ -394,15 +373,15 @@ class ContactSolver {
ContactManifoldSolver& contactPoint) const; ContactManifoldSolver& contactPoint) const;
/// Compute a penetration constraint impulse /// Compute a penetration constraint impulse
const Impulse computePenetrationImpulse(decimal deltaLambda, const Impulse computePenetrationImpulse(float deltaLambda,
const ContactPointSolver& contactPoint) const; const ContactPointSolver& contactPoint) const;
/// Compute the first friction constraint impulse /// Compute the first friction constraint impulse
const Impulse computeFriction1Impulse(decimal deltaLambda, const Impulse computeFriction1Impulse(float deltaLambda,
const ContactPointSolver& contactPoint) const; const ContactPointSolver& contactPoint) const;
/// Compute the second friction constraint impulse /// Compute the second friction constraint impulse
const Impulse computeFriction2Impulse(decimal deltaLambda, const Impulse computeFriction2Impulse(float deltaLambda,
const ContactPointSolver& contactPoint) const; const ContactPointSolver& contactPoint) const;
public: public:
@ -410,13 +389,13 @@ class ContactSolver {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex); ContactSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex);
/// Destructor /// Destructor
virtual ~ContactSolver(); virtual ~ContactSolver();
/// Initialize the constraint solver for a given island /// Initialize the constraint solver for a given island
void initializeForIsland(decimal dt, Island* island); void initializeForIsland(float dt, Island* island);
/// Set the split velocities arrays /// Set the split velocities arrays
void setSplitVelocitiesArrays(Vector3* splitLinearVelocities, void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
@ -485,17 +464,17 @@ inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool
} }
// Compute the collision restitution factor from the restitution factor of each body // Compute the collision restitution factor from the restitution factor of each body
inline decimal ContactSolver::computeMixedRestitutionFactor(RigidBody* body1, inline float ContactSolver::computeMixedRestitutionFactor(RigidBody* body1,
RigidBody* body2) const { RigidBody* body2) const {
decimal restitution1 = body1->getMaterial().getBounciness(); float restitution1 = body1->getMaterial().getBounciness();
decimal restitution2 = body2->getMaterial().getBounciness(); float restitution2 = body2->getMaterial().getBounciness();
// Return the largest restitution factor // Return the largest restitution factor
return (restitution1 > restitution2) ? restitution1 : restitution2; return (restitution1 > restitution2) ? restitution1 : restitution2;
} }
// Compute the mixed friction coefficient from the friction coefficient of each body // Compute the mixed friction coefficient from the friction coefficient of each body
inline decimal ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1, inline float ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1,
RigidBody *body2) const { RigidBody *body2) const {
// Use the geometric mean to compute the mixed friction coefficient // Use the geometric mean to compute the mixed friction coefficient
return sqrt(body1->getMaterial().getFrictionCoefficient() * return sqrt(body1->getMaterial().getFrictionCoefficient() *
@ -503,13 +482,13 @@ inline decimal ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1,
} }
// Compute th mixed rolling resistance factor between two bodies // Compute th mixed rolling resistance factor between two bodies
inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1, inline float ContactSolver::computeMixedRollingResistance(RigidBody* body1,
RigidBody* body2) const { RigidBody* body2) const {
return decimal(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance()); return float(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance());
} }
// Compute a penetration constraint impulse // Compute a penetration constraint impulse
inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda, inline const Impulse ContactSolver::computePenetrationImpulse(float deltaLambda,
const ContactPointSolver& contactPoint) const ContactPointSolver& contactPoint)
const { const {
return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda, return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda,
@ -517,7 +496,7 @@ inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambd
} }
// Compute the first friction constraint impulse // Compute the first friction constraint impulse
inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda, inline const Impulse ContactSolver::computeFriction1Impulse(float deltaLambda,
const ContactPointSolver& contactPoint) const ContactPointSolver& contactPoint)
const { const {
return Impulse(-contactPoint.frictionVector1 * deltaLambda, return Impulse(-contactPoint.frictionVector1 * deltaLambda,
@ -527,7 +506,7 @@ inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda,
} }
// Compute the second friction constraint impulse // Compute the second friction constraint impulse
inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda, inline const Impulse ContactSolver::computeFriction2Impulse(float deltaLambda,
const ContactPointSolver& contactPoint) const ContactPointSolver& contactPoint)
const { const {
return Impulse(-contactPoint.frictionVector2 * deltaLambda, return Impulse(-contactPoint.frictionVector2 * deltaLambda,
@ -537,5 +516,3 @@ inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda,
} }
} }
#endif

171
ephysics/engine/DynamicsWorld.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/engine/DynamicsWorld.h> #include <ephysics/engine/DynamicsWorld.h>
@ -76,7 +57,7 @@ DynamicsWorld::~DynamicsWorld() {
} }
// Release the memory allocated for the islands // Release the memory allocated for the islands
for (uint i=0; i<mNbIslands; i++) { for (uint32_t i=0; i<mNbIslands; i++) {
// Call the island destructor // Call the island destructor
mIslands[i]->~Island(); mIslands[i]->~Island();
@ -103,8 +84,8 @@ DynamicsWorld::~DynamicsWorld() {
#ifdef IS_PROFILING_ACTIVE #ifdef IS_PROFILING_ACTIVE
// Print the profiling report // Print32_t the profiling report
Profiler::printReport(std::cout); Profiler::print32_tReport(std::cout);
// Destroy the profiler (release the allocated memory) // Destroy the profiler (release the allocated memory)
Profiler::destroy(); Profiler::destroy();
@ -116,7 +97,7 @@ DynamicsWorld::~DynamicsWorld() {
/** /**
* @param timeStep The amount of time to step the simulation by (in seconds) * @param timeStep The amount of time to step the simulation by (in seconds)
*/ */
void DynamicsWorld::update(decimal timeStep) { void DynamicsWorld::update(float timeStep) {
#ifdef IS_PROFILING_ACTIVE #ifdef IS_PROFILING_ACTIVE
// Increment the frame counter of the profiler // Increment the frame counter of the profiler
@ -127,7 +108,7 @@ void DynamicsWorld::update(decimal timeStep) {
mTimeStep = timeStep; mTimeStep = timeStep;
// Notify the event listener about the beginning of an internal tick // Notify the event listener about the beginning of an int32_ternal tick
if (mEventListener != NULL) { if (mEventListener != NULL) {
mEventListener->beginInternalTick(); mEventListener->beginInternalTick();
} }
@ -147,13 +128,13 @@ void DynamicsWorld::update(decimal timeStep) {
computeIslands(); computeIslands();
// Integrate the velocities // Integrate the velocities
integrateRigidBodiesVelocities(); int32_tegrateRigidBodiesVelocities();
// Solve the contacts and constraints // Solve the contacts and constraints
solveContactsAndConstraints(); solveContactsAndConstraints();
// Integrate the position and orientation of each body // Integrate the position and orientation of each body
integrateRigidBodiesPositions(); int32_tegrateRigidBodiesPositions();
// Solve the position correction for constraints // Solve the position correction for constraints
solvePositionCorrection(); solvePositionCorrection();
@ -163,7 +144,7 @@ void DynamicsWorld::update(decimal timeStep) {
if (mIsSleepingEnabled) updateSleepingBodies(); if (mIsSleepingEnabled) updateSleepingBodies();
// Notify the event listener about the end of an internal tick // Notify the event listener about the end of an int32_ternal tick
if (mEventListener != NULL) mEventListener->endInternalTick(); if (mEventListener != NULL) mEventListener->endInternalTick();
// Reset the external force and torque applied to the bodies // Reset the external force and torque applied to the bodies
@ -171,22 +152,22 @@ void DynamicsWorld::update(decimal timeStep) {
} }
// Integrate position and orientation of the rigid bodies. // Integrate position and orientation of the rigid bodies.
/// The positions and orientations of the bodies are integrated using /// The positions and orientations of the bodies are int32_tegrated using
/// the sympletic Euler time stepping scheme. /// the sympletic Euler time stepping scheme.
void DynamicsWorld::integrateRigidBodiesPositions() { void DynamicsWorld::int32_tegrateRigidBodiesPositions() {
PROFILE("DynamicsWorld::integrateRigidBodiesPositions()"); PROFILE("DynamicsWorld::int32_tegrateRigidBodiesPositions()");
// For each island of the world // For each island of the world
for (uint i=0; i < mNbIslands; i++) { for (uint32_t i=0; i < mNbIslands; i++) {
RigidBody** bodies = mIslands[i]->getBodies(); RigidBody** bodies = mIslands[i]->getBodies();
// For each body of the island // For each body of the island
for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { for (uint32_t b=0; b < mIslands[i]->getNbBodies(); b++) {
// Get the constrained velocity // Get the constrained velocity
uint indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; uint32_t indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray]; Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray]; Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
@ -206,7 +187,7 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * mTimeStep; mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * mTimeStep;
mConstrainedOrientations[indexArray] = currentOrientation + mConstrainedOrientations[indexArray] = currentOrientation +
Quaternion(0, newAngVelocity) * Quaternion(0, newAngVelocity) *
currentOrientation * decimal(0.5) * mTimeStep; currentOrientation * float(0.5) * mTimeStep;
} }
} }
} }
@ -217,14 +198,14 @@ void DynamicsWorld::updateBodiesState() {
PROFILE("DynamicsWorld::updateBodiesState()"); PROFILE("DynamicsWorld::updateBodiesState()");
// For each island of the world // For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) { for (uint32_t islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// For each body of the island // For each body of the island
RigidBody** bodies = mIslands[islandIndex]->getBodies(); RigidBody** bodies = mIslands[islandIndex]->getBodies();
for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) { for (uint32_t b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; uint32_t index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
// Update the linear and angular velocity of the body // Update the linear and angular velocity of the body
bodies[b]->mLinearVelocity = mConstrainedLinearVelocities[index]; bodies[b]->mLinearVelocity = mConstrainedLinearVelocities[index];
@ -249,7 +230,7 @@ void DynamicsWorld::updateBodiesState() {
void DynamicsWorld::initVelocityArrays() { void DynamicsWorld::initVelocityArrays() {
// Allocate memory for the bodies velocity arrays // Allocate memory for the bodies velocity arrays
uint nbBodies = mRigidBodies.size(); uint32_t nbBodies = mRigidBodies.size();
if (mNbBodiesCapacity != nbBodies && nbBodies > 0) { if (mNbBodiesCapacity != nbBodies && nbBodies > 0) {
if (mNbBodiesCapacity > 0) { if (mNbBodiesCapacity > 0) {
delete[] mSplitLinearVelocities; delete[] mSplitLinearVelocities;
@ -272,7 +253,7 @@ void DynamicsWorld::initVelocityArrays() {
} }
// Reset the velocities arrays // Reset the velocities arrays
for (uint i=0; i<mNbBodiesCapacity; i++) { for (uint32_t i=0; i<mNbBodiesCapacity; i++) {
mSplitLinearVelocities[i].setToZero(); mSplitLinearVelocities[i].setToZero();
mSplitAngularVelocities[i].setToZero(); mSplitAngularVelocities[i].setToZero();
} }
@ -280,10 +261,10 @@ void DynamicsWorld::initVelocityArrays() {
// Initialize the map of body indexes in the velocity arrays // Initialize the map of body indexes in the velocity arrays
mMapBodyToConstrainedVelocityIndex.clear(); mMapBodyToConstrainedVelocityIndex.clear();
std::set<RigidBody*>::const_iterator it; std::set<RigidBody*>::const_iterator it;
uint indexBody = 0; uint32_t indexBody = 0;
for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
// Add the body into the map // Add the body int32_to the map
mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(*it, indexBody)); mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(*it, indexBody));
indexBody++; indexBody++;
} }
@ -294,23 +275,23 @@ void DynamicsWorld::initVelocityArrays() {
/// the actual velocitiy of the bodies. The velocities updated in this method /// the actual velocitiy of the bodies. The velocities updated in this method
/// might violate the constraints and will be corrected in the constraint and /// might violate the constraints and will be corrected in the constraint and
/// contact solver. /// contact solver.
void DynamicsWorld::integrateRigidBodiesVelocities() { void DynamicsWorld::int32_tegrateRigidBodiesVelocities() {
PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()"); PROFILE("DynamicsWorld::int32_tegrateRigidBodiesVelocities()");
// Initialize the bodies velocity arrays // Initialize the bodies velocity arrays
initVelocityArrays(); initVelocityArrays();
// For each island of the world // For each island of the world
for (uint i=0; i < mNbIslands; i++) { for (uint32_t i=0; i < mNbIslands; i++) {
RigidBody** bodies = mIslands[i]->getBodies(); RigidBody** bodies = mIslands[i]->getBodies();
// For each body of the island // For each body of the island
for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { for (uint32_t b=0; b < mIslands[i]->getNbBodies(); b++) {
// Insert the body into the map of constrained velocities // Insert the body int32_to the map of constrained velocities
uint indexBody = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; uint32_t indexBody = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0)); assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0));
assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0)); assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0));
@ -343,10 +324,10 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
// Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ... // Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ...
// => e^(-x) ~ 1 - x // => e^(-x) ~ 1 - x
// => v2 = v1 * (1 - c * dt) // => v2 = v1 * (1 - c * dt)
decimal linDampingFactor = bodies[b]->getLinearDamping(); float linDampingFactor = bodies[b]->getLinearDamping();
decimal angDampingFactor = bodies[b]->getAngularDamping(); float angDampingFactor = bodies[b]->getAngularDamping();
decimal linearDamping = pow(decimal(1.0) - linDampingFactor, mTimeStep); float linearDamping = pow(float(1.0) - linDampingFactor, mTimeStep);
decimal angularDamping = pow(decimal(1.0) - angDampingFactor, mTimeStep); float angularDamping = pow(float(1.0) - angDampingFactor, mTimeStep);
mConstrainedLinearVelocities[indexBody] *= linearDamping; mConstrainedLinearVelocities[indexBody] *= linearDamping;
mConstrainedAngularVelocities[indexBody] *= angularDamping; mConstrainedAngularVelocities[indexBody] *= angularDamping;
@ -372,7 +353,7 @@ void DynamicsWorld::solveContactsAndConstraints() {
// ---------- Solve velocity constraints for joints and contacts ---------- // // ---------- Solve velocity constraints for joints and contacts ---------- //
// For each island of the world // For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) { for (uint32_t islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// Check if there are contacts and constraints to solve // Check if there are contacts and constraints to solve
bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0; bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
@ -397,7 +378,7 @@ void DynamicsWorld::solveContactsAndConstraints() {
} }
// For each iteration of the velocity solver // For each iteration of the velocity solver
for (uint i=0; i<mNbVelocitySolverIterations; i++) { for (uint32_t i=0; i<mNbVelocitySolverIterations; i++) {
// Solve the constraints // Solve the constraints
if (isConstraintsToSolve) { if (isConstraintsToSolve) {
@ -426,12 +407,12 @@ void DynamicsWorld::solvePositionCorrection() {
if (mJoints.empty()) return; if (mJoints.empty()) return;
// For each island of the world // For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) { for (uint32_t islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// ---------- Solve the position error correction for the constraints ---------- // // ---------- Solve the position error correction for the constraints ---------- //
// For each iteration of the position (error correction) solver // For each iteration of the position (error correction) solver
for (uint i=0; i<mNbPositionSolverIterations; i++) { for (uint32_t i=0; i<mNbPositionSolverIterations; i++) {
// Solve the position constraints // Solve the position constraints
mConstraintSolver.solvePositionConstraints(mIslands[islandIndex]); mConstraintSolver.solvePositionConstraints(mIslands[islandIndex]);
@ -439,7 +420,7 @@ void DynamicsWorld::solvePositionCorrection() {
} }
} }
// Create a rigid body into the physics world // Create a rigid body int32_to the physics world
/** /**
* @param transform Transformation from body local-space to world-space * @param transform Transformation from body local-space to world-space
* @return A pointer to the body that has been created in the world * @return A pointer to the body that has been created in the world
@ -560,10 +541,10 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
mCollisionDetection.addNoCollisionPair(jointInfo.body1, jointInfo.body2); mCollisionDetection.addNoCollisionPair(jointInfo.body1, jointInfo.body2);
} }
// Add the joint into the world // Add the joint int32_to the world
mJoints.insert(newJoint); mJoints.insert(newJoint);
// Add the joint into the joint list of the bodies involved in the joint // Add the joint int32_to the joint list of the bodies involved in the joint
addJointToBody(newJoint); addJointToBody(newJoint);
// Return the pointer to the created joint // Return the pointer to the created joint
@ -634,10 +615,10 @@ void DynamicsWorld::computeIslands() {
PROFILE("DynamicsWorld::computeIslands()"); PROFILE("DynamicsWorld::computeIslands()");
uint nbBodies = mRigidBodies.size(); uint32_t nbBodies = mRigidBodies.size();
// Clear all the islands // Clear all the islands
for (uint i=0; i<mNbIslands; i++) { for (uint32_t i=0; i<mNbIslands; i++) {
// Call the island destructor // Call the island destructor
mIslands[i]->~Island(); mIslands[i]->~Island();
@ -656,11 +637,11 @@ void DynamicsWorld::computeIslands() {
} }
mNbIslands = 0; mNbIslands = 0;
int nbContactManifolds = 0; int32_t nbContactManifolds = 0;
// Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds // Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
int nbBodyManifolds = (*it)->resetIsAlreadyInIslandAndCountManifolds(); int32_t nbBodyManifolds = (*it)->resetIsAlreadyInIslandAndCountManifolds();
nbContactManifolds += nbBodyManifolds; nbContactManifolds += nbBodyManifolds;
} }
for (std::set<Joint*>::iterator it = mJoints.begin(); it != mJoints.end(); ++it) { for (std::set<Joint*>::iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
@ -686,7 +667,7 @@ void DynamicsWorld::computeIslands() {
if (body->isSleeping() || !body->isActive()) continue; if (body->isSleeping() || !body->isActive()) continue;
// Reset the stack of bodies to visit // Reset the stack of bodies to visit
uint stackIndex = 0; uint32_t stackIndex = 0;
stackBodiesToVisit[stackIndex] = body; stackBodiesToVisit[stackIndex] = body;
stackIndex++; stackIndex++;
body->mIsAlreadyInIsland = true; body->mIsAlreadyInIsland = true;
@ -708,7 +689,7 @@ void DynamicsWorld::computeIslands() {
// Awake the body if it is slepping // Awake the body if it is slepping
bodyToVisit->setIsSleeping(false); bodyToVisit->setIsSleeping(false);
// Add the body into the island // Add the body int32_to the island
mIslands[mNbIslands]->addBody(bodyToVisit); mIslands[mNbIslands]->addBody(bodyToVisit);
// If the current body is static, we do not want to perform the DFS // If the current body is static, we do not want to perform the DFS
@ -724,10 +705,10 @@ void DynamicsWorld::computeIslands() {
assert(contactManifold->getNbContactPoints() > 0); assert(contactManifold->getNbContactPoints() > 0);
// Check if the current contact manifold has already been added into an island // Check if the current contact manifold has already been added int32_to an island
if (contactManifold->isAlreadyInIsland()) continue; if (contactManifold->isAlreadyInIsland()) continue;
// Add the contact manifold into the island // Add the contact manifold int32_to the island
mIslands[mNbIslands]->addContactManifold(contactManifold); mIslands[mNbIslands]->addContactManifold(contactManifold);
contactManifold->mIsAlreadyInIsland = true; contactManifold->mIsAlreadyInIsland = true;
@ -739,7 +720,7 @@ void DynamicsWorld::computeIslands() {
// Check if the other body has already been added to the island // Check if the other body has already been added to the island
if (otherBody->mIsAlreadyInIsland) continue; if (otherBody->mIsAlreadyInIsland) continue;
// Insert the other body into the stack of bodies to visit // Insert the other body int32_to the stack of bodies to visit
stackBodiesToVisit[stackIndex] = otherBody; stackBodiesToVisit[stackIndex] = otherBody;
stackIndex++; stackIndex++;
otherBody->mIsAlreadyInIsland = true; otherBody->mIsAlreadyInIsland = true;
@ -752,10 +733,10 @@ void DynamicsWorld::computeIslands() {
Joint* joint = jointElement->joint; Joint* joint = jointElement->joint;
// Check if the current joint has already been added into an island // Check if the current joint has already been added int32_to an island
if (joint->isAlreadyInIsland()) continue; if (joint->isAlreadyInIsland()) continue;
// Add the joint into the island // Add the joint int32_to the island
mIslands[mNbIslands]->addJoint(joint); mIslands[mNbIslands]->addJoint(joint);
joint->mIsAlreadyInIsland = true; joint->mIsAlreadyInIsland = true;
@ -767,7 +748,7 @@ void DynamicsWorld::computeIslands() {
// Check if the other body has already been added to the island // Check if the other body has already been added to the island
if (otherBody->mIsAlreadyInIsland) continue; if (otherBody->mIsAlreadyInIsland) continue;
// Insert the other body into the stack of bodies to visit // Insert the other body int32_to the stack of bodies to visit
stackBodiesToVisit[stackIndex] = otherBody; stackBodiesToVisit[stackIndex] = otherBody;
stackIndex++; stackIndex++;
otherBody->mIsAlreadyInIsland = true; otherBody->mIsAlreadyInIsland = true;
@ -776,7 +757,7 @@ void DynamicsWorld::computeIslands() {
// Reset the isAlreadyIsland variable of the static bodies so that they // Reset the isAlreadyIsland variable of the static bodies so that they
// can also be included in the other islands // can also be included in the other islands
for (uint i=0; i < mIslands[mNbIslands]->mNbBodies; i++) { for (uint32_t i=0; i < mIslands[mNbIslands]->mNbBodies; i++) {
if (mIslands[mNbIslands]->mBodies[i]->getType() == STATIC) { if (mIslands[mNbIslands]->mBodies[i]->getType() == STATIC) {
mIslands[mNbIslands]->mBodies[i]->mIsAlreadyInIsland = false; mIslands[mNbIslands]->mBodies[i]->mIsAlreadyInIsland = false;
@ -797,17 +778,17 @@ void DynamicsWorld::updateSleepingBodies() {
PROFILE("DynamicsWorld::updateSleepingBodies()"); PROFILE("DynamicsWorld::updateSleepingBodies()");
const decimal sleepLinearVelocitySquare = mSleepLinearVelocity * mSleepLinearVelocity; const float sleepLinearVelocitySquare = mSleepLinearVelocity * mSleepLinearVelocity;
const decimal sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity; const float sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity;
// For each island of the world // For each island of the world
for (uint i=0; i<mNbIslands; i++) { for (uint32_t i=0; i<mNbIslands; i++) {
decimal minSleepTime = DECIMAL_LARGEST; float minSleepTime = DECIMAL_LARGEST;
// For each body of the island // For each body of the island
RigidBody** bodies = mIslands[i]->getBodies(); RigidBody** bodies = mIslands[i]->getBodies();
for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { for (uint32_t b=0; b < mIslands[i]->getNbBodies(); b++) {
// Skip static bodies // Skip static bodies
if (bodies[b]->getType() == STATIC) continue; if (bodies[b]->getType() == STATIC) continue;
@ -818,8 +799,8 @@ void DynamicsWorld::updateSleepingBodies() {
!bodies[b]->isAllowedToSleep()) { !bodies[b]->isAllowedToSleep()) {
// Reset the sleep time of the body // Reset the sleep time of the body
bodies[b]->mSleepTime = decimal(0.0); bodies[b]->mSleepTime = float(0.0);
minSleepTime = decimal(0.0); minSleepTime = float(0.0);
} }
else { // If the body velocity is bellow the sleeping velocity threshold else { // If the body velocity is bellow the sleeping velocity threshold
@ -837,7 +818,7 @@ void DynamicsWorld::updateSleepingBodies() {
if (minSleepTime >= mTimeBeforeSleep) { if (minSleepTime >= mTimeBeforeSleep) {
// Put all the bodies of the island to sleep // Put all the bodies of the island to sleep
for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { for (uint32_t b=0; b < mIslands[i]->getNbBodies(); b++) {
bodies[b]->setIsSleeping(true); bodies[b]->setIsSleeping(true);
} }
} }
@ -845,7 +826,7 @@ void DynamicsWorld::updateSleepingBodies() {
} }
// Enable/Disable the sleeping technique. // Enable/Disable the sleeping technique.
/// The sleeping technique is used to put bodies that are not moving into sleep /// The sleeping technique is used to put bodies that are not moving int32_to sleep
/// to speed up the simulation. /// to speed up the simulation.
/** /**
* @param isSleepingEnabled True if you want to enable the sleeping technique * @param isSleepingEnabled True if you want to enable the sleeping technique
@ -878,9 +859,9 @@ void DynamicsWorld::testCollision(const ProxyShape* shape,
CollisionCallback* callback) { CollisionCallback* callback) {
// Create the sets of shapes // Create the sets of shapes
std::set<uint> shapes; std::set<uint32_t> shapes;
shapes.insert(shape->mBroadPhaseID); shapes.insert(shape->mBroadPhaseID);
std::set<uint> emptySet; std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet); mCollisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet);
@ -899,9 +880,9 @@ void DynamicsWorld::testCollision(const ProxyShape* shape1,
CollisionCallback* callback) { CollisionCallback* callback) {
// Create the sets of shapes // Create the sets of shapes
std::set<uint> shapes1; std::set<uint32_t> shapes1;
shapes1.insert(shape1->mBroadPhaseID); shapes1.insert(shape1->mBroadPhaseID);
std::set<uint> shapes2; std::set<uint32_t> shapes2;
shapes2.insert(shape2->mBroadPhaseID); shapes2.insert(shape2->mBroadPhaseID);
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
@ -920,7 +901,7 @@ void DynamicsWorld::testCollision(const CollisionBody* body,
CollisionCallback* callback) { CollisionCallback* callback) {
// Create the sets of shapes // Create the sets of shapes
std::set<uint> shapes1; std::set<uint32_t> shapes1;
// For each shape of the body // For each shape of the body
for (const ProxyShape* shape=body->getProxyShapesList(); shape != NULL; for (const ProxyShape* shape=body->getProxyShapesList(); shape != NULL;
@ -928,7 +909,7 @@ void DynamicsWorld::testCollision(const CollisionBody* body,
shapes1.insert(shape->mBroadPhaseID); shapes1.insert(shape->mBroadPhaseID);
} }
std::set<uint> emptySet; std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet); mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet);
@ -947,13 +928,13 @@ void DynamicsWorld::testCollision(const CollisionBody* body1,
CollisionCallback* callback) { CollisionCallback* callback) {
// Create the sets of shapes // Create the sets of shapes
std::set<uint> shapes1; std::set<uint32_t> shapes1;
for (const ProxyShape* shape=body1->getProxyShapesList(); shape != NULL; for (const ProxyShape* shape=body1->getProxyShapesList(); shape != NULL;
shape = shape->getNext()) { shape = shape->getNext()) {
shapes1.insert(shape->mBroadPhaseID); shapes1.insert(shape->mBroadPhaseID);
} }
std::set<uint> shapes2; std::set<uint32_t> shapes2;
for (const ProxyShape* shape=body2->getProxyShapesList(); shape != NULL; for (const ProxyShape* shape=body2->getProxyShapesList(); shape != NULL;
shape = shape->getNext()) { shape = shape->getNext()) {
shapes2.insert(shape->mBroadPhaseID); shapes2.insert(shape->mBroadPhaseID);
@ -971,7 +952,7 @@ void DynamicsWorld::testCollision(const CollisionBody* body1,
*/ */
void DynamicsWorld::testCollision(CollisionCallback* callback) { void DynamicsWorld::testCollision(CollisionCallback* callback) {
std::set<uint> emptySet; std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet); mCollisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet);
@ -991,7 +972,7 @@ std::vector<const ContactManifold*> DynamicsWorld::getContactsList() const {
// For each contact manifold of the pair // For each contact manifold of the pair
const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
for (int i=0; i<manifoldSet.getNbContactManifolds(); i++) { for (int32_t i=0; i<manifoldSet.getNbContactManifolds(); i++) {
ContactManifold* manifold = manifoldSet.getContactManifold(i); ContactManifold* manifold = manifoldSet.getContactManifold(i);

118
ephysics/engine/DynamicsWorld.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_DYNAMICS_WORLD_H
#define REACTPHYSICS3D_DYNAMICS_WORLD_H
// Libraries // Libraries
#include <ephysics/engine/CollisionWorld.h> #include <ephysics/engine/CollisionWorld.h>
@ -57,10 +36,10 @@ class DynamicsWorld : public CollisionWorld {
ConstraintSolver mConstraintSolver; ConstraintSolver mConstraintSolver;
/// Number of iterations for the velocity solver of the Sequential Impulses technique /// Number of iterations for the velocity solver of the Sequential Impulses technique
uint mNbVelocitySolverIterations; uint32_t mNbVelocitySolverIterations;
/// Number of iterations for the position solver of the Sequential Impulses technique /// Number of iterations for the position solver of the Sequential Impulses technique
uint mNbPositionSolverIterations; uint32_t mNbPositionSolverIterations;
/// True if the spleeping technique for inactive bodies is enabled /// True if the spleeping technique for inactive bodies is enabled
bool mIsSleepingEnabled; bool mIsSleepingEnabled;
@ -75,7 +54,7 @@ class DynamicsWorld : public CollisionWorld {
Vector3 mGravity; Vector3 mGravity;
/// Current frame time step (in seconds) /// Current frame time step (in seconds)
decimal mTimeStep; float mTimeStep;
/// True if the gravity force is on /// True if the gravity force is on
bool mIsGravityEnabled; bool mIsGravityEnabled;
@ -101,29 +80,29 @@ class DynamicsWorld : public CollisionWorld {
Quaternion* mConstrainedOrientations; Quaternion* mConstrainedOrientations;
/// Map body to their index in the constrained velocities array /// Map body to their index in the constrained velocities array
std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex; std::map<RigidBody*, uint32_t> mMapBodyToConstrainedVelocityIndex;
/// Number of islands in the world /// Number of islands in the world
uint mNbIslands; uint32_t mNbIslands;
/// Current allocated capacity for the islands /// Current allocated capacity for the islands
uint mNbIslandsCapacity; uint32_t mNbIslandsCapacity;
/// Array with all the islands of awaken bodies /// Array with all the islands of awaken bodies
Island** mIslands; Island** mIslands;
/// Current allocated capacity for the bodies /// Current allocated capacity for the bodies
uint mNbBodiesCapacity; uint32_t mNbBodiesCapacity;
/// Sleep linear velocity threshold /// Sleep linear velocity threshold
decimal mSleepLinearVelocity; float mSleepLinearVelocity;
/// Sleep angular velocity threshold /// Sleep angular velocity threshold
decimal mSleepAngularVelocity; float mSleepAngularVelocity;
/// Time (in seconds) before a body is put to sleep if its velocity /// Time (in seconds) before a body is put to sleep if its velocity
/// becomes smaller than the sleep velocity. /// becomes smaller than the sleep velocity.
decimal mTimeBeforeSleep; float mTimeBeforeSleep;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -134,7 +113,7 @@ class DynamicsWorld : public CollisionWorld {
DynamicsWorld& operator=(const DynamicsWorld& world); DynamicsWorld& operator=(const DynamicsWorld& world);
/// Integrate the positions and orientations of rigid bodies. /// Integrate the positions and orientations of rigid bodies.
void integrateRigidBodiesPositions(); void int32_tegrateRigidBodiesPositions();
/// Update the AABBs of the bodies /// Update the AABBs of the bodies
void updateRigidBodiesAABB(); void updateRigidBodiesAABB();
@ -146,14 +125,14 @@ class DynamicsWorld : public CollisionWorld {
void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity, void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
Vector3 newAngVelocity); Vector3 newAngVelocity);
/// Compute and set the interpolation factor to all bodies /// Compute and set the int32_terpolation factor to all bodies
void setInterpolationFactorToAllBodies(); void setInterpolationFactorToAllBodies();
/// Initialize the bodies velocities arrays for the next simulation step. /// Initialize the bodies velocities arrays for the next simulation step.
void initVelocityArrays(); void initVelocityArrays();
/// Integrate the velocities of rigid bodies. /// Integrate the velocities of rigid bodies.
void integrateRigidBodiesVelocities(); void int32_tegrateRigidBodiesVelocities();
/// Solve the contacts and constraints /// Solve the contacts and constraints
void solveContactsAndConstraints(); void solveContactsAndConstraints();
@ -187,19 +166,19 @@ class DynamicsWorld : public CollisionWorld {
virtual ~DynamicsWorld(); virtual ~DynamicsWorld();
/// Update the physics simulation /// Update the physics simulation
void update(decimal timeStep); void update(float timeStep);
/// Get the number of iterations for the velocity constraint solver /// Get the number of iterations for the velocity constraint solver
uint getNbIterationsVelocitySolver() const; uint32_t getNbIterationsVelocitySolver() const;
/// Set the number of iterations for the velocity constraint solver /// Set the number of iterations for the velocity constraint solver
void setNbIterationsVelocitySolver(uint nbIterations); void setNbIterationsVelocitySolver(uint32_t nbIterations);
/// Get the number of iterations for the position constraint solver /// Get the number of iterations for the position constraint solver
uint getNbIterationsPositionSolver() const; uint32_t getNbIterationsPositionSolver() const;
/// Set the number of iterations for the position constraint solver /// Set the number of iterations for the position constraint solver
void setNbIterationsPositionSolver(uint nbIterations); void setNbIterationsPositionSolver(uint32_t nbIterations);
/// Set the position correction technique used for contacts /// Set the position correction technique used for contacts
void setContactsPositionCorrectionTechnique(ContactsPositionCorrectionTechnique technique); void setContactsPositionCorrectionTechnique(ContactsPositionCorrectionTechnique technique);
@ -211,7 +190,7 @@ class DynamicsWorld : public CollisionWorld {
/// the contact manifold instead of solving them at each contact point /// the contact manifold instead of solving them at each contact point
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
/// Create a rigid body into the physics world. /// Create a rigid body int32_to the physics world.
RigidBody* createRigidBody(const Transform& transform); RigidBody* createRigidBody(const Transform& transform);
/// Destroy a rigid body and all the joints which it belongs /// Destroy a rigid body and all the joints which it belongs
@ -236,10 +215,10 @@ class DynamicsWorld : public CollisionWorld {
void setIsGratityEnabled(bool isGravityEnabled); void setIsGratityEnabled(bool isGravityEnabled);
/// Return the number of rigid bodies in the world /// Return the number of rigid bodies in the world
uint getNbRigidBodies() const; uint32_t getNbRigidBodies() const;
/// Return the number of joints in the world /// Return the number of joints in the world
uint getNbJoints() const; uint32_t getNbJoints() const;
/// Return an iterator to the beginning of the rigid bodies of the physics world /// Return an iterator to the beginning of the rigid bodies of the physics world
std::set<RigidBody*>::iterator getRigidBodiesBeginIterator(); std::set<RigidBody*>::iterator getRigidBodiesBeginIterator();
@ -254,22 +233,22 @@ class DynamicsWorld : public CollisionWorld {
void enableSleeping(bool isSleepingEnabled); void enableSleeping(bool isSleepingEnabled);
/// Return the current sleep linear velocity /// Return the current sleep linear velocity
decimal getSleepLinearVelocity() const; float getSleepLinearVelocity() const;
/// Set the sleep linear velocity. /// Set the sleep linear velocity.
void setSleepLinearVelocity(decimal sleepLinearVelocity); void setSleepLinearVelocity(float sleepLinearVelocity);
/// Return the current sleep angular velocity /// Return the current sleep angular velocity
decimal getSleepAngularVelocity() const; float getSleepAngularVelocity() const;
/// Set the sleep angular velocity. /// Set the sleep angular velocity.
void setSleepAngularVelocity(decimal sleepAngularVelocity); void setSleepAngularVelocity(float sleepAngularVelocity);
/// Return the time a body is required to stay still before sleeping /// Return the time a body is required to stay still before sleeping
decimal getTimeBeforeSleep() const; float getTimeBeforeSleep() const;
/// Set the time a body is required to stay still before sleeping /// Set the time a body is required to stay still before sleeping
void setTimeBeforeSleep(decimal timeBeforeSleep); void setTimeBeforeSleep(float timeBeforeSleep);
/// Set an event listener object to receive events callbacks. /// Set an event listener object to receive events callbacks.
void setEventListener(EventListener* eventListener); void setEventListener(EventListener* eventListener);
@ -317,7 +296,7 @@ inline void DynamicsWorld::resetBodiesForceAndTorque() {
} }
// Get the number of iterations for the velocity constraint solver // Get the number of iterations for the velocity constraint solver
inline uint DynamicsWorld::getNbIterationsVelocitySolver() const { inline uint32_t DynamicsWorld::getNbIterationsVelocitySolver() const {
return mNbVelocitySolverIterations; return mNbVelocitySolverIterations;
} }
@ -325,12 +304,12 @@ inline uint DynamicsWorld::getNbIterationsVelocitySolver() const {
/** /**
* @param nbIterations Number of iterations for the velocity solver * @param nbIterations Number of iterations for the velocity solver
*/ */
inline void DynamicsWorld::setNbIterationsVelocitySolver(uint nbIterations) { inline void DynamicsWorld::setNbIterationsVelocitySolver(uint32_t nbIterations) {
mNbVelocitySolverIterations = nbIterations; mNbVelocitySolverIterations = nbIterations;
} }
// Get the number of iterations for the position constraint solver // Get the number of iterations for the position constraint solver
inline uint DynamicsWorld::getNbIterationsPositionSolver() const { inline uint32_t DynamicsWorld::getNbIterationsPositionSolver() const {
return mNbPositionSolverIterations; return mNbPositionSolverIterations;
} }
@ -338,7 +317,7 @@ inline uint DynamicsWorld::getNbIterationsPositionSolver() const {
/** /**
* @param nbIterations Number of iterations for the position solver * @param nbIterations Number of iterations for the position solver
*/ */
inline void DynamicsWorld::setNbIterationsPositionSolver(uint nbIterations) { inline void DynamicsWorld::setNbIterationsPositionSolver(uint32_t nbIterations) {
mNbPositionSolverIterations = nbIterations; mNbPositionSolverIterations = nbIterations;
} }
@ -417,7 +396,7 @@ inline void DynamicsWorld::setIsGratityEnabled(bool isGravityEnabled) {
/** /**
* @return Number of rigid bodies in the world * @return Number of rigid bodies in the world
*/ */
inline uint DynamicsWorld::getNbRigidBodies() const { inline uint32_t DynamicsWorld::getNbRigidBodies() const {
return mRigidBodies.size(); return mRigidBodies.size();
} }
@ -425,7 +404,7 @@ inline uint DynamicsWorld::getNbRigidBodies() const {
/** /**
* @return Number of joints in the world * @return Number of joints in the world
*/ */
inline uint DynamicsWorld::getNbJoints() const { inline uint32_t DynamicsWorld::getNbJoints() const {
return mJoints.size(); return mJoints.size();
} }
@ -457,7 +436,7 @@ inline bool DynamicsWorld::isSleepingEnabled() const {
/** /**
* @return The sleep linear velocity (in meters per second) * @return The sleep linear velocity (in meters per second)
*/ */
inline decimal DynamicsWorld::getSleepLinearVelocity() const { inline float DynamicsWorld::getSleepLinearVelocity() const {
return mSleepLinearVelocity; return mSleepLinearVelocity;
} }
@ -468,8 +447,8 @@ inline decimal DynamicsWorld::getSleepLinearVelocity() const {
/** /**
* @param sleepLinearVelocity The sleep linear velocity (in meters per second) * @param sleepLinearVelocity The sleep linear velocity (in meters per second)
*/ */
inline void DynamicsWorld::setSleepLinearVelocity(decimal sleepLinearVelocity) { inline void DynamicsWorld::setSleepLinearVelocity(float sleepLinearVelocity) {
assert(sleepLinearVelocity >= decimal(0.0)); assert(sleepLinearVelocity >= float(0.0));
mSleepLinearVelocity = sleepLinearVelocity; mSleepLinearVelocity = sleepLinearVelocity;
} }
@ -477,7 +456,7 @@ inline void DynamicsWorld::setSleepLinearVelocity(decimal sleepLinearVelocity) {
/** /**
* @return The sleep angular velocity (in radian per second) * @return The sleep angular velocity (in radian per second)
*/ */
inline decimal DynamicsWorld::getSleepAngularVelocity() const { inline float DynamicsWorld::getSleepAngularVelocity() const {
return mSleepAngularVelocity; return mSleepAngularVelocity;
} }
@ -488,8 +467,8 @@ inline decimal DynamicsWorld::getSleepAngularVelocity() const {
/** /**
* @param sleepAngularVelocity The sleep angular velocity (in radian per second) * @param sleepAngularVelocity The sleep angular velocity (in radian per second)
*/ */
inline void DynamicsWorld::setSleepAngularVelocity(decimal sleepAngularVelocity) { inline void DynamicsWorld::setSleepAngularVelocity(float sleepAngularVelocity) {
assert(sleepAngularVelocity >= decimal(0.0)); assert(sleepAngularVelocity >= float(0.0));
mSleepAngularVelocity = sleepAngularVelocity; mSleepAngularVelocity = sleepAngularVelocity;
} }
@ -497,7 +476,7 @@ inline void DynamicsWorld::setSleepAngularVelocity(decimal sleepAngularVelocity)
/** /**
* @return Time a body is required to stay still before sleeping (in seconds) * @return Time a body is required to stay still before sleeping (in seconds)
*/ */
inline decimal DynamicsWorld::getTimeBeforeSleep() const { inline float DynamicsWorld::getTimeBeforeSleep() const {
return mTimeBeforeSleep; return mTimeBeforeSleep;
} }
@ -506,8 +485,8 @@ inline decimal DynamicsWorld::getTimeBeforeSleep() const {
/** /**
* @param timeBeforeSleep Time a body is required to stay still before sleeping (in seconds) * @param timeBeforeSleep Time a body is required to stay still before sleeping (in seconds)
*/ */
inline void DynamicsWorld::setTimeBeforeSleep(decimal timeBeforeSleep) { inline void DynamicsWorld::setTimeBeforeSleep(float timeBeforeSleep) {
assert(timeBeforeSleep >= decimal(0.0)); assert(timeBeforeSleep >= float(0.0));
mTimeBeforeSleep = timeBeforeSleep; mTimeBeforeSleep = timeBeforeSleep;
} }
@ -524,4 +503,3 @@ inline void DynamicsWorld::setEventListener(EventListener* eventListener) {
} }
#endif

47
ephysics/engine/EventListener.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_EVENT_LISTENER_H
#define REACTPHYSICS3D_EVENT_LISTENER_H
// Libraries // Libraries
#include <ephysics/constraint/ContactPoint.h> #include <ephysics/constraint/ContactPoint.h>
@ -61,19 +40,17 @@ class EventListener {
*/ */
virtual void newContact(const ContactPointInfo& contact) {} virtual void newContact(const ContactPointInfo& contact) {}
/// Called at the beginning of an internal tick of the simulation step. /// Called at the beginning of an int32_ternal tick of the simulation step.
/// Each time the DynamicsWorld::update() method is called, the physics /// Each time the DynamicsWorld::update() method is called, the physics
/// engine will do several internal simulation steps. This method is /// engine will do several int32_ternal simulation steps. This method is
/// called at the beginning of each internal simulation step. /// called at the beginning of each int32_ternal simulation step.
virtual void beginInternalTick() {} virtual void beginInternalTick() {}
/// Called at the end of an internal tick of the simulation step. /// Called at the end of an int32_ternal tick of the simulation step.
/// Each time the DynamicsWorld::update() metho is called, the physics /// Each time the DynamicsWorld::update() metho is called, the physics
/// engine will do several internal simulation steps. This method is /// engine will do several int32_ternal simulation steps. This method is
/// called at the end of each internal simulation step. /// called at the end of each int32_ternal simulation step.
virtual void endInternalTick() {} virtual void endInternalTick() {}
}; };
} }
#endif

35
ephysics/engine/Impulse.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_IMPULSE_H
#define REACTPHYSICS3D_IMPULSE_H
// Libraries // Libraries
#include <ephysics/mathematics/mathematics.h> #include <ephysics/mathematics/mathematics.h>
@ -83,5 +62,3 @@ struct Impulse {
}; };
} }
#endif

31
ephysics/engine/Island.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/engine/Island.h> #include <ephysics/engine/Island.h>
@ -29,7 +10,7 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, Island::Island(uint32_t nbMaxBodies, uint32_t nbMaxContactManifolds, uint32_t nbMaxJoints,
MemoryAllocator& memoryAllocator) MemoryAllocator& memoryAllocator)
: mBodies(NULL), mContactManifolds(NULL), mJoints(NULL), mNbBodies(0), : mBodies(NULL), mContactManifolds(NULL), mJoints(NULL), mNbBodies(0),
mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) { mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) {

67
ephysics/engine/Island.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_ISLAND_H
#define REACTPHYSICS3D_ISLAND_H
// Libraries // Libraries
#include <ephysics/memory/MemoryAllocator.h> #include <ephysics/memory/MemoryAllocator.h>
@ -55,13 +34,13 @@ class Island {
Joint** mJoints; Joint** mJoints;
/// Current number of bodies in the island /// Current number of bodies in the island
uint mNbBodies; uint32_t mNbBodies;
/// Current number of contact manifold in the island /// Current number of contact manifold in the island
uint mNbContactManifolds; uint32_t mNbContactManifolds;
/// Current number of joints in the island /// Current number of joints in the island
uint mNbJoints; uint32_t mNbJoints;
/// Reference to the memory allocator /// Reference to the memory allocator
MemoryAllocator& mMemoryAllocator; MemoryAllocator& mMemoryAllocator;
@ -88,29 +67,29 @@ class Island {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, Island(uint32_t nbMaxBodies, uint32_t nbMaxContactManifolds, uint32_t nbMaxJoints,
MemoryAllocator& memoryAllocator); MemoryAllocator& memoryAllocator);
/// Destructor /// Destructor
~Island(); ~Island();
/// Add a body into the island /// Add a body int32_to the island
void addBody(RigidBody* body); void addBody(RigidBody* body);
/// Add a contact manifold into the island /// Add a contact manifold int32_to the island
void addContactManifold(ContactManifold* contactManifold); void addContactManifold(ContactManifold* contactManifold);
/// Add a joint into the island /// Add a joint int32_to the island
void addJoint(Joint* joint); void addJoint(Joint* joint);
/// Return the number of bodies in the island /// Return the number of bodies in the island
uint getNbBodies() const; uint32_t getNbBodies() const;
/// Return the number of contact manifolds in the island /// Return the number of contact manifolds in the island
uint getNbContactManifolds() const; uint32_t getNbContactManifolds() const;
/// Return the number of joints in the island /// Return the number of joints in the island
uint getNbJoints() const; uint32_t getNbJoints() const;
/// Return a pointer to the array of bodies /// Return a pointer to the array of bodies
RigidBody** getBodies(); RigidBody** getBodies();
@ -126,37 +105,37 @@ class Island {
friend class DynamicsWorld; friend class DynamicsWorld;
}; };
// Add a body into the island // Add a body int32_to the island
inline void Island::addBody(RigidBody* body) { inline void Island::addBody(RigidBody* body) {
assert(!body->isSleeping()); assert(!body->isSleeping());
mBodies[mNbBodies] = body; mBodies[mNbBodies] = body;
mNbBodies++; mNbBodies++;
} }
// Add a contact manifold into the island // Add a contact manifold int32_to the island
inline void Island::addContactManifold(ContactManifold* contactManifold) { inline void Island::addContactManifold(ContactManifold* contactManifold) {
mContactManifolds[mNbContactManifolds] = contactManifold; mContactManifolds[mNbContactManifolds] = contactManifold;
mNbContactManifolds++; mNbContactManifolds++;
} }
// Add a joint into the island // Add a joint int32_to the island
inline void Island::addJoint(Joint* joint) { inline void Island::addJoint(Joint* joint) {
mJoints[mNbJoints] = joint; mJoints[mNbJoints] = joint;
mNbJoints++; mNbJoints++;
} }
// Return the number of bodies in the island // Return the number of bodies in the island
inline uint Island::getNbBodies() const { inline uint32_t Island::getNbBodies() const {
return mNbBodies; return mNbBodies;
} }
// Return the number of contact manifolds in the island // Return the number of contact manifolds in the island
inline uint Island::getNbContactManifolds() const { inline uint32_t Island::getNbContactManifolds() const {
return mNbContactManifolds; return mNbContactManifolds;
} }
// Return the number of joints in the island // Return the number of joints in the island
inline uint Island::getNbJoints() const { inline uint32_t Island::getNbJoints() const {
return mNbJoints; return mNbJoints;
} }
@ -176,5 +155,3 @@ inline Joint** Island::getJoints() {
} }
} }
#endif

29
ephysics/engine/Material.cpp
View File

@ -1,27 +1,8 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries // Libraries
#include <ephysics/engine/Material.h> #include <ephysics/engine/Material.h>

69
ephysics/engine/Material.h
View File

@ -1,30 +1,9 @@
/******************************************************************************** /** @file
* ReactPhysics3D physics library, http://www.reactphysics3d.com * * @author Daniel Chappuis
* Copyright (c) 2010-2016 Daniel Chappuis * * @copyright 2010-2016 Daniel Chappuis
********************************************************************************* * @license BSD 3 clauses (see license file)
* * */
* This software is provided 'as-is', without any express or implied warranty. * #pragma once
* 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_MATERIAL_H
#define REACTPHYSICS3D_MATERIAL_H
// Libraries // Libraries
#include <cassert> #include <cassert>
@ -45,13 +24,13 @@ class Material {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Friction coefficient (positive value) /// Friction coefficient (positive value)
decimal mFrictionCoefficient; float mFrictionCoefficient;
/// Rolling resistance factor (positive value) /// Rolling resistance factor (positive value)
decimal mRollingResistance; float mRollingResistance;
/// Bounciness during collisions (between 0 and 1) where 1 is for a very bouncy body /// Bounciness during collisions (between 0 and 1) where 1 is for a very bouncy body
decimal mBounciness; float mBounciness;
public : public :
@ -67,22 +46,22 @@ class Material {
~Material(); ~Material();
/// Return the bounciness /// Return the bounciness
decimal getBounciness() const; float getBounciness() const;
/// Set the bounciness. /// Set the bounciness.
void setBounciness(decimal bounciness); void setBounciness(float bounciness);
/// Return the friction coefficient /// Return the friction coefficient
decimal getFrictionCoefficient() const; float getFrictionCoefficient() const;
/// Set the friction coefficient. /// Set the friction coefficient.
void setFrictionCoefficient(decimal frictionCoefficient); void setFrictionCoefficient(float frictionCoefficient);
/// Return the rolling resistance factor /// Return the rolling resistance factor
decimal getRollingResistance() const; float getRollingResistance() const;
/// Set the rolling resistance factor /// Set the rolling resistance factor
void setRollingResistance(decimal rollingResistance); void setRollingResistance(float rollingResistance);
/// Overloaded assignment operator /// Overloaded assignment operator
Material& operator=(const Material& material); Material& operator=(const Material& material);
@ -92,7 +71,7 @@ class Material {
/** /**
* @return Bounciness factor (between 0 and 1) where 1 is very bouncy * @return Bounciness factor (between 0 and 1) where 1 is very bouncy
*/ */
inline decimal Material::getBounciness() const { inline float Material::getBounciness() const {
return mBounciness; return mBounciness;
} }
@ -102,8 +81,8 @@ inline decimal Material::getBounciness() const {
/** /**
* @param bounciness Bounciness factor (between 0 and 1) where 1 is very bouncy * @param bounciness Bounciness factor (between 0 and 1) where 1 is very bouncy
*/ */
inline void Material::setBounciness(decimal bounciness) { inline void Material::setBounciness(float bounciness) {
assert(bounciness >= decimal(0.0) && bounciness <= decimal(1.0)); assert(bounciness >= float(0.0) && bounciness <= float(1.0));
mBounciness = bounciness; mBounciness = bounciness;
} }
@ -111,7 +90,7 @@ inline void Material::setBounciness(decimal bounciness) {
/** /**
* @return Friction coefficient (positive value) * @return Friction coefficient (positive value)
*/ */
inline decimal Material::getFrictionCoefficient() const { inline float Material::getFrictionCoefficient() const {
return mFrictionCoefficient; return mFrictionCoefficient;
} }
@ -121,8 +100,8 @@ inline decimal Material::getFrictionCoefficient() const {
/** /**
* @param frictionCoefficient Friction coefficient (positive value) * @param frictionCoefficient Friction coefficient (positive value)
*/ */
inline void Material::setFrictionCoefficient(decimal frictionCoefficient) { inline void Material::setFrictionCoefficient(float frictionCoefficient) {
assert(frictionCoefficient >= decimal(0.0)); assert(frictionCoefficient >= float(0.0));
mFrictionCoefficient = frictionCoefficient; mFrictionCoefficient = frictionCoefficient;
} }
@ -132,7 +111,7 @@ inline void Material::setFrictionCoefficient(decimal frictionCoefficient) {
/** /**
* @return The rolling resistance factor (positive value) * @return The rolling resistance factor (positive value)
*/ */
inline decimal Material::getRollingResistance() const { inline float Material::getRollingResistance() const {
return mRollingResistance; return mRollingResistance;
} }
@ -142,7 +121,7 @@ inline decimal Material::getRollingResistance() const {
/** /**
* @param rollingResistance The rolling resistance factor * @param rollingResistance The rolling resistance factor
*/ */
inline void Material::setRollingResistance(decimal rollingResistance) { inline void Material::setRollingResistance(float rollingResistance) {
assert(rollingResistance >= 0); assert(rollingResistance >= 0);
mRollingResistance = rollingResistance; mRollingResistance = rollingResistance;
} }
@ -162,5 +141,3 @@ inline Material& Material::operator=(const Material& material) {
} }
} }
#endif

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