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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/body/Body.h>

41
ephysics/body/Body.h
View File

@ -1,30 +1,9 @@
/********************************************************************************
* 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_BODY_H
#define REACTPHYSICS3D_BODY_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <stdexcept>
@ -70,7 +49,7 @@ class Body {
bool mIsSleeping;
/// 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
void* mUserData;
@ -191,11 +170,11 @@ inline void Body::setIsActive(bool isActive) {
inline void Body::setIsSleeping(bool isSleeping) {
if (isSleeping) {
mSleepTime = decimal(0.0);
mSleepTime = float(0.0);
}
else {
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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#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 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
* 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
ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate(
sizeof(ProxyShape))) ProxyShape(this, collisionShape,
transform, decimal(1));
transform, float(1));
// Add it to the list of proxy collision shapes of the body
if (mProxyCollisionShapes == NULL) {
@ -262,11 +243,11 @@ void CollisionBody::askForBroadPhaseCollisionCheck() const {
// Reset the mIsAlreadyInIsland variable of the body and contact manifolds.
/// This method also returns the number of contact manifolds of the body.
int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
int32_t CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
mIsAlreadyInIsland = false;
int nbManifolds = 0;
int32_t nbManifolds = 0;
// Reset the mIsAlreadyInIsland variable of the contact manifolds for
// this body

43
ephysics/body/CollisionBody.h
View File

@ -1,30 +1,9 @@
/********************************************************************************
* 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_COLLISION_BODY_H
#define REACTPHYSICS3D_COLLISION_BODY_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <stdexcept>
@ -77,7 +56,7 @@ class CollisionBody : public Body {
ProxyShape* mProxyCollisionShapes;
/// Number of collision shapes
uint mNbCollisionShapes;
uint32_t mNbCollisionShapes;
/// First element of the linked list of contact manifolds involving this body
ContactManifoldListElement* mContactManifoldsList;
@ -110,7 +89,7 @@ class CollisionBody : public Body {
void askForBroadPhaseCollisionCheck() const;
/// Reset the mIsAlreadyInIsland variable of the body and contact manifolds
int resetIsAlreadyInIslandAndCountManifolds();
int32_t resetIsAlreadyInIslandAndCountManifolds();
public :
@ -218,7 +197,7 @@ inline void CollisionBody::setType(BodyType type) {
// Return the current position and orientation
/**
* @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 {
return mTransform;
@ -227,7 +206,7 @@ inline const Transform& CollisionBody::getTransform() const {
// Set the current position and orientation
/**
* @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) {
@ -302,5 +281,3 @@ inline Vector3 CollisionBody::getLocalVector(const Vector3& worldVector) const {
}
}
#endif

89
ephysics/body/RigidBody.cpp
View File

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/body/RigidBody.h>
@ -39,13 +20,13 @@ using namespace reactphysics3d;
* @param id The ID of the body
*/
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()),
mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)),
mIsGravityEnabled(true), mLinearDamping(float(0.0)), mAngularDamping(float(0.0)),
mJointsList(NULL) {
// Compute the inverse mass
mMassInverse = decimal(1.0) / mInitMass;
mMassInverse = float(1.0) / mInitMass;
}
// Destructor
@ -87,13 +68,13 @@ void RigidBody::setType(BodyType type) {
if (mType == STATIC || mType == KINEMATIC) {
// Reset the inverse mass and inverse inertia tensor to zero
mMassInverse = decimal(0.0);
mMassInverse = float(0.0);
mInertiaTensorLocal.setToZero();
mInertiaTensorLocalInverse.setToZero();
}
else { // If it is a dynamic body
mMassInverse = decimal(1.0) / mInitMass;
mMassInverse = float(1.0) / mInitMass;
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
}
@ -150,18 +131,18 @@ void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) {
/**
* @param mass The mass (in kilograms) of the body
*/
void RigidBody::setMass(decimal mass) {
void RigidBody::setMass(float mass) {
if (mType != DYNAMIC) return;
mInitMass = mass;
if (mInitMass > decimal(0.0)) {
mMassInverse = decimal(1.0) / mInitMass;
if (mInitMass > float(0.0)) {
mMassInverse = float(1.0) / mInitMass;
}
else {
mInitMass = decimal(1.0);
mMassInverse = decimal(1.0);
mInitMass = float(1.0);
mMassInverse = float(1.0);
}
}
@ -194,8 +175,8 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons
}
// Add a collision shape to the body.
/// When you add a collision shape to the body, an internal copy of this
/// collision shape will be created internally. Therefore, you can delete it
/// When you add a collision shape to the body, an int32_ternal copy of this
/// 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.
/// 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
@ -204,16 +185,16 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons
/**
* @param collisionShape The collision shape you want to add to the body
* @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
* @return A pointer to the proxy shape that has been created to link the body to
* the new collision shape you have added.
*/
ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
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
ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate(
@ -275,7 +256,7 @@ void RigidBody::setLinearVelocity(const Vector3& linearVelocity) {
mLinearVelocity = linearVelocity;
// If the linear velocity is not zero, awake the body
if (mLinearVelocity.lengthSquare() > decimal(0.0)) {
if (mLinearVelocity.lengthSquare() > float(0.0)) {
setIsSleeping(false);
}
}
@ -293,7 +274,7 @@ void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
mAngularVelocity = angularVelocity;
// If the velocity is not zero, awake the body
if (mAngularVelocity.lengthSquare() > decimal(0.0)) {
if (mAngularVelocity.lengthSquare() > float(0.0)) {
setIsSleeping(false);
}
}
@ -301,7 +282,7 @@ void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
// Set the current position and orientation
/**
* @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) {
@ -324,8 +305,8 @@ void RigidBody::setTransform(const Transform& transform) {
// the collision shapes attached to the body.
void RigidBody::recomputeMassInformation() {
mInitMass = decimal(0.0);
mMassInverse = decimal(0.0);
mInitMass = float(0.0);
mMassInverse = float(0.0);
mInertiaTensorLocal.setToZero();
mInertiaTensorLocalInverse.setToZero();
mCenterOfMassLocal.setToZero();
@ -344,12 +325,12 @@ void RigidBody::recomputeMassInformation() {
mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass();
}
if (mInitMass > decimal(0.0)) {
mMassInverse = decimal(1.0) / mInitMass;
if (mInitMass > float(0.0)) {
mMassInverse = float(1.0) / mInitMass;
}
else {
mInitMass = decimal(1.0);
mMassInverse = decimal(1.0);
mInitMass = float(1.0);
mMassInverse = float(1.0);
}
// Compute the center of mass
@ -364,19 +345,19 @@ void RigidBody::recomputeMassInformation() {
Matrix3x3 inertiaTensor;
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();
Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix();
inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose();
// Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape
// center 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;
decimal offsetSquare = offset.lengthSquare();
float offsetSquare = offset.lengthSquare();
Matrix3x3 offsetMatrix;
offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0));
offsetMatrix[1].setAllValues(decimal(0.0), offsetSquare, decimal(0.0));
offsetMatrix[2].setAllValues(decimal(0.0), decimal(0.0), offsetSquare);
offsetMatrix[0].setAllValues(offsetSquare, float(0.0), float(0.0));
offsetMatrix[1].setAllValues(float(0.0), offsetSquare, float(0.0));
offsetMatrix[2].setAllValues(float(0.0), float(0.0), offsetSquare);
offsetMatrix[0] += offset * (-offset.x);
offsetMatrix[1] += offset * (-offset.y);
offsetMatrix[2] += offset * (-offset.z);

70
ephysics/body/RigidBody.h
View File

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

59
ephysics/collision/CollisionDetection.cpp
View File

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/CollisionDetection.h>
@ -72,8 +53,8 @@ void CollisionDetection::computeCollisionDetection() {
// Compute the collision detection
void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1,
const std::set<uint>& shapes2) {
const std::set<uint32_t>& shapes1,
const std::set<uint32_t>& shapes2) {
// Compute the broad-phase collision detection
computeBroadPhase();
@ -87,8 +68,8 @@ void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback,
// Report collision between two sets of shapes
void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1,
const std::set<uint>& shapes2) {
const std::set<uint32_t>& shapes1,
const std::set<uint32_t>& shapes2) {
// For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it;
@ -121,12 +102,12 @@ void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callbac
// For each contact manifold set of the overlapping pair
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);
// 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);
@ -246,8 +227,8 @@ void CollisionDetection::computeNarrowPhase() {
// Compute the narrow-phase collision detection
void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1,
const std::set<uint>& shapes2) {
const std::set<uint32_t>& shapes1,
const std::set<uint32_t>& shapes2) {
mContactOverlappingPairs.clear();
@ -367,10 +348,10 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return;
// 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());
// 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(shape1, shape2, nbMaxManifolds, mWorld->mMemoryAllocator);
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
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(),
overlappingPair->getShape2());
mContactOverlappingPairs[pairId] = overlappingPair;
@ -453,7 +434,7 @@ void CollisionDetection::addAllContactManifoldsToBodies() {
std::map<overlappingpairid, OverlappingPair*>::iterator 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
addContactManifoldToBody(it->second);
}
@ -470,7 +451,7 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
// 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);
@ -508,8 +489,8 @@ void CollisionDetection::clearContactPoints() {
void CollisionDetection::fillInCollisionMatrix() {
// For each possible type of collision shape
for (int i=0; i<NB_COLLISION_SHAPE_TYPES; i++) {
for (int j=0; j<NB_COLLISION_SHAPE_TYPES; j++) {
for (int32_t i=0; i<NB_COLLISION_SHAPE_TYPES; i++) {
for (int32_t j=0; j<NB_COLLISION_SHAPE_TYPES; j++) {
mCollisionMatrix[i][j] = mCollisionDispatch->selectAlgorithm(i, j);
}
}

47
ephysics/collision/CollisionDetection.h
View File

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

34
ephysics/collision/CollisionShapeInfo.h
View File

@ -1,30 +1,9 @@
/********************************************************************************
* 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_COLLISION_SHAPE_INFO_H
#define REACTPHYSICS3D_COLLISION_SHAPE_INFO_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/collision/shapes/CollisionShape.h>
@ -71,5 +50,4 @@ struct CollisionShapeInfo {
}
#endif

72
ephysics/collision/ContactManifold.cpp
View File

@ -1,27 +1,9 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <iostream>
@ -48,11 +30,11 @@ ContactManifold::~ContactManifold() {
void ContactManifold::addContactPoint(ContactPoint* contact) {
// 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
// already in the manifold.
decimal distance = (mContactPoints[i]->getWorldPointOnBody1() -
float distance = (mContactPoints[i]->getWorldPointOnBody1() -
contact->getWorldPointOnBody1()).lengthSquare();
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 (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) {
int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
int32_t indexMaxPenetration = getIndexOfDeepestPenetration(contact);
int32_t indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
removeContactPoint(indexToRemove);
}
@ -81,7 +63,7 @@ void ContactManifold::addContactPoint(ContactPoint* contact) {
}
// Remove a contact point from the manifold
void ContactManifold::removeContactPoint(uint index) {
void ContactManifold::removeContactPoint(uint32_t index) {
assert(index < mNbContactPoints);
assert(mNbContactPoints > 0);
@ -109,7 +91,7 @@ void ContactManifold::update(const Transform& transform1, const Transform& trans
if (mNbContactPoints == 0) return;
// 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]->getLocalPointOnBody1());
mContactPoints[i]->setWorldPointOnBody2(transform2 *
@ -118,15 +100,15 @@ void ContactManifold::update(const Transform& transform1, const Transform& trans
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;
// Remove the contact points that don't represent very well the contact manifold
for (int i=static_cast<int>(mNbContactPoints)-1; i>=0; i--) {
assert(i < static_cast<int>(mNbContactPoints));
for (int32_t i=static_cast<int32_t>(mNbContactPoints)-1; i>=0; i--) {
assert(i < static_cast<int32_t>(mNbContactPoints));
// 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 (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.
/// This corresponding contact will be kept in the cache. The method returns -1 is
/// 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);
int indexMaxPenetrationDepth = -1;
decimal maxPenetrationDepth = newContact->getPenetrationDepth();
int32_t indexMaxPenetrationDepth = -1;
float maxPenetrationDepth = newContact->getPenetrationDepth();
// 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 (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,
/// 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).
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);
decimal area0 = 0.0; // Area with contact 1,2,3 and newPoint
decimal area1 = 0.0; // Area with contact 0,2,3 and newPoint
decimal area2 = 0.0; // Area with contact 0,1,3 and newPoint
decimal area3 = 0.0; // Area with contact 0,1,2 and newPoint
float area0 = 0.0; // Area with contact 1,2,3 and newPoint
float area1 = 0.0; // Area with contact 0,2,3 and newPoint
float area2 = 0.0; // Area with contact 0,1,3 and newPoint
float area3 = 0.0; // Area with contact 0,1,2 and newPoint
if (indexMaxPenetration != 0) {
// Compute the area
@ -227,7 +209,7 @@ int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& ne
}
// 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 (area1 < area2) {
if (area2 < area3) return 3;
@ -252,7 +234,7 @@ int ContactManifold::getMaxArea(decimal area0, decimal area1, decimal area2, dec
// Clear the contact manifold
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
// the corresponding memory block is now free

111
ephysics/collision/ContactManifold.h
View File

@ -1,30 +1,9 @@
/********************************************************************************
* 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_CONTACT_MANIFOLD_H
#define REACTPHYSICS3D_CONTACT_MANIFOLD_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <vector>
@ -37,7 +16,7 @@
namespace reactphysics3d {
// 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 ContactManifold;
@ -99,10 +78,10 @@ class ContactManifold {
ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD];
/// Normal direction Id (Unique Id representing the normal direction)
short int mNormalDirectionId;
int16_t mNormalDirectionId;
/// Number of contacts in the cache
uint mNbContactPoints;
uint32_t mNbContactPoints;
/// First friction vector of the contact manifold
Vector3 mFrictionVector1;
@ -111,18 +90,18 @@ class ContactManifold {
Vector3 mFrictionVector2;
/// First friction constraint accumulated impulse
decimal mFrictionImpulse1;
float mFrictionImpulse1;
/// Second friction constraint accumulated impulse
decimal mFrictionImpulse2;
float mFrictionImpulse2;
/// Twist friction constraint accumulated impulse
decimal mFrictionTwistImpulse;
float mFrictionTwistImpulse;
/// Accumulated rolling resistance impulse
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;
/// Reference to the memory allocator
@ -137,18 +116,18 @@ class ContactManifold {
ContactManifold& operator=(const ContactManifold& contactManifold);
/// 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.
int getIndexOfDeepestPenetration(ContactPoint* newContact) const;
int32_t getIndexOfDeepestPenetration(ContactPoint* newContact) const;
/// 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
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;
public:
@ -157,7 +136,7 @@ class ContactManifold {
/// Constructor
ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, short int normalDirectionId);
MemoryAllocator& memoryAllocator, int16_t normalDirectionId);
/// Destructor
~ContactManifold();
@ -175,7 +154,7 @@ class ContactManifold {
CollisionBody* getBody2() const;
/// Return the normal direction Id
short int getNormalDirectionId() const;
int16_t getNormalDirectionId() const;
/// Add a contact point to the manifold
void addContactPoint(ContactPoint* contact);
@ -187,7 +166,7 @@ class ContactManifold {
void clear();
/// 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
const Vector3& getFrictionVector1() const;
@ -202,34 +181,34 @@ class ContactManifold {
void setFrictionVector2(const Vector3& mFrictionVector2);
/// Return the first friction accumulated impulse
decimal getFrictionImpulse1() const;
float getFrictionImpulse1() const;
/// Set the first friction accumulated impulse
void setFrictionImpulse1(decimal frictionImpulse1);
void setFrictionImpulse1(float frictionImpulse1);
/// Return the second friction accumulated impulse
decimal getFrictionImpulse2() const;
float getFrictionImpulse2() const;
/// Set the second friction accumulated impulse
void setFrictionImpulse2(decimal frictionImpulse2);
void setFrictionImpulse2(float frictionImpulse2);
/// Return the friction twist accumulated impulse
decimal getFrictionTwistImpulse() const;
float getFrictionTwistImpulse() const;
/// Set the friction twist accumulated impulse
void setFrictionTwistImpulse(decimal frictionTwistImpulse);
void setFrictionTwistImpulse(float frictionTwistImpulse);
/// Set the accumulated rolling resistance impulse
void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse);
/// Return a contact point of the manifold
ContactPoint* getContactPoint(uint index) const;
ContactPoint* getContactPoint(uint32_t index) const;
/// Return the normalized averaged normal vector
Vector3 getAverageContactNormal() const;
/// Return the largest depth of all the contact points
decimal getLargestContactDepth() const;
float getLargestContactDepth() const;
// -------------------- Friendship -------------------- //
@ -259,12 +238,12 @@ inline CollisionBody* ContactManifold::getBody2() const {
}
// Return the normal direction Id
inline short int ContactManifold::getNormalDirectionId() const {
inline int16_t ContactManifold::getNormalDirectionId() const {
return mNormalDirectionId;
}
// Return the number of contact points in the manifold
inline uint ContactManifold::getNbContactPoints() const {
inline uint32_t ContactManifold::getNbContactPoints() const {
return mNbContactPoints;
}
@ -289,32 +268,32 @@ inline void ContactManifold::setFrictionVector2(const Vector3& frictionVector2)
}
// Return the first friction accumulated impulse
inline decimal ContactManifold::getFrictionImpulse1() const {
inline float ContactManifold::getFrictionImpulse1() const {
return mFrictionImpulse1;
}
// Set the first friction accumulated impulse
inline void ContactManifold::setFrictionImpulse1(decimal frictionImpulse1) {
inline void ContactManifold::setFrictionImpulse1(float frictionImpulse1) {
mFrictionImpulse1 = frictionImpulse1;
}
// Return the second friction accumulated impulse
inline decimal ContactManifold::getFrictionImpulse2() const {
inline float ContactManifold::getFrictionImpulse2() const {
return mFrictionImpulse2;
}
// Set the second friction accumulated impulse
inline void ContactManifold::setFrictionImpulse2(decimal frictionImpulse2) {
inline void ContactManifold::setFrictionImpulse2(float frictionImpulse2) {
mFrictionImpulse2 = frictionImpulse2;
}
// Return the friction twist accumulated impulse
inline decimal ContactManifold::getFrictionTwistImpulse() const {
inline float ContactManifold::getFrictionTwistImpulse() const {
return mFrictionTwistImpulse;
}
// Set the friction twist accumulated impulse
inline void ContactManifold::setFrictionTwistImpulse(decimal frictionTwistImpulse) {
inline void ContactManifold::setFrictionTwistImpulse(float frictionTwistImpulse) {
mFrictionTwistImpulse = frictionTwistImpulse;
}
@ -324,12 +303,12 @@ inline void ContactManifold::setRollingResistanceImpulse(const Vector3& rollingR
}
// 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);
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 {
return mIsAlreadyInIsland;
}
@ -338,7 +317,7 @@ inline bool ContactManifold::isAlreadyInIsland() const {
inline Vector3 ContactManifold::getAverageContactNormal() const {
Vector3 averageNormal;
for (uint i=0; i<mNbContactPoints; i++) {
for (uint32_t i=0; i<mNbContactPoints; i++) {
averageNormal += mContactPoints[i]->getNormal();
}
@ -346,11 +325,11 @@ inline Vector3 ContactManifold::getAverageContactNormal() const {
}
// Return the largest depth of all the contact points
inline decimal ContactManifold::getLargestContactDepth() const {
decimal largestDepth = 0.0f;
inline float ContactManifold::getLargestContactDepth() const {
float largestDepth = 0.0f;
for (uint i=0; i<mNbContactPoints; i++) {
decimal depth = mContactPoints[i]->getPenetrationDepth();
for (uint32_t i=0; i<mNbContactPoints; i++) {
float depth = mContactPoints[i]->getPenetrationDepth();
if (depth > largestDepth) {
largestDepth = depth;
}
@ -360,5 +339,5 @@ inline decimal ContactManifold::getLargestContactDepth() const {
}
}
#endif

83
ephysics/collision/ContactManifoldSet.cpp
View File

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/ContactManifoldSet.h>
@ -30,7 +11,7 @@ using namespace reactphysics3d;
// Constructor
ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, int nbMaxManifolds)
MemoryAllocator& memoryAllocator, int32_t nbMaxManifolds)
: mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1),
mShape2(shape2), mMemoryAllocator(memoryAllocator) {
assert(nbMaxManifolds >= 1);
@ -47,7 +28,7 @@ ContactManifoldSet::~ContactManifoldSet() {
void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
// 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 (mNbManifolds == 0) {
@ -55,7 +36,7 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
createManifold(normalDirectionId);
mManifolds[0]->addContactPoint(contact);
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);
}
@ -63,7 +44,7 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
}
// Select the manifold with the most similar normal (if exists)
int similarManifoldIndex = 0;
int32_t similarManifoldIndex = 0;
if (mNbMaxManifolds > 1) {
similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId);
}
@ -84,7 +65,7 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
// Create a new manifold for the contact point
createManifold(normalDirectionId);
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);
}
@ -94,11 +75,11 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
// 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
// with the largest contact depth among their points
int smallestDepthIndex = -1;
decimal minDepth = contact->getPenetrationDepth();
int32_t smallestDepthIndex = -1;
float minDepth = contact->getPenetrationDepth();
assert(mNbManifolds == mNbMaxManifolds);
for (int i=0; i<mNbManifolds; i++) {
decimal depth = mManifolds[i]->getLargestContactDepth();
for (int32_t i=0; i<mNbManifolds; i++) {
float depth = mManifolds[i]->getLargestContactDepth();
if (depth < minDepth) {
minDepth = depth;
smallestDepthIndex = i;
@ -124,7 +105,7 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
createManifold(normalDirectionId);
mManifolds[mNbManifolds-1]->addContactPoint(contact);
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);
}
@ -133,10 +114,10 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
// Return the index of the contact manifold with a similar average normal.
// 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)
for (int i=0; i<mNbManifolds; i++) {
for (int32_t i=0; i<mNbManifolds; i++) {
if (normalDirectionId == mManifolds[i]->getNormalDirectionId()) {
return i;
}
@ -145,16 +126,16 @@ int ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectio
return -1;
}
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided into 4x4 buckets. This method maps the
// normal vector into of the of the bucket and returns a unique Id for the bucket
short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) const {
// Map the normal vector int32_to a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided int32_to 4x4 buckets. This method maps the
// normal vector int32_to of the of the bucket and returns a unique Id for the bucket
int16_t ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) const {
assert(normal.lengthSquare() > MACHINE_EPSILON);
int faceNo;
decimal u, v;
decimal max = max3(fabs(normal.x), fabs(normal.y), fabs(normal.z));
int32_t faceNo;
float u, v;
float max = max3(fabs(normal.x), fabs(normal.y), fabs(normal.z));
Vector3 normalScaled = normal / max;
if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) {
@ -173,19 +154,19 @@ short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) cons
v = normalScaled.y;
}
int indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
int indexV = floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
int32_t indexU = floor(((u + 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 (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;
}
// Update the contact manifolds
void ContactManifoldSet::update() {
for (int i=mNbManifolds-1; i>=0; i--) {
for (int32_t i=mNbManifolds-1; i>=0; i--) {
// Update the contact manifold
mManifolds[i]->update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(),
@ -202,7 +183,7 @@ void ContactManifoldSet::update() {
void ContactManifoldSet::clear() {
// Destroy all the contact manifolds
for (int i=mNbManifolds-1; i>=0; i--) {
for (int32_t i=mNbManifolds-1; i>=0; i--) {
removeManifold(i);
}
@ -210,7 +191,7 @@ void ContactManifoldSet::clear() {
}
// 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);
mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
@ -219,7 +200,7 @@ void ContactManifoldSet::createManifold(short int normalDirectionId) {
}
// Remove a contact manifold from the set
void ContactManifoldSet::removeManifold(int index) {
void ContactManifoldSet::removeManifold(int32_t index) {
assert(mNbManifolds > 0);
assert(index >= 0 && index < mNbManifolds);
@ -228,7 +209,7 @@ void ContactManifoldSet::removeManifold(int index) {
mManifolds[index]->~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];
}

73
ephysics/collision/ContactManifoldSet.h
View File

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

35
ephysics/collision/ProxyShape.cpp
View File

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#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 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),
mNext(NULL), mBroadPhaseID(-1), mCachedCollisionData(NULL), mUserData(NULL),
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 (!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 worldToLocalTransform = localToWorldTransform.getInverse();
Ray rayLocal(worldToLocalTransform * ray.point1,
@ -83,7 +64,7 @@ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
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.worldNormal = localToWorldTransform.getOrientation() * raycastInfo.worldNormal;
raycastInfo.worldNormal.normalize();

48
ephysics/collision/ProxyShape.h
View File

@ -1,30 +1,9 @@
/********************************************************************************
* 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_PROXY_SHAPE_H
#define REACTPHYSICS3D_PROXY_SHAPE_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/body/CollisionBody.h>
@ -58,13 +37,13 @@ class ProxyShape {
Transform mLocalToBodyTransform;
/// Mass (in kilogramms) of the corresponding collision shape
decimal mMass;
float mMass;
/// Pointer to the next proxy shape of the body (linked list)
ProxyShape* mNext;
/// Broad-phase ID (node ID in the dynamic AABB tree)
int mBroadPhaseID;
int32_t mBroadPhaseID;
/// Cached collision data
void* mCachedCollisionData;
@ -99,7 +78,7 @@ class ProxyShape {
/// Constructor
ProxyShape(CollisionBody* body, CollisionShape* shape,
const Transform& transform, decimal mass);
const Transform& transform, float mass);
/// Destructor
virtual ~ProxyShape();
@ -111,7 +90,7 @@ class ProxyShape {
CollisionBody* getBody() const;
/// Return the mass of the collision shape
decimal getMass() const;
float getMass() const;
/// Return a pointer to the user data attached to this body
void* getUserData() const;
@ -184,7 +163,7 @@ inline void** ProxyShape::getCachedCollisionData() {
// 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 {
return mCollisionShape;
@ -202,13 +181,13 @@ inline CollisionBody* ProxyShape::getBody() const {
/**
* @return Mass of the collision shape (in kilograms)
*/
inline decimal ProxyShape::getMass() const {
inline float ProxyShape::getMass() const {
return mMass;
}
// 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 {
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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/decimal.h>
#include <ephysics/collision/RaycastInfo.h>
#include <ephysics/collision/ProxyShape.h>
using namespace reactphysics3d;
// 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
RaycastInfo raycastInfo;

44
ephysics/collision/RaycastInfo.h
View File

@ -1,30 +1,9 @@
/********************************************************************************
* 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_RAYCAST_INFO_H
#define REACTPHYSICS3D_RAYCAST_INFO_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/mathematics/Vector3.h>
@ -66,13 +45,13 @@ struct RaycastInfo {
/// 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)
decimal hitFraction;
float hitFraction;
/// 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)
int triangleIndex;
int32_t triangleIndex;
/// Pointer to the hit collision body
CollisionBody* body;
@ -126,7 +105,7 @@ class RaycastCallback {
* @param raycastInfo Information about the raycast 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
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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/TriangleMesh.h>

42
ephysics/collision/TriangleMesh.h
View File

@ -1,30 +1,9 @@
/********************************************************************************
* 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_TRIANGLE_MESH_H
#define REACTPHYSICS3D_TRIANGLE_MESH_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <vector>
@ -60,10 +39,10 @@ class TriangleMesh {
void addSubpart(TriangleVertexArray* triangleVertexArray);
/// 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
uint getNbSubparts() const;
uint32_t getNbSubparts() const;
};
// 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
inline TriangleVertexArray* TriangleMesh::getSubpart(uint indexSubpart) const {
inline TriangleVertexArray* TriangleMesh::getSubpart(uint32_t indexSubpart) const {
assert(indexSubpart < mTriangleArrays.size());
return mTriangleArrays[indexSubpart];
}
// Return the number of subparts of the mesh
inline uint TriangleMesh::getNbSubparts() const {
inline uint32_t TriangleMesh::getNbSubparts() const {
return mTriangleArrays.size();
}
}
#endif

37
ephysics/collision/TriangleVertexArray.cpp
View File

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/TriangleVertexArray.h>
@ -29,7 +10,7 @@
using namespace reactphysics3d;
// 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
/// the lifetime of the TriangleVertexArray.
/**
@ -40,10 +21,10 @@ using namespace reactphysics3d;
* @param indexesStart Pointer to the first triangle index
* @param indexesStride Number of bytes between the beginning of two consecutive triangle indices
* @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,
uint nbTriangles, void* indexesStart, int indexesStride,
TriangleVertexArray::TriangleVertexArray(uint32_t nbVertices, void* verticesStart, int32_t verticesStride,
uint32_t nbTriangles, void* indexesStart, int32_t indexesStride,
VertexDataType vertexDataType, IndexDataType indexDataType) {
m_numberVertices = nbVertices;
m_verticesStart = reinterpret_cast<unsigned char*>(verticesStart);

64
ephysics/collision/TriangleVertexArray.h
View File

@ -1,30 +1,9 @@
/********************************************************************************
* 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_TRIANGLE_VERTEX_ARRAY_H
#define REACTPHYSICS3D_TRIANGLE_VERTEX_ARRAY_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/configuration.h>
@ -36,7 +15,7 @@ namespace reactphysics3d {
* This class is used to describe the vertices and faces of a triangular mesh.
* A TriangleVertexArray represents a continuous array of vertices and indexes
* 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
* part. Therefore, make sure that the data pointed by a TriangleVertexArray
* remains valid during the TriangleVertexArray life.
@ -54,24 +33,24 @@ class TriangleVertexArray {
protected:
/// Number of vertices in the array
uint m_numberVertices;
uint32_t m_numberVertices;
/// Pointer to the first vertex value in the array
unsigned char* m_verticesStart;
/// Stride (number of bytes) between the beginning of two vertices
/// values in the array
int m_verticesStride;
int32_t m_verticesStride;
/// Number of triangles in the array
uint mNbTriangles;
uint32_t mNbTriangles;
/// Pointer to the first vertex index of the array
unsigned char* mIndicesStart;
/// Stride (number of bytes) between the beginning of two indices in
/// the array
int mIndicesStride;
int32_t mIndicesStride;
/// Data type of the vertices in the array
VertexDataType mVertexDataType;
@ -82,8 +61,8 @@ class TriangleVertexArray {
public:
/// Constructor
TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
uint nbTriangles, void* indexesStart, int indexesStride,
TriangleVertexArray(uint32_t nbVertices, void* verticesStart, int32_t verticesStride,
uint32_t nbTriangles, void* indexesStart, int32_t indexesStride,
VertexDataType vertexDataType, IndexDataType indexDataType);
/// Destructor
@ -96,16 +75,16 @@ class TriangleVertexArray {
IndexDataType getIndexDataType() const;
/// Return the number of vertices
uint getNbVertices() const;
uint32_t getNbVertices() const;
/// Return the number of triangles
uint getNbTriangles() const;
uint32_t getNbTriangles() const;
/// Return the vertices stride (number of bytes)
int getVerticesStride() const;
int32_t getVerticesStride() const;
/// Return the indices stride (number of bytes)
int getIndicesStride() const;
int32_t getIndicesStride() const;
/// Return the pointer to the start of the vertices array
unsigned char* getVerticesStart() const;
@ -125,22 +104,22 @@ inline TriangleVertexArray::IndexDataType TriangleVertexArray::getIndexDataType(
}
// Return the number of vertices
inline uint TriangleVertexArray::getNbVertices() const {
inline uint32_t TriangleVertexArray::getNbVertices() const {
return m_numberVertices;
}
// Return the number of triangles
inline uint TriangleVertexArray::getNbTriangles() const {
inline uint32_t TriangleVertexArray::getNbTriangles() const {
return mNbTriangles;
}
// Return the vertices stride (number of bytes)
inline int TriangleVertexArray::getVerticesStride() const {
inline int32_t TriangleVertexArray::getVerticesStride() const {
return m_verticesStride;
}
// Return the indices stride (number of bytes)
inline int TriangleVertexArray::getIndicesStride() const {
inline int32_t TriangleVertexArray::getIndicesStride() const {
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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/broadphase/BroadPhaseAlgorithm.h>
@ -38,7 +19,7 @@ BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
mCollisionDetection(collisionDetection) {
// 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);
// 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
// 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
if (mNbAllocatedMovedShapes == mNbMovedShapes) {
mNbAllocatedMovedShapes *= 2;
int* oldArray = mMovedShapes;
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
int32_t* oldArray = mMovedShapes;
mMovedShapes = (int32_t*) malloc(mNbAllocatedMovedShapes * sizeof(int32_t));
assert(mMovedShapes != NULL);
memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int));
memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int32_t));
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(mMovedShapes != NULL);
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
// 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);
@ -89,11 +70,11 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
mNbAllocatedMovedShapes > 8) {
mNbAllocatedMovedShapes /= 2;
int* oldArray = mMovedShapes;
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
int32_t* oldArray = mMovedShapes;
mMovedShapes = (int32_t*) malloc(mNbAllocatedMovedShapes * sizeof(int32_t));
assert(mMovedShapes != NULL);
uint nbElements = 0;
for (uint i=0; i<mNbMovedShapes; i++) {
uint32_t nbElements = 0;
for (uint32_t i=0; i<mNbMovedShapes; i++) {
if (oldArray[i] != -1) {
mMovedShapes[nbElements] = oldArray[i];
nbElements++;
@ -105,7 +86,7 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
}
// 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) {
mMovedShapes[i] = -1;
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) {
// Add the collision shape into the dynamic AABB tree and get its broad-phase ID
int nodeId = m_dynamicAABBTree.addObject(aabb, proxyShape);
// Add the collision shape int32_to the dynamic AABB tree and get its broad-phase ID
int32_t nodeId = m_dynamicAABBTree.addObject(aabb, proxyShape);
// Set the broad-phase ID of the proxy shape
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
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
void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
int broadPhaseID = proxyShape->mBroadPhaseID;
int32_t broadPhaseID = proxyShape->mBroadPhaseID;
// Remove the collision shape from the dynamic AABB tree
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
removeMovedCollisionShape(broadPhaseID);
}
@ -145,7 +126,7 @@ void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb,
const Vector3& displacement, bool forceReinsert) {
int broadPhaseID = proxyShape->mBroadPhaseID;
int32_t broadPhaseID = proxyShape->mBroadPhaseID;
assert(broadPhaseID >= 0);
@ -153,10 +134,10 @@ void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, cons
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
// into the tree).
// int32_to the tree).
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
addMovedCollisionShape(broadPhaseID);
}
@ -170,8 +151,8 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
// For all collision shapes that have moved (or have been created) during the
// last simulation step
for (uint i=0; i<mNbMovedShapes; i++) {
int shapeID = mMovedShapes[i];
for (uint32_t i=0; i<mNbMovedShapes; i++) {
int32_t shapeID = mMovedShapes[i];
if (shapeID == -1) continue;
@ -195,7 +176,7 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
// Check all the potential overlapping pairs avoiding duplicates to report unique
// overlapping pairs
uint i=0;
uint32_t i=0;
while (i < mNbPotentialPairs) {
// 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
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 (node1ID == node2ID) return;
@ -258,7 +239,7 @@ void BroadPhaseAlgorithm::notifyOverlappingNodes(int node1ID, int node2ID) {
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].collisionShape2ID = std::max(node1ID, node2ID);
mNbPotentialPairs++;
@ -266,15 +247,15 @@ void BroadPhaseAlgorithm::notifyOverlappingNodes(int node1ID, int node2ID) {
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
void AABBOverlapCallback::notifyOverlappingNode(int nodeId) {
void AABBOverlapCallback::notifyOverlappingNode(int32_t nodeId) {
mBroadPhaseAlgorithm.notifyOverlappingNodes(mReferenceNodeId, nodeId);
}
// 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
ProxyShape* proxyShape = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(nodeId));

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_BROAD_PHASE_ALGORITHM_H
#define REACTPHYSICS3D_BROAD_PHASE_ALGORITHM_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <vector>
@ -50,10 +29,10 @@ struct BroadPhasePair {
// -------------------- Attributes -------------------- //
/// Broad-phase ID of the first collision shape
int collisionShape1ID;
int32_t collisionShape1ID;
/// Broad-phase ID of the second collision shape
int collisionShape2ID;
int32_t collisionShape2ID;
// -------------------- Methods -------------------- //
@ -68,19 +47,19 @@ class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
BroadPhaseAlgorithm& mBroadPhaseAlgorithm;
int mReferenceNodeId;
int32_t mReferenceNodeId;
public:
// Constructor
AABBOverlapCallback(BroadPhaseAlgorithm& broadPhaseAlgo, int referenceNodeId)
AABBOverlapCallback(BroadPhaseAlgorithm& broadPhaseAlgo, int32_t referenceNodeId)
: mBroadPhaseAlgorithm(broadPhaseAlgo), mReferenceNodeId(referenceNodeId) {
}
// Called when a overlapping node has been found during the call to
// 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
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
/// created) during the last simulation step. Those are the shapes that need to be tested
/// 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
/// simulation step.
uint mNbMovedShapes;
uint32_t mNbMovedShapes;
/// Number of allocated elements for the array of shapes that have moved during the last
/// simulation step.
uint mNbAllocatedMovedShapes;
uint32_t mNbAllocatedMovedShapes;
/// Number of non-used elements in the array of shapes that have moved during the last
/// simulation step.
uint mNbNonUsedMovedShapes;
uint32_t mNbNonUsedMovedShapes;
/// Temporary array of potential overlapping pairs (with potential duplicates)
BroadPhasePair* mPotentialPairs;
/// Number of potential overlapping pairs
uint mNbPotentialPairs;
uint32_t mNbPotentialPairs;
/// Number of allocated elements for the array of potential overlapping pairs
uint mNbAllocatedPotentialPairs;
uint32_t mNbAllocatedPotentialPairs;
/// Reference to the collision detection object
CollisionDetection& mCollisionDetection;
@ -178,7 +157,7 @@ class BroadPhaseAlgorithm {
/// Destructor
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);
/// 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
/// 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
/// 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
void notifyOverlappingNodes(int broadPhaseId1, int broadPhaseId2);
void notifyOverlappingNodes(int32_t broadPhaseId1, int32_t broadPhaseId2);
/// Compute all the overlapping pairs of collision shapes
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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/broadphase/DynamicAABBTree.h>
@ -32,10 +13,10 @@
using namespace reactphysics3d;
// Initialization of static variables
const int TreeNode::NULL_TREE_NODE = -1;
const int32_t TreeNode::NULL_TREE_NODE = -1;
// Constructor
DynamicAABBTree::DynamicAABBTree(decimal extraAABBGap) : mExtraAABBGap(extraAABBGap) {
DynamicAABBTree::DynamicAABBTree(float extraAABBGap) : mExtraAABBGap(extraAABBGap) {
init();
}
@ -60,7 +41,7 @@ void DynamicAABBTree::init() {
memset(mNodes, 0, mNbAllocatedNodes * sizeof(TreeNode));
// 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].height = -1;
}
@ -80,7 +61,7 @@ void DynamicAABBTree::reset() {
}
// 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 (mFreeNodeID == TreeNode::NULL_TREE_NODE) {
@ -96,7 +77,7 @@ int DynamicAABBTree::allocateNode() {
free(oldNodes);
// 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].height = -1;
}
@ -106,7 +87,7 @@ int DynamicAABBTree::allocateNode() {
}
// Get the next free node
int freeNodeID = mFreeNodeID;
int32_t freeNodeID = mFreeNodeID;
mFreeNodeID = mNodes[freeNodeID].nextNodeID;
mNodes[freeNodeID].parentID = TreeNode::NULL_TREE_NODE;
mNodes[freeNodeID].height = 0;
@ -116,7 +97,7 @@ int DynamicAABBTree::allocateNode() {
}
// Release a node
void DynamicAABBTree::releaseNode(int nodeID) {
void DynamicAABBTree::releaseNode(int32_t nodeID) {
assert(mNbNodes > 0);
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
@ -127,11 +108,11 @@ void DynamicAABBTree::releaseNode(int nodeID) {
mNbNodes--;
}
// Internally add an object into the tree
int DynamicAABBTree::addObjectInternal(const AABB& aabb) {
// Internally add an object int32_to the tree
int32_t DynamicAABBTree::addObjectInternal(const AABB& aabb) {
// 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
const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap);
@ -152,7 +133,7 @@ int DynamicAABBTree::addObjectInternal(const AABB& aabb) {
}
// Remove an object from the tree
void DynamicAABBTree::removeObject(int nodeID) {
void DynamicAABBTree::removeObject(int32_t nodeID) {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf());
@ -164,12 +145,12 @@ void DynamicAABBTree::removeObject(int nodeID) {
// 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
/// nothing is done. Otherwise, the corresponding node is removed and reinserted into the tree.
/// The method returns true if the object has been reinserted into the tree. The "displacement"
/// 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 int32_to the tree. The "displacement"
/// argument is the linear velocity of the AABB multiplied by the elapsed time between two
/// frames. If the "forceReinsert" parameter is true, we force a removal and reinsertion of the node
/// (this can be useful if the shape AABB has become much smaller than the previous one for instance).
bool DynamicAABBTree::updateObject(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()");
@ -192,19 +173,19 @@ bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector
mNodes[nodeID].aabb.mMaxCoordinates += gap;
// 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;
}
else {
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;
}
else {
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;
}
else {
@ -213,7 +194,7 @@ bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector
assert(mNodes[nodeID].aabb.contains(newAABB));
// Reinsert the node into the tree
// Reinsert the node int32_to the tree
insertLeafNode(nodeID);
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
// in the dynamic tree is described in the book "Introduction to Game Physics
// with Box2D" by Ian Parberry.
void DynamicAABBTree::insertLeafNode(int nodeID) {
void DynamicAABBTree::insertLeafNode(int32_t nodeID) {
// If the tree is empty
if (mRootNodeID == TreeNode::NULL_TREE_NODE) {
@ -235,53 +216,53 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
// Find the best sibling node for the new node
AABB newNodeAABB = mNodes[nodeID].aabb;
int currentNodeID = mRootNodeID;
int32_t currentNodeID = mRootNodeID;
while (!mNodes[currentNodeID].isLeaf()) {
int leftChild = mNodes[currentNodeID].children[0];
int rightChild = mNodes[currentNodeID].children[1];
int32_t leftChild = mNodes[currentNodeID].children[0];
int32_t rightChild = mNodes[currentNodeID].children[1];
// Compute the merged AABB
decimal volumeAABB = mNodes[currentNodeID].aabb.getVolume();
float volumeAABB = mNodes[currentNodeID].aabb.getVolume();
AABB mergedAABBs;
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
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)
decimal costI = decimal(2.0) * (mergedVolume - volumeAABB);
float costI = float(2.0) * (mergedVolume - volumeAABB);
// Compute the cost of descending into the left child
decimal costLeft;
// Compute the cost of descending int32_to the left child
float costLeft;
AABB currentAndLeftAABB;
currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, mNodes[leftChild].aabb);
if (mNodes[leftChild].isLeaf()) { // If the left child is a leaf
costLeft = currentAndLeftAABB.getVolume() + costI;
}
else {
decimal leftChildVolume = mNodes[leftChild].aabb.getVolume();
float leftChildVolume = mNodes[leftChild].aabb.getVolume();
costLeft = costI + currentAndLeftAABB.getVolume() - leftChildVolume;
}
// Compute the cost of descending into the right child
decimal costRight;
// Compute the cost of descending int32_to the right child
float costRight;
AABB currentAndRightAABB;
currentAndRightAABB.mergeTwoAABBs(newNodeAABB, mNodes[rightChild].aabb);
if (mNodes[rightChild].isLeaf()) { // If the right child is a leaf
costRight = currentAndRightAABB.getVolume() + costI;
}
else {
decimal rightChildVolume = mNodes[rightChild].aabb.getVolume();
float rightChildVolume = mNodes[rightChild].aabb.getVolume();
costRight = costI + currentAndRightAABB.getVolume() - rightChildVolume;
}
// 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;
// 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) {
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
int oldParentNode = mNodes[siblingNode].parentID;
int newParentNode = allocateNode();
int32_t oldParentNode = mNodes[siblingNode].parentID;
int32_t newParentNode = allocateNode();
mNodes[newParentNode].parentID = oldParentNode;
mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB);
mNodes[newParentNode].height = mNodes[siblingNode].height + 1;
@ -332,8 +313,8 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
assert(mNodes[nodeID].isLeaf());
assert(!mNodes[currentNodeID].isLeaf());
int leftChild = mNodes[currentNodeID].children[0];
int rightChild = mNodes[currentNodeID].children[1];
int32_t leftChild = mNodes[currentNodeID].children[0];
int32_t rightChild = mNodes[currentNodeID].children[1];
assert(leftChild != 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
void DynamicAABBTree::removeLeafNode(int nodeID) {
void DynamicAABBTree::removeLeafNode(int32_t nodeID) {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf());
@ -363,9 +344,9 @@ void DynamicAABBTree::removeLeafNode(int nodeID) {
return;
}
int parentNodeID = mNodes[nodeID].parentID;
int grandParentNodeID = mNodes[parentNodeID].parentID;
int siblingNodeID;
int32_t parentNodeID = mNodes[nodeID].parentID;
int32_t grandParentNodeID = mNodes[parentNodeID].parentID;
int32_t siblingNodeID;
if (mNodes[parentNodeID].children[0] == nodeID) {
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
// and make sure that the tree is still balanced
int currentNodeID = grandParentNodeID;
int32_t currentNodeID = grandParentNodeID;
while(currentNodeID != TreeNode::NULL_TREE_NODE) {
// Balance the current sub-tree if necessary
@ -398,8 +379,8 @@ void DynamicAABBTree::removeLeafNode(int nodeID) {
assert(!mNodes[currentNodeID].isLeaf());
// Get the two children of the current node
int leftChildID = mNodes[currentNodeID].children[0];
int rightChildID = mNodes[currentNodeID].children[1];
int32_t leftChildID = mNodes[currentNodeID].children[0];
int32_t rightChildID = mNodes[currentNodeID].children[1];
// Recompute the AABB and the height of the current node
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.
/// 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.
int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) {
assert(nodeID != TreeNode::NULL_TREE_NODE);
@ -437,23 +418,23 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
}
// Get the two children nodes
int nodeBID = nodeA->children[0];
int nodeCID = nodeA->children[1];
int32_t nodeBID = nodeA->children[0];
int32_t nodeCID = nodeA->children[1];
assert(nodeBID >= 0 && nodeBID < mNbAllocatedNodes);
assert(nodeCID >= 0 && nodeCID < mNbAllocatedNodes);
TreeNode* nodeB = mNodes + nodeBID;
TreeNode* nodeC = mNodes + nodeCID;
// 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 (balanceFactor > 1) {
assert(!nodeC->isLeaf());
int nodeFID = nodeC->children[0];
int nodeGID = nodeC->children[1];
int32_t nodeFID = nodeC->children[0];
int32_t nodeGID = nodeC->children[1];
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
TreeNode* nodeF = mNodes + nodeFID;
@ -522,8 +503,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
assert(!nodeB->isLeaf());
int nodeFID = nodeB->children[0];
int nodeGID = nodeB->children[1];
int32_t nodeFID = nodeB->children[0];
int32_t nodeGID = nodeB->children[1];
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
TreeNode* nodeF = mNodes + nodeFID;
@ -596,14 +577,14 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb,
DynamicAABBTreeOverlapCallback& callback) const {
// Create a stack with the nodes to visit
Stack<int, 64> stack;
Stack<int32_t, 64> stack;
stack.push(mRootNodeID);
// While there are still nodes to visit
while(stack.getNbElements() > 0) {
// Get the next node ID to visit
int nodeIDToVisit = stack.pop();
int32_t nodeIDToVisit = stack.pop();
// Skip it if it is a null node
if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) continue;
@ -635,9 +616,9 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &ca
PROFILE("DynamicAABBTree::raycast()");
decimal maxFraction = ray.maxFraction;
float maxFraction = ray.maxFraction;
Stack<int, 128> stack;
Stack<int32_t, 128> stack;
stack.push(mRootNodeID);
// 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) {
// 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 (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);
// 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 the node is a leaf of the tree
if (node->isLeaf()) {
// 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
// the raycasting should stop here
if (hitFraction == decimal(0.0)) {
if (hitFraction == float(0.0)) {
return;
}
// 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
// AABB using the new maximum fraction
@ -700,8 +681,8 @@ void DynamicAABBTree::check() const {
// Recursively check each node
checkNode(mRootNodeID);
int nbFreeNodes = 0;
int freeNodeID = mFreeNodeID;
int32_t nbFreeNodes = 0;
int32_t freeNodeID = mFreeNodeID;
// Check the free nodes
while(freeNodeID != TreeNode::NULL_TREE_NODE) {
@ -714,7 +695,7 @@ void DynamicAABBTree::check() const {
}
// 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;
@ -726,8 +707,8 @@ void DynamicAABBTree::checkNode(int nodeID) const {
// Get the children nodes
TreeNode* pNode = mNodes + nodeID;
assert(!pNode->isLeaf());
int leftChild = pNode->children[0];
int rightChild = pNode->children[1];
int32_t leftChild = pNode->children[0];
int32_t rightChild = pNode->children[1];
assert(pNode->height >= 0);
assert(pNode->aabb.getVolume() > 0);
@ -751,7 +732,7 @@ void DynamicAABBTree::checkNode(int nodeID) const {
assert(mNodes[rightChild].parentID == nodeID);
// 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);
// Check the AABB of the node
@ -767,12 +748,12 @@ void DynamicAABBTree::checkNode(int nodeID) const {
}
// Compute the height of the tree
int DynamicAABBTree::computeHeight() {
int32_t DynamicAABBTree::computeHeight() {
return computeHeight(mRootNodeID);
}
// 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);
TreeNode* node = mNodes + nodeID;
@ -782,8 +763,8 @@ int DynamicAABBTree::computeHeight(int nodeID) {
}
// Compute the height of the left and right sub-tree
int leftHeight = computeHeight(node->children[0]);
int rightHeight = computeHeight(node->children[1]);
int32_t leftHeight = computeHeight(node->children[0]);
int32_t rightHeight = computeHeight(node->children[1]);
// Return the height of the node
return 1 + std::max(leftHeight, rightHeight);

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_DYNAMIC_AABB_TREE_H
#define REACTPHYSICS3D_DYNAMIC_AABB_TREE_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/configuration.h>
@ -49,7 +28,7 @@ struct TreeNode {
// -------------------- Constants -------------------- //
/// Null tree node constant
const static int NULL_TREE_NODE;
const static int32_t NULL_TREE_NODE;
// -------------------- Attributes -------------------- //
@ -58,27 +37,27 @@ struct TreeNode {
union {
/// Parent node ID
int32 parentID;
int32_t parentID;
/// 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 {
/// 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)
union {
int32 dataInt[2];
int32_t dataInt[2];
void* dataPointer;
};
};
/// Height of the node in the tree
int16 height;
int16_t height;
/// Fat axis aligned bounding box (AABB) corresponding to the node
AABB aabb;
@ -101,7 +80,7 @@ class DynamicAABBTreeOverlapCallback {
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int nodeId)=0;
virtual void notifyOverlappingNode(int32_t nodeId)=0;
};
// Class DynamicAABBTreeRaycastCallback
@ -115,7 +94,7 @@ class DynamicAABBTreeRaycastCallback {
virtual ~DynamicAABBTreeRaycastCallback() = default;
// 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;
/// 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
int mFreeNodeID;
int32_t mFreeNodeID;
/// Number of allocated nodes in the tree
int mNbAllocatedNodes;
int32_t mNbAllocatedNodes;
/// 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
/// without triggering a large modification of the tree which can be costly
decimal mExtraAABBGap;
float mExtraAABBGap;
// -------------------- Methods -------------------- //
/// Allocate and return a node to use in the tree
int allocateNode();
int32_t allocateNode();
/// Release a node
void releaseNode(int nodeID);
void releaseNode(int32_t nodeID);
/// Insert a leaf node in the tree
void insertLeafNode(int nodeID);
void insertLeafNode(int32_t nodeID);
/// 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.
int balanceSubTreeAtNode(int nodeID);
int32_t balanceSubTreeAtNode(int32_t nodeID);
/// 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
int addObjectInternal(const AABB& aabb);
/// Internally add an object int32_to the tree
int32_t addObjectInternal(const AABB& aabb);
/// Initialize the tree
void init();
@ -184,7 +163,7 @@ class DynamicAABBTree {
void check() const;
/// Check if the node structure is valid (for debugging purpose)
void checkNode(int nodeID) const;
void checkNode(int32_t nodeID) const;
#endif
@ -193,31 +172,31 @@ class DynamicAABBTree {
// -------------------- Methods -------------------- //
/// Constructor
DynamicAABBTree(decimal extraAABBGap = decimal(0.0));
DynamicAABBTree(float extraAABBGap = float(0.0));
/// Destructor
virtual ~DynamicAABBTree();
/// Add an object into the tree (where node data are two integers)
int addObject(const AABB& aabb, int32 data1, int32 data2);
/// Add an object int32_to the tree (where node data are two int32_tegers)
int32_t addObject(const AABB& aabb, int32_t data1, int32_t data2);
/// Add an object into the tree (where node data is a pointer)
int addObject(const AABB& aabb, void* data);
/// Add an object int32_to the tree (where node data is a pointer)
int32_t addObject(const AABB& aabb, void* data);
/// Remove an object from the tree
void removeObject(int nodeID);
void removeObject(int32_t nodeID);
/// 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
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
int32* getNodeDataInt(int nodeID) const;
int32_t* getNodeDataInt(int32_t nodeID) const;
/// 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.
void reportAllShapesOverlappingWithAABB(const AABB& aabb,
@ -227,7 +206,7 @@ class DynamicAABBTree {
void raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& callback) const;
/// Compute the height of the tree
int computeHeight();
int32_t computeHeight();
/// Return the root AABB of the tree
AABB getRootAABB() const;
@ -242,20 +221,20 @@ inline bool TreeNode::isLeaf() const {
}
// 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);
return mNodes[nodeID].aabb;
}
// 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(mNodes[nodeID].isLeaf());
return mNodes[nodeID].dataInt;
}
// 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(mNodes[nodeID].isLeaf());
return mNodes[nodeID].dataPointer;
@ -266,11 +245,11 @@ inline AABB DynamicAABBTree::getRootAABB() const {
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.
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[1] = data2;
@ -278,11 +257,11 @@ inline int DynamicAABBTree::addObject(const AABB& aabb, int32 data1, int32 data2
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.
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;
@ -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 @@
/********************************************************************************
* 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_COLLISION_DISPATCH_H
#define REACTPHYSICS3D_COLLISION_DISPATCH_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h>
@ -52,16 +31,14 @@ class CollisionDispatch {
/// Initialize the collision dispatch configuration
virtual void init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator) {
}
/// Select and return the narrow-phase collision detection algorithm to
/// use between two types of collision shapes.
virtual NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type,
int shape2Type)=0;
virtual NarrowPhaseAlgorithm* selectAlgorithm(int32_t shape1Type,
int32_t shape2Type)=0;
};
}
#endif

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

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/shapes/ConcaveShape.h>
@ -107,7 +88,7 @@ void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Inf
void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) {
// Create a triangle collision shape
decimal margin = mConcaveShape->getTriangleMargin();
float margin = mConcaveShape->getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
// 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
// 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,
std::vector<SmoothMeshContactInfo> contactPoints,
NarrowPhaseCallback* narrowPhaseCallback) {
// 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
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;
// Compute the barycentric coordinates of the point in the triangle
decimal u, v, w;
float u, v, w;
computeBarycentricCoordinatesInTriangle(info.triangleVertices[0],
info.triangleVertices[1],
info.triangleVertices[2],
contactPoint, u, v, w);
int nbZeros = 0;
int32_t nbZeros = 0;
bool isUZero = approxEqual(u, 0, 0.0001);
bool isVZero = approxEqual(v, 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
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);
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]);
// 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
mContactPoints.push_back(smoothContactInfo);
}

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_CONCAVE_VS_CONVEX_ALGORITHM_H
#define REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h>
@ -199,12 +178,12 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
std::vector<SmoothMeshContactInfo> contactPoints,
NarrowPhaseCallback* narrowPhaseCallback);
/// Add a triangle vertex into the set of processed triangles
void addProcessedVertex(std::unordered_multimap<int, Vector3>& processTriangleVertices,
/// Add a triangle vertex int32_to the set of processed triangles
void addProcessedVertex(std::unordered_multimap<int32_t, Vector3>& processTriangleVertices,
const Vector3& vertex);
/// 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;
public :
@ -223,12 +202,10 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
NarrowPhaseCallback* narrowPhaseCallback);
};
// Add a triangle vertex into the set of processed triangles
inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap<int, Vector3>& processTriangleVertices, const Vector3& vertex) {
processTriangleVertices.insert(std::make_pair(int(vertex.x * vertex.y * vertex.z), vertex));
// Add a triangle vertex int32_to the set of processed triangles
inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap<int32_t, Vector3>& processTriangleVertices, const Vector3& vertex) {
processTriangleVertices.insert(std::make_pair(int32_t(vertex.x * vertex.y * vertex.z), vertex));
}
}
#endif

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

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#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
// 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 shape2Type = static_cast<CollisionShapeType>(type2);
// Sphere vs Sphere algorithm
if (shape1Type == SPHERE && shape2Type == SPHERE) {
return &mSphereVsSphereAlgorithm;
}
} else if ( (!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))
|| (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
// Concave vs Convex algorithm
else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) ||
(!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
return &mConcaveVsConvexAlgorithm;
}
} else if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) {
// Convex vs Convex algorithm (GJK algorithm)
else if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) {
return &mGJKAlgorithm;
}
else {
return NULL;
} else {
return nullptr;
}
}

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_DEFAULT_COLLISION_DISPATCH_H
#define REACTPHYSICS3D_DEFAULT_COLLISION_DISPATCH_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/collision/narrowphase/CollisionDispatch.h>
@ -67,12 +46,9 @@ class DefaultCollisionDispatch : public CollisionDispatch {
/// Select and return the narrow-phase collision detection algorithm to
/// 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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/narrowphase/EPA/EPAAlgorithm.h>
@ -45,7 +26,7 @@ EPAAlgorithm::~EPAAlgorithm() {
// Decide if the origin is in the tetrahedron.
/// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of
/// the vertex that is wrong if the origin is not in the tetrahedron
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 {
// Check vertex 1
@ -79,7 +60,7 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
// Compute the penetration depth with the EPA algorithm.
/// This method computes the penetration depth and contact points between two
/// 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
/// the correct penetration depth
void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex,
@ -113,18 +94,18 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
Transform body2Tobody1 = transform1.getInverse() * transform2;
// 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() *
transform1.getOrientation();
// 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
decimal tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint();
float tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint();
// Number of triangles in the polytope
unsigned int nbTriangles = 0;
uint32_t nbTriangles = 0;
// Clear the storing of triangles
triangleStore.clear();
@ -152,10 +133,10 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
Vector3 d = (points[1] - points[0]).getUnit();
// 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)
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
// v2 and v3
@ -217,7 +198,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// where the GJK algorithm compute a simplex of three vertices.
// 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 (badVertex == 0) {
@ -357,7 +338,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
}
TriangleEPA* triangle = 0;
decimal upperBoundSquarePenDepth = DECIMAL_LARGEST;
float upperBoundSquarePenDepth = DECIMAL_LARGEST;
do {
triangle = triangleHeap[0];
@ -383,19 +364,19 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
(-triangle->getClosestPoint()), shape2CachedCollisionData);
points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
int indexNewVertex = nbVertices;
int32_t indexNewVertex = nbVertices;
nbVertices++;
// 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);
decimal wDotVSquare = wDotv * wDotv / triangle->getDistSquare();
float wDotVSquare = wDotv * wDotv / triangle->getDistSquare();
if (wDotVSquare < upperBoundSquarePenDepth) {
upperBoundSquarePenDepth = wDotVSquare;
}
// Compute the error
decimal error = wDotv - triangle->getDistSquare();
float error = wDotv - triangle->getDistSquare();
if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) ||
points[indexNewVertex] == points[(*triangle)[0]] ||
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
// face will not be in the convex hull. We start the local recursive silhouette
// algorithm from the current triangle face.
int i = triangleStore.getNbTriangles();
int32_t i = triangleStore.getNbTriangles();
if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) {
break;
}
@ -426,7 +407,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
Vector3 normal = v.getUnit();
decimal penetrationDepth = v.length();
float penetrationDepth = v.length();
assert(penetrationDepth > 0.0);
if (normal.lengthSquare() < MACHINE_EPSILON) return;

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_EPA_ALGORITHM_H
#define REACTPHYSICS3D_EPA_ALGORITHM_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/collision/narrowphase/GJK/Simplex.h>
@ -43,10 +22,10 @@ namespace reactphysics3d {
// ---------- Constants ---------- //
/// 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
const unsigned int MAX_FACETS = 200;
const uint32_t MAX_FACETS = 200;
// Class TriangleComparison
@ -72,7 +51,7 @@ class TriangleComparison {
* This class is the implementation of the Expanding Polytope Algorithm (EPA).
* The EPA algorithm computes the penetration depth and contact points between
* 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
* 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
@ -102,11 +81,11 @@ class EPAAlgorithm {
EPAAlgorithm& operator=(const EPAAlgorithm& algorithm);
/// Add a triangle face in the candidate triangle heap
void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, uint& nbTriangles,
decimal upperBoundSquarePenDepth);
void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, uint32_t& nbTriangles,
float upperBoundSquarePenDepth);
/// 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;
public:
@ -134,10 +113,10 @@ class EPAAlgorithm {
// Add a triangle face in the candidate triangle heap in the EPA algorithm
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
// 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
// penetration depth upper bound
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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/narrowphase/EPA/EdgeEPA.h>
@ -39,7 +20,7 @@ EdgeEPA::EdgeEPA() {
}
// Constructor
EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int index)
EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int32_t index)
: mOwnerTriangle(ownerTriangle), mIndex(index) {
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)
uint EdgeEPA::getSourceVertexIndex() const {
uint32_t EdgeEPA::getSourceVertexIndex() const {
return (*mOwnerTriangle)[mIndex];
}
// 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)];
}
// 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) {
// If the edge has not already been visited
if (!mOwnerTriangle->getIsObsolete()) {
@ -90,7 +71,7 @@ bool EdgeEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex,
// The current triangle is visible and therefore obsolete
mOwnerTriangle->setIsObsolete(true);
int backup = triangleStore.getNbTriangles();
int32_t backup = triangleStore.getNbTriangles();
if(!mOwnerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(
this->mIndex)).computeSilhouette(vertices,

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_EDGE_EPA_H
#define REACTPHYSICS3D_EDGE_EPA_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
@ -52,7 +31,7 @@ class EdgeEPA {
/// Index of the edge in the triangle (between 0 and 2).
/// The edge with index i connect triangle vertices i and (i+1 % 3)
int mIndex;
int32_t mIndex;
public:
@ -62,7 +41,7 @@ class EdgeEPA {
EdgeEPA();
/// Constructor
EdgeEPA(TriangleEPA* ownerTriangle, int index);
EdgeEPA(TriangleEPA* ownerTriangle, int32_t index);
/// Copy-constructor
EdgeEPA(const EdgeEPA& edge);
@ -74,16 +53,16 @@ class EdgeEPA {
TriangleEPA* getOwnerTriangle() const;
/// 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
uint getSourceVertexIndex() const;
uint32_t getSourceVertexIndex() const;
/// 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
bool computeSilhouette(const Vector3* vertices, uint index, TrianglesStore& triangleStore);
bool computeSilhouette(const Vector3* vertices, uint32_t index, TrianglesStore& triangleStore);
/// Assignment operator
EdgeEPA& operator=(const EdgeEPA& edge);
@ -95,7 +74,7 @@ inline TriangleEPA* EdgeEPA::getOwnerTriangle() const {
}
// Return the edge index
inline int EdgeEPA::getIndex() const {
inline int32_t EdgeEPA::getIndex() const {
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
inline int indexOfNextCounterClockwiseEdge(int i) {
inline int32_t indexOfNextCounterClockwiseEdge(int32_t i) {
return (i + 1) % 3;
}
// 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;
}
}
#endif

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

@ -1,28 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/narrowphase/EPA/TriangleEPA.h>
@ -38,7 +18,7 @@ TriangleEPA::TriangleEPA() {
}
// Constructor
TriangleEPA::TriangleEPA(uint indexVertex1, uint indexVertex2, uint indexVertex3)
TriangleEPA::TriangleEPA(uint32_t indexVertex1, uint32_t indexVertex2, uint32_t indexVertex3)
: mIsObsolete(false) {
mIndicesVertices[0] = indexVertex1;
mIndicesVertices[1] = indexVertex2;
@ -56,11 +36,11 @@ bool TriangleEPA::computeClosestPoint(const Vector3* vertices) {
Vector3 v1 = vertices[mIndicesVertices[1]] - p0;
Vector3 v2 = vertices[mIndicesVertices[2]] - p0;
decimal v1Dotv1 = v1.dot(v1);
decimal v1Dotv2 = v1.dot(v2);
decimal v2Dotv2 = v2.dot(v2);
decimal p0Dotv1 = p0.dot(v1);
decimal p0Dotv2 = p0.dot(v2);
float v1Dotv1 = v1.dot(v1);
float v1Dotv2 = v1.dot(v2);
float v2Dotv2 = v2.dot(v2);
float p0Dotv1 = p0.dot(v1);
float p0Dotv2 = p0.dot(v2);
// Compute determinant
mDet = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2;
@ -72,7 +52,7 @@ bool TriangleEPA::computeClosestPoint(const Vector3* vertices) {
// If the determinant is positive
if (mDet > 0.0) {
// Compute the closest point v
mClosestPoint = p0 + 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
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
/// order that we always have a convex polytope. The faces visible from the new vertex are set
/// obselete and will not be considered as being a candidate face in the future.
bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex,
bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint32_t indexNewVertex,
TrianglesStore& triangleStore) {
uint first = triangleStore.getNbTriangles();
uint32_t first = triangleStore.getNbTriangles();
// Mark the current triangle as obsolete because it
setIsObsolete(true);
@ -136,7 +116,7 @@ bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex
mAdjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore);
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 (i=first, j=triangleStore.getNbTriangles()-1;

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_TRIANGLE_EPA_H
#define REACTPHYSICS3D_TRIANGLE_EPA_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/mathematics/mathematics.h>
@ -51,7 +30,7 @@ class TriangleEPA {
// -------------------- Attributes -------------------- //
/// 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)
EdgeEPA mAdjacentEdges[3];
@ -60,19 +39,19 @@ class TriangleEPA {
bool mIsObsolete;
/// Determinant
decimal mDet;
float mDet;
/// Point v closest to the origin on the affine hull of the triangle
Vector3 mClosestPoint;
/// 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
decimal mLambda2;
float mLambda2;
/// Square distance of the point closest point v to the origin
decimal mDistSquare;
float mDistSquare;
// -------------------- Methods -------------------- //
@ -90,19 +69,19 @@ class TriangleEPA {
TriangleEPA();
/// Constructor
TriangleEPA(uint v1, uint v2, uint v3);
TriangleEPA(uint32_t v1, uint32_t v2, uint32_t v3);
/// Destructor
~TriangleEPA();
/// Return an adjacent edge of the triangle
EdgeEPA& getAdjacentEdge(int index);
EdgeEPA& getAdjacentEdge(int32_t index);
/// 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
decimal getDistSquare() const;
float getDistSquare() const;
/// Set the isObsolete value
void setIsObsolete(bool isObsolete);
@ -117,7 +96,7 @@ class TriangleEPA {
bool isClosestPointInternalToTriangle() const;
/// 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
bool computeClosestPoint(const Vector3* vertices);
@ -126,10 +105,10 @@ class TriangleEPA {
Vector3 computeClosestPointOfObject(const Vector3* supportPointsOfObject) const;
/// 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
uint operator[](int i) const;
uint32_t operator[](int32_t i) const;
/// Associate two edges
friend bool link(const EdgeEPA& edge0, const EdgeEPA& edge1);
@ -139,19 +118,19 @@ class TriangleEPA {
};
// Return an edge of the triangle
inline EdgeEPA& TriangleEPA::getAdjacentEdge(int index) {
inline EdgeEPA& TriangleEPA::getAdjacentEdge(int32_t index) {
assert(index >= 0 && index < 3);
return mAdjacentEdges[index];
}
// 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);
mAdjacentEdges[index] = edge;
}
// Return the square distance of the closest point to origin
inline decimal TriangleEPA::getDistSquare() const {
inline float TriangleEPA::getDistSquare() const {
return mDistSquare;
}
@ -176,7 +155,7 @@ inline bool TriangleEPA::isClosestPointInternalToTriangle() const {
}
// 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;
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
inline Vector3 TriangleEPA::computeClosestPointOfObject(const Vector3* supportPointsOfObject) const{
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));
}
// Access operator
inline uint TriangleEPA::operator[](int i) const {
inline uint32_t TriangleEPA::operator[](int32_t i) const {
assert(i >= 0 && i <3);
return mIndicesVertices[i];
}
}
#endif

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

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/narrowphase/EPA/TrianglesStore.h>

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

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

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

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.h>
@ -49,9 +30,9 @@ GJKAlgorithm::~GJKAlgorithm() {
// Compute a contact info if the two collision shapes collide.
/// 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
/// (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
/// 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
@ -67,8 +48,8 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
Vector3 w; // Support point of Minkowski difference A-B
Vector3 pA; // Closest point of object A
Vector3 pB; // Closest point of object B
decimal vDotw;
decimal prevDistSquare;
float vDotw;
float prevDistSquare;
assert(shape1Info.collisionShape->isConvex());
assert(shape2Info.collisionShape->isConvex());
@ -88,13 +69,13 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
Transform body2Tobody1 = transform1.getInverse() * transform2;
// 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() *
transform1.getOrientation().getMatrix();
// Initialize the margin (sum of margins of both objects)
decimal margin = shape1->getMargin() + shape2->getMargin();
decimal marginSquare = margin * margin;
float margin = shape1->getMargin() + shape2->getMargin();
float marginSquare = margin * margin;
assert(margin > 0.0);
// Create a simplex set
@ -104,7 +85,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis();
// Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_LARGEST;
float distSquare = DECIMAL_LARGEST;
do {
@ -118,17 +99,17 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
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) {
// Cache the current separating axis for frame coherence
mCurrentOverlappingPair->setCachedSeparatingAxis(v);
// No intersection, we return
// No int32_tersection, we 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) {
// 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
// object with the margins
decimal dist = sqrt(distSquare);
float dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
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)
if (penetrationDepth <= 0.0) return;
@ -154,7 +135,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return
// There is an int32_tersection, therefore we 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
// object with the margins
decimal dist = sqrt(distSquare);
float dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
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)
if (penetrationDepth <= 0.0) return;
@ -187,7 +168,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return
// There is an int32_tersection, therefore we 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
// object with the margins
decimal dist = sqrt(distSquare);
float dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
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)
if (penetrationDepth <= 0.0) return;
@ -218,7 +199,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return
// There is an int32_tersection, therefore we 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
// object with the margins
decimal dist = sqrt(distSquare);
float dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
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)
if (penetrationDepth <= 0.0) return;
@ -256,13 +237,13 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return
// There is an int32_tersection, therefore we return
return;
}
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
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
// 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.
@ -272,7 +253,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
/// 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
/// 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
/// compute the correct penetration depth and contact points of the enlarged objects.
void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
@ -287,9 +268,9 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
Vector3 suppA;
Vector3 suppB;
Vector3 w;
decimal vDotw;
decimal distSquare = DECIMAL_LARGEST;
decimal prevDistSquare;
float vDotw;
float distSquare = DECIMAL_LARGEST;
float prevDistSquare;
assert(shape1Info.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)
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() *
transform1.getOrientation().getMatrix();
@ -318,10 +299,10 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
vDotw = v.dot(w);
// If the enlarge objects do not intersect
// If the enlarge objects do not int32_tersect
if (vDotw > 0.0) {
// No intersection, we return
// No int32_tersection, we return
return;
}
@ -360,7 +341,7 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
Vector3 suppA; // Support point of object A
Vector3 w; // Support point of Minkowski difference A-B
decimal prevDistSquare;
float prevDistSquare;
assert(proxyShape->getCollisionShape()->isConvex());
@ -378,7 +359,7 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
Vector3 v(1, 1, 1);
// Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_LARGEST;
float distSquare = DECIMAL_LARGEST;
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 suppB; // Support point on the collision shape
const decimal machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON;
const decimal epsilon = decimal(0.0001);
const float machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON;
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;
// 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
Simplex simplex;
Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0));
decimal lambda = decimal(0.0);
Vector3 n(float(0.0), float(0.0), float(0.0));
float lambda = float(0.0);
suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin)
suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData);
Vector3 v = suppA - suppB;
decimal vDotW, vDotR;
decimal distSquare = v.lengthSquare();
int nbIterations = 0;
float vDotW, vDotR;
float distSquare = v.lengthSquare();
int32_t nbIterations = 0;
// GJK Algorithm loop
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);
if (vDotW > decimal(0)) {
if (vDotW > float(0)) {
vDotR = v.dot(rayDirection);
@ -494,7 +475,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
distSquare = v.lengthSquare();
}
else {
distSquare = decimal(0.0);
distSquare = float(0.0);
}
// 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;
}
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;

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_GJK_ALGORITHM_H
#define REACTPHYSICS3D_GJK_ALGORITHM_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h>
@ -37,9 +16,9 @@
namespace reactphysics3d {
// Constants
const decimal REL_ERROR = decimal(1.0e-3);
const decimal REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
const int MAX_ITERATIONS_GJK_RAYCAST = 32;
const float REL_ERROR = float(1.0e-3);
const float REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
const int32_t MAX_ITERATIONS_GJK_RAYCAST = 32;
// Class GJKAlgorithm
/**
@ -49,9 +28,9 @@ const int MAX_ITERATIONS_GJK_RAYCAST = 32;
* "Collision Detection in Interactive 3D Environments" by Gino van den Bergen.
* This method implements the Hybrid Technique for calculating the
* 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).
* 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
* polytope that contains the origin and give it to the EPA (Expanding
* 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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#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
/// suppPointB : support point of object B in a direction v
/// 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);
// Add the point into the simplex
// Add the point int32_to the simplex
mPoints[mLastFound] = point;
mPointsLengthSquare[mLastFound] = point.dot(point);
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
bool Simplex::isPointInSimplex(const Vector3& point) const {
int i;
int32_t i;
Bits bit;
// 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
void Simplex::updateCache() {
int i;
int32_t i;
Bits bit;
// For each of the four possible points of the simplex
@ -112,10 +93,10 @@ void Simplex::updateCache() {
}
// 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 {
unsigned int nbVertices = 0;
int i;
uint32_t nbVertices = 0;
int32_t i;
Bits bit;
// For each four point in the possible simplex
@ -143,7 +124,7 @@ void Simplex::computeDeterminants() {
// If the current simplex is not empty
if (!isEmpty()) {
int i;
int32_t i;
Bits bitI;
// 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]);
int j;
int32_t j;
Bits bitJ;
for (j=0, bitJ = 0x1; j<i; j++, bitJ <<= 1) {
if (overlap(mBitsCurrentSimplex, bitJ)) {
int k;
int32_t k;
Bits bit3 = bitJ | bit2;
k = mNormSquare[i][j] < mNormSquare[mLastFound][j] ? i : mLastFound;
@ -187,7 +168,7 @@ void Simplex::computeDeterminants() {
}
if (mAllBits == 0xf) {
int k;
int32_t k;
k = mNormSquare[1][0] < mNormSquare[2][0] ?
(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
/// detX_i value bigger than zero
bool Simplex::isProperSubset(Bits subset) const {
int i;
int32_t i;
Bits bit;
// 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
/// is an affine combination of other points in the set
bool Simplex::isAffinelyDependent() const {
decimal sum = 0.0;
int i;
float sum = 0.0;
int32_t i;
Bits bit;
// 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
/// 2. delta(X U {y_j})_j <= 0 for each "j" not in I_x_
bool Simplex::isValidSubset(Bits subset) const {
int i;
int32_t i;
Bits bit;
// 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
/// with lambda_i = deltaX_i / deltaX
void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {
decimal deltaX = 0.0;
float deltaX = 0.0;
pA.setAllValues(0.0, 0.0, 0.0);
pB.setAllValues(0.0, 0.0, 0.0);
int i;
int32_t i;
Bits bit;
// 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);
decimal factor = decimal(1.0) / deltaX;
float factor = float(1.0) / deltaX;
pA *= factor;
pB *= factor;
}
@ -346,13 +327,13 @@ bool Simplex::computeClosestPoint(Vector3& v) {
// Backup the closest point
void Simplex::backupClosestPointInSimplex(Vector3& v) {
decimal minDistSquare = DECIMAL_LARGEST;
float minDistSquare = DECIMAL_LARGEST;
Bits bit;
for (bit = mAllBits; bit != 0x0; bit--) {
if (isSubset(bit, mAllBits) && isProperSubset(bit)) {
Vector3 u = computeClosestPointForSubset(bit);
decimal distSquare = u.dot(u);
float distSquare = u.dot(u);
if (distSquare < minDistSquare) {
minDistSquare = distSquare;
mBitsCurrentSimplex = bit;
@ -367,8 +348,8 @@ void Simplex::backupClosestPointInSimplex(Vector3& v) {
Vector3 Simplex::computeClosestPointForSubset(Bits subset) {
Vector3 v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i])
mMaxLengthSquare = 0.0;
decimal deltaX = 0.0; // deltaX = sum of all det[subset][i]
int i;
float deltaX = 0.0; // deltaX = sum of all det[subset][i]
int32_t i;
Bits bit;
// For each four point in the possible simplex set
@ -390,5 +371,5 @@ Vector3 Simplex::computeClosestPointForSubset(Bits subset) {
assert(deltaX > 0.0);
// Return the closet point "v" in the convex hull for the given subset
return (decimal(1.0) / deltaX) * v;
return (float(1.0) / deltaX) * v;
}

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_SIMPLEX_H
#define REACTPHYSICS3D_SIMPLEX_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/mathematics/mathematics.h>
@ -34,7 +13,7 @@
namespace reactphysics3d {
// Type definitions
typedef unsigned int Bits;
typedef uint32_t Bits;
// Class Simplex
/**
@ -55,10 +34,10 @@ class Simplex {
Vector3 mPoints[4];
/// pointsLengthSquare[i] = (points[i].length)^2
decimal mPointsLengthSquare[4];
float mPointsLengthSquare[4];
/// Maximum length of pointsLengthSquare[i]
decimal mMaxLengthSquare;
float mMaxLengthSquare;
/// Support points of object A in local coordinates
Vector3 mSuppPointsA[4];
@ -70,10 +49,10 @@ class Simplex {
Vector3 mDiffLength[4][4];
/// Cached determinant values
decimal mDet[16][4];
float mDet[16][4];
/// 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
/// For instance, 0101 means that points[1] and points[3] are in the simplex
@ -134,13 +113,13 @@ class Simplex {
bool isEmpty() const;
/// Return the points of the simplex
unsigned int getSimplex(Vector3* mSuppPointsA, Vector3* mSuppPointsB,
uint32_t getSimplex(Vector3* mSuppPointsA, Vector3* mSuppPointsB,
Vector3* mPoints) const;
/// 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);
/// 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
inline decimal Simplex::getMaxLengthSquareOfAPoint() const {
inline float Simplex::getMaxLengthSquareOfAPoint() const {
return mMaxLengthSquare;
}
}
#endif

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

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h>

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_NARROW_PHASE_ALGORITHM_H
#define REACTPHYSICS3D_NARROW_PHASE_ALGORITHM_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/narrowphase/SphereVsSphereAlgorithm.h>
@ -31,7 +12,8 @@
using namespace reactphysics3d;
// Constructor
SphereVsSphereAlgorithm::SphereVsSphereAlgorithm() : NarrowPhaseAlgorithm() {
SphereVsSphereAlgorithm::SphereVsSphereAlgorithm() :
NarrowPhaseAlgorithm() {
}
@ -43,37 +25,31 @@ SphereVsSphereAlgorithm::~SphereVsSphereAlgorithm() {
void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) {
// Get the sphere collision shapes
const SphereShape* sphereShape1 = static_cast<const SphereShape*>(shape1Info.collisionShape);
const SphereShape* sphereShape2 = static_cast<const SphereShape*>(shape2Info.collisionShape);
// Get the local-space to world-space transforms
const Transform& transform1 = shape1Info.shapeToWorldTransform;
const Transform& transform2 = shape2Info.shapeToWorldTransform;
// Compute the distance between the centers
Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
float squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
// Compute the sum of the radius
decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius();
// If the sphere collision shapes intersect
float sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius();
// If the sphere collision shapes int32_tersect
if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
Vector3 intersectionOnBody1 = sphereShape1->getRadius() *
Vector3 int32_tersectionOnBody1 = sphereShape1->getRadius() *
centerSphere2InBody1LocalSpace.getUnit();
Vector3 intersectionOnBody2 = sphereShape2->getRadius() *
Vector3 int32_tersectionOnBody2 = sphereShape2->getRadius() *
centerSphere1InBody2LocalSpace.getUnit();
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
float penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
// Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, vectorBetweenCenters.getUnit(), penetrationDepth,
intersectionOnBody1, intersectionOnBody2);
int32_tersectionOnBody1, int32_tersectionOnBody2);
// Notify about the new contact
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
}

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_SPHERE_VS_SPHERE_ALGORITHM_H
#define REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/shapes/AABB.h>
@ -114,7 +96,7 @@ AABB AABB::createAABBForTriangle(const Vector3* trianglePoints) {
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
/// Real-time Collision Detection by Christer Ericson.
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;
// 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;
decimal ady = std::abs(d.y);
float ady = std::abs(d.y);
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;
// Add in an epsilon term to counteract arithmetic errors when segment is
// (near) parallel to a coordinate axis (see text for detail)
const decimal epsilon = 0.00001;
const float epsilon = 0.00001;
adx += epsilon;
ady += epsilon;
adz += epsilon;

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_AABB_H
#define REACTPHYSICS3D_AABB_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/mathematics/mathematics.h>
@ -86,13 +65,13 @@ class AABB {
Vector3 getExtent() const;
/// 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
bool testCollision(const AABB& aabb) const;
/// Return the volume of the AABB
decimal getVolume() const;
float getVolume() const;
/// Merge the AABB in parameter with the current one
void mergeWithAABB(const AABB& aabb);
@ -106,10 +85,10 @@ class AABB {
/// Return true if a point is inside the AABB
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;
/// Return true if the ray intersects the AABB
/// Return true if the ray int32_tersects the AABB
bool testRayIntersect(const Ray& ray) const;
/// Create and return an AABB for a triangle
@ -125,7 +104,7 @@ class AABB {
// Return the center point of the AABB in world coordinates
inline Vector3 AABB::getCenter() const {
return (mMinCoordinates + mMaxCoordinates) * decimal(0.5);
return (mMinCoordinates + mMaxCoordinates) * float(0.5);
}
// 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
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);
mMinCoordinates -= Vector3(dx, dy, dz);
}
@ -172,12 +151,12 @@ inline bool AABB::testCollision(const AABB& aabb) const {
}
// Return the volume of the AABB
inline decimal AABB::getVolume() const {
inline float AABB::getVolume() const {
const Vector3 diff = mMaxCoordinates - mMinCoordinates;
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 {
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 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#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 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)) {
assert(extent.x > decimal(0.0) && extent.x > margin);
assert(extent.y > decimal(0.0) && extent.y > margin);
assert(extent.z > decimal(0.0) && extent.z > margin);
assert(extent.x > float(0.0) && extent.x > margin);
assert(extent.y > float(0.0) && extent.y > margin);
assert(extent.z > float(0.0) && extent.z > margin);
}
// Destructor
@ -55,12 +36,12 @@ BoxShape::~BoxShape() {
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal factor = (decimal(1.0) / decimal(3.0)) * mass;
void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
float factor = (float(1.0) / float(3.0)) * mass;
Vector3 realExtent = mExtent + Vector3(mMargin, mMargin, mMargin);
decimal xSquare = realExtent.x * realExtent.x;
decimal ySquare = realExtent.y * realExtent.y;
decimal zSquare = realExtent.z * realExtent.z;
float xSquare = realExtent.x * realExtent.x;
float ySquare = realExtent.y * realExtent.y;
float zSquare = realExtent.z * realExtent.z;
tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0,
0.0, factor * (xSquare + zSquare), 0.0,
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 {
Vector3 rayDirection = ray.point2 - ray.point1;
decimal tMin = DECIMAL_SMALLEST;
decimal tMax = DECIMAL_LARGEST;
Vector3 normalDirection(decimal(0), decimal(0), decimal(0));
float tMin = DECIMAL_SMALLEST;
float tMax = DECIMAL_LARGEST;
Vector3 normalDirection(float(0), float(0), float(0));
Vector3 currentNormal;
// 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 (std::abs(rayDirection[i]) < MACHINE_EPSILON) {
@ -86,22 +67,22 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro
}
else {
// Compute the intersection of the ray with the near and far plane of the slab
decimal oneOverD = decimal(1.0) / rayDirection[i];
decimal t1 = (-mExtent[i] - ray.point1[i]) * oneOverD;
decimal t2 = (mExtent[i] - ray.point1[i]) * oneOverD;
currentNormal[0] = (i == 0) ? -mExtent[i] : decimal(0.0);
currentNormal[1] = (i == 1) ? -mExtent[i] : decimal(0.0);
currentNormal[2] = (i == 2) ? -mExtent[i] : decimal(0.0);
// Compute the int32_tersection of the ray with the near and far plane of the slab
float oneOverD = float(1.0) / rayDirection[i];
float t1 = (-mExtent[i] - ray.point1[i]) * oneOverD;
float t2 = (mExtent[i] - ray.point1[i]) * oneOverD;
currentNormal[0] = (i == 0) ? -mExtent[i] : float(0.0);
currentNormal[1] = (i == 1) ? -mExtent[i] : float(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
if (t1 > t2) {
std::swap(t1, t2);
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) {
tMin = t1;
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 > 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 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;
raycastInfo.body = proxyShape->getBody();

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_BOX_SHAPE_H
#define REACTPHYSICS3D_BOX_SHAPE_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <cfloat>
@ -85,7 +64,7 @@ class BoxShape : public ConvexShape {
// -------------------- Methods -------------------- //
/// Constructor
BoxShape(const Vector3& extent, decimal margin = OBJECT_MARGIN);
BoxShape(const Vector3& extent, float margin = OBJECT_MARGIN);
/// Destructor
virtual ~BoxShape();
@ -100,7 +79,7 @@ class BoxShape : public ConvexShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// 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
@ -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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/shapes/CapsuleShape.h>
@ -36,10 +17,10 @@ using namespace reactphysics3d;
* @param radius The radius of the capsule (in meters)
* @param height The height of the capsule (in meters)
*/
CapsuleShape::CapsuleShape(decimal radius, decimal height)
: ConvexShape(CAPSULE, radius), mHalfHeight(height * decimal(0.5)) {
assert(radius > decimal(0.0));
assert(height > decimal(0.0));
CapsuleShape::CapsuleShape(float radius, float height)
: ConvexShape(CAPSULE, radius), mHalfHeight(height * float(0.5)) {
assert(radius > float(0.0));
assert(height > float(0.0));
}
// Destructor
@ -53,21 +34,21 @@ CapsuleShape::~CapsuleShape() {
* coordinates
* @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
decimal height = mHalfHeight + mHalfHeight;
decimal radiusSquare = mMargin * mMargin;
decimal heightSquare = height * height;
decimal radiusSquareDouble = radiusSquare + radiusSquare;
decimal factor1 = decimal(2.0) * mMargin / (decimal(4.0) * mMargin + decimal(3.0) * height);
decimal factor2 = decimal(3.0) * height / (decimal(4.0) * mMargin + decimal(3.0) * height);
decimal sum1 = decimal(0.4) * radiusSquareDouble;
decimal sum2 = decimal(0.75) * height * mMargin + decimal(0.5) * heightSquare;
decimal sum3 = decimal(0.25) * radiusSquare + decimal(1.0 / 12.0) * heightSquare;
decimal IxxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3;
decimal Iyy = factor1 * mass * sum1 + factor2 * mass * decimal(0.25) * radiusSquareDouble;
float height = mHalfHeight + mHalfHeight;
float radiusSquare = mMargin * mMargin;
float heightSquare = height * height;
float radiusSquareDouble = radiusSquare + radiusSquare;
float factor1 = float(2.0) * mMargin / (float(4.0) * mMargin + float(3.0) * height);
float factor2 = float(3.0) * height / (float(4.0) * mMargin + float(3.0) * height);
float sum1 = float(0.4) * radiusSquareDouble;
float sum2 = float(0.75) * height * mMargin + float(0.5) * heightSquare;
float sum3 = float(0.25) * radiusSquare + float(1.0 / 12.0) * heightSquare;
float IxxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3;
float Iyy = factor1 * mass * sum1 + factor2 * mass * float(0.25) * radiusSquareDouble;
tensor.setAllValues(IxxAndzz, 0.0, 0.0,
0.0, Iyy, 0.0,
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
bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
const decimal diffYCenterSphere1 = localPoint.y - mHalfHeight;
const decimal diffYCenterSphere2 = localPoint.y + mHalfHeight;
const decimal xSquare = localPoint.x * localPoint.x;
const decimal zSquare = localPoint.z * localPoint.z;
const decimal squareRadius = mMargin * mMargin;
const float diffYCenterSphere1 = localPoint.y - mHalfHeight;
const float diffYCenterSphere2 = localPoint.y + mHalfHeight;
const float xSquare = localPoint.x * localPoint.x;
const float zSquare = localPoint.z * localPoint.z;
const float squareRadius = mMargin * mMargin;
// Return true if the point is inside the cylinder or one of the two spheres of the capsule
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 decimal epsilon = decimal(0.01);
Vector3 p(decimal(0), -mHalfHeight, decimal(0));
Vector3 q(decimal(0), mHalfHeight, decimal(0));
const float epsilon = float(0.01);
Vector3 p(float(0), -mHalfHeight, float(0));
Vector3 q(float(0), mHalfHeight, float(0));
Vector3 d = q - p;
Vector3 m = ray.point1 - p;
decimal t;
float t;
decimal mDotD = m.dot(d);
decimal nDotD = n.dot(d);
decimal dDotD = d.dot(d);
float mDotD = m.dot(d);
float nDotD = n.dot(d);
float dDotD = d.dot(d);
// Test if the segment is outside the cylinder
decimal vec1DotD = (ray.point1 - Vector3(decimal(0.0), -mHalfHeight - mMargin, decimal(0.0))).dot(d);
if (vec1DotD < decimal(0.0) && vec1DotD + nDotD < decimal(0.0)) return false;
decimal ddotDExtraCaps = decimal(2.0) * mMargin * d.y;
float vec1DotD = (ray.point1 - Vector3(float(0.0), -mHalfHeight - mMargin, float(0.0))).dot(d);
if (vec1DotD < float(0.0) && vec1DotD + nDotD < float(0.0)) return false;
float ddotDExtraCaps = float(2.0) * mMargin * d.y;
if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) return false;
decimal nDotN = n.dot(n);
decimal mDotN = m.dot(n);
float nDotN = n.dot(n);
float mDotN = m.dot(n);
decimal a = dDotD * nDotN - nDotD * nDotD;
decimal k = m.dot(m) - mMargin * mMargin;
decimal c = dDotD * k - mDotD * mDotD;
float a = dDotD * nDotN - nDotD * nDotD;
float k = m.dot(m) - mMargin * mMargin;
float c = dDotD * k - mDotD * mDotD;
// If the ray is parallel to the capsule axis
if (std::abs(a) < epsilon) {
// If the origin is outside the surface of the capusle's cylinder, we return no hit
if (c > 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 (mDotD < decimal(0.0)) {
// If the ray int32_tersects with the "p" endcap of the capsule
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;
decimal hitFraction;
float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
@ -145,11 +126,11 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
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;
decimal hitFraction;
float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
@ -167,22 +148,22 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
return false;
}
}
decimal b = dDotD * mDotN - nDotD * mDotD;
decimal discriminant = b * b - a * c;
float b = dDotD * mDotN - nDotD * mDotD;
float discriminant = b * b - a * c;
// 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)
decimal t0 = t = (-b - std::sqrt(discriminant)) / a;
// Compute the smallest root (first int32_tersection along the ray)
float t0 = t = (-b - std::sqrt(discriminant)) / a;
// If the intersection is outside the finite cylinder of the capsule on "p" endcap side
decimal value = mDotD + t * nDotD;
if (value < decimal(0.0)) {
// If the int32_tersection is outside the finite cylinder of the capsule on "p" endcap side
float value = mDotD + t * nDotD;
if (value < float(0.0)) {
// 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;
decimal hitFraction;
float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
@ -196,11 +177,11 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
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;
decimal hitFraction;
float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
@ -217,9 +198,9 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
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
if (t < decimal(0.0) || t > ray.maxFraction) return false;
if (t < float(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
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
bool CapsuleShape::raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,
const Vector3& sphereCenter, decimal maxFraction,
Vector3& hitLocalPoint, decimal& hitFraction) const {
const Vector3& sphereCenter, float maxFraction,
Vector3& hitLocalPoint, float& hitFraction) const {
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 (c < decimal(0.0)) return false;
// If the origin of the ray is inside the sphere, we return no int32_tersection
if (c < float(0.0)) return false;
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
// is pointing away from the sphere, there is no intersection
if (b > decimal(0.0)) return false;
// is pointing away from the sphere, there is no int32_tersection
if (b > float(0.0)) return false;
decimal raySquareLength = rayDirection.lengthSquare();
float raySquareLength = rayDirection.lengthSquare();
// 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 (discriminant < decimal(0.0) || raySquareLength < MACHINE_EPSILON) return false;
// If the discriminant is negative or the ray length is very small, there is no int32_tersection
if (discriminant < float(0.0) || raySquareLength < MACHINE_EPSILON) return false;
// 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 (t < maxFraction * raySquareLength) {
// Compute the intersection information
// Compute the int32_tersection information
t /= raySquareLength;
hitFraction = t;
hitLocalPoint = point1 + t * rayDirection;

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

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

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_COLLISION_SHAPE_H
#define REACTPHYSICS3D_COLLISION_SHAPE_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <cassert>
@ -42,7 +21,7 @@ namespace reactphysics3d {
/// Type of the collision shape
enum CollisionShapeType {TRIANGLE, BOX, SPHERE, CONE, CYLINDER,
CAPSULE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD};
const int NB_COLLISION_SHAPE_TYPES = 9;
const int32_t NB_COLLISION_SHAPE_TYPES = 9;
// Declarations
class ProxyShape;
@ -108,7 +87,7 @@ class CollisionShape {
virtual void setLocalScaling(const Vector3& scaling);
/// 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
virtual void computeAABB(AABB& aabb, const Transform& transform) const;
@ -117,7 +96,7 @@ class CollisionShape {
static bool isConvex(CollisionShapeType shapeType);
/// 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);
// -------------------- Friendship -------------------- //
@ -151,7 +130,7 @@ inline void CollisionShape::setLocalScaling(const Vector3& scaling) {
// Return the maximum number of contact manifolds allowed in an overlapping
// pair wit the given two collision shape types
inline int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1,
inline int32_t CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1,
CollisionShapeType shapeType2) {
// If both shapes are convex
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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/shapes/ConcaveMeshShape.h>
@ -34,7 +15,7 @@ ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh):
ConcaveShape(CONCAVE_MESH) {
m_triangleMesh = triangleMesh;
m_raycastTestType = FRONT;
// Insert all the triangles into the dynamic AABB tree
// Insert all the triangles int32_to the dynamic AABB tree
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() {
// 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 (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
TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(subPart);
TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
int vertexStride = triangleVertexArray->getVerticesStride();
int indexStride = triangleVertexArray->getIndicesStride();
int32_t vertexStride = triangleVertexArray->getVerticesStride();
int32_t indexStride = triangleVertexArray->getIndicesStride();
// 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);
Vector3 trianglePoints[3];
// 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
int vertexIndex = 0;
int32_t vertexIndex = 0;
if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
vertexIndex = ((uint*)vertexIndexPointer)[k];
vertexIndex = ((uint32_t*)vertexIndexPointer)[k];
} else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
} else {
@ -74,14 +55,14 @@ void ConcaveMeshShape::initBVHTree() {
// Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x;
trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y;
trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z;
trianglePoints[k][0] = float(vertices[0]) * mScaling.x;
trianglePoints[k][1] = float(vertices[1]) * mScaling.y;
trianglePoints[k][2] = float(vertices[2]) * mScaling.z;
} else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x;
trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y;
trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z;
trianglePoints[k][0] = float(vertices[0]) * mScaling.x;
trianglePoints[k][1] = float(vertices[1]) * mScaling.y;
trianglePoints[k][2] = float(vertices[2]) * mScaling.z;
} else {
assert(false);
}
@ -89,7 +70,7 @@ void ConcaveMeshShape::initBVHTree() {
// Create the AABB for the triangle
AABB aabb = AABB::createAABBForTriangle(trianglePoints);
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);
}
}
@ -97,7 +78,7 @@ void ConcaveMeshShape::initBVHTree() {
// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
// given the start vertex index pointer of the triangle
void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 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
TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(subPart);
if (triangleVertexArray == nullptr) {
@ -107,15 +88,15 @@ void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32
TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
int vertexStride = triangleVertexArray->getVerticesStride();
int indexStride = triangleVertexArray->getIndicesStride();
int32_t vertexStride = triangleVertexArray->getVerticesStride();
int32_t indexStride = triangleVertexArray->getIndicesStride();
void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
// 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
int vertexIndex = 0;
int32_t vertexIndex = 0;
if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
vertexIndex = ((uint*)vertexIndexPointer)[k];
vertexIndex = ((uint32_t*)vertexIndexPointer)[k];
} else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
} else {
@ -125,14 +106,14 @@ void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32
// Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x;
outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y;
outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z;
outTriangleVertices[k][0] = float(vertices[0]) * mScaling.x;
outTriangleVertices[k][1] = float(vertices[1]) * mScaling.y;
outTriangleVertices[k][2] = float(vertices[2]) * mScaling.z;
} else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x;
outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y;
outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z;
outTriangleVertices[k][0] = float(vertices[0]) * mScaling.x;
outTriangleVertices[k][1] = float(vertices[1]) * mScaling.y;
outTriangleVertices[k][2] = float(vertices[2]) * mScaling.z;
} else {
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
decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) {
// Add the id of the hit AABB node into
float ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32_t nodeId, const Ray& ray) {
// Add the id of the hit AABB node int32_to
m_hitAABBNodes.push_back(nodeId);
return ray.maxFraction;
}
// Raycast all collision shapes that have been collected
void ConcaveMeshRaycastCallback::raycastTriangles() {
std::vector<int>::const_iterator it;
decimal smallestHitFraction = m_ray.maxFraction;
std::vector<int32_t>::const_iterator it;
float smallestHitFraction = m_ray.maxFraction;
for (it = m_hitAABBNodes.begin(); it != m_hitAABBNodes.end(); ++it) {
// 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
Vector3 trianglePoints[3];
m_concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
// 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.setRaycastTestType(m_concaveMeshShape.getRaycastTestType());
// Ray casting test against the collision shape
@ -188,7 +169,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
bool isTriangleHit = triangleShape.raycast(m_ray, raycastInfo, m_proxyShape);
// If the ray hit the collision shape
if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) {
assert(raycastInfo.hitFraction >= decimal(0.0));
assert(raycastInfo.hitFraction >= float(0.0));
m_raycastInfo.body = raycastInfo.body;
m_raycastInfo.proxyShape = raycastInfo.proxyShape;
m_raycastInfo.hitFraction = raycastInfo.hitFraction;

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_CONCAVE_MESH_SHAPE_H
#define REACTPHYSICS3D_CONCAVE_MESH_SHAPE_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#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
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int nodeId);
virtual void notifyOverlappingNode(int32_t nodeId);
};
@ -70,7 +49,7 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
private :
std::vector<int32> m_hitAABBNodes;
std::vector<int32_t> m_hitAABBNodes;
const DynamicAABBTree& m_dynamicAABBTree;
const ConcaveMeshShape& m_concaveMeshShape;
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
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
void raycastTriangles();
@ -132,12 +111,12 @@ class ConcaveMeshShape : public ConcaveShape {
/// Return the number of bytes used by the collision shape
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();
/// Return the three vertices coordinates (in the array outTriangleVertices) of a 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;
public:
@ -155,7 +134,7 @@ class ConcaveMeshShape : public ConcaveShape {
virtual void setLocalScaling(const Vector3& scaling);
/// 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
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const;
@ -204,7 +183,7 @@ inline void ConcaveMeshShape::setLocalScaling(const Vector3& scaling) {
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
inline void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
// Default inertia tensor
// 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
// 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)
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
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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/shapes/ConcaveShape.h>

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_CONCAVE_SHAPE_H
#define REACTPHYSICS3D_CONCAVE_SHAPE_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/collision/shapes/CollisionShape.h>
@ -64,7 +43,7 @@ class ConcaveShape : public CollisionShape {
bool m_isSmoothMeshCollisionEnabled;
// Margin use for collision detection for each triangle
decimal m_triangleMargin;
float m_triangleMargin;
/// Raycast test type for the triangle (front, back, front-back)
TriangleRaycastSide m_raycastTestType;
@ -91,7 +70,7 @@ class ConcaveShape : public CollisionShape {
virtual ~ConcaveShape();
/// Return the triangle margin
decimal getTriangleMargin() const;
float getTriangleMargin() const;
/// Return the raycast test type (front, back, front-back)
TriangleRaycastSide getRaycastTestType() const;
@ -113,7 +92,7 @@ class ConcaveShape : public CollisionShape {
};
// Return the triangle margin
inline decimal ConcaveShape::getTriangleMargin() const {
inline float ConcaveShape::getTriangleMargin() const {
return m_triangleMargin;
}
@ -133,7 +112,7 @@ inline bool ConcaveShape::getIsSmoothMeshCollisionEnabled() const {
}
// 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
/// but collisions computation is a bit more expensive.
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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <complex>
@ -37,10 +18,10 @@ using namespace reactphysics3d;
* @param height Height of the cone (in meters)
* @param margin Collision margin (in meters) around the collision shape
*/
ConeShape::ConeShape(decimal radius, decimal height, decimal margin)
: ConvexShape(CONE, margin), mRadius(radius), mHalfHeight(height * decimal(0.5)) {
assert(mRadius > decimal(0.0));
assert(mHalfHeight > decimal(0.0));
ConeShape::ConeShape(float radius, float height, float margin)
: ConvexShape(CONE, margin), mRadius(radius), mHalfHeight(height * float(0.5)) {
assert(mRadius > float(0.0));
assert(mHalfHeight > float(0.0));
// Compute the sine of the semi-angle at the apex point
mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height));
@ -56,16 +37,16 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
const Vector3& v = direction;
decimal sinThetaTimesLengthV = mSinTheta * v.length();
float sinThetaTimesLengthV = mSinTheta * v.length();
Vector3 supportPoint;
if (v.y > sinThetaTimesLengthV) {
supportPoint = Vector3(0.0, mHalfHeight, 0.0);
}
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) {
decimal d = mRadius / projectedLength;
float d = mRadius / projectedLength;
supportPoint = Vector3(v.x * d, -mHalfHeight, v.z * d);
}
else {
@ -84,40 +65,40 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
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 centerBase(0, -mHalfHeight, 0);
Vector3 axis(0, decimal(-1.0), 0);
decimal heightSquare = decimal(4.0) * mHalfHeight * mHalfHeight;
decimal cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius);
decimal factor = decimal(1.0) - cosThetaSquare;
Vector3 axis(0, float(-1.0), 0);
float heightSquare = float(4.0) * mHalfHeight * mHalfHeight;
float cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius);
float factor = float(1.0) - cosThetaSquare;
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;
decimal 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;
decimal tHit[] = {decimal(-1.0), decimal(-1.0), decimal(-1.0)};
float c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z;
float c2 = -cosThetaSquare * r.x * r.x + factor * r.y * r.y - cosThetaSquare * r.z * r.z;
float tHit[] = {float(-1.0), float(-1.0), float(-1.0)};
Vector3 localHitPoint[3];
Vector3 localNormal[3];
// If c2 is different from zero
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 (gamma < decimal(0.0)) {
if (gamma < float(0.0)) {
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
decimal sqrRoot = std::sqrt(gamma);
// Compute two int32_tersections
float sqrRoot = std::sqrt(gamma);
tHit[0] = (-c1 - sqrRoot) / c2;
tHit[1] = (-c1 + sqrRoot) / c2;
}
else { // If the equation has a single real root
// Compute the intersection
// Compute the int32_tersection
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 (std::abs(c1) > MACHINE_EPSILON) {
tHit[0] = -c0 / (decimal(2.0) * c1);
tHit[0] = -c0 / (float(2.0) * c1);
}
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[1] = ray.point1 + tHit[1] * r;
// Only keep hit points in one side of the double cone (the cone we are interested in)
if (axis.dot(localHitPoint[0] - V) < decimal(0.0)) {
tHit[0] = decimal(-1.0);
// Only keep hit points in one side of the double cone (the cone we are int32_terested in)
if (axis.dot(localHitPoint[0] - V) < float(0.0)) {
tHit[0] = float(-1.0);
}
if (axis.dot(localHitPoint[1] - V) < decimal(0.0)) {
tHit[1] = decimal(-1.0);
if (axis.dot(localHitPoint[1] - V) < float(0.0)) {
tHit[1] = float(-1.0);
}
// Only keep hit points that are within the correct height of the cone
if (localHitPoint[0].y < decimal(-mHalfHeight)) {
tHit[0] = decimal(-1.0);
if (localHitPoint[0].y < float(-mHalfHeight)) {
tHit[0] = float(-1.0);
}
if (localHitPoint[1].y < decimal(-mHalfHeight)) {
tHit[1] = decimal(-1.0);
if (localHitPoint[1].y < float(-mHalfHeight)) {
tHit[1] = float(-1.0);
}
// If the ray is in direction of the base plane of the cone
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);
// 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;
if ((localHitPoint[2] - centerBase).lengthSquare() > mRadius * mRadius) {
tHit[2] = decimal(-1.0);
tHit[2] = float(-1.0);
}
// Compute the normal direction
@ -176,10 +157,10 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
}
// Find the smallest positive t value
int hitIndex = -1;
decimal t = DECIMAL_LARGEST;
for (int i=0; i<3; i++) {
if (tHit[i] < decimal(0.0)) continue;
int32_t hitIndex = -1;
float t = DECIMAL_LARGEST;
for (int32_t i=0; i<3; i++) {
if (tHit[i] < float(0.0)) continue;
if (tHit[i] < t) {
hitIndex = i;
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
if (hitIndex != 2) {
decimal h = decimal(2.0) * mHalfHeight;
decimal value1 = (localHitPoint[hitIndex].x * localHitPoint[hitIndex].x +
float h = float(2.0) * mHalfHeight;
float value1 = (localHitPoint[hitIndex].x * localHitPoint[hitIndex].x +
localHitPoint[hitIndex].z * localHitPoint[hitIndex].z);
decimal rOverH = mRadius / h;
decimal value2 = decimal(1.0) + rOverH * rOverH;
decimal factor = decimal(1.0) / std::sqrt(value1 * value2);
decimal x = localHitPoint[hitIndex].x * factor;
decimal z = localHitPoint[hitIndex].z * factor;
float rOverH = mRadius / h;
float value2 = float(1.0) + rOverH * rOverH;
float factor = float(1.0) / std::sqrt(value1 * value2);
float x = localHitPoint[hitIndex].x * factor;
float z = localHitPoint[hitIndex].z * factor;
localNormal[hitIndex].x = x;
localNormal[hitIndex].y = std::sqrt(x * x + z * z) * rOverH;
localNormal[hitIndex].z = z;

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

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

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

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <complex>
@ -31,14 +12,14 @@
using namespace reactphysics3d;
// 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 nbVertices Number of vertices in the convex mesh
* @param stride Stride between the beginning of two elements in the vertices array
* @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),
m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(false) {
assert(nbVertices > 0);
@ -46,9 +27,9 @@ ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices,
const unsigned char* vertexPointer = (const unsigned char*) arrayVertices;
// Copy all the vertices into the internal array
for (uint i=0; i<m_numberVertices; i++) {
const decimal* newPoint = (const decimal*) vertexPointer;
// Copy all the vertices int32_to the int32_ternal array
for (uint32_t i=0; i<m_numberVertices; i++) {
const float* newPoint = (const float*) vertexPointer;
m_vertices.push_back(Vector3(newPoint[0], newPoint[1], newPoint[2]));
vertexPointer += stride;
}
@ -58,13 +39,13 @@ ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices,
}
// 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 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
*/
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),
m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(isEdgesInformationUsed) {
@ -72,11 +53,11 @@ ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool
TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
int vertexStride = triangleVertexArray->getVerticesStride();
int indexStride = triangleVertexArray->getIndicesStride();
int32_t vertexStride = triangleVertexArray->getVerticesStride();
int32_t indexStride = triangleVertexArray->getIndicesStride();
// 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
if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
@ -99,18 +80,18 @@ ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool
if (m_isEdgesInformationUsed) {
// 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);
uint vertexIndex[3] = {0, 0, 0};
uint32_t vertexIndex[3] = {0, 0, 0};
// 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
if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
vertexIndex[k] = ((uint*)vertexIndexPointer)[k];
vertexIndex[k] = ((uint32_t*)vertexIndexPointer)[k];
}
else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
vertexIndex[k] = ((unsigned short*)vertexIndexPointer)[k];
@ -134,7 +115,7 @@ ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool
// Constructor.
/// If you use this constructor, you will need to set the vertices manually one by one using
/// the addVertex() method.
ConvexMeshShape::ConvexMeshShape(decimal margin)
ConvexMeshShape::ConvexMeshShape(float margin)
: ConvexShape(CONVEX_MESH, margin), m_numberVertices(0), m_minBounds(0, 0, 0),
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
if ((*cachedCollisionData) == NULL) {
*cachedCollisionData = (int*) malloc(sizeof(int));
*((int*)(*cachedCollisionData)) = 0;
*cachedCollisionData = (int32_t*) malloc(sizeof(int32_t));
*((int32_t*)(*cachedCollisionData)) = 0;
}
// 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);
uint maxVertex = *((int*)(*cachedCollisionData));
decimal maxDotProduct = direction.dot(m_vertices[maxVertex]);
uint32_t maxVertex = *((int32_t*)(*cachedCollisionData));
float maxDotProduct = direction.dot(m_vertices[maxVertex]);
bool isOptimal;
// Perform hill-climbing (local search)
@ -181,13 +162,13 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct
assert(m_edgesAdjacencyList.at(maxVertex).size() > 0);
// For all neighbors of the current vertex
std::set<uint>::const_iterator it;
std::set<uint>::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 it;
std::set<uint32_t>::const_iterator itBegin = m_edgesAdjacencyList.at(maxVertex).begin();
std::set<uint32_t>::const_iterator itEnd = m_edgesAdjacencyList.at(maxVertex).end();
for (it = itBegin; it != itEnd; ++it) {
// 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 (dotProduct > maxDotProduct) {
@ -200,7 +181,7 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct
} while(!isOptimal);
// Cache the support vertex
*((int*)(*cachedCollisionData)) = maxVertex;
*((int32_t*)(*cachedCollisionData)) = maxVertex;
// Return the support vertex
return m_vertices[maxVertex] * mScaling;
@ -208,10 +189,10 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct
else { // If the edges information is not used
double maxDotProduct = DECIMAL_SMALLEST;
uint indexMaxDotProduct = 0;
uint32_t indexMaxDotProduct = 0;
// 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
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 m_vertices[indexMaxDotProduct] * mScaling;
@ -240,7 +221,7 @@ void ConvexMeshShape::recalculateBounds() {
m_maxBounds.setToZero();
// 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_minBounds.x) m_minBounds.x = m_vertices[i].x;

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_CONVEX_MESH_SHAPE_H
#define REACTPHYSICS3D_CONVEX_MESH_SHAPE_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/collision/shapes/ConvexShape.h>
@ -68,7 +47,7 @@ class ConvexMeshShape : public ConvexShape {
std::vector<Vector3> m_vertices;
/// 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
Vector3 m_minBounds;
@ -81,7 +60,7 @@ class ConvexMeshShape : public ConvexShape {
bool m_isEdgesInformationUsed;
/// 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 -------------------- //
@ -115,15 +94,15 @@ class ConvexMeshShape : public ConvexShape {
// -------------------- Methods -------------------- //
/// Constructor to initialize with an array of 3D vertices.
ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride,
decimal margin = OBJECT_MARGIN);
ConvexMeshShape(const float* arrayVertices, uint32_t nbVertices, int32_t stride,
float margin = OBJECT_MARGIN);
/// Constructor to initialize with a triangle vertex array
ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed = true,
decimal margin = OBJECT_MARGIN);
float margin = OBJECT_MARGIN);
/// Constructor.
ConvexMeshShape(decimal margin = OBJECT_MARGIN);
ConvexMeshShape(float margin = OBJECT_MARGIN);
/// Destructor
virtual ~ConvexMeshShape();
@ -132,13 +111,13 @@ class ConvexMeshShape : public ConvexShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// 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);
/// Add an edge into the convex mesh by specifying the two vertex indices of the edge.
void addEdge(uint v1, uint v2);
/// Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge.
void addEdge(uint32_t v1, uint32_t v2);
/// Return true if the edges information is used to speed up the collision detection
bool isEdgesInformationUsed() const;
@ -177,19 +156,19 @@ inline void ConvexMeshShape::getLocalBounds(Vector3& min, Vector3& max) const {
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal factor = (decimal(1.0) / decimal(3.0)) * mass;
Vector3 realExtent = decimal(0.5) * (m_maxBounds - m_minBounds);
inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
float factor = (float(1.0) / float(3.0)) * mass;
Vector3 realExtent = float(0.5) * (m_maxBounds - m_minBounds);
assert(realExtent.x > 0 && realExtent.y > 0 && realExtent.z > 0);
decimal xSquare = realExtent.x * realExtent.x;
decimal ySquare = realExtent.y * realExtent.y;
decimal zSquare = realExtent.z * realExtent.z;
float xSquare = realExtent.x * realExtent.x;
float ySquare = realExtent.y * realExtent.y;
float zSquare = realExtent.z * realExtent.z;
tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0,
0.0, factor * (xSquare + zSquare), 0.0,
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
*/
@ -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;
}
// 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
/// 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 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 (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 (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
@ -261,5 +240,3 @@ inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint,
}
}
#endif

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

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/shapes/ConvexShape.h>
@ -31,7 +12,7 @@
using namespace reactphysics3d;
// Constructor
ConvexShape::ConvexShape(CollisionShapeType type, decimal margin)
ConvexShape::ConvexShape(CollisionShapeType type, float margin)
: CollisionShape(type), mMargin(margin) {
}
@ -48,7 +29,7 @@ Vector3 ConvexShape::getLocalSupportPointWithMargin(const Vector3& direction,
// Get the support point without margin
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
if (mMargin != decimal(0.0)) {
if (mMargin != float(0.0)) {
// Add the margin to the support point
Vector3 unitVec(0.0, -1.0, 0.0);

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

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

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

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/shapes/CylinderShape.h>
@ -36,11 +17,11 @@ using namespace reactphysics3d;
* @param height Height of the cylinder (in meters)
* @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),
mHalfHeight(height/decimal(2.0)) {
assert(radius > decimal(0.0));
assert(height > decimal(0.0));
mHalfHeight(height/float(2.0)) {
assert(radius > float(0.0));
assert(height > float(0.0));
}
// Destructor
@ -53,9 +34,9 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio
void** cachedCollisionData) const {
Vector3 supportPoint(0.0, 0.0, 0.0);
decimal uDotv = direction.y;
float uDotv = direction.y;
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 (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 decimal epsilon = decimal(0.01);
Vector3 p(decimal(0), -mHalfHeight, decimal(0));
Vector3 q(decimal(0), mHalfHeight, decimal(0));
const float epsilon = float(0.01);
Vector3 p(float(0), -mHalfHeight, float(0));
Vector3 q(float(0), mHalfHeight, float(0));
Vector3 d = q - p;
Vector3 m = ray.point1 - p;
decimal t;
float t;
decimal mDotD = m.dot(d);
decimal nDotD = n.dot(d);
decimal dDotD = d.dot(d);
float mDotD = m.dot(d);
float nDotD = n.dot(d);
float dDotD = d.dot(d);
// 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;
decimal nDotN = n.dot(n);
decimal mDotN = m.dot(n);
float nDotN = n.dot(n);
float mDotN = m.dot(n);
decimal a = dDotD * nDotN - nDotD * nDotD;
decimal k = m.dot(m) - mRadius * mRadius;
decimal c = dDotD * k - mDotD * mDotD;
float a = dDotD * nDotN - nDotD * nDotD;
float k = m.dot(m) - mRadius * mRadius;
float c = dDotD * k - mDotD * mDotD;
// If the ray is parallel to the cylinder axis
if (std::abs(a) < epsilon) {
// 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 (mDotD < decimal(0.0)) {
// If the ray int32_tersects with the "p" endcap of the cylinder
if (mDotD < float(0.0)) {
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
if (t < decimal(0.0) || t > ray.maxFraction) return false;
if (t < float(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n;
@ -122,18 +103,18 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(-1), 0);
Vector3 normalDirection(0, float(-1), 0);
raycastInfo.worldNormal = normalDirection;
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;
// 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
if (t < decimal(0.0) || t > ray.maxFraction) return false;
if (t < float(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n;
@ -141,7 +122,7 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(1.0), 0);
Vector3 normalDirection(0, float(1.0), 0);
raycastInfo.worldNormal = normalDirection;
return true;
@ -150,31 +131,31 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
return false;
}
}
decimal b = dDotD * mDotN - nDotD * mDotD;
decimal discriminant = b * b - a * c;
float b = dDotD * mDotN - nDotD * mDotD;
float discriminant = b * b - a * c;
// 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)
decimal t0 = t = (-b - std::sqrt(discriminant)) / a;
// Compute the smallest root (first int32_tersection along the ray)
float t0 = t = (-b - std::sqrt(discriminant)) / a;
// If the intersection is outside the cylinder on "p" endcap side
decimal value = mDotD + t * nDotD;
if (value < decimal(0.0)) {
// If the int32_tersection is outside the cylinder on "p" endcap side
float value = mDotD + t * nDotD;
if (value < float(0.0)) {
// 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;
// Keep the intersection if the it is inside the cylinder radius
if (k + t * (decimal(2.0) * mDotN + t) > decimal(0.0)) return false;
// Keep the int32_tersection if the it is inside the cylinder radius
if (k + t * (float(2.0) * mDotN + t) > float(0.0)) return false;
// If 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
if (t < decimal(0.0) || t > ray.maxFraction) return false;
if (t < float(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n;
@ -182,26 +163,26 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(-1.0), 0);
Vector3 normalDirection(0, float(-1.0), 0);
raycastInfo.worldNormal = normalDirection;
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 (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;
// Keep the intersection if it is inside the cylinder radius
if (k + dDotD - decimal(2.0) * mDotD + t * (decimal(2.0) * (mDotN - nDotD) + t) >
decimal(0.0)) return false;
// Keep the int32_tersection if it is inside the cylinder radius
if (k + dDotD - float(2.0) * mDotD + t * (float(2.0) * (mDotN - nDotD) + t) >
float(0.0)) return false;
// 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
if (t < decimal(0.0) || t > ray.maxFraction) return false;
if (t < float(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n;
@ -209,7 +190,7 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(1.0), 0);
Vector3 normalDirection(0, float(1.0), 0);
raycastInfo.worldNormal = normalDirection;
return true;
@ -217,9 +198,9 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
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
if (t < decimal(0.0) || t > ray.maxFraction) return false;
if (t < float(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n;

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_CYLINDER_SHAPE_H
#define REACTPHYSICS3D_CYLINDER_SHAPE_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/collision/shapes/ConvexShape.h>
@ -56,10 +35,10 @@ class CylinderShape : public ConvexShape {
// -------------------- Attributes -------------------- //
/// Radius of the base
decimal mRadius;
float mRadius;
/// Half height of the cylinder
decimal mHalfHeight;
float mHalfHeight;
// -------------------- Methods -------------------- //
@ -87,16 +66,16 @@ class CylinderShape : public ConvexShape {
// -------------------- Methods -------------------- //
/// Constructor
CylinderShape(decimal radius, decimal height, decimal margin = OBJECT_MARGIN);
CylinderShape(float radius, float height, float margin = OBJECT_MARGIN);
/// Destructor
virtual ~CylinderShape();
/// Return the radius
decimal getRadius() const;
float getRadius() const;
/// Return the height
decimal getHeight() const;
float getHeight() const;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
@ -105,14 +84,14 @@ class CylinderShape : public ConvexShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// 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 Radius of the cylinder (in meters)
*/
inline decimal CylinderShape::getRadius() const {
inline float CylinderShape::getRadius() const {
return mRadius;
}
@ -120,7 +99,7 @@ inline decimal CylinderShape::getRadius() const {
/**
* @return Height of the cylinder (in meters)
*/
inline decimal CylinderShape::getHeight() const {
inline float CylinderShape::getHeight() const {
return mHalfHeight + mHalfHeight;
}
@ -162,11 +141,11 @@ inline void CylinderShape::getLocalBounds(Vector3& min, Vector3& max) const {
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal height = decimal(2.0) * mHalfHeight;
decimal diag = (decimal(1.0) / decimal(12.0)) * mass * (3 * mRadius * mRadius + height * height);
inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
float height = float(2.0) * mHalfHeight;
float diag = (float(1.0) / float(12.0)) * mass * (3 * mRadius * mRadius + height * height);
tensor.setAllValues(diag, 0.0, 0.0, 0.0,
decimal(0.5) * mass * mRadius * mRadius, 0.0,
float(0.5) * mass * mRadius * mRadius, 0.0,
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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/shapes/HeightFieldShape.h>
@ -35,16 +16,16 @@ using namespace reactphysics3d;
* @param minHeight Minimum 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 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 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,
const void* heightFieldData, HeightDataType dataType, int upAxis,
decimal integerHeightScale)
HeightFieldShape::HeightFieldShape(int32_t nbGridColumns, int32_t nbGridRows, float minHeight, float maxHeight,
const void* heightFieldData, HeightDataType dataType, int32_t upAxis,
float int32_tegerHeightScale)
: ConcaveShape(HEIGHTFIELD), mNbColumns(nbGridColumns), mNbRows(nbGridRows),
mWidth(nbGridColumns - 1), mLength(nbGridRows - 1), mMinHeight(minHeight),
mMaxHeight(maxHeight), mUpAxis(upAxis), mIntegerHeightScale(integerHeightScale),
mMaxHeight(maxHeight), mUpAxis(upAxis), mIntegerHeightScale(int32_tegerHeightScale),
mHeightDataType(dataType) {
assert(nbGridColumns >= 2);
@ -56,21 +37,21 @@ HeightFieldShape::HeightFieldShape(int nbGridColumns, int nbGridRows, decimal mi
mHeightFieldData = heightFieldData;
decimal halfHeight = (mMaxHeight - mMinHeight) * decimal(0.5);
float halfHeight = (mMaxHeight - mMinHeight) * float(0.5);
assert(halfHeight >= 0);
// Compute the local AABB of the height field
if (mUpAxis == 0) {
mAABB.setMin(Vector3(-halfHeight, -mWidth * decimal(0.5), -mLength * decimal(0.5)));
mAABB.setMax(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 * float(0.5), mLength* float(0.5)));
}
else if (mUpAxis == 1) {
mAABB.setMin(Vector3(-mWidth * decimal(0.5), -halfHeight, -mLength * decimal(0.5)));
mAABB.setMax(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 * float(0.5), halfHeight, mLength * float(0.5)));
}
else if (mUpAxis == 2) {
mAABB.setMin(Vector3(-mWidth * decimal(0.5), -mLength * decimal(0.5), -halfHeight));
mAABB.setMax(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 * 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 {
// 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);
// Compute the integer grid coordinates inside the area we need to test for collision
int minGridCoords[3];
int maxGridCoords[3];
// Compute the int32_teger grid coordinates inside the area we need to test for collision
int32_t minGridCoords[3];
int32_t maxGridCoords[3];
computeMinMaxGridCoordinates(minGridCoords, maxGridCoords, aabb);
// Compute the starting and ending coords of the sub-grid according to the up axis
int iMin = 0;
int iMax = 0;
int jMin = 0;
int jMax = 0;
int32_t iMin = 0;
int32_t iMax = 0;
int32_t jMin = 0;
int32_t jMax = 0;
switch(mUpAxis) {
case 0 : iMin = clamp(minGridCoords[1], 0, mNbColumns - 1);
iMax = clamp(maxGridCoords[1], 0, mNbColumns - 1);
@ -134,8 +115,8 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB&
assert(jMax >= 0 && jMax < mNbRows);
// For each sub-grid points (except the last ones one each dimension)
for (int i = iMin; i < iMax; i++) {
for (int j = jMin; j < jMax; j++) {
for (int32_t i = iMin; i < iMax; i++) {
for (int32_t j = jMin; j < jMax; j++) {
// Compute the four point of the current quad
Vector3 p1 = getVertexAt(i, j);
@ -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
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
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]
// and from [0 ... mNbLengthGridPoints] because the AABB coordinates range are [-mWdith/2 ... mWidth/2]
// and [-mLength/2 ... mLength/2]
const Vector3 translateVec = mAABB.getExtent() * decimal(0.5);
const Vector3 translateVec = mAABB.getExtent() * float(0.5);
minPoint += 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
// of the AABB)
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
Vector3 HeightFieldShape::getVertexAt(int x, int y) const {
Vector3 HeightFieldShape::getVertexAt(int32_t x, int32_t y) const {
// Get the height value
const decimal height = getHeightAt(x, y);
const float height = getHeightAt(x, y);
// Height values origin
const decimal heightOrigin = -(mMaxHeight - mMinHeight) * decimal(0.5) - mMinHeight;
const float heightOrigin = -(mMaxHeight - mMinHeight) * float(0.5) - mMinHeight;
Vector3 vertex;
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;
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;
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;
default: assert(false);
}
@ -240,7 +221,7 @@ Vector3 HeightFieldShape::getVertexAt(int x, int y) const {
void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) {
// Create a triangle collision shape
decimal margin = mHeightFieldShape.getTriangleMargin();
float margin = mHeightFieldShape.getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType());
@ -251,7 +232,7 @@ void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) {
// If the ray hit the collision shape
if (isTriangleHit && raycastInfo.hitFraction <= mSmallestHitFraction) {
assert(raycastInfo.hitFraction >= decimal(0.0));
assert(raycastInfo.hitFraction >= float(0.0));
m_raycastInfo.body = raycastInfo.body;
m_raycastInfo.proxyShape = raycastInfo.proxyShape;

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_HEIGHTFIELD_SHAPE_H
#define REACTPHYSICS3D_HEIGHTFIELD_SHAPE_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/collision/shapes/ConcaveShape.h>
@ -47,7 +26,7 @@ class TriangleOverlapCallback : public TriangleCallback {
ProxyShape* m_proxyShape;
RaycastInfo& m_raycastInfo;
bool mIsHit;
decimal mSmallestHitFraction;
float mSmallestHitFraction;
const HeightFieldShape& mHeightFieldShape;
public:
@ -72,8 +51,8 @@ class TriangleOverlapCallback : public TriangleCallback {
/**
* This class represents a static height field that can be used to represent
* a terrain. The height field is made of a grid with rows and columns with a
* height value at each grid point. Note that the height values are not copied into the shape
* but are shared instead. The height values can be of type integer, float or double.
* height value at each grid point. Note that the height values are not copied int32_to the shape
* but are shared instead. The height values can be of type int32_teger, float or double.
* 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
* 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 -------------------- //
/// 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
int mNbRows;
int32_t mNbRows;
/// Height field width
decimal mWidth;
float mWidth;
/// Height field length
decimal mLength;
float mLength;
/// Minimum height of the height field
decimal mMinHeight;
float mMinHeight;
/// Maximum height of the height field
decimal mMaxHeight;
float mMaxHeight;
/// Up axis direction (0 => x, 1 => y, 2 => z)
int mUpAxis;
int32_t mUpAxis;
/// Height values scale for height field with integer height values
decimal mIntegerHeightScale;
/// Height values scale for height field with int32_teger height values
float mIntegerHeightScale;
/// Data type of the height values
HeightDataType mHeightDataType;
@ -137,41 +116,41 @@ class HeightFieldShape : public ConcaveShape {
/// Return the number of bytes used by the collision shape
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();
/// Return the three vertices coordinates (in the array outTriangleVertices) of a 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;
/// 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
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
int computeIntegerGridValue(decimal value) const;
/// Return the closest inside int32_teger grid value of a given floating grid value
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
void computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, const AABB& aabbToCollide) const;
/// 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(int32_t* minCoords, int32_t* maxCoords, const AABB& aabbToCollide) const;
public:
/// 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,
int upAxis = 1, decimal integerHeightScale = 1.0f);
int32_t upAxis = 1, float int32_tegerHeightScale = 1.0f);
/// Destructor
~HeightFieldShape();
/// 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
int getNbColumns() const;
int32_t getNbColumns() const;
/// Return the type of height value in the height field
HeightDataType getHeightDataType() const;
@ -183,7 +162,7 @@ class HeightFieldShape : public ConcaveShape {
virtual void setLocalScaling(const Vector3& scaling);
/// 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
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
inline int HeightFieldShape::getNbRows() const {
inline int32_t HeightFieldShape::getNbRows() const {
return mNbRows;
}
// Return the number of columns in the height field
inline int HeightFieldShape::getNbColumns() const {
inline int32_t HeightFieldShape::getNbColumns() const {
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
inline decimal HeightFieldShape::getHeightAt(int x, int y) const {
inline float HeightFieldShape::getHeightAt(int32_t x, int32_t y) const {
switch(mHeightDataType) {
case HEIGHT_FLOAT_TYPE : return ((float*)mHeightFieldData)[y * mNbColumns + x];
case HEIGHT_DOUBLE_TYPE : return ((double*)mHeightFieldData)[y * mNbColumns + x];
case HEIGHT_INT_TYPE : return ((int*)mHeightFieldData)[y * mNbColumns + x] * mIntegerHeightScale;
case HEIGHT_INT_TYPE : return ((int32_t*)mHeightFieldData)[y * mNbColumns + x] * mIntegerHeightScale;
default: assert(false); return 0;
}
}
// Return the closest inside integer grid value of a given floating grid value
inline int HeightFieldShape::computeIntegerGridValue(decimal value) const {
return (value < decimal(0.0)) ? value - decimal(0.5) : value + decimal(0.5);
// Return the closest inside int32_teger grid value of a given floating grid value
inline int32_t HeightFieldShape::computeIntegerGridValue(float value) const {
return (value < float(0.0)) ? value - float(0.5) : value + float(0.5);
}
// Return the local inertia tensor
@ -241,7 +220,7 @@ inline int HeightFieldShape::computeIntegerGridValue(decimal value) const {
* coordinates
* @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
// 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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/shapes/SphereShape.h>
@ -35,8 +16,8 @@ using namespace reactphysics3d;
/**
* @param radius Radius of the sphere (in meters)
*/
SphereShape::SphereShape(decimal radius) : ConvexShape(SPHERE, radius) {
assert(radius > decimal(0.0));
SphereShape::SphereShape(float radius) : ConvexShape(SPHERE, radius) {
assert(radius > float(0.0));
}
// Destructor
@ -48,35 +29,35 @@ SphereShape::~SphereShape() {
bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
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 (c < decimal(0.0)) return false;
// If the origin of the ray is inside the sphere, we return no int32_tersection
if (c < float(0.0)) return false;
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
// is pointing away from the sphere, there is no intersection
if (b > decimal(0.0)) return false;
// is pointing away from the sphere, there is no int32_tersection
if (b > float(0.0)) return false;
decimal raySquareLength = rayDirection.lengthSquare();
float raySquareLength = rayDirection.lengthSquare();
// 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 (discriminant < decimal(0.0) || raySquareLength < MACHINE_EPSILON) return false;
// If the discriminant is negative or the ray length is very small, there is no int32_tersection
if (discriminant < float(0.0) || raySquareLength < MACHINE_EPSILON) return false;
// 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 (t < ray.maxFraction * raySquareLength) {
// Compute the intersection information
// Compute the int32_tersection information
t /= raySquareLength;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_SPHERE_SHAPE_H
#define REACTPHYSICS3D_SPHERE_SHAPE_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/collision/shapes/ConvexShape.h>
@ -75,13 +54,13 @@ class SphereShape : public ConvexShape {
// -------------------- Methods -------------------- //
/// Constructor
SphereShape(decimal radius);
SphereShape(float radius);
/// Destructor
virtual ~SphereShape();
/// Return the radius of the sphere
decimal getRadius() const;
float getRadius() const;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
@ -90,7 +69,7 @@ class SphereShape : public ConvexShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// 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
virtual void computeAABB(AABB& aabb, const Transform& transform) const;
@ -100,7 +79,7 @@ class SphereShape : public ConvexShape {
/**
* @return Radius of the sphere (in meters)
*/
inline decimal SphereShape::getRadius() const {
inline float SphereShape::getRadius() const {
return mMargin;
}
@ -121,7 +100,7 @@ inline size_t SphereShape::getSizeInBytes() const {
inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
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);
}
@ -150,8 +129,8 @@ inline void SphereShape::getLocalBounds(Vector3& min, Vector3& max) const {
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal diag = decimal(0.4) * mass * mMargin * mMargin;
inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
float diag = float(0.4) * mass * mMargin * mMargin;
tensor.setAllValues(diag, 0.0, 0.0,
0.0, diag, 0.0,
0.0, 0.0, diag);
@ -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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/shapes/TriangleShape.h>
@ -39,7 +20,7 @@ using namespace reactphysics3d;
* @param point3 Third point of the triangle
* @param margin The collision margin (in meters) around the collision shape
*/
TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, decimal margin)
TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, float margin)
: ConvexShape(TRIANGLE, margin) {
mPoints[0] = point1;
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
// product for this test.
const Vector3 m = pq.cross(pc);
decimal u = pb.dot(m);
float u = pb.dot(m);
if (m_raycastTestType == FRONT) {
if (u < decimal(0.0)) return false;
if (u < float(0.0)) return false;
}
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 (v < decimal(0.0)) return false;
if (v < float(0.0)) return false;
}
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) {
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 (w < decimal(0.0)) return false;
if (w < float(0.0)) return false;
}
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) {
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;
// Compute the barycentric coordinates (u, v, w) to determine the
// intersection point R, R = u * a + v * b + w * c
decimal denom = decimal(1.0) / (u + v + w);
// int32_tersection point R, R = u * a + v * b + w * c
float denom = float(1.0) / (u + v + w);
u *= denom;
v *= denom;
w *= denom;
// Compute the local hit point using the barycentric coordinates
const Vector3 localHitPoint = u * mPoints[0] + v * mPoints[1] + w * mPoints[2];
const 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]);
if (localHitNormal.dot(pq) > decimal(0.0)) localHitNormal = -localHitNormal;
if (localHitNormal.dot(pq) > float(0.0)) localHitNormal = -localHitNormal;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;

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

@ -1,30 +1,9 @@
/********************************************************************************
* 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_TRIANGLE_SHAPE_H
#define REACTPHYSICS3D_TRIANGLE_SHAPE_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/mathematics/mathematics.h>
@ -90,7 +69,7 @@ class TriangleShape : public ConvexShape {
/// Constructor
TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3,
decimal margin = OBJECT_MARGIN);
float margin = OBJECT_MARGIN);
/// Destructor
virtual ~TriangleShape();
@ -102,7 +81,7 @@ class TriangleShape : public ConvexShape {
virtual void setLocalScaling(const Vector3& scaling);
/// 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
virtual void computeAABB(AABB& aabb, const Transform& transform) const;
@ -114,7 +93,7 @@ class TriangleShape : public ConvexShape {
void setRaycastTestType(TriangleRaycastSide testType);
/// Return the coordinates of a given vertex of the triangle
Vector3 getVertex(int index) const;
Vector3 getVertex(int32_t index) const;
// ---------- Friendship ---------- //
@ -168,7 +147,7 @@ inline void TriangleShape::setLocalScaling(const Vector3& scaling) {
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void TriangleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
inline void TriangleShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
tensor.setToZero();
}
@ -213,12 +192,10 @@ inline void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) {
/**
* @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);
return mPoints[index];
}
}
#endif

99
ephysics/configuration.h
View File

@ -1,61 +1,22 @@
/********************************************************************************
* 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_CONFIGURATION_H
#define REACTPHYSICS3D_CONFIGURATION_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <limits>
#include <cfloat>
#include <utility>
#include <ephysics/decimal.h>
// 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
#include <cstdint>
/// Namespace reactphysics3d
namespace reactphysics3d {
// ------------------- Type definitions ------------------- //
typedef unsigned int uint;
typedef long unsigned int luint;
typedef luint bodyindex;
typedef uint64_t bodyindex;
typedef std::pair<bodyindex, bodyindex> bodyindexpair;
typedef signed short int16;
typedef signed int int32;
typedef unsigned short uint16;
typedef unsigned int uint32;
// ------------------- Enumerations ------------------- //
/// Position correction technique used in the constraint solver (for joints).
@ -73,77 +34,75 @@ enum ContactsPositionCorrectionTechnique {BAUMGARTE_CONTACTS, SPLIT_IMPULSES};
// ------------------- Constants ------------------- //
/// Smallest decimal value (negative)
const decimal DECIMAL_SMALLEST = - std::numeric_limits<decimal>::max();
/// Smallest float value (negative)
const float DECIMAL_SMALLEST = - std::numeric_limits<float>::max();
/// Maximum decimal value
const decimal DECIMAL_LARGEST = std::numeric_limits<decimal>::max();
/// Maximum float value
const float DECIMAL_LARGEST = std::numeric_limits<float>::max();
/// Machine epsilon
const decimal MACHINE_EPSILON = std::numeric_limits<decimal>::epsilon();
const float MACHINE_EPSILON = std::numeric_limits<float>::epsilon();
/// Pi constant
const decimal PI = decimal(3.14159265);
const float PI = float(3.14159265);
/// 2*Pi constant
const decimal PI_TIMES_2 = decimal(6.28318530);
const float PI_TIMES_2 = float(6.28318530);
/// Default friction coefficient for a rigid body
const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3);
const float DEFAULT_FRICTION_COEFFICIENT = float(0.3);
/// Default bounciness factor for a rigid body
const decimal DEFAULT_BOUNCINESS = decimal(0.5);
const float DEFAULT_BOUNCINESS = float(0.5);
/// Default rolling resistance
const decimal DEFAULT_ROLLING_RESISTANCE = decimal(0.0);
const float DEFAULT_ROLLING_RESISTANCE = float(0.0);
/// True if the spleeping technique is enabled
const bool SPLEEPING_ENABLED = true;
/// Object margin for collision detection in meters (for the GJK-EPA Algorithm)
const decimal OBJECT_MARGIN = decimal(0.04);
const float OBJECT_MARGIN = float(0.04);
/// Distance threshold for two contact points for a valid persistent contact (in meters)
const decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03);
const float PERSISTENT_CONTACT_DIST_THRESHOLD = float(0.03);
/// Velocity threshold for contact velocity restitution
const decimal RESTITUTION_VELOCITY_THRESHOLD = decimal(1.0);
const float RESTITUTION_VELOCITY_THRESHOLD = float(1.0);
/// Number of iterations when solving the velocity constraints of the Sequential Impulse technique
const uint DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 10;
const uint32_t DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 10;
/// Number of iterations when solving the position constraints of the Sequential Impulse technique
const uint DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5;
const uint32_t DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5;
/// Time (in seconds) that a body must stay still to be considered sleeping
const float DEFAULT_TIME_BEFORE_SLEEP = 1.0f;
/// A body with a linear velocity smaller than the sleep linear velocity (in m/s)
/// might enter sleeping mode.
const decimal DEFAULT_SLEEP_LINEAR_VELOCITY = decimal(0.02);
const float DEFAULT_SLEEP_LINEAR_VELOCITY = float(0.02);
/// A body with angular velocity smaller than the sleep angular velocity (in rad/s)
/// might enter sleeping mode
const decimal DEFAULT_SLEEP_ANGULAR_VELOCITY = decimal(3.0 * (PI / 180.0));
const float DEFAULT_SLEEP_ANGULAR_VELOCITY = float(3.0 * (PI / 180.0));
/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are
/// 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 decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1);
const float DYNAMIC_TREE_AABB_GAP = float(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);
const float DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = float(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;
const int32_t 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;
const int32_t NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3;
}
#endif

45
ephysics/constraint/BallAndSocketJoint.cpp
View File

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/constraint/BallAndSocketJoint.h>
@ -30,7 +11,7 @@
using namespace reactphysics3d;
// Static variables definition
const decimal BallAndSocketJoint::BETA = decimal(0.2);
const float BallAndSocketJoint::BETA = float(0.2);
// Constructor
BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo)
@ -72,7 +53,7 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// 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,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
@ -88,7 +69,7 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
// Compute the bias "b" of the constraint
mBiasVector.setToZero();
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
decimal biasFactor = (BETA / constraintSolverData.timeStep);
float biasFactor = (BETA / constraintSolverData.timeStep);
mBiasVector = biasFactor * (x2 + mR2World - x1 - mR1World);
}
@ -171,8 +152,8 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
float inverseMassBody1 = mBody1->mMassInverse;
float inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
@ -187,7 +168,7 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// 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,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
@ -216,7 +197,7 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
// Update the body center of mass and orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize();
// Compute the impulse of body 2
@ -228,7 +209,7 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize();
}

37
ephysics/constraint/BallAndSocketJoint.h
View File

@ -1,30 +1,9 @@
/********************************************************************************
* 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_BALL_AND_SOCKET_JOINT_H
#define REACTPHYSICS3D_BALL_AND_SOCKET_JOINT_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/constraint/Joint.h>
@ -72,7 +51,7 @@ class BallAndSocketJoint : public Joint {
// -------------------- Constants -------------------- //
// Beta value for the bias factor of position correction
static const decimal BETA;
static const float BETA;
// -------------------- Attributes -------------------- //
@ -143,5 +122,3 @@ inline size_t BallAndSocketJoint::getSizeInBytes() const {
}
}
#endif

29
ephysics/constraint/ContactPoint.cpp
View File

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/constraint/ContactPoint.h>

79
ephysics/constraint/ContactPoint.h
View File

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

61
ephysics/constraint/FixedJoint.cpp
View File

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/constraint/FixedJoint.h>
@ -30,7 +11,7 @@
using namespace reactphysics3d;
// Static variables definition
const decimal FixedJoint::BETA = decimal(0.2);
const float FixedJoint::BETA = float(0.2);
// Constructor
FixedJoint::FixedJoint(const FixedJointInfo& jointInfo)
@ -80,7 +61,7 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// 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,
0, inverseMassBodies, 0,
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
decimal biasFactor = (BETA / constraintSolverData.timeStep);
float biasFactor = (BETA / constraintSolverData.timeStep);
mBiasTranslation.setToZero();
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBiasTranslation = biasFactor * (x2 + mR2World - x1 - mR1World);
@ -113,7 +94,7 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
currentOrientationDifference.normalize();
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
@ -135,8 +116,8 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass of the bodies
const decimal inverseMassBody1 = mBody1->mMassInverse;
const decimal inverseMassBody2 = mBody2->mMassInverse;
const float inverseMassBody1 = mBody1->mMassInverse;
const float inverseMassBody2 = mBody2->mMassInverse;
// Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1
Vector3 linearImpulseBody1 = -mImpulseTranslation;
@ -170,8 +151,8 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
float inverseMassBody1 = mBody1->mMassInverse;
float inverseMassBody2 = mBody2->mMassInverse;
// --------------- Translation Constraints --------------- //
@ -231,8 +212,8 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
float inverseMassBody1 = mBody1->mMassInverse;
float inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
@ -249,7 +230,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// --------------- 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,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
@ -276,7 +257,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize();
// Compute the impulse of body 2
@ -288,7 +269,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize();
// --------------- Rotation Constraints --------------- //
@ -304,7 +285,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
Quaternion currentOrientationDifference = q2 * q1.getInverse();
currentOrientationDifference.normalize();
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
Vector3 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation);
@ -316,14 +297,14 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
w1 = mI1 * angularImpulseBody1;
// 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();
// Compute the pseudo velocity of body 2
w2 = mI2 * lambdaRotation;
// 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();
}

37
ephysics/constraint/FixedJoint.h
View File

@ -1,30 +1,9 @@
/********************************************************************************
* 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_FIXED_JOINT_H
#define REACTPHYSICS3D_FIXED_JOINT_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/constraint/Joint.h>
@ -71,7 +50,7 @@ class FixedJoint : public Joint {
// -------------------- Constants -------------------- //
// Beta value for the bias factor of position correction
static const decimal BETA;
static const float BETA;
// -------------------- Attributes -------------------- //
@ -154,5 +133,3 @@ inline size_t FixedJoint::getSizeInBytes() const {
}
}
#endif

169
ephysics/constraint/HingeJoint.cpp
View File

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/constraint/HingeJoint.h>
@ -31,7 +12,7 @@
using namespace reactphysics3d;
// Static variables definition
const decimal HingeJoint::BETA = decimal(0.2);
const float HingeJoint::BETA = float(0.2);
// Constructor
HingeJoint::HingeJoint(const HingeJointInfo& jointInfo)
@ -91,11 +72,11 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
mR2World = orientationBody2 * mLocalAnchorPointBody2;
// 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
decimal lowerLimitError = hingeAngle - mLowerLimit;
decimal upperLimitError = mUpperLimit - hingeAngle;
float lowerLimitError = hingeAngle - mLowerLimit;
float upperLimitError = mUpperLimit - hingeAngle;
bool oldIsLowerLimitViolated = mIsLowerLimitViolated;
mIsLowerLimitViolated = lowerLimitError <= 0;
if (mIsLowerLimitViolated != oldIsLowerLimitViolated) {
@ -122,7 +103,7 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// 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,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
@ -135,7 +116,7 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Compute the bias "b" of the translation constraints
mBTranslation.setToZero();
decimal biasFactor = (BETA / constraintSolverData.timeStep);
float biasFactor = (BETA / constraintSolverData.timeStep);
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBTranslation = biasFactor * (x2 + mR2World - x1 - mR1World);
}
@ -145,13 +126,13 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
Vector3 I1C2CrossA1 = mI1 * mC2CrossA1;
Vector3 I2B2CrossA1 = mI2 * mB2CrossA1;
Vector3 I2C2CrossA1 = mI2 * mC2CrossA1;
const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) +
const float el11 = mB2CrossA1.dot(I1B2CrossA1) +
mB2CrossA1.dot(I2B2CrossA1);
const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) +
const float el12 = mB2CrossA1.dot(I1C2CrossA1) +
mB2CrossA1.dot(I2C2CrossA1);
const decimal el21 = mC2CrossA1.dot(I1B2CrossA1) +
const float el21 = mC2CrossA1.dot(I1B2CrossA1) +
mC2CrossA1.dot(I2B2CrossA1);
const decimal el22 = mC2CrossA1.dot(I1C2CrossA1) +
const float el22 = mC2CrossA1.dot(I1C2CrossA1) +
mC2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
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)
mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1);
mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0);
float(1.0) / mInverseMassMatrixLimitMotor : float(0.0);
if (mIsLimitEnabled) {
@ -211,8 +192,8 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = mBody1->mMassInverse;
const decimal inverseMassBody2 = mBody2->mMassInverse;
const float inverseMassBody1 = mBody1->mMassInverse;
const float inverseMassBody2 = mBody2->mMassInverse;
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y;
@ -267,8 +248,8 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
float inverseMassBody1 = mBody1->mMassInverse;
float inverseMassBody2 = mBody2->mMassInverse;
// --------------- Translation Constraints --------------- //
@ -327,12 +308,12 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
if (mIsLowerLimitViolated) {
// 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
decimal deltaLambdaLower = mInverseMassMatrixLimitMotor * (-JvLowerLimit -mBLowerLimit);
decimal lambdaTemp = mImpulseLowerLimit;
mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0));
float deltaLambdaLower = mInverseMassMatrixLimitMotor * (-JvLowerLimit -mBLowerLimit);
float lambdaTemp = mImpulseLowerLimit;
mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, float(0.0));
deltaLambdaLower = mImpulseLowerLimit - lambdaTemp;
// 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) {
// 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
decimal deltaLambdaUpper = mInverseMassMatrixLimitMotor * (-JvUpperLimit -mBUpperLimit);
decimal lambdaTemp = mImpulseUpperLimit;
mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0));
float deltaLambdaUpper = mInverseMassMatrixLimitMotor * (-JvUpperLimit -mBUpperLimit);
float lambdaTemp = mImpulseUpperLimit;
mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, float(0.0));
deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp;
// 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) {
// 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
const decimal maxMotorImpulse = mMaxMotorTorque * constraintSolverData.timeStep;
decimal deltaLambdaMotor = mInverseMassMatrixLimitMotor * (-JvMotor - mMotorSpeed);
decimal lambdaTemp = mImpulseMotor;
const float maxMotorImpulse = mMaxMotorTorque * constraintSolverData.timeStep;
float deltaLambdaMotor = mInverseMassMatrixLimitMotor * (-JvMotor - mMotorSpeed);
float lambdaTemp = mImpulseMotor;
mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
deltaLambdaMotor = mImpulseMotor - lambdaTemp;
@ -417,8 +398,8 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
float inverseMassBody1 = mBody1->mMassInverse;
float inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
@ -429,11 +410,11 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
mR2World = q2 * mLocalAnchorPointBody2;
// 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
decimal lowerLimitError = hingeAngle - mLowerLimit;
decimal upperLimitError = mUpperLimit - hingeAngle;
float lowerLimitError = hingeAngle - mLowerLimit;
float upperLimitError = mUpperLimit - hingeAngle;
mIsLowerLimitViolated = lowerLimitError <= 0;
mIsUpperLimitViolated = upperLimitError <= 0;
@ -454,7 +435,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// --------------- 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,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
@ -481,7 +462,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize();
// Compute the impulse of body 2
@ -493,7 +474,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize();
// --------------- Rotation Constraints --------------- //
@ -503,13 +484,13 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
Vector3 I1C2CrossA1 = mI1 * mC2CrossA1;
Vector3 I2B2CrossA1 = mI2 * mB2CrossA1;
Vector3 I2C2CrossA1 = mI2 * mC2CrossA1;
const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) +
const float el11 = mB2CrossA1.dot(I1B2CrossA1) +
mB2CrossA1.dot(I2B2CrossA1);
const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) +
const float el12 = mB2CrossA1.dot(I1C2CrossA1) +
mB2CrossA1.dot(I2C2CrossA1);
const decimal el21 = mC2CrossA1.dot(I1B2CrossA1) +
const float el21 = mC2CrossA1.dot(I1B2CrossA1) +
mC2CrossA1.dot(I2B2CrossA1);
const decimal el22 = mC2CrossA1.dot(I1C2CrossA1) +
const float el22 = mC2CrossA1.dot(I1C2CrossA1) +
mC2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mInverseMassMatrixRotation.setToZero();
@ -530,7 +511,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
w1 = mI1 * angularImpulseBody1;
// 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();
// Compute the impulse of body 2
@ -540,7 +521,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
w2 = mI2 * angularImpulseBody2;
// 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();
// --------------- 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)
mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1);
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 (mIsLowerLimitViolated) {
// 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
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1;
@ -568,7 +549,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
const Vector3 w1 = mI1 * angularImpulseBody1;
// 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();
// 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;
// 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();
}
@ -586,7 +567,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
if (mIsUpperLimitViolated) {
// 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
const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1;
@ -595,7 +576,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
const Vector3 w1 = mI1 * angularImpulseBody1;
// 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();
// 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;
// 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();
}
}
@ -647,7 +628,7 @@ void HingeJoint::enableMotor(bool isMotorEnabled) {
/**
* @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);
@ -664,7 +645,7 @@ void HingeJoint::setMinAngleLimit(decimal lowerLimit) {
/**
* @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);
@ -690,7 +671,7 @@ void HingeJoint::resetLimits() {
}
// Set the motor speed
void HingeJoint::setMotorSpeed(decimal motorSpeed) {
void HingeJoint::setMotorSpeed(float motorSpeed) {
if (motorSpeed != mMotorSpeed) {
@ -706,7 +687,7 @@ void HingeJoint::setMotorSpeed(decimal motorSpeed) {
/**
* @param maxMotorTorque The maximum torque (in Newtons) of the joint motor
*/
void HingeJoint::setMaxMotorTorque(decimal maxMotorTorque) {
void HingeJoint::setMaxMotorTorque(float maxMotorTorque) {
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]
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);
// Convert it into the range [-pi; pi]
// Convert it int32_to the range [-pi; pi]
if (angle < -PI) {
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
// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the
// two angle limits in arguments.
decimal HingeJoint::computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle,
decimal upperLimitAngle) const {
float HingeJoint::computeCorrespondingAngleNearLimits(float inputAngle, float lowerLimitAngle,
float upperLimitAngle) const {
if (upperLimitAngle <= lowerLimitAngle) {
return inputAngle;
}
else if (inputAngle > upperLimitAngle) {
decimal diffToUpperLimit = fabs(computeNormalizedAngle(inputAngle - upperLimitAngle));
decimal diffToLowerLimit = fabs(computeNormalizedAngle(inputAngle - lowerLimitAngle));
float diffToUpperLimit = fabs(computeNormalizedAngle(inputAngle - upperLimitAngle));
float diffToLowerLimit = fabs(computeNormalizedAngle(inputAngle - lowerLimitAngle));
return (diffToUpperLimit > diffToLowerLimit) ? (inputAngle - PI_TIMES_2) : inputAngle;
}
else if (inputAngle < lowerLimitAngle) {
decimal diffToUpperLimit = fabs(computeNormalizedAngle(upperLimitAngle - inputAngle));
decimal diffToLowerLimit = fabs(computeNormalizedAngle(lowerLimitAngle - inputAngle));
float diffToUpperLimit = fabs(computeNormalizedAngle(upperLimitAngle - inputAngle));
float diffToLowerLimit = fabs(computeNormalizedAngle(lowerLimitAngle - inputAngle));
return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + PI_TIMES_2);
}
else {
@ -761,10 +742,10 @@ decimal HingeJoint::computeCorrespondingAngleNearLimits(decimal inputAngle, deci
}
// Compute the current angle around the hinge axis
decimal HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1,
float HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1,
const Quaternion& orientationBody2) {
decimal hingeAngle;
float hingeAngle;
// Compute the current orientation difference between the two bodies
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
// 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).
decimal cosHalfAngle = relativeRotation.w;
decimal sinHalfAngleAbs = relativeRotation.getVectorV().length();
float cosHalfAngle = relativeRotation.w;
float sinHalfAngleAbs = relativeRotation.getVectorV().length();
// 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 (dotProduct >= decimal(0.0)) {
hingeAngle = decimal(2.0) * std::atan2(sinHalfAngleAbs, cosHalfAngle);
if (dotProduct >= float(0.0)) {
hingeAngle = float(2.0) * std::atan2(sinHalfAngleAbs, cosHalfAngle);
}
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);
// Compute and return the corresponding angle near one the two limits

117
ephysics/constraint/HingeJoint.h
View File

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

30
ephysics/constraint/Joint.cpp
View File

@ -1,27 +1,9 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/constraint/Joint.h>

45
ephysics/constraint/Joint.h
View File

@ -1,30 +1,9 @@
/********************************************************************************
* 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_CONSTRAINT_H
#define REACTPHYSICS3D_CONSTRAINT_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/configuration.h>
@ -130,10 +109,10 @@ class Joint {
const JointType mType;
/// 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
uint mIndexBody2;
uint32_t mIndexBody2;
/// Position correction technique used for the constraint (used for joints)
JointsPositionCorrectionTechnique mPositionCorrectionTechnique;
@ -141,7 +120,7 @@ class Joint {
/// True if the two bodies of the constraint are allowed to collide with each other
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;
// -------------------- Methods -------------------- //
@ -152,7 +131,7 @@ class Joint {
/// Private assignment operator
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;
/// Return the number of bytes used by the joint
@ -243,11 +222,9 @@ inline bool Joint::isCollisionEnabled() const {
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 {
return mIsAlreadyInIsland;
}
}
#endif

147
ephysics/constraint/SliderJoint.cpp
View File

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/constraint/SliderJoint.h>
@ -29,7 +10,7 @@
using namespace reactphysics3d;
// Static variables definition
const decimal SliderJoint::BETA = decimal(0.2);
const float SliderJoint::BETA = float(0.2);
// Constructor
SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
@ -99,9 +80,9 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
mN2 = mSliderAxisWorld.cross(mN1);
// Check if the limit constraints are violated or not
decimal uDotSliderAxis = u.dot(mSliderAxisWorld);
decimal lowerLimitError = uDotSliderAxis - mLowerLimit;
decimal upperLimitError = mUpperLimit - uDotSliderAxis;
float uDotSliderAxis = u.dot(mSliderAxisWorld);
float lowerLimitError = uDotSliderAxis - mLowerLimit;
float upperLimitError = mUpperLimit - uDotSliderAxis;
bool oldIsLowerLimitViolated = mIsLowerLimitViolated;
mIsLowerLimitViolated = lowerLimitError <= 0;
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
// constraints (2x2 matrix)
decimal sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse;
float sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse;
Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
Vector3 I2R2CrossN1 = mI2 * mR2CrossN1;
Vector3 I2R2CrossN2 = mI2 * mR2CrossN2;
const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
const float el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1);
const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
const float el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
mR2CrossN1.dot(I2R2CrossN2);
const decimal el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
const float el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
mR2CrossN2.dot(I2R2CrossN1);
const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
const float el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
mR2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
mInverseMassMatrixTranslationConstraint.setToZero();
@ -145,7 +126,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Compute the bias "b" of the translation constraint
mBTranslation.setToZero();
decimal biasFactor = (BETA / constraintSolverData.timeStep);
float biasFactor = (BETA / constraintSolverData.timeStep);
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBTranslation.x = u.dot(mN1);
mBTranslation.y = u.dot(mN2);
@ -165,7 +146,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
mBRotation = biasFactor * decimal(2.0) * qError.getVectorV();
mBRotation = biasFactor * float(2.0) * qError.getVectorV();
}
// If the limits are enabled
@ -176,7 +157,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
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
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)
mInverseMassMatrixMotor = mBody1->mMassInverse + mBody2->mMassInverse;
mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ?
decimal(1.0) / mInverseMassMatrixMotor : decimal(0.0);
float(1.0) / mInverseMassMatrixMotor : float(0.0);
}
// If warm-starting is not enabled
@ -222,11 +203,11 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = mBody1->mMassInverse;
const decimal inverseMassBody2 = mBody2->mMassInverse;
const float inverseMassBody1 = mBody1->mMassInverse;
const float inverseMassBody2 = mBody2->mMassInverse;
// 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;
// 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];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
float inverseMassBody1 = mBody1->mMassInverse;
float inverseMassBody2 = mBody2->mMassInverse;
// --------------- 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);
const decimal el2 = -mN2.dot(v1) - w1.dot(mR1PlusUCrossN2) +
const float el2 = -mN2.dot(v1) - w1.dot(mR1PlusUCrossN2) +
mN2.dot(v2) + w2.dot(mR2CrossN2);
const Vector2 JvTranslation(el1, el2);
@ -343,13 +324,13 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
if (mIsLowerLimitViolated) {
// 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);
// Compute the Lagrange multiplier lambda for the lower limit constraint
decimal deltaLambdaLower = mInverseMassMatrixLimit * (-JvLowerLimit -mBLowerLimit);
decimal lambdaTemp = mImpulseLowerLimit;
mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0));
float deltaLambdaLower = mInverseMassMatrixLimit * (-JvLowerLimit -mBLowerLimit);
float lambdaTemp = mImpulseLowerLimit;
mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, float(0.0));
deltaLambdaLower = mImpulseLowerLimit - lambdaTemp;
// 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) {
// 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);
// Compute the Lagrange multiplier lambda for the upper limit constraint
decimal deltaLambdaUpper = mInverseMassMatrixLimit * (-JvUpperLimit -mBUpperLimit);
decimal lambdaTemp = mImpulseUpperLimit;
mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0));
float deltaLambdaUpper = mInverseMassMatrixLimit * (-JvUpperLimit -mBUpperLimit);
float lambdaTemp = mImpulseUpperLimit;
mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, float(0.0));
deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp;
// 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) {
// 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
const decimal maxMotorImpulse = mMaxMotorForce * constraintSolverData.timeStep;
decimal deltaLambdaMotor = mInverseMassMatrixMotor * (-JvMotor - mMotorSpeed);
decimal lambdaTemp = mImpulseMotor;
const float maxMotorImpulse = mMaxMotorForce * constraintSolverData.timeStep;
float deltaLambdaMotor = mInverseMassMatrixMotor * (-JvMotor - mMotorSpeed);
float lambdaTemp = mImpulseMotor;
mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
deltaLambdaMotor = mImpulseMotor - lambdaTemp;
@ -442,8 +423,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
float inverseMassBody1 = mBody1->mMassInverse;
float inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld();
@ -463,9 +444,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
mN2 = mSliderAxisWorld.cross(mN1);
// Check if the limit constraints are violated or not
decimal uDotSliderAxis = u.dot(mSliderAxisWorld);
decimal lowerLimitError = uDotSliderAxis - mLowerLimit;
decimal upperLimitError = mUpperLimit - uDotSliderAxis;
float uDotSliderAxis = u.dot(mSliderAxisWorld);
float lowerLimitError = uDotSliderAxis - mLowerLimit;
float upperLimitError = mUpperLimit - uDotSliderAxis;
mIsLowerLimitViolated = lowerLimitError <= 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
// constraints (2x2 matrix)
decimal sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse;
float sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse;
Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
Vector3 I2R2CrossN1 = mI2 * mR2CrossN1;
Vector3 I2R2CrossN2 = mI2 * mR2CrossN2;
const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
const float el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1);
const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
const float el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
mR2CrossN1.dot(I2R2CrossN2);
const decimal el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
const float el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
mR2CrossN2.dot(I2R2CrossN1);
const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
const float el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
mR2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
mInverseMassMatrixTranslationConstraint.setToZero();
@ -518,7 +499,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize();
// 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
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize();
// --------------- Rotation Constraints --------------- //
@ -548,7 +529,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
Quaternion currentOrientationDifference = q2 * q1.getInverse();
currentOrientationDifference.normalize();
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
Vector3 lambdaRotation = mInverseMassMatrixRotationConstraint * (-errorRotation);
@ -560,7 +541,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
w1 = mI1 * angularImpulseBody1;
// 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();
// 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;
// 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();
// --------------- Limits Constraints --------------- //
@ -584,14 +565,14 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
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 (mIsLowerLimitViolated) {
// 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
const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld;
@ -603,7 +584,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize();
// 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
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize();
}
@ -624,7 +605,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
if (mIsUpperLimitViolated) {
// 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
const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld;
@ -636,7 +617,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize();
// 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
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2 += Quaternion(0, w2) * q2 * float(0.5);
q2.normalize();
}
}
@ -690,7 +671,7 @@ void SliderJoint::enableMotor(bool isMotorEnabled) {
/**
* @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
@ -719,7 +700,7 @@ decimal SliderJoint::getTranslation() const {
/**
* @param lowerLimit The minimum translation limit of the joint (in meters)
*/
void SliderJoint::setMinTranslationLimit(decimal lowerLimit) {
void SliderJoint::setMinTranslationLimit(float lowerLimit) {
assert(lowerLimit <= mUpperLimit);
@ -736,7 +717,7 @@ void SliderJoint::setMinTranslationLimit(decimal lowerLimit) {
/**
* @param lowerLimit The maximum translation limit of the joint (in meters)
*/
void SliderJoint::setMaxTranslationLimit(decimal upperLimit) {
void SliderJoint::setMaxTranslationLimit(float upperLimit) {
assert(mLowerLimit <= upperLimit);
@ -765,7 +746,7 @@ void SliderJoint::resetLimits() {
/**
* @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) {
@ -781,7 +762,7 @@ void SliderJoint::setMotorSpeed(decimal motorSpeed) {
/**
* @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) {

107
ephysics/constraint/SliderJoint.h
View File

@ -1,30 +1,9 @@
/********************************************************************************
* 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_SLIDER_JOINT_H
#define REACTPHYSICS3D_SLIDER_JOINT_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/mathematics/mathematics.h>
@ -56,16 +35,16 @@ struct SliderJointInfo : public JointInfo {
bool isMotorEnabled;
/// Mininum allowed translation if limits are enabled
decimal minTranslationLimit;
float minTranslationLimit;
/// Maximum allowed translation if limits are enabled
decimal maxTranslationLimit;
float maxTranslationLimit;
/// Motor speed
decimal motorSpeed;
float motorSpeed;
/// 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
/**
@ -95,7 +74,7 @@ struct SliderJointInfo : public JointInfo {
SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace,
const Vector3& initSliderAxisWorldSpace,
decimal initMinTranslationLimit, decimal initMaxTranslationLimit)
float initMinTranslationLimit, float initMaxTranslationLimit)
: JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace),
sliderAxisWorldSpace(initSliderAxisWorldSpace),
@ -118,8 +97,8 @@ struct SliderJointInfo : public JointInfo {
SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace,
const Vector3& initSliderAxisWorldSpace,
decimal initMinTranslationLimit, decimal initMaxTranslationLimit,
decimal initMotorSpeed, decimal initMaxMotorForce)
float initMinTranslationLimit, float initMaxTranslationLimit,
float initMotorSpeed, float initMaxMotorForce)
: JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace),
sliderAxisWorldSpace(initSliderAxisWorldSpace),
@ -142,7 +121,7 @@ class SliderJoint : public Joint {
// -------------------- Constants -------------------- //
// Beta value for the position correction bias factor
static const decimal BETA;
static const float BETA;
// -------------------- Attributes -------------------- //
@ -201,10 +180,10 @@ class SliderJoint : public Joint {
Vector3 mBRotation;
/// Bias of the lower limit constraint
decimal mBLowerLimit;
float mBLowerLimit;
/// Bias of the upper limit constraint
decimal mBUpperLimit;
float mBUpperLimit;
/// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix)
Matrix2x2 mInverseMassMatrixTranslationConstraint;
@ -213,10 +192,10 @@ class SliderJoint : public Joint {
Matrix3x3 mInverseMassMatrixRotationConstraint;
/// 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
decimal mInverseMassMatrixMotor;
float mInverseMassMatrixMotor;
/// Accumulated impulse for the 2 translation constraints
Vector2 mImpulseTranslation;
@ -225,13 +204,13 @@ class SliderJoint : public Joint {
Vector3 mImpulseRotation;
/// Accumulated impulse for the lower limit constraint
decimal mImpulseLowerLimit;
float mImpulseLowerLimit;
/// Accumulated impulse for the upper limit constraint
decimal mImpulseUpperLimit;
float mImpulseUpperLimit;
/// Accumulated impulse for the motor
decimal mImpulseMotor;
float mImpulseMotor;
/// True if the slider limits are enabled
bool mIsLimitEnabled;
@ -243,10 +222,10 @@ class SliderJoint : public Joint {
Vector3 mSliderAxisWorld;
/// Lower limit (minimum translation distance)
decimal mLowerLimit;
float mLowerLimit;
/// Upper limit (maximum translation distance)
decimal mUpperLimit;
float mUpperLimit;
/// True if the lower limit is violated
bool mIsLowerLimitViolated;
@ -255,10 +234,10 @@ class SliderJoint : public Joint {
bool mIsUpperLimitViolated;
/// Motor speed (in m/s)
decimal mMotorSpeed;
float mMotorSpeed;
/// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
decimal mMaxMotorForce;
float mMaxMotorForce;
// -------------------- Methods -------------------- //
@ -309,34 +288,34 @@ class SliderJoint : public Joint {
void enableMotor(bool isMotorEnabled);
/// Return the current translation value of the joint
decimal getTranslation() const;
float getTranslation() const;
/// Return the minimum translation limit
decimal getMinTranslationLimit() const;
float getMinTranslationLimit() const;
/// Set the minimum translation limit
void setMinTranslationLimit(decimal lowerLimit);
void setMinTranslationLimit(float lowerLimit);
/// Return the maximum translation limit
decimal getMaxTranslationLimit() const;
float getMaxTranslationLimit() const;
/// Set the maximum translation limit
void setMaxTranslationLimit(decimal upperLimit);
void setMaxTranslationLimit(float upperLimit);
/// Return the motor speed
decimal getMotorSpeed() const;
float getMotorSpeed() const;
/// Set the motor speed
void setMotorSpeed(decimal motorSpeed);
void setMotorSpeed(float motorSpeed);
/// Return the maximum motor force
decimal getMaxMotorForce() const;
float getMaxMotorForce() const;
/// 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
decimal getMotorForce(decimal timeStep) const;
/// Return the int32_tensity of the current force applied for the joint motor
float getMotorForce(float timeStep) const;
};
// 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)
*/
inline decimal SliderJoint::getMinTranslationLimit() const {
inline float SliderJoint::getMinTranslationLimit() const {
return mLowerLimit;
}
@ -367,7 +346,7 @@ inline decimal SliderJoint::getMinTranslationLimit() const {
/**
* @return The maximum translation limit of the joint (in meters)
*/
inline decimal SliderJoint::getMaxTranslationLimit() const {
inline float SliderJoint::getMaxTranslationLimit() const {
return mUpperLimit;
}
@ -375,7 +354,7 @@ inline decimal SliderJoint::getMaxTranslationLimit() const {
/**
* @return The current motor speed of the joint (in meters per second)
*/
inline decimal SliderJoint::getMotorSpeed() const {
inline float SliderJoint::getMotorSpeed() const {
return mMotorSpeed;
}
@ -383,16 +362,16 @@ inline decimal SliderJoint::getMotorSpeed() const {
/**
* @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 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)
* @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;
}
@ -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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/engine/CollisionWorld.h>
@ -161,9 +142,9 @@ void CollisionWorld::testCollision(const ProxyShape* shape,
resetContactManifoldListsOfBodies();
// Create the sets of shapes
std::set<uint> shapes;
std::set<uint32_t> shapes;
shapes.insert(shape->mBroadPhaseID);
std::set<uint> emptySet;
std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts
mCollisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet);
@ -183,9 +164,9 @@ void CollisionWorld::testCollision(const ProxyShape* shape1,
resetContactManifoldListsOfBodies();
// Create the sets of shapes
std::set<uint> shapes1;
std::set<uint32_t> shapes1;
shapes1.insert(shape1->mBroadPhaseID);
std::set<uint> shapes2;
std::set<uint32_t> shapes2;
shapes2.insert(shape2->mBroadPhaseID);
// Perform the collision detection and report contacts
@ -205,7 +186,7 @@ void CollisionWorld::testCollision(const CollisionBody* body,
resetContactManifoldListsOfBodies();
// Create the sets of shapes
std::set<uint> shapes1;
std::set<uint32_t> shapes1;
// For each shape of the body
for (const ProxyShape* shape=body->getProxyShapesList(); shape != NULL;
@ -213,7 +194,7 @@ void CollisionWorld::testCollision(const CollisionBody* body,
shapes1.insert(shape->mBroadPhaseID);
}
std::set<uint> emptySet;
std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts
mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet);
@ -233,13 +214,13 @@ void CollisionWorld::testCollision(const CollisionBody* body1,
resetContactManifoldListsOfBodies();
// Create the sets of shapes
std::set<uint> shapes1;
std::set<uint32_t> shapes1;
for (const ProxyShape* shape=body1->getProxyShapesList(); shape != NULL;
shape = shape->getNext()) {
shapes1.insert(shape->mBroadPhaseID);
}
std::set<uint> shapes2;
std::set<uint32_t> shapes2;
for (const ProxyShape* shape=body2->getProxyShapesList(); shape != NULL;
shape = shape->getNext()) {
shapes2.insert(shape->mBroadPhaseID);
@ -258,7 +239,7 @@ void CollisionWorld::testCollision(CollisionCallback* callback) {
// Reset all the contact manifolds lists of each body
resetContactManifoldListsOfBodies();
std::set<uint> emptySet;
std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts
mCollisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet);

37
ephysics/engine/CollisionWorld.h
View File

@ -1,30 +1,9 @@
/********************************************************************************
* 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_COLLISION_WORLD_H
#define REACTPHYSICS3D_COLLISION_WORLD_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <vector>
@ -70,7 +49,7 @@ class CollisionWorld {
bodyindex mCurrentBodyID;
/// List of free ID for rigid bodies
std::vector<luint> mFreeBodiesIDs;
std::vector<uint64_t> mFreeBodiesIDs;
/// Memory allocator
MemoryAllocator mMemoryAllocator;
@ -233,5 +212,3 @@ class CollisionCallback {
};
}
#endif

39
ephysics/engine/ConstraintSolver.cpp
View File

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/engine/ConstraintSolver.h>
@ -30,7 +11,7 @@
using namespace reactphysics3d;
// Constructor
ConstraintSolver::ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
ConstraintSolver::ConstraintSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex)
: mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mConstraintSolverData(mapBodyToVelocityIndex) {
@ -42,7 +23,7 @@ ConstraintSolver::~ConstraintSolver() {
}
// 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()");
@ -59,7 +40,7 @@ void ConstraintSolver::initializeForIsland(decimal dt, Island* island) {
// For each joint of the island
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
joints[i]->initBeforeSolve(mConstraintSolverData);
@ -81,7 +62,7 @@ void ConstraintSolver::solveVelocityConstraints(Island* island) {
// For each joint of the island
Joint** joints = island->getJoints();
for (uint i=0; i<island->getNbJoints(); i++) {
for (uint32_t i=0; i<island->getNbJoints(); i++) {
// Solve the constraint
joints[i]->solveVelocityConstraint(mConstraintSolverData);
@ -98,7 +79,7 @@ void ConstraintSolver::solvePositionConstraints(Island* island) {
// For each joint of the island
Joint** joints = island->getJoints();
for (uint i=0; i < island->getNbJoints(); i++) {
for (uint32_t i=0; i < island->getNbJoints(); i++) {
// Solve the constraint
joints[i]->solvePositionConstraint(mConstraintSolverData);

53
ephysics/engine/ConstraintSolver.h
View File

@ -1,30 +1,9 @@
/********************************************************************************
* 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_CONSTRAINT_SOLVER_H
#define REACTPHYSICS3D_CONSTRAINT_SOLVER_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/configuration.h>
@ -46,7 +25,7 @@ struct ConstraintSolverData {
public :
/// Current time step of the simulation
decimal timeStep;
float timeStep;
/// Array with the bodies linear velocities
Vector3* linearVelocities;
@ -62,13 +41,13 @@ struct ConstraintSolverData {
/// Reference to the map that associates rigid body to their index
/// 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
bool isWarmStartingActive;
/// Constructor
ConstraintSolverData(const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
ConstraintSolverData(const std::map<RigidBody*, uint32_t>& refMapBodyToConstrainedVelocityIndex)
:linearVelocities(NULL), angularVelocities(NULL),
positions(NULL), orientations(NULL),
mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
@ -99,7 +78,7 @@ struct ConstraintSolverData {
*
* --- 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.
*
* v2' = v1 + dt * M^-1 * F_a
@ -120,7 +99,7 @@ struct ConstraintSolverData {
*
* --- 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.
*
* 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
/// the constrained velocities array
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
const std::map<RigidBody*, uint32_t>& mMapBodyToConstrainedVelocityIndex;
/// Current time step
decimal mTimeStep;
float mTimeStep;
/// True if the warm starting of the solver is active
bool mIsWarmStartingActive;
@ -170,13 +149,13 @@ class ConstraintSolver {
// -------------------- Methods -------------------- //
/// Constructor
ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
ConstraintSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex);
/// Destructor
~ConstraintSolver();
/// Initialize the constraint solver for a given island
void initializeForIsland(decimal dt, Island* island);
void initializeForIsland(float dt, Island* island);
/// Solve the constraints
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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/engine/ContactSolver.h>
@ -34,12 +15,12 @@ using namespace reactphysics3d;
using namespace std;
// Constants initialization
const decimal ContactSolver::BETA = decimal(0.2);
const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
const decimal ContactSolver::SLOP= decimal(0.01);
const float ContactSolver::BETA = float(0.2);
const float ContactSolver::BETA_SPLIT_IMPULSE = float(0.2);
const float ContactSolver::SLOP= float(0.01);
// Constructor
ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
ContactSolver::ContactSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex)
:mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
mContactConstraints(NULL), mLinearVelocities(NULL), mAngularVelocities(NULL),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
@ -54,7 +35,7 @@ ContactSolver::~ContactSolver() {
}
// 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()");
@ -74,11 +55,11 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
// For each contact manifold of the island
ContactManifold** contactManifolds = island->getContactManifold();
for (uint i=0; i<mNbContactManifolds; i++) {
for (uint32_t i=0; i<mNbContactManifolds; i++) {
ContactManifold* externalManifold = contactManifolds[i];
ContactManifoldSolver& internalManifold = mContactConstraints[i];
ContactManifoldSolver& int32_ternalManifold = mContactConstraints[i];
assert(externalManifold->getNbContactPoints() > 0);
@ -92,32 +73,32 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
const Vector3& x1 = body1->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
internalManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
internalManifold.massInverseBody1 = body1->mMassInverse;
internalManifold.massInverseBody2 = body2->mMassInverse;
internalManifold.nbContacts = externalManifold->getNbContactPoints();
internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2);
internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
internalManifold.rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
internalManifold.externalContactManifold = externalManifold;
internalManifold.isBody1DynamicType = body1->getType() == DYNAMIC;
internalManifold.isBody2DynamicType = body2->getType() == DYNAMIC;
int32_ternalManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
int32_ternalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
int32_ternalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
int32_ternalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
int32_ternalManifold.massInverseBody1 = body1->mMassInverse;
int32_ternalManifold.massInverseBody2 = body2->mMassInverse;
int32_ternalManifold.nbContacts = externalManifold->getNbContactPoints();
int32_ternalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2);
int32_ternalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
int32_ternalManifold.rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
int32_ternalManifold.externalContactManifold = externalManifold;
int32_ternalManifold.isBody1DynamicType = body1->getType() == DYNAMIC;
int32_ternalManifold.isBody2DynamicType = body2->getType() == DYNAMIC;
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
internalManifold.frictionPointBody1 = Vector3::zero();
internalManifold.frictionPointBody2 = Vector3::zero();
int32_ternalManifold.frictionPointBody1 = Vector3::zero();
int32_ternalManifold.frictionPointBody2 = Vector3::zero();
}
// 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
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 (mIsSolveFrictionAtContactManifoldCenterActive) {
internalManifold.frictionPointBody1 += p1;
internalManifold.frictionPointBody2 += p2;
int32_ternalManifold.frictionPointBody1 += p1;
int32_ternalManifold.frictionPointBody2 += p2;
}
}
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
internalManifold.frictionPointBody1 /=static_cast<decimal>(internalManifold.nbContacts);
internalManifold.frictionPointBody2 /=static_cast<decimal>(internalManifold.nbContacts);
internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1;
internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2;
internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1();
internalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2();
int32_ternalManifold.frictionPointBody1 /=static_cast<float>(int32_ternalManifold.nbContacts);
int32_ternalManifold.frictionPointBody2 /=static_cast<float>(int32_ternalManifold.nbContacts);
int32_ternalManifold.r1Friction = int32_ternalManifold.frictionPointBody1 - x1;
int32_ternalManifold.r2Friction = int32_ternalManifold.frictionPointBody2 - x2;
int32_ternalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1();
int32_ternalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2();
// If warm starting is active
if (mIsWarmStartingActive) {
// Initialize the accumulated impulses with the previous step accumulated impulses
internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1();
internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2();
internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
int32_ternalManifold.friction1Impulse = externalManifold->getFrictionImpulse1();
int32_ternalManifold.friction2Impulse = externalManifold->getFrictionImpulse2();
int32_ternalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
}
else {
// Initialize the accumulated impulses to zero
internalManifold.friction1Impulse = 0.0;
internalManifold.friction2Impulse = 0.0;
internalManifold.frictionTwistImpulse = 0.0;
internalManifold.rollingResistanceImpulse = Vector3(0, 0, 0);
int32_ternalManifold.friction1Impulse = 0.0;
int32_ternalManifold.friction2Impulse = 0.0;
int32_ternalManifold.frictionTwistImpulse = 0.0;
int32_ternalManifold.rollingResistanceImpulse = Vector3(0, 0, 0);
}
}
}
@ -184,7 +165,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
void ContactSolver::initializeContactConstraints() {
// For each contact constraint
for (uint c=0; c<mNbContactManifolds; c++) {
for (uint32_t c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& manifold = mContactConstraints[c];
@ -204,7 +185,7 @@ void ContactSolver::initializeContactConstraints() {
const Vector3& w2 = mAngularVelocities[manifold.indexBody2];
// 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];
ContactPoint* externalContact = contactPoint.externalContact;
@ -216,12 +197,12 @@ void ContactSolver::initializeContactConstraints() {
contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal);
// 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) +
((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 :
decimal(0.0);
float(0.0);
// If we do not solve the friction constraints at the center of the contact manifold
if (!mIsSolveFrictionAtContactManifoldCenterActive) {
@ -236,22 +217,22 @@ void ContactSolver::initializeContactConstraints() {
// Compute the inverse mass matrix K for the friction
// constraints at each contact point
decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot(
contactPoint.frictionVector1) +
((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot(
contactPoint.frictionVector1);
decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot(
contactPoint.frictionVector2) +
((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot(
contactPoint.frictionVector2);
friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) /
friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = float(1.0) /
friction1Mass :
decimal(0.0);
friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = decimal(1.0) /
float(0.0);
friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = float(1.0) /
friction2Mass :
decimal(0.0);
float(0.0);
}
// 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
// velocity bellow a given threshold), we do not add a restitution velocity bias
contactPoint.restitutionBias = 0.0;
decimal deltaVDotN = deltaV.dot(contactPoint.normal);
float deltaVDotN = deltaV.dot(contactPoint.normal);
if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) {
contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN;
}
@ -307,27 +288,27 @@ void ContactSolver::initializeContactConstraints() {
manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2);
manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1);
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(
manifold.frictionVector1) +
((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot(
manifold.frictionVector1);
decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot(
manifold.frictionVector2) +
((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot(
manifold.frictionVector2);
decimal frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 *
float frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 *
manifold.normal) +
manifold.normal.dot(manifold.inverseInertiaTensorBody2 *
manifold.normal);
friction1Mass > 0.0 ? manifold.inverseFriction1Mass = decimal(1.0)/friction1Mass
: decimal(0.0);
friction2Mass > 0.0 ? manifold.inverseFriction2Mass = decimal(1.0)/friction2Mass
: decimal(0.0);
frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = decimal(1.0) /
friction1Mass > 0.0 ? manifold.inverseFriction1Mass = float(1.0)/friction1Mass
: float(0.0);
friction2Mass > 0.0 ? manifold.inverseFriction2Mass = float(1.0)/friction2Mass
: float(0.0);
frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = float(1.0) /
frictionTwistMass :
decimal(0.0);
float(0.0);
}
}
}
@ -342,13 +323,13 @@ void ContactSolver::warmStart() {
if (!mIsWarmStartingActive) return;
// For each constraint
for (uint c=0; c<mNbContactManifolds; c++) {
for (uint32_t c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c];
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];
@ -369,7 +350,7 @@ void ContactSolver::warmStart() {
// If we do not solve the friction constraints at the center of the contact manifold
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
Vector3 oldFrictionImpulse = contactPoint.friction1Impulse *
contactPoint.oldFrictionVector1 +
@ -425,7 +406,7 @@ void ContactSolver::warmStart() {
// at least one resting contact point in the contact manifold
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
Vector3 oldFrictionImpulse = contactManifold.friction1Impulse *
contactManifold.oldFrictionVector1 +
@ -510,15 +491,15 @@ void ContactSolver::solve() {
PROFILE("ContactSolver::solve()");
decimal deltaLambda;
decimal lambdaTemp;
float deltaLambda;
float lambdaTemp;
// For each contact manifold
for (uint c=0; c<mNbContactManifolds; c++) {
for (uint32_t c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c];
decimal sumPenetrationImpulse = 0.0;
float sumPenetrationImpulse = 0.0;
// Get the constrained velocities
const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
@ -526,7 +507,7 @@ void ContactSolver::solve() {
const Vector3& v2 = mLinearVelocities[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];
@ -534,15 +515,15 @@ void ContactSolver::solve() {
// Compute J*v
Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
decimal deltaVDotN = deltaV.dot(contactPoint.normal);
decimal Jv = deltaVDotN;
float deltaVDotN = deltaV.dot(contactPoint.normal);
float Jv = deltaVDotN;
// Compute the bias "b" of the constraint
decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
decimal biasPenetrationDepth = 0.0;
float beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
float biasPenetrationDepth = 0.0;
if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
max(0.0f, float(contactPoint.penetrationDepth - SLOP));
decimal b = biasPenetrationDepth + contactPoint.restitutionBias;
float b = biasPenetrationDepth + contactPoint.restitutionBias;
// Compute the Lagrange multiplier lambda
if (mIsSplitImpulseActive) {
@ -554,7 +535,7 @@ void ContactSolver::solve() {
}
lambdaTemp = contactPoint.penetrationImpulse;
contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse +
deltaLambda, decimal(0.0));
deltaLambda, float(0.0));
deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
@ -576,13 +557,13 @@ void ContactSolver::solve() {
const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2];
Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) -
v1Split - w1Split.cross(contactPoint.r1);
decimal JvSplit = deltaVSplit.dot(contactPoint.normal);
decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
float JvSplit = deltaVSplit.dot(contactPoint.normal);
float deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
contactPoint.inversePenetrationMass;
decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse;
float lambdaTempSplit = contactPoint.penetrationSplitImpulse;
contactPoint.penetrationSplitImpulse = std::max(
contactPoint.penetrationSplitImpulse +
deltaLambdaSplit, decimal(0.0));
deltaLambdaSplit, float(0.0));
deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
// Compute the impulse P=J^T * lambda
@ -604,7 +585,7 @@ void ContactSolver::solve() {
// Compute the Lagrange multiplier lambda
deltaLambda = -Jv;
deltaLambda *= contactPoint.inverseFriction1Mass;
decimal frictionLimit = contactManifold.frictionCoefficient *
float frictionLimit = contactManifold.frictionCoefficient *
contactPoint.penetrationImpulse;
lambdaTemp = contactPoint.friction1Impulse;
contactPoint.friction1Impulse = std::max(-frictionLimit,
@ -652,7 +633,7 @@ void ContactSolver::solve() {
// Compute the Lagrange multiplier lambda
Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
decimal rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse;
float rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse;
Vector3 lambdaTempRolling = contactPoint.rollingResistanceImpulse;
contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse +
deltaLambdaRolling, rollingLimit);
@ -676,11 +657,11 @@ void ContactSolver::solve() {
// Compute J*v
Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
- v1 - w1.cross(contactManifold.r1Friction);
decimal Jv = deltaV.dot(contactManifold.frictionVector1);
float Jv = deltaV.dot(contactManifold.frictionVector1);
// Compute the Lagrange multiplier lambda
decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
float deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
float frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.friction1Impulse;
contactManifold.friction1Impulse = std::max(-frictionLimit,
std::min(contactManifold.friction1Impulse +
@ -759,7 +740,7 @@ void ContactSolver::solve() {
// Compute the Lagrange multiplier lambda
Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
decimal rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse;
float rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse;
Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse;
contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse +
deltaLambdaRolling, rollingLimit);
@ -783,11 +764,11 @@ void ContactSolver::solve() {
void ContactSolver::storeImpulses() {
// For each contact manifold
for (uint c=0; c<mNbContactManifolds; c++) {
for (uint32_t c=0; c<mNbContactManifolds; 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];
@ -855,7 +836,7 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
Vector3 tangentVelocity = deltaVelocity - normalVelocity;
// If the velocty difference in the tangential plane is not zero
decimal lengthTangenVelocity = tangentVelocity.length();
float lengthTangenVelocity = tangentVelocity.length();
if (lengthTangenVelocity > MACHINE_EPSILON) {
// 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;
// If the velocty difference in the tangential plane is not zero
decimal lengthTangenVelocity = tangentVelocity.length();
float lengthTangenVelocity = tangentVelocity.length();
if (lengthTangenVelocity > MACHINE_EPSILON) {
// Compute the first friction vector in the direction of the tangent

135
ephysics/engine/ContactSolver.h
View File

@ -1,30 +1,9 @@
/********************************************************************************
* 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_CONTACT_SOLVER_H
#define REACTPHYSICS3D_CONTACT_SOLVER_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/constraint/ContactPoint.h>
@ -62,7 +41,7 @@ namespace reactphysics3d {
*
* --- 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.
*
* v2' = v1 + dt * M^-1 * F_a
@ -83,7 +62,7 @@ namespace reactphysics3d {
*
* --- 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.
*
* 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
/**
* 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
*/
struct ContactPointSolver {
/// Accumulated normal impulse
decimal penetrationImpulse;
float penetrationImpulse;
/// Accumulated impulse in the 1st friction direction
decimal friction1Impulse;
float friction1Impulse;
/// Accumulated impulse in the 2nd friction direction
decimal friction2Impulse;
float friction2Impulse;
/// Accumulated split impulse for penetration correction
decimal penetrationSplitImpulse;
float penetrationSplitImpulse;
/// Accumulated rolling resistance impulse
Vector3 rollingResistanceImpulse;
@ -175,19 +154,19 @@ class ContactSolver {
Vector3 r2CrossN;
/// Penetration depth
decimal penetrationDepth;
float penetrationDepth;
/// Velocity restitution bias
decimal restitutionBias;
float restitutionBias;
/// Inverse of the matrix K for the penenetration
decimal inversePenetrationMass;
float inversePenetrationMass;
/// Inverse of the matrix K for the 1st friction
decimal inverseFriction1Mass;
float inverseFriction1Mass;
/// Inverse of the matrix K for the 2nd friction
decimal inverseFriction2Mass;
float inverseFriction2Mass;
/// True if the contact was existing last time step
bool isRestingContact;
@ -198,22 +177,22 @@ class ContactSolver {
// 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.
*/
struct ContactManifoldSolver {
/// Index of body 1 in the constraint solver
uint indexBody1;
uint32_t indexBody1;
/// Index of body 2 in the constraint solver
uint indexBody2;
uint32_t indexBody2;
/// Inverse of the mass of body 1
decimal massInverseBody1;
float massInverseBody1;
// Inverse of the mass of body 2
decimal massInverseBody2;
float massInverseBody2;
/// Inverse inertia tensor of body 1
Matrix3x3 inverseInertiaTensorBody1;
@ -225,7 +204,7 @@ class ContactSolver {
ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];
/// Number of contact points
uint nbContacts;
uint32_t nbContacts;
/// True if the body 1 is of type dynamic
bool isBody1DynamicType;
@ -234,13 +213,13 @@ class ContactSolver {
bool isBody2DynamicType;
/// Mix of the restitution factor for two bodies
decimal restitutionFactor;
float restitutionFactor;
/// Mix friction coefficient for the two bodies
decimal frictionCoefficient;
float frictionCoefficient;
/// Rolling resistance factor between the two bodies
decimal rollingResistanceFactor;
float rollingResistanceFactor;
/// Pointer to the external contact manifold
ContactManifold* externalContactManifold;
@ -275,13 +254,13 @@ class ContactSolver {
Vector3 r2CrossT2;
/// Matrix K for the first friction constraint
decimal inverseFriction1Mass;
float inverseFriction1Mass;
/// Matrix K for the second friction constraint
decimal inverseFriction2Mass;
float inverseFriction2Mass;
/// Matrix K for the twist friction constraint
decimal inverseTwistFrictionMass;
float inverseTwistFrictionMass;
/// Matrix K for the rolling resistance constraint
Matrix3x3 inverseRollingResistance;
@ -299,13 +278,13 @@ class ContactSolver {
Vector3 oldFrictionVector2;
/// First friction direction impulse at manifold center
decimal friction1Impulse;
float friction1Impulse;
/// Second friction direction impulse at manifold center
decimal friction2Impulse;
float friction2Impulse;
/// Twist friction impulse at contact manifold center
decimal frictionTwistImpulse;
float frictionTwistImpulse;
/// Rolling resistance impulse
Vector3 rollingResistanceImpulse;
@ -314,13 +293,13 @@ class ContactSolver {
// -------------------- Constants --------------------- //
/// 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
static const decimal BETA_SPLIT_IMPULSE;
static const float BETA_SPLIT_IMPULSE;
/// Slop distance (allowed penetration distance between bodies)
static const decimal SLOP;
static const float SLOP;
// -------------------- Attributes -------------------- //
@ -331,13 +310,13 @@ class ContactSolver {
Vector3* mSplitAngularVelocities;
/// Current time step
decimal mTimeStep;
float mTimeStep;
/// Contact constraints
ContactManifoldSolver* mContactConstraints;
/// Number of contact constraints
uint mNbContactManifolds;
uint32_t mNbContactManifolds;
/// Array of linear velocities
Vector3* mLinearVelocities;
@ -346,7 +325,7 @@ class ContactSolver {
Vector3* mAngularVelocities;
/// 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
bool mIsWarmStartingActive;
@ -371,15 +350,15 @@ class ContactSolver {
const ContactManifoldSolver& manifold);
/// Compute the collision restitution factor from the restitution factor of each body
decimal computeMixedRestitutionFactor(RigidBody *body1,
float computeMixedRestitutionFactor(RigidBody *body1,
RigidBody *body2) const;
/// Compute the mixed friction coefficient from the friction coefficient of each body
decimal computeMixedFrictionCoefficient(RigidBody* body1,
float computeMixedFrictionCoefficient(RigidBody* body1,
RigidBody* body2) const;
/// 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
/// plane for a contact point. The two vectors have to be
@ -394,15 +373,15 @@ class ContactSolver {
ContactManifoldSolver& contactPoint) const;
/// Compute a penetration constraint impulse
const Impulse computePenetrationImpulse(decimal deltaLambda,
const Impulse computePenetrationImpulse(float deltaLambda,
const ContactPointSolver& contactPoint) const;
/// Compute the first friction constraint impulse
const Impulse computeFriction1Impulse(decimal deltaLambda,
const Impulse computeFriction1Impulse(float deltaLambda,
const ContactPointSolver& contactPoint) const;
/// Compute the second friction constraint impulse
const Impulse computeFriction2Impulse(decimal deltaLambda,
const Impulse computeFriction2Impulse(float deltaLambda,
const ContactPointSolver& contactPoint) const;
public:
@ -410,13 +389,13 @@ class ContactSolver {
// -------------------- Methods -------------------- //
/// Constructor
ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
ContactSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex);
/// Destructor
virtual ~ContactSolver();
/// 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
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
inline decimal ContactSolver::computeMixedRestitutionFactor(RigidBody* body1,
inline float ContactSolver::computeMixedRestitutionFactor(RigidBody* body1,
RigidBody* body2) const {
decimal restitution1 = body1->getMaterial().getBounciness();
decimal restitution2 = body2->getMaterial().getBounciness();
float restitution1 = body1->getMaterial().getBounciness();
float restitution2 = body2->getMaterial().getBounciness();
// Return the largest restitution factor
return (restitution1 > restitution2) ? restitution1 : restitution2;
}
// 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 {
// Use the geometric mean to compute the mixed friction coefficient
return sqrt(body1->getMaterial().getFrictionCoefficient() *
@ -503,13 +482,13 @@ inline decimal ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1,
}
// Compute th mixed rolling resistance factor between two bodies
inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1,
inline float ContactSolver::computeMixedRollingResistance(RigidBody* body1,
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
inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda,
inline const Impulse ContactSolver::computePenetrationImpulse(float deltaLambda,
const ContactPointSolver& contactPoint)
const {
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
inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda,
inline const Impulse ContactSolver::computeFriction1Impulse(float deltaLambda,
const ContactPointSolver& contactPoint)
const {
return Impulse(-contactPoint.frictionVector1 * deltaLambda,
@ -527,7 +506,7 @@ inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda,
}
// Compute the second friction constraint impulse
inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda,
inline const Impulse ContactSolver::computeFriction2Impulse(float deltaLambda,
const ContactPointSolver& contactPoint)
const {
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 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/engine/DynamicsWorld.h>
@ -76,7 +57,7 @@ DynamicsWorld::~DynamicsWorld() {
}
// 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
mIslands[i]->~Island();
@ -103,8 +84,8 @@ DynamicsWorld::~DynamicsWorld() {
#ifdef IS_PROFILING_ACTIVE
// Print the profiling report
Profiler::printReport(std::cout);
// Print32_t the profiling report
Profiler::print32_tReport(std::cout);
// Destroy the profiler (release the allocated memory)
Profiler::destroy();
@ -116,7 +97,7 @@ DynamicsWorld::~DynamicsWorld() {
/**
* @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
// Increment the frame counter of the profiler
@ -127,7 +108,7 @@ void DynamicsWorld::update(decimal 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) {
mEventListener->beginInternalTick();
}
@ -147,13 +128,13 @@ void DynamicsWorld::update(decimal timeStep) {
computeIslands();
// Integrate the velocities
integrateRigidBodiesVelocities();
int32_tegrateRigidBodiesVelocities();
// Solve the contacts and constraints
solveContactsAndConstraints();
// Integrate the position and orientation of each body
integrateRigidBodiesPositions();
int32_tegrateRigidBodiesPositions();
// Solve the position correction for constraints
solvePositionCorrection();
@ -163,7 +144,7 @@ void DynamicsWorld::update(decimal timeStep) {
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();
// 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.
/// 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.
void DynamicsWorld::integrateRigidBodiesPositions() {
void DynamicsWorld::int32_tegrateRigidBodiesPositions() {
PROFILE("DynamicsWorld::integrateRigidBodiesPositions()");
PROFILE("DynamicsWorld::int32_tegrateRigidBodiesPositions()");
// 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();
// 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
uint indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
uint32_t indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
@ -206,7 +187,7 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * mTimeStep;
mConstrainedOrientations[indexArray] = currentOrientation +
Quaternion(0, newAngVelocity) *
currentOrientation * decimal(0.5) * mTimeStep;
currentOrientation * float(0.5) * mTimeStep;
}
}
}
@ -217,14 +198,14 @@ void DynamicsWorld::updateBodiesState() {
PROFILE("DynamicsWorld::updateBodiesState()");
// 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
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
bodies[b]->mLinearVelocity = mConstrainedLinearVelocities[index];
@ -249,7 +230,7 @@ void DynamicsWorld::updateBodiesState() {
void DynamicsWorld::initVelocityArrays() {
// Allocate memory for the bodies velocity arrays
uint nbBodies = mRigidBodies.size();
uint32_t nbBodies = mRigidBodies.size();
if (mNbBodiesCapacity != nbBodies && nbBodies > 0) {
if (mNbBodiesCapacity > 0) {
delete[] mSplitLinearVelocities;
@ -272,7 +253,7 @@ void DynamicsWorld::initVelocityArrays() {
}
// Reset the velocities arrays
for (uint i=0; i<mNbBodiesCapacity; i++) {
for (uint32_t i=0; i<mNbBodiesCapacity; i++) {
mSplitLinearVelocities[i].setToZero();
mSplitAngularVelocities[i].setToZero();
}
@ -280,10 +261,10 @@ void DynamicsWorld::initVelocityArrays() {
// Initialize the map of body indexes in the velocity arrays
mMapBodyToConstrainedVelocityIndex.clear();
std::set<RigidBody*>::const_iterator it;
uint indexBody = 0;
uint32_t indexBody = 0;
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));
indexBody++;
}
@ -294,23 +275,23 @@ void DynamicsWorld::initVelocityArrays() {
/// the actual velocitiy of the bodies. The velocities updated in this method
/// might violate the constraints and will be corrected in the constraint and
/// contact solver.
void DynamicsWorld::integrateRigidBodiesVelocities() {
void DynamicsWorld::int32_tegrateRigidBodiesVelocities() {
PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()");
PROFILE("DynamicsWorld::int32_tegrateRigidBodiesVelocities()");
// Initialize the bodies velocity arrays
initVelocityArrays();
// 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();
// 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
uint indexBody = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
// Insert the body int32_to the map of constrained velocities
uint32_t indexBody = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
assert(mSplitLinearVelocities[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! + ...
// => e^(-x) ~ 1 - x
// => v2 = v1 * (1 - c * dt)
decimal linDampingFactor = bodies[b]->getLinearDamping();
decimal angDampingFactor = bodies[b]->getAngularDamping();
decimal linearDamping = pow(decimal(1.0) - linDampingFactor, mTimeStep);
decimal angularDamping = pow(decimal(1.0) - angDampingFactor, mTimeStep);
float linDampingFactor = bodies[b]->getLinearDamping();
float angDampingFactor = bodies[b]->getAngularDamping();
float linearDamping = pow(float(1.0) - linDampingFactor, mTimeStep);
float angularDamping = pow(float(1.0) - angDampingFactor, mTimeStep);
mConstrainedLinearVelocities[indexBody] *= linearDamping;
mConstrainedAngularVelocities[indexBody] *= angularDamping;
@ -372,7 +353,7 @@ void DynamicsWorld::solveContactsAndConstraints() {
// ---------- Solve velocity constraints for joints and contacts ---------- //
// 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
bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
@ -397,7 +378,7 @@ void DynamicsWorld::solveContactsAndConstraints() {
}
// 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
if (isConstraintsToSolve) {
@ -426,12 +407,12 @@ void DynamicsWorld::solvePositionCorrection() {
if (mJoints.empty()) return;
// 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 ---------- //
// 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
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
* @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);
}
// Add the joint into the world
// Add the joint int32_to the world
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);
// Return the pointer to the created joint
@ -634,10 +615,10 @@ void DynamicsWorld::computeIslands() {
PROFILE("DynamicsWorld::computeIslands()");
uint nbBodies = mRigidBodies.size();
uint32_t nbBodies = mRigidBodies.size();
// Clear all the islands
for (uint i=0; i<mNbIslands; i++) {
for (uint32_t i=0; i<mNbIslands; i++) {
// Call the island destructor
mIslands[i]->~Island();
@ -656,11 +637,11 @@ void DynamicsWorld::computeIslands() {
}
mNbIslands = 0;
int nbContactManifolds = 0;
int32_t nbContactManifolds = 0;
// Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
int nbBodyManifolds = (*it)->resetIsAlreadyInIslandAndCountManifolds();
int32_t nbBodyManifolds = (*it)->resetIsAlreadyInIslandAndCountManifolds();
nbContactManifolds += nbBodyManifolds;
}
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;
// Reset the stack of bodies to visit
uint stackIndex = 0;
uint32_t stackIndex = 0;
stackBodiesToVisit[stackIndex] = body;
stackIndex++;
body->mIsAlreadyInIsland = true;
@ -708,7 +689,7 @@ void DynamicsWorld::computeIslands() {
// Awake the body if it is slepping
bodyToVisit->setIsSleeping(false);
// Add the body into the island
// Add the body int32_to the island
mIslands[mNbIslands]->addBody(bodyToVisit);
// 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);
// 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;
// Add the contact manifold into the island
// Add the contact manifold int32_to the island
mIslands[mNbIslands]->addContactManifold(contactManifold);
contactManifold->mIsAlreadyInIsland = true;
@ -739,7 +720,7 @@ void DynamicsWorld::computeIslands() {
// Check if the other body has already been added to the island
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;
stackIndex++;
otherBody->mIsAlreadyInIsland = true;
@ -752,10 +733,10 @@ void DynamicsWorld::computeIslands() {
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;
// Add the joint into the island
// Add the joint int32_to the island
mIslands[mNbIslands]->addJoint(joint);
joint->mIsAlreadyInIsland = true;
@ -767,7 +748,7 @@ void DynamicsWorld::computeIslands() {
// Check if the other body has already been added to the island
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;
stackIndex++;
otherBody->mIsAlreadyInIsland = true;
@ -776,7 +757,7 @@ void DynamicsWorld::computeIslands() {
// Reset the isAlreadyIsland variable of the static bodies so that they
// 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) {
mIslands[mNbIslands]->mBodies[i]->mIsAlreadyInIsland = false;
@ -797,17 +778,17 @@ void DynamicsWorld::updateSleepingBodies() {
PROFILE("DynamicsWorld::updateSleepingBodies()");
const decimal sleepLinearVelocitySquare = mSleepLinearVelocity * mSleepLinearVelocity;
const decimal sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity;
const float sleepLinearVelocitySquare = mSleepLinearVelocity * mSleepLinearVelocity;
const float sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity;
// 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
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
if (bodies[b]->getType() == STATIC) continue;
@ -818,8 +799,8 @@ void DynamicsWorld::updateSleepingBodies() {
!bodies[b]->isAllowedToSleep()) {
// Reset the sleep time of the body
bodies[b]->mSleepTime = decimal(0.0);
minSleepTime = decimal(0.0);
bodies[b]->mSleepTime = float(0.0);
minSleepTime = float(0.0);
}
else { // If the body velocity is bellow the sleeping velocity threshold
@ -837,7 +818,7 @@ void DynamicsWorld::updateSleepingBodies() {
if (minSleepTime >= mTimeBeforeSleep) {
// 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);
}
}
@ -845,7 +826,7 @@ void DynamicsWorld::updateSleepingBodies() {
}
// 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.
/**
* @param isSleepingEnabled True if you want to enable the sleeping technique
@ -878,9 +859,9 @@ void DynamicsWorld::testCollision(const ProxyShape* shape,
CollisionCallback* callback) {
// Create the sets of shapes
std::set<uint> shapes;
std::set<uint32_t> shapes;
shapes.insert(shape->mBroadPhaseID);
std::set<uint> emptySet;
std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts
mCollisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet);
@ -899,9 +880,9 @@ void DynamicsWorld::testCollision(const ProxyShape* shape1,
CollisionCallback* callback) {
// Create the sets of shapes
std::set<uint> shapes1;
std::set<uint32_t> shapes1;
shapes1.insert(shape1->mBroadPhaseID);
std::set<uint> shapes2;
std::set<uint32_t> shapes2;
shapes2.insert(shape2->mBroadPhaseID);
// Perform the collision detection and report contacts
@ -920,7 +901,7 @@ void DynamicsWorld::testCollision(const CollisionBody* body,
CollisionCallback* callback) {
// Create the sets of shapes
std::set<uint> shapes1;
std::set<uint32_t> shapes1;
// For each shape of the body
for (const ProxyShape* shape=body->getProxyShapesList(); shape != NULL;
@ -928,7 +909,7 @@ void DynamicsWorld::testCollision(const CollisionBody* body,
shapes1.insert(shape->mBroadPhaseID);
}
std::set<uint> emptySet;
std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts
mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet);
@ -947,13 +928,13 @@ void DynamicsWorld::testCollision(const CollisionBody* body1,
CollisionCallback* callback) {
// Create the sets of shapes
std::set<uint> shapes1;
std::set<uint32_t> shapes1;
for (const ProxyShape* shape=body1->getProxyShapesList(); shape != NULL;
shape = shape->getNext()) {
shapes1.insert(shape->mBroadPhaseID);
}
std::set<uint> shapes2;
std::set<uint32_t> shapes2;
for (const ProxyShape* shape=body2->getProxyShapesList(); shape != NULL;
shape = shape->getNext()) {
shapes2.insert(shape->mBroadPhaseID);
@ -971,7 +952,7 @@ void DynamicsWorld::testCollision(const CollisionBody* body1,
*/
void DynamicsWorld::testCollision(CollisionCallback* callback) {
std::set<uint> emptySet;
std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts
mCollisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet);
@ -991,7 +972,7 @@ std::vector<const ContactManifold*> DynamicsWorld::getContactsList() const {
// For each contact manifold of the pair
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);

118
ephysics/engine/DynamicsWorld.h
View File

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

47
ephysics/engine/EventListener.h
View File

@ -1,30 +1,9 @@
/********************************************************************************
* 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_EVENT_LISTENER_H
#define REACTPHYSICS3D_EVENT_LISTENER_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/constraint/ContactPoint.h>
@ -61,19 +40,17 @@ class EventListener {
*/
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
/// engine will do several internal simulation steps. This method is
/// called at the beginning of each internal simulation step.
/// engine will do several int32_ternal simulation steps. This method is
/// called at the beginning of each int32_ternal simulation step.
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
/// engine will do several internal simulation steps. This method is
/// called at the end of each internal simulation step.
/// engine will do several int32_ternal simulation steps. This method is
/// called at the end of each int32_ternal simulation step.
virtual void endInternalTick() {}
};
}
#endif

35
ephysics/engine/Impulse.h
View File

@ -1,30 +1,9 @@
/********************************************************************************
* 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_IMPULSE_H
#define REACTPHYSICS3D_IMPULSE_H
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/mathematics/mathematics.h>
@ -83,5 +62,3 @@ struct Impulse {
};
}
#endif

31
ephysics/engine/Island.cpp
View File

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/engine/Island.h>
@ -29,7 +10,7 @@
using namespace reactphysics3d;
// Constructor
Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints,
Island::Island(uint32_t nbMaxBodies, uint32_t nbMaxContactManifolds, uint32_t nbMaxJoints,
MemoryAllocator& memoryAllocator)
: mBodies(NULL), mContactManifolds(NULL), mJoints(NULL), mNbBodies(0),
mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) {

67
ephysics/engine/Island.h
View File

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

29
ephysics/engine/Material.cpp
View File

@ -1,27 +1,8 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
/** @file
* @author Daniel Chappuis
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/engine/Material.h>

69
ephysics/engine/Material.h
View File

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