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

Merge branch 'joints' into develop

Browse Source
This commit is contained in:
Daniel Chappuis 2013-06-26 22:40:45 +02:00
commit 3f90564149
65 changed files with 6867 additions and 916 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
CMakeLists.txt
View File

@ -7,11 +7,15 @@ PROJECT(REACTPHYSICS3D)
# Where to build the library
SET(LIBRARY_OUTPUT_PATH lib/)
# Where to find the module to find special packages/libraries
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/")
# Options
OPTION(COMPILE_EXAMPLES "Select this if you want to build the examples" OFF)
OPTION(COMPILE_TESTS "Select this if you want to build the tests" OFF)
OPTION(PROFILING_ENABLED "Select this if you want to compile with enabled profiling" OFF)
OPTION(DOUBLE_PRECISION_ENABLED "Select this if you want to compile using double precision floating
values" OFF)
# Headers
INCLUDE_DIRECTORIES(src)
@ -19,6 +23,10 @@ IF (PROFILING_ENABLED)
ADD_DEFINITIONS(-DIS_PROFILING_ACTIVE)
ENDIF (PROFILING_ENABLED)
IF (DOUBLE_PRECISION_ENABLED)
ADD_DEFINITIONS(-DIS_DOUBLE_PRECISION_ENABLED)
ENDIF (DOUBLE_PRECISION_ENABLED)
# Library configuration
file (
GLOB_RECURSE

65
cmake/FindGLEW.cmake Normal file
View File

@ -0,0 +1,65 @@
#
# Try to find GLEW library and include path.
# Once done this will define
#
# GLEW_FOUND
# GLEW_INCLUDE_PATH
# GLEW_LIBRARY
#
IF (WIN32)
FIND_PATH( GLEW_INCLUDE_PATH GL/glew.h
$ENV{PROGRAMFILES}/GLEW/include
${GLEW_ROOT_DIR}/include
DOC "The directory where GL/glew.h resides")
IF (NV_SYSTEM_PROCESSOR STREQUAL "AMD64")
FIND_LIBRARY( GLEW_LIBRARY
NAMES glew64 glew64s
PATHS
$ENV{PROGRAMFILES}/GLEW/lib
${PROJECT_SOURCE_DIR}/src/nvgl/glew/bin
${PROJECT_SOURCE_DIR}/src/nvgl/glew/lib
DOC "The GLEW library (64-bit)"
)
ELSE(NV_SYSTEM_PROCESSOR STREQUAL "AMD64")
FIND_LIBRARY( GLEW_LIBRARY
NAMES glew GLEW glew32 glew32s
PATHS
$ENV{PROGRAMFILES}/GLEW/lib
${PROJECT_SOURCE_DIR}/src/nvgl/glew/bin
${PROJECT_SOURCE_DIR}/src/nvgl/glew/lib
DOC "The GLEW library"
)
ENDIF(NV_SYSTEM_PROCESSOR STREQUAL "AMD64")
ELSE (WIN32)
FIND_PATH( GLEW_INCLUDE_PATH GL/glew.h
/usr/include
/usr/local/include
/sw/include
/opt/local/include
${GLEW_ROOT_DIR}/include
DOC "The directory where GL/glew.h resides")
FIND_LIBRARY( GLEW_LIBRARY
NAMES GLEW glew
PATHS
/usr/lib64
/usr/lib
/usr/local/lib64
/usr/local/lib
/sw/lib
/opt/local/lib
${GLEW_ROOT_DIR}/lib
DOC "The GLEW library")
ENDIF (WIN32)
SET(GLEW_FOUND "NO")
IF (GLEW_INCLUDE_PATH AND GLEW_LIBRARY)
SET(GLEW_LIBRARIES ${GLEW_LIBRARY})
SET(GLEW_FOUND "YES")
ENDIF (GLEW_INCLUDE_PATH AND GLEW_LIBRARY)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(GLEW DEFAULT_MSG GLEW_LIBRARY GLEW_INCLUDE_PATH)

1
examples/CMakeLists.txt
View File

@ -3,3 +3,4 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
add_subdirectory(common/)
add_subdirectory(fallingcubes/)
add_subdirectory(joints/)

180
examples/fallingcubes/Box.cpp
View File

@ -1,180 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "Box.h"
// Macros
#define MEMBER_OFFSET(s,m) ((char *)NULL + (offsetof(s,m)))
// Initialize static variables
openglframework::VertexBufferObject Box::mVBOVertices(GL_ARRAY_BUFFER);
openglframework::VertexBufferObject Box::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER);
bool Box::areVBOsCreated = false;
VertexData Box::mCubeVertices[8] = {
{openglframework::Vector3(1,1,1),openglframework::Vector3(1,1,1),openglframework::Color(0,0,1,1)},
{openglframework::Vector3(-1,1,1),openglframework::Vector3(-1,1,1),openglframework::Color(0,0,1,1)},
{openglframework::Vector3(-1,-1,1),openglframework::Vector3(-1,-1,1),openglframework::Color(0,0,1,1)},
{openglframework::Vector3(1,-1,1),openglframework::Vector3(1,-1,1),openglframework::Color(0,0,1,1)},
{openglframework::Vector3(1,-1,-1),openglframework::Vector3(1,-1,-1),openglframework::Color(0,0,1,1)},
{openglframework::Vector3(-1,-1,-1),openglframework::Vector3(-1,-1,-1),openglframework::Color(0,0,1,1)},
{openglframework::Vector3(-1,1,-1),openglframework::Vector3(-1,1,-1),openglframework::Color(0,0,1,1)},
{openglframework::Vector3(1,1,-1),openglframework::Vector3(1,1,-1),openglframework::Color(0,0,1,1)}
};
GLuint Box::mCubeIndices[36] = { 0, 1, 2,
2, 3, 0,
7, 4, 5,
5, 6, 7,
6, 5, 2,
2, 1, 6,
7, 0, 3,
3, 4, 7,
7, 6, 1,
1, 0, 7,
3, 2, 5,
5, 4, 3};
// Constructor
Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &position,
float mass, reactphysics3d::DynamicsWorld* dynamicsWorld)
: openglframework::Object3D() {
// Initialize the size of the box
mSize[0] = size.x * 0.5f;
mSize[1] = size.y * 0.5f;
mSize[2] = size.z * 0.5f;
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4(mSize[0], 0, 0, 0,
0, mSize[1], 0, 0,
0, 0, mSize[2], 0,
0, 0, 0, 1);
// Initialize the position where the cube will be rendered
translateWorld(position);
// Create the collision shape for the rigid body (box shape)
mCollisionShape = new rp3d::BoxShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2]));
// Compute the inertia tensor of the body using its collision shape
rp3d::Matrix3x3 inertiaTensor;
mCollisionShape->computeLocalInertiaTensor(inertiaTensor, mass);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the cube in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, inertiaTensor, mCollisionShape);
// If the Vertex Buffer object has not been created yet
if (!areVBOsCreated) {
// Create the Vertex Buffer
createVBO();
}
}
// Destructor
Box::~Box() {
// Destroy the collision shape
delete mCollisionShape;
}
// Render the cube at the correct position and with the correct orientation
void Box::render(openglframework::Shader& shader) {
// Bind the shader
shader.bind();
// Set the model to World matrix
shader.setMatrix4x4Uniform("modelToWorldMatrix", mTransformMatrix);
// Bind the vertices VBO
mVBOVertices.bind();
// Enable the vertex, normal and color arrays
glEnableClientState(GL_VERTEX_ARRAY);
glEnableClientState(GL_COLOR_ARRAY);
glEnableClientState(GL_NORMAL_ARRAY);
// Set the arrays pointers
glVertexPointer(3, GL_FLOAT, sizeof(VertexData), MEMBER_OFFSET(VertexData, position));
glNormalPointer(GL_FLOAT, sizeof(VertexData), MEMBER_OFFSET(VertexData, normal));
glColorPointer(3, GL_FLOAT, sizeof(VertexData), MEMBER_OFFSET(VertexData, color));
// Bind the indices VBO
mVBOIndices.bind();
// Draw the geometry of the box
glDrawElements(GL_TRIANGLES, 36, GL_UNSIGNED_INT, (char*)NULL);
// Unbind the VBOs
mVBOVertices.unbind();
mVBOIndices.unbind();
// Disable the arrays
glDisableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_COLOR_ARRAY);
glDisableClientState(GL_NORMAL_ARRAY);
// Unbind the shader
shader.unbind();
}
// Update the transform matrix of the box
void Box::updateTransform() {
// Get the interpolated transform of the rigid body
rp3d::Transform transform = mRigidBody->getInterpolatedTransform();
// Compute the transform used for rendering the box
float matrix[16];
transform.getOpenGLMatrix(matrix);
openglframework::Matrix4 newMatrix(matrix[0], matrix[4], matrix[8], matrix[12],
matrix[1], matrix[5], matrix[9], matrix[13],
matrix[2], matrix[6], matrix[10], matrix[14],
matrix[3], matrix[7], matrix[11], matrix[15]);
// Apply the scaling matrix to have the correct box dimensions
mTransformMatrix = newMatrix * mScalingMatrix;
}
// Create the Vertex Buffer Objects used to render to box with OpenGL.
/// We create two VBOs (one for vertices and one for indices) to render all the boxes
/// in the simulation.
void Box::createVBO() {
// Create the VBOs
mVBOVertices.create();
mVBOIndices.create();
// Copy the data into the VBOs
mVBOVertices.copyDataIntoVBO(sizeof(mCubeVertices), mCubeVertices, GL_STATIC_DRAW);
mVBOIndices.copyDataIntoVBO(sizeof(mCubeIndices), mCubeIndices, GL_STATIC_DRAW);
areVBOsCreated = true;
}

111
examples/fallingcubes/Box.h
View File

@ -1,111 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 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 BOX_H
#define BOX_H
// Libraries
#include "openglframework.h"
#include "reactphysics3d.h"
// Structure VertexData
struct VertexData {
/// Vertex position
openglframework::Vector3 position;
/// Vertex normal
openglframework::Vector3 normal;
// Vertex color
openglframework::Color color;
};
// Class Box
class Box : public openglframework::Object3D {
private :
// -------------------- Attributes -------------------- //
/// Size of each side of the box
float mSize[3];
/// Rigid body used to simulate the dynamics of the box
rp3d::RigidBody* mRigidBody;
/// Collision shape of the rigid body
rp3d::BoxShape* mCollisionShape;
/// Scaling matrix (applied to a cube to obtain the correct box dimensions)
openglframework::Matrix4 mScalingMatrix;
/// Vertex Buffer Object for the vertices data used to render the box with OpenGL
static openglframework::VertexBufferObject mVBOVertices;
/// Vertex Buffer Object for the indices used to render the box with OpenGL
static openglframework::VertexBufferObject mVBOIndices;
/// Vertex data for each vertex of the cube (used to render the box)
static VertexData mCubeVertices[8];
/// Indices of the cube (used to render the box)
static GLuint mCubeIndices[36];
/// True if the VBOs have already been created
static bool areVBOsCreated;
// -------------------- Methods -------------------- //
/// Create a Vertex Buffer Object to render to box with OpenGL
static void createVBO();
public :
// -------------------- Methods -------------------- //
/// Constructor
Box(const openglframework::Vector3& size, const openglframework::Vector3& position,
float mass, rp3d::DynamicsWorld* dynamicsWorld);
/// Destructor
~Box();
/// Return a pointer to the rigid body of the box
rp3d::RigidBody* getRigidBody();
/// Update the transform matrix of the box
void updateTransform();
/// Render the cube at the correct position and with the correct orientation
void render(openglframework::Shader& shader);
};
// Return a pointer to the rigid body of the box
inline rp3d::RigidBody* Box::getRigidBody() {
return mRigidBody;
}
#endif

6
examples/fallingcubes/FallingCubes.cpp
View File

@ -66,12 +66,11 @@ int main(int argc, char** argv) {
glutMouseFunc(mouseButton);
glutMotionFunc(mouseMotion);
glutKeyboardFunc(keyboard);
glutCloseFunc(finish);
// Glut main looop
glutMainLoop();
finish();
return 0;
}
@ -115,8 +114,7 @@ void keyboard(unsigned char key, int x, int y) {
// Escape key
case 27:
finish();
exit(0);
glutLeaveMainLoop();
break;
// Space bar

2
examples/fallingcubes/Scene.cpp
View File

@ -54,7 +54,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
mDynamicsWorld = new rp3d::DynamicsWorld(gravity, timeStep);
// Set the number of iterations of the constraint solver
mDynamicsWorld->setNbIterationsSolver(15);
mDynamicsWorld->setNbIterationsVelocitySolver(15);
float radius = 2.0f;

6
examples/fallingcubes/Scene.h
View File

@ -45,13 +45,13 @@ class Scene {
// -------------------- Attributes -------------------- //
// Pointer to the viewer
/// Pointer to the viewer
openglframework::GlutViewer* mViewer;
// Light 0
/// Light 0
openglframework::Light mLight0;
// Phong shader
/// Phong shader
openglframework::Shader mPhongShader;
/// All the boxes of the scene

17
examples/joints/CMakeLists.txt Normal file
View File

@ -0,0 +1,17 @@
# Minimum cmake version required
cmake_minimum_required(VERSION 2.6)
# Project configuration
PROJECT(Joints)
# Copy the shaders used for the demo into the build directory
FILE(COPY "../common/opengl-framework/src/shaders/" DESTINATION "${CMAKE_CURRENT_BINARY_DIR}/shaders/")
# Headers
INCLUDE_DIRECTORIES("../common/opengl-framework/src/" "../common/")
# Create the example executable using the
# compiled reactphysics3d static library
ADD_EXECUTABLE(joints Joints.cpp Scene.cpp Scene.h "../common/Box.cpp" "../common/Box.h" "../common/Viewer.cpp" "../common/Viewer.h")
TARGET_LINK_LIBRARIES(joints reactphysics3d openglframework)

151
examples/joints/Joints.cpp Normal file
View File

@ -0,0 +1,151 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "Scene.h"
#include "Viewer.h"
// Declarations
void simulate();
void display();
void finish();
void reshape(int width, int height);
void mouseButton(int button, int state, int x, int y);
void mouseMotion(int x, int y);
void keyboard(unsigned char key, int x, int y);
void init();
// Namespaces
using namespace openglframework;
// Global variables
Viewer* viewer;
Scene* scene;
// Main function
int main(int argc, char** argv) {
// Create and initialize the Viewer
viewer = new Viewer();
Vector2 windowsSize = Vector2(800, 600);
Vector2 windowsPosition = Vector2(100, 100);
bool initOK = viewer->init(argc, argv, "ReactPhysics3D Examples - Joints", windowsSize, windowsPosition);
if (!initOK) return 1;
// Create the scene
scene = new Scene(viewer);
init();
// Glut Idle function that is continuously called
glutIdleFunc(simulate);
glutDisplayFunc(display);
glutReshapeFunc(reshape);
glutMouseFunc(mouseButton);
glutMotionFunc(mouseMotion);
glutKeyboardFunc(keyboard);
glutCloseFunc(finish);
// Glut main looop
glutMainLoop();
return 0;
}
// Simulate function
void simulate() {
// Physics simulation
scene->simulate();
viewer->computeFPS();
// Ask GLUT to render the scene
glutPostRedisplay ();
}
// Initialization
void init() {
// Define the background color (black)
glClearColor(0.0, 0.0, 0.0, 1.0);
}
// Reshape function
void reshape(int newWidth, int newHeight) {
viewer->reshape(newWidth, newHeight);
}
// Called when a mouse button event occurs
void mouseButton(int button, int state, int x, int y) {
viewer->mouseButtonEvent(button, state, x, y);
}
// Called when a mouse motion event occurs
void mouseMotion(int x, int y) {
viewer->mouseMotionEvent(x, y);
}
// Called when the user hits a special key on the keyboard
void keyboard(unsigned char key, int x, int y) {
switch(key) {
// Escape key
case 27:
glutLeaveMainLoop();
break;
// Space bar
case 32:
scene->pauseContinueSimulation();
break;
}
}
// End of the application
void finish() {
// Destroy the viewer and the scene
delete viewer;
delete scene;
}
// Display the scene
void display() {
// Render the scene
scene->render();
// Display the FPS
viewer->displayGUI();
// Swap the buffers
glutSwapBuffers();
// Check the OpenGL errors
GlutViewer::checkOpenGLErrors();
}

394
examples/joints/Scene.cpp Normal file
View File

@ -0,0 +1,394 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "Scene.h"
#include <cmath>
// Namespaces
using namespace openglframework;
// Constructor
Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
mPhongShader("shaders/phong.vert",
"shaders/phong.frag"), mIsRunning(false) {
// Move the light 0
mLight0.translateWorld(Vector3(7, 15, 15));
// Compute the radius and the center of the scene
float radiusScene = 10.0f;
openglframework::Vector3 center(0, 5, 0);
// Set the center of the scene
mViewer->setScenePosition(center, radiusScene);
// Gravity vector in the dynamics world
rp3d::Vector3 gravity(0, -9.81, 0);
// Time step for the physics simulation
rp3d::decimal timeStep = 1.0f / 60.0f;
// Create the dynamics world for the physics simulation
mDynamicsWorld = new rp3d::DynamicsWorld(gravity, timeStep);
// Set the number of iterations of the constraint solver
mDynamicsWorld->setNbIterationsVelocitySolver(15);
// Create the Ball-and-Socket joint
createBallAndSocketJoints();
// Create the Slider joint
createSliderJoint();
// Create the Hinge joint
createPropellerHingeJoint();
// Create the Fixed joint
createFixedJoints();
// Create the floor
createFloor();
// Start the simulation
startSimulation();
}
// Destructor
Scene::~Scene() {
// Stop the physics simulation
stopSimulation();
// Destroy the shader
mPhongShader.destroy();
// Destroy the joints
mDynamicsWorld->destroyJoint(mSliderJoint);
mDynamicsWorld->destroyJoint(mPropellerHingeJoint);
mDynamicsWorld->destroyJoint(mFixedJoint1);
mDynamicsWorld->destroyJoint(mFixedJoint2);
for (int i=0; i<NB_BALLSOCKETJOINT_BOXES-1; i++) {
mDynamicsWorld->destroyJoint(mBallAndSocketJoints[i]);
}
// Destroy all the rigid bodies of the scene
mDynamicsWorld->destroyRigidBody(mSliderJointBottomBox->getRigidBody());
mDynamicsWorld->destroyRigidBody(mSliderJointTopBox->getRigidBody());
mDynamicsWorld->destroyRigidBody(mPropellerBox->getRigidBody());
mDynamicsWorld->destroyRigidBody(mFixedJointBox1->getRigidBody());
mDynamicsWorld->destroyRigidBody(mFixedJointBox2->getRigidBody());
for (int i=0; i<NB_BALLSOCKETJOINT_BOXES; i++) {
mDynamicsWorld->destroyRigidBody(mBallAndSocketJointChainBoxes[i]->getRigidBody());
}
delete mSliderJointBottomBox;
delete mSliderJointTopBox;
delete mPropellerBox;
delete mFixedJointBox1;
delete mFixedJointBox2;
for (int i=0; i<NB_BALLSOCKETJOINT_BOXES; i++) {
delete mBallAndSocketJointChainBoxes[i];
}
// Destroy the floor
mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody());
delete mFloor;
// Destroy the dynamics world
delete mDynamicsWorld;
}
// Take a step for the simulation
void Scene::simulate() {
// If the physics simulation is running
if (mIsRunning) {
// Update the motor speed of the Slider Joint (to move up and down)
long double motorSpeed = 3 * cos(mDynamicsWorld->getPhysicsTime() * 1.5);
mSliderJoint->setMotorSpeed(motorSpeed);
// Take a simulation step
mDynamicsWorld->update();
// Update the position and orientation of the boxes
mSliderJointBottomBox->updateTransform();
mSliderJointTopBox->updateTransform();
mPropellerBox->updateTransform();
mFixedJointBox1->updateTransform();
mFixedJointBox2->updateTransform();
for (int i=0; i<NB_BALLSOCKETJOINT_BOXES; i++) {
mBallAndSocketJointChainBoxes[i]->updateTransform();
}
// Update the position and orientation of the floor
mFloor->updateTransform();
}
}
// Render the scene
void Scene::render() {
glEnable(GL_DEPTH_TEST);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glEnable(GL_CULL_FACE);
// Bind the shader
mPhongShader.bind();
// Set the variables of the shader
const Camera& camera = mViewer->getCamera();
Matrix4 matrixIdentity;
matrixIdentity.setToIdentity();
mPhongShader.setVector3Uniform("cameraWorldPosition", mViewer->getCamera().getOrigin());
mPhongShader.setMatrix4x4Uniform("worldToCameraMatrix", camera.getTransformMatrix().getInverse());
mPhongShader.setMatrix4x4Uniform("projectionMatrix", camera.getProjectionMatrix());
mPhongShader.setVector3Uniform("lightWorldPosition", mLight0.getOrigin());
mPhongShader.setVector3Uniform("lightAmbientColor", Vector3(0.3f, 0.3f, 0.3f));
Color& diffCol = mLight0.getDiffuseColor();
Color& specCol = mLight0.getSpecularColor();
mPhongShader.setVector3Uniform("lightDiffuseColor", Vector3(diffCol.r, diffCol.g, diffCol.b));
mPhongShader.setVector3Uniform("lightSpecularColor", Vector3(specCol.r, specCol.g, specCol.b));
mPhongShader.setFloatUniform("shininess", 60.0f);
// Render all the boxes
mSliderJointBottomBox->render(mPhongShader);
mSliderJointTopBox->render(mPhongShader);
mPropellerBox->render(mPhongShader);
mFixedJointBox1->render(mPhongShader);
mFixedJointBox2->render(mPhongShader);
for (int i=0; i<NB_BALLSOCKETJOINT_BOXES; i++) {
mBallAndSocketJointChainBoxes[i]->render(mPhongShader);
}
// Render the floor
mFloor->render(mPhongShader);
// Unbind the shader
mPhongShader.unbind();
}
// Create the boxes and joints for the Ball-and-Socket joint example
void Scene::createBallAndSocketJoints() {
// --------------- Create the boxes --------------- //
openglframework::Vector3 positionBox(0, 15, 5);
openglframework::Vector3 boxDimension(1, 1, 1);
const float boxMass = 0.5f;
for (int i=0; i<NB_BALLSOCKETJOINT_BOXES; i++) {
// Create a box and a corresponding rigid in the dynamics world
mBallAndSocketJointChainBoxes[i] = new Box(boxDimension, positionBox , boxMass,
mDynamicsWorld);
// The fist box cannot move
if (i == 0) mBallAndSocketJointChainBoxes[i]->getRigidBody()->setIsMotionEnabled(false);
else mBallAndSocketJointChainBoxes[i]->getRigidBody()->setIsMotionEnabled(true);
// Set the bouncing factor of the box
mBallAndSocketJointChainBoxes[i]->getRigidBody()->setRestitution(0.4);
positionBox.y -= boxDimension.y + 0.5;
}
// --------------- Create the joints --------------- //
for (int i=0; i<NB_BALLSOCKETJOINT_BOXES-1; i++) {
// Create the joint info object
rp3d::RigidBody* body1 = mBallAndSocketJointChainBoxes[i]->getRigidBody();
rp3d::RigidBody* body2 = mBallAndSocketJointChainBoxes[i+1]->getRigidBody();
rp3d::Vector3 body1Position = body1->getTransform().getPosition();
rp3d::Vector3 body2Position = body2->getTransform().getPosition();
const rp3d::Vector3 anchorPointWorldSpace = 0.5 * (body1Position + body2Position);
rp3d::BallAndSocketJointInfo jointInfo(body1, body2, anchorPointWorldSpace);
// Create the joint in the dynamics world
mBallAndSocketJoints[i] = dynamic_cast<rp3d::BallAndSocketJoint*>(
mDynamicsWorld->createJoint(jointInfo));
}
}
/// Create the boxes and joint for the Slider joint example
void Scene::createSliderJoint() {
// --------------- Create the first box --------------- //
// Position of the box
openglframework::Vector3 positionBox1(0, 2.1, 0);
// Create a box and a corresponding rigid in the dynamics world
openglframework::Vector3 box1Dimension(2, 4, 2);
mSliderJointBottomBox = new Box(box1Dimension, positionBox1 , BOX_MASS, mDynamicsWorld);
// The fist box cannot move
mSliderJointBottomBox->getRigidBody()->setIsMotionEnabled(false);
// Set the bouncing factor of the box
mSliderJointBottomBox->getRigidBody()->setRestitution(0.4);
// --------------- Create the second box --------------- //
// Position of the box
openglframework::Vector3 positionBox2(0, 4.2, 0);
// Create a box and a corresponding rigid in the dynamics world
openglframework::Vector3 box2Dimension(1.5, 4, 1.5);
mSliderJointTopBox = new Box(box2Dimension, positionBox2 , BOX_MASS, mDynamicsWorld);
// The second box is allowed to move
mSliderJointTopBox->getRigidBody()->setIsMotionEnabled(true);
// Set the bouncing factor of the box
mSliderJointTopBox->getRigidBody()->setRestitution(0.4);
// --------------- Create the joint --------------- //
// Create the joint info object
rp3d::RigidBody* body1 = mSliderJointBottomBox->getRigidBody();
rp3d::RigidBody* body2 = mSliderJointTopBox->getRigidBody();
const rp3d::Vector3& body1Position = body1->getTransform().getPosition();
const rp3d::Vector3& body2Position = body2->getTransform().getPosition();
const rp3d::Vector3 anchorPointWorldSpace = 0.5 * (body2Position + body1Position);
const rp3d::Vector3 sliderAxisWorldSpace = (body2Position - body1Position);
rp3d::SliderJointInfo jointInfo(body1, body2, anchorPointWorldSpace, sliderAxisWorldSpace,
-1.7, 1.7);
jointInfo.isMotorEnabled = true;
jointInfo.motorSpeed = 0.0;
jointInfo.maxMotorForce = 10000.0;
jointInfo.isCollisionEnabled = false;
// Create the joint in the dynamics world
mSliderJoint = dynamic_cast<rp3d::SliderJoint*>(mDynamicsWorld->createJoint(jointInfo));
}
/// Create the boxes and joint for the Hinge joint example
void Scene::createPropellerHingeJoint() {
// --------------- Create the propeller box --------------- //
// Position of the box
openglframework::Vector3 positionBox1(0, 7, 0);
// Create a box and a corresponding rigid in the dynamics world
openglframework::Vector3 boxDimension(10, 1, 1);
mPropellerBox = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld);
// The fist box cannot move
mPropellerBox->getRigidBody()->setIsMotionEnabled(true);
// Set the bouncing factor of the box
mPropellerBox->getRigidBody()->setRestitution(0.4);
// --------------- Create the Hinge joint --------------- //
// Create the joint info object
rp3d::RigidBody* body1 = mPropellerBox->getRigidBody();
rp3d::RigidBody* body2 = mSliderJointTopBox->getRigidBody();
const rp3d::Vector3& body1Position = body1->getTransform().getPosition();
const rp3d::Vector3& body2Position = body2->getTransform().getPosition();
const rp3d::Vector3 anchorPointWorldSpace = 0.5 * (body2Position + body1Position);
const rp3d::Vector3 hingeAxisWorldSpace(0, 1, 0);
rp3d::HingeJointInfo jointInfo(body1, body2, anchorPointWorldSpace, hingeAxisWorldSpace);
jointInfo.isMotorEnabled = true;
jointInfo.motorSpeed = -0.5 * PI;
jointInfo.maxMotorTorque = 60.0;
jointInfo.isCollisionEnabled = false;
// Create the joint in the dynamics world
mPropellerHingeJoint = dynamic_cast<rp3d::HingeJoint*>(mDynamicsWorld->createJoint(jointInfo));
}
/// Create the boxes and joints for the fixed joints
void Scene::createFixedJoints() {
// --------------- Create the first box --------------- //
// Position of the box
openglframework::Vector3 positionBox1(5, 7, 0);
// Create a box and a corresponding rigid in the dynamics world
openglframework::Vector3 boxDimension(1.5, 1.5, 1.5);
mFixedJointBox1 = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld);
// The fist box cannot move
mFixedJointBox1->getRigidBody()->setIsMotionEnabled(true);
// Set the bouncing factor of the box
mFixedJointBox1->getRigidBody()->setRestitution(0.4);
// --------------- Create the second box --------------- //
// Position of the box
openglframework::Vector3 positionBox2(-5, 7, 0);
// Create a box and a corresponding rigid in the dynamics world
mFixedJointBox2 = new Box(boxDimension, positionBox2 , BOX_MASS, mDynamicsWorld);
// The second box is allowed to move
mFixedJointBox2->getRigidBody()->setIsMotionEnabled(true);
// Set the bouncing factor of the box
mFixedJointBox2->getRigidBody()->setRestitution(0.4);
// --------------- Create the first fixed joint --------------- //
// Create the joint info object
rp3d::RigidBody* body1 = mFixedJointBox1->getRigidBody();
rp3d::RigidBody* propellerBody = mPropellerBox->getRigidBody();
const rp3d::Vector3 anchorPointWorldSpace1(5, 7, 0);
rp3d::FixedJointInfo jointInfo1(body1, propellerBody, anchorPointWorldSpace1);
// Create the joint in the dynamics world
mFixedJoint1 = dynamic_cast<rp3d::FixedJoint*>(mDynamicsWorld->createJoint(jointInfo1));
// --------------- Create the second fixed joint --------------- //
// Create the joint info object
rp3d::RigidBody* body2 = mFixedJointBox2->getRigidBody();
const rp3d::Vector3 anchorPointWorldSpace2(-5, 7, 0);
rp3d::FixedJointInfo jointInfo2(body2, propellerBody, anchorPointWorldSpace2);
// Create the joint in the dynamics world
mFixedJoint2 = dynamic_cast<rp3d::FixedJoint*>(mDynamicsWorld->createJoint(jointInfo2));
}
// Create the floor
void Scene::createFloor() {
// Create the floor
openglframework::Vector3 floorPosition(0, 0, 0);
mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld);
// The floor must be a non-moving rigid body
mFloor->getRigidBody()->setIsMotionEnabled(false);
// Set the bouncing factor of the floor
mFloor->getRigidBody()->setRestitution(0.3);
}

171
examples/joints/Scene.h Normal file
View File

@ -0,0 +1,171 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 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 SCENE_H
#define SCENE_H
// Libraries
#include "openglframework.h"
#include "reactphysics3d.h"
#include "Box.h"
// Constants
const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters
const openglframework::Vector3 FLOOR_SIZE(20, 0.5f, 20); // Floor dimensions in meters
const float BOX_MASS = 1.0f; // Box mass in kilograms
const float FLOOR_MASS = 100.0f; // Floor mass in kilograms
const int NB_BALLSOCKETJOINT_BOXES = 7; // Number of Ball-And-Socket chain boxes
const int NB_HINGE_BOXES = 7; // Number of Hinge chain boxes
// Class Scene
class Scene {
private :
// -------------------- Attributes -------------------- //
/// Pointer to the viewer
openglframework::GlutViewer* mViewer;
/// Light 0
openglframework::Light mLight0;
/// Phong shader
openglframework::Shader mPhongShader;
/// Boxes of Ball-And-Socket joint chain
Box* mBallAndSocketJointChainBoxes[NB_BALLSOCKETJOINT_BOXES];
/// Boxes of the Hinge joint chain
Box* mHingeJointChainBoxes[NB_HINGE_BOXES];
/// Ball-And-Socket joints of the chain
rp3d::BallAndSocketJoint* mBallAndSocketJoints[NB_BALLSOCKETJOINT_BOXES-1];
/// Hinge joints of the chain
rp3d::HingeJoint* mHingeJoints[NB_HINGE_BOXES-1];
/// Bottom box of the Slider joint
Box* mSliderJointBottomBox;
/// Top box of the Slider joint
Box* mSliderJointTopBox;
/// Slider joint
rp3d::SliderJoint* mSliderJoint;
/// Propeller box
Box* mPropellerBox;
/// Box 1 of Fixed joint
Box* mFixedJointBox1;
/// Box 2 of Fixed joint
Box* mFixedJointBox2;
/// Hinge joint
rp3d::HingeJoint* mPropellerHingeJoint;
/// First Fixed joint
rp3d::FixedJoint* mFixedJoint1;
/// Second Fixed joint
rp3d::FixedJoint* mFixedJoint2;
/// Box for the floor
Box* mFloor;
/// Dynamics world used for the physics simulation
rp3d::DynamicsWorld* mDynamicsWorld;
/// True if the physics simulation is running
bool mIsRunning;
// -------------------- Methods -------------------- //
/// Create the boxes and joints for the Ball-and-Socket joint example
void createBallAndSocketJoints();
/// Create the boxes and joint for the Slider joint example
void createSliderJoint();
/// Create the boxes and joint for the Hinge joint example
void createPropellerHingeJoint();
/// Create the boxes and joint for the Fixed joint example
void createFixedJoints();
/// Create the floor
void createFloor();
public:
// -------------------- Methods -------------------- //
/// Constructor
Scene(openglframework::GlutViewer* viewer);
/// Destructor
~Scene();
/// Take a step for the simulation
void simulate();
/// Stop the simulation
void stopSimulation();
/// Start the simulation
void startSimulation();
/// Pause or continue simulation
void pauseContinueSimulation();
/// Render the scene
void render();
};
// Stop the simulation
inline void Scene::stopSimulation() {
mDynamicsWorld->stop();
mIsRunning = false;
}
// Start the simulation
inline void Scene::startSimulation() {
mDynamicsWorld->start();
mIsRunning = true;
}
// Pause or continue simulation
inline void Scene::pauseContinueSimulation() {
if (mIsRunning) {
stopSimulation();
}
else {
startSimulation();
}
}
#endif

4
src/body/CollisionBody.h
View File

@ -208,7 +208,7 @@ inline const Transform& CollisionBody::getTransform() const {
inline void CollisionBody::setTransform(const Transform& transform) {
// Check if the body has moved
if (this->mTransform != transform) {
if (mTransform != transform) {
mHasMoved = true;
}
@ -239,8 +239,6 @@ inline void CollisionBody::updateOldTransform() {
// Update the rigid body in order to reflect a change in the body state
inline void CollisionBody::updateAABB() {
// TODO : An AABB should not be updated every frame but only if the body has moved
// Update the AABB
mCollisionShape->updateAABB(mAabb, mTransform);
}

4
src/body/RigidBody.h
View File

@ -135,7 +135,7 @@ class RigidBody : public CollisionBody {
decimal getMassInverse() const;
/// Return the local inertia tensor of the body (in body coordinates)
Matrix3x3 getInertiaTensorLocal() const;
const Matrix3x3& getInertiaTensorLocal() const;
/// Set the local inertia tensor of the body (in body coordinates)
void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal);
@ -222,7 +222,7 @@ inline decimal RigidBody::getMassInverse() const {
}
// Return the local inertia tensor of the body (in body coordinates)
inline Matrix3x3 RigidBody::getInertiaTensorLocal() const {
inline const Matrix3x3& RigidBody::getInertiaTensorLocal() const {
return mInertiaTensorLocal;
}

15
src/collision/BroadPhasePair.h
View File

@ -60,6 +60,9 @@ struct BroadPhasePair {
/// Destructor
~BroadPhasePair();
/// Return the pair of bodies index
static bodyindexpair computeBodiesIndexPair(CollisionBody* body1, CollisionBody* body2);
/// Return the pair of bodies index
bodyindexpair getBodiesIndexPair() const;
@ -77,16 +80,22 @@ struct BroadPhasePair {
};
// Return the pair of bodies index
inline bodyindexpair BroadPhasePair::getBodiesIndexPair() const {
inline bodyindexpair BroadPhasePair::computeBodiesIndexPair(CollisionBody* body1,
CollisionBody* body2) {
// Construct the pair of body index
bodyindexpair indexPair = body1->getID() < body2->getID() ?
bodyindexpair indexPair = body1->getID() < body2->getID() ?
std::make_pair(body1->getID(), body2->getID()) :
std::make_pair(body2->getID(), body1->getID());
assert(indexPair.first != indexPair.second);
return indexPair;
}
// Return the pair of bodies index
inline bodyindexpair BroadPhasePair::getBodiesIndexPair() const {
return computeBodiesIndexPair(body1, body2);
}
// Smaller than operator
inline bool BroadPhasePair::operator<(const BroadPhasePair& broadPhasePair2) const {
return (body1 < broadPhasePair2.body1 ? true : (body2 < broadPhasePair2.body2));

15
src/collision/CollisionDetection.cpp
View File

@ -99,7 +99,7 @@ void CollisionDetection::computeNarrowPhase() {
// For each possible collision pair of bodies
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); it++) {
ContactInfo* contactInfo = NULL;
ContactPointInfo* contactInfo = NULL;
BroadPhasePair* pair = (*it).second;
assert(pair != NULL);
@ -109,6 +109,9 @@ void CollisionDetection::computeNarrowPhase() {
// Update the contact cache of the overlapping pair
mWorld->updateOverlappingPair(pair);
// Check if the two bodies are allowed to collide, otherwise, we do not test for collision
if (mNoCollisionPairs.count(pair->getBodiesIndexPair()) > 0) continue;
// Select the narrow phase algorithm to use according to the two collision shapes
NarrowPhaseAlgorithm& narrowPhaseAlgorithm = SelectNarrowPhaseAlgorithm(
@ -125,12 +128,18 @@ void CollisionDetection::computeNarrowPhase() {
contactInfo)) {
assert(contactInfo != NULL);
// Set the bodies of the contact
contactInfo->body1 = dynamic_cast<RigidBody*>(body1);
contactInfo->body2 = dynamic_cast<RigidBody*>(body2);
assert(contactInfo->body1 != NULL);
assert(contactInfo->body2 != NULL);
// Notify the world about the new narrow-phase contact
mWorld->notifyNewContact(pair, contactInfo);
// Delete and remove the contact info from the memory allocator
contactInfo->ContactInfo::~ContactInfo();
mMemoryAllocator.release(contactInfo, sizeof(ContactInfo));
contactInfo->ContactPointInfo::~ContactPointInfo();
mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
}
}
}

25
src/collision/CollisionDetection.h
View File

@ -33,7 +33,7 @@
#include "narrowphase/GJK/GJKAlgorithm.h"
#include "narrowphase/SphereVsSphereAlgorithm.h"
#include "../memory/MemoryAllocator.h"
#include "ContactInfo.h"
#include "../constraint/ContactPoint.h"
#include <vector>
#include <map>
#include <set>
@ -79,6 +79,9 @@ class CollisionDetection {
/// Narrow-phase Sphere vs Sphere algorithm
SphereVsSphereAlgorithm mNarrowPhaseSphereVsSphereAlgorithm;
/// Set of pair of bodies that cannot collide between each other
std::set<bodyindexpair> mNoCollisionPairs;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -113,6 +116,12 @@ class CollisionDetection {
/// Remove a body from the collision detection
void removeBody(CollisionBody* body);
/// Add a pair of bodies that cannot collide with each other
void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2);
/// Remove a pair of bodies that cannot collide with each other
void removeNoCollisionPair(CollisionBody *body1, CollisionBody *body2);
/// Compute the collision detection
void computeCollisionDetection();
@ -148,7 +157,19 @@ inline void CollisionDetection::removeBody(CollisionBody* body) {
// Remove the body from the broad-phase
mBroadPhaseAlgorithm->removeObject(body);
}
}
// Add a pair of bodies that cannot collide with each other
inline void CollisionDetection::addNoCollisionPair(CollisionBody* body1,
CollisionBody* body2) {
mNoCollisionPairs.insert(BroadPhasePair::computeBodiesIndexPair(body1, body2));
}
// Remove a pair of bodies that cannot collide with each other
inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1,
CollisionBody* body2) {
mNoCollisionPairs.erase(BroadPhasePair::computeBodiesIndexPair(body1, body2));
}
}

10
src/collision/narrowphase/EPA/EPAAlgorithm.cpp
View File

@ -87,7 +87,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
const Transform& transform1,
const CollisionShape* collisionShape2,
const Transform& transform2,
Vector3& v, ContactInfo*& contactInfo) {
Vector3& v, ContactPointInfo*& contactInfo) {
Vector3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
Vector3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
@ -98,7 +98,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// Transform a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2Tobody1 = transform1.inverse() * transform2;
Transform body2Tobody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local
// space of body 1 into local space of body 2
@ -143,7 +143,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
int minAxis = d.getAbsoluteVector().getMinAxis();
// Compute sin(60)
const decimal sin60 = sqrt(3.0) * 0.5;
const decimal sin60 = decimal(sqrt(3.0)) * decimal(0.5);
// Create a rotation quaternion to rotate the vector v1 to get the vectors
// v2 and v3
@ -394,13 +394,13 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// Compute the contact info
v = transform1.getOrientation().getMatrix() * triangle->getClosestPoint();
Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
Vector3 pBLocal = body2Tobody1.inverse() * triangle->computeClosestPointOfObject(suppPointsB);
Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
Vector3 normal = v.getUnit();
decimal penetrationDepth = v.length();
assert(penetrationDepth > 0.0);
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal,
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
penetrationDepth,
pALocal, pBLocal);

4
src/collision/narrowphase/EPA/EPAAlgorithm.h
View File

@ -29,7 +29,7 @@
// Libraries
#include "../GJK/Simplex.h"
#include "../../shapes/CollisionShape.h"
#include "../../ContactInfo.h"
#include "../../../constraint/ContactPoint.h"
#include "../../../mathematics/mathematics.h"
#include "TriangleEPA.h"
#include "../../../memory/MemoryAllocator.h"
@ -123,7 +123,7 @@ class EPAAlgorithm {
const Transform& transform1,
const CollisionShape* collisionShape2,
const Transform& transform2,
Vector3& v, ContactInfo*& contactInfo);
Vector3& v, ContactPointInfo*& contactInfo);
};
// Add a triangle face in the candidate triangle heap in the EPA algorithm

24
src/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

@ -61,7 +61,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
const Transform& transform1,
const CollisionShape* collisionShape2,
const Transform& transform2,
ContactInfo*& contactInfo) {
ContactPointInfo*& contactInfo) {
Vector3 suppA; // Support point of object A
Vector3 suppB; // Support point of object B
@ -73,7 +73,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
// Transform a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2Tobody1 = transform1.inverse() * transform2;
Transform body2Tobody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local
// space of body 1 into local space of body 2
@ -127,7 +127,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
@ -137,7 +137,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
if (penetrationDepth <= 0.0) return false;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal,
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
penetrationDepth,
pA, pB);
@ -159,7 +159,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
@ -169,7 +169,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
if (penetrationDepth <= 0.0) return false;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal,
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
penetrationDepth,
pA, pB);
@ -189,7 +189,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
@ -199,7 +199,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
if (penetrationDepth <= 0.0) return false;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal,
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
penetrationDepth,
pA, pB);
@ -226,7 +226,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
@ -236,7 +236,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
if (penetrationDepth <= 0.0) return false;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal,
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
penetrationDepth,
pA, pB);
@ -263,7 +263,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
const Transform& transform1,
const CollisionShape* collisionShape2,
const Transform& transform2,
ContactInfo*& contactInfo,
ContactPointInfo*& contactInfo,
Vector3& v) {
Simplex simplex;
Vector3 suppA;
@ -275,7 +275,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
// Transform a point from local space of body 2 to local space
// of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2ToBody1 = transform1.inverse() * transform2;
Transform body2ToBody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local space of body 1 into local space of body 2
Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *

6
src/collision/narrowphase/GJK/GJKAlgorithm.h
View File

@ -28,7 +28,7 @@
// Libraries
#include "../NarrowPhaseAlgorithm.h"
#include "../../ContactInfo.h"
#include "../../../constraint/ContactPoint.h"
#include "../../../collision/shapes/CollisionShape.h"
#include "../EPA/EPAAlgorithm.h"
@ -78,7 +78,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
const Transform& transform1,
const CollisionShape* collisionShape2,
const Transform& transform2,
ContactInfo*& contactInfo, Vector3& v);
ContactPointInfo*& contactInfo, Vector3& v);
public :
@ -95,7 +95,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
const Transform& transform1,
const CollisionShape* collisionShape2,
const Transform& transform2,
ContactInfo*& contactInfo);
ContactPointInfo*& contactInfo);
};
}

6
src/collision/narrowphase/NarrowPhaseAlgorithm.h
View File

@ -28,7 +28,7 @@
// Libraries
#include "../../body/Body.h"
#include "../ContactInfo.h"
#include "../../constraint/ContactPoint.h"
#include "../broadphase/PairManager.h"
#include "../../memory/MemoryAllocator.h"
#include "../BroadPhasePair.h"
@ -36,7 +36,7 @@
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Class NarrowPhaseAlgorithm
/**
* This class is an abstract class that represents an algorithm
@ -82,7 +82,7 @@ class NarrowPhaseAlgorithm {
const Transform& transform1,
const CollisionShape* collisionShape2,
const Transform& transform2,
ContactInfo*& contactInfo)=0;
ContactPointInfo*& contactInfo)=0;
};
// Set the current overlapping pair of bodies

8
src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
View File

@ -45,7 +45,7 @@ bool SphereVsSphereAlgorithm::testCollision(const CollisionShape* collisionShape
const Transform& transform1,
const CollisionShape* collisionShape2,
const Transform& transform2,
ContactInfo*& contactInfo) {
ContactPointInfo*& contactInfo) {
// Get the sphere collision shapes
const SphereShape* sphereShape1 = dynamic_cast<const SphereShape*>(collisionShape1);
@ -60,8 +60,8 @@ bool SphereVsSphereAlgorithm::testCollision(const CollisionShape* collisionShape
// If the sphere collision shapes intersect
if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
Vector3 centerSphere2InBody1LocalSpace = transform1.inverse() * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2.inverse() * transform1.getPosition();
Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
Vector3 intersectionOnBody1 = sphereShape1->getRadius() *
centerSphere2InBody1LocalSpace.getUnit();
Vector3 intersectionOnBody2 = sphereShape2->getRadius() *
@ -69,7 +69,7 @@ bool SphereVsSphereAlgorithm::testCollision(const CollisionShape* collisionShape
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(
vectorBetweenCenters.getUnit(), penetrationDepth,
intersectionOnBody1, intersectionOnBody2);

4
src/collision/narrowphase/SphereVsSphereAlgorithm.h
View File

@ -28,7 +28,7 @@
// Libraries
#include "../../body/Body.h"
#include "../ContactInfo.h"
#include "../../constraint/ContactPoint.h"
#include "NarrowPhaseAlgorithm.h"
@ -67,7 +67,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
const Transform& transform1,
const CollisionShape* collisionShape2,
const Transform& transform2,
ContactInfo*& contactInfo);
ContactPointInfo*& contactInfo);
};
}

27
src/configuration.h
View File

@ -51,6 +51,21 @@ typedef long unsigned int luint;
typedef luint bodyindex;
typedef std::pair<bodyindex, bodyindex> bodyindexpair;
// ------------------- Enumerations ------------------- //
/// Position correction technique used in the constraint solver (for joints).
/// BAUMGARTE_JOINTS : Faster but can be innacurate in some situations.
/// NON_LINEAR_GAUSS_SEIDEL : Slower but more precise. This is the option used by default.
enum JointsPositionCorrectionTechnique {BAUMGARTE_JOINTS, NON_LINEAR_GAUSS_SEIDEL};
/// Position correction technique used in the contact solver (for contacts)
/// BAUMGARTE_CONTACTS : Faster but can be innacurate and can lead to unexpected bounciness
/// in some situations (due to error correction factor being added to
/// the bodies momentum).
/// SPLIT_IMPULSES : A bit slower but the error correction factor is not added to the
/// bodies momentum. This is the option used by default.
enum ContactsPositionCorrectionTechnique {BAUMGARTE_CONTACTS, SPLIT_IMPULSES};
// ------------------- Constants ------------------- //
/// Smallest decimal value (negative)
@ -65,6 +80,9 @@ const decimal MACHINE_EPSILON = std::numeric_limits<decimal>::epsilon();
/// Pi constant
const decimal PI = decimal(3.14159265);
/// 2*Pi constant
const decimal PI_TIMES_2 = decimal(6.28318530);
/// Default internal constant timestep in seconds
const decimal DEFAULT_TIMESTEP = decimal(1.0 / 60.0);
@ -74,7 +92,7 @@ const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3);
/// True if the deactivation (sleeping) of inactive bodies is enabled
const bool DEACTIVATION_ENABLED = true;
///Object margin for collision detection in cm (For GJK-EPA Algorithm)
/// Object margin for collision detection in cm (For GJK-EPA Algorithm)
const decimal OBJECT_MARGIN = decimal(0.04);
/// Distance threshold for two contact points for a valid persistent contact (in meters)
@ -83,8 +101,11 @@ const decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03);
/// Velocity threshold for contact velocity restitution
const decimal RESTITUTION_VELOCITY_THRESHOLD = decimal(1.0);
/// Number of iterations when solving a LCP problem
const uint DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS = 15;
/// Number of iterations when solving the velocity constraints of the Sequential Impulse technique
const uint DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 15;
/// Number of iterations when solving the position constraints of the Sequential Impulse technique
const uint DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5;
}

281
src/constraint/BallAndSocketJoint.cpp Normal file
View File

@ -0,0 +1,281 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "BallAndSocketJoint.h"
#include "../engine/ConstraintSolver.h"
using namespace reactphysics3d;
// Static variables definition
const decimal BallAndSocketJoint::BETA = decimal(0.2);
// Constructor
BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo)
: Constraint(jointInfo), mImpulse(Vector3(0, 0, 0)) {
// Compute the local-space anchor point for each body
mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
}
// Destructor
BallAndSocketJoint::~BallAndSocketJoint() {
}
// Initialize before solving the constraint
void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the velocity array
mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->getTransform().getPosition();
const Vector3& x2 = mBody2->getTransform().getPosition();
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
// Get the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space
mR1World = orientationBody1 * mLocalAnchorPointBody1;
mR2World = orientationBody2 * mLocalAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix)
decimal inverseMassBodies = 0.0;
if (mBody1->getIsMotionEnabled()) {
inverseMassBodies += mBody1->getMassInverse();
}
if (mBody2->getIsMotionEnabled()) {
inverseMassBodies += mBody2->getMassInverse();
}
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies);
if (mBody1->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
}
if (mBody2->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
}
// Compute the inverse mass matrix K^-1
mInverseMassMatrix.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrix = massMatrix.getInverse();
}
// Compute the bias "b" of the constraint
mBiasVector.setToZero();
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
decimal biasFactor = (BETA / constraintSolverData.timeStep);
mBiasVector = biasFactor * (x2 + mR2World - x1 - mR1World);
}
// If warm-starting is not enabled
if (!constraintSolverData.isWarmStartingActive) {
// Reset the accumulated impulse
mImpulse.setToZero();
}
}
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass of the bodies
const decimal inverseMassBody1 = mBody1->getMassInverse();
const decimal inverseMassBody2 = mBody2->getMassInverse();
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody1 = -mImpulse;
const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World);
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody2 = mImpulse;
const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World);
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
}
// Solve the velocity constraint
void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
// Compute J*v
const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World);
// Compute the Lagrange multiplier lambda
const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector);
mImpulse += deltaLambda;
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody1 = -deltaLambda;
const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody2 = deltaLambda;
const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
}
// Solve the position constraint (for position error correction)
void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
Vector3& x1 = constraintSolverData.positions[mIndexBody1];
Vector3& x2 = constraintSolverData.positions[mIndexBody2];
Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space
mR1World = q1 * mLocalAnchorPointBody1;
mR2World = q2 * mLocalAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints
decimal inverseMassBodies = 0.0;
if (mBody1->getIsMotionEnabled()) {
inverseMassBodies += inverseMassBody1;
}
if (mBody2->getIsMotionEnabled()) {
inverseMassBodies += inverseMassBody2;
}
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies);
if (mBody1->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
}
if (mBody2->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
}
mInverseMassMatrix.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrix = massMatrix.getInverse();
}
// Compute the constraint error (value of the C(x) function)
const Vector3 constraintError = (x2 + mR2World - x1 - mR1World);
// Compute the Lagrange multiplier lambda
// TODO : Do not solve the system by computing the inverse each time and multiplying with the
// right-hand side vector but instead use a method to directly solve the linear system.
const Vector3 lambda = mInverseMassMatrix * (-constraintError);
// Apply the impulse to the bodies of the joint (directly update the position/orientation)
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse
const Vector3 linearImpulseBody1 = -lambda;
const Vector3 angularImpulseBody1 = lambda.cross(mR1World);
// Compute the pseudo velocity
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse
const Vector3 linearImpulseBody2 = lambda;
const Vector3 angularImpulseBody2 = -lambda.cross(mR2World);
// Compute the pseudo velocity
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
}

132
src/constraint/BallAndSocketJoint.h Normal file
View File

@ -0,0 +1,132 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 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
// Libraries
#include "Constraint.h"
#include "../mathematics/mathematics.h"
namespace reactphysics3d {
// Structure BallAndSocketJointInfo
/**
* This structure is used to gather the information needed to create a ball-and-socket
* joint. This structure will be used to create the actual ball-and-socket joint.
*/
struct BallAndSocketJointInfo : public ConstraintInfo {
public :
// -------------------- Attributes -------------------- //
/// Anchor point (in world-space coordinates)
Vector3 anchorPointWorldSpace;
/// Constructor
BallAndSocketJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace)
: ConstraintInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace) {}
};
// Class BallAndSocketJoint
/**
* This class represents a ball-and-socket joint that allows arbitrary rotation
* between two bodies.
*/
class BallAndSocketJoint : public Constraint {
private :
// -------------------- Constants -------------------- //
// Beta value for the bias factor of position correction
static const decimal BETA;
// -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local-space coordinates of body 1)
Vector3 mLocalAnchorPointBody1;
/// Anchor point of body 2 (in local-space coordinates of body 2)
Vector3 mLocalAnchorPointBody2;
/// Vector from center of body 2 to anchor point in world-space
Vector3 mR1World;
/// Vector from center of body 2 to anchor point in world-space
Vector3 mR2World;
/// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 mI1;
/// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 mI2;
/// Bias vector for the constraint
Vector3 mBiasVector;
/// Inverse mass matrix K=JM^-1J^-t of the constraint
Matrix3x3 mInverseMassMatrix;
/// Accumulated impulse
Vector3 mImpulse;
public :
// -------------------- Methods -------------------- //
/// Constructor
BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo);
/// Destructor
virtual ~BallAndSocketJoint();
/// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const;
/// Initialize before solving the constraint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
/// Warm start the constraint (apply the previous impulse at the beginning of the step)
virtual void warmstart(const ConstraintSolverData& constraintSolverData);
/// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
/// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
};
// Return the number of bytes used by the joint
inline size_t BallAndSocketJoint::getSizeInBytes() const {
return sizeof(BallAndSocketJoint);
}
}
#endif

17
src/constraint/Constraint.cpp
View File

@ -29,15 +29,14 @@
using namespace reactphysics3d;
// Constructor
Constraint::Constraint(RigidBody* const body1, RigidBody* const body2,
uint nbConstraints, bool active, ConstraintType type)
:mBody1(body1), mBody2(body2), mActive(active),
mNbConstraints(nbConstraints), mType(type) {
// Initialize the cached lambda values
for (uint i=0; i<nbConstraints; i++) {
mCachedLambdas.push_back(0.0);
}
Constraint::Constraint(const ConstraintInfo& constraintInfo)
:mBody1(constraintInfo.body1), mBody2(constraintInfo.body2), mActive(true),
mType(constraintInfo.type),
mPositionCorrectionTechnique(constraintInfo.positionCorrectionTechnique),
mIsCollisionEnabled(constraintInfo.isCollisionEnabled){
assert(mBody1 != NULL);
assert(mBody2 != NULL);
}
// Destructor

114
src/constraint/Constraint.h
View File

@ -27,14 +27,62 @@
#define REACTPHYSICS3D_CONSTRAINT_H
// Libraries
#include "../configuration.h"
#include "../body/RigidBody.h"
#include "../mathematics/mathematics.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
// Enumeration for the type of a constraint
enum ConstraintType {CONTACT};
enum ConstraintType {CONTACT, BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT, FIXEDJOINT};
// Class declarations
struct ConstraintSolverData;
// Structure ConstraintInfo
/**
* This structure is used to gather the information needed to create a constraint.
*/
struct ConstraintInfo {
public :
// -------------------- Attributes -------------------- //
/// First rigid body of the constraint
RigidBody* body1;
/// Second rigid body of the constraint
RigidBody* body2;
/// Type of the constraint
ConstraintType type;
/// True if the two bodies of the constraint are allowed to collide with each other
bool isCollisionEnabled;
/// Position correction technique used for the constraint (used for joints).
/// By default, the BAUMGARTE technique is used
JointsPositionCorrectionTechnique positionCorrectionTechnique;
/// Constructor
ConstraintInfo(ConstraintType constraintType)
: body1(NULL), body2(NULL), type(constraintType),
positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL),
isCollisionEnabled(true) {}
/// Constructor
ConstraintInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, ConstraintType constraintType)
: body1(rigidBody1), body2(rigidBody2), type(constraintType),
positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL),
isCollisionEnabled(true) {
}
/// Destructor
virtual ~ConstraintInfo() {}
};
// Class Constraint
/**
@ -58,15 +106,20 @@ class Constraint {
/// True if the constraint is active
bool mActive;
/// Number mathematical constraints associated with this Constraint
uint mNbConstraints;
/// Type of the constraint
const ConstraintType mType;
/// Cached lambda values of each mathematical constraint for
/// more precise initializaton of LCP solver
std::vector<decimal> mCachedLambdas;
/// Body 1 index in the velocity array to solve the constraint
uint mIndexBody1;
/// Body 2 index in the velocity array to solve the constraint
uint mIndexBody2;
/// Position correction technique used for the constraint (used for joints)
JointsPositionCorrectionTechnique mPositionCorrectionTechnique;
/// True if the two bodies of the constraint are allowed to collide with each other
bool mIsCollisionEnabled;
// -------------------- Methods -------------------- //
@ -81,8 +134,7 @@ class Constraint {
// -------------------- Methods -------------------- //
/// Constructor
Constraint(RigidBody* const body1, RigidBody* const body2, uint nbConstraints,
bool active, ConstraintType type);
Constraint(const ConstraintInfo& constraintInfo);
/// Destructor
virtual ~Constraint();
@ -99,14 +151,23 @@ class Constraint {
/// Return the type of the constraint
ConstraintType getType() const;
/// Return the number of mathematical constraints
unsigned int getNbConstraints() const;
/// Return true if the collision between the two bodies of the constraint is enabled
bool isCollisionEnabled() const;
/// Get one cached lambda value
decimal getCachedLambda(uint index) const;
/// Return the number of bytes used by the constraint
virtual size_t getSizeInBytes() const = 0;
/// Set on cached lambda value
void setCachedLambda(uint index, decimal lambda);
/// Initialize before solving the constraint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) = 0;
/// Warm start the constraint (apply the previous impulse at the beginning of the step)
virtual void warmstart(const ConstraintSolverData& constraintSolverData) = 0;
/// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) = 0;
/// Solve the position constraint
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) = 0;
};
// Return the reference to the body 1
@ -127,25 +188,12 @@ inline bool Constraint::isActive() const {
// Return the type of the constraint
inline ConstraintType Constraint::getType() const {
return mType;
}
// Return the number auxiliary constraints
inline uint Constraint::getNbConstraints() const {
return mNbConstraints;
}
// Get one previous lambda value
inline decimal Constraint::getCachedLambda(uint index) const {
assert(index < mNbConstraints);
return mCachedLambdas[index];
}
// Set on cached lambda value
inline void Constraint::setCachedLambda(uint index, decimal lambda) {
assert(index < mNbConstraints);
mCachedLambdas[index] = lambda;
}
// Return true if the collision between the two bodies of the constraint is enabled
inline bool Constraint::isCollisionEnabled() const {
return mIsCollisionEnabled;
}
}

40
src/constraint/ContactPoint.cpp
View File

@ -30,15 +30,17 @@ using namespace reactphysics3d;
using namespace std;
// Constructor
ContactPoint::ContactPoint(RigidBody* const body1, RigidBody* const body2,
const ContactInfo* contactInfo)
: Constraint(body1, body2, 3, true, CONTACT), mNormal(contactInfo->normal),
mPenetrationDepth(contactInfo->penetrationDepth),
mLocalPointOnBody1(contactInfo->localPoint1),
mLocalPointOnBody2(contactInfo->localPoint2),
mWorldPointOnBody1(body1->getTransform() * contactInfo->localPoint1),
mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2),
mIsRestingContact(false), mFrictionVectors(2, Vector3(0, 0, 0)) {
ContactPoint::ContactPoint(const ContactPointInfo& contactInfo)
: Constraint(contactInfo), mNormal(contactInfo.normal),
mPenetrationDepth(contactInfo.penetrationDepth),
mLocalPointOnBody1(contactInfo.localPoint1),
mLocalPointOnBody2(contactInfo.localPoint2),
mWorldPointOnBody1(contactInfo.body1->getTransform() * contactInfo.localPoint1),
mWorldPointOnBody2(contactInfo.body2->getTransform() * contactInfo.localPoint2),
mIsRestingContact(false) {
mFrictionVectors[0] = Vector3(0, 0, 0);
mFrictionVectors[1] = Vector3(0, 0, 0);
assert(mPenetrationDepth > 0.0);
@ -48,3 +50,23 @@ ContactPoint::ContactPoint(RigidBody* const body1, RigidBody* const body2,
ContactPoint::~ContactPoint() {
}
// Initialize before solving the constraint
void ContactPoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
}
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void ContactPoint::warmstart(const ConstraintSolverData& constraintSolverData) {
}
// Solve the velocity constraint
void ContactPoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
}
// Solve the position constraint
void ContactPoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
}

127
src/constraint/ContactPoint.h
View File

@ -28,7 +28,6 @@
// Libraries
#include "Constraint.h"
#include "../collision/ContactInfo.h"
#include "../body/RigidBody.h"
#include "../configuration.h"
#include "../mathematics/mathematics.h"
@ -50,6 +49,52 @@
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Structure ContactPointInfo
/**
* This structure contains informations about a collision contact
* computed during the narrow-phase collision detection. Those
* informations are used to compute the contact set for a contact
* between two bodies.
*/
struct ContactPointInfo : public ConstraintInfo {
private:
// -------------------- Methods -------------------- //
/// Private copy-constructor
ContactPointInfo(const ContactPointInfo& contactInfo);
/// Private assignment operator
ContactPointInfo& operator=(const ContactPointInfo& contactInfo);
public:
// -------------------- Attributes -------------------- //
/// Normal vector the the collision contact in world space
const Vector3 normal;
/// Penetration depth of the contact
const decimal penetrationDepth;
/// Contact point of body 1 in local space of body 1
const Vector3 localPoint1;
/// Contact point of body 2 in local space of body 2
const Vector3 localPoint2;
// -------------------- Methods -------------------- //
/// Constructor
ContactPointInfo(const Vector3& normal, decimal penetrationDepth,
const Vector3& localPoint1, const Vector3& localPoint2)
: ConstraintInfo(CONTACT), normal(normal), penetrationDepth(penetrationDepth),
localPoint1(localPoint1), localPoint2(localPoint2) {
}
};
// Class ContactPoint
/**
* This class represents a collision contact point between two
@ -84,7 +129,16 @@ class ContactPoint : public Constraint {
bool mIsRestingContact;
/// Two orthogonal vectors that span the tangential friction plane
std::vector<Vector3> mFrictionVectors;
Vector3 mFrictionVectors[2];
/// Cached penetration impulse
decimal mPenetrationImpulse;
/// Cached first friction impulse
decimal mFrictionImpulse1;
/// Cached second friction impulse
decimal mFrictionImpulse2;
// -------------------- Methods -------------------- //
@ -99,7 +153,7 @@ class ContactPoint : public Constraint {
// -------------------- Methods -------------------- //
/// Constructor
ContactPoint(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo);
ContactPoint(const ContactPointInfo& contactInfo);
/// Destructor
virtual ~ContactPoint();
@ -122,6 +176,24 @@ class ContactPoint : public Constraint {
/// Return the contact world point on body 2
Vector3 getWorldPointOnBody2() const;
/// Return the cached penetration impulse
decimal getPenetrationImpulse() const;
/// Return the cached first friction impulse
decimal getFrictionImpulse1() const;
/// Return the cached second friction impulse
decimal getFrictionImpulse2() const;
/// Set the cached penetration impulse
void setPenetrationImpulse(decimal impulse);
/// Set the first cached friction impulse
void setFrictionImpulse1(decimal impulse);
/// Set the second cached friction impulse
void setFrictionImpulse2(decimal impulse);
/// Set the contact world point on body 1
void setWorldPointOnBody1(const Vector3& worldPoint);
@ -149,6 +221,21 @@ class ContactPoint : public Constraint {
/// Return the penetration depth
decimal getPenetrationDepth() const;
/// Return the number of bytes used by the contact point
virtual size_t getSizeInBytes() const;
/// Initialize before solving the constraint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
/// Warm start the constraint (apply the previous impulse at the beginning of the step)
virtual void warmstart(const ConstraintSolverData& constraintSolverData);
/// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
/// Solve the position constraint
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
#ifdef VISUAL_DEBUG
/// Draw the contact (for debugging)
void draw() const;
@ -185,6 +272,36 @@ inline Vector3 ContactPoint::getWorldPointOnBody2() const {
return mWorldPointOnBody2;
}
// Return the cached penetration impulse
inline decimal ContactPoint::getPenetrationImpulse() const {
return mPenetrationImpulse;
}
// Return the cached first friction impulse
inline decimal ContactPoint::getFrictionImpulse1() const {
return mFrictionImpulse1;
}
// Return the cached second friction impulse
inline decimal ContactPoint::getFrictionImpulse2() const {
return mFrictionImpulse2;
}
// Set the cached penetration impulse
inline void ContactPoint::setPenetrationImpulse(decimal impulse) {
mPenetrationImpulse = impulse;
}
// Set the first cached friction impulse
inline void ContactPoint::setFrictionImpulse1(decimal impulse) {
mFrictionImpulse1 = impulse;
}
// Set the second cached friction impulse
inline void ContactPoint::setFrictionImpulse2(decimal impulse) {
mFrictionImpulse2 = impulse;
}
// Set the contact world point on body 1
inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) {
mWorldPointOnBody1 = worldPoint;
@ -230,6 +347,10 @@ inline decimal ContactPoint::getPenetrationDepth() const {
return mPenetrationDepth;
}
// Return the number of bytes used by the contact point
inline size_t ContactPoint::getSizeInBytes() const {
return sizeof(ContactPoint);
}
#ifdef VISUAL_DEBUG
inline void ContactPoint::draw() const {

397
src/constraint/FixedJoint.cpp Normal file
View File

@ -0,0 +1,397 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "FixedJoint.h"
#include "../engine/ConstraintSolver.h"
using namespace reactphysics3d;
// Static variables definition
const decimal FixedJoint::BETA = decimal(0.2);
// Constructor
FixedJoint::FixedJoint(const FixedJointInfo& jointInfo)
: Constraint(jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0, 0) {
// Compute the local-space anchor point for each body
const Transform& transform1 = mBody1->getTransform();
const Transform& transform2 = mBody2->getTransform();
mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
// Compute the inverse of the initial orientation difference between the two bodies
mInitOrientationDifferenceInv = transform2.getOrientation() *
transform1.getOrientation().getInverse();
mInitOrientationDifferenceInv.normalize();
mInitOrientationDifferenceInv.inverse();
}
// Destructor
FixedJoint::~FixedJoint() {
}
// Initialize before solving the constraint
void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the velocity array
mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->getTransform().getPosition();
const Vector3& x2 = mBody2->getTransform().getPosition();
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
// Get the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space
mR1World = orientationBody1 * mLocalAnchorPointBody1;
mR2World = orientationBody2 * mLocalAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = 0.0;
if (mBody1->getIsMotionEnabled()) {
inverseMassBodies += mBody1->getMassInverse();
}
if (mBody2->getIsMotionEnabled()) {
inverseMassBodies += mBody2->getMassInverse();
}
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies);
if (mBody1->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
}
if (mBody2->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
}
// Compute the inverse mass matrix K^-1 for the 3 translation constraints
mInverseMassMatrixTranslation.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixTranslation = massMatrix.getInverse();
}
// Compute the bias "b" of the constraint for the 3 translation constraints
decimal biasFactor = (BETA / constraintSolverData.timeStep);
mBiasTranslation.setToZero();
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBiasTranslation = biasFactor * (x2 + mR2World - x1 - mR1World);
}
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix)
mInverseMassMatrixRotation.setToZero();
if (mBody1->getIsMotionEnabled()) {
mInverseMassMatrixRotation += mI1;
}
if (mBody2->getIsMotionEnabled()) {
mInverseMassMatrixRotation += mI2;
}
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse();
}
// Compute the bias "b" for the 3 rotation constraints
mBiasRotation.setToZero();
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
mBiasRotation = biasFactor * decimal(2.0) * qError.getVectorV();
}
// If warm-starting is not enabled
if (!constraintSolverData.isWarmStartingActive) {
// Reset the accumulated impulses
mImpulseTranslation.setToZero();
mImpulseRotation.setToZero();
}
}
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass of the bodies
const decimal inverseMassBody1 = mBody1->getMassInverse();
const decimal inverseMassBody2 = mBody2->getMassInverse();
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 translation constraints
Vector3 linearImpulseBody1 = -mImpulseTranslation;
Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
angularImpulseBody1 += -mImpulseRotation;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 translation constraints
Vector3 linearImpulseBody2 = mImpulseTranslation;
Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
angularImpulseBody2 += mImpulseRotation;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
}
// Solve the velocity constraint
void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
// --------------- Translation Constraints --------------- //
// Compute J*v for the 3 translation constraints
const Vector3 JvTranslation = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World);
// Compute the Lagrange multiplier lambda
const Vector3 deltaLambda = mInverseMassMatrixTranslation *
(-JvTranslation - mBiasTranslation);
mImpulseTranslation += deltaLambda;
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody1 = -deltaLambda;
const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody2 = deltaLambda;
const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
// --------------- Rotation Constraints --------------- //
// Compute J*v for the 3 rotation constraints
const Vector3 JvRotation = w2 - w1;
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 deltaLambda2 = mInverseMassMatrixRotation * (-JvRotation - mBiasRotation);
mImpulseRotation += deltaLambda2;
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -deltaLambda2;
// Apply the impulse to the body
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody2 = deltaLambda2;
// Apply the impulse to the body
w2 += mI2 * angularImpulseBody2;
}
}
// Solve the position constraint (for position error correction)
void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
Vector3& x1 = constraintSolverData.positions[mIndexBody1];
Vector3& x2 = constraintSolverData.positions[mIndexBody2];
Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space
mR1World = q1 * mLocalAnchorPointBody1;
mR2World = q2 * mLocalAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = 0.0;
if (mBody1->getIsMotionEnabled()) {
inverseMassBodies += mBody1->getMassInverse();
}
if (mBody2->getIsMotionEnabled()) {
inverseMassBodies += mBody2->getMassInverse();
}
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies);
if (mBody1->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
}
if (mBody2->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
}
mInverseMassMatrixTranslation.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixTranslation = massMatrix.getInverse();
}
// Compute position error for the 3 translation constraints
const Vector3 errorTranslation = x2 + mR2World - x1 - mR1World;
// Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation);
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse
Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World);
// Compute the pseudo velocity
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse
Vector3 linearImpulseBody2 = lambdaTranslation;
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World);
// Compute the pseudo velocity
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// --------------- Rotation Constraints --------------- //
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix)
mInverseMassMatrixRotation.setToZero();
if (mBody1->getIsMotionEnabled()) {
mInverseMassMatrixRotation += mI1;
}
if (mBody2->getIsMotionEnabled()) {
mInverseMassMatrixRotation += mI2;
}
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse();
}
// Compute the position error for the 3 rotation constraints
Quaternion currentOrientationDifference = q2 * q1.getInverse();
currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
const Vector3 errorRotation = decimal(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation);
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -lambdaRotation;
// Compute the pseudo velocity
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse
const Vector3 angularImpulseBody2 = lambdaRotation;
// Compute the pseudo velocity
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
}

144
src/constraint/FixedJoint.h Normal file
View File

@ -0,0 +1,144 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 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
// Libraries
#include "Constraint.h"
#include "../mathematics/mathematics.h"
namespace reactphysics3d {
// Structure FixedJointInfo
/**
* This structure is used to gather the information needed to create a fixed
* joint. This structure will be used to create the actual fixed joint.
*/
struct FixedJointInfo : public ConstraintInfo {
public :
// -------------------- Attributes -------------------- //
/// Anchor point (in world-space coordinates)
Vector3 anchorPointWorldSpace;
/// Constructor
FixedJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace)
: ConstraintInfo(rigidBody1, rigidBody2, FIXEDJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace){}
};
// Class FixedJoint
/**
* This class represents a fixed joint that is used to forbid any translation or rotation
* between two bodies.
*/
class FixedJoint : public Constraint {
private :
// -------------------- Constants -------------------- //
// Beta value for the bias factor of position correction
static const decimal BETA;
// -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local-space coordinates of body 1)
Vector3 mLocalAnchorPointBody1;
/// Anchor point of body 2 (in local-space coordinates of body 2)
Vector3 mLocalAnchorPointBody2;
/// Vector from center of body 2 to anchor point in world-space
Vector3 mR1World;
/// Vector from center of body 2 to anchor point in world-space
Vector3 mR2World;
/// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 mI1;
/// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 mI2;
/// Accumulated impulse for the 3 translation constraints
Vector3 mImpulseTranslation;
/// Accumulate impulse for the 3 rotation constraints
Vector3 mImpulseRotation;
/// Inverse mass matrix K=JM^-1J^-t of the 3 translation constraints (3x3 matrix)
Matrix3x3 mInverseMassMatrixTranslation;
/// Inverse mass matrix K=JM^-1J^-t of the 3 rotation constraints (3x3 matrix)
Matrix3x3 mInverseMassMatrixRotation;
/// Bias vector for the 3 translation constraints
Vector3 mBiasTranslation;
/// Bias vector for the 3 rotation constraints
Vector3 mBiasRotation;
/// Inverse of the initial orientation difference between the two bodies
Quaternion mInitOrientationDifferenceInv;
public :
// -------------------- Methods -------------------- //
/// Constructor
FixedJoint(const FixedJointInfo& jointInfo);
/// Destructor
virtual ~FixedJoint();
/// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const;
/// Initialize before solving the constraint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
/// Warm start the constraint (apply the previous impulse at the beginning of the step)
virtual void warmstart(const ConstraintSolverData& constraintSolverData);
/// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
/// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
};
// Return the number of bytes used by the joint
inline size_t FixedJoint::getSizeInBytes() const {
return sizeof(FixedJoint);
}
}
#endif

881
src/constraint/HingeJoint.cpp Normal file
View File

@ -0,0 +1,881 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "HingeJoint.h"
#include "../engine/ConstraintSolver.h"
#include <cmath>
using namespace reactphysics3d;
// Static variables definition
const decimal HingeJoint::BETA = decimal(0.2);
// Constructor
HingeJoint::HingeJoint(const HingeJointInfo& jointInfo)
: Constraint(jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0),
mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0),
mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled),
mLowerLimit(jointInfo.minAngleLimit), mUpperLimit(jointInfo.maxAngleLimit),
mIsLowerLimitViolated(false), mIsUpperLimitViolated(false),
mMotorSpeed(jointInfo.motorSpeed), mMaxMotorTorque(jointInfo.maxMotorTorque) {
assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI);
assert(mUpperLimit >= 0 && mUpperLimit <= 2.0 * PI);
// Compute the local-space anchor point for each body
Transform transform1 = mBody1->getTransform();
Transform transform2 = mBody2->getTransform();
mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
// Compute the local-space hinge axis
mHingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld;
mHingeLocalAxisBody2 = transform2.getOrientation().getInverse() * jointInfo.rotationAxisWorld;
mHingeLocalAxisBody1.normalize();
mHingeLocalAxisBody2.normalize();
// Compute the inverse of the initial orientation difference between the two bodies
mInitOrientationDifferenceInv = transform2.getOrientation() *
transform1.getOrientation().getInverse();
mInitOrientationDifferenceInv.normalize();
mInitOrientationDifferenceInv.inverse();
}
// Destructor
HingeJoint::~HingeJoint() {
}
// Initialize before solving the constraint
void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the velocity array
mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->getTransform().getPosition();
const Vector3& x2 = mBody2->getTransform().getPosition();
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
// Get the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space
mR1World = orientationBody1 * mLocalAnchorPointBody1;
mR2World = orientationBody2 * mLocalAnchorPointBody2;
// Compute the current angle around the hinge axis
decimal hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2);
// Check if the limit constraints are violated or not
decimal lowerLimitError = hingeAngle - mLowerLimit;
decimal upperLimitError = mUpperLimit - hingeAngle;
bool oldIsLowerLimitViolated = mIsLowerLimitViolated;
mIsLowerLimitViolated = lowerLimitError <= 0;
if (mIsLowerLimitViolated != oldIsLowerLimitViolated) {
mImpulseLowerLimit = 0.0;
}
bool oldIsUpperLimitViolated = mIsUpperLimitViolated;
mIsUpperLimitViolated = upperLimitError <= 0;
if (mIsUpperLimitViolated != oldIsUpperLimitViolated) {
mImpulseUpperLimit = 0.0;
}
// Compute vectors needed in the Jacobian
mA1 = orientationBody1 * mHingeLocalAxisBody1;
Vector3 a2 = orientationBody2 * mHingeLocalAxisBody2;
mA1.normalize();
a2.normalize();
const Vector3 b2 = a2.getOneUnitOrthogonalVector();
const Vector3 c2 = a2.cross(b2);
mB2CrossA1 = b2.cross(mA1);
mC2CrossA1 = c2.cross(mA1);
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix)
decimal inverseMassBodies = 0.0;
if (mBody1->getIsMotionEnabled()) {
inverseMassBodies += mBody1->getMassInverse();
}
if (mBody2->getIsMotionEnabled()) {
inverseMassBodies += mBody2->getMassInverse();
}
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies);
if (mBody1->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
}
if (mBody2->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
}
mInverseMassMatrixTranslation.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixTranslation = massMatrix.getInverse();
}
// Compute the bias "b" of the translation constraints
mBTranslation.setToZero();
decimal biasFactor = (BETA / constraintSolverData.timeStep);
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBTranslation = biasFactor * (x2 + mR2World - x1 - mR1World);
}
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
Vector3 I1B2CrossA1(0, 0, 0);
Vector3 I1C2CrossA1(0, 0, 0);
Vector3 I2B2CrossA1(0, 0, 0);
Vector3 I2C2CrossA1(0, 0, 0);
if (mBody1->getIsMotionEnabled()) {
I1B2CrossA1 = mI1 * mB2CrossA1;
I1C2CrossA1 = mI1 * mC2CrossA1;
}
if (mBody2->getIsMotionEnabled()) {
I2B2CrossA1 = mI2 * mB2CrossA1;
I2C2CrossA1 = mI2 * mC2CrossA1;
}
const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) +
mB2CrossA1.dot(I2B2CrossA1);
const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) +
mB2CrossA1.dot(I2C2CrossA1);
const decimal el21 = mC2CrossA1.dot(I1B2CrossA1) +
mC2CrossA1.dot(I2B2CrossA1);
const decimal el22 = mC2CrossA1.dot(I1C2CrossA1) +
mC2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mInverseMassMatrixRotation.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixRotation = matrixKRotation.getInverse();
}
// Compute the bias "b" of the rotation constraints
mBRotation.setToZero();
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBRotation = biasFactor * Vector2(mA1.dot(b2), mA1.dot(c2));
}
// If warm-starting is not enabled
if (!constraintSolverData.isWarmStartingActive) {
// Reset all the accumulated impulses
mImpulseTranslation.setToZero();
mImpulseRotation.setToZero();
mImpulseLowerLimit = 0.0;
mImpulseUpperLimit = 0.0;
mImpulseMotor = 0.0;
}
if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mInverseMassMatrixLimitMotor = 0.0;
if (mBody1->getIsMotionEnabled()) {
mInverseMassMatrixLimitMotor += mA1.dot(mI1 * mA1);
}
if (mBody2->getIsMotionEnabled()) {
mInverseMassMatrixLimitMotor += mA1.dot(mI2 * mA1);
}
mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0);
// Compute the bias "b" of the lower limit constraint
mBLowerLimit = 0.0;
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBLowerLimit = biasFactor * lowerLimitError;
}
// Compute the bias "b" of the upper limit constraint
mBUpperLimit = 0.0;
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBUpperLimit = biasFactor * upperLimitError;
}
}
}
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = mBody1->getMassInverse();
const decimal inverseMassBody2 = mBody2->getMassInverse();
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
const Vector3 limitsImpulse = (mImpulseUpperLimit - mImpulseLowerLimit) * mA1;
// Compute the impulse P=J^T * lambda for the motor constraint
const Vector3 motorImpulse = -mImpulseMotor * mA1;
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 translation constraints
Vector3 linearImpulseBody1 = -mImpulseTranslation;
Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World);
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
angularImpulseBody1 += rotationImpulse;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
angularImpulseBody1 += limitsImpulse;
// Compute the impulse P=J^T * lambda for the motor constraint
angularImpulseBody1 += motorImpulse;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 translation constraints
Vector3 linearImpulseBody2 = mImpulseTranslation;
Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World);
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
angularImpulseBody2 += -rotationImpulse;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
angularImpulseBody2 += -limitsImpulse;
// Compute the impulse P=J^T * lambda for the motor constraint
angularImpulseBody2 += -motorImpulse;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
}
// Solve the velocity constraint
void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
// --------------- Translation Constraints --------------- //
// Compute J*v
const Vector3 JvTranslation = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World);
// Compute the Lagrange multiplier lambda
const Vector3 deltaLambdaTranslation = mInverseMassMatrixTranslation *
(-JvTranslation - mBTranslation);
mImpulseTranslation += deltaLambdaTranslation;
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody1 = -deltaLambdaTranslation;
const Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(mR1World);
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody2 = deltaLambdaTranslation;
const Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(mR2World);
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
// --------------- Rotation Constraints --------------- //
// Compute J*v for the 2 rotation constraints
const Vector2 JvRotation(-mB2CrossA1.dot(w1) + mB2CrossA1.dot(w2),
-mC2CrossA1.dot(w1) + mC2CrossA1.dot(w2));
// Compute the Lagrange multiplier lambda for the 2 rotation constraints
Vector2 deltaLambdaRotation = mInverseMassMatrixRotation * (-JvRotation - mBRotation);
mImpulseRotation += deltaLambdaRotation;
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
const Vector3 angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x -
mC2CrossA1 * deltaLambdaRotation.y;
// Apply the impulse to the body
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
const Vector3 angularImpulseBody2 = mB2CrossA1 * deltaLambdaRotation.x +
mC2CrossA1 * deltaLambdaRotation.y;
// Apply the impulse to the body
w2 += mI2 * angularImpulseBody2;
}
// --------------- Limits Constraints --------------- //
if (mIsLimitEnabled) {
// If the lower limit is violated
if (mIsLowerLimitViolated) {
// Compute J*v for the lower limit constraint
const decimal 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));
deltaLambdaLower = mImpulseLowerLimit - lambdaTemp;
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1;
// Apply the impulse to the body
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 angularImpulseBody2 = deltaLambdaLower * mA1;
// Apply the impulse to the body
w2 += mI2 * angularImpulseBody2;
}
}
// If the upper limit is violated
if (mIsUpperLimitViolated) {
// Compute J*v for the upper limit constraint
const decimal 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));
deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp;
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1;
// Apply the impulse to the body
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mA1;
// Apply the impulse to the body
w2 += mI2 * angularImpulseBody2;
}
}
}
// --------------- Motor --------------- //
if (mIsMotorEnabled) {
// Compute J*v for the motor
const decimal 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;
mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
deltaLambdaMotor = mImpulseMotor - lambdaTemp;
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the motor
const Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1;
// Apply the impulse to the body
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the motor
const Vector3 angularImpulseBody2 = deltaLambdaMotor * mA1;
// Apply the impulse to the body
w2 += mI2 * angularImpulseBody2;
}
}
}
// Solve the position constraint (for position error correction)
void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
Vector3& x1 = constraintSolverData.positions[mIndexBody1];
Vector3& x2 = constraintSolverData.positions[mIndexBody2];
Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space
mR1World = q1 * mLocalAnchorPointBody1;
mR2World = q2 * mLocalAnchorPointBody2;
// Compute the current angle around the hinge axis
decimal hingeAngle = computeCurrentHingeAngle(q1, q2);
// Check if the limit constraints are violated or not
decimal lowerLimitError = hingeAngle - mLowerLimit;
decimal upperLimitError = mUpperLimit - hingeAngle;
mIsLowerLimitViolated = lowerLimitError <= 0;
mIsUpperLimitViolated = upperLimitError <= 0;
// Compute vectors needed in the Jacobian
mA1 = q1 * mHingeLocalAxisBody1;
Vector3 a2 = q2 * mHingeLocalAxisBody2;
mA1.normalize();
a2.normalize();
const Vector3 b2 = a2.getOneUnitOrthogonalVector();
const Vector3 c2 = a2.cross(b2);
mB2CrossA1 = b2.cross(mA1);
mC2CrossA1 = c2.cross(mA1);
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = 0.0;
if (mBody1->getIsMotionEnabled()) {
inverseMassBodies += mBody1->getMassInverse();
}
if (mBody2->getIsMotionEnabled()) {
inverseMassBodies += mBody2->getMassInverse();
}
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies);
if (mBody1->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
}
if (mBody2->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
}
mInverseMassMatrixTranslation.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixTranslation = massMatrix.getInverse();
}
// Compute position error for the 3 translation constraints
const Vector3 errorTranslation = x2 + mR2World - x1 - mR1World;
// Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation);
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse
Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World);
// Compute the pseudo velocity
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse
Vector3 linearImpulseBody2 = lambdaTranslation;
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World);
// Compute the pseudo velocity
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// --------------- Rotation Constraints --------------- //
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
Vector3 I1B2CrossA1(0, 0, 0);
Vector3 I1C2CrossA1(0, 0, 0);
Vector3 I2B2CrossA1(0, 0, 0);
Vector3 I2C2CrossA1(0, 0, 0);
if (mBody1->getIsMotionEnabled()) {
I1B2CrossA1 = mI1 * mB2CrossA1;
I1C2CrossA1 = mI1 * mC2CrossA1;
}
if (mBody2->getIsMotionEnabled()) {
I2B2CrossA1 = mI2 * mB2CrossA1;
I2C2CrossA1 = mI2 * mC2CrossA1;
}
const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) +
mB2CrossA1.dot(I2B2CrossA1);
const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) +
mB2CrossA1.dot(I2C2CrossA1);
const decimal el21 = mC2CrossA1.dot(I1B2CrossA1) +
mC2CrossA1.dot(I2B2CrossA1);
const decimal el22 = mC2CrossA1.dot(I1C2CrossA1) +
mC2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mInverseMassMatrixRotation.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixRotation = matrixKRotation.getInverse();
}
// Compute the position error for the 3 rotation constraints
const Vector2 errorRotation = Vector2(mA1.dot(b2), mA1.dot(c2));
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector2 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation);
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -mB2CrossA1 * lambdaRotation.x -
mC2CrossA1 * lambdaRotation.y;
// Compute the pseudo velocity
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse
const Vector3 angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x +
mC2CrossA1 * lambdaRotation.y;
// Compute the pseudo velocity
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// --------------- Limits Constraints --------------- //
if (mIsLimitEnabled) {
if (mIsLowerLimitViolated || mIsUpperLimitViolated) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mInverseMassMatrixLimitMotor = 0.0;
if (mBody1->getIsMotionEnabled()) {
mInverseMassMatrixLimitMotor += mA1.dot(mI1 * mA1);
}
if (mBody2->getIsMotionEnabled()) {
mInverseMassMatrixLimitMotor += mA1.dot(mI2 * mA1);
}
mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0);
}
// If the lower limit is violated
if (mIsLowerLimitViolated) {
// Compute the Lagrange multiplier lambda for the lower limit constraint
decimal lambdaLowerLimit = mInverseMassMatrixLimitMotor * (-lowerLimitError );
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1;
// Compute the pseudo velocity
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 angularImpulseBody2 = lambdaLowerLimit * mA1;
// Compute the pseudo velocity
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
}
// If the upper limit is violated
if (mIsUpperLimitViolated) {
// Compute the Lagrange multiplier lambda for the upper limit constraint
decimal lambdaUpperLimit = mInverseMassMatrixLimitMotor * (-upperLimitError);
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1;
// Compute the pseudo velocity
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mA1;
// Compute the pseudo velocity
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
}
}
}
// Enable/Disable the limits of the joint
void HingeJoint::enableLimit(bool isLimitEnabled) {
if (isLimitEnabled != mIsLimitEnabled) {
mIsLimitEnabled = isLimitEnabled;
// Reset the limits
resetLimits();
}
}
// Enable/Disable the motor of the joint
void HingeJoint::enableMotor(bool isMotorEnabled) {
mIsMotorEnabled = isMotorEnabled;
mImpulseMotor = 0.0;
// TODO : Wake up the bodies of the joint here when sleeping is implemented
}
// Set the minimum angle limit
void HingeJoint::setMinAngleLimit(decimal lowerLimit) {
assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI);
if (lowerLimit != mLowerLimit) {
mLowerLimit = lowerLimit;
// Reset the limits
resetLimits();
}
}
// Set the maximum angle limit
void HingeJoint::setMaxAngleLimit(decimal upperLimit) {
assert(upperLimit >= 0 && upperLimit <= 2.0 * PI);
if (upperLimit != mUpperLimit) {
mUpperLimit = upperLimit;
// Reset the limits
resetLimits();
}
}
// Reset the limits
void HingeJoint::resetLimits() {
// Reset the accumulated impulses for the limits
mImpulseLowerLimit = 0.0;
mImpulseUpperLimit = 0.0;
// TODO : Wake up the bodies of the joint here when sleeping is implemented
}
// Set the motor speed
void HingeJoint::setMotorSpeed(decimal motorSpeed) {
if (motorSpeed != mMotorSpeed) {
mMotorSpeed = motorSpeed;
// TODO : Wake up the bodies of the joint here when sleeping is implemented
}
}
// Set the maximum motor torque
void HingeJoint::setMaxMotorTorque(decimal maxMotorTorque) {
if (maxMotorTorque != mMaxMotorTorque) {
assert(mMaxMotorTorque >= 0.0);
mMaxMotorTorque = maxMotorTorque;
// TODO : Wake up the bodies of the joint here when sleeping is implemented
}
}
// Given an angle in radian, this method returns the corresponding angle in the range [-pi; pi]
decimal HingeJoint::computeNormalizedAngle(decimal angle) const {
// Convert it into the range [-2*pi; 2*pi]
angle = fmod(angle, PI_TIMES_2);
// Convert it into the range [-pi; pi]
if (angle < -PI) {
return angle + PI_TIMES_2;
}
else if (angle > PI) {
return angle - PI_TIMES_2;
}
else {
return angle;
}
}
// 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 {
if (upperLimitAngle <= lowerLimitAngle) {
return inputAngle;
}
else if (inputAngle > upperLimitAngle) {
decimal diffToUpperLimit = fabs(computeNormalizedAngle(inputAngle - upperLimitAngle));
decimal 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));
return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + PI_TIMES_2);
}
else {
return inputAngle;
}
}
// Compute the current angle around the hinge axis
decimal HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1,
const Quaternion& orientationBody2) {
decimal hingeAngle;
// Compute the current orientation difference between the two bodies
Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse();
currentOrientationDiff.normalize();
// Compute the relative rotation considering the initial orientation difference
Quaternion relativeRotation = currentOrientationDiff * mInitOrientationDifferenceInv;
relativeRotation.normalize();
// A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit
// length vector. We can extract cos(theta/2) with q.w and we can extract |sin(theta/2)| with :
// |sin(theta/2)| = q.getVectorV().length() since rotAxis is unit length. Note that any
// rotation can be represented by a quaternion q and -q. Therefore, if the relative rotation
// 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();
// Compute the dot product of the relative rotation axis and the hinge axis
decimal 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);
}
else {
hingeAngle = decimal(2.0) * std::atan2(sinHalfAngleAbs, -cosHalfAngle);
}
// Convert the angle from range [-2*pi; 2*pi] into the range [-pi; pi]
hingeAngle = computeNormalizedAngle(hingeAngle);
// Compute and return the corresponding angle near one the two limits
return computeCorrespondingAngleNearLimits(hingeAngle, mLowerLimit, mUpperLimit);
}

351
src/constraint/HingeJoint.h Normal file
View File

@ -0,0 +1,351 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 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
// Libraries
#include "Constraint.h"
#include "../mathematics/mathematics.h"
namespace reactphysics3d {
// Structure HingeJointInfo
/**
* This structure is used to gather the information needed to create a hinge joint.
* This structure will be used to create the actual hinge joint.
*/
struct HingeJointInfo : public ConstraintInfo {
public :
// -------------------- Attributes -------------------- //
/// Anchor point (in world-space coordinates)
Vector3 anchorPointWorldSpace;
/// Hinge rotation axis (in world-space coordinates)
Vector3 rotationAxisWorld;
/// True if the slider limits are enabled
bool isLimitEnabled;
/// True if the slider motor is enabled
bool isMotorEnabled;
/// Minimum allowed rotation angle (in radian) if limits are enabled.
/// The angle must be in the range [-2*pi, 0]
decimal minAngleLimit;
/// Maximum allowed rotation angle (in radian) if limits are enabled.
/// The angle must be in the range [0, 2*pi]
decimal maxAngleLimit;
/// Motor speed (in radian/second)
decimal motorSpeed;
/// Maximum motor torque (in Newtons * meters) that can be applied to reach
/// to desired motor speed
decimal maxMotorTorque;
/// Constructor without limits and without motor
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace,
const Vector3& initRotationAxisWorld)
: ConstraintInfo(rigidBody1, rigidBody2, HINGEJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(false),
isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1),
motorSpeed(0), maxMotorTorque(0) {}
/// Constructor with limits but without motor
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace,
const Vector3& initRotationAxisWorld,
decimal initMinAngleLimit, decimal initMaxAngleLimit)
: ConstraintInfo(rigidBody1, rigidBody2, HINGEJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true),
isMotorEnabled(false), minAngleLimit(initMinAngleLimit),
maxAngleLimit(initMaxAngleLimit), motorSpeed(0),
maxMotorTorque(0) {}
/// Constructor with limits and motor
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace,
const Vector3& initRotationAxisWorld,
decimal initMinAngleLimit, decimal initMaxAngleLimit,
decimal initMotorSpeed, decimal initMaxMotorTorque)
: ConstraintInfo(rigidBody1, rigidBody2, HINGEJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true),
isMotorEnabled(false), minAngleLimit(initMinAngleLimit),
maxAngleLimit(initMaxAngleLimit), motorSpeed(initMotorSpeed),
maxMotorTorque(initMaxMotorTorque) {}
};
// Class HingeJoint
/**
* This class represents a hinge joint that allows arbitrary rotation
* between two bodies around a single axis.
*/
class HingeJoint : public Constraint {
private :
// -------------------- Constants -------------------- //
// Beta value for the bias factor of position correction
static const decimal BETA;
// -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local-space coordinates of body 1)
Vector3 mLocalAnchorPointBody1;
/// Anchor point of body 2 (in local-space coordinates of body 2)
Vector3 mLocalAnchorPointBody2;
/// Hinge rotation axis (in local-space coordinates of body 1)
Vector3 mHingeLocalAxisBody1;
/// Hinge rotation axis (in local-space coordiantes of body 2)
Vector3 mHingeLocalAxisBody2;
/// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 mI1;
/// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 mI2;
/// Hinge rotation axis (in world-space coordinates) computed from body 1
Vector3 mA1;
/// Vector from center of body 2 to anchor point in world-space
Vector3 mR1World;
/// Vector from center of body 2 to anchor point in world-space
Vector3 mR2World;
/// Cross product of vector b2 and a1
Vector3 mB2CrossA1;
/// Cross product of vector c2 and a1;
Vector3 mC2CrossA1;
/// Impulse for the 3 translation constraints
Vector3 mImpulseTranslation;
/// Impulse for the 2 rotation constraints
Vector2 mImpulseRotation;
/// Accumulated impulse for the lower limit constraint
decimal mImpulseLowerLimit;
/// Accumulated impulse for the upper limit constraint
decimal mImpulseUpperLimit;
/// Accumulated impulse for the motor constraint;
decimal mImpulseMotor;
/// Inverse mass matrix K=JM^-1J^t for the 3 translation constraints
Matrix3x3 mInverseMassMatrixTranslation;
/// Inverse mass matrix K=JM^-1J^t for the 2 rotation constraints
Matrix2x2 mInverseMassMatrixRotation;
/// Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix)
decimal mInverseMassMatrixLimitMotor;
/// Inverse of mass matrix K=JM^-1J^t for the motor
decimal mInverseMassMatrixMotor;
/// Bias vector for the error correction for the translation constraints
Vector3 mBTranslation;
/// Bias vector for the error correction for the rotation constraints
Vector2 mBRotation;
/// Bias of the lower limit constraint
decimal mBLowerLimit;
/// Bias of the upper limit constraint
decimal mBUpperLimit;
/// Inverse of the initial orientation difference between the bodies
Quaternion mInitOrientationDifferenceInv;
/// True if the joint limits are enabled
bool mIsLimitEnabled;
/// True if the motor of the joint in enabled
bool mIsMotorEnabled;
/// Lower limit (minimum allowed rotation angle in radi)
decimal mLowerLimit;
/// Upper limit (maximum translation distance)
decimal mUpperLimit;
/// True if the lower limit is violated
bool mIsLowerLimitViolated;
/// True if the upper limit is violated
bool mIsUpperLimitViolated;
/// Motor speed
decimal mMotorSpeed;
/// Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed
decimal mMaxMotorTorque;
// -------------------- Methods -------------------- //
/// Reset the limits
void resetLimits();
/// Given an angle in radian, this method returns the corresponding
/// angle in the range [-pi; pi]
decimal 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 computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle,
decimal upperLimitAngle) const;
/// Compute the current angle around the hinge axis
decimal computeCurrentHingeAngle(const Quaternion& orientationBody1,
const Quaternion& orientationBody2);
public :
// -------------------- Methods -------------------- //
/// Constructor
HingeJoint(const HingeJointInfo& jointInfo);
/// Destructor
virtual ~HingeJoint();
/// Return true if the limits or the joint are enabled
bool isLimitEnabled() const;
/// Return true if the motor of the joint is enabled
bool isMotorEnabled() const;
/// Enable/Disable the limits of the joint
void enableLimit(bool isLimitEnabled);
/// Enable/Disable the motor of the joint
void enableMotor(bool isMotorEnabled);
/// Return the minimum angle limit
decimal getMinAngleLimit() const;
/// Set the minimum angle limit
void setMinAngleLimit(decimal lowerLimit);
/// Return the maximum angle limit
decimal getMaxAngleLimit() const;
/// Set the maximum angle limit
void setMaxAngleLimit(decimal upperLimit);
/// Return the motor speed
decimal getMotorSpeed() const;
/// Set the motor speed
void setMotorSpeed(decimal motorSpeed);
/// Return the maximum motor torque
decimal getMaxMotorTorque() const;
/// Set the maximum motor torque
void setMaxMotorTorque(decimal maxMotorTorque);
/// Return the intensity of the current torque applied for the joint motor
decimal getMotorTorque(decimal timeStep) const;
/// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const;
/// Initialize before solving the constraint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
/// Warm start the constraint (apply the previous impulse at the beginning of the step)
virtual void warmstart(const ConstraintSolverData& constraintSolverData);
/// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
/// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
};
// Return true if the limits or the joint are enabled
inline bool HingeJoint::isLimitEnabled() const {
return mIsLimitEnabled;
}
// Return true if the motor of the joint is enabled
inline bool HingeJoint::isMotorEnabled() const {
return mIsMotorEnabled;
}
// Return the minimum angle limit
inline decimal HingeJoint::getMinAngleLimit() const {
return mLowerLimit;
}
// Return the maximum angle limit
inline decimal HingeJoint::getMaxAngleLimit() const {
return mUpperLimit;
}
// Return the motor speed
inline decimal HingeJoint::getMotorSpeed() const {
return mMotorSpeed;
}
// Return the maximum motor torque
inline decimal HingeJoint::getMaxMotorTorque() const {
return mMaxMotorTorque;
}
// Return the intensity of the current torque applied for the joint motor
inline decimal HingeJoint::getMotorTorque(decimal timeStep) const {
return mImpulseMotor / timeStep;
}
// Return the number of bytes used by the joint
inline size_t HingeJoint::getSizeInBytes() const {
return sizeof(HingeJoint);
}
}
#endif

857
src/constraint/SliderJoint.cpp Normal file
View File

@ -0,0 +1,857 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "SliderJoint.h"
using namespace reactphysics3d;
// Static variables definition
const decimal SliderJoint::BETA = decimal(0.2);
// Constructor
SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
: Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0),
mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0),
mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled),
mLowerLimit(jointInfo.minTranslationLimit),
mUpperLimit(jointInfo.maxTranslationLimit), mIsLowerLimitViolated(false),
mIsUpperLimitViolated(false), mMotorSpeed(jointInfo.motorSpeed),
mMaxMotorForce(jointInfo.maxMotorForce){
assert(mUpperLimit >= 0.0);
assert(mLowerLimit <= 0.0);
assert(mMaxMotorForce >= 0.0);
// Compute the local-space anchor point for each body
const Transform& transform1 = mBody1->getTransform();
const Transform& transform2 = mBody2->getTransform();
mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
// Compute the inverse of the initial orientation difference between the two bodies
mInitOrientationDifferenceInv = transform2.getOrientation() *
transform1.getOrientation().getInverse();
mInitOrientationDifferenceInv.normalize();
mInitOrientationDifferenceInv.inverse();
// Compute the slider axis in local-space of body 1
mSliderAxisBody1 = mBody1->getTransform().getOrientation().getInverse() *
jointInfo.sliderAxisWorldSpace;
mSliderAxisBody1.normalize();
}
// Destructor
SliderJoint::~SliderJoint() {
}
// Initialize before solving the constraint
void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the veloc ity array
mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->getTransform().getPosition();
const Vector3& x2 = mBody2->getTransform().getPosition();
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
// Get the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
// Vector from body center to the anchor point
mR1 = orientationBody1 * mLocalAnchorPointBody1;
mR2 = orientationBody2 * mLocalAnchorPointBody2;
// Compute the vector u (difference between anchor points)
const Vector3 u = x2 + mR2 - x1 - mR1;
// Compute the two orthogonal vectors to the slider axis in world-space
mSliderAxisWorld = orientationBody1 * mSliderAxisBody1;
mSliderAxisWorld.normalize();
mN1 = mSliderAxisWorld.getOneUnitOrthogonalVector();
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;
bool oldIsLowerLimitViolated = mIsLowerLimitViolated;
mIsLowerLimitViolated = lowerLimitError <= 0;
if (mIsLowerLimitViolated != oldIsLowerLimitViolated) {
mImpulseLowerLimit = 0.0;
}
bool oldIsUpperLimitViolated = mIsUpperLimitViolated;
mIsUpperLimitViolated = upperLimitError <= 0;
if (mIsUpperLimitViolated != oldIsUpperLimitViolated) {
mImpulseUpperLimit = 0.0;
}
// Compute the cross products used in the Jacobians
mR2CrossN1 = mR2.cross(mN1);
mR2CrossN2 = mR2.cross(mN2);
mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld);
const Vector3 r1PlusU = mR1 + u;
mR1PlusUCrossN1 = (r1PlusU).cross(mN1);
mR1PlusUCrossN2 = (r1PlusU).cross(mN2);
mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld);
// Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
// constraints (2x2 matrix)
decimal sumInverseMass = 0.0;
Vector3 I1R1PlusUCrossN1(0, 0, 0);
Vector3 I1R1PlusUCrossN2(0, 0, 0);
Vector3 I2R2CrossN1(0, 0, 0);
Vector3 I2R2CrossN2(0, 0, 0);
if (mBody1->getIsMotionEnabled()) {
sumInverseMass += mBody1->getMassInverse();
I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
}
if (mBody2->getIsMotionEnabled()) {
sumInverseMass += mBody2->getMassInverse();
I2R2CrossN1 = mI2 * mR2CrossN1;
I2R2CrossN2 = mI2 * mR2CrossN2;
}
const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1);
const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
mR2CrossN1.dot(I2R2CrossN2);
const decimal el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
mR2CrossN2.dot(I2R2CrossN1);
const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
mR2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
mInverseMassMatrixTranslationConstraint.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
}
// Compute the bias "b" of the translation constraint
mBTranslation.setToZero();
decimal biasFactor = (BETA / constraintSolverData.timeStep);
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBTranslation.x = u.dot(mN1);
mBTranslation.y = u.dot(mN2);
mBTranslation *= biasFactor;
}
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix)
mInverseMassMatrixRotationConstraint.setToZero();
if (mBody1->getIsMotionEnabled()) {
mInverseMassMatrixRotationConstraint += mI1;
}
if (mBody2->getIsMotionEnabled()) {
mInverseMassMatrixRotationConstraint += mI2;
}
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse();
}
// Compute the bias "b" of the rotation constraint
mBRotation.setToZero();
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
mBRotation = biasFactor * decimal(2.0) * qError.getVectorV();
}
if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mInverseMassMatrixLimit = 0.0;
if (mBody1->getIsMotionEnabled()) {
mInverseMassMatrixLimit += mBody1->getMassInverse() +
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis);
}
if (mBody2->getIsMotionEnabled()) {
mInverseMassMatrixLimit += mBody2->getMassInverse() +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
}
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0);
// Compute the bias "b" of the lower limit constraint
mBLowerLimit = 0.0;
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBLowerLimit = biasFactor * lowerLimitError;
}
// Compute the bias "b" of the upper limit constraint
mBUpperLimit = 0.0;
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBUpperLimit = biasFactor * upperLimitError;
}
}
// Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix)
mInverseMassMatrixMotor = 0.0;
if (mBody1->getIsMotionEnabled()) {
mInverseMassMatrixMotor += mBody1->getMassInverse();
}
if (mBody2->getIsMotionEnabled()) {
mInverseMassMatrixMotor += mBody2->getMassInverse();
}
mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ?
decimal(1.0) / mInverseMassMatrixMotor : decimal(0.0);
// If warm-starting is not enabled
if (!constraintSolverData.isWarmStartingActive) {
// Reset all the accumulated impulses
mImpulseTranslation.setToZero();
mImpulseRotation.setToZero();
mImpulseLowerLimit = 0.0;
mImpulseUpperLimit = 0.0;
mImpulseMotor = 0.0;
}
}
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = mBody1->getMassInverse();
const decimal inverseMassBody2 = mBody2->getMassInverse();
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
decimal impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit;
Vector3 linearImpulseLimits = impulseLimits * mSliderAxisWorld;
// Compute the impulse P=J^T * lambda for the motor constraint
Vector3 impulseMotor = mImpulseMotor * mSliderAxisWorld;
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints
Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - mN2 * mImpulseTranslation.y;
Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * mImpulseTranslation.x -
mR1PlusUCrossN2 * mImpulseTranslation.y;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
angularImpulseBody1 += -mImpulseRotation;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
linearImpulseBody1 += linearImpulseLimits;
angularImpulseBody1 += impulseLimits * mR1PlusUCrossSliderAxis;
// Compute the impulse P=J^T * lambda for the motor constraint
linearImpulseBody1 += impulseMotor;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints
Vector3 linearImpulseBody2 = mN1 * mImpulseTranslation.x + mN2 * mImpulseTranslation.y;
Vector3 angularImpulseBody2 = mR2CrossN1 * mImpulseTranslation.x +
mR2CrossN2 * mImpulseTranslation.y;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
angularImpulseBody2 += mImpulseRotation;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
linearImpulseBody2 += -linearImpulseLimits;
angularImpulseBody2 += -impulseLimits * mR2CrossSliderAxis;
// Compute the impulse P=J^T * lambda for the motor constraint
linearImpulseBody2 += -impulseMotor;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
}
// Solve the velocity constraint
void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
// --------------- Translation Constraints --------------- //
// Compute J*v for the 2 translation constraints
const decimal el1 = -mN1.dot(v1) - w1.dot(mR1PlusUCrossN1) +
mN1.dot(v2) + w2.dot(mR2CrossN1);
const decimal el2 = -mN2.dot(v1) - w1.dot(mR1PlusUCrossN2) +
mN2.dot(v2) + w2.dot(mR2CrossN2);
const Vector2 JvTranslation(el1, el2);
// Compute the Lagrange multiplier lambda for the 2 translation constraints
Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation);
mImpulseTranslation += deltaLambda;
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints
const Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y;
const Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x -
mR1PlusUCrossN2 * deltaLambda.y;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints
const Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y;
const Vector3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x + mR2CrossN2 * deltaLambda.y;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
// --------------- Rotation Constraints --------------- //
// Compute J*v for the 3 rotation constraints
const Vector3 JvRotation = w2 - w1;
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - mBRotation);
mImpulseRotation += deltaLambda2;
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -deltaLambda2;
// Apply the impulse to the body
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody2 = deltaLambda2;
// Apply the impulse to the body
w2 += mI2 * angularImpulseBody2;
}
// --------------- Limits Constraints --------------- //
if (mIsLimitEnabled) {
// If the lower limit is violated
if (mIsLowerLimitViolated) {
// Compute J*v for the lower limit constraint
const decimal 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));
deltaLambdaLower = mImpulseLowerLimit - lambdaTemp;
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
}
// If the upper limit is violated
if (mIsUpperLimitViolated) {
// Compute J*v for the upper limit constraint
const decimal 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));
deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp;
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = deltaLambdaUpper * mR1PlusUCrossSliderAxis;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mR2CrossSliderAxis;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
}
}
// --------------- Motor --------------- //
if (mIsMotorEnabled) {
// Compute J*v for the motor
const decimal 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;
mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
deltaLambdaMotor = mImpulseMotor - lambdaTemp;
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the motor
const Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the motor
const Vector3 linearImpulseBody2 = -deltaLambdaMotor * mSliderAxisWorld;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
}
}
}
// Solve the position constraint (for position error correction)
void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
Vector3& x1 = constraintSolverData.positions[mIndexBody1];
Vector3& x2 = constraintSolverData.positions[mIndexBody2];
Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
// Recompute the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
// Vector from body center to the anchor point
mR1 = q1 * mLocalAnchorPointBody1;
mR2 = q2 * mLocalAnchorPointBody2;
// Compute the vector u (difference between anchor points)
const Vector3 u = x2 + mR2 - x1 - mR1;
// Compute the two orthogonal vectors to the slider axis in world-space
mSliderAxisWorld = q1 * mSliderAxisBody1;
mSliderAxisWorld.normalize();
mN1 = mSliderAxisWorld.getOneUnitOrthogonalVector();
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;
mIsLowerLimitViolated = lowerLimitError <= 0;
mIsUpperLimitViolated = upperLimitError <= 0;
// Compute the cross products used in the Jacobians
mR2CrossN1 = mR2.cross(mN1);
mR2CrossN2 = mR2.cross(mN2);
mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld);
const Vector3 r1PlusU = mR1 + u;
mR1PlusUCrossN1 = (r1PlusU).cross(mN1);
mR1PlusUCrossN2 = (r1PlusU).cross(mN2);
mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld);
// --------------- Translation Constraints --------------- //
// Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
// constraints (2x2 matrix)
decimal sumInverseMass = 0.0;
Vector3 I1R1PlusUCrossN1(0, 0, 0);
Vector3 I1R1PlusUCrossN2(0, 0, 0);
Vector3 I2R2CrossN1(0, 0, 0);
Vector3 I2R2CrossN2(0, 0, 0);
if (mBody1->getIsMotionEnabled()) {
sumInverseMass += mBody1->getMassInverse();
I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
}
if (mBody2->getIsMotionEnabled()) {
sumInverseMass += mBody2->getMassInverse();
I2R2CrossN1 = mI2 * mR2CrossN1;
I2R2CrossN2 = mI2 * mR2CrossN2;
}
const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1);
const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
mR2CrossN1.dot(I2R2CrossN2);
const decimal el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
mR2CrossN2.dot(I2R2CrossN1);
const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
mR2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
mInverseMassMatrixTranslationConstraint.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
}
// Compute the position error for the 2 translation constraints
const Vector2 translationError(u.dot(mN1), u.dot(mN2));
// Compute the Lagrange multiplier lambda for the 2 translation constraints
Vector2 lambdaTranslation = mInverseMassMatrixTranslationConstraint * (-translationError);
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints
const Vector3 linearImpulseBody1 = -mN1 * lambdaTranslation.x - mN2 * lambdaTranslation.y;
const Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * lambdaTranslation.x -
mR1PlusUCrossN2 * lambdaTranslation.y;
// Apply the impulse to the body
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints
const Vector3 linearImpulseBody2 = mN1 * lambdaTranslation.x + mN2 * lambdaTranslation.y;
const Vector3 angularImpulseBody2 = mR2CrossN1 * lambdaTranslation.x +
mR2CrossN2 * lambdaTranslation.y;
// Apply the impulse to the body
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// --------------- Rotation Constraints --------------- //
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix)
mInverseMassMatrixRotationConstraint.setToZero();
if (mBody1->getIsMotionEnabled()) {
mInverseMassMatrixRotationConstraint += mI1;
}
if (mBody2->getIsMotionEnabled()) {
mInverseMassMatrixRotationConstraint += mI2;
}
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse();
}
// Compute the position error for the 3 rotation constraints
Quaternion currentOrientationDifference = q2 * q1.getInverse();
currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
const Vector3 errorRotation = decimal(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = mInverseMassMatrixRotationConstraint * (-errorRotation);
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -lambdaRotation;
// Apply the impulse to the body
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody2 = lambdaRotation;
// Apply the impulse to the body
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// --------------- Limits Constraints --------------- //
if (mIsLimitEnabled) {
if (mIsLowerLimitViolated || mIsUpperLimitViolated) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mInverseMassMatrixLimit = 0.0;
if (mBody1->getIsMotionEnabled()) {
mInverseMassMatrixLimit += mBody1->getMassInverse() +
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis);
}
if (mBody2->getIsMotionEnabled()) {
mInverseMassMatrixLimit += mBody2->getMassInverse() +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
}
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0);
}
// If the lower limit is violated
if (mIsLowerLimitViolated) {
// Compute the Lagrange multiplier lambda for the lower limit constraint
decimal lambdaLowerLimit = mInverseMassMatrixLimit * (-lowerLimitError);
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mR1PlusUCrossSliderAxis;
// Apply the impulse to the body
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 linearImpulseBody2 = lambdaLowerLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = lambdaLowerLimit * mR2CrossSliderAxis;
// Apply the impulse to the body
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
}
// If the upper limit is violated
if (mIsUpperLimitViolated) {
// Compute the Lagrange multiplier lambda for the upper limit constraint
decimal lambdaUpperLimit = mInverseMassMatrixLimit * (-upperLimitError);
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = lambdaUpperLimit * mR1PlusUCrossSliderAxis;
// Apply the impulse to the body
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 linearImpulseBody2 = -lambdaUpperLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mR2CrossSliderAxis;
// Apply the impulse to the body
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
}
}
}
// Enable/Disable the limits of the joint
void SliderJoint::enableLimit(bool isLimitEnabled) {
if (isLimitEnabled != mIsLimitEnabled) {
mIsLimitEnabled = isLimitEnabled;
// Reset the limits
resetLimits();
}
}
// Enable/Disable the motor of the joint
void SliderJoint::enableMotor(bool isMotorEnabled) {
mIsMotorEnabled = isMotorEnabled;
mImpulseMotor = 0.0;
// TODO : Wake up the bodies of the joint here when sleeping is implemented
}
// Return the current translation value of the joint
decimal SliderJoint::getTranslation() const {
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->getTransform().getPosition();
const Vector3& x2 = mBody2->getTransform().getPosition();
const Quaternion& q1 = mBody1->getTransform().getOrientation();
const Quaternion& q2 = mBody2->getTransform().getOrientation();
// Compute the two anchor points in world-space coordinates
const Vector3 anchorBody1 = x1 + q1 * mLocalAnchorPointBody1;
const Vector3 anchorBody2 = x2 + q2 * mLocalAnchorPointBody2;
// Compute the vector u (difference between anchor points)
const Vector3 u = anchorBody2 - anchorBody1;
// Compute the slider axis in world-space
Vector3 sliderAxisWorld = q1 * mSliderAxisBody1;
sliderAxisWorld.normalize();
// Compute and return the translation value
return u.dot(sliderAxisWorld);
}
// Set the minimum translation limit
void SliderJoint::setMinTranslationLimit(decimal lowerLimit) {
assert(lowerLimit <= mUpperLimit);
if (lowerLimit != mLowerLimit) {
mLowerLimit = lowerLimit;
// Reset the limits
resetLimits();
}
}
// Set the maximum translation limit
void SliderJoint::setMaxTranslationLimit(decimal upperLimit) {
assert(mLowerLimit <= upperLimit);
if (upperLimit != mUpperLimit) {
mUpperLimit = upperLimit;
// Reset the limits
resetLimits();
}
}
// Reset the limits
void SliderJoint::resetLimits() {
// Reset the accumulated impulses for the limits
mImpulseLowerLimit = 0.0;
mImpulseUpperLimit = 0.0;
// TODO : Wake up the bodies of the joint here when sleeping is implemented
}
// Set the motor speed
void SliderJoint::setMotorSpeed(decimal motorSpeed) {
if (motorSpeed != mMotorSpeed) {
mMotorSpeed = motorSpeed;
// TODO : Wake up the bodies of the joint here when sleeping is implemented
}
}
// Set the maximum motor force
void SliderJoint::setMaxMotorForce(decimal maxMotorForce) {
if (maxMotorForce != mMaxMotorForce) {
assert(mMaxMotorForce >= 0.0);
mMaxMotorForce = maxMotorForce;
// TODO : Wake up the bodies of the joint here when sleeping is implemented
}
}

352
src/constraint/SliderJoint.h Normal file
View File

@ -0,0 +1,352 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 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
// Libraries
#include "../mathematics/mathematics.h"
#include "../engine/ConstraintSolver.h"
namespace reactphysics3d {
// Structure SliderJointInfo
/**
* This structure is used to gather the information needed to create a slider
* joint. This structure will be used to create the actual slider joint.
*/
struct SliderJointInfo : public ConstraintInfo {
public :
// -------------------- Attributes -------------------- //
/// Anchor point (in world-space coordinates)
Vector3 anchorPointWorldSpace;
/// Slider axis (in world-space coordinates)
Vector3 sliderAxisWorldSpace;
/// True if the slider limits are enabled
bool isLimitEnabled;
/// True if the slider motor is enabled
bool isMotorEnabled;
/// Mininum allowed translation if limits are enabled
decimal minTranslationLimit;
/// Maximum allowed translation if limits are enabled
decimal maxTranslationLimit;
/// Motor speed
decimal motorSpeed;
/// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
decimal maxMotorForce;
/// Constructor without limits and without motor
SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace,
const Vector3& initSliderAxisWorldSpace)
: ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace),
sliderAxisWorldSpace(initSliderAxisWorldSpace),
isLimitEnabled(false), isMotorEnabled(false), minTranslationLimit(-1.0),
maxTranslationLimit(1.0), motorSpeed(0), maxMotorForce(0) {}
/// Constructor with limits and no motor
SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace,
const Vector3& initSliderAxisWorldSpace,
decimal initMinTranslationLimit, decimal initMaxTranslationLimit)
: ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace),
sliderAxisWorldSpace(initSliderAxisWorldSpace),
isLimitEnabled(true), isMotorEnabled(false),
minTranslationLimit(initMinTranslationLimit),
maxTranslationLimit(initMaxTranslationLimit), motorSpeed(0),
maxMotorForce(0) {}
/// Constructor with limits and motor
SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace,
const Vector3& initSliderAxisWorldSpace,
decimal initMinTranslationLimit, decimal initMaxTranslationLimit,
decimal initMotorSpeed, decimal initMaxMotorForce)
: ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace),
sliderAxisWorldSpace(initSliderAxisWorldSpace),
isLimitEnabled(true), isMotorEnabled(true),
minTranslationLimit(initMinTranslationLimit),
maxTranslationLimit(initMaxTranslationLimit), motorSpeed(initMotorSpeed),
maxMotorForce(initMaxMotorForce) {}
};
// Class SliderJoint
/**
* This class represents a slider joint.
*/
class SliderJoint : public Constraint {
private :
// -------------------- Constants -------------------- //
// Beta value for the position correction bias factor
static const decimal BETA;
// -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local-space coordinates of body 1)
Vector3 mLocalAnchorPointBody1;
/// Anchor point of body 2 (in local-space coordinates of body 2)
Vector3 mLocalAnchorPointBody2;
/// Slider axis (in local-space coordinates of body 1)
Vector3 mSliderAxisBody1;
/// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 mI1;
/// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 mI2;
/// Inverse of the initial orientation difference between the two bodies
Quaternion mInitOrientationDifferenceInv;
/// First vector orthogonal to the slider axis local-space of body 1
Vector3 mN1;
/// Second vector orthogonal to the slider axis and mN1 in local-space of body 1
Vector3 mN2;
/// Vector r1 in world-space coordinates
Vector3 mR1;
/// Vector r2 in world-space coordinates
Vector3 mR2;
/// Cross product of r2 and n1
Vector3 mR2CrossN1;
/// Cross product of r2 and n2
Vector3 mR2CrossN2;
/// Cross product of r2 and the slider axis
Vector3 mR2CrossSliderAxis;
/// Cross product of vector (r1 + u) and n1
Vector3 mR1PlusUCrossN1;
/// Cross product of vector (r1 + u) and n2
Vector3 mR1PlusUCrossN2;
/// Cross product of vector (r1 + u) and the slider axis
Vector3 mR1PlusUCrossSliderAxis;
/// Bias of the 2 translation constraints
Vector2 mBTranslation;
/// Bias of the 3 rotation constraints
Vector3 mBRotation;
/// Bias of the lower limit constraint
decimal mBLowerLimit;
/// Bias of the upper limit constraint
decimal mBUpperLimit;
/// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix)
Matrix2x2 mInverseMassMatrixTranslationConstraint;
/// Inverse of mass matrix K=JM^-1J^t for the rotation constraint (3x3 matrix)
Matrix3x3 mInverseMassMatrixRotationConstraint;
/// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix)
decimal mInverseMassMatrixLimit;
/// Inverse of mass matrix K=JM^-1J^t for the motor
decimal mInverseMassMatrixMotor;
/// Accumulated impulse for the 2 translation constraints
Vector2 mImpulseTranslation;
/// Accumulated impulse for the 3 rotation constraints
Vector3 mImpulseRotation;
/// Accumulated impulse for the lower limit constraint
decimal mImpulseLowerLimit;
/// Accumulated impulse for the upper limit constraint
decimal mImpulseUpperLimit;
/// Accumulated impulse for the motor
decimal mImpulseMotor;
/// True if the slider limits are enabled
bool mIsLimitEnabled;
/// True if the motor of the joint in enabled
bool mIsMotorEnabled;
/// Slider axis in world-space coordinates
Vector3 mSliderAxisWorld;
/// Lower limit (minimum translation distance)
decimal mLowerLimit;
/// Upper limit (maximum translation distance)
decimal mUpperLimit;
/// True if the lower limit is violated
bool mIsLowerLimitViolated;
/// True if the upper limit is violated
bool mIsUpperLimitViolated;
/// Motor speed
decimal mMotorSpeed;
/// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
decimal mMaxMotorForce;
// -------------------- Methods -------------------- //
/// Reset the limits
void resetLimits();
public :
// -------------------- Methods -------------------- //
/// Constructor
SliderJoint(const SliderJointInfo& jointInfo);
/// Destructor
virtual ~SliderJoint();
/// Return true if the limits or the joint are enabled
bool isLimitEnabled() const;
/// Return true if the motor of the joint is enabled
bool isMotorEnabled() const;
/// Enable/Disable the limits of the joint
void enableLimit(bool isLimitEnabled);
/// Enable/Disable the motor of the joint
void enableMotor(bool isMotorEnabled);
/// Return the current translation value of the joint
decimal getTranslation() const;
/// Return the minimum translation limit
decimal getMinTranslationLimit() const;
/// Set the minimum translation limit
void setMinTranslationLimit(decimal lowerLimit);
/// Return the maximum translation limit
decimal getMaxTranslationLimit() const;
/// Set the maximum translation limit
void setMaxTranslationLimit(decimal upperLimit);
/// Return the motor speed
decimal getMotorSpeed() const;
/// Set the motor speed
void setMotorSpeed(decimal motorSpeed);
/// Return the maximum motor force
decimal getMaxMotorForce() const;
/// Set the maximum motor force
void setMaxMotorForce(decimal maxMotorForce);
/// Return the intensity of the current force applied for the joint motor
decimal getMotorForce(decimal timeStep) const;
/// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const;
/// Initialize before solving the constraint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
/// Warm start the constraint (apply the previous impulse at the beginning of the step)
virtual void warmstart(const ConstraintSolverData& constraintSolverData);
/// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
/// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
};
// Return true if the limits or the joint are enabled
inline bool SliderJoint::isLimitEnabled() const {
return mIsLimitEnabled;
}
// Return true if the motor of the joint is enabled
inline bool SliderJoint::isMotorEnabled() const {
return mIsMotorEnabled;
}
// Return the minimum translation limit
inline decimal SliderJoint::getMinTranslationLimit() const {
return mLowerLimit;
}
// Return the maximum translation limit
inline decimal SliderJoint::getMaxTranslationLimit() const {
return mUpperLimit;
}
// Return the motor speed
inline decimal SliderJoint::getMotorSpeed() const {
return mMotorSpeed;
}
// Return the maximum motor force
inline decimal SliderJoint::getMaxMotorForce() const {
return mMaxMotorForce;
}
// Return the intensity of the current force applied for the joint motor
inline decimal SliderJoint::getMotorForce(decimal timeStep) const {
return mImpulseMotor / timeStep;
}
// Return the number of bytes used by the joint
inline size_t SliderJoint::getSizeInBytes() const {
return sizeof(SliderJoint);
}
}
#endif

13
src/engine/CollisionWorld.cpp
View File

@ -56,7 +56,7 @@ void CollisionWorld::notifyRemovedOverlappingPair(const BroadPhasePair* removedP
// Notify the world about a new narrow-phase contact
void CollisionWorld::notifyNewContact(const BroadPhasePair* broadPhasePair,
const ContactInfo* contactInfo) {
const ContactPointInfo* contactInfo) {
// TODO : Implement this method
}
@ -101,7 +101,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
// Add the body ID to the list of free IDs
mFreeBodiesIDs.push_back(collisionBody->getID());
// Call the constructor of the collision body
// Call the destructor of the collision body
collisionBody->CollisionBody::~CollisionBody();
// Remove the collision body from the list of bodies
@ -154,6 +154,7 @@ CollisionShape* CollisionWorld::createCollisionShape(const CollisionShape& colli
// A similar collision shape does not already exist in the world, so we create a
// new one and add it to the world
void* allocatedMemory = mMemoryAllocator.allocate(collisionShape.getSizeInBytes());
size_t test = collisionShape.getSizeInBytes();
CollisionShape* newCollisionShape = collisionShape.clone(allocatedMemory);
mCollisionShapes.push_back(newCollisionShape);
@ -181,8 +182,14 @@ void CollisionWorld::removeCollisionShape(CollisionShape* collisionShape) {
// Remove the shape from the set of shapes in the world
mCollisionShapes.remove(collisionShape);
// Compute the size (in bytes) of the collision shape
size_t nbBytesShape = collisionShape->getSizeInBytes();
// Call the destructor of the collision shape
collisionShape->CollisionShape::~CollisionShape();
// Deallocate the memory used by the collision shape
mMemoryAllocator.release(collisionShape, collisionShape->getSizeInBytes());
mMemoryAllocator.release(collisionShape, nbBytesShape);
}
}

2
src/engine/CollisionWorld.h
View File

@ -91,7 +91,7 @@ class CollisionWorld {
virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair);
/// Notify the world about a new narrow-phase contact
virtual void notifyNewContact(const BroadPhasePair* pair, const ContactInfo* contactInfo);
virtual void notifyNewContact(const BroadPhasePair* pair, const ContactPointInfo* contactInfo);
/// Update the overlapping pair
virtual void updateOverlappingPair(const BroadPhasePair* pair);

115
src/engine/ConstraintSolver.cpp Normal file
View File

@ -0,0 +1,115 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "ConstraintSolver.h"
#include "Profiler.h"
using namespace reactphysics3d;
// Constructor
ConstraintSolver::ConstraintSolver(std::set<Constraint*>& joints,
std::vector<Vector3>& linearVelocities,
std::vector<Vector3>& angularVelocities,
std::vector<Vector3>& positions,
std::vector<Quaternion>& orientations,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
: mJoints(joints), mLinearVelocities(linearVelocities),
mAngularVelocities(angularVelocities), mPositions(positions),
mOrientations(orientations),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mConstraintSolverData(linearVelocities,
angularVelocities, positions, orientations, mapBodyToVelocityIndex){
}
// Destructor
ConstraintSolver::~ConstraintSolver() {
}
// Initialize the constraint solver
void ConstraintSolver::initialize(decimal dt) {
PROFILE("ConstraintSolver::initialize()");
// Set the current time step
mTimeStep = dt;
// Initialize the constraint solver data used to initialize and solve the constraints
mConstraintSolverData.timeStep = mTimeStep;
mConstraintSolverData.isWarmStartingActive = mIsWarmStartingActive;
// For each joint
std::set<Constraint*>::iterator it;
for (it = mJoints.begin(); it != mJoints.end(); ++it) {
Constraint* joint = (*it);
// Get the rigid bodies of the joint
RigidBody* body1 = joint->getBody1();
RigidBody* body2 = joint->getBody2();
// Add the bodies to the set of constrained bodies
mConstraintBodies.insert(body1);
mConstraintBodies.insert(body2);
// Initialize the constraint before solving it
joint->initBeforeSolve(mConstraintSolverData);
// Warm-start the constraint if warm-starting is enabled
if (mIsWarmStartingActive) {
joint->warmstart(mConstraintSolverData);
}
}
}
// Solve the velocity constraints
void ConstraintSolver::solveVelocityConstraints() {
PROFILE("ConstraintSolver::solveVelocityConstraints()");
// For each joint
std::set<Constraint*>::iterator it;
for (it = mJoints.begin(); it != mJoints.end(); ++it) {
// Solve the constraint
(*it)->solveVelocityConstraint(mConstraintSolverData);
}
}
// Solve the position constraints
void ConstraintSolver::solvePositionConstraints() {
PROFILE("ConstraintSolver::solvePositionConstraints()");
// For each joint
std::set<Constraint*>::iterator it;
for (it = mJoints.begin(); it != mJoints.end(); ++it) {
// Solve the constraint
(*it)->solvePositionConstraint(mConstraintSolverData);
}
}

233
src/engine/ConstraintSolver.h Normal file
View File

@ -0,0 +1,233 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 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
// Libraries
#include "../configuration.h"
#include "mathematics/mathematics.h"
#include "../constraint/Constraint.h"
#include <map>
#include <set>
namespace reactphysics3d {
// Structure ConstraintSolverData
/**
* This structure contains data from the constraint solver that are used to solve
* each joint constraint.
*/
struct ConstraintSolverData {
public :
/// Current time step of the simulation
decimal timeStep;
/// Reference to the bodies linear velocities
std::vector<Vector3>& linearVelocities;
/// Reference to the bodies angular velocities
std::vector<Vector3>& angularVelocities;
/// Reference to the bodies positions
std::vector<Vector3>& positions;
/// Reference to the bodies orientations
std::vector<Quaternion>& orientations;
/// Reference to the map that associates rigid body to their index
/// in the constrained velocities array
const std::map<RigidBody*, uint>& mapBodyToConstrainedVelocityIndex;
/// True if warm starting of the solver is active
bool isWarmStartingActive;
/// Constructor
ConstraintSolverData(std::vector<Vector3>& refLinearVelocities,
std::vector<Vector3>& refAngularVelocities,
std::vector<Vector3>& refPositions,
std::vector<Quaternion>& refOrientations,
const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
:linearVelocities(refLinearVelocities),
angularVelocities(refAngularVelocities),
positions(refPositions), orientations(refOrientations),
mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
}
};
// Class ConstraintSolver
/**
* This class represents the constraint solver that is used to solve constraints between
* the rigid bodies. The constraint solver is based on the "Sequential Impulse" technique
* described by Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list).
*
* A constraint between two bodies is represented by a function C(x) which is equal to zero
* when the constraint is satisfied. The condition C(x)=0 describes a valid position and the
* condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is
* the Jacobian matrix of the constraint, v is a vector that contains the velocity of both
* bodies and b is the constraint bias. We are looking for a force F_c that will act on the
* bodies to keep the constraint satisfied. Note that from the virtual work principle, we have
* F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a
* Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange
* multiplier lambda.
* An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a
* body to change its velocity. The idea of the Sequential Impulse technique is to apply
* impulses to bodies of each constraints in order to keep the constraint satisfied.
*
* --- Step 1 ---
*
* First, we integrate 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
*
* where M is a matrix that contains mass and inertia tensor information.
*
* --- Step 2 ---
*
* During the second step, we iterate over all the constraints for a certain number of
* iterations and for each constraint we compute the impulse to apply to the bodies needed
* so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that
* M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and
* P_c is the constraint impulse to apply to the body. Therefore, we have
* v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda
* using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the
* Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to
* the bodies to satisfy the constraint.
*
* --- Step 3 ---
*
* In the third step, we integrate 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),
* the value lambda is not only the lagrange multiplier but is the multiplication of the
* Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use
* lambda, we mean (lambda * dt).
*
* We are using the accumulated impulse technique that is also described in the slides from
* Erin Catto.
*
* We are also using warm starting. The idea is to warm start the solver at the beginning of
* each step by applying the last impulstes for the constraints that we already existing at the
* previous step. This allows the iterative solver to converge faster towards the solution.
*
* For contact constraints, we are also using split impulses so that the position correction
* that uses Baumgarte stabilization does not change the momentum of the bodies.
*
* There are two ways to apply the friction constraints. Either the friction constraints are
* applied at each contact point or they are applied only at the center of the contact manifold
* between two bodies. If we solve the friction constraints at each contact point, we need
* two constraints (two tangential friction directions) and if we solve the friction
* constraints at the center of the contact manifold, we need two constraints for tangential
* friction but also another twist friction constraint to prevent spin of the body around the
* contact manifold center.
*/
class ConstraintSolver {
private :
// -------------------- Attributes -------------------- //
/// Reference to all the joints of the world
std::set<Constraint*>& mJoints;
/// Constrained bodies
std::set<RigidBody*> mConstraintBodies;
/// Reference to the array of constrained linear velocities (state of the linear velocities
/// after solving the constraints)
std::vector<Vector3>& mLinearVelocities;
/// Reference to the array of constrained angular velocities (state of the angular velocities
/// after solving the constraints)
std::vector<Vector3>& mAngularVelocities;
/// Reference to the array of bodies positions (for position error correction)
std::vector<Vector3>& mPositions;
/// Reference to the array of bodies orientations (for position error correction)
std::vector<Quaternion>& mOrientations;
/// Reference to the map that associates rigid body to their index in
/// the constrained velocities array
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
/// Current time step
decimal mTimeStep;
/// True if the warm starting of the solver is active
bool mIsWarmStartingActive;
/// Constraint solver data used to initialize and solve the constraints
ConstraintSolverData mConstraintSolverData;
public :
// -------------------- Methods -------------------- //
/// Constructor
ConstraintSolver(std::set<Constraint*>& joints,
std::vector<Vector3>& linearVelocities,
std::vector<Vector3>& angularVelocities,
std::vector<Vector3>& positions,
std::vector<Quaternion>& orientations,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
/// Destructor
~ConstraintSolver();
/// Initialize the constraint solver
void initialize(decimal dt);
/// Solve the constraints
void solveVelocityConstraints();
/// Solve the position constraints
void solvePositionConstraints();
/// Return true if the Non-Linear-Gauss-Seidel position correction technique is active
bool getIsNonLinearGaussSeidelPositionCorrectionActive() const;
/// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive);
/// Return true if the body is in at least one constraint
bool isConstrainedBody(RigidBody* body) const;
};
// Return true if the body is in at least one constraint
inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const {
return mConstraintBodies.count(body) == 1;
}
}
#endif

454
src/engine/ContactSolver.cpp
View File

@ -39,14 +39,15 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
const decimal ContactSolver::SLOP = decimal(0.01);
// Constructor
ContactSolver::ContactSolver(DynamicsWorld& world,std::vector<Vector3>& constrainedLinearVelocities,
ContactSolver::ContactSolver(std::vector<ContactManifold*>& contactManifolds,
std::vector<Vector3>& constrainedLinearVelocities,
std::vector<Vector3>& constrainedAngularVelocities,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
:mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS),
:mContactManifolds(contactManifolds),
mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
mContactConstraints(NULL),
mConstrainedLinearVelocities(constrainedLinearVelocities),
mConstrainedAngularVelocities(constrainedAngularVelocities),
mLinearVelocities(constrainedLinearVelocities),
mAngularVelocities(constrainedAngularVelocities),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mIsSplitImpulseActive(true),
mIsSolveFrictionAtContactManifoldCenterActive(true) {
@ -59,17 +60,21 @@ ContactSolver::~ContactSolver() {
}
// Initialize the constraint solver
void ContactSolver::initialize() {
void ContactSolver::initialize(decimal dt) {
PROFILE("ContactSolver::initialize()");
// Set the current time step
mTimeStep = dt;
// TODO : Use better memory allocation here
mContactConstraints = new ContactManifoldSolver[mWorld.getNbContactManifolds()];
mContactConstraints = new ContactManifoldSolver[mContactManifolds.size()];
mNbContactManifolds = 0;
// For each contact manifold of the world
vector<ContactManifold*>::iterator it;
for (it = mWorld.getContactManifoldsBeginIterator();
it != mWorld.getContactManifoldsEndIterator(); ++it) {
for (it = mContactManifolds.begin(); it != mContactManifolds.end(); ++it) {
ContactManifold* externalManifold = *it;
@ -174,18 +179,21 @@ void ContactSolver::initialize() {
// Allocated memory for split impulse velocities
// TODO : Use better memory allocation here
mSplitLinearVelocities = new Vector3[mWorld.getNbRigidBodies()];
mSplitAngularVelocities = new Vector3[mWorld.getNbRigidBodies()];
mSplitLinearVelocities = new Vector3[mMapBodyToConstrainedVelocityIndex.size()];
mSplitAngularVelocities = new Vector3[mMapBodyToConstrainedVelocityIndex.size()];
assert(mSplitLinearVelocities != NULL);
assert(mSplitAngularVelocities != NULL);
assert(mConstraintBodies.size() > 0);
assert(mMapBodyToConstrainedVelocityIndex.size() >= mConstraintBodies.size());
assert(mConstrainedLinearVelocities.size() >= mConstraintBodies.size());
assert(mConstrainedAngularVelocities.size() >= mConstraintBodies.size());
assert(mLinearVelocities.size() >= mConstraintBodies.size());
assert(mAngularVelocities.size() >= mConstraintBodies.size());
// Initialize the split impulse velocities
initializeSplitImpulseVelocities();
// Fill-in all the matrices needed to solve the LCP problem
initializeContactConstraints();
}
// Initialize the split impulse velocities
@ -223,10 +231,10 @@ void ContactSolver::initializeContactConstraints() {
}
// Get the velocities of the bodies
const Vector3& v1 = mConstrainedLinearVelocities[manifold.indexBody1];
const Vector3& w1 = mConstrainedAngularVelocities[manifold.indexBody1];
const Vector3& v2 = mConstrainedLinearVelocities[manifold.indexBody2];
const Vector3& w2 = mConstrainedAngularVelocities[manifold.indexBody2];
const Vector3& v1 = mLinearVelocities[manifold.indexBody1];
const Vector3& w1 = mAngularVelocities[manifold.indexBody1];
const Vector3& v2 = mLinearVelocities[manifold.indexBody2];
const Vector3& w2 = mAngularVelocities[manifold.indexBody2];
// For each contact point constraint
for (uint i=0; i<manifold.nbContacts; i++) {
@ -307,9 +315,9 @@ void ContactSolver::initializeContactConstraints() {
if (mIsWarmStartingActive) {
// Get the cached accumulated impulses from the previous step
contactPoint.penetrationImpulse = externalContact->getCachedLambda(0);
contactPoint.friction1Impulse = externalContact->getCachedLambda(1);
contactPoint.friction2Impulse = externalContact->getCachedLambda(2);
contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse();
contactPoint.friction1Impulse = externalContact->getFrictionImpulse1();
contactPoint.friction2Impulse = externalContact->getFrictionImpulse2();
}
// Initialize the split impulses to zero
@ -379,6 +387,9 @@ void ContactSolver::initializeContactConstraints() {
/// the solution of the linear system
void ContactSolver::warmStart() {
// Check that warm starting is active
if (!mIsWarmStartingActive) return;
// For each constraint
for (uint c=0; c<mNbContactManifolds; c++) {
@ -518,260 +529,231 @@ void ContactSolver::warmStart() {
}
}
// Solve the contact constraints by applying sequential impulses
void ContactSolver::solveContactConstraints() {
// Solve the contacts
void ContactSolver::solve() {
PROFILE("ContactSolver::solve()");
decimal deltaLambda;
decimal lambdaTemp;
uint iter;
// For each iteration of the contact solver
for(iter=0; iter<mNbIterations; iter++) {
// For each contact manifold
for (uint c=0; c<mNbContactManifolds; c++) {
// For each contact manifold
for (uint c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c];
ContactManifoldSolver& contactManifold = mContactConstraints[c];
decimal sumPenetrationImpulse = 0.0;
decimal sumPenetrationImpulse = 0.0;
// Get the constrained velocities
const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1];
const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2];
const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2];
// Get the constrained velocities
const Vector3& v1 = mConstrainedLinearVelocities[contactManifold.indexBody1];
const Vector3& w1 = mConstrainedAngularVelocities[contactManifold.indexBody1];
const Vector3& v2 = mConstrainedLinearVelocities[contactManifold.indexBody2];
const Vector3& w2 = mConstrainedAngularVelocities[contactManifold.indexBody2];
for (uint i=0; i<contactManifold.nbContacts; i++) {
for (uint i=0; i<contactManifold.nbContacts; i++) {
ContactPointSolver& contactPoint = contactManifold.contacts[i];
ContactPointSolver& contactPoint = contactManifold.contacts[i];
// --------- Penetration --------- //
// --------- Penetration --------- //
// Compute J*v
Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
decimal deltaVDotN = deltaV.dot(contactPoint.normal);
decimal Jv = deltaVDotN;
// Compute J*v
Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
decimal deltaVDotN = deltaV.dot(contactPoint.normal);
decimal Jv = deltaVDotN;
// Compute the bias "b" of the constraint
decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
decimal biasPenetrationDepth = 0.0;
if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
max(0.0f, float(contactPoint.penetrationDepth - SLOP));
decimal b = biasPenetrationDepth + contactPoint.restitutionBias;
// Compute the bias "b" of the constraint
decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
decimal biasPenetrationDepth = 0.0;
if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
max(0.0f, float(contactPoint.penetrationDepth - SLOP));
decimal b = biasPenetrationDepth + contactPoint.restitutionBias;
// Compute the Lagrange multiplier lambda
if (mIsSplitImpulseActive) {
deltaLambda = - (Jv + contactPoint.restitutionBias) *
contactPoint.inversePenetrationMass;
}
else {
deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass;
}
lambdaTemp = contactPoint.penetrationImpulse;
contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse +
deltaLambda, decimal(0.0));
deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
// Compute the Lagrange multiplier lambda
if (mIsSplitImpulseActive) {
deltaLambda = - (Jv + contactPoint.restitutionBias) *
contactPoint.inversePenetrationMass;
}
else {
deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass;
}
lambdaTemp = contactPoint.penetrationImpulse;
contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse +
deltaLambda, decimal(0.0));
deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda,
contactPoint);
// Apply the impulse to the bodies of the constraint
applyImpulse(impulsePenetration, contactManifold);
sumPenetrationImpulse += contactPoint.penetrationImpulse;
// If the split impulse position correction is active
if (mIsSplitImpulseActive) {
// Split impulse (position correction)
const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1];
const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1];
const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2];
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) *
contactPoint.inversePenetrationMass;
decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse;
contactPoint.penetrationSplitImpulse = std::max(
contactPoint.penetrationSplitImpulse +
deltaLambdaSplit, decimal(0.0));
deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
// Compute the impulse P=J^T * lambda
const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda,
contactPoint);
const Impulse splitImpulsePenetration = computePenetrationImpulse(
deltaLambdaSplit, contactPoint);
// Apply the impulse to the bodies of the constraint
applyImpulse(impulsePenetration, contactManifold);
sumPenetrationImpulse += contactPoint.penetrationImpulse;
// If the split impulse position correction is active
if (mIsSplitImpulseActive) {
// Split impulse (position correction)
const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1];
const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1];
const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2];
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) *
contactPoint.inversePenetrationMass;
decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse;
contactPoint.penetrationSplitImpulse = std::max(
contactPoint.penetrationSplitImpulse +
deltaLambdaSplit, decimal(0.0));
deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
// Compute the impulse P=J^T * lambda
const Impulse splitImpulsePenetration = computePenetrationImpulse(
deltaLambdaSplit, contactPoint);
applySplitImpulse(splitImpulsePenetration, contactManifold);
}
// If we do not solve the friction constraints at the center of the contact manifold
if (!mIsSolveFrictionAtContactManifoldCenterActive) {
// --------- Friction 1 --------- //
// Compute J*v
deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
Jv = deltaV.dot(contactPoint.frictionVector1);
// Compute the Lagrange multiplier lambda
deltaLambda = -Jv;
deltaLambda *= contactPoint.inverseFriction1Mass;
decimal frictionLimit = contactManifold.frictionCoefficient *
contactPoint.penetrationImpulse;
lambdaTemp = contactPoint.friction1Impulse;
contactPoint.friction1Impulse = std::max(-frictionLimit,
std::min(contactPoint.friction1Impulse
+ deltaLambda, frictionLimit));
deltaLambda = contactPoint.friction1Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda,
contactPoint);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction1, contactManifold);
// --------- Friction 2 --------- //
// Compute J*v
deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
Jv = deltaV.dot(contactPoint.frictionVector2);
// Compute the Lagrange multiplier lambda
deltaLambda = -Jv;
deltaLambda *= contactPoint.inverseFriction2Mass;
frictionLimit = contactManifold.frictionCoefficient *
contactPoint.penetrationImpulse;
lambdaTemp = contactPoint.friction2Impulse;
contactPoint.friction2Impulse = std::max(-frictionLimit,
std::min(contactPoint.friction2Impulse
+ deltaLambda, frictionLimit));
deltaLambda = contactPoint.friction2Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda,
contactPoint);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction2, contactManifold);
}
applySplitImpulse(splitImpulsePenetration, contactManifold);
}
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
// If we do not solve the friction constraints at the center of the contact manifold
if (!mIsSolveFrictionAtContactManifoldCenterActive) {
// ------ First friction constraint at the center of the contact manifol ------ //
// --------- Friction 1 --------- //
// Compute J*v
Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
- v1 - w1.cross(contactManifold.r1Friction);
decimal Jv = deltaV.dot(contactManifold.frictionVector1);
deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
Jv = deltaV.dot(contactPoint.frictionVector1);
// Compute the Lagrange multiplier lambda
decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.friction1Impulse;
contactManifold.friction1Impulse = std::max(-frictionLimit,
std::min(contactManifold.friction1Impulse +
deltaLambda, frictionLimit));
deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
deltaLambda = -Jv;
deltaLambda *= contactPoint.inverseFriction1Mass;
decimal frictionLimit = contactManifold.frictionCoefficient *
contactPoint.penetrationImpulse;
lambdaTemp = contactPoint.friction1Impulse;
contactPoint.friction1Impulse = std::max(-frictionLimit,
std::min(contactPoint.friction1Impulse
+ deltaLambda, frictionLimit));
deltaLambda = contactPoint.friction1Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda;
Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda,
contactPoint);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction1, contactManifold);
// ------ Second friction constraint at the center of the contact manifol ----- //
// --------- Friction 2 --------- //
// Compute J*v
deltaV = v2 + w2.cross(contactManifold.r2Friction)
- v1 - w1.cross(contactManifold.r1Friction);
Jv = deltaV.dot(contactManifold.frictionVector2);
deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
Jv = deltaV.dot(contactPoint.frictionVector2);
// Compute the Lagrange multiplier lambda
deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.friction2Impulse;
contactManifold.friction2Impulse = std::max(-frictionLimit,
std::min(contactManifold.friction2Impulse +
deltaLambda, frictionLimit));
deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
deltaLambda = -Jv;
deltaLambda *= contactPoint.inverseFriction2Mass;
frictionLimit = contactManifold.frictionCoefficient *
contactPoint.penetrationImpulse;
lambdaTemp = contactPoint.friction2Impulse;
contactPoint.friction2Impulse = std::max(-frictionLimit,
std::min(contactPoint.friction2Impulse
+ deltaLambda, frictionLimit));
deltaLambda = contactPoint.friction2Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda;
angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda;
angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda,
contactPoint);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction2, contactManifold);
// ------ Twist friction constraint at the center of the contact manifol ------ //
// Compute J*v
deltaV = w2 - w1;
Jv = deltaV.dot(contactManifold.normal);
deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.frictionTwistImpulse;
contactManifold.frictionTwistImpulse = std::max(-frictionLimit,
std::min(contactManifold.frictionTwistImpulse
+ deltaLambda, frictionLimit));
deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
angularImpulseBody1 = -contactManifold.normal * deltaLambda;
linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);;
angularImpulseBody2 = contactManifold.normal * deltaLambda;
const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseTwistFriction, contactManifold);
}
}
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
// ------ First friction constraint at the center of the contact manifol ------ //
// Compute J*v
Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
- v1 - w1.cross(contactManifold.r1Friction);
decimal Jv = deltaV.dot(contactManifold.frictionVector1);
// Compute the Lagrange multiplier lambda
decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.friction1Impulse;
contactManifold.friction1Impulse = std::max(-frictionLimit,
std::min(contactManifold.friction1Impulse +
deltaLambda, frictionLimit));
deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda;
Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction1, contactManifold);
// ------ Second friction constraint at the center of the contact manifol ----- //
// Compute J*v
deltaV = v2 + w2.cross(contactManifold.r2Friction)
- v1 - w1.cross(contactManifold.r1Friction);
Jv = deltaV.dot(contactManifold.frictionVector2);
// Compute the Lagrange multiplier lambda
deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.friction2Impulse;
contactManifold.friction2Impulse = std::max(-frictionLimit,
std::min(contactManifold.friction2Impulse +
deltaLambda, frictionLimit));
deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda;
angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda;
angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction2, contactManifold);
// ------ Twist friction constraint at the center of the contact manifol ------ //
// Compute J*v
deltaV = w2 - w1;
Jv = deltaV.dot(contactManifold.normal);
deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.frictionTwistImpulse;
contactManifold.frictionTwistImpulse = std::max(-frictionLimit,
std::min(contactManifold.frictionTwistImpulse
+ deltaLambda, frictionLimit));
deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
angularImpulseBody1 = -contactManifold.normal * deltaLambda;
linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);;
angularImpulseBody2 = contactManifold.normal * deltaLambda;
const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseTwistFriction, contactManifold);
}
}
}
// Solve the constraints
void ContactSolver::solve(decimal timeStep) {
PROFILE("ContactSolver::solve()");
// Set the current time step
mTimeStep = timeStep;
// Initialize the solver
initialize();
// Fill-in all the matrices needed to solve the LCP problem
initializeContactConstraints();
// Warm start the solver
if (mIsWarmStartingActive) {
warmStart();
}
// Solve the contact constraints
solveContactConstraints();
// Cache the lambda values in order to use them in the next step
storeImpulses();
}
// Store the computed impulses to use them to
// warm start the solver at the next iteration
void ContactSolver::storeImpulses() {
@ -785,9 +767,9 @@ void ContactSolver::storeImpulses() {
ContactPointSolver& contactPoint = manifold.contacts[i];
contactPoint.externalContact->setCachedLambda(0, contactPoint.penetrationImpulse);
contactPoint.externalContact->setCachedLambda(1, contactPoint.friction1Impulse);
contactPoint.externalContact->setCachedLambda(2, contactPoint.friction2Impulse);
contactPoint.externalContact->setPenetrationImpulse(contactPoint.penetrationImpulse);
contactPoint.externalContact->setFrictionImpulse1(contactPoint.friction1Impulse);
contactPoint.externalContact->setFrictionImpulse2(contactPoint.friction2Impulse);
contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1);
contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2);
@ -807,15 +789,15 @@ void ContactSolver::applyImpulse(const Impulse& impulse,
// Update the velocities of the bodies by applying the impulse P
if (manifold.isBody1Moving) {
mConstrainedLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
mLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
impulse.linearImpulseBody1;
mConstrainedAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
mAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
impulse.angularImpulseBody1;
}
if (manifold.isBody2Moving) {
mConstrainedLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
mLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
impulse.linearImpulseBody2;
mConstrainedAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
mAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
impulse.angularImpulseBody2;
}
}

94
src/engine/ContactSolver.h
View File

@ -31,44 +31,13 @@
#include "../configuration.h"
#include "../constraint/Constraint.h"
#include "ContactManifold.h"
#include "Impulse.h"
#include <map>
#include <set>
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Declarations
class DynamicsWorld;
// Structure Impulse
/**
* Represents an impulse that we can apply to bodies in the contact or constraint solver.
*/
struct Impulse {
public:
/// Linear impulse applied to the first body
const Vector3 linearImpulseBody1;
/// Linear impulse applied to the second body
const Vector3 linearImpulseBody2;
/// Angular impulse applied to the first body
const Vector3 angularImpulseBody1;
/// Angular impulse applied to the second body
const Vector3 angularImpulseBody2;
/// Constructor
Impulse(const Vector3& linearImpulseBody1, const Vector3& angularImpulseBody1,
const Vector3& linearImpulseBody2, const Vector3& angularImpulseBody2)
: linearImpulseBody1(linearImpulseBody1), angularImpulseBody1(angularImpulseBody1),
linearImpulseBody2(linearImpulseBody2), angularImpulseBody2(angularImpulseBody2) {
}
};
// Class Contact Solver
/**
@ -85,7 +54,7 @@ struct Impulse {
* F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a
* Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange
* multiplier lambda.
*
* An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a
* body to change its velocity. The idea of the Sequential Impulse technique is to apply
* impulses to bodies of each constraints in order to keep the constraint satisfied.
@ -342,11 +311,8 @@ class ContactSolver {
// -------------------- Attributes -------------------- //
/// Reference to the world
DynamicsWorld& mWorld;
/// Number of iterations of the constraints solver
uint mNbIterations;
/// Reference to all the contact manifold of the world
std::vector<ContactManifold*>& mContactManifolds;
/// Split linear velocities for the position contact solver (split impulse)
Vector3* mSplitLinearVelocities;
@ -366,13 +332,11 @@ class ContactSolver {
/// Constrained bodies
std::set<RigidBody*> mConstraintBodies;
/// Pointer to the array of constrained linear velocities (state of the linear velocities
/// after solving the constraints)
std::vector<Vector3>& mConstrainedLinearVelocities;
/// Reference to the array of linear velocities
std::vector<Vector3>& mLinearVelocities;
/// Pointer to the array of constrained angular velocities (state of the angular velocities
/// after solving the constraints)
std::vector<Vector3>& mConstrainedAngularVelocities;
/// Reference to the array of angular velocities
std::vector<Vector3>& mAngularVelocities;
/// Reference to the map of rigid body to their index in the constrained velocities array
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
@ -389,25 +353,12 @@ class ContactSolver {
// -------------------- Methods -------------------- //
/// Initialize the constraint solver
void initialize();
/// Initialize the split impulse velocities
void initializeSplitImpulseVelocities();
/// Initialize the contact constraints before solving the system
void initializeContactConstraints();
/// Store the computed impulses to use them to
/// warm start the solver at the next iteration
void storeImpulses();
/// Warm start the solver.
void warmStart();
/// Solve the contact constraints by applying sequential impulses
void solveContactConstraints();
/// Apply an impulse to the two bodies of a constraint
void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
@ -452,15 +403,26 @@ class ContactSolver {
// -------------------- Methods -------------------- //
/// Constructor
ContactSolver(DynamicsWorld& mWorld, std::vector<Vector3>& constrainedLinearVelocities,
ContactSolver(std::vector<ContactManifold*>& contactManifolds,
std::vector<Vector3>& constrainedLinearVelocities,
std::vector<Vector3>& constrainedAngularVelocities,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
/// Destructor
virtual ~ContactSolver();
/// Solve the constraints
void solve(decimal timeStep);
/// Initialize the constraint solver
void initialize(decimal dt);
/// Warm start the solver.
void warmStart();
/// Store the computed impulses to use them to
/// warm start the solver at the next iteration
void storeImpulses();
/// Solve the contacts
void solve();
/// Return true if the body is in at least one constraint
bool isConstrainedBody(RigidBody* body) const;
@ -480,8 +442,8 @@ class ContactSolver {
/// Clean up the constraint solver
void cleanup();
/// Set the number of iterations of the constraint solver
void setNbIterationsSolver(uint nbIterations);
/// Return true if the split impulses position correction technique is used for contacts
bool isSplitImpulseActive() const;
/// Activate or Deactivate the split impulses for contacts
void setIsSplitImpulseActive(bool isActive);
@ -510,9 +472,9 @@ inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) {
return mSplitAngularVelocities[indexBody];
}
// Set the number of iterations of the constraint solver
inline void ContactSolver::setNbIterationsSolver(uint nbIterations) {
mNbIterations = nbIterations;
// Return true if the split impulses position correction technique is used for contacts
inline bool ContactSolver::isSplitImpulseActive() const {
return mIsSplitImpulseActive;
}
// Activate or Deactivate the split impulses for contacts
@ -528,7 +490,7 @@ inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool
// Compute the collision restitution factor from the restitution factor of each body
inline decimal ContactSolver::computeMixedRestitutionFactor(const RigidBody* body1,
const RigidBody* body2) const {
const RigidBody* body2) const {
decimal restitution1 = body1->getRestitution();
decimal restitution2 = body2->getRestitution();

328
src/engine/DynamicsWorld.cpp
View File

@ -25,6 +25,10 @@
// Libraries
#include "DynamicsWorld.h"
#include "constraint/BallAndSocketJoint.h"
#include "constraint/SliderJoint.h"
#include "constraint/HingeJoint.h"
#include "constraint/FixedJoint.h"
// Namespaces
using namespace reactphysics3d;
@ -33,8 +37,13 @@ using namespace std;
// Constructor
DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
: CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true),
mContactSolver(*this, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
mContactSolver(mContactManifolds, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
mMapBodyToConstrainedVelocityIndex),
mConstraintSolver(mJoints, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
mConstrainedPositions, mConstrainedOrientations,
mMapBodyToConstrainedVelocityIndex),
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
mIsDeactivationActive(DEACTIVATION_ENABLED) {
}
@ -91,24 +100,26 @@ void DynamicsWorld::update() {
// Compute the collision detection
mCollisionDetection.computeCollisionDetection();
// Initialize the constrained velocities
initConstrainedVelocitiesArray();
// If there are contacts
if (!mContactManifolds.empty()) {
// Solve the contacts
mContactSolver.solve(static_cast<decimal>(mTimer.getTimeStep()));
}
// Update the timer
mTimer.nextStep();
// Integrate the velocities
integrateRigidBodiesVelocities();
// Reset the movement boolean variable of each body to false
resetBodiesMovementVariable();
// Update the position and orientation of each body
updateRigidBodiesPositionAndOrientation();
// Update the timer
mTimer.nextStep();
// Solve the contacts and constraints
solveContactsAndConstraints();
// Integrate the position and orientation of each body
integrateRigidBodiesPositions();
// Solve the position correction for constraints
solvePositionCorrection();
// Update the AABBs of the bodies
updateRigidBodiesAABB();
// Cleanup of the contact solver
mContactSolver.cleanup();
@ -121,10 +132,12 @@ void DynamicsWorld::update() {
setInterpolationFactorToAllBodies();
}
// Update the position and orientation of the rigid bodies
void DynamicsWorld::updateRigidBodiesPositionAndOrientation() {
// Integrate position and orientation of the rigid bodies.
/// The positions and orientations of the bodies are integrated using
/// the sympletic Euler time stepping scheme.
void DynamicsWorld::integrateRigidBodiesPositions() {
PROFILE("DynamicsWorld::updateRigidBodiesPositionAndOrientation()");
PROFILE("DynamicsWorld::integrateRigidBodiesPositions()");
decimal dt = static_cast<decimal>(mTimer.getTimeStep());
@ -138,9 +151,6 @@ void DynamicsWorld::updateRigidBodiesPositionAndOrientation() {
// If the body is allowed to move
if (rigidBody->getIsMotionEnabled()) {
// Update the old Transform of the body
rigidBody->updateOldTransform();
// Get the constrained velocity
uint indexArray = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second;
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
@ -151,7 +161,9 @@ void DynamicsWorld::updateRigidBodiesPositionAndOrientation() {
rigidBody->setAngularVelocity(newAngVelocity);
// Add the split impulse velocity from Contact Solver (only used to update the position)
if (mContactSolver.isConstrainedBody(rigidBody)) {
if (mContactSolver.isConstrainedBody(rigidBody) &&
mContactSolver.isSplitImpulseActive()) {
newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody);
newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody);
}
@ -162,17 +174,30 @@ void DynamicsWorld::updateRigidBodiesPositionAndOrientation() {
// Compute the new position of the body
Vector3 newPosition = currentPosition + newLinVelocity * dt;
Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.x,
newAngVelocity.y,
newAngVelocity.z, 0) *
currentOrientation * 0.5 * dt;
Quaternion newOrientation = currentOrientation + Quaternion(0, newAngVelocity) *
currentOrientation * decimal(0.5) * dt;
// Update the Transform of the body
Transform newTransform(newPosition, newOrientation.getUnit());
rigidBody->setTransform(newTransform);
}
}
}
// Update the AABBs of the bodies
void DynamicsWorld::updateRigidBodiesAABB() {
PROFILE("DynamicsWorld::updateRigidBodiesAABB()");
// For each rigid body of the world
set<RigidBody*>::iterator it;
for (it = getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
// If the body has moved
if ((*it)->getHasMoved()) {
// Update the AABB of the rigid body
rigidBody->updateAABB();
(*it)->updateAABB();
}
}
}
@ -197,14 +222,20 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() {
}
}
// Initialize the constrained velocities array at each step
void DynamicsWorld::initConstrainedVelocitiesArray() {
// Integrate the velocities of rigid bodies.
/// This method only set the temporary velocities but does not update
/// 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() {
PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()");
// TODO : Use better memory allocation here
mConstrainedLinearVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
mConstrainedAngularVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
double dt = mTimer.getTimeStep();
decimal dt = static_cast<decimal>(mTimer.getTimeStep());
// Fill in the mapping of rigid body to their index in the constrained
// velocities arrays
@ -213,14 +244,125 @@ void DynamicsWorld::initConstrainedVelocitiesArray() {
RigidBody* rigidBody = *it;
mMapBodyToConstrainedVelocityIndex.insert(std::make_pair<RigidBody*, uint>(rigidBody, i));
// Integrate the external force to get the new velocity of the body
mConstrainedLinearVelocities[i] = rigidBody->getLinearVelocity() +
dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() +
dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
// If the body is allowed to move
if (rigidBody->getIsMotionEnabled()) {
// Integrate the external force to get the new velocity of the body
mConstrainedLinearVelocities[i] = rigidBody->getLinearVelocity() +
dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() +
dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
// Update the old Transform of the body
rigidBody->updateOldTransform();
}
i++;
}
assert(mMapBodyToConstrainedVelocityIndex.size() == mRigidBodies.size());
}
// Solve the contacts and constraints
void DynamicsWorld::solveContactsAndConstraints() {
PROFILE("DynamicsWorld::solveContactsAndConstraints()");
// Get the current time step
decimal dt = static_cast<decimal>(mTimer.getTimeStep());
// Check if there are contacts and constraints to solve
bool isConstraintsToSolve = !mJoints.empty();
bool isContactsToSolve = !mContactManifolds.empty();
if (!isConstraintsToSolve && !isContactsToSolve) return;
// ---------- Solve velocity constraints for joints and contacts ---------- //
// If there are contacts
if (isContactsToSolve) {
// Initialize the solver
mContactSolver.initialize(dt);
// Warm start the contact solver
mContactSolver.warmStart();
}
// If there are constraints
if (isConstraintsToSolve) {
// Initialize the constraint solver
mConstraintSolver.initialize(dt);
}
// For each iteration of the velocity solver
for (uint i=0; i<mNbVelocitySolverIterations; i++) {
// Solve the constraints
if (isConstraintsToSolve) mConstraintSolver.solveVelocityConstraints();
// Solve the contacts
if (isContactsToSolve) mContactSolver.solve();
}
// Cache the lambda values in order to use them in the next step
if (isContactsToSolve) mContactSolver.storeImpulses();
}
// Solve the position error correction of the constraints
void DynamicsWorld::solvePositionCorrection() {
PROFILE("DynamicsWorld::solvePositionCorrection()");
// Do not continue if there is no constraints
if (mJoints.empty()) return;
// ---------- Get the position/orientation of the rigid bodies ---------- //
// TODO : Use better memory allocation here
mConstrainedPositions = std::vector<Vector3>(mRigidBodies.size());
mConstrainedOrientations = std::vector<Quaternion>(mRigidBodies.size());
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
// If it is a constrained bodies (by a joint)
if (mConstraintSolver.isConstrainedBody(*it)) {
uint index = mMapBodyToConstrainedVelocityIndex.find(*it)->second;
// Get the position/orientation of the rigid body
const Transform& transform = (*it)->getTransform();
mConstrainedPositions[index] = transform.getPosition();
mConstrainedOrientations[index]= transform.getOrientation();
}
}
// ---------- Solve the position error correction for the constraints ---------- //
// For each iteration of the position (error correction) solver
for (uint i=0; i<mNbPositionSolverIterations; i++) {
// Solve the position constraints
mConstraintSolver.solvePositionConstraints();
}
// ---------- Update the position/orientation of the rigid bodies ---------- //
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
// If it is a constrained bodies (by a joint)
if (mConstraintSolver.isConstrainedBody(*it)) {
uint index = mMapBodyToConstrainedVelocityIndex.find(*it)->second;
// Get the new position/orientation of the body
const Vector3& newPosition = mConstrainedPositions[index];
const Quaternion& newOrientation = mConstrainedOrientations[index];
// Update the Transform of the body
Transform newTransform(newPosition, newOrientation.getUnit());
(*it)->setTransform(newTransform);
}
}
}
// Cleanup the constrained velocities array at each step
@ -288,7 +430,7 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal ma
return rigidBody;
}
// Destroy a rigid body
// Destroy a rigid body and all the joints which it belongs
void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
// Remove the body from the collision detection
@ -300,7 +442,15 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
// Remove the collision shape from the world
removeCollisionShape(rigidBody->getCollisionShape());
// Call the constructor of the rigid body
// Destroy all the joints that contains the rigid body to be destroyed
bodyindex idToRemove = rigidBody->getID();
for (std::set<Constraint*>::iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
if ((*it)->getBody1()->getID() == idToRemove || (*it)->getBody2()->getID() == idToRemove) {
destroyJoint(*it);
}
}
// Call the destructor of the rigid body
rigidBody->RigidBody::~RigidBody();
// Remove the rigid body from the list of rigid bodies
@ -311,9 +461,95 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
mMemoryAllocator.release(rigidBody, sizeof(RigidBody));
}
// Remove all constraints in the physics world
void DynamicsWorld::removeAllConstraints() {
mConstraints.clear();
// Create a joint between two bodies in the world and return a pointer to the new joint
Constraint* DynamicsWorld::createJoint(const ConstraintInfo& jointInfo) {
Constraint* newJoint = NULL;
// Allocate memory to create the new joint
switch(jointInfo.type) {
// Ball-and-Socket joint
case BALLSOCKETJOINT:
{
void* allocatedMemory = mMemoryAllocator.allocate(sizeof(BallAndSocketJoint));
const BallAndSocketJointInfo& info = dynamic_cast<const BallAndSocketJointInfo&>(
jointInfo);
newJoint = new (allocatedMemory) BallAndSocketJoint(info);
break;
}
// Slider joint
case SLIDERJOINT:
{
void* allocatedMemory = mMemoryAllocator.allocate(sizeof(SliderJoint));
const SliderJointInfo& info = dynamic_cast<const SliderJointInfo&>(jointInfo);
newJoint = new (allocatedMemory) SliderJoint(info);
break;
}
// Hinge joint
case HINGEJOINT:
{
void* allocatedMemory = mMemoryAllocator.allocate(sizeof(HingeJoint));
const HingeJointInfo& info = dynamic_cast<const HingeJointInfo&>(jointInfo);
newJoint = new (allocatedMemory) HingeJoint(info);
break;
}
// Fixed joint
case FIXEDJOINT:
{
void* allocatedMemory = mMemoryAllocator.allocate(sizeof(FixedJoint));
const FixedJointInfo& info = dynamic_cast<const FixedJointInfo&>(jointInfo);
newJoint = new (allocatedMemory) FixedJoint(info);
break;
}
default:
{
assert(false);
return NULL;
}
}
// If the collision between the two bodies of the constraint is disabled
if (!jointInfo.isCollisionEnabled) {
// Add the pair of bodies in the set of body pairs that cannot collide with each other
mCollisionDetection.addNoCollisionPair(jointInfo.body1, jointInfo.body2);
}
// Add the joint into the world
mJoints.insert(newJoint);
// Return the pointer to the created joint
return newJoint;
}
// Destroy a joint
void DynamicsWorld::destroyJoint(Constraint* joint) {
assert(joint != NULL);
// If the collision between the two bodies of the constraint was disabled
if (!joint->isCollisionEnabled()) {
// Remove the pair of bodies from the set of body pairs that cannot collide with each other
mCollisionDetection.removeNoCollisionPair(joint->getBody1(), joint->getBody2());
}
// Remove the joint from the world
mJoints.erase(joint);
// Get the size in bytes of the joint
size_t nbBytes = joint->getSizeInBytes();
// Call the destructor of the joint
joint->Constraint::~Constraint();
// Release the allocated memory
mMemoryAllocator.release(joint, nbBytes);
}
// Notify the world about a new broad-phase overlapping pair
@ -345,19 +581,11 @@ void DynamicsWorld::notifyRemovedOverlappingPair(const BroadPhasePair* removedPa
// Notify the world about a new narrow-phase contact
void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair,
const ContactInfo* contactInfo) {
RigidBody* const rigidBody1 = dynamic_cast<RigidBody* const>(broadPhasePair->body1);
RigidBody* const rigidBody2 = dynamic_cast<RigidBody* const>(broadPhasePair->body2);
assert(rigidBody1 != NULL);
assert(rigidBody2 != NULL);
const ContactPointInfo* contactInfo) {
// Create a new contact
ContactPoint* contact = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(
rigidBody1,
rigidBody2,
contactInfo);
*contactInfo);
assert(contact != NULL);
// Get the corresponding overlapping pair

168
src/engine/DynamicsWorld.h
View File

@ -30,6 +30,7 @@
#include "CollisionWorld.h"
#include "../collision/CollisionDetection.h"
#include "ContactSolver.h"
#include "ConstraintSolver.h"
#include "../body/RigidBody.h"
#include "Timer.h"
#include "../configuration.h"
@ -55,6 +56,15 @@ class DynamicsWorld : public CollisionWorld {
/// Contact solver
ContactSolver mContactSolver;
/// Constraint solver
ConstraintSolver mConstraintSolver;
/// Number of iterations for the velocity solver of the Sequential Impulses technique
uint mNbVelocitySolverIterations;
/// Number of iterations for the position solver of the Sequential Impulses technique
uint mNbPositionSolverIterations;
/// True if the deactivation (sleeping) of inactive bodies is enabled
bool mIsDeactivationActive;
@ -64,8 +74,8 @@ class DynamicsWorld : public CollisionWorld {
/// All the contact constraints
std::vector<ContactManifold*> mContactManifolds;
/// All the constraints (except contact constraints)
std::vector<Constraint*> mConstraints;
/// All the joints of the world
std::set<Constraint*> mJoints;
/// Gravity vector of the world
Vector3 mGravity;
@ -81,6 +91,12 @@ class DynamicsWorld : public CollisionWorld {
/// after solving the constraints)
std::vector<Vector3> mConstrainedAngularVelocities;
/// Array of constrained rigid bodies position (for position error correction)
std::vector<Vector3> mConstrainedPositions;
/// Array of constrained rigid bodies orientation (for position error correction)
std::vector<Quaternion> mConstrainedOrientations;
/// Map body to their index in the constrained velocities array
std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex;
@ -92,8 +108,11 @@ class DynamicsWorld : public CollisionWorld {
/// Private assignment operator
DynamicsWorld& operator=(const DynamicsWorld& world);
/// Compute the motion of all bodies and update their positions and orientations
void updateRigidBodiesPositionAndOrientation();
/// Integrate the positions and orientations of rigid bodies.
void integrateRigidBodiesPositions();
/// Update the AABBs of the bodies
void updateRigidBodiesAABB();
/// Update the position and orientation of a body
void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
@ -102,8 +121,14 @@ class DynamicsWorld : public CollisionWorld {
/// Compute and set the interpolation factor to all bodies
void setInterpolationFactorToAllBodies();
/// Initialize the constrained velocities array at each step
void initConstrainedVelocitiesArray();
/// Integrate the velocities of rigid bodies.
void integrateRigidBodiesVelocities();
/// Solve the contacts and constraints
void solveContactsAndConstraints();
/// Solve the position error correction of the constraints
void solvePositionCorrection();
/// Cleanup the constrained velocities array at each step
void cleanupConstrainedVelocitiesArray();
@ -124,7 +149,8 @@ class DynamicsWorld : public CollisionWorld {
virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair);
/// Notify the world about a new narrow-phase contact
virtual void notifyNewContact(const BroadPhasePair* pair, const ContactInfo* contactInfo);
virtual void notifyNewContact(const BroadPhasePair* pair,
const ContactPointInfo* contactInfo);
public :
@ -145,27 +171,36 @@ public :
/// Update the physics simulation
void update();
/// Set the number of iterations of the constraint solver
void setNbIterationsSolver(uint nbIterations);
/// Set the number of iterations for the velocity constraint solver
void setNbIterationsVelocitySolver(uint nbIterations);
/// Activate or Deactivate the split impulses for contacts
void setIsSplitImpulseActive(bool isActive);
/// Set the number of iterations for the position constraint solver
void setNbIterationsPositionSolver(uint nbIterations);
/// Set the position correction technique used for contacts
void setContactsPositionCorrectionTechnique(ContactsPositionCorrectionTechnique technique);
/// Set the position correction technique used for joints
void setJointsPositionCorrectionTechnique(JointsPositionCorrectionTechnique technique);
/// Activate or deactivate the solving of friction constraints at the center of
/// the contact manifold instead of solving them at each contact point
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
/// Set the isErrorCorrectionActive value
void setIsErrorCorrectionActive(bool isErrorCorrectionActive);
/// Create a rigid body into the physics world.
RigidBody* createRigidBody(const Transform& transform, decimal mass,
const Matrix3x3& inertiaTensorLocal,
const CollisionShape& collisionShape);
/// Destroy a rigid body
/// Destroy a rigid body and all the joints which it belongs
void destroyRigidBody(RigidBody* rigidBody);
/// Create a joint between two bodies in the world and return a pointer to the new joint
Constraint* createJoint(const ConstraintInfo& jointInfo);
/// Destroy a joint
void destroyJoint(Constraint* joint);
/// Return the gravity vector of the world
Vector3 getGravity() const;
@ -178,29 +213,14 @@ public :
/// Return the number of rigid bodies in the world
uint getNbRigidBodies() const;
/// Add a constraint
void addConstraint(Constraint* constraint);
/// Return the number of joints in the world
uint getNbJoints() const;
/// Remove a constraint
void removeConstraint(Constraint* constraint);
/// Remove all constraints and delete them (free their memory)
void removeAllConstraints();
/// Return the number of contact constraints in the world
/// Return the number of contact manifolds in the world
uint getNbContactManifolds() const;
/// Return a start iterator on the constraint list
std::vector<Constraint*>::iterator getConstraintsBeginIterator();
/// Return a end iterator on the constraint list
std::vector<Constraint*>::iterator getConstraintsEndIterator();
/// Return a start iterator on the contact manifolds list
std::vector<ContactManifold*>::iterator getContactManifoldsBeginIterator();
/// Return a end iterator on the contact manifolds list
std::vector<ContactManifold*>::iterator getContactManifoldsEndIterator();
/// Return the current physics time (in seconds)
long double getPhysicsTime() const;
/// Return an iterator to the beginning of the rigid bodies of the physics world
std::set<RigidBody*>::iterator getRigidBodiesBeginIterator();
@ -218,14 +238,36 @@ inline void DynamicsWorld::stop() {
mTimer.stop();
}
// Set the number of iterations of the constraint solver
inline void DynamicsWorld::setNbIterationsSolver(uint nbIterations) {
mContactSolver.setNbIterationsSolver(nbIterations);
// Set the number of iterations for the velocity constraint solver
inline void DynamicsWorld::setNbIterationsVelocitySolver(uint nbIterations) {
mNbVelocitySolverIterations = nbIterations;
}
// Activate or Deactivate the split impulses for contacts
inline void DynamicsWorld::setIsSplitImpulseActive(bool isActive) {
mContactSolver.setIsSplitImpulseActive(isActive);
// Set the number of iterations for the position constraint solver
inline void DynamicsWorld::setNbIterationsPositionSolver(uint nbIterations) {
mNbPositionSolverIterations = nbIterations;
}
// Set the position correction technique used for contacts
inline void DynamicsWorld::setContactsPositionCorrectionTechnique(
ContactsPositionCorrectionTechnique technique) {
if (technique == BAUMGARTE_CONTACTS) {
mContactSolver.setIsSplitImpulseActive(false);
}
else {
mContactSolver.setIsSplitImpulseActive(true);
}
}
// Set the position correction technique used for joints
inline void DynamicsWorld::setJointsPositionCorrectionTechnique(
JointsPositionCorrectionTechnique technique) {
if (technique == BAUMGARTE_JOINTS) {
mConstraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(false);
}
else {
mConstraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(true);
}
}
// Activate or deactivate the solving of friction constraints at the center of
@ -259,24 +301,6 @@ inline void DynamicsWorld::updateOverlappingPair(const BroadPhasePair* pair) {
overlappingPair->update();
}
// Add a constraint into the physics world
inline void DynamicsWorld::addConstraint(Constraint* constraint) {
assert(constraint != 0);
mConstraints.push_back(constraint);
}
// Remove a constraint and free its memory
inline void DynamicsWorld::removeConstraint(Constraint* constraint) {
std::vector<Constraint*>::iterator it;
assert(constraint != NULL);
it = std::find(mConstraints.begin(), mConstraints.end(), constraint);
assert(*it == constraint);
delete *it;
mConstraints.erase(it);
}
// Return the gravity vector of the world
inline Vector3 DynamicsWorld::getGravity() const {
return mGravity;
@ -297,6 +321,11 @@ inline uint DynamicsWorld::getNbRigidBodies() const {
return mRigidBodies.size();
}
/// Return the number of joints in the world
inline uint DynamicsWorld::getNbJoints() const {
return mJoints.size();
}
// Return an iterator to the beginning of the bodies of the physics world
inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesBeginIterator() {
return mRigidBodies.begin();
@ -312,24 +341,9 @@ inline uint DynamicsWorld::getNbContactManifolds() const {
return mContactManifolds.size();
}
// Return a start iterator on the constraint list
inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsBeginIterator() {
return mConstraints.begin();
}
// Return a end iterator on the constraint list
inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsEndIterator() {
return mConstraints.end();
}
// Return a start iterator on the contact manifolds list
inline std::vector<ContactManifold*>::iterator DynamicsWorld::getContactManifoldsBeginIterator() {
return mContactManifolds.begin();
}
// Return a end iterator on the contact manifolds list
inline std::vector<ContactManifold*>::iterator DynamicsWorld::getContactManifoldsEndIterator() {
return mContactManifolds.end();
/// Return the current physics time (in seconds)
inline long double DynamicsWorld::getPhysicsTime() const {
return mTimer.getPhysicsTime();
}
}

54
src/collision/ContactInfo.h → src/engine/Impulse.h
View File

@ -23,59 +23,43 @@
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_CONTACT_INFO_H
#define REACTPHYSICS3D_CONTACT_INFO_H
#ifndef REACTPHYSICS3D_IMPULSE_H
#define REACTPHYSICS3D_IMPULSE_H
// Libraries
#include "../collision/shapes/BoxShape.h"
#include "../mathematics/mathematics.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
// Structure ContactInfo
// Structure Impulse
/**
* This structure contains informations about a collision contact
* computed during the narrow-phase collision detection. Those
* informations are used to compute the contact set for a contact
* between two bodies.
* Represents an impulse that we can apply to bodies in the contact or constraint solver.
*/
struct ContactInfo {
private:
// -------------------- Methods -------------------- //
/// Private copy-constructor
ContactInfo(const ContactInfo& contactInfo);
/// Private assignment operator
ContactInfo& operator=(const ContactInfo& contactInfo);
struct Impulse {
public:
// -------------------- Attributes -------------------- //
/// Linear impulse applied to the first body
const Vector3 linearImpulseBody1;
/// Normal vector the the collision contact in world space
const Vector3 normal;
/// Linear impulse applied to the second body
const Vector3 linearImpulseBody2;
/// Penetration depth of the contact
const decimal penetrationDepth;
/// Angular impulse applied to the first body
const Vector3 angularImpulseBody1;
/// Contact point of body 1 in local space of body 1
const Vector3 localPoint1;
/// Contact point of body 2 in local space of body 2
const Vector3 localPoint2;
// -------------------- Methods -------------------- //
/// Angular impulse applied to the second body
const Vector3 angularImpulseBody2;
/// Constructor
ContactInfo(const Vector3& normal, decimal penetrationDepth,
const Vector3& localPoint1, const Vector3& localPoint2);
Impulse(const Vector3& linearImpulseBody1, const Vector3& angularImpulseBody1,
const Vector3& linearImpulseBody2, const Vector3& angularImpulseBody2)
: linearImpulseBody1(linearImpulseBody1), angularImpulseBody1(angularImpulseBody1),
linearImpulseBody2(linearImpulseBody2), angularImpulseBody2(angularImpulseBody2) {
}
};
}
#endif

10
src/engine/Timer.h
View File

@ -47,7 +47,7 @@ namespace reactphysics3d {
// Class Timer
/**
* This class will take care of the time in the physics engine. It
* uses fuunctions that depend on the current platform to get the
* uses functions that depend on the current platform to get the
* current time.
*/
class Timer {
@ -59,9 +59,6 @@ class Timer {
/// Timestep dt of the physics engine (timestep > 0.0)
double mTimeStep;
/// Current time of the physics engine
long double mTime;
/// Last time the timer has been updated
long double mLastUpdateTime;
@ -139,7 +136,7 @@ inline void Timer::setTimeStep(double timeStep) {
// Return the current time
inline long double Timer::getPhysicsTime() const {
return mTime;
return mLastUpdateTime;
}
// Return if the timer is running
@ -173,9 +170,6 @@ inline bool Timer::isPossibleToTakeStep() const {
inline void Timer::nextStep() {
assert(mIsRunning);
// Update the current time of the physics engine
mTime += mTimeStep;
// Update the accumulator value
mAccumulator -= mTimeStep;
}

87
src/mathematics/Matrix2x2.cpp Normal file
View File

@ -0,0 +1,87 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "Matrix2x2.h"
using namespace reactphysics3d;
// Constructor of the class Matrix2x2
Matrix2x2::Matrix2x2() {
// Initialize all values in the matrix to zero
setAllValues(0.0, 0.0, 0.0, 0.0);
}
// Constructor
Matrix2x2::Matrix2x2(decimal value) {
setAllValues(value, value, value, value);
}
// Constructor with arguments
Matrix2x2::Matrix2x2(decimal a1, decimal a2, decimal b1, decimal b2) {
// Initialize the matrix with the values
setAllValues(a1, a2, b1, b2);
}
// Destructor
Matrix2x2::~Matrix2x2() {
}
// Copy-constructor
Matrix2x2::Matrix2x2(const Matrix2x2& matrix) {
setAllValues(matrix.mRows[0][0], matrix.mRows[0][1],
matrix.mRows[1][0], matrix.mRows[1][1]);
}
// Assignment operator
Matrix2x2& Matrix2x2::operator=(const Matrix2x2& matrix) {
// Check for self-assignment
if (&matrix != this) {
setAllValues(matrix.mRows[0][0], matrix.mRows[0][1],
matrix.mRows[1][0], matrix.mRows[1][1]);
}
return *this;
}
// Return the inverse matrix
Matrix2x2 Matrix2x2::getInverse() const {
// Compute the determinant of the matrix
decimal determinant = getDeterminant();
// Check if the determinant is equal to zero
assert(std::abs(determinant) > MACHINE_EPSILON);
decimal invDeterminant = decimal(1.0) / determinant;
Matrix2x2 tempMatrix(mRows[1][1], -mRows[0][1], -mRows[1][0], mRows[0][0]);
// Return the inverse matrix
return (invDeterminant * tempMatrix);
}

312
src/mathematics/Matrix2x2.h Normal file
View File

@ -0,0 +1,312 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 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_MATRIX2X2_H
#define REACTPHYSICS3D_MATRIX2X2_H
// Libraries
#include <cassert>
#include "Vector2.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Class Matrix2x2
/**
* This class represents a 2x2 matrix.
*/
class Matrix2x2 {
private :
// -------------------- Attributes -------------------- //
/// Rows of the matrix;
Vector2 mRows[2];
public :
// -------------------- Methods -------------------- //
/// Constructor
Matrix2x2();
/// Constructor
Matrix2x2(decimal value);
/// Constructor
Matrix2x2(decimal a1, decimal a2, decimal b1, decimal b2);
/// Destructor
~Matrix2x2();
/// Copy-constructor
Matrix2x2(const Matrix2x2& matrix);
/// Assignment operator
Matrix2x2& operator=(const Matrix2x2& matrix);
/// Set all the values in the matrix
void setAllValues(decimal a1, decimal a2, decimal b1, decimal b2);
/// Set the matrix to zero
void setToZero();
/// Return a column
Vector2 getColumn(int i) const;
/// Return a row
Vector2 getRow(int i) const;
/// Return the transpose matrix
Matrix2x2 getTranspose() const;
/// Return the determinant of the matrix
decimal getDeterminant() const;
/// Return the trace of the matrix
decimal getTrace() const;
/// Return the inverse matrix
Matrix2x2 getInverse() const;
/// Return the matrix with absolute values
Matrix2x2 getAbsoluteMatrix() const;
/// Set the matrix to the identity matrix
void setToIdentity();
/// Return the 2x2 identity matrix
static Matrix2x2 identity();
/// Overloaded operator for addition
friend Matrix2x2 operator+(const Matrix2x2& matrix1, const Matrix2x2& matrix2);
/// Overloaded operator for substraction
friend Matrix2x2 operator-(const Matrix2x2& matrix1, const Matrix2x2& matrix2);
/// Overloaded operator for the negative of the matrix
friend Matrix2x2 operator-(const Matrix2x2& matrix);
/// Overloaded operator for multiplication with a number
friend Matrix2x2 operator*(decimal nb, const Matrix2x2& matrix);
/// Overloaded operator for multiplication with a matrix
friend Matrix2x2 operator*(const Matrix2x2& matrix, decimal nb);
/// Overloaded operator for matrix multiplication
friend Matrix2x2 operator*(const Matrix2x2& matrix1, const Matrix2x2& matrix2);
/// Overloaded operator for multiplication with a vector
friend Vector2 operator*(const Matrix2x2& matrix, const Vector2& vector);
/// Overloaded operator for equality condition
bool operator==(const Matrix2x2& matrix) const;
/// Overloaded operator for the is different condition
bool operator!= (const Matrix2x2& matrix) const;
/// Overloaded operator for addition with assignment
Matrix2x2& operator+=(const Matrix2x2& matrix);
/// Overloaded operator for substraction with assignment
Matrix2x2& operator-=(const Matrix2x2& matrix);
/// Overloaded operator for multiplication with a number with assignment
Matrix2x2& operator*=(decimal nb);
/// Overloaded operator to read element of the matrix.
const Vector2& operator[](int row) const;
/// Overloaded operator to read/write element of the matrix.
Vector2& operator[](int row);
};
// Method to set all the values in the matrix
inline void Matrix2x2::setAllValues(decimal a1, decimal a2,
decimal b1, decimal b2) {
mRows[0][0] = a1; mRows[0][1] = a2;
mRows[1][0] = b1; mRows[1][1] = b2;
}
// Set the matrix to zero
inline void Matrix2x2::setToZero() {
mRows[0].setToZero();
mRows[1].setToZero();
}
// Return a column
inline Vector2 Matrix2x2::getColumn(int i) const {
assert(i>= 0 && i<2);
return Vector2(mRows[0][i], mRows[1][i]);
}
// Return a row
inline Vector2 Matrix2x2::getRow(int i) const {
assert(i>= 0 && i<2);
return mRows[i];
}
// Return the transpose matrix
inline Matrix2x2 Matrix2x2::getTranspose() const {
// Return the transpose matrix
return Matrix2x2(mRows[0][0], mRows[1][0],
mRows[0][1], mRows[1][1]);
}
// Return the determinant of the matrix
inline decimal Matrix2x2::getDeterminant() const {
// Compute and return the determinant of the matrix
return mRows[0][0] * mRows[1][1] - mRows[1][0] * mRows[0][1];
}
// Return the trace of the matrix
inline decimal Matrix2x2::getTrace() const {
// Compute and return the trace
return (mRows[0][0] + mRows[1][1]);
}
// Set the matrix to the identity matrix
inline void Matrix2x2::setToIdentity() {
mRows[0][0] = 1.0; mRows[0][1] = 0.0;
mRows[1][0] = 0.0; mRows[1][1] = 1.0;
}
// Return the 2x2 identity matrix
inline Matrix2x2 Matrix2x2::identity() {
// Return the isdentity matrix
return Matrix2x2(1.0, 0.0, 0.0, 1.0);
}
// Return the matrix with absolute values
inline Matrix2x2 Matrix2x2::getAbsoluteMatrix() const {
return Matrix2x2(fabs(mRows[0][0]), fabs(mRows[0][1]),
fabs(mRows[1][0]), fabs(mRows[1][1]));
}
// Overloaded operator for addition
inline Matrix2x2 operator+(const Matrix2x2& matrix1, const Matrix2x2& matrix2) {
return Matrix2x2(matrix1.mRows[0][0] + matrix2.mRows[0][0],
matrix1.mRows[0][1] + matrix2.mRows[0][1],
matrix1.mRows[1][0] + matrix2.mRows[1][0],
matrix1.mRows[1][1] + matrix2.mRows[1][1]);
}
// Overloaded operator for substraction
inline Matrix2x2 operator-(const Matrix2x2& matrix1, const Matrix2x2& matrix2) {
return Matrix2x2(matrix1.mRows[0][0] - matrix2.mRows[0][0],
matrix1.mRows[0][1] - matrix2.mRows[0][1],
matrix1.mRows[1][0] - matrix2.mRows[1][0],
matrix1.mRows[1][1] - matrix2.mRows[1][1]);
}
// Overloaded operator for the negative of the matrix
inline Matrix2x2 operator-(const Matrix2x2& matrix) {
return Matrix2x2(-matrix.mRows[0][0], -matrix.mRows[0][1],
-matrix.mRows[1][0], -matrix.mRows[1][1]);
}
// Overloaded operator for multiplication with a number
inline Matrix2x2 operator*(decimal nb, const Matrix2x2& matrix) {
return Matrix2x2(matrix.mRows[0][0] * nb, matrix.mRows[0][1] * nb,
matrix.mRows[1][0] * nb, matrix.mRows[1][1] * nb);
}
// Overloaded operator for multiplication with a matrix
inline Matrix2x2 operator*(const Matrix2x2& matrix, decimal nb) {
return nb * matrix;
}
// Overloaded operator for matrix multiplication
inline Matrix2x2 operator*(const Matrix2x2& matrix1, const Matrix2x2& matrix2) {
return Matrix2x2(matrix1.mRows[0][0] * matrix2.mRows[0][0] + matrix1.mRows[0][1] *
matrix2.mRows[1][0],
matrix1.mRows[0][0] * matrix2.mRows[0][1] + matrix1.mRows[0][1] *
matrix2.mRows[1][1],
matrix1.mRows[1][0] * matrix2.mRows[0][0] + matrix1.mRows[1][1] *
matrix2.mRows[1][0],
matrix1.mRows[1][0] * matrix2.mRows[0][1] + matrix1.mRows[1][1] *
matrix2.mRows[1][1]);
}
// Overloaded operator for multiplication with a vector
inline Vector2 operator*(const Matrix2x2& matrix, const Vector2& vector) {
return Vector2(matrix.mRows[0][0]*vector.x + matrix.mRows[0][1]*vector.y,
matrix.mRows[1][0]*vector.x + matrix.mRows[1][1]*vector.y);
}
// Overloaded operator for equality condition
inline bool Matrix2x2::operator==(const Matrix2x2& matrix) const {
return (mRows[0][0] == matrix.mRows[0][0] && mRows[0][1] == matrix.mRows[0][1] &&
mRows[1][0] == matrix.mRows[1][0] && mRows[1][1] == matrix.mRows[1][1]);
}
// Overloaded operator for the is different condition
inline bool Matrix2x2::operator!= (const Matrix2x2& matrix) const {
return !(*this == matrix);
}
// Overloaded operator for addition with assignment
inline Matrix2x2& Matrix2x2::operator+=(const Matrix2x2& matrix) {
mRows[0][0] += matrix.mRows[0][0]; mRows[0][1] += matrix.mRows[0][1];
mRows[1][0] += matrix.mRows[1][0]; mRows[1][1] += matrix.mRows[1][1];
return *this;
}
// Overloaded operator for substraction with assignment
inline Matrix2x2& Matrix2x2::operator-=(const Matrix2x2& matrix) {
mRows[0][0] -= matrix.mRows[0][0]; mRows[0][1] -= matrix.mRows[0][1];
mRows[1][0] -= matrix.mRows[1][0]; mRows[1][1] -= matrix.mRows[1][1];
return *this;
}
// Overloaded operator for multiplication with a number with assignment
inline Matrix2x2& Matrix2x2::operator*=(decimal nb) {
mRows[0][0] *= nb; mRows[0][1] *= nb;
mRows[1][0] *= nb; mRows[1][1] *= nb;
return *this;
}
// Overloaded operator to return a row of the matrix.
/// This operator is also used to access a matrix value using the syntax
/// matrix[row][col].
inline const Vector2& Matrix2x2::operator[](int row) const {
return mRows[row];
}
// Overloaded operator to return a row of the matrix.
/// This operator is also used to access a matrix value using the syntax
/// matrix[row][col].
inline Vector2& Matrix2x2::operator[](int row) {
return mRows[row];
}
}
#endif

10
src/mathematics/Matrix3x3.h
View File

@ -105,6 +105,10 @@ class Matrix3x3 {
/// Return the 3x3 identity matrix
static Matrix3x3 identity();
/// Return a skew-symmetric matrix using a given vector that can be used
/// to compute cross product with another vector using matrix multiplication
static Matrix3x3 computeSkewSymmetricMatrixForCrossProduct(const Vector3& vector);
/// Overloaded operator for addition
friend Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2);
@ -215,6 +219,12 @@ inline Matrix3x3 Matrix3x3::identity() {
return Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
}
// Return a skew-symmetric matrix using a given vector that can be used
// to compute cross product with another vector using matrix multiplication
inline Matrix3x3 Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(const Vector3& vector) {
return Matrix3x3(0, -vector.z, vector.y, vector.z, 0, -vector.x, -vector.y, vector.x, 0);
}
// Return the matrix with absolute values
inline Matrix3x3 Matrix3x3::getAbsoluteMatrix() const {
return Matrix3x3(fabs(mRows[0][0]), fabs(mRows[0][1]), fabs(mRows[0][2]),

74
src/mathematics/Quaternion.h
View File

@ -84,15 +84,24 @@ struct Quaternion {
/// Set the quaternion to zero
void setToZero();
/// Set to the identity quaternion
void setToIdentity();
/// Return the vector v=(x y z) of the quaternion
Vector3 getVectorV() const;
/// Return the length of the quaternion
decimal length() const;
/// Return the square of the length of the quaternion
decimal lengthSquare() const;
/// Normalize the quaternion
void normalize();
/// Inverse the quaternion
void inverse();
/// Return the unit quaternion
Quaternion getUnit() const;
@ -124,6 +133,12 @@ struct Quaternion {
/// Overloaded operator for the substraction
Quaternion operator-(const Quaternion& quaternion) const;
/// Overloaded operator for addition with assignment
Quaternion& operator+=(const Quaternion& quaternion);
/// Overloaded operator for substraction with assignment
Quaternion& operator-=(const Quaternion& quaternion);
/// Overloaded operator for the multiplication with a constant
Quaternion operator*(decimal nb) const;
@ -131,7 +146,7 @@ struct Quaternion {
Quaternion operator*(const Quaternion& quaternion) const;
/// Overloaded operator for the multiplication with a vector
Vector3 operator*(const Vector3& point);
Vector3 operator*(const Vector3& point) const;
/// Overloaded operator for assignment
Quaternion& operator=(const Quaternion& quaternion);
@ -156,6 +171,14 @@ inline void Quaternion::setToZero() {
w = 0;
}
// Set to the identity quaternion
inline void Quaternion::setToIdentity() {
x = 0;
y = 0;
z = 0;
w = 1;
}
// Return the vector v=(x y z) of the quaternion
inline Vector3 Quaternion::getVectorV() const {
@ -168,6 +191,11 @@ inline decimal Quaternion::length() const {
return sqrt(x*x + y*y + z*z + w*w);
}
// Return the square of the length of the quaternion
inline decimal Quaternion::lengthSquare() const {
return x*x + y*y + z*z + w*w;
}
// Normalize the quaternion
inline void Quaternion::normalize() {
@ -182,6 +210,21 @@ inline void Quaternion::normalize() {
w /= l;
}
// Inverse the quaternion
inline void Quaternion::inverse() {
// Get the square length of the quaternion
decimal lengthSquareQuaternion = lengthSquare();
assert (lengthSquareQuaternion > MACHINE_EPSILON);
// Compute and return the inverse quaternion
x /= -lengthSquareQuaternion;
y /= -lengthSquareQuaternion;
z /= -lengthSquareQuaternion;
w /= lengthSquareQuaternion;
}
// Return the unit quaternion
inline Quaternion Quaternion::getUnit() const {
decimal lengthQuaternion = length();
@ -207,14 +250,13 @@ inline Quaternion Quaternion::getConjugate() const {
// Return the inverse of the quaternion (inline)
inline Quaternion Quaternion::getInverse() const {
decimal lengthQuaternion = length();
lengthQuaternion = lengthQuaternion * lengthQuaternion;
decimal lengthSquareQuaternion = lengthSquare();
assert (lengthQuaternion > MACHINE_EPSILON);
assert (lengthSquareQuaternion > MACHINE_EPSILON);
// Compute and return the inverse quaternion
return Quaternion(-x / lengthQuaternion, -y / lengthQuaternion,
-z / lengthQuaternion, w / lengthQuaternion);
return Quaternion(-x / lengthSquareQuaternion, -y / lengthSquareQuaternion,
-z / lengthSquareQuaternion, w / lengthSquareQuaternion);
}
// Scalar product between two quaternions
@ -236,6 +278,24 @@ inline Quaternion Quaternion::operator-(const Quaternion& quaternion) const {
return Quaternion(x - quaternion.x, y - quaternion.y, z - quaternion.z, w - quaternion.w);
}
// Overloaded operator for addition with assignment
inline Quaternion& Quaternion::operator+=(const Quaternion& quaternion) {
x += quaternion.x;
y += quaternion.y;
z += quaternion.z;
w += quaternion.w;
return *this;
}
// Overloaded operator for substraction with assignment
inline Quaternion& Quaternion::operator-=(const Quaternion& quaternion) {
x -= quaternion.x;
y -= quaternion.y;
z -= quaternion.z;
w -= quaternion.w;
return *this;
}
// Overloaded operator for the multiplication with a constant
inline Quaternion Quaternion::operator*(decimal nb) const {
return Quaternion(nb * x, nb * y, nb * z, nb * w);
@ -250,7 +310,7 @@ inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const {
// Overloaded operator for the multiplication with a vector.
/// This methods rotates a point given the rotation of a quaternion.
inline Vector3 Quaternion::operator*(const Vector3& point) {
inline Vector3 Quaternion::operator*(const Vector3& point) const {
Quaternion p(point.x, point.y, point.z, 0.0);
return (((*this) * p) * getConjugate()).getVectorV();
}

4
src/mathematics/Transform.h
View File

@ -92,7 +92,7 @@ class Transform {
void getOpenGLMatrix(decimal* openglMatrix) const;
/// Return the inverse of the transform
Transform inverse() const;
Transform getInverse() const;
/// Return an interpolated transform
static Transform interpolateTransforms(const Transform& oldTransform,
@ -167,7 +167,7 @@ inline void Transform::getOpenGLMatrix(decimal* openglMatrix) const {
}
// Return the inverse of the transform
inline Transform Transform::inverse() const {
inline Transform Transform::getInverse() const {
const Quaternion& invQuaternion = mOrientation.getInverse();
Matrix3x3 invMatrix = invQuaternion.getMatrix();
return Transform(invMatrix * (-mPosition), invQuaternion);

45
src/collision/ContactInfo.cpp → src/mathematics/Vector2.cpp
View File

@ -24,15 +24,48 @@
********************************************************************************/
// Libraries
#include "ContactInfo.h"
#include "Vector2.h"
#include <vector>
// Namespaces
using namespace reactphysics3d;
// Constructor
ContactInfo::ContactInfo(const Vector3& normal, decimal penetrationDepth,
const Vector3& localPoint1, const Vector3& localPoint2)
: normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1),
localPoint2(localPoint2) {
Vector2::Vector2() : x(0.0), y(0.0) {
}
// Constructor with arguments
Vector2::Vector2(decimal newX, decimal newY) : x(newX), y(newY) {
}
// Copy-constructor
Vector2::Vector2(const Vector2& vector) : x(vector.x), y(vector.y) {
}
// Destructor
Vector2::~Vector2() {
}
// Return the corresponding unit vector
Vector2 Vector2::getUnit() const {
decimal lengthVector = length();
assert(lengthVector > MACHINE_EPSILON);
// Compute and return the unit vector
decimal lengthInv = decimal(1.0) / lengthVector;
return Vector2(x * lengthInv, y * lengthInv);
}
// Return one unit orthogonal vector of the current vector
Vector2 Vector2::getOneUnitOrthogonalVector() const {
decimal l = length();
assert(l > MACHINE_EPSILON);
return Vector2(-y / l, x / l);
}

296
src/mathematics/Vector2.h Normal file
View File

@ -0,0 +1,296 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 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_VECTOR2_H
#define REACTPHYSICS3D_VECTOR2_H
// Libraries
#include <cmath>
#include <cassert>
#include "mathematics_functions.h"
#include "../decimal.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Class Vector2
/**
* This class represents a 2D vector.
*/
struct Vector2 {
public:
// -------------------- Attributes -------------------- //
/// Component x
decimal x;
/// Component y
decimal y;
// -------------------- Methods -------------------- //
/// Constructor of the class Vector3D
Vector2();
/// Constructor with arguments
Vector2(decimal newX, decimal newY);
/// Copy-constructor
Vector2(const Vector2& vector);
/// Destructor
~Vector2();
/// Set all the values of the vector
void setAllValues(decimal newX, decimal newY);
/// Set the vector to zero
void setToZero();
/// Return the length of the vector
decimal length() const;
/// Return the square of the length of the vector
decimal lengthSquare() const;
/// Return the corresponding unit vector
Vector2 getUnit() const;
/// Return one unit orthogonal vector of the current vector
Vector2 getOneUnitOrthogonalVector() const;
/// Return true if the vector is unit and false otherwise
bool isUnit() const;
/// Return true if the current vector is the zero vector
bool isZero() const;
/// Dot product of two vectors
decimal dot(const Vector2& vector) const;
/// Normalize the vector
void normalize();
/// Return the corresponding absolute value vector
Vector2 getAbsoluteVector() const;
/// Return the axis with the minimal value
int getMinAxis() const;
/// Return the axis with the maximal value
int getMaxAxis() const;
/// Overloaded operator for the equality condition
bool operator== (const Vector2& vector) const;
/// Overloaded operator for the is different condition
bool operator!= (const Vector2& vector) const;
/// Overloaded operator for addition with assignment
Vector2& operator+=(const Vector2& vector);
/// Overloaded operator for substraction with assignment
Vector2& operator-=(const Vector2& vector);
/// Overloaded operator for multiplication with a number with assignment
Vector2& operator*=(decimal number);
/// Overloaded operator for division by a number with assignment
Vector2& operator/=(decimal number);
/// Overloaded operator for value access
decimal& operator[] (int index);
/// Overloaded operator for value access
const decimal& operator[] (int index) const;
/// Overloaded operator
Vector2& operator=(const Vector2& vector);
// -------------------- Friends -------------------- //
friend Vector2 operator+(const Vector2& vector1, const Vector2& vector2);
friend Vector2 operator-(const Vector2& vector1, const Vector2& vector2);
friend Vector2 operator-(const Vector2& vector);
friend Vector2 operator*(const Vector2& vector, decimal number);
friend Vector2 operator*(decimal number, const Vector2& vector);
friend Vector2 operator/(const Vector2& vector, decimal number);
};
// Set the vector to zero
inline void Vector2::setToZero() {
x = 0;
y = 0;
}
// Set all the values of the vector
inline void Vector2::setAllValues(decimal newX, decimal newY) {
x = newX;
y = newY;
}
// Return the length of the vector
inline decimal Vector2::length() const {
return sqrt(x*x + y*y);
}
// Return the square of the length of the vector
inline decimal Vector2::lengthSquare() const {
return x*x + y*y;
}
// Scalar product of two vectors (inline)
inline decimal Vector2::dot(const Vector2& vector) const {
return (x*vector.x + y*vector.y);
}
// Normalize the vector
inline void Vector2::normalize() {
decimal l = length();
assert(l > std::numeric_limits<decimal>::epsilon());
x /= l;
y /= l;
}
// Return the corresponding absolute value vector
inline Vector2 Vector2::getAbsoluteVector() const {
return Vector2(std::abs(x), std::abs(y));
}
// Return the axis with the minimal value
inline int Vector2::getMinAxis() const {
return (x < y ? 0 : 1);
}
// Return the axis with the maximal value
inline int Vector2::getMaxAxis() const {
return (x < y ? 1 : 0);
}
// Return true if the vector is unit and false otherwise
inline bool Vector2::isUnit() const {
return approxEqual(lengthSquare(), 1.0);
}
// Return true if the vector is the zero vector
inline bool Vector2::isZero() const {
return approxEqual(lengthSquare(), 0.0);
}
// Overloaded operator for the equality condition
inline bool Vector2::operator== (const Vector2& vector) const {
return (x == vector.x && y == vector.y);
}
// Overloaded operator for the is different condition
inline bool Vector2::operator!= (const Vector2& vector) const {
return !(*this == vector);
}
// Overloaded operator for addition with assignment
inline Vector2& Vector2::operator+=(const Vector2& vector) {
x += vector.x;
y += vector.y;
return *this;
}
// Overloaded operator for substraction with assignment
inline Vector2& Vector2::operator-=(const Vector2& vector) {
x -= vector.x;
y -= vector.y;
return *this;
}
// Overloaded operator for multiplication with a number with assignment
inline Vector2& Vector2::operator*=(decimal number) {
x *= number;
y *= number;
return *this;
}
// Overloaded operator for division by a number with assignment
inline Vector2& Vector2::operator/=(decimal number) {
assert(number > std::numeric_limits<decimal>::epsilon());
x /= number;
y /= number;
return *this;
}
// Overloaded operator for value access
inline decimal& Vector2::operator[] (int index) {
return (&x)[index];
}
// Overloaded operator for value access
inline const decimal& Vector2::operator[] (int index) const {
return (&x)[index];
}
// Overloaded operator for addition
inline Vector2 operator+(const Vector2& vector1, const Vector2& vector2) {
return Vector2(vector1.x + vector2.x, vector1.y + vector2.y);
}
// Overloaded operator for substraction
inline Vector2 operator-(const Vector2& vector1, const Vector2& vector2) {
return Vector2(vector1.x - vector2.x, vector1.y - vector2.y);
}
// Overloaded operator for the negative of a vector
inline Vector2 operator-(const Vector2& vector) {
return Vector2(-vector.x, -vector.y);
}
// Overloaded operator for multiplication with a number
inline Vector2 operator*(const Vector2& vector, decimal number) {
return Vector2(number * vector.x, number * vector.y);
}
// Overloaded operator for division by a number
inline Vector2 operator/(const Vector2& vector, decimal number) {
assert(number > MACHINE_EPSILON);
return Vector2(vector.x / number, vector.y / number);
}
// Overloaded operator for multiplication with a number
inline Vector2 operator*(decimal number, const Vector2& vector) {
return vector * number;
}
// Assignment operator
inline Vector2& Vector2::operator=(const Vector2& vector) {
if (&vector != this) {
x = vector.x;
y = vector.y;
}
return *this;
}
}
#endif

3
src/mathematics/Vector3.h
View File

@ -111,9 +111,6 @@ struct Vector3 {
/// Return the axis with the maximal value
int getMaxAxis() const;
/// Return true if two vectors are parallel
bool isParallelWith(const Vector3& vector) const;
/// Overloaded operator for the equality condition
bool operator== (const Vector3& vector) const;

2
src/mathematics/mathematics.h
View File

@ -28,8 +28,10 @@
// Libraries
#include "Matrix3x3.h"
#include "Matrix2x2.h"
#include "Quaternion.h"
#include "Vector3.h"
#include "Vector2.h"
#include "Transform.h"
#include "../configuration.h"
#include "mathematics_functions.h"

8
src/mathematics/mathematics_functions.h
View File

@ -29,6 +29,7 @@
// Libraries
#include "../configuration.h"
#include "../decimal.h"
#include <algorithm>
/// ReactPhysics3D namespace
namespace reactphysics3d {
@ -43,6 +44,13 @@ inline bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON)
return (difference < epsilon && difference > -epsilon);
}
/// Function that returns the result of the "value" clamped by
/// two others values "lowerLimit" and "upperLimit"
inline decimal clamp(decimal value, decimal lowerLimit, decimal upperLimit) {
assert(lowerLimit <= upperLimit);
return std::min(std::max(value, lowerLimit), upperLimit);
}
}

4
src/reactphysics3d.h
View File

@ -47,6 +47,10 @@
#include "collision/shapes/ConeShape.h"
#include "collision/shapes/CylinderShape.h"
#include "collision/shapes/AABB.h"
#include "constraint/BallAndSocketJoint.h"
#include "constraint/SliderJoint.h"
#include "constraint/HingeJoint.h"
#include "constraint/FixedJoint.h"
/// Alias to the ReactPhysics3D namespace
namespace rp3d = reactphysics3d;

4
test/main.cpp
View File

@ -25,9 +25,11 @@
// Libraries
#include "TestSuite.h"
#include "tests/mathematics/TestVector2.h"
#include "tests/mathematics/TestVector3.h"
#include "tests/mathematics/TestTransform.h"
#include "tests/mathematics/TestQuaternion.h"
#include "tests/mathematics/TestMatrix2x2.h"
#include "tests/mathematics/TestMatrix3x3.h"
using namespace reactphysics3d;
@ -38,10 +40,12 @@ int main() {
// ---------- Mathematics tests ---------- //
testSuite.addTest(new TestVector2);
testSuite.addTest(new TestVector3);
testSuite.addTest(new TestTransform);
testSuite.addTest(new TestQuaternion);
testSuite.addTest(new TestMatrix3x3);
testSuite.addTest(new TestMatrix2x2);
// ----------------------------- --------- //

239
test/tests/mathematics/TestMatrix2x2.h Normal file
View File

@ -0,0 +1,239 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 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 TEST_MATRIX2X2_H
#define TEST_MATRIX2X2_H
// Libraries
#include "../../Test.h"
#include "../../../src/mathematics/Matrix2x2.h"
/// Reactphysics3D namespace
namespace reactphysics3d {
// Class TestMatrix2x2
/**
* Unit test for the Matrix2x2 class
*/
class TestMatrix2x2 : public Test {
private :
// ---------- Atributes ---------- //
/// Identity transform
Matrix2x2 mIdentity;
/// First example matrix
Matrix2x2 mMatrix1;
public :
// ---------- Methods ---------- //
/// Constructor
TestMatrix2x2() : mIdentity(Matrix2x2::identity()),
mMatrix1(2, 24, -4, 5) {
}
/// Run the tests
void run() {
testConstructors();
testGetSet();
testIdentity();
testOthersMethods();
testOperators();
}
/// Test the constructors
void testConstructors() {
Matrix2x2 test1(5.0);
Matrix2x2 test2(2, 3, 4, 5);
Matrix2x2 test3(mMatrix1);
test(test1[0][0] == 5);
test(test1[0][1] == 5);
test(test1[1][0] == 5);
test(test1[1][1] == 5);
test(test2[0][0] == 2);
test(test2[0][1] == 3);
test(test2[1][0] == 4);
test(test2[1][1] == 5);
test(test3 == mMatrix1);
}
/// Test the getter and setter methods
void testGetSet() {
// Test method to set all the values
Matrix2x2 test2;
test2.setAllValues(2, 24, -4, 5);
test(test2 == mMatrix1);
// Test method to set to zero
test2.setToZero();
test(test2 == Matrix2x2(0, 0, 0, 0));
// Test method that returns a column
Vector2 column1 = mMatrix1.getColumn(0);
Vector2 column2 = mMatrix1.getColumn(1);
test(column1 == Vector2(2, -4));
test(column2 == Vector2(24, 5));
// Test method that returns a row
Vector2 row1 = mMatrix1.getRow(0);
Vector2 row2 = mMatrix1.getRow(1);
test(row1 == Vector2(2, 24));
test(row2 == Vector2(-4, 5));
}
/// Test the identity methods
void testIdentity() {
Matrix2x2 identity = Matrix2x2::identity();
Matrix2x2 test1;
test1.setToIdentity();
test(identity[0][0] == 1);
test(identity[0][1] == 0);
test(identity[1][0] == 0);
test(identity[1][1] == 1);
test(test1 == Matrix2x2::identity());
}
/// Test others methods
void testOthersMethods() {
// Test transpose
Matrix2x2 transpose = mMatrix1.getTranspose();
test(transpose == Matrix2x2(2, -4, 24, 5));
// Test trace
test(mMatrix1.getTrace() ==7);
test(Matrix2x2::identity().getTrace() == 2);
// Test determinant
Matrix2x2 matrix(-24, 64, 253, -35);
test(mMatrix1.getDeterminant() == 106);
test(matrix.getDeterminant() == -15352);
test(mIdentity.getDeterminant() == 1);
// Test inverse
Matrix2x2 matrix2(1, 2, 3, 4);
Matrix2x2 inverseMatrix = matrix2.getInverse();
test(approxEqual(inverseMatrix[0][0], decimal(-2), decimal(10e-6)));
test(approxEqual(inverseMatrix[0][1], decimal(1), decimal(10e-6)));
test(approxEqual(inverseMatrix[1][0], decimal(1.5), decimal(10e-6)));
test(approxEqual(inverseMatrix[1][1], decimal(-0.5), decimal(10e-6)));
Matrix2x2 inverseMatrix1 = mMatrix1.getInverse();
test(approxEqual(inverseMatrix1[0][0], decimal(0.047169811), decimal(10e-6)));
test(approxEqual(inverseMatrix1[0][1], decimal(-0.226415094), decimal(10e-6)));
test(approxEqual(inverseMatrix1[1][0], decimal(0.037735849), decimal(10e-6)));
test(approxEqual(inverseMatrix1[1][1], decimal(0.018867925), decimal(10e-6)));
// Test absolute matrix
Matrix2x2 matrix3(-2, -3, -4, -5);
test(matrix.getAbsoluteMatrix() == Matrix2x2(24, 64, 253, 35));
Matrix2x2 absoluteMatrix = matrix3.getAbsoluteMatrix();
test(absoluteMatrix == Matrix2x2(2, 3, 4, 5));
}
/// Test the operators
void testOperators() {
// Test addition
Matrix2x2 matrix1(2, 3, 4, 5);
Matrix2x2 matrix2(-2, 3, -5, 10);
Matrix2x2 addition1 = matrix1 + matrix2;
Matrix2x2 addition2(matrix1);
addition2 += matrix2;
test(addition1 == Matrix2x2(0, 6, -1, 15));
test(addition2 == Matrix2x2(0, 6, -1, 15));
// Test substraction
Matrix2x2 substraction1 = matrix1 - matrix2;
Matrix2x2 substraction2(matrix1);
substraction2 -= matrix2;
test(substraction1 == Matrix2x2(4, 0, 9, -5));
test(substraction2 == Matrix2x2(4, 0, 9, -5));
// Test negative operator
Matrix2x2 negative = -matrix1;
test(negative == Matrix2x2(-2, -3, -4, -5));
// Test multiplication with a number
Matrix2x2 multiplication1 = 3 * matrix1;
Matrix2x2 multiplication2 = matrix1 * 3;
Matrix2x2 multiplication3(matrix1);
multiplication3 *= 3;
test(multiplication1 == Matrix2x2(6, 9, 12, 15));
test(multiplication2 == Matrix2x2(6, 9, 12, 15));
test(multiplication3 == Matrix2x2(6, 9, 12, 15));
// Test multiplication with a matrix
Matrix2x2 multiplication4 = matrix1 * matrix2;
Matrix2x2 multiplication5 = matrix2 * matrix1;
test(multiplication4 == Matrix2x2(-19, 36, -33, 62));
test(multiplication5 == Matrix2x2(8, 9, 30, 35));
// Test multiplication with a vector
Vector2 vector1(3, -32);
Vector2 vector2(-31, -422);
Vector2 test1 = matrix1 * vector1;
Vector2 test2 = matrix2 * vector2;
test(test1 == Vector2(-90, -148));
test(test2 == Vector2(-1204, -4065));
// Test equality operators
test(Matrix2x2(34, 38, 43, 64) ==
Matrix2x2(34, 38, 43, 64));
test(Matrix2x2(34, 64, 43, 7) !=
Matrix2x2(34, 38, 43, 64));
// Test operator to read a value
test(mMatrix1[0][0] == 2);
test(mMatrix1[0][1] == 24);
test(mMatrix1[1][0] == -4);
test(mMatrix1[1][1] == 5);
// Test operator to set a value
Matrix2x2 test3;
test3[0][0] = 2;
test3[0][1] = 24;
test3[1][0] = -4;
test3[1][1] = 5;
test(test3 == mMatrix1);
}
};
}
#endif

16
test/tests/mathematics/TestMatrix3x3.h
View File

@ -1,4 +1,3 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
@ -27,14 +26,10 @@
#ifndef TEST_MATRIX3X3_H
#define TEST_MATRIX3X3_H
#endif
// Libraries
#include "../../Test.h"
#include "../../../src/mathematics/Matrix3x3.h"
using namespace reactphysics3d;
/// Reactphysics3D namespace
namespace reactphysics3d {
@ -196,6 +191,15 @@ class TestMatrix3x3 : public Test {
test(matrix.getAbsoluteMatrix() == Matrix3x3(24, 64, 253, 35, 52, 72, 21, 35, 363));
Matrix3x3 absoluteMatrix = matrix2.getAbsoluteMatrix();
test(absoluteMatrix == Matrix3x3(2, 3, 4, 5, 6, 7, 8, 9, 10));
// Test method that computes skew-symmetric matrix for cross product
Vector3 vector1(3, -5, 6);
Vector3 vector2(73, 42, 26);
Matrix3x3 skewMatrix = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(vector1);
test(skewMatrix == Matrix3x3(0, -6, -5, 6, 0, -3, 5, 3, 0));
Vector3 crossProduct1 = vector1.cross(vector2);
Vector3 crossProduct2 = skewMatrix * vector2;
test(crossProduct1 == crossProduct2);
}
/// Test the operators
@ -278,3 +282,5 @@ class TestMatrix3x3 : public Test {
};
}
#endif

27
test/tests/mathematics/TestQuaternion.h
View File

@ -27,14 +27,10 @@
#ifndef TEST_QUATERNION_H
#define TEST_QUATERNION_H
#endif
// Libraries
#include "../../Test.h"
#include "../../../src/mathematics/Quaternion.h"
using namespace reactphysics3d;
/// Reactphysics3D namespace
namespace reactphysics3d {
@ -124,6 +120,12 @@ class TestQuaternion : public Test {
quaternion.setToZero();
test(quaternion == Quaternion(0, 0, 0, 0));
// Tes the methods to get or set to identity
Quaternion identity1(1, 2, 3, 4);
identity1.setToIdentity();
test(identity1 == Quaternion(0, 0, 0, 1));
test(Quaternion::identity() == Quaternion(0, 0, 0, 1));
// Test the method to get the vector (x, y, z)
Vector3 v = mQuaternion1.getVectorV();
test(v.x == mQuaternion1.x);
@ -137,9 +139,12 @@ class TestQuaternion : public Test {
test(conjugate.z == -mQuaternion1.z);
test(conjugate.w == mQuaternion1.w);
// Test the inverse method
Quaternion inverse = mQuaternion1.getInverse();
Quaternion product = mQuaternion1 * inverse;
// Test the inverse methods
Quaternion inverse1 = mQuaternion1.getInverse();
Quaternion inverse2(mQuaternion1);
inverse2.inverse();
test(inverse2 == inverse1);
Quaternion product = mQuaternion1 * inverse1;
test(approxEqual(product.x, mIdentity.x, decimal(10e-6)));
test(approxEqual(product.y, mIdentity.y, decimal(10e-6)));
test(approxEqual(product.z, mIdentity.z, decimal(10e-6)));
@ -192,11 +197,17 @@ class TestQuaternion : public Test {
Quaternion quat1(4, 5, 2, 10);
Quaternion quat2(-2, 7, 8, 3);
Quaternion test1 = quat1 + quat2;
Quaternion test11(-6, 52, 2, 8);
test11 += quat1;
test(test1 == Quaternion(2, 12, 10, 13));
test(test11 == Quaternion(-2, 57, 4, 18));
// Test substraction
Quaternion test2 = quat1 - quat2;
Quaternion test22(-73, 62, 25, 9);
test22 -= quat1;
test(test2 == Quaternion(6, -2, -6, 7));
test(test22 == Quaternion(-77, 57, 23, -1));
// Test multiplication with a number
Quaternion test3 = quat1 * 3.0;
@ -229,3 +240,5 @@ class TestQuaternion : public Test {
};
}
#endif

8
test/tests/mathematics/TestTransform.h
View File

@ -27,14 +27,10 @@
#ifndef TEST_TRANSFORM_H
#define TEST_TRANSFORM_H
#endif
// Libraries
#include "../../Test.h"
#include "../../../src/mathematics/Transform.h"
using namespace reactphysics3d;
/// Reactphysics3D namespace
namespace reactphysics3d {
@ -114,7 +110,7 @@ class TestTransform : public Test {
/// Test the inverse
void testInverse() {
Transform inverseTransform = mTransform1.inverse();
Transform inverseTransform = mTransform1.getInverse();
Vector3 vector(2, 3, 4);
Vector3 tempVector = mTransform1 * vector;
Vector3 tempVector2 = inverseTransform * tempVector;
@ -216,3 +212,5 @@ class TestTransform : public Test {
};
}
#endif

208
test/tests/mathematics/TestVector2.h Normal file
View File

@ -0,0 +1,208 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 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 TEST_VECTOR2_H
#define TEST_VECTOR2_H
// Libraries
#include "../../Test.h"
#include "../../../src/mathematics/Vector2.h"
/// Reactphysics3D namespace
namespace reactphysics3d {
// Class TestVector2
/**
* Unit test for the Vector2 class
*/
class TestVector2 : public Test {
private :
// ---------- Atributes ---------- //
/// Zero vector
Vector2 mVectorZero;
// Vector (3, 4)
Vector2 mVector34;
public :
// ---------- Methods ---------- //
/// Constructor
TestVector2() : mVectorZero(0, 0), mVector34(3, 4) {}
/// Run the tests
void run() {
testConstructors();
testLengthMethods();
testDotProduct();
testOthersMethods();
testOperators();
}
/// Test the constructors, getter and setter
void testConstructors() {
// Test constructor
test(mVectorZero.x == 0.0);
test(mVectorZero.y == 0.0);
test(mVector34.x == 3.0);
test(mVector34.y == 4.0);
// Test copy-constructor
Vector2 newVector(mVector34);
test(newVector.x == 3.0);
test(newVector.y == 4.0);
// Test method to set values
Vector2 newVector2;
newVector2.setAllValues(decimal(6.1), decimal(7.2));
test(approxEqual(newVector2.x, decimal(6.1)));
test(approxEqual(newVector2.y, decimal(7.2)));
// Test method to set to zero
newVector2.setToZero();
test(newVector2 == Vector2(0, 0));
}
/// Test the length, unit vector and normalize methods
void testLengthMethods() {
// Test length methods
test(mVectorZero.length() == 0.0);
test(mVectorZero.lengthSquare() == 0.0);
test(Vector2(1, 0).length() == 1.0);
test(Vector2(0, 1).length() == 1.0);
test(mVector34.lengthSquare() == 25.0);
// Test unit vector methods
test(Vector2(1, 0).isUnit());
test(Vector2(0, 1).isUnit());
test(!mVector34.isUnit());
test(Vector2(5, 0).getUnit() == Vector2(1, 0));
test(Vector2(0, 5).getUnit() == Vector2(0, 1));
test(!mVector34.isZero());
test(mVectorZero.isZero());
// Test normalization method
Vector2 mVector10(1, 0);
Vector2 mVector01(0, 1);
Vector2 mVector50(5, 0);
Vector2 mVector05(0, 5);
mVector10.normalize();
mVector01.normalize();
mVector50.normalize();
mVector05.normalize();
test(mVector10 == Vector2(1, 0));
test(mVector01 == Vector2(0, 1));
test(mVector50 == Vector2(1, 0));
test(mVector05 == Vector2(0, 1));
}
/// Test the dot product
void testDotProduct() {
// Test the dot product
test(Vector2(5, 0).dot(Vector2(0, 8)) == 0);
test(Vector2(5, 8).dot(Vector2(0, 0)) == 0);
test(Vector2(12, 45).dot(Vector2(0, 0)) == 0);
test(Vector2(5, 7).dot(Vector2(5, 7)) == 74);
test(Vector2(3, 6).dot(Vector2(-3, -6)) == -45);
test(Vector2(2, 3).dot(Vector2(-7, 4)) == -2);
test(Vector2(4, 3).dot(Vector2(8, 9)) == 59);
}
/// Test others methods
void testOthersMethods() {
// Test the method that returns the absolute vector
test(Vector2(4, 5).getAbsoluteVector() == Vector2(4, 5));
test(Vector2(-7, -24).getAbsoluteVector() == Vector2(7, 24));
// Test the method that returns the minimal element
test(Vector2(6, 35).getMinAxis() == 0);
test(Vector2(564, 45).getMinAxis() == 1);
test(Vector2(98, 23).getMinAxis() == 1);
test(Vector2(-53, -25).getMinAxis() == 0);
// Test the method that returns the maximal element
test(Vector2(6, 35).getMaxAxis() == 1);
test(Vector2(7, 537).getMaxAxis() == 1);
test(Vector2(98, 23).getMaxAxis() == 0);
test(Vector2(-53, -25).getMaxAxis() == 1);
}
/// Test the operators
void testOperators() {
// Test the [] operator
test(mVector34[0] == 3);
test(mVector34[1] == 4);
// Assignment operator
Vector2 newVector(6, 4);
newVector = Vector2(7, 8);
test(newVector == Vector2(7, 8));
// Equality, inequality operators
test(Vector2(5, 7) == Vector2(5, 7));
test(Vector2(63, 64) != Vector2(63, 84));
test(Vector2(63, 64) != Vector2(12, 64));
// Addition, substraction
Vector2 vector1(6, 33);
Vector2 vector2(7, 68);
test(Vector2(63, 24) + Vector2(3, 4) == Vector2(66, 28));
test(Vector2(63, 24) - Vector2(3, 4) == Vector2(60, 20));
vector1 += Vector2(5, 10);
vector2 -= Vector2(10, 21);
test(vector1 == Vector2(11, 43));
test(vector2 == Vector2(-3, 47));
// Multiplication, division
Vector2 vector3(6, 33);
Vector2 vector4(15, 60);
test(Vector2(63, 24) * 3 == Vector2(189, 72));
test(3 * Vector2(63, 24) == Vector2(189, 72));
test(Vector2(14, 8) / 2 == Vector2(7, 4));
vector3 *= 10;
vector4 /= 3;
test(vector3 == Vector2(60, 330));
test(vector4 == Vector2(5, 20));
// Negative operator
Vector2 vector5(-34, 5);
Vector2 negative = -vector5;
test(negative == Vector2(34, -5));
}
};
}
#endif

6
test/tests/mathematics/TestVector3.h
View File

@ -26,14 +26,10 @@
#ifndef TEST_VECTOR3_H
#define TEST_VECTOR3_H
#endif
// Libraries
#include "../../Test.h"
#include "../../../src/mathematics/Vector3.h"
using namespace reactphysics3d;
/// Reactphysics3D namespace
namespace reactphysics3d {
@ -232,3 +228,5 @@ class TestVector3 : public Test {
};
}
#endif