diff --git a/src/demo/Camera.cpp b/src/demo/Camera.cpp new file mode 100644 index 00000000..7c1f969f --- /dev/null +++ b/src/demo/Camera.cpp @@ -0,0 +1,34 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +// Libraries +#include "Camera.h" + +// Initialization of static variables +double Camera::speed=0.5; + +// Constructor of the camera +Camera::Camera() { + +} + +// Destructor of the camera +Camera::~Camera() { + +} diff --git a/src/demo/Camera.h b/src/demo/Camera.h new file mode 100644 index 00000000..badc8752 --- /dev/null +++ b/src/demo/Camera.h @@ -0,0 +1,91 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +#ifndef CAMERA_H +#define CAMERA_H + +// Libraries +#include "../reactphysics3d/reactphysics3d.h" // We want the mathematics stuff of reactphysics3d + +// Namespaces +using namespace reactphysics3d; + +// Class Camera (abstract class) +// In the project we will use two different camera. This is the superclass of the two +// cameras. We will use a OutSideCamera that move arround the scene and an OnBoardCamera +// that will simulate the deplacement of a viewer inside the scene +class Camera { + protected : + Vector3D position; // Position of the camera + Vector3D lookAtPoint; // Point where the camera is looking + Vector3D viewVector; // Vector from the camera position to the view point + static double speed; // Speed movement of the camera + + public : + Camera(); // Constructor + Camera(const Camera& camera); // Copy-constructor + virtual ~Camera(); // Destructor + virtual Vector3D getPosition() const; // Get the position of the camera + virtual void setPosition(const Vector3D& pos); // Set the position of the camera + virtual Vector3D getLookAtPoint() const; // Return the point where the camera is looking + virtual Vector3D getViewVector() const; // Return the view vector of the camera + virtual void updateViewVector()=0; // Update the view vector of the camera + static void increaseSpeed(); // Increase the speed of camera movement + static void decreaseSpeed(); // Decrease the speed of camera movement +}; + +// Get the position of the camera (inline function) +inline Vector3D Camera::getPosition() const { + // Return the position of the camera + return position; +} + +// Set the position of the camera (inline function) +inline void Camera::setPosition(const Vector3D& pos) { + // Set the position of the camera + position = pos; +} + +// Return the point where the camera is looking (inline function) +inline Vector3D Camera::getLookAtPoint() const { + return lookAtPoint; +} + +// Return the view vector of the camera (inline function) +inline Vector3D Camera::getViewVector() const { + return viewVector; +} + +// Increase the speed movement of the camera (inline function) +inline void Camera::increaseSpeed() +{ + speed= speed+0.05; + if(speed>1) + speed=1; +} + +// Decrease the speep movement of the camera (inline function) +inline void Camera::decreaseSpeed() +{ + speed = speed-0.05; + if(speed<=0) + speed=0.005; +} + +#endif diff --git a/src/demo/Context.cpp b/src/demo/Context.cpp new file mode 100755 index 00000000..e80f0a6a --- /dev/null +++ b/src/demo/Context.cpp @@ -0,0 +1,111 @@ +/**************************************************************************** +* Copyright (C) 2009 Daniel Chappuis * +**************************************************************************** +* This file is part of ReactPhysics3D. * +* * +* ReactPhysics3D is free software: you can redistribute it and/or modify * +* it under the terms of the GNU Lesser General Public License as published * +* by the Free Software Foundation, either version 3 of the License, or * +* (at your option) any later version. * +* * +* ReactPhysics3D is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU Lesser General Public License for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with ReactPhysics3D. If not, see . * +***************************************************************************/ + +// Libraries +#include "Context.h" +#include "../reactphysics3d/reactphysics3d.h" +#include +#include + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor of the class Context +Context::Context() { + + /* + Cube* cube1 = new Cube(Vector3D(0.0, 10.0, 0.0), Quaternion(1.0, 1.0, 0.0, 0.0), 3.0, Kilogram(1.0)); + Cube* cube2 = new Cube(Vector3D(0.0, 0.0, 0.0), Quaternion(1.0, 1.0, 0.0, 0.0), 3.0, Kilogram(1.0)); + //cube1->getRigidBody()->setLinearVelocity(Vector3D(0.0, 0.0, 0.0)); + //cube2->getRigidBody()->setLinearVelocity(Vector3D(0.0, 0.0, 0.0)); + cube2->getRigidBody()->setIsMotionEnabled(false); + cube1->getRigidBody()->setRestitution(0.5); + cube2->getRigidBody()->setRestitution(0.5); + */ + + + for (int i=20; i>1; i=i-3) { + Cube* cube = new Cube(Vector3D(3.0, i, i*0.2), Quaternion(1.0, 1.0, 0.0, 0.0), 2.0, Kilogram(1.0)); + cube->getRigidBody()->setRestitution(0.7); + addObject(cube); + } + + Plane* plane1 = new Plane(Vector3D(0.0, 0.0, 0.0), Quaternion(0.0, 1.0, 0.0 , 0.0), 20.0, 30.0, Vector3D(-1.0, 0.0, 0.0), Vector3D(0.0, 0.0, 1.0), Kilogram(1.0)); + plane1->getRigidBody()->setRestitution(0.7); + plane1->getRigidBody()->setIsMotionEnabled(false); + addObject(plane1); + + + /* + Cube* cube1 = new Cube(Vector3D(4.0, 11.0, 5.0), Quaternion(1.0, 0.3, 0.8, 0.0), 2.0, Kilogram(3.0)); + //Cube* cube2 = new Cube(Vector3D(3.0, 9, 3.0), Quaternion(1.0, 1.0, 0.0, 0.0), 2.0, Kilogram(2.0)); + cube1->getRigidBody()->setRestitution(0.4); + //cube2->getRigidBody()->setRestitution(0.4); + + //Cube* cube3 = new Cube(Vector3D(5.0, 13, 0.0), Quaternion(1.0, 1.0, 0.3, 0.0), 2.0, Kilogram(1.0)); + //cube3->getRigidBody()->setRestitution(0.8); + + Plane* plane1 = new Plane(Vector3D(0.0, 0.0, 0.0), Quaternion(0.0, 1.0, 0.2, 0.0), 20.0, 30.0, Vector3D(-1.0, 0.0, 0.0), Vector3D(0.0, 0.0, 1.0), Kilogram(1.0)); + plane1->getRigidBody()->setRestitution(0.4); + plane1->getRigidBody()->setIsMotionEnabled(false); + + + addObject(cube1); + //addObject(cube2); + //addObject(cube3); + addObject(plane1); + */ + +} + + +// Destructor of the class Context +Context::~Context() { + // Delete all the objects in vectObjects + for(unsigned int i=0; i. * + ***************************************************************************/ + +#ifndef CONTEXT_H +#define CONTEXT_H + +// Libraries +#include "Objects.h" +#include + +// Class Context +class Context { + private : + std::vector vectObjects; // Vector of Objects in the simulation + + public : + Context(); // Constructor of the class + ~Context(); // Destructor of the class + int getNbObjects() const; // Return the number of objects in the context + Object& getObject(int objectIndex) const; // Get an object from the context + void addObject(Object* object); // Add an object into the context + void removeObject(int objectIndex); // Remove an object from the context +}; + +// Method (inline) to get the number of objects in the context +inline int Context::getNbObjects() const { + return vectObjects.size(); +} + +#endif diff --git a/src/demo/Objects.cpp b/src/demo/Objects.cpp new file mode 100755 index 00000000..826a54a0 --- /dev/null +++ b/src/demo/Objects.cpp @@ -0,0 +1,152 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +// Libraries +#include "Objects.h" + +//#include // To avoid an error due to the #include +#include +#include +#include +#include +#include + +// ----- Class Object ----- // + +// Constructor of the class Object +Object::Object(const Vector3D& position, const Quaternion& orientation, const Kilogram& mass, const Matrix3x3& inertiaTensor, const OBB& obb) + :rigidBody(new RigidBody(position, orientation, mass, inertiaTensor, obb)) { + +} + +// Destructor of the class Object +Object::~Object() { + // Delete the rigid body object + delete rigidBody; +} + +// Return the pointer to the rigid body +RigidBody* Object::getRigidBody() { + return rigidBody; +} + +// ----- Class Cube ----- // + +// Static attributes +const Matrix3x3 Cube::inertiaTensor; + +// Constructor of the class Cube +Cube::Cube(const Vector3D& position, const Quaternion& orientation, float size, const Kilogram& mass) + :Object(position, orientation, mass, Matrix3x3(1.0/12.0*mass.getValue()*2*size*size, 0.0, 0.0, + 0.0, 1.0/12.0*mass.getValue()*2*size*size, 0.0, + 0.0, 0.0, 1.0/12.0*mass.getValue()*2*size*size), OBB(position, Vector3D(1.0, 0.0, 0.0), Vector3D(0.0, 1.0, 0.0), Vector3D(0.0, 0.0, 1.0), + size/2.0, size/2.0, size/2)) { + this->size = size; +} + +// Destructor of the classe Cube +Cube::~Cube() { + +} + +// Method to draw the cube +void Cube::draw() const { + + // Get the interpolated state of the rigid body + BodyState state = rigidBody->getInterpolatedState(); + + // Position of the cube + double x = state.getPosition().getX(); + double y = state.getPosition().getY(); + double z = state.getPosition().getZ(); + + // Orientation of the cube + Vector3D orientationAxis; + double orientationAngle; + state.getOrientation().getRotationAngleAxis(orientationAngle, orientationAxis); + + // Translation of the cube to its position + glTranslatef(x, y, z); + + // Rotation of the cube according to its orientation + glRotatef(orientationAngle/PI*180.0, orientationAxis.getX(), orientationAxis.getY(), orientationAxis.getZ()); + + // Draw the cube + glutSolidCube(size); +} + + +// ----- Class Plane ----- // + +// Constructor of the class Plane +Plane::Plane(const Vector3D& position, const Quaternion& orientation, float width, float height, const Vector3D& d1, const Vector3D& d2, const Kilogram& mass) + :Object(position, orientation, mass, Matrix3x3(1.0/12.0*mass.getValue()*height*height, 0.0, 0.0, + 0.0, 1.0/12.0*mass.getValue()*(width*width+height*height), 0.0, + 0.0, 0.0, 1.0/12.0*mass.getValue()*width*width), OBB(position, Vector3D(1.0, 0.0, 0.0), Vector3D(0.0, 1.0, 0.0), Vector3D(0.0, 0.0, 1.0), + width/2, 0.5, height/2)) { // TODO : Change the height of the OBB + this->width = width; + this->height = height; + this->d1 = d1; + this->d2 = d2; + + // By default Planes in the demo cannot move + rigidBody->setIsMotionEnabled(false); + + // Compute the unit normal vector of the plane by a cross product + normalVector = d1.crossProduct(d2).getUnit(); +} + +// Destructor of the class Plane +Plane::~Plane() { + +} + +// Method used to draw the plane +void Plane::draw() const { + + // Get the interpolated state of the rigid body + BodyState state = rigidBody->getInterpolatedState(); + + // Get the position of the rigid body + double x = state.getPosition().getX(); + double y = state.getPosition().getY(); + double z = state.getPosition().getZ(); + + // Translation of the cube to its position + glTranslatef(x, y, z); + + float halfWidth = width / 2.0; + float halfHeight = height / 2.0; + + // Draw the plane + glBegin(GL_POLYGON); + glColor3f(1.0, 1.0, 1.0); + glVertex3f(x + d1.getX() * halfWidth + d2.getX() * halfHeight , y + d1.getY() * halfWidth + d2.getY() * halfHeight + , z + d1.getZ() * halfWidth + d2.getZ() * halfHeight); + glNormal3f(normalVector.getX(), normalVector.getY(), normalVector.getZ()); + glVertex3f(x + d1.getX() * halfWidth - d2.getX() * halfHeight , y + d1.getY() * halfWidth - d2.getY() * halfHeight + , z + d1.getZ() * halfWidth - d2.getZ() * halfHeight); + glNormal3f(normalVector.getX(), normalVector.getY(), normalVector.getZ()); + glVertex3f(x - d1.getX() * halfWidth - d2.getX() * halfHeight , y - d1.getY() * halfWidth - d2.getY() * halfHeight + , z - d1.getZ() * halfWidth - d2.getZ() * halfHeight); + glNormal3f(normalVector.getX(), normalVector.getY(), normalVector.getZ()); + glVertex3f(x - d1.getX() * halfWidth + d2.getX() * halfHeight , y - d1.getY() * halfWidth + d2.getY() * halfHeight + , z - d1.getZ() * halfWidth + d2.getZ() * halfHeight); + glEnd(); +} diff --git a/src/demo/Objects.h b/src/demo/Objects.h new file mode 100755 index 00000000..06a4c4f9 --- /dev/null +++ b/src/demo/Objects.h @@ -0,0 +1,76 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +#ifndef OBJECTS_H +#define OBJECTS_H + +// Libraries +#include "../reactphysics3d/reactphysics3d.h" + +// Namespaces +using namespace reactphysics3d; + +/* + Here we define all the objects that can appear in the simulation like cube, sphere, plane, ... +*/ + +// ----- Class Object (abstract) ----- // +// Represent an object of the simulation +class Object { + protected : + RigidBody* rigidBody; // Rigid Body that represents the object + + public : + Object(const Vector3D& position, const Quaternion& orientation, const Kilogram& mass, const Matrix3x3& inertiaTensor, const OBB& obb); // Constructor of the class Object + virtual ~Object(); // Destructor of the class Object + + virtual void draw() const =0; // pure virtual method to draw the object + RigidBody* getRigidBody(); // Return the pointer to the rigid body +}; + +// ----- Class Cube ----- // +// Represente a Box in the simulation +class Cube : public Object { + private : + float size; // Size of a side in the box + static const Matrix3x3 inertiaTensor; // Inertia tensor of a box + public : + Cube(const Vector3D& position, const Quaternion& orientation, float size, const Kilogram& mass); // Constructor of the class cube + virtual ~Cube(); // Destructor of the class cube + virtual void draw() const; // Method to draw the cube +}; + + +// ----- Class Plane ---- // +// Represent a plane in the simulation +class Plane : public Object { + private : + float width; // Width of the plane + float height; // Height of the plane + Vector3D d1; // Unit vector in the plane + Vector3D d2; // Unit vector in the plane + Vector3D normalVector; // Unit normal vector of the plane + + public : + Plane(const Vector3D& position, const Quaternion& orientation, float width, float height, const Vector3D& d1, const Vector3D& d2, const Kilogram& mass); // Constructor of the class Plane + virtual ~Plane(); // Destructor of the class Plane + virtual void draw() const; // Method to draw the plane +}; + +#endif diff --git a/src/demo/OutSideCamera.cpp b/src/demo/OutSideCamera.cpp new file mode 100644 index 00000000..9ea2bd8f --- /dev/null +++ b/src/demo/OutSideCamera.cpp @@ -0,0 +1,112 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +// Libraries +#include "OutSideCamera.h" +#include + +// Constructor +OutSideCamera::OutSideCamera() { + + // Initialize the attributes + heightFromFloor = 10.0; + horizontalAngleRotation = 45; + verticalAngleRotation = 45; + distanceFromOrigin = 30.0; + lookAtPoint.setAllValues(0.0, 0.0, 0.0); + + // Update the position of the camera + updatePosition(); + + // Update the view vector of the camera + updateViewVector(); +} + +// Destructor +OutSideCamera::~OutSideCamera() { + +} + +// Compute the new position of the camera +void OutSideCamera::updatePosition() { + + // Compute the floor distance from origin + double floorDistance = distanceFromOrigin * cos(PI/180.0 * verticalAngleRotation); + + // Update the position of the camera + position.setAllValues(floorDistance*cos(PI/180.0 * horizontalAngleRotation), distanceFromOrigin*sin(PI/180*verticalAngleRotation), + floorDistance*sin(PI/180.0 * horizontalAngleRotation)); +} + +// Set the camera rotation angle and update the position of the camera +void OutSideCamera::modifyHorizontalAngleRotation(int screenDistance, float fps) { + + // Update the horizontal rotation angle of the camera + horizontalAngleRotation = (horizontalAngleRotation + int(screenDistance * 700.0 / fps)) % 360; + + // Update the position and the view vector of the camera + updatePosition(); + updateViewVector(); +} + +// Set the vertical camera rotation angle +void OutSideCamera::modifyVerticalAngleRotation(int screenDistance, float fps) { + + // Update the vertical rotation angle of the camera + verticalAngleRotation = verticalAngleRotation + (screenDistance * 700.0 / fps); + + // Vertical angle limits + if (verticalAngleRotation > 89) { + verticalAngleRotation = 89; + } + if (verticalAngleRotation < 1) { + verticalAngleRotation = 1; + } + + // Update the position and the view vector of the camera + updatePosition(); + updateViewVector(); +} + +// Increase the distance from origine of the camera (used for the zoom) +void OutSideCamera::increaseDistance(float fps) { + + // Increase the distance from origin + distanceFromOrigin = distanceFromOrigin + (speed * 60 / fps); + + // Update the position and the view vector of the camera + updatePosition(); + updateViewVector(); +} + +// Decrease the distance from origine of the camera (used for the zoom) +void OutSideCamera::decreaseDistance(float fps) { + + // Decrease the distance from origin + distanceFromOrigin = distanceFromOrigin - (speed * 60 / fps); + + // Limit condition + if(distanceFromOrigin < 1) { + distanceFromOrigin=1; + } + + // Update the position and the view vector of the camera + updatePosition(); + updateViewVector(); +} diff --git a/src/demo/OutSideCamera.h b/src/demo/OutSideCamera.h new file mode 100644 index 00000000..6c8f4ba6 --- /dev/null +++ b/src/demo/OutSideCamera.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +#ifndef OUTSIDE_CAMERA_H +#define OUTSIDE_CAMERA_H + +// Libraries +#include "Camera.h" + +// ---- Class OutSideCamera ---- +// This camera can be move everywhere around the scene to obtain a outside view of the house +class OutSideCamera : public Camera { + private : + double heightFromFloor; // Height from the floor + int horizontalAngleRotation; // Horizontal rotation angle (in degree) + int verticalAngleRotation; // Vertical rotation angle (in degree) + double distanceFromOrigin; // Distance of the camera from the origin (used to zoom) + + public : + OutSideCamera(); // Constructor + OutSideCamera(const OutSideCamera& camera); // Copy-constructor + virtual ~OutSideCamera(); // Destructor + void updatePosition(); // Compute the new position of the camera + void updateViewVector(); // Update the view vector of the camera + double getHeightFromFloor() const; // Get the height of the camera from the floor + void setHeightFromFloor(double height); // Set the height of the camera from the floor + void modifyHorizontalAngleRotation(int screenDistance, float fps); // Modify the horizontal camera rotation angle + void modifyVerticalAngleRotation(int screenDistance, float fps); // Modify the vertical camera rotation angle + void increaseDistance(float fps); // Increase the distance of the camera from the origin + void decreaseDistance(float fps); // Decrease the distance of the camera from the origin +}; + +// Compute the new view vector of the camera (inline function) +inline void OutSideCamera::updateViewVector() { + viewVector = Vector3D(0.0, 0.0, 0.0) - position; +} + +// Get the height of the camera from the floor (inline function) +inline double OutSideCamera::getHeightFromFloor() const { + return heightFromFloor; +} + +// Set the height of the camera from the floor (inline function) +inline void OutSideCamera::setHeightFromFloor(double height) { + heightFromFloor = height; +} + +#endif diff --git a/src/demo/ReactDemo.h b/src/demo/ReactDemo.h new file mode 100755 index 00000000..e2e17d05 --- /dev/null +++ b/src/demo/ReactDemo.h @@ -0,0 +1,27 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +#ifndef REACT_DEMO_H +#define REACT_DEMO_H + +// Constants +const int WINWIDTH = 640; // Width of the OpenGL windows +const int WINHEIGHT = 480; // Height of the OpenGL windows + +#endif diff --git a/src/demo/Scene.cpp b/src/demo/Scene.cpp new file mode 100755 index 00000000..a8191104 --- /dev/null +++ b/src/demo/Scene.cpp @@ -0,0 +1,151 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +// Librairies +#include "Scene.h" +#include "Objects.h" +#include +#include + +// Constructor of the class Scene +Scene::Scene(rp3d::PhysicsWorld* world) { + + this->world = world; + + // Initialise the material specular color + mat_specular[0] = 1.0; + mat_specular[1] = 1.0; + mat_specular[2] = 1.0; + mat_specular[3] = 1.0; + + // Initialize the material shininess + mat_shininess[0] = 50.0; + + // Initialise the light source position + light_position[0] = 20.0; + light_position[1] = 9.0; + light_position[2] = 15.0; + light_position[3] = 0.0; + + // Initialise the ambient color of the light + ambient_color[0] = 1.0; + ambient_color[1] = 1.0; + ambient_color[2] = 1.0; + ambient_color[3] = 0.7; + + // Initialise the diffuse light color + white_light[0] = 1.0; + white_light[1] = 1.0; + white_light[2] = 1.0; + white_light[3] = 1.0; +} + +// Destructor of the class Scene +Scene::~Scene() { + +} + +// Init method +void Scene::init() { + glClearColor(0.0, 0.0, 0.0, 0.0); // Select the color for the background + glShadeModel(GL_SMOOTH); + glClearDepth(1.0); + + // Lighting settings + glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular); // Specular color of the material + glMaterialfv(GL_FRONT, GL_SHININESS, mat_shininess); // Shininess of the material + glLightfv(GL_LIGHT0, GL_POSITION, light_position); // Position of the light source + glLightfv(GL_LIGHT0, GL_AMBIENT, ambient_color); // Ambient color of the light + glLightfv(GL_LIGHT0, GL_DIFFUSE, white_light); // Diffuse color of the light + glLightfv(GL_LIGHT0, GL_SPECULAR, white_light); // Specular color of the light + + glEnable(GL_LIGHTING); // Activate the lighting + glEnable(GL_LIGHT0); // Activate a light source + glEnable(GL_DEPTH_TEST); // Activate the Depth buffer + //glEnable(GL_CULL_FACE); +} + +// Display method +void Scene::display(const Context& context) const { + glClearColor(0,0,0,0); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glMatrixMode(GL_MODELVIEW); + glLoadIdentity(); + + // Define the position and the direction of the camera + //gluLookAt(30,10,0,0,0,0,0,1,0); + double x = outsideCamera.getPosition().getX(); + double y = outsideCamera.getPosition().getY(); + double z = outsideCamera.getPosition().getZ(); + gluLookAt(x,y,z,0,0,0,0,1,0); + + // Draw all objects in the context + for(int i=0; igetOBB()->draw(); + + // Remove the matrix on the top of the matrix stack + glPopMatrix(); + + + } + + // Draw all the contact points + for (std::vector::iterator it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); ++it) { + RigidBody* rigidBody1 = dynamic_cast((*it)->getBody1()); + RigidBody* rigidBody2 = dynamic_cast((*it)->getBody2()); + //rigidBody1->setIsMotionEnabled(false); + //rigidBody2->setIsMotionEnabled(false); + + Contact* contact = dynamic_cast((*it)); + assert(contact != 0); + + // Draw the contact points + glPushMatrix(); + glTranslatef(contact->getPoint().getX(), contact->getPoint().getY(), contact->getPoint().getZ()); + contact->draw(); + glPopMatrix(); + } + + glFlush(); + + // Swap the buffers + SDL_GL_SwapBuffers(); +} + +// Reshape the window +void Scene::reshape(int width, int height) { + glViewport(0,0,width,height); + glMatrixMode(GL_PROJECTION); // Specify the matrix that will be modified + glLoadIdentity(); // Load the identity matrix before the transformations + gluPerspective(45.0, (float) width/height, 0.1f, 150.0f); + glMatrixMode(GL_MODELVIEW); // Specify the matrix that will be modified + glLoadIdentity(); // Load the identity matrix before the transformations +} diff --git a/src/demo/Scene.h b/src/demo/Scene.h new file mode 100755 index 00000000..dc9409a9 --- /dev/null +++ b/src/demo/Scene.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +#ifndef SCENE_H +#define SCENE_H + +// Libraries +#include "Context.h" +#include "OutSideCamera.h" +#include +#include // Used only to draw cubes +#include +#include + + +// Scene class +class Scene { + private : + GLfloat mat_specular[4]; // Material specular light color + GLfloat mat_shininess[1]; // Material shininess + GLfloat light_position[4]; // Position of the light source + GLfloat ambient_color[4]; // Ambient color of the light + GLfloat white_light[4]; // White light color + OutSideCamera outsideCamera; // OutSide camera (Camera that can move around the scene) + rp3d::PhysicsWorld* world; //"Pointer to the physics world + + public : + Scene(rp3d::PhysicsWorld* world); // constructor of the class + ~Scene(); // Destructor of the class + void init(); // Initialize the values of OpenGL + void display(const Context& context) const; // display the scene + void reshape(int width, int height); // Reshape the window + OutSideCamera& getOutSideCamera() ; // Return a reference to the outside camera + float getCameraAngle1() const; // Return the angle of the camera + float getCameraAngle2() const; // Return the angle of the camera + void setCameraAngle1(float angle); // Set the angle of the camera + void setCameraAngle2(float angle); // Set the angle of the camera +}; + +// Return a reference to the camera +inline OutSideCamera& Scene::getOutSideCamera() { + return outsideCamera; +} + +#endif diff --git a/src/demo/Simulation.cpp b/src/demo/Simulation.cpp new file mode 100755 index 00000000..54cac320 --- /dev/null +++ b/src/demo/Simulation.cpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +// Libraries +#include "Simulation.h" +#include "ReactDemo.h" +#include + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor of the class Simulation +Simulation::Simulation() + :world(new PhysicsWorld(Vector3D(0.0, -9.8, 0.0))), engine(world, Time(0.01)), scene(this->world) { // TODO : Change the timestep here after debugging + simRunning = false; + mouseButtonPressed = false; + nbFrame = 0; + lastFrameTime = 0.0; + fps = 0.0; +} + +// Destructor of the class Simulation +Simulation::~Simulation() { + // Delete the physics world object + delete world; +} + +// Method to start the simulation +void Simulation::start() { + // Initialisation of the OpenGL settings for the scene + scene.init(); + + // Reshape the windows for the first time + scene.reshape(WINWIDTH, WINHEIGHT); + + // Add every rigid body to the dynamic world + for (int i=0; iaddBody(context.getObject(i).getRigidBody()); + } + + // Activation of the simulation + simRunning = true; + + // Get the current time + lastFrameTime = SDL_GetTicks(); + + PhysicsEngine* pEngine = &engine; + + // Initialize the display time + pEngine->initializeDisplayTime(Time(SDL_GetTicks()/1000.0)); + + // Start the physics simulation + pEngine->start(); + + //double time = 1.0; + + // Main loop of the simulation + while(simRunning) { + // Check if an SDL event occured and make the apropriate actions + checkEvents(); + + double time = SDL_GetTicks()/1000.0; + //time += 0.01; + + //std::cout << "************************************************* Time : " << time << std::endl; + + // Update the display time + pEngine->updateDisplayTime(Time(time)); + + // Update the physics + pEngine->update(); + + // Display the actual scene + scene.display(context); + + // Compute the fps (framerate) + computeFps(); + //std::cout << "FPS : " << fps << std::endl; + + /* + BodyState state = context.getObject(0).getRigidBody()->getInterpolatedState(); + Vector3D velocity = context.getObject(0).getRigidBody()->getInterpolatedState().getAngularVelocity(); + //std::cout << "Velocity 0 : " << velocity.getX() << ", " << velocity.getY() << ", " << velocity.getZ() << ")" << std::endl; + double x = state.getPosition().getX(); + double y = state.getPosition().getY(); + double z = state.getPosition().getZ(); + std::cout << "Position Cube 0 : (" << x << ", " << y << ", " << z << ")" << std::endl; + std::cout << "angular velocity 0 : " << velocity.length() << std::endl;; + + BodyState state1 = context.getObject(1).getRigidBody()->getInterpolatedState(); + Vector3D velocity1 = context.getObject(1).getRigidBody()->getInterpolatedState().getAngularVelocity(); + //std::cout << "Velocity 1 : " << velocity1.getX() << ", " << velocity1.getY() << ", " << velocity1.getZ() << ")" << std::endl; + double x1 = state1.getPosition().getX(); + double y1 = state1.getPosition().getY(); + double z1 = state1.getPosition().getZ(); + std::cout << "Position Cube 1 : (" << x1 << ", " << y1 << ", " << z1 << ")" << std::endl; + std::cout << "angular velocity 1 : " << velocity1.length() << std::endl; + + BodyState state2 = context.getObject(2).getRigidBody()->getInterpolatedState(); + Quaternion velocity2 = context.getObject(2).getRigidBody()->getInterpolatedState().getOrientation(); + //std::cout << "Velocity 2 : " << velocity2.getX() << ", " << velocity2.getY() << ", " << velocity2.getZ() << ")" << std::endl; + double x2 = state2.getPosition().getX(); + double y2 = state2.getPosition().getY(); + double z2 = state2.getPosition().getZ(); + std::cout << "Position Cube 2: (" << x2 << ", " << y2 << ", " << z2 << ")" << std::endl; + std::cout << "quaternion orientation 2 : " << velocity2.getX() << ", " << velocity2.getY() << ", " << velocity2.getZ() << ", " << velocity2.getW() << ")" << std::endl;; + */ + + /* + double a; + if (time > 5.0) { + std::cin >> a; + } + */ + + } +} + +// This method checks if an events occur and call the apropriate method +void Simulation::checkEvents() { + SDL_Event event; // An SDL event + + // Zoom of the outside camera + if (SDL_GetKeyState(NULL)[SDLK_UP]) { + scene.getOutSideCamera().decreaseDistance(fps); + } + else if(SDL_GetKeyState(NULL)[SDLK_DOWN]) { + scene.getOutSideCamera().increaseDistance(fps); + } + + // Check in the stack of events + while(SDL_PollEvent(&event)) { + // Check an event + switch(event.type) { + // An QUIT event occur + case SDL_QUIT: simRunning = false; + break; + + // A keyboard key has been pushed + case SDL_KEYDOWN: // The Esc key has been pushed then we end the simulation + if (event.key.keysym.sym == SDLK_ESCAPE) + simRunning = false; + break; + + // The size of the windows changed then we reshape the windows + case SDL_VIDEORESIZE: scene.reshape(event.resize.w, event.resize.h); + break; + + // If the mouse moved + case SDL_MOUSEMOTION: if (SDL_GetMouseState(NULL, NULL)&SDL_BUTTON(1)) { + // Rotation of the outSideCamera + scene.getOutSideCamera().modifyHorizontalAngleRotation(event.motion.xrel, fps); + scene.getOutSideCamera().modifyVerticalAngleRotation(event.motion.yrel, fps); + } + } + } +} + +// Compute the framerate (fps) of the application +void Simulation::computeFps() { + + // Increment the number of frame in the last second + nbFrame++; + + // Get the current time + double currentTime = SDL_GetTicks(); + + // Compute the framerate + if (currentTime - lastFrameTime > 1000.0) { + fps = nbFrame * 1000.0/(currentTime-lastFrameTime); + lastFrameTime = currentTime; + nbFrame = 0; + } +} diff --git a/src/demo/Simulation.h b/src/demo/Simulation.h new file mode 100755 index 00000000..788ad1e9 --- /dev/null +++ b/src/demo/Simulation.h @@ -0,0 +1,50 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +#ifndef SIMULATION_H +#define SIMULATION_H + +// Librairies +#include "Context.h" +#include "Scene.h" +#include "../reactphysics3d/reactphysics3d.h" + +// Class Simulation +class Simulation { + private : + rp3d::PhysicsWorld* world; // Pointer to the collision world that contains bodies of the simulation + Scene scene; // Scene object for displaying the simulation + Context context; // Context of the simulation + rp3d::PhysicsEngine engine; // Collision engine for the physics of the simulation + bool simRunning; // True if the simulation is running and false otherwise + bool mouseButtonPressed; // True if the left mouse button is pressed + double lastFrameTime; // Last frame time + int nbFrame; // Number of frame (used to compute the framerate) + double fps; // Framerate of the application + + void computeFps(); // Compute the framerate of the application + + public : + Simulation(); // Constructor of the class + ~Simulation(); // Destructor of the class + void start(); // Start the simulation + void checkEvents(); // Check if SDL events occured and make the apropriate actions +}; + +#endif diff --git a/src/demo/main.cpp b/src/demo/main.cpp new file mode 100755 index 00000000..02120d89 --- /dev/null +++ b/src/demo/main.cpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +// Libraries +#include "../reactphysics3d/reactphysics3d.h" +#include "Simulation.h" +#include "ReactDemo.h" + +#include +#include + +// Prototypes +int initSDL(); + +// global variables +SDL_Surface * pScreen; // Pointer to the SDL windows + +// Main function +int main(int argc, char** argv) { + + if(initSDL() > 0) { + // If initSDL return an error then we exit the program + return EXIT_FAILURE; + } + + // Initialize the glut library. We will use glut only to display shapes like cube or sphere + glutInit(&argc, argv); + + // Create a Simulation object used to simulate a physic world + Simulation simulation; + + // Start the simulation + simulation.start(); + + std::cerr << "Fin normale du programme" << std::endl; + + // To avoid warnings notifying that argc et argv aren't used + (argc); + (argv); + + return (EXIT_SUCCESS); +} + +// Methode to initialise the SDL and the windows +int initSDL() { + // Initialisation of SDL + if( SDL_Init(SDL_INIT_VIDEO) < 0) { + std::cerr << "Echec SDL_Init : " << SDL_GetError() << std::endl; + return 1; + } + + // Select the method to exit + atexit( SDL_Quit ); + + // Active double buffer mode + SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1); + + // Select the Depth Buffer size + SDL_GL_SetAttribute( SDL_GL_DEPTH_SIZE, 16); + + // Activation of displaying in windows mode + SDL_Surface* pScreen = SDL_SetVideoMode(WINWIDTH, WINHEIGHT, 32, SDL_OPENGL | SDL_GL_DOUBLEBUFFER | SDL_RESIZABLE | SDL_SWSURFACE); + + // Check that displaying is activated + if( !pScreen ) { + std::cerr << "Echec de creation de la fenetre en 640x480 : " << SDL_GetError() << std::endl; + return 1; + } + + // Define the window title and the window icon + SDL_WM_SetCaption("ReactPhysics3D Demo 0.01", NULL); + + // Get the state of the Double Buffer parameter + int nValue; + if( SDL_GL_GetAttribute(SDL_GL_DOUBLEBUFFER, &nValue) < 0) { + std::cerr << "Echec de recuperation du parametre SDL_GL_DOUBLEBUFFER : " << SDL_GetError() << std::endl; + return 1; + } + + // Check that Double Buffer mode is activated + if(nValue != 1) { + std::cerr << "Erreur : SDL_GL_DOUBLEBUFFER inactif" << std::endl; + return 1; + } + + return 0; +} diff --git a/src/reactphysics3d/body/AABB.cpp b/src/reactphysics3d/body/AABB.cpp new file mode 100644 index 00000000..03ae3443 --- /dev/null +++ b/src/reactphysics3d/body/AABB.cpp @@ -0,0 +1,114 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "AABB.h" +#include // TODO : Remove this in the final version +#include // TODO : Remove this in the final version +#include + +using namespace reactphysics3d; +using namespace std; + +// Constructor +AABB::AABB(const Vector3D& center,double extentX, double extentY, double extentZ) { + this->center = center; + + this->extent[0] = extentX; + this->extent[1] = extentY; + this->extent[2] = extentZ; + + this->originalAABBExtent[0] = extentX; + this->originalAABBExtent[1] = extentY; + this->originalAABBExtent[2] = extentZ; +} + +// Destructor +AABB::~AABB() { + +} + +// Draw the OBB (only for testing purpose) +void AABB::draw() const { + + Vector3D s1 = center + Vector3D(extent[0], extent[1], -extent[2]); + Vector3D s2 = center + Vector3D(extent[0], extent[1], extent[2]); + Vector3D s3 = center + Vector3D(-extent[0], extent[1], extent[2]); + Vector3D s4 = center + Vector3D(-extent[0], extent[1], -extent[2]); + Vector3D s5 = center + Vector3D(extent[0], -extent[1], -extent[2]); + Vector3D s6 = center + Vector3D(extent[0], -extent[1], extent[2]); + Vector3D s7 = center + Vector3D(-extent[0], -extent[1], extent[2]); + Vector3D s8 = center + Vector3D(-extent[0], -extent[1], -extent[2]); + + // Draw in red + glColor3f(1.0, 0.0, 0.0); + + // Draw the OBB + glBegin(GL_LINES); + glVertex3f(s1.getX(), s1.getY(), s1.getZ()); + glVertex3f(s2.getX(), s2.getY(), s2.getZ()); + + glVertex3f(s2.getX(), s2.getY(), s2.getZ()); + glVertex3f(s3.getX(), s3.getY(), s3.getZ()); + + glVertex3f(s3.getX(), s3.getY(), s3.getZ()); + glVertex3f(s4.getX(), s4.getY(), s4.getZ()); + + glVertex3f(s4.getX(), s4.getY(), s4.getZ()); + glVertex3f(s1.getX(), s1.getY(), s1.getZ()); + + glVertex3f(s5.getX(), s5.getY(), s5.getZ()); + glVertex3f(s6.getX(), s6.getY(), s6.getZ()); + + glVertex3f(s6.getX(), s6.getY(), s6.getZ()); + glVertex3f(s7.getX(), s7.getY(), s7.getZ()); + + glVertex3f(s7.getX(), s7.getY(), s7.getZ()); + glVertex3f(s8.getX(), s8.getY(), s8.getZ()); + + glVertex3f(s8.getX(), s8.getY(), s8.getZ()); + glVertex3f(s5.getX(), s5.getY(), s5.getZ()); + + glVertex3f(s1.getX(), s1.getY(), s1.getZ()); + glVertex3f(s5.getX(), s5.getY(), s5.getZ()); + + glVertex3f(s4.getX(), s4.getY(), s4.getZ()); + glVertex3f(s8.getX(), s8.getY(), s8.getZ()); + + glVertex3f(s3.getX(), s3.getY(), s3.getZ()); + glVertex3f(s7.getX(), s7.getY(), s7.getZ()); + + glVertex3f(s2.getX(), s2.getY(), s2.getZ()); + glVertex3f(s6.getX(), s6.getY(), s6.getZ()); + + glEnd(); +} + +// Static method that computes an AABB from a set of vertices. The "center" argument corresponds to the center of the AABB +// This method allocates a new AABB object and return a pointer to the new allocated AABB object +AABB* AABB::computeFromVertices(const vector& vertices, const Vector3D& center) { + // TODO : Implement this method; + return 0; +} + diff --git a/src/reactphysics3d/body/AABB.h b/src/reactphysics3d/body/AABB.h new file mode 100644 index 00000000..17b10c0a --- /dev/null +++ b/src/reactphysics3d/body/AABB.h @@ -0,0 +1,178 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef AABB_H +#define AABB_H + +// Libraries +#include "BroadBoundingVolume.h" +#include "../mathematics/mathematics.h" + +// ReactPhysics3D namespace +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class AABB : + This class represents a bounding volume of type "Axis Aligned + Bounding Box". It's a box where all the edges are always aligned + with the world coordinate system. The AABB is defined by a center + point and three extent size in the x,y and z directions. + ------------------------------------------------------------------- +*/ +class AABB : public BroadBoundingVolume { + protected : + Vector3D center; // Center point of the AABB + double extent[3]; // Three extents size in the x, y and z directions + double originalAABBExtent[3]; // Extents of the original AABB (this is used to update the AABB) + + public : + AABB(const Vector3D& center, double extentX, double extentY, double extentZ); // Constructor + virtual ~AABB(); // Destructor + + Vector3D getCenter() const; // Return the center point + void setCenter(const Vector3D& center); // Set the center point + Vector3D getVertex(uint index) const throw (std::invalid_argument); // Return a vertex of the AABB + double getExtent(uint index) const throw(std::invalid_argument); // Return an extent value + void setExtent(uint index, double extent) throw(std::invalid_argument); // Set an extent value + double getMinValueOnAxis(uint axis) const throw(std::invalid_argument); // Return the minimum position value on the given axis + double getMaxValueOnAxis(uint axis) const throw(std::invalid_argument); // Return the maximum position value on the given axis + bool testCollision(const AABB& aabb) const; // Return true if the current AABB is overlapping is the AABB in argument + virtual void update(const Vector3D& newCenter, const Quaternion& rotationQuaternion); // Update the oriented bounding box orientation according to a new orientation of the rigid body + virtual void draw() const; // Draw the AABB (only for testing purpose) + static AABB* computeFromVertices(const std::vector& vertices, const Vector3D& center); // Compute an AABB from a set of vertices +}; + +// Return the center point +inline Vector3D AABB::getCenter() const { + return center; +} + +// Set the center point +inline void AABB::setCenter(const Vector3D& center) { + this->center = center; +} + +// Return one of the 8 vertices of the AABB +inline Vector3D AABB::getVertex(unsigned int index) const throw (std::invalid_argument) { + // Check if the index value is valid + if (index >= 0 && index <8) { + switch(index) { + case 0 : return center + Vector3D(extent[0], extent[1], -extent[2]); + case 1 : return center + Vector3D(extent[0], extent[1], extent[2]); + case 2 : return center + Vector3D(-extent[0], extent[1], extent[2]); + case 3 : return center + Vector3D(-extent[0], extent[1], -extent[2]); + case 4 : return center + Vector3D(extent[0], -extent[1], -extent[2]); + case 5 : return center + Vector3D(extent[0], -extent[1], extent[2]); + case 6 : return center + Vector3D(-extent[0], -extent[1], extent[2]); + case 7 : return center + Vector3D(-extent[0], -extent[1], -extent[2]); + } + } + else { + // The index value is not valid, we throw an exception + throw std::invalid_argument("Exception : The index value has to be between 0 and 8"); + } +} + + +// Return an extent value +inline double AABB::getExtent(unsigned int index) const throw(std::invalid_argument) { + // Check if the index value is valid + if (index >= 0 && index <3) { + return extent[index]; + } + else { + // The index value is not valid, we throw an exception + throw std::invalid_argument("Exception : The index value has to be between 0 and 2"); + } +} + +// Set an extent value +inline void AABB::setExtent(unsigned int index, double extent) throw(std::invalid_argument) { + // Check if the index value is valid + if (index >= 0 && index <3) { + this->extent[index] = extent; + } + else { + // The index value is not valid, we throw an exception + throw std::invalid_argument("Exception : The index value has to be between 0 and 2"); + } +} + +// Return the minimum position value on the given axis +inline double AABB::getMinValueOnAxis(uint axis) const throw(std::invalid_argument) { + switch (axis) { + case 0: return center.getX() - extent[0]; + case 1: return center.getY() - extent[1]; + case 2: return center.getZ() - extent[2]; + default: // The index value is not valid, we throw an exception + throw std::invalid_argument("Exception : The index value has to be between 0 and 2"); + } +} + +// Return the maximum position value on the given axis +inline double AABB::getMaxValueOnAxis(uint axis) const throw(std::invalid_argument) { + switch (axis) { + case 0: return center.getX() + extent[0]; + case 1: return center.getY() + extent[1]; + case 2: return center.getZ() + extent[2]; + default: // The index value is not valid, we throw an exception + throw std::invalid_argument("Exception : The index value has to be between 0 and 2"); + } +} + +// Return true if the current AABB is overlapping is the AABB in argument +// Two AABB overlap if they overlap in the three x, y and z axis at the same time +inline bool AABB::testCollision(const AABB& aabb) const { + Vector3D center2 = aabb.getCenter(); + if (std::abs(center.getX() - center2.getX()) > (extent[0] + aabb.getExtent(0))) return false; + if (std::abs(center.getY() - center2.getY()) > (extent[1] + aabb.getExtent(1))) return false; + if (std::abs(center.getZ() - center2.getZ()) > (extent[2] + aabb.getExtent(2))) return false; + return true; +} + +// Update the orientation of the AABB according to the orientation of the rigid body +// In order to compute the new AABB we use the original AABB (represented by the originalAABBExtent +// values). The goal is to rotate the original AABB according to the current rotation (rotationQuaternion) +// and then compute the new extent values from the rotated axis of the original AABB. The three columns of +// the rotation matrix correspond to the rotated axis of the rotated original AABB. The we have to compute +// the projections of the three rotated axis onto the x, y and z axis. The projections are easy to compute +// because for instance if the size of the projection of the vector (4, 5, 6) onto the x axis is simply 4. +inline void AABB::update(const Vector3D& newCenter, const Quaternion& rotationQuaternion) { + // Update the center of the AABB + center = newCenter; + + // Recompute the new extents size from the rotated AABB + Matrix rotationMatrix = rotationQuaternion.getMatrix(); // Rotation matrix + for (int i=0; i<3; i++) { // For each x, y and z axis + extent[i] = 0.0; + for (int j=0; j<3; j++) { // For each rotated axis of the rotated original AABB + // Add the size of the projection of the current rotated axis to the extent of the current (x, y, or z) axis + extent[i] += std::abs(rotationMatrix.getValue(i, j)) * originalAABBExtent[j]; + } + } +} + +}; // End of the ReactPhysics3D namespace + +#endif diff --git a/src/reactphysics3d/body/Body.cpp b/src/reactphysics3d/body/Body.cpp new file mode 100644 index 00000000..64079491 --- /dev/null +++ b/src/reactphysics3d/body/Body.cpp @@ -0,0 +1,65 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + + // Libraries +#include "Body.h" +#include "BroadBoundingVolume.h" +#include "NarrowBoundingVolume.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +Body::Body(double mass) throw(std::invalid_argument) + : mass(mass), broadBoundingVolume(0), narrowBoundingVolume(0) { + // Check if the mass is not larger than zero + if (mass <= 0.0) { + // We throw an exception + throw std::invalid_argument("Exception in Body constructor : the mass has to be different larger than zero"); + } +} + +// Destructor +Body::~Body() { + if (broadBoundingVolume) { + delete broadBoundingVolume; + } + if (narrowBoundingVolume) { + delete narrowBoundingVolume; + } +} + +// Set the broad-phase bounding volume +void Body::setBroadBoundingVolume(BroadBoundingVolume* broadBoundingVolume) { + assert(broadBoundingVolume); + this->broadBoundingVolume = broadBoundingVolume; + broadBoundingVolume->setBodyPointer(this); +} + +// Set the narrow-phase bounding volume +void Body::setNarrowBoundingVolume(NarrowBoundingVolume* narrowBoundingVolume) { + assert(narrowBoundingVolume); + this->narrowBoundingVolume = narrowBoundingVolume; + narrowBoundingVolume->setBodyPointer(this); +} diff --git a/src/reactphysics3d/body/Body.h b/src/reactphysics3d/body/Body.h new file mode 100644 index 00000000..7bc4555a --- /dev/null +++ b/src/reactphysics3d/body/Body.h @@ -0,0 +1,112 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef BODY_H +#define BODY_H + +// Libraries +#include +#include + + +// Namespace reactphysics3d +namespace reactphysics3d { + +class BroadBoundingVolume; +class NarrowBoundingVolume; + +/* ------------------------------------------------------------------- + Class Body : + This class is an abstract class to represent body of the physics + engine. + ------------------------------------------------------------------- +*/ +class Body { + protected : + double mass; // Mass of the body + BroadBoundingVolume* broadBoundingVolume; // Bounding volume used for the broad-phase collision detection + NarrowBoundingVolume* narrowBoundingVolume; // Bounding volume used for the narrow-phase collision detection + bool isMotionEnabled; // True if the body is able to move + bool isCollisionEnabled; // True if the body can collide with others bodies + + void setBroadBoundingVolume(BroadBoundingVolume* broadBoundingVolume); // Set the broad-phase bounding volume + void setNarrowBoundingVolume(NarrowBoundingVolume* narrowBoundingVolume); // Set the narrow-phase bounding volume + + public : + Body(double mass) throw(std::invalid_argument); // Constructor + virtual ~Body(); // Destructor + + double getMass() const; // Return the mass of the body + void setMass(double mass); // Set the mass of the body + bool getIsMotionEnabled() const; // Return if the rigid body can move + void setIsMotionEnabled(bool isMotionEnabled); // Set the value to true if the body can move + bool getIsCollisionEnabled() const; // Return true if the body can collide with others bodies + void setIsCollisionEnabled(bool isCollisionEnabled); // Set the isCollisionEnabled value + const BroadBoundingVolume* getBroadBoundingVolume() const; // Return the broad-phase bounding volume + const NarrowBoundingVolume* getNarrowBoundingVolume() const; // Return the narrow-phase bounding volume of the body +}; + +// Method that return the mass of the body +inline double Body::getMass() const { + return mass; +}; + +// Return if the rigid body can move +inline bool Body::getIsMotionEnabled() const { + return isMotionEnabled; +} + +// Set the value to true if the body can move +inline void Body::setIsMotionEnabled(bool isMotionEnabled) { + this->isMotionEnabled = isMotionEnabled; +} + +// Method that set the mass of the body +inline void Body::setMass(double mass) { + this->mass = mass; +} + + // Return true if the body can collide with others bodies +inline bool Body::getIsCollisionEnabled() const { + return isCollisionEnabled; +} + +// Set the isCollisionEnabled value +inline void Body::setIsCollisionEnabled(bool isCollisionEnabled) { + this->isCollisionEnabled = isCollisionEnabled; +} + +// Return the broad-phase bounding volume +inline const BroadBoundingVolume* Body::getBroadBoundingVolume() const { + return broadBoundingVolume; +} + +// Return the oriented bounding box of the rigid body +inline const NarrowBoundingVolume* Body::getNarrowBoundingVolume() const { + return narrowBoundingVolume; +} + +} + + #endif diff --git a/src/reactphysics3d/body/BoundingVolume.cpp b/src/reactphysics3d/body/BoundingVolume.cpp new file mode 100644 index 00000000..6d13a672 --- /dev/null +++ b/src/reactphysics3d/body/BoundingVolume.cpp @@ -0,0 +1,39 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "BoundingVolume.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +BoundingVolume::BoundingVolume() { + this->body = 0; +} + +// Destructor +BoundingVolume::~BoundingVolume() { + +} diff --git a/src/reactphysics3d/body/BoundingVolume.h b/src/reactphysics3d/body/BoundingVolume.h new file mode 100644 index 00000000..163671b6 --- /dev/null +++ b/src/reactphysics3d/body/BoundingVolume.h @@ -0,0 +1,73 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef BOUNDING_VOLUME_H +#define BOUNDING_VOLUME_H + +// Libraries +#include "Body.h" +#include "../mathematics/mathematics.h" +#include + + +// ReactPhysics3D namespace +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class BoundingVolume : + This class represents the volume that contains a rigid body. + This volume will be used to compute the collisions with others + bodies. + ------------------------------------------------------------------- +*/ +class BoundingVolume { + protected : + Body* body; // Pointer to the body + + public : + BoundingVolume(); // Constructor + virtual ~BoundingVolume(); // Destructor + + Body* getBodyPointer() const; // Return the body pointer + void setBodyPointer(Body* body); // Set the body pointer + + virtual void update(const Vector3D& newCenter, const Quaternion& rotationQuaternion)=0; // Update the orientation of the bounding volume according to the new orientation of the body + virtual void draw() const=0; // Display the bounding volume (only for testing purpose) +}; + +// Return the body pointer +inline Body* BoundingVolume::getBodyPointer() const { + assert(body != 0); + return body; +} + +// Set the body pointer +inline void BoundingVolume::setBodyPointer(Body* bodyPointer) { + this->body = bodyPointer; +} + + +} // End of the ReactPhysics3D namespace + +#endif diff --git a/src/reactphysics3d/body/BroadBoundingVolume.cpp b/src/reactphysics3d/body/BroadBoundingVolume.cpp new file mode 100644 index 00000000..fa7a59ef --- /dev/null +++ b/src/reactphysics3d/body/BroadBoundingVolume.cpp @@ -0,0 +1,39 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "BroadBoundingVolume.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +BroadBoundingVolume::BroadBoundingVolume() : BoundingVolume() { + +} + +// Destructor +BroadBoundingVolume::~BroadBoundingVolume() { + +} diff --git a/src/reactphysics3d/body/BroadBoundingVolume.h b/src/reactphysics3d/body/BroadBoundingVolume.h new file mode 100644 index 00000000..dad2e361 --- /dev/null +++ b/src/reactphysics3d/body/BroadBoundingVolume.h @@ -0,0 +1,52 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef BROAD_BOUNDING_VOLUME_H +#define BROAD_BOUNDING_VOLUME_H + +// Libraries +#include "BoundingVolume.h" + + +// ReactPhysics3D namespace +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class BroadBoundingVolume : + This class represents the volume that contains a rigid body + This volume will be used to compute the broad-phase collision + detection. + ------------------------------------------------------------------- +*/ +class BroadBoundingVolume : public BoundingVolume { + protected : + + public : + BroadBoundingVolume(); // Constructor + virtual ~BroadBoundingVolume(); // Destructor +}; + +} + +#endif \ No newline at end of file diff --git a/src/reactphysics3d/body/NarrowBoundingVolume.cpp b/src/reactphysics3d/body/NarrowBoundingVolume.cpp new file mode 100644 index 00000000..848a333c --- /dev/null +++ b/src/reactphysics3d/body/NarrowBoundingVolume.cpp @@ -0,0 +1,39 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "NarrowBoundingVolume.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +NarrowBoundingVolume::NarrowBoundingVolume() : BoundingVolume() { + +} + +// Destructor +NarrowBoundingVolume::~NarrowBoundingVolume() { + +} diff --git a/src/reactphysics3d/body/NarrowBoundingVolume.h b/src/reactphysics3d/body/NarrowBoundingVolume.h new file mode 100644 index 00000000..64d07a30 --- /dev/null +++ b/src/reactphysics3d/body/NarrowBoundingVolume.h @@ -0,0 +1,56 @@ +/**************************************************************************** +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef NARROW_BOUNDING_VOLUME_H +#define NARROW_BOUNDING_VOLUME_H + +// Libraries +#include "BoundingVolume.h" +#include "AABB.h" + + +// ReactPhysics3D namespace +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class NarrowBoundingVolume : + This class represents the volume that contains a rigid body + This volume will be used to compute the narrow-phase collision + detection. + ------------------------------------------------------------------- +*/ +class NarrowBoundingVolume : public BoundingVolume { + protected : + + public : + NarrowBoundingVolume(); // Constructor + virtual ~NarrowBoundingVolume(); // Destructor + + virtual AABB* computeAABB() const=0; // Return the corresponding AABB +}; + +} + +#endif \ No newline at end of file diff --git a/src/reactphysics3d/body/OBB.cpp b/src/reactphysics3d/body/OBB.cpp new file mode 100644 index 00000000..adea91be --- /dev/null +++ b/src/reactphysics3d/body/OBB.cpp @@ -0,0 +1,210 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "OBB.h" +#include +#include // TODO : Remove this in the final version +#include // TODO : Remove this in the final version +#include + +using namespace reactphysics3d; +using namespace std; + +// Constructor +OBB::OBB(const Vector3D& center, const Vector3D& axis1, const Vector3D& axis2, + const Vector3D& axis3, double extent1, double extent2, double extent3) { + this->center = center; + + oldAxis[0] = axis1.getUnit(); + oldAxis[1] = axis2.getUnit(); + oldAxis[2] = axis3.getUnit(); + + this->axis[0] = oldAxis[0]; + this->axis[1] = oldAxis[1]; + this->axis[2] = oldAxis[2]; + + this->extent[0] = extent1; + this->extent[1] = extent2; + this->extent[2] = extent3; +} + +// Destructor +OBB::~OBB() { + +} + +// Draw the OBB (only for testing purpose) +void OBB::draw() const { + double e0 = extent[0]; + double e1 = extent[1]; + double e2 = extent[2]; + + Vector3D s1 = center + (axis[0]*e0) + (axis[1]*e1) - (axis[2]*e2); + Vector3D s2 = center + (axis[0]*e0) + (axis[1]*e1) + (axis[2]*e2); + Vector3D s3 = center - (axis[0]*e0) + (axis[1]*e1) + (axis[2]*e2); + Vector3D s4 = center - (axis[0]*e0) + (axis[1]*e1) - (axis[2]*e2); + Vector3D s5 = center + (axis[0]*e0) - (axis[1]*e1) - (axis[2]*e2); + Vector3D s6 = center + (axis[0]*e0) - (axis[1]*e1) + (axis[2]*e2); + Vector3D s7 = center - (axis[0]*e0) - (axis[1]*e1) + (axis[2]*e2); + Vector3D s8 = center - (axis[0]*e0) - (axis[1]*e1) - (axis[2]*e2); + + // Draw in red + glColor3f(1.0, 0.0, 0.0); + + // Draw the OBB + glBegin(GL_LINES); + glVertex3f(s1.getX(), s1.getY(), s1.getZ()); + glVertex3f(s2.getX(), s2.getY(), s2.getZ()); + + glVertex3f(s2.getX(), s2.getY(), s2.getZ()); + glVertex3f(s3.getX(), s3.getY(), s3.getZ()); + + glVertex3f(s3.getX(), s3.getY(), s3.getZ()); + glVertex3f(s4.getX(), s4.getY(), s4.getZ()); + + glVertex3f(s4.getX(), s4.getY(), s4.getZ()); + glVertex3f(s1.getX(), s1.getY(), s1.getZ()); + + glVertex3f(s5.getX(), s5.getY(), s5.getZ()); + glVertex3f(s6.getX(), s6.getY(), s6.getZ()); + + glVertex3f(s6.getX(), s6.getY(), s6.getZ()); + glVertex3f(s7.getX(), s7.getY(), s7.getZ()); + + glVertex3f(s7.getX(), s7.getY(), s7.getZ()); + glVertex3f(s8.getX(), s8.getY(), s8.getZ()); + + glVertex3f(s8.getX(), s8.getY(), s8.getZ()); + glVertex3f(s5.getX(), s5.getY(), s5.getZ()); + + glVertex3f(s1.getX(), s1.getY(), s1.getZ()); + glVertex3f(s5.getX(), s5.getY(), s5.getZ()); + + glVertex3f(s4.getX(), s4.getY(), s4.getZ()); + glVertex3f(s8.getX(), s8.getY(), s8.getZ()); + + glVertex3f(s3.getX(), s3.getY(), s3.getZ()); + glVertex3f(s7.getX(), s7.getY(), s7.getZ()); + + glVertex3f(s2.getX(), s2.getY(), s2.getZ()); + glVertex3f(s6.getX(), s6.getY(), s6.getZ()); + + glVertex3f(center.getX(), center.getY(), center.getZ()); + glVertex3f(center.getX() + 8.0 * axis[1].getX(), center.getY() + 8.0 * axis[1].getY(), center.getZ() + 8.0 * axis[1].getZ()); + + glEnd(); +} + +// Return all the vertices that are projected at the extreme of the projection of the bouding volume on the axis. +// If the extreme vertices are part of a face of the OBB, the returned vertices will be ordered according to the face. +vector OBB::getExtremeVertices(const Vector3D& directionAxis) const { + assert(directionAxis.length() != 0); + + vector extremeVertices; + + // Check if the given axis is parallel to an axis on the OBB + if (axis[0].isParallelWith(directionAxis)) { + if (axis[0].scalarProduct(directionAxis) >= 0) { // If both axis are in the same direction + extremeVertices = getFace(0); // The extreme is the face 0 + } + else { + extremeVertices = getFace(1); // The extreme is the face 1 + } + } + else if(axis[1].isParallelWith(directionAxis)) { + if (axis[1].scalarProduct(directionAxis) >= 0) { // If both axis are in the same direction + extremeVertices = getFace(2); // The extreme is the face 2 + } + else { + extremeVertices = getFace(3); // The extreme is the face 3 + } + + } + else if(axis[2].isParallelWith(directionAxis)) { + if (axis[2].scalarProduct(directionAxis) >= 0) { // If both axis are in the same direction + extremeVertices = getFace(4); // The extreme is the face 4 + } + else { + extremeVertices = getFace(5); // The extreme is the face 5 + } + } + else { // The extreme is made of an unique vertex or an edge + double maxProjectionLength = 0.0; // Longest projection length of a vertex onto the projection axis + + // For each vertex of the OBB + for (unsigned int i=0; i<8; ++i) { + Vector3D vertex = getVertex(i); + + // Compute the projection length of the current vertex onto the projection axis + double projectionLength = directionAxis.scalarProduct(vertex-center) / directionAxis.length(); + + // If we found a bigger projection length + if (projectionLength > maxProjectionLength + EPSILON) { + maxProjectionLength = projectionLength; + extremeVertices.clear(); + extremeVertices.push_back(vertex); + } + else if (approxEqual(projectionLength, maxProjectionLength)) { + extremeVertices.push_back(vertex); + } + } + + assert(extremeVertices.size() == 1 || extremeVertices.size() == 2); + } + + // An extreme should be a unique vertex, an edge or a face + assert(extremeVertices.size() == 1 || extremeVertices.size() == 2 || extremeVertices.size() == 4); + + // Return the extreme vertices + return extremeVertices; +} + +// Static method that computes an OBB from a set of vertices. The "center" argument corresponds to the center of the OBB +// This method allocates a new OBB object and return a pointer to the new allocated OBB object +OBB* OBB::computeFromVertices(const vector& vertices, const Vector3D& center) { + // TODO : Implement this method; + return 0; +} + +// Return the corresponding AABB +AABB* OBB::computeAABB() const { + double maxLength[] = {0.0, 0.0, 0.0}; // Maximum length for each of the three x,y and z axis + Vector3D vertex; + double length; + + // For each vertex of the OBB + for (int i = 0; i<8; i++) { + vertex = getVertex(i) - center; + for (int j=0; j<3; j++) { + length = std::abs(vertex.getValue(j)); + if (length > maxLength[j]) { + maxLength[j] = length; + } + } + } + + // Create and return the AABB + return new AABB(center, maxLength[0], maxLength[1], maxLength[2]); +} diff --git a/src/reactphysics3d/body/OBB.h b/src/reactphysics3d/body/OBB.h new file mode 100644 index 00000000..c72a8fff --- /dev/null +++ b/src/reactphysics3d/body/OBB.h @@ -0,0 +1,228 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef OBB_H +#define OBB_H + +// Libraries +#include +#include "NarrowBoundingVolume.h" +#include "../mathematics/mathematics.h" + +// ReactPhysics3D namespace +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class OBB : + This class represents a bounding volume of type "Oriented Bounding + Box". It's a box that has a given orientation is space. The three + axis of the OBB are three axis that give the orientation of the + OBB. The three axis are normal vectors to the faces of the OBB. + Those axis are unit length. The three extents are half-widths + of the box along the three axis of the OBB. + ------------------------------------------------------------------- +*/ +class OBB : public NarrowBoundingVolume { + protected : + Vector3D center; // Center point of the OBB + Vector3D oldAxis[3]; // Array that contains the three unit length axis at the beginning + Vector3D axis[3]; // Array that contains the three unit length axis of the OBB + double extent[3]; // Array that contains the three extents size of the OBB + + public : + OBB(const Vector3D& center, const Vector3D& axis1, const Vector3D& axis2, + const Vector3D& axis3, double extent1, double extent2, double extent3); // Constructor + virtual ~OBB(); // Destructor + + Vector3D getCenter() const; // Return the center point of the OBB + void setCenter(const Vector3D& center); // Set the center point + Vector3D getAxis(unsigned int index) const throw(std::invalid_argument); // Return an axis of the OBB + void setAxis(unsigned int index, const Vector3D& axis) throw(std::invalid_argument); // Set an axis + Vector3D getVertex(unsigned int index) const throw (std::invalid_argument); // Return a vertex of the OBB + std::vector getFace(unsigned int index) const throw(std::invalid_argument); // Return the 4 vertices the OBB's face in the direction of a given axis + double getExtent(unsigned int index) const throw(std::invalid_argument); // Return an extent value + void setExtent(unsigned int index, double extent) throw(std::invalid_argument); // Set an extent value + virtual std::vector getExtremeVertices(const Vector3D& axis) const; // Return all the vertices that are projected at the extreme of the projection of the bouding volume on the axis + virtual void update(const Vector3D& newCenter, const Quaternion& rotationQuaternion); // Update the oriented bounding box orientation according to a new orientation of the rigid body + virtual AABB* computeAABB() const; // Return the corresponding AABB + virtual void draw() const; // Draw the OBB (only for testing purpose) + static OBB* computeFromVertices(const std::vector& vertices, const Vector3D& center); // Compute an OBB from a set of vertices +}; + +// Return the center point of the OBB +inline Vector3D OBB::getCenter() const { + return center; +} + +// Set the center point +inline void OBB::setCenter(const Vector3D& center) { + this->center = center; +} + +// Return an axis of the OBB +inline Vector3D OBB::getAxis(unsigned int index) const throw(std::invalid_argument) { + // Check if the index value is valid + if (index >= 0 && index <3) { + return axis[index]; + } + else { + // The index value is not valid, we throw an exception + throw std::invalid_argument("Exception : The index value has to be between 0 and 2"); + } +} + +// Set an axis +inline void OBB::setAxis(unsigned int index, const Vector3D& axis) throw(std::invalid_argument) { + // Check if the index value is valid + if (index >= 0 && index <3) { + this->axis[index] = axis; + } + else { + // The index value is not valid, we throw an exception + throw std::invalid_argument("Exception : The index value has to be between 0 and 2"); + } +} + +// Return a vertex of the OBB +inline Vector3D OBB::getVertex(unsigned int index) const throw (std::invalid_argument) { + // Check if the index value is valid + if (index >= 0 && index <8) { + Vector3D vertex; + + switch(index) { + case 0 : vertex = center + (axis[0]*extent[0]) + (axis[1]*extent[1]) - (axis[2]*extent[2]); + break; + case 1 : vertex = center + (axis[0]*extent[0]) + (axis[1]*extent[1]) + (axis[2]*extent[2]); + break; + case 2 : vertex = center - (axis[0]*extent[0]) + (axis[1]*extent[1]) + (axis[2]*extent[2]); + break; + case 3 : vertex = center - (axis[0]*extent[0]) + (axis[1]*extent[1]) - (axis[2]*extent[2]); + break; + case 4 : vertex = center + (axis[0]*extent[0]) - (axis[1]*extent[1]) - (axis[2]*extent[2]); + break; + case 5 : vertex = center + (axis[0]*extent[0]) - (axis[1]*extent[1]) + (axis[2]*extent[2]); + break; + case 6 : vertex = center - (axis[0]*extent[0]) - (axis[1]*extent[1]) + (axis[2]*extent[2]); + break; + case 7 : vertex = center - (axis[0]*extent[0]) - (axis[1]*extent[1]) - (axis[2]*extent[2]); + break; + } + + // Return the vertex + return vertex; + } + else { + // The index value is not valid, we throw an exception + throw std::invalid_argument("Exception : The index value has to be between 0 and 8"); + } +} + +// Return the 4 vertices of a face of the OBB. The 4 vertices will be ordered. The convention is that the index 0 corresponds to +// the face in the direction of the axis[0], 1 corresponds to the face in the opposite direction of the axis[0], 2 corresponds to +// the face in the direction of the axis[1], etc. +inline std::vector OBB::getFace(unsigned int index) const throw(std::invalid_argument) { + // Check the argument + if (index >=0 && index <6) { + std::vector vertices; + switch(index) { + case 0: vertices.push_back(center + (axis[0]*extent[0]) + (axis[1]*extent[1]) - (axis[2]*extent[2])); + vertices.push_back(center + (axis[0]*extent[0]) + (axis[1]*extent[1]) + (axis[2]*extent[2])); + vertices.push_back(center + (axis[0]*extent[0]) - (axis[1]*extent[1]) + (axis[2]*extent[2])); + vertices.push_back(center + (axis[0]*extent[0]) - (axis[1]*extent[1]) - (axis[2]*extent[2])); + break; + case 1: vertices.push_back(center - (axis[0]*extent[0]) + (axis[1]*extent[1]) - (axis[2]*extent[2])); + vertices.push_back(center - (axis[0]*extent[0]) + (axis[1]*extent[1]) + (axis[2]*extent[2])); + vertices.push_back(center - (axis[0]*extent[0]) - (axis[1]*extent[1]) + (axis[2]*extent[2])); + vertices.push_back(center - (axis[0]*extent[0]) - (axis[1]*extent[1]) - (axis[2]*extent[2])); + break; + case 2: vertices.push_back(center + (axis[1]*extent[1]) + (axis[0]*extent[0]) - (axis[2]*extent[2])); + vertices.push_back(center + (axis[1]*extent[1]) + (axis[0]*extent[0]) + (axis[2]*extent[2])); + vertices.push_back(center + (axis[1]*extent[1]) - (axis[0]*extent[0]) + (axis[2]*extent[2])); + vertices.push_back(center + (axis[1]*extent[1]) - (axis[0]*extent[0]) - (axis[2]*extent[2])); + break; + case 3: vertices.push_back(center - (axis[1]*extent[1]) + (axis[0]*extent[0]) - (axis[2]*extent[2])); + vertices.push_back(center - (axis[1]*extent[1]) + (axis[0]*extent[0]) + (axis[2]*extent[2])); + vertices.push_back(center - (axis[1]*extent[1]) - (axis[0]*extent[0]) + (axis[2]*extent[2])); + vertices.push_back(center - (axis[1]*extent[1]) - (axis[0]*extent[0]) - (axis[2]*extent[2])); + break; + case 4: vertices.push_back(center + (axis[2]*extent[2]) + (axis[0]*extent[0]) - (axis[1]*extent[1])); + vertices.push_back(center + (axis[2]*extent[2]) + (axis[0]*extent[0]) + (axis[1]*extent[1])); + vertices.push_back(center + (axis[2]*extent[2]) - (axis[0]*extent[0]) + (axis[1]*extent[1])); + vertices.push_back(center + (axis[2]*extent[2]) - (axis[0]*extent[0]) - (axis[1]*extent[1])); + break; + case 5: vertices.push_back(center - (axis[2]*extent[2]) + (axis[0]*extent[0]) - (axis[1]*extent[1])); + vertices.push_back(center - (axis[2]*extent[2]) + (axis[0]*extent[0]) + (axis[1]*extent[1])); + vertices.push_back(center - (axis[2]*extent[2]) - (axis[0]*extent[0]) + (axis[1]*extent[1])); + vertices.push_back(center - (axis[2]*extent[2]) - (axis[0]*extent[0]) - (axis[1]*extent[1])); + break; + } + + // Return the vertices + assert(vertices.size() == 4); + return vertices; + } + else { + // Throw an exception + throw std::invalid_argument("Exception: The argument must be between 0 and 5"); + } +} + +// Return an extent value +inline double OBB::getExtent(unsigned int index) const throw(std::invalid_argument) { + // Check if the index value is valid + if (index >= 0 && index <3) { + return extent[index]; + } + else { + // The index value is not valid, we throw an exception + throw std::invalid_argument("Exception : The index value has to be between 0 and 2"); + } +} + +// Set an extent value +inline void OBB::setExtent(unsigned int index, double extent) throw(std::invalid_argument) { + // Check if the index value is valid + if (index >= 0 && index <3) { + this->extent[index] = extent; + } + else { + // The index value is not valid, we throw an exception + throw std::invalid_argument("Exception : The index value has to be between 0 and 2"); + } +} + +// Update the orientation of the OBB according to the orientation of the rigid body +inline void OBB::update(const Vector3D& newCenter, const Quaternion& rotationQuaternion) { + // Update the center of the OBB + center = newCenter; + + // Update the three axis of the OBB by rotating and normalize then + axis[0] = rotateVectorWithQuaternion(oldAxis[0], rotationQuaternion).getUnit(); + axis[1] = rotateVectorWithQuaternion(oldAxis[1], rotationQuaternion).getUnit(); + axis[2] = rotateVectorWithQuaternion(oldAxis[2], rotationQuaternion).getUnit(); +} + +}; // End of the ReactPhysics3D namespace + +#endif diff --git a/src/reactphysics3d/body/RigidBody.cpp b/src/reactphysics3d/body/RigidBody.cpp new file mode 100644 index 00000000..9e375edb --- /dev/null +++ b/src/reactphysics3d/body/RigidBody.cpp @@ -0,0 +1,67 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "RigidBody.h" +#include "BroadBoundingVolume.h" +#include "NarrowBoundingVolume.h" + + // We want to use the ReactPhysics3D namespace + using namespace reactphysics3d; + + // Constructor + RigidBody::RigidBody(const Vector3D& position, const Quaternion& orientation, double mass, const Matrix3x3& inertiaTensorLocal, + NarrowBoundingVolume* narrowBoundingVolume) + : Body(mass), position(position), orientation(orientation.getUnit()), inertiaTensorLocal(inertiaTensorLocal), + inertiaTensorLocalInverse(inertiaTensorLocal.getInverse()), massInverse(1.0/mass), oldPosition(position), oldOrientation(orientation) { + + restitution = 1.0; + isMotionEnabled = true; + isCollisionEnabled = true; + interpolationFactor = 0.0; + + // Set the bounding volume for the narrow-phase collision detection + setNarrowBoundingVolume(narrowBoundingVolume); + + // Compute the broad-phase bounding volume (an AABB) + setBroadBoundingVolume(narrowBoundingVolume->computeAABB()); + + // Update the orientation of the OBB according to the orientation of the rigid body + update(); + + assert(broadBoundingVolume); + assert(narrowBoundingVolume); +} + +// Destructor +RigidBody::~RigidBody() { + +}; + +// Update the rigid body in order to reflect a change in the body state +void RigidBody::update() { + // Update the orientation of the corresponding bounding volumes of the rigid body + broadBoundingVolume->update(position, orientation); + narrowBoundingVolume->update(position, orientation); +} \ No newline at end of file diff --git a/src/reactphysics3d/body/RigidBody.h b/src/reactphysics3d/body/RigidBody.h new file mode 100644 index 00000000..e3ecf788 --- /dev/null +++ b/src/reactphysics3d/body/RigidBody.h @@ -0,0 +1,246 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef RIGIDBODY_H +#define RIGIDBODY_H + +// Libraries +#include +#include "Body.h" +#include "../mathematics/mathematics.h" + +// Namespace reactphysics3d +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class RigidBody : + This class represents a rigid body of the physics + engine. A rigid body is a non-deformable body that + has a constant mass. + ------------------------------------------------------------------- +*/ +class RigidBody : public Body { + protected : + Vector3D position; // Position of the center of mass of the body + Vector3D oldPosition; // Old position used to compute the interpolated position + Quaternion orientation; // Orientation quaternion of the body + Quaternion oldOrientation; // Old orientation used to compute the interpolated orientation + Vector3D linearVelocity; // Linear velocity of the body + Vector3D angularVelocity; // Angular velocity of the body + Vector3D externalForce; // Current external force on the body + Vector3D externalTorque; // Current external torque on the body + Matrix3x3 inertiaTensorLocal; // Local inertia tensor of the body (in body coordinates) + Matrix3x3 inertiaTensorLocalInverse; // Inverse of the inertia tensor of the body (in body coordinates) + double massInverse; // Inverse of the mass of the body + double interpolationFactor; // Interpolation factor used for the state interpolation + double restitution; // Coefficient of restitution (between 0 and 1), 1 for a very boucing body + + public : + RigidBody(const Vector3D& position, const Quaternion& orientation, double mass, + const Matrix3x3& inertiaTensorLocal, NarrowBoundingVolume* narrowBoundingVolume); // Constructor // Copy-constructor + virtual ~RigidBody(); // Destructor + + Vector3D getPosition() const; // Return the position of the body + void setPosition(const Vector3D& position); // Set the position of the body + Quaternion getOrientation() const; // Return the orientation quaternion + void setOrientation(const Quaternion& orientation); // Set the orientation quaternion + Vector3D getLinearVelocity() const; // Return the linear velocity + void setLinearVelocity(const Vector3D& linearVelocity); // Set the linear velocity of the body + Vector3D getAngularVelocity() const; // Return the angular velocity + void setAngularVelocity(const Vector3D& angularVelocity); // Set the angular velocity + void setMassInverse(double massInverse); // Set the inverse of the mass + Vector3D getExternalForce() const; // Return the current external force of the body + void setExternalForce(const Vector3D& force); // Set the current external force on the body + Vector3D getExternalTorque() const; // Return the current external torque of the body + void setExternalTorque(const Vector3D& torque); // Set the current external torque of the body + double getMassInverse() const; // Return the inverse of the mass of the body + Matrix3x3 getInertiaTensorLocal() const; // Return the local inertia tensor of the body (in body coordinates) + void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal); // Set the local inertia tensor of the body (in body coordinates) + Matrix3x3 getInertiaTensorLocalInverse() const; // Get the inverse of the inertia tensor + Matrix3x3 getInertiaTensorWorld() const; // Return the inertia tensor in world coordinates + Matrix3x3 getInertiaTensorInverseWorld() const; // Return the inverse of the inertia tensor in world coordinates + void setInterpolationFactor(double factor); // Set the interpolation factor of the body + Vector3D getInterpolatedPosition() const; // Return the interpolated position + Quaternion getInterpolatedOrientation() const; // Return the interpolated orientation + double getRestitution() const; // Get the restitution coefficient + void setRestitution(double restitution) throw(std::invalid_argument); // Set the restitution coefficient + void updateOldPositionAndOrientation(); // Update the previous position and orientation of the body + void update(); // Update the rigid body in order to reflect a change in the body state +}; + +// --- Inline functions --- // + +// Return the position of the body +inline Vector3D RigidBody::getPosition() const { + return position; +} + +// Set the position of the body +inline void RigidBody::setPosition(const Vector3D& position) { + this->position = position; +} + +// Return the orientation quaternion of the body +inline Quaternion RigidBody::getOrientation() const { + return orientation; +} + +// Set the orientation quaternion +inline void RigidBody::setOrientation(const Quaternion& orientation) { + this->orientation = orientation; + + // Normalize the orientation quaternion + orientation.getUnit(); +} + +// Return the linear velocity +inline Vector3D RigidBody::getLinearVelocity() const { + return linearVelocity; +} + +// Return the angular velocity of the body +inline Vector3D RigidBody::getAngularVelocity() const { + return angularVelocity; +} + +inline void RigidBody::setAngularVelocity(const Vector3D& angularVelocity) { + this->angularVelocity = angularVelocity; +} + +// Set the inverse of the mass +inline void RigidBody::setMassInverse(double massInverse) { + this->massInverse = massInverse; +} + +// Get the inverse of the inertia tensor +inline Matrix3x3 RigidBody::getInertiaTensorLocalInverse() const { + return inertiaTensorLocalInverse; +} + +// Return the external force on the body +inline Vector3D RigidBody::getExternalForce() const { + return externalForce; +} + +// Set the external force on the body +inline void RigidBody::setExternalForce(const Vector3D& force) { + this->externalForce = force; +} + +// Return the current external torque on the body +inline Vector3D RigidBody::getExternalTorque() const { + return externalTorque; +} + + // Set the current external torque on the body +inline void RigidBody::setExternalTorque(const Vector3D& torque) { + this->externalTorque = torque; +} + +// Return the inverse of the mass of the body +inline double RigidBody::getMassInverse() const { + return massInverse; +} + +// Return the local inertia tensor of the body (in body coordinates) +inline Matrix3x3 RigidBody::getInertiaTensorLocal() const { + return inertiaTensorLocal; +} + +// Set the local inertia tensor of the body (in body coordinates) +inline void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) { + this->inertiaTensorLocal = inertiaTensorLocal; +} + +// Return the inertia tensor in world coordinates +// The inertia tensor I_w in world coordinates in computed with the local inertia tensor I_b in body coordinates +// by I_w = R * I_b * R^T +// where R is the rotation matrix (and R^T its transpose) of the current orientation quaternion of the body +inline Matrix3x3 RigidBody::getInertiaTensorWorld() const { + // Compute and return the inertia tensor in world coordinates + return orientation.getMatrix() * inertiaTensorLocal * orientation.getMatrix().getTranspose(); +} + +// Return the inverse of the inertia tensor in world coordinates +// The inertia tensor I_w in world coordinates in computed with the local inverse inertia tensor I_b^-1 in body coordinates +// by I_w = R * I_b^-1 * R^T +// where R is the rotation matrix (and R^T its transpose) of the current orientation quaternion of the body +inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const { + // Compute and return the inertia tensor in world coordinates + return orientation.getMatrix() * inertiaTensorLocalInverse * orientation.getMatrix().getTranspose(); +} + +// Set the interpolation factor of the body +inline void RigidBody::setInterpolationFactor(double factor) { + // Set the factor + interpolationFactor = factor; +} + +// Return the interpolated position +inline Vector3D RigidBody::getInterpolatedPosition() const { + // Compute the interpolated position + return oldPosition * (1-interpolationFactor) + position * interpolationFactor; +} + + // Return the interpolated orientation +inline Quaternion RigidBody::getInterpolatedOrientation() const { + // Compute the interpolated orientation + return Quaternion::slerp(oldOrientation, orientation, interpolationFactor); +} + +// Set the linear velocity of the rigid body +inline void RigidBody::setLinearVelocity(const Vector3D& linearVelocity) { + // If the body is able to move + if (isMotionEnabled) { + // Update the linear velocity of the current body state + this->linearVelocity = linearVelocity; + } +} + +// Get the restitution coeffficient of the rigid body +inline double RigidBody::getRestitution() const { + return restitution; +} + +// Set the restitution coefficient +inline void RigidBody::setRestitution(double restitution) throw(std::invalid_argument) { + // Check if the restitution coefficient is between 0 and 1 + if (restitution >= 0.0 && restitution <= 1.0) { + this->restitution = restitution; + } + else { + throw std::invalid_argument("Error : the restitution coefficent must be between 0 and 1"); + } +} + +// Update the previous body state of the body +// This is used to compute the interpolated position and orientation of the body +inline void RigidBody::updateOldPositionAndOrientation() { + oldPosition = position; + oldOrientation = orientation; +} + +} // End of the ReactPhyscis3D namespace + + #endif diff --git a/src/reactphysics3d/collision/BroadPhaseAlgorithm.cpp b/src/reactphysics3d/collision/BroadPhaseAlgorithm.cpp new file mode 100644 index 00000000..08253311 --- /dev/null +++ b/src/reactphysics3d/collision/BroadPhaseAlgorithm.cpp @@ -0,0 +1,39 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "BroadPhaseAlgorithm.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +BroadPhaseAlgorithm::BroadPhaseAlgorithm() { + +} + +// Destructor +BroadPhaseAlgorithm::~BroadPhaseAlgorithm() { + +} diff --git a/src/reactphysics3d/collision/BroadPhaseAlgorithm.h b/src/reactphysics3d/collision/BroadPhaseAlgorithm.h new file mode 100644 index 00000000..b592f221 --- /dev/null +++ b/src/reactphysics3d/collision/BroadPhaseAlgorithm.h @@ -0,0 +1,61 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef BROADPHASEALGORITHM_H +#define BROADPHASEALGORITHM_H + +// Libraries +#include "../body/BoundingVolume.h" + +// Namespace ReactPhysics3D +namespace reactphysics3d { + +/* -------------------------------------------------------------------- + Class BroadPhaseAlgorithm : + This class is an abstract class that represents an algorithm + used to perform the broad-phase of a collision detection. The + goal of the broad-phase algorithm is to compute the pair of bodies + that can collide. But it's important to understand that the + broad-phase doesn't compute only body pairs that can collide but + could also pairs of body that doesn't collide but are very close. + The goal of the broad-phase is to remove pairs of body that cannot + collide in order to avoid to much bodies to be tested in the + narrow-phase. + -------------------------------------------------------------------- +*/ +class BroadPhaseAlgorithm { + protected : + + public : + BroadPhaseAlgorithm(); // Constructor + virtual ~BroadPhaseAlgorithm(); // Destructor + + virtual void computePossibleCollisionPairs(std::vector addedBodies, std::vector removedBodies, + std::vector >& possibleCollisionPairs)=0; // Compute the possible collision pairs of bodies +}; + +} // End of reactphysics3d namespace + +#endif + diff --git a/src/reactphysics3d/collision/CollisionDetection.cpp b/src/reactphysics3d/collision/CollisionDetection.cpp new file mode 100644 index 00000000..c0fb0e8d --- /dev/null +++ b/src/reactphysics3d/collision/CollisionDetection.cpp @@ -0,0 +1,253 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "CollisionDetection.h" +#include "SAPAlgorithm.h" +#include "SATAlgorithm.h" +#include "../body/Body.h" +#include "../body/OBB.h" +#include "../body/RigidBody.h" +#include +#include + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; +using namespace std; + +// Constructor +CollisionDetection::CollisionDetection(PhysicsWorld* world) { + this->world = world; + + // Create the broad-phase algorithm that will be used (Sweep and Prune with AABB) + broadPhaseAlgorithm = new SAPAlgorithm(); + + // Create the narrow-phase algorithm that will be used (Separating axis algorithm) + narrowPhaseAlgorithm = new SATAlgorithm(); +} + +// Destructor +CollisionDetection::~CollisionDetection() { + +} + +// Compute the collision detection +bool CollisionDetection::computeCollisionDetection() { + + world->removeAllContactConstraints(); + possibleCollisionPairs.clear(); + contactInfos.clear(); + + // Compute the broad-phase collision detection + computeBroadPhase(); + + // Compute the narrow-phase collision detection + computeNarrowPhase(); + + // Compute all the new contacts + computeAllContacts(); + + // Return true if at least one contact has been found + return (contactInfos.size() > 0); +} + +// Compute the broad-phase collision detection +void CollisionDetection::computeBroadPhase() { + + // Clear the set of possible colliding pairs of bodies + possibleCollisionPairs.clear(); + + // Compute the set of possible collision pairs of bodies + broadPhaseAlgorithm->computePossibleCollisionPairs(world->getAddedBodies(), world->getRemovedBodies(), possibleCollisionPairs); +} + +// Compute the narrow-phase collision detection +void CollisionDetection::computeNarrowPhase() { + + // For each possible collision pair of bodies + for (unsigned int i=0; itestCollision(possibleCollisionPairs.at(i).first->getNarrowBoundingVolume(), possibleCollisionPairs.at(i).second->getNarrowBoundingVolume(), contactInfo)) { + assert(contactInfo != 0); + + // Add the contact info the current list of collision informations + contactInfos.push_back(contactInfo); + } + } +} + +// Compute all the contacts from the contact info list +void CollisionDetection::computeAllContacts() { + // For each possible contact info (computed during narrow-phase collision detection) + for (unsigned int i=0; iobb1; + const OBB* const obb2 = contactInfo->obb2; + Vector3D normal = contactInfo->normal; + double penetrationDepth = contactInfo->penetrationDepth; + + const vector obb1ExtremePoints = obb1->getExtremeVertices(normal); + const vector obb2ExtremePoints = obb2->getExtremeVertices(normal.getOpposite()); + unsigned int nbVerticesExtremeOBB1 = obb1ExtremePoints.size(); + unsigned int nbVerticesExtremeOBB2 = obb2ExtremePoints.size(); + assert(nbVerticesExtremeOBB1==1 || nbVerticesExtremeOBB1==2 || nbVerticesExtremeOBB1==4); + assert(nbVerticesExtremeOBB2==1 || nbVerticesExtremeOBB2==2 || nbVerticesExtremeOBB2==4); + assert(approxEqual(normal.length(), 1.0)); + + // If it's a Vertex-Something contact + if (nbVerticesExtremeOBB1 == 1) { + // Create a new contact and add it to the physics world + world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, obb1ExtremePoints.at(0))); + } + else if(nbVerticesExtremeOBB2 == 1) { // If its a Vertex-Something contact + // Create a new contact and add it to the physics world + world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, obb2ExtremePoints.at(0))); + } + else if (nbVerticesExtremeOBB1 == 2 && nbVerticesExtremeOBB2 == 2) { // If it's an edge-edge contact + // Compute the two vectors of the segment lines + Vector3D d1 = obb1ExtremePoints[1] - obb1ExtremePoints[0]; + Vector3D d2 = obb2ExtremePoints[1] - obb2ExtremePoints[0]; + + double alpha, beta; + vector contactSet; + + // If the two edges are parallel + if (d1.isParallelWith(d2)) { + Vector3D contactPointA; + Vector3D contactPointB; + + // Compute the intersection between the two edges + computeParallelSegmentsIntersection(obb1ExtremePoints[0], obb1ExtremePoints[1], obb2ExtremePoints[0], obb2ExtremePoints[1], + contactPointA, contactPointB); + + // Add the two contact points in the contact set + contactSet.push_back(contactPointA); + contactSet.push_back(contactPointB); + } + else { // If the two edges are not parallel + // Compute the closest two points between the two line segments + closestPointsBetweenTwoLines(obb1ExtremePoints[0], d1, obb2ExtremePoints[0], d2, &alpha, &beta); + Vector3D pointA = obb1ExtremePoints[0] + d1 * alpha; + Vector3D pointB = obb2ExtremePoints[0] + d2 * beta; + + // Compute the contact point as halfway between the 2 closest points + Vector3D contactPoint = 0.5 * (pointA + pointB); + + // Add the contact point into the contact set + contactSet.push_back(contactPoint); + } + + // For each point of the set of contact points + for (unsigned int i=0; iaddConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, contactSet.at(i))); + } + } + else if(nbVerticesExtremeOBB1 == 2 && nbVerticesExtremeOBB2 == 4) { // If it's an edge-face contact + // Compute the projection of the edge of OBB1 onto the same plane of the face of OBB2 + vector edge = projectPointsOntoPlane(obb1ExtremePoints, obb2ExtremePoints[0], normal); + + // Clip the edge of OBB1 using the face of OBB2 + vector clippedEdge = clipSegmentWithRectangleInPlane(edge, obb2ExtremePoints); + + // TODO : Correct this bug + // The following code is to correct a bug when the projected "edge" is not inside the clip rectangle + // of obb1ExtremePoints. Therefore, we compute the nearest two points that are on the rectangle. + if (clippedEdge.size() != 2) { + edge.clear(); + edge.push_back(computeNearestPointOnRectangle(edge[0], obb2ExtremePoints)); + edge.push_back(computeNearestPointOnRectangle(edge[1], obb2ExtremePoints)); + clippedEdge = clipSegmentWithRectangleInPlane(edge, obb2ExtremePoints); + } + + // Move the clipped edge halway between the edge of OBB1 and the face of OBB2 + clippedEdge = movePoints(clippedEdge, penetrationDepth/2.0 * normal.getOpposite()); + + assert(clippedEdge.size() == 2); + + // For each point of the contact set + for (unsigned int i=0; iaddConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedEdge.at(i))); + } + } + else if(nbVerticesExtremeOBB1 == 4 && nbVerticesExtremeOBB2 == 2) { // If it's an edge-face contact + // Compute the projection of the edge of OBB2 onto the same plane of the face of OBB1 + vector edge = projectPointsOntoPlane(obb2ExtremePoints, obb1ExtremePoints[0], normal); + + // Clip the edge of OBB2 using the face of OBB1 + vector clippedEdge = clipSegmentWithRectangleInPlane(edge, obb1ExtremePoints); + + // TODO : Correct this bug + // The following code is to correct a bug when the projected "edge" is not inside the clip rectangle + // of obb1ExtremePoints. Therefore, we compute the nearest two points that are on the rectangle. + if (clippedEdge.size() != 2) { + edge.clear(); + edge.push_back(computeNearestPointOnRectangle(edge[0], obb1ExtremePoints)); + edge.push_back(computeNearestPointOnRectangle(edge[1], obb1ExtremePoints)); + clippedEdge = clipSegmentWithRectangleInPlane(edge, obb1ExtremePoints); + } + + // Move the clipped edge halfway between the face of OBB1 and the edge of OBB2 + clippedEdge = movePoints(clippedEdge, penetrationDepth/2.0 * normal); + + assert(clippedEdge.size() == 2); + + // For each point of the contact set + for (unsigned int i=0; iaddConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedEdge.at(i))); + } + } + else { // If it's a face-face contact + // Compute the projection of the face vertices of OBB2 onto the plane of the face of OBB1 + vector faceOBB2 = projectPointsOntoPlane(obb2ExtremePoints, obb1ExtremePoints[0], normal); + + // Clip the face of OBB2 using the face of OBB1 + vector clippedFace = clipPolygonWithRectangleInPlane(faceOBB2, obb1ExtremePoints); + + // Move the clipped face halfway between the face of OBB1 and the face of OBB2 + clippedFace = movePoints(clippedFace, penetrationDepth/2.0 * normal); + assert(clippedFace.size() >= 3); + + // For each point of the contact set + for (unsigned int i=0; iaddConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedFace.at(i))); + } + } +} \ No newline at end of file diff --git a/src/reactphysics3d/collision/CollisionDetection.h b/src/reactphysics3d/collision/CollisionDetection.h new file mode 100644 index 00000000..c6c41648 --- /dev/null +++ b/src/reactphysics3d/collision/CollisionDetection.h @@ -0,0 +1,70 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef COLLISIONDETECTION_H +#define COLLISIONDETECTION_H + +// Libraries +#include "BroadPhaseAlgorithm.h" +#include "NarrowPhaseAlgorithm.h" +#include "../body/Body.h" +#include "../engine/PhysicsWorld.h" +#include "ContactInfo.h" +#include + +// ReactPhysics3D namespace +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class CollisionDetection : + This class computes the collision detection algorithms. We first + perfom a broad-phase algorithm to know wich pairs of bodies can + collide and then we run a narrow-phase algorithm to compute the + collision contacts between bodies. + ------------------------------------------------------------------- +*/ +class CollisionDetection { + private : + PhysicsWorld* world; // Pointer to the physics world + std::vector > possibleCollisionPairs; // Possible collision pairs of bodies (computed by broadphase) + std::vector contactInfos; // Contact informations (computed by narrowphase) + BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm + NarrowPhaseAlgorithm* narrowPhaseAlgorithm; // Narrow-phase algorithm + + void computeBroadPhase(); // Compute the broad-phase collision detection + void computeNarrowPhase(); // Compute the narrow-phase collision detection + void computeAllContacts(); // Compute all the contacts from the collision info list + void computeContact(const ContactInfo* const contactInfo); // Compute a contact (and add it to the physics world) for two colliding bodies + void computeContact2(const ContactInfo* const contactInfo); // Compute a contact (and add it to the physics world) for two colliding bodies + + public : + CollisionDetection(PhysicsWorld* physicsWorld); // Constructor + ~CollisionDetection(); // Destructor + + bool computeCollisionDetection(); // Compute the collision detection +}; + +} // End of the ReactPhysics3D namespace + +#endif diff --git a/src/reactphysics3d/collision/ContactInfo.cpp b/src/reactphysics3d/collision/ContactInfo.cpp new file mode 100644 index 00000000..9d7c9016 --- /dev/null +++ b/src/reactphysics3d/collision/ContactInfo.cpp @@ -0,0 +1,34 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "ContactInfo.h" + +using namespace reactphysics3d; + +// Constructor +ContactInfo::ContactInfo(const OBB* const obb1, const OBB* const obb2, const Vector3D& normal, double penetrationDepth) + : obb1(obb1), obb2(obb2), normal(normal), penetrationDepth(penetrationDepth) { + +} diff --git a/src/reactphysics3d/collision/ContactInfo.h b/src/reactphysics3d/collision/ContactInfo.h new file mode 100644 index 00000000..54a765ca --- /dev/null +++ b/src/reactphysics3d/collision/ContactInfo.h @@ -0,0 +1,57 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef CONTACTINFO_H +#define CONTACTINFO_H + +// Libraries +#include "../body/OBB.h" +#include "../mathematics/mathematics.h" + +// ReactPhysics3D namespace +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Structure ContactInfo : + This structure contains informations about a collision contact + computed durring the narow phase collision detection. Those + informations are use to compute the contact set for a contact + between two bodies. + ------------------------------------------------------------------- +*/ +struct ContactInfo { + public: + // TODO : Use polymorphism here (change OBB into BoundingVolume to be more general) + const OBB* const obb1; // Body pointer of the first bounding volume + const OBB* const obb2; // Body pointer of the second bounding volume + const Vector3D normal; // Normal vector the the collision contact + const double penetrationDepth; // Penetration depth of the contact + + ContactInfo(const OBB* const obb1, const OBB* const obb2, const Vector3D& normal, double penetrationDepth); // Constructor +}; + +} // End of the ReactPhysics3D namespace + +#endif + diff --git a/src/reactphysics3d/collision/NarrowPhaseAlgorithm.cpp b/src/reactphysics3d/collision/NarrowPhaseAlgorithm.cpp new file mode 100644 index 00000000..47493c7e --- /dev/null +++ b/src/reactphysics3d/collision/NarrowPhaseAlgorithm.cpp @@ -0,0 +1,39 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "NarrowPhaseAlgorithm.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +NarrowPhaseAlgorithm::NarrowPhaseAlgorithm() { + +} + +// Destructor +NarrowPhaseAlgorithm::~NarrowPhaseAlgorithm() { + +} diff --git a/src/reactphysics3d/collision/NarrowPhaseAlgorithm.h b/src/reactphysics3d/collision/NarrowPhaseAlgorithm.h new file mode 100644 index 00000000..37b5f52a --- /dev/null +++ b/src/reactphysics3d/collision/NarrowPhaseAlgorithm.h @@ -0,0 +1,57 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef NARROWPHASEALGORITHM_H +#define NARROWPHASEALGORITHM_H + +// Libraries +#include "../body/BoundingVolume.h" +#include "ContactInfo.h" + +// Namespace ReactPhysics3D +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class NarrowPhaseAlgorithm : + This class is an abstract class that represents an algorithm + used to perform the narrow phase of a collision detection. The + goal of the narrow phase algorithm is to compute contact + informations of a collision between two bodies. + ------------------------------------------------------------------- +*/ +class NarrowPhaseAlgorithm { + private : + + public : + NarrowPhaseAlgorithm(); // Constructor + virtual ~NarrowPhaseAlgorithm(); // Destructor + + virtual bool testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, ContactInfo*& contactInfo)=0; // Return true and compute a contact info if the two bounding volume collide +}; + +} // End of reactphysics3d namespace + +#endif + + diff --git a/src/reactphysics3d/collision/NoBroadPhaseAlgorithm.cpp b/src/reactphysics3d/collision/NoBroadPhaseAlgorithm.cpp new file mode 100644 index 00000000..d0ed8790 --- /dev/null +++ b/src/reactphysics3d/collision/NoBroadPhaseAlgorithm.cpp @@ -0,0 +1,41 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "NoBroadPhaseAlgorithm.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +NoBroadPhaseAlgorithm::NoBroadPhaseAlgorithm() { + +} + +// Destructor +NoBroadPhaseAlgorithm::~NoBroadPhaseAlgorithm() { + +} + + diff --git a/src/reactphysics3d/collision/NoBroadPhaseAlgorithm.h b/src/reactphysics3d/collision/NoBroadPhaseAlgorithm.h new file mode 100644 index 00000000..fc047547 --- /dev/null +++ b/src/reactphysics3d/collision/NoBroadPhaseAlgorithm.h @@ -0,0 +1,94 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef NOBROADPHASEALGORITHM_H +#define NOBROADPHASEALGORITHM_H + +// Libraries +#include "BroadPhaseAlgorithm.h" +#include + +// Namespace ReactPhysics3D +namespace reactphysics3d { + +/* -------------------------------------------------------------------- + Class NoBroadPhaseAlgorithm : + This class implements a broad-phase algorithm that does nothing. + It should be use if we don't want to perform a broad-phase for + the collision detection. + -------------------------------------------------------------------- +*/ +class NoBroadPhaseAlgorithm : public BroadPhaseAlgorithm { + protected : + std::vector bodies; // All bodies of the engine + + public : + NoBroadPhaseAlgorithm(); // Constructor + virtual ~NoBroadPhaseAlgorithm(); // Destructor + + virtual void computePossibleCollisionPairs(std::vector addedBodies, std::vector removedBodies, + std::vector >& possibleCollisionPairs); // Compute the possible collision pairs of bodies +}; + + +// Compute the possible collision pairs of bodies +// The arguments "addedBodies" and "removedBodies" are respectively the set +// of bodies that have been added and removed since the last broad-phase +// computation. Before the call, the argument "possibleCollisionPairs" +// correspond to the possible colliding pairs of bodies from the last broad-phase +// computation. This methods computes the current possible collision pairs of +// bodies and update the "possibleCollisionPairs" argument. This broad-phase +// algorithm doesn't do anything and therefore the "possibleCollisionPairs" set +// must contains all the possible pairs of bodies +inline void NoBroadPhaseAlgorithm::computePossibleCollisionPairs(std::vector addedBodies, std::vector removedBodies, + std::vector >& possibleCollisionPairs) { + // Add the new bodies + for (std::vector::iterator it = addedBodies.begin(); it < addedBodies.end(); it++) { + bodies.push_back(*it); + } + + // Remove the bodies to be removed + for (std::vector::iterator it = removedBodies.begin(); it < removedBodies.end(); it++) { + bodies.erase(std::find(bodies.begin(), bodies.end(), *it)); + } + + // If the set of bodies have been changed + if (addedBodies.size() + removedBodies.size() > 0) { + // Recompute all the possible pairs of bodies + possibleCollisionPairs.clear(); + for (std::vector::iterator it1 = addedBodies.begin(); it1 < addedBodies.end(); it1++) { + for (std::vector::iterator it2 = addedBodies.begin(); it2 < addedBodies.end(); it2++) { + if (*it1 != *it2) { + possibleCollisionPairs.push_back(std::make_pair(*it1, *it2)); + } + } + } + } +} + +} // End of reactphysics3d namespace + +#endif + + diff --git a/src/reactphysics3d/collision/SAPAlgorithm.cpp b/src/reactphysics3d/collision/SAPAlgorithm.cpp new file mode 100644 index 00000000..ffac1c73 --- /dev/null +++ b/src/reactphysics3d/collision/SAPAlgorithm.cpp @@ -0,0 +1,151 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "SAPAlgorithm.h" +#include + +// Namespaces +using namespace reactphysics3d; +using namespace std; + +// Initialize the static attributes +unsigned short int SAPAlgorithm::sortAxis = 0; + +// Constructor +SAPAlgorithm::SAPAlgorithm() { + +} + +// Destructor +SAPAlgorithm::~SAPAlgorithm() { + +} + +// Remove the AABB representation of a given set of bodies from the sortedAABBs set +void SAPAlgorithm::removeBodiesAABB(vector bodies) { + vector::iterator elemToRemove; + const AABB* aabb; + + // Removed the AABB of the bodies that have been removed + for (vector::iterator it = bodies.begin(); it != bodies.end(); it++) { + aabb = dynamic_cast((*it)->getBroadBoundingVolume()); + assert(aabb); + elemToRemove = find(sortedAABBs.begin(), sortedAABBs.end(), aabb); + assert((*elemToRemove) == aabb); + sortedAABBs.erase(elemToRemove); + } +} + +// Add the AABB representation of a given body in the sortedAABBs set +void SAPAlgorithm::addBodiesAABB(vector bodies) { + const AABB* aabb; + + for (vector::iterator it = bodies.begin(); it != bodies.end(); it++) { + aabb = 0; + aabb = dynamic_cast((*it)->getBroadBoundingVolume()); + assert(aabb); + sortedAABBs.push_back(aabb); + } +} + +// Compute the possible collision pairs of bodies +// The arguments "addedBodies" and "removedBodies" are respectively the set +// of bodies that have been added and removed since the last broad-phase +// computation. Before the call, the argument "possibleCollisionPairs" +// correspond to the possible colliding pairs of bodies from the last broad-phase +// computation. This methods computes the current possible collision pairs of +// bodies and update the "possibleCollisionPairs" argument. +void SAPAlgorithm::computePossibleCollisionPairs(vector addedBodies, vector removedBodies, + vector >& possibleCollisionPairs) { + double variance[3]; // Variance of the distribution of the AABBs on the three x, y and z axis + double esperance[] = {0.0, 0.0, 0.0}; // Esperance of the distribution of the AABBs on the three x, y and z axis + double esperanceSquare[] = {0.0, 0.0, 0.0}; // Esperance of the square of the distribution values of the AABBs on the three x, y and z axis + vector::iterator it; // Iterator on the sortedAABBs set + vector::iterator it2; // Second iterator + Vector3D center3D; // Center of the current AABB + double center[3]; // Coordinates of the center of the current AABB + int i; + const Body* body; // Body pointer on the body corresponding to an AABB + uint nbAABBs = sortedAABBs.size(); // Number of AABBs + + // Removed the bodies to remove + removeBodiesAABB(removedBodies); + + // Add the bodies to add + addBodiesAABB(addedBodies); + + // Sort the set of AABBs + sort(sortedAABBs.begin(), sortedAABBs.end(), compareAABBs); + + // Sweep the sorted set of AABBs + for (vector::iterator it = sortedAABBs.begin(); it != sortedAABBs.end(); it++) { + + // If the collision of the AABB's corresponding body is disabled + if (!(*it)->getBodyPointer()->getIsCollisionEnabled()) { + // Go to the next AABB to test + continue; + } + + // Center of the current AABB + center3D = (*it)->getCenter(); + center[0] = center3D.getX(); + center[1] = center3D.getY(); + center[2] = center3D.getZ(); + + // Update the esperance and esperanceSquare values to compute the variance + for (i=0; i<3; i++) { + esperance[i] += center[i]; + esperanceSquare[i] += center[i] * center[i]; + } + + // Test collision against all possible overlapping AABBs following the current one + for (it2 = it + 1; it2 < sortedAABBs.end(); it2++) { + // Stop when the tested AABBs are beyond the end of the current AABB + if ((*it2)->getMinValueOnAxis(sortAxis) > (*it)->getMaxValueOnAxis(sortAxis)) { + break; + } + + body = (*it2)->getBodyPointer(); + + // Test if both AABBs overlap + if (body->getIsCollisionEnabled() && (*it)->testCollision(*(*it2))) { + // Add the current pair of AABBs into the possibleCollisionPairs set + possibleCollisionPairs.push_back(make_pair((*it)->getBodyPointer(), (*it2)->getBodyPointer())); + } + } + } + + // Compute the variance of the distribution of the AABBs on the three x,y and z axis + for (i=0; i<3; i++) { + variance[i] = esperanceSquare[i] - esperance[i] * esperance[i] / nbAABBs; + } + + // Update the sorted Axis according to the axis with the largest variance + sortAxis = 0; + if (variance[1] > variance[0]) sortAxis = 1; + if (variance[2] > variance[sortAxis]) sortAxis = 2; +} + + diff --git a/src/reactphysics3d/collision/SAPAlgorithm.h b/src/reactphysics3d/collision/SAPAlgorithm.h new file mode 100644 index 00000000..39c9a34a --- /dev/null +++ b/src/reactphysics3d/collision/SAPAlgorithm.h @@ -0,0 +1,82 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef SAPALGORITHM_H +#define SAPALGORITHM_H + +// Libraries +#include "BroadPhaseAlgorithm.h" +#include "../body/AABB.h" + +// Namespace ReactPhysics3D +namespace reactphysics3d { + +/* -------------------------------------------------------------------- + Class SAPAlgorithm : + This class implements the Sweep and Prune (SAP) broad-phase + algorithm. This algorithm uses the AABB bounding-volume of the + bodies and keep a sorted representation of the intervals of the + bodies' AABB on the three x. y and z axis. Given this sorted + representation, we can efficiently compute the set of possible + colliding pairs of bodies. At each broad-phase computation, we + should sort the AABB according to the axis (x, y or z) with the + largest variance of the AABBs positions in order that the sorted + AABB's set does not change a lot between each computations. To do + so, we compute at each time the variance of each axis and select + the axis (sortAxis) with the largest variance for the next + broad-phase computation. + -------------------------------------------------------------------- +*/ +class SAPAlgorithm : public BroadPhaseAlgorithm { + protected : + std::vector sortedAABBs; // Sorted set of AABB of the bodies on one of the x.y or z axis + static unsigned short int sortAxis; // Current sorting axis (0 for x, 1 for y, 2 for z axis) + + static bool compareAABBs(const AABB* a, const AABB* b); // Static method that compare two AABBs (in order to sort them) + void removeBodiesAABB(std::vector bodies); // Remove the AABB representation of a given set of bodies from the sortedAABBs set + void addBodiesAABB(std::vector bodies); // Add the AABB representation of a given set of bodies in the sortedAABBs set + + public : + SAPAlgorithm(); // Constructor + virtual ~SAPAlgorithm(); // Destructor + + virtual void computePossibleCollisionPairs(std::vector addedBodies, std::vector removedBodies, + std::vector >& possibleCollisionPairs); // Compute the possible collision pairs of bodies +}; + +// Static method that compare two AABBs. This method will be used to compare to AABBs +// in order to sort them with the sort() function to obtain the sortedAABBs set. +// This method must return true if the AABB "a" goes before the AABB "b". We +// consider that "a" goes before "b" if the minimum value of "a" on the current +// sorting axis (sortAxis) is smaller than the minimum value of "b" on this same +// axis. +inline bool SAPAlgorithm::compareAABBs(const AABB* a, const AABB* b) { + return (a->getMinValueOnAxis(sortAxis) < b->getMinValueOnAxis(sortAxis)); +} + +} // End of reactphysics3d namespace + +#endif + + diff --git a/src/reactphysics3d/collision/SATAlgorithm.cpp b/src/reactphysics3d/collision/SATAlgorithm.cpp new file mode 100644 index 00000000..e47c782e --- /dev/null +++ b/src/reactphysics3d/collision/SATAlgorithm.cpp @@ -0,0 +1,420 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "SATAlgorithm.h" +#include "../body/OBB.h" +#include "../body/RigidBody.h" +#include "../constraint/Contact.h" +#include +#include +#include +#include + + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +SATAlgorithm::SATAlgorithm() { + +} + +// Destructor +SATAlgorithm::~SATAlgorithm() { + +} + +// Return true and compute a contact info if the two bounding volume collide. +// The method returns false if there is no collision between the two bounding volumes. +bool SATAlgorithm::testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, ContactInfo*& contactInfo) { + + assert(boundingVolume1 != boundingVolume2); + + // If the two bounding volumes are OBB + //const OBB* const obb1 = dynamic_cast(boundingVolume1); + //const OBB* const obb2 = dynamic_cast(boundingVolume2); + const OBB* const obb1 = dynamic_cast(boundingVolume1); + const OBB* const obb2 = dynamic_cast(boundingVolume2); + + // If the two bounding volumes are OBB + if (obb1 && obb2) { + // Compute the collision test between two OBB + return computeCollisionTest(obb1, obb2, contactInfo); + } + else { + return false; + } +} + + +// This method returns true and computes a contact info if the two OBB intersect. +// This method implements the separating algorithm between two OBBs. The goal of this method is to test if the +// two OBBs intersect or not. If they intersect we report a contact info and the method returns true. If +// they don't intersect, the method returns false. The separation axis that have to be tested for two +// OBB are the six face normals (3 for each OBB) and the nine vectors V = Ai x Bj where Ai is the ith face normal +// vector of OBB 1 and Bj is the jth face normal vector of OBB 2. We will use the notation Ai for the ith face +// normal of OBB 1 and Bj for the jth face normal of OBB 2. +bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const obb2, ContactInfo*& contactInfo) const { + + + double center; // Center of a projection interval + double radius1; // Radius of projection interval [min1, max1] + double radius2; // Radius of projection interval [min2, max2] + double min1; // Minimum of interval 1 + double max1; // Maximum of interval 1 + double min2; // Minimm of interval 2 + double max2; // Maximum of interval 2 + Vector3D normal; // Contact normal (correspond to the separation axis with the smallest positive penetration depth) + // The contact normal point out of OBB1 toward OBB2 + double minPenetrationDepth = DBL_MAX; // Minimum penetration depth detected among all separated axis + const double cutoff = 0.99; // Cutoff for cosine of angles between box axes + bool existsParallelPair = false; // True if there exists two face normals that are parallel. + // This is used because if a parallel pair exists, it is sufficient + // to test only the face normals of the OBBs for separation. Two nearly + // parallel faces can lead to all face normal tests reporting no separation + // along those directions. The cross product directions are tested next, but + // Ai x Bj is nearly the zero vector and can cause a report that the two OBBs + // are not intersecting when in fact they are. + double c[3][3]; // c[i][j] = DotProduct(obb1.Ai, obb2.Bj) + double absC[3][3]; // absC[i][j] = abs(DotProduct(obb1.Ai, obb2.Bj)) + double udc1[3]; // DotProduct(obb1.Ai, obb2.center - obb1.center) + double udc2[3]; // DotProduct(obb2.Ai, obb2.center - obb1.center) + + Vector3D boxDistance = obb2->getCenter() - obb1->getCenter(); // Vector between the centers of the OBBs + + // Axis A0 + for (int i=0; i<3; ++i) { + c[0][i] = obb1->getAxis(0).scalarProduct(obb2->getAxis(i)); + absC[0][i] = fabs(c[0][i]); + if (absC[0][i] > cutoff) { + existsParallelPair = true; + } + } + udc1[0] = obb1->getAxis(0).scalarProduct(boxDistance); + center = udc1[0]; + radius1 = obb1->getExtent(0); + radius2 = obb2->getExtent(0)*absC[0][0] + obb2->getExtent(1)*absC[0][1] + obb2->getExtent(2) * absC[0][2]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); + if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(0), boxDistance); // Compute the contact normal with the correct sign + } + + // Axis A1 + for (int i=0; i<3; ++i) { + c[1][i] = obb1->getAxis(1).scalarProduct(obb2->getAxis(i)); + absC[1][i] = fabs(c[1][i]); + if (absC[1][i] > cutoff) { + existsParallelPair = true; + } + } + udc1[1] = obb1->getAxis(1).scalarProduct(boxDistance); + center = udc1[1]; + radius1 = obb1->getExtent(1); + radius2 = obb2->getExtent(0)*absC[1][0] + obb2->getExtent(1)*absC[1][1] + obb2->getExtent(2) * absC[1][2]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); + if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(1), boxDistance); // Compute the contact normal with the correct sign + } + + // Axis A2 + for (int i=0; i<3; ++i) { + c[2][i] = obb1->getAxis(2).scalarProduct(obb2->getAxis(i)); + absC[2][i] = fabs(c[2][i]); + if (absC[2][i] > cutoff) { + existsParallelPair = true; + } + } + udc1[2] = obb1->getAxis(2).scalarProduct(boxDistance); + center = udc1[2]; + radius1 = obb1->getExtent(2); + radius2 = obb2->getExtent(0)*absC[2][0] + obb2->getExtent(1)*absC[2][1] + obb2->getExtent(2)*absC[2][2]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); + if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + } + + // Axis B0 + udc2[0] = obb2->getAxis(0).scalarProduct(boxDistance); + center = udc2[0]; + radius1 = obb1->getExtent(0)*absC[0][0] + obb1->getExtent(1)*absC[1][0] + obb1->getExtent(2) * absC[2][0]; + radius2 = obb2->getExtent(0); + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); + if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb2->getAxis(0), boxDistance); // Compute the contact normal with the correct sign + } + + // Axis B1 + udc2[1] = obb2->getAxis(1).scalarProduct(boxDistance); + center = udc2[1]; + radius1 = obb1->getExtent(0)*absC[0][1] + obb1->getExtent(1)*absC[1][1] + obb1->getExtent(2) * absC[2][1]; + radius2 = obb2->getExtent(1); + min1 = - radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); + if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb2->getAxis(1), boxDistance); // Compute the contact normal with the correct sign + } + + // Axis B2 + udc2[2] = obb2->getAxis(2).scalarProduct(boxDistance); + center = udc2[2]; + radius1 = obb1->getExtent(0)*absC[0][2] + obb1->getExtent(1)*absC[1][2] + obb1->getExtent(2)*absC[2][2]; + radius2 = obb2->getExtent(2); + min1 = - radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); + if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb2->getAxis(2), boxDistance); // Compute the contact normal with the correct sign + } + + // If there exists a parallel pair of face normals + if (existsParallelPair) { + // There exists a parallel pair of face normals and we have already checked all the face + // normals for separation. Therefore the OBBs must intersect + + // Compute the contact info + contactInfo = new ContactInfo(obb1, obb2, normal.getUnit(), minPenetrationDepth); + + return true; + } + + // Axis A0 x B0 + center = udc1[2] * c[1][0] - udc1[1] * c[2][0]; + radius1 = obb1->getExtent(1) * absC[2][0] + obb1->getExtent(2) * absC[1][0]; + radius2 = obb2->getExtent(1) * absC[0][2] + obb2->getExtent(2) * absC[0][1]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); + if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(0).crossProduct(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign + } + + // Axis A0 x B1 + center = udc1[2] * c[1][1] - udc1[1] * c[2][1]; + radius1 = obb1->getExtent(1) * absC[2][1] + obb1->getExtent(2) * absC[1][1]; + radius2 = obb2->getExtent(0) * absC[0][2] + obb2->getExtent(2) * absC[0][0]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); + if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(0).crossProduct(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign + } + + // Axis A0 x B2 + center = udc1[2] * c[1][2] - udc1[1] * c[2][2]; + radius1 = obb1->getExtent(1) * absC[2][2] + obb1->getExtent(2) * absC[1][2]; + radius2 = obb2->getExtent(0) * absC[0][1] + obb2->getExtent(1) * absC[0][0]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); + if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(0).crossProduct(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign + } + + // Axis A1 x B0 + center = udc1[0] * c[2][0] - udc1[2] * c[0][0]; + radius1 = obb1->getExtent(0) * absC[2][0] + obb1->getExtent(2) * absC[0][0]; + radius2 = obb2->getExtent(1) * absC[1][2] + obb2->getExtent(2) * absC[1][1]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); + if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(1).crossProduct(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign + } + + // Axis A1 x B1 + center = udc1[0] * c[2][1] - udc1[2] * c[0][1]; + radius1 = obb1->getExtent(0) * absC[2][1] + obb1->getExtent(2) * absC[0][1]; + radius2 = obb2->getExtent(0) * absC[1][2] + obb2->getExtent(2) * absC[1][0]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); + if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(1).crossProduct(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign + } + + // Axis A1 x B2 + center = udc1[0] * c[2][2] - udc1[2] * c[0][2]; + radius1 = obb1->getExtent(0) * absC[2][2] + obb1->getExtent(2) * absC[0][2]; + radius2 = obb2->getExtent(0) * absC[1][1] + obb2->getExtent(1) * absC[1][0]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); + if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(1).crossProduct(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign + } + + // Axis A2 x B0 + center = udc1[1] * c[0][0] - udc1[0] * c[1][0]; + radius1 = obb1->getExtent(0) * absC[1][0] + obb1->getExtent(1) * absC[0][0]; + radius2 = obb2->getExtent(1) * absC[2][2] + obb2->getExtent(2) * absC[2][1]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); + if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(2).crossProduct(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign + } + + // Axis A2 x B1 + center = udc1[1] * c[0][1] - udc1[0] * c[1][1]; + radius1 = obb1->getExtent(0) * absC[1][1] + obb1->getExtent(1) * absC[0][1]; + radius2 = obb2->getExtent(0) * absC[2][2] + obb2->getExtent(2) * absC[2][0]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); + if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(2).crossProduct(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign + } + + // Axis A2 x B2 + center = udc1[1] * c[0][2] - udc1[0] * c[1][2]; + radius1 = obb1->getExtent(0) * absC[1][2] + obb1->getExtent(1) * absC[0][2]; + radius2 = obb2->getExtent(0) * absC[2][1] + obb2->getExtent(1) * absC[2][0]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); + if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(2).crossProduct(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign + } + + // Compute the contact info + contactInfo = new ContactInfo(obb1, obb2, normal.getUnit(), minPenetrationDepth); + + return true; +} + +// This method computes and returns the penetration depth between two intervals. This method returns the computed +// penetration depth (note that it could return a negative penetration depth if the intervals are separated. +double SATAlgorithm::computePenetrationDepth(double min1, double max1, double min2, double max2) const { + + // Compute the length of both intervals + double lengthInterval1 = max1 - min1; + double lengthInterval2 = max2 - min2; + + // Compute the total length of both intervals + double minExtreme = std::min(min1, min2); + double maxExtreme = std::max(max1, max2); + double lengthBothIntervals = maxExtreme - minExtreme; + + // Compute the current penetration depth + return (lengthInterval1 + lengthInterval2) - lengthBothIntervals; +} diff --git a/src/reactphysics3d/collision/SATAlgorithm.h b/src/reactphysics3d/collision/SATAlgorithm.h new file mode 100644 index 00000000..4d465d95 --- /dev/null +++ b/src/reactphysics3d/collision/SATAlgorithm.h @@ -0,0 +1,76 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef SATALGORITHM_H +#define SATALGORITHM_H + +// Libraries +#include "NarrowPhaseAlgorithm.h" +#include "../constraint/Contact.h" +#include "../body/OBB.h" + +// ReactPhysics3D namespace +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class SATAlgorithm : + This class implements a narrow-phase algorithm. This algorithm + uses a separating axis theorem (SAT) to check if two bounding + volumes collide or not. If the + two bounding volumes collide we have to create a contact object + to describe the collision contact. The idea is to check if there + exists an axis where, if we project the two bounding volumes on + this axis, the two projections are separated. If we find at + least an axis where the projections of the two bounding volumes + are separated then we know that the two bounding volumes don't + intersect. + ------------------------------------------------------------------- +*/ +class SATAlgorithm : public NarrowPhaseAlgorithm { + private : + bool computeCollisionTest(const OBB* const obb1, const OBB* const obb2, ContactInfo*& contactInfo) const; // Return true and compute a contact info if the two OBB collide + double computePenetrationDepth(double min1, double max1, double min2, double max2) const; // Compute the penetration depth of two projection intervals + Vector3D computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const; // Compute a contact normal + + public : + SATAlgorithm(); // Constructor + ~SATAlgorithm(); // Destructor + + virtual bool testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide +}; + +// Return the contact normal with the correct sign (from obb1 toward obb2). "axis" is the axis vector direction where the +// collision occur and "distanceOfOBBs" is the vector (obb2.center - obb1.center). +inline Vector3D SATAlgorithm::computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const { + if (distanceOfOBBs.scalarProduct(axis) >= 0.0) { + return axis; + } + else { + return axis.getOpposite(); + } +} + +} // End of the ReactPhysics3D namespace + +#endif diff --git a/src/reactphysics3d/constraint/Constraint.cpp b/src/reactphysics3d/constraint/Constraint.cpp new file mode 100644 index 00000000..e5bfd3e0 --- /dev/null +++ b/src/reactphysics3d/constraint/Constraint.cpp @@ -0,0 +1,40 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "Constraint.h" + +// We want ot use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +Constraint::Constraint(Body* const body1, Body* const body2, uint nbAuxConstraints, bool active) + :body1(body1), body2(body2), active(active), nbAuxConstraints(nbAuxConstraints) { + +} + +// Destructor +Constraint::~Constraint() { + +} diff --git a/src/reactphysics3d/constraint/Constraint.h b/src/reactphysics3d/constraint/Constraint.h new file mode 100644 index 00000000..96121f97 --- /dev/null +++ b/src/reactphysics3d/constraint/Constraint.h @@ -0,0 +1,96 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef CONSTRAINT_H +#define CONSTRAINT_H + +// Libraries +#include "../body/Body.h" +#include "../mathematics/mathematics.h" + +// ReactPhysics3D namespace +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class Constraint : + This abstract class represents a constraint in the physics engine. + A constraint can be a collision contact or a joint for + instance. Each constraint have a jacobian matrix associated with + the first body of the constraint and a jacobian matrix associated + with the second body. Each constraint can also have some auxiliary + constraints. Those auxiliary constraint are all represented in the + auxiliary Jacobian matrix. Auxiliary constraints represents some + constraints associated with a main constraint. For instance, a + contact constraint between two bodies can have two associated + auxiliary constraints to represent the frictions force constraints + in two directions. + ------------------------------------------------------------------- +*/ +class Constraint { + protected : + Body* const body1; // Pointer to the first body of the constraint + Body* const body2; // Pointer to the second body of the constraint + bool active; // True if the constraint is active + uint nbAuxConstraints; // Number of auxiliary constraints associated with this constraint + + public : + Constraint(Body* const body1, Body* const body2, uint nbAuxConstraints, bool active); // Constructor // Constructor + virtual ~Constraint(); // Destructor + Body* const getBody1() const; // Return the reference to the body 1 + Body* const getBody2() const; // Return the reference to the body 2 // Evaluate the constraint + bool isActive() const; // Return true if the constraint is active // Return the jacobian matrix of body 2 + virtual void computeJacobian(int noBody, Matrix& jacobian) const=0; // Compute a part of the jacobian for a given body + virtual void computeAuxJacobian(int noBody, int noAuxConstraint, Matrix& auxJacobian) const=0; // Compute a part of the jacobian for an auxiliary constraint + virtual double computeLowerBound() const=0; // Compute the lowerbound of the constraint + virtual double computeUpperBound() const=0; // Compute the upperbound of the constraint + virtual void computeAuxLowerBounds(int beginIndex, Vector& auxLowerBounds) const=0; // Compute lowerbounds for the auxiliary constraints + virtual void computeAuxUpperBounds(int beginIndex, Vector& auxUpperBounds) const=0; // Compute upperbounds for the auxiliary constraints + virtual double computeErrorValue() const=0; // Compute the error value for the constraint + virtual void computeAuxErrorValues(int beginIndex, Vector& errorValues) const=0; // Compute the errors values of the auxiliary constraints + unsigned int getNbAuxConstraints() const; // Return the number of auxiliary constraints // Return the number of auxiliary constraints +}; + +// Return the reference to the body 1 +inline Body* const Constraint::getBody1() const { + return body1; +} + +// Return the reference to the body 2 +inline Body* const Constraint::getBody2() const { + return body2; +} + +// Return true if the constraint is active +inline bool Constraint::isActive() const { + return active; +} + +// Return the number auxiliary constraints +inline uint Constraint::getNbAuxConstraints() const { + return nbAuxConstraints; +} + +} // End of the ReactPhysics3D namespace + +#endif diff --git a/src/reactphysics3d/constraint/Contact.cpp b/src/reactphysics3d/constraint/Contact.cpp new file mode 100644 index 00000000..9a37b9b5 --- /dev/null +++ b/src/reactphysics3d/constraint/Contact.cpp @@ -0,0 +1,47 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "Contact.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +Contact::Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const Vector3D& point) + :Constraint(body1, body2, 2, true), normal(normal), penetrationDepth(penetrationDepth), point(point) { + + // Compute the auxiliary lower and upper bounds + // TODO : Now mC is only the mass of the first body but it is probably wrong + // TODO : Now g is 9.81 but we should use the true gravity value of the physics world. + mu_mc_g = FRICTION_COEFFICIENT * body1->getMass() * 9.81; + + // Compute the friction vectors that span the tangential friction plane + computeFrictionVectors(); +} + +// Destructor +Contact::~Contact() { + +} diff --git a/src/reactphysics3d/constraint/Contact.h b/src/reactphysics3d/constraint/Contact.h new file mode 100644 index 00000000..753447c2 --- /dev/null +++ b/src/reactphysics3d/constraint/Contact.h @@ -0,0 +1,246 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef CONTACT_H +#define CONTACT_H + +// Libraries +#include "../typeDefinitions.h" +#include "Constraint.h" +#include "../body/RigidBody.h" +#include "../mathematics/mathematics.h" +#include // TODO : Remove this in the final version +#include // TODO : Remove this in the final version + +// ReactPhysics3D namespace +namespace reactphysics3d { + +// Constants +const double FRICTION_COEFFICIENT = 0.3; // Friction coefficient +const double PENETRATION_FACTOR = 0.0; // Penetration factor (between 0 and 1) which specify the importance of the + // penetration depth in order to calculate the correct impulse for the contact + +/* ------------------------------------------------------------------- + Class Contact : + This class represents a collision contact between two bodies in + the physics engine. The contact class inherits from the + Constraint class. The collision detection system computes + contact informations that will be used to perform the collision + response. Each contact contains only one contact point. A contact + constraint has two auxiliary constraints in order two represent + the two friction forces at the contact surface. + ------------------------------------------------------------------- +*/ +class Contact : public Constraint { + protected : + const Vector3D normal; // Normal vector of the contact (From body1 toward body2) + const double penetrationDepth; // Penetration depth + const Vector3D point; // Contact point + std::vector frictionVectors; // Two orthogonal vectors that span the tangential friction plane + double mu_mc_g; + + void computeFrictionVectors(); // Compute the two friction vectors that span the tangential friction plane + + public : + Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const Vector3D& point); // Constructor + virtual ~Contact(); // Destructor + + Vector3D getNormal() const; // Return the normal vector of the contact + Vector3D getPoint() const; // Return the contact point + virtual void computeJacobian(int noBody, Matrix& jacobian) const; // Compute a part of the jacobian for a given body + virtual void computeAuxJacobian(int noBody, int noAuxConstraint, Matrix& auxJacobian) const; // Compute a part of the jacobian for an auxiliary constraint + virtual double computeLowerBound() const; // Compute the lowerbound of the constraint + virtual double computeUpperBound() const; // Compute the upperbound of the constraint + virtual void computeAuxLowerBounds(int beginIndex, Vector& auxLowerBounds) const; // Compute the lowerbounds for the auxiliary constraints + virtual void computeAuxUpperBounds(int beginIndex, Vector& auxLowerBounds) const; // Compute the upperbounds for the auxiliary constraints + virtual double computeErrorValue() const; // Compute the error value for the constraint + virtual void computeAuxErrorValues(int beginIndex, Vector& errorValues) const; // Compute the errors values of the auxiliary constraints + uint getNbAuxConstraints() const; // Return the number of auxiliary constraints + double getPenetrationDepth() const; // Return the penetration depth + void draw() const; // TODO : Delete this (Used to debug collision detection) +}; + +// Compute the two unit orthogonal vectors "v1" and "v2" that span the tangential friction plane +// The two vectors have to be such that : v1 x v2 = contactNormal +inline void Contact::computeFrictionVectors() { + // Delete the current friction vectors + frictionVectors.clear(); + + // Compute the first orthogonal vector + Vector3D vector1 = normal.getOneOrthogonalVector(); + frictionVectors.push_back(vector1); + + // Compute the second orthogonal vector using the cross product + frictionVectors.push_back(normal.crossProduct(vector1)); +} + +// Return the normal vector of the contact +inline Vector3D Contact::getNormal() const { + return normal; +} + +// Return the contact points +inline Vector3D Contact::getPoint() const { + return point; +} + +// Return the penetration depth of the contact +inline double Contact::getPenetrationDepth() const { + return penetrationDepth; +} + +// This method computes a part of the jacobian matrix for a given body. +// The argument "noBody" is 1 or 2 and corresponds to which one of the two +// bodies of the constraint we will compute the jacobian part. The argument +// "jacobian" is a 1x6 jacobian matrix of the constraint corresponding to one of +// the two bodies of the constraint. +inline void Contact::computeJacobian(int noBody, Matrix& jacobian) const { + RigidBody* rigidBody; + Vector3D rCrossN; + Vector3D r; + Vector3D norm = normal; + + assert(noBody == 1 || noBody == 2); + assert(jacobian.getNbRow() == 1 && jacobian.getNbColumn() == 6); + + if (noBody == 1) { + rigidBody = dynamic_cast(body1); + assert(rigidBody); + r = point - rigidBody->getPosition(); + rCrossN = r.crossProduct(normal).getOpposite(); + norm = normal.getOpposite(); + } + else { + rigidBody = dynamic_cast(body2); + assert(rigidBody); + r = point - rigidBody->getPosition(); + rCrossN = r.crossProduct(normal); + } + + // Compute the jacobian matrix for the body 1 + jacobian.setValue(0, 0, norm.getX()); + jacobian.setValue(0, 1, norm.getY()); + jacobian.setValue(0, 2, norm.getZ()); + jacobian.setValue(0, 3, rCrossN.getX()); + jacobian.setValue(0, 4, rCrossN.getY()); + jacobian.setValue(0, 5, rCrossN.getZ()); +} + +// Compute a part of the jacobian matrix for an auxiliary constraint (given by "noAuxConstraint") +// and one of the two bodies (given by "noBody") of the contact. The argument "noBody" is 1 or 2 and +// argument auxJacobian is a 1x6 matrix. +inline void Contact::computeAuxJacobian(int noBody, int noAuxConstraint, Matrix& auxJacobian) const { + Vector3D r; + Vector3D rCrossU; + RigidBody* rigidBody; + double sign; + + assert(noBody == 1 || noBody == 2); + assert(noAuxConstraint == 1 || noAuxConstraint == 2); + assert(auxJacobian.getNbRow() == 1 && auxJacobian.getNbColumn() == 6); + + if (noBody == 1) { + rigidBody = dynamic_cast(body1); + assert(rigidBody); + r = point - rigidBody->getPosition(); + sign = -1.0; + } + else { + rigidBody = dynamic_cast(body2); + assert(rigidBody); + r = point - rigidBody->getPosition(); + sign = 1.0; + } + + rCrossU = r.crossProduct(frictionVectors[noAuxConstraint-1]); + + auxJacobian.setValue(0, 0, sign * frictionVectors[noAuxConstraint-1].getX()); + auxJacobian.setValue(0, 1, sign * frictionVectors[noAuxConstraint-1].getY()); + auxJacobian.setValue(0, 2, sign * frictionVectors[noAuxConstraint-1].getZ()); + auxJacobian.setValue(0, 3, sign * rCrossU.getX()); + auxJacobian.setValue(0, 4, sign * rCrossU.getY()); + auxJacobian.setValue(0, 5, sign * rCrossU.getZ()); +} + +// Compute the lowerbounds for the auxiliary constraints +inline double Contact::computeLowerBound() const { + return 0.0; +} + +// Compute the upperbounds for the auxiliary constraints +inline double Contact::computeUpperBound() const { + return INFINITY_CONST; +} + +// Compute the lowerbounds for the auxiliary constraints. This method fills the "auxLowerBounds" +// vector starting at the index "beginIndex" in this vector. +inline void Contact::computeAuxLowerBounds(int beginIndex, Vector& auxLowerBounds) const { + assert(beginIndex + nbAuxConstraints <= auxLowerBounds.getNbComponent()); + + auxLowerBounds.setValue(beginIndex, -mu_mc_g); + auxLowerBounds.setValue(beginIndex + 1, -mu_mc_g); + +} + +// Compute the upperbounds for the auxiliary constraints. This method fills the "auxUpperBounds" +// vector starting at the index "beginIndex" in this vector. +inline void Contact::computeAuxUpperBounds(int beginIndex, Vector& auxUpperBounds) const { + assert(beginIndex + nbAuxConstraints <= auxUpperBounds.getNbComponent()); + + auxUpperBounds.setValue(beginIndex, mu_mc_g); + auxUpperBounds.setValue(beginIndex + 1, mu_mc_g); +} + +// Compute the error value for the constraint +inline double Contact::computeErrorValue() const { + RigidBody* rigidBody1 = dynamic_cast(body1); + RigidBody* rigidBody2 = dynamic_cast(body2); + + assert(rigidBody1); + assert(rigidBody2); + + Vector3D velocity1 = rigidBody1->getLinearVelocity(); + Vector3D velocity2 = rigidBody2->getLinearVelocity(); + double restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution(); + double errorValue = restitutionCoeff * (normal.scalarProduct(velocity1) - normal.scalarProduct(velocity2)) + PENETRATION_FACTOR * penetrationDepth; + return errorValue; +} + +// Compute the errors values of the auxiliary constraints +inline void Contact::computeAuxErrorValues(int beginIndex, Vector& errorValues) const { + assert(beginIndex + nbAuxConstraints <= errorValues.getNbComponent()); + + errorValues.setValue(beginIndex, 0.0); + errorValues.setValue(beginIndex + 1, 0.0); +} + +// TODO : Delete this (Used to debug collision detection) +inline void Contact::draw() const { + glColor3f(1.0, 0.0, 0.0); + glutSolidSphere(0.3, 20, 20); +} + +} // End of the ReactPhysics3D namespace + +#endif diff --git a/src/reactphysics3d/engine/ConstraintSolver.cpp b/src/reactphysics3d/engine/ConstraintSolver.cpp new file mode 100644 index 00000000..2476226e --- /dev/null +++ b/src/reactphysics3d/engine/ConstraintSolver.cpp @@ -0,0 +1,382 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "ConstraintSolver.h" +#include "../mathematics/lcp/LCPProjectedGaussSeidel.h" +#include "../body/RigidBody.h" + +using namespace reactphysics3d; +using namespace std; + +// Constructor +ConstraintSolver::ConstraintSolver(PhysicsWorld* world) + :physicsWorld(world), bodyMapping(0), nbConstraints(0), constraintsCapacity(0), + bodiesCapacity(0), avConstraintsCapacity(0), avBodiesCapacity(0), avBodiesNumber(0), + avConstraintsNumber(0), avBodiesCounter(0), avConstraintsCounter(0), + lcpSolver(new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) { + +} + +// Destructor +ConstraintSolver::~ConstraintSolver() { + +} + + // Initialize the constraint solver before each solving +void ConstraintSolver::initialize() { + Constraint* constraint; + + nbConstraints = 0; + + // For each constraint + vector::iterator it; + for (it = physicsWorld->getConstraintsBeginIterator(); it != physicsWorld->getConstraintsEndIterator(); it++) { + constraint = *it; + + // If the constraint is active + if (constraint->isActive()) { + activeConstraints.push_back(constraint); + + // Add the two bodies of the constraint in the constraintBodies list + constraintBodies.insert(constraint->getBody1()); + constraintBodies.insert(constraint->getBody2()); + + // Fill in the body number maping + bodyNumberMapping.insert(pair(constraint->getBody1(), bodyNumberMapping.size())); + bodyNumberMapping.insert(pair(constraint->getBody2(), bodyNumberMapping.size())); + + // Update the size of the jacobian matrix + nbConstraints += (1 + constraint->getNbAuxConstraints()); + } + } + + // Compute the number of bodies that are part of some active constraint + nbBodies = bodyNumberMapping.size(); + + assert(nbConstraints > 0); + assert(nbBodies > 0); + + // Update the average bodies and constraints capacities + if (avBodiesCounter > AV_COUNTER_LIMIT) { + avBodiesCounter = 0; + avBodiesNumber = 0; + } + if (avConstraintsCounter > AV_COUNTER_LIMIT) { + avConstraintsCounter = 0; + avConstraintsNumber = 0; + } + avBodiesCounter++; + avConstraintsCounter++; + avBodiesNumber += nbBodies; + avConstraintsNumber += nbConstraints; + avBodiesCapacity += (avBodiesNumber / avBodiesCounter); + avConstraintsCapacity += (avConstraintsNumber / avConstraintsCounter); + + // Allocate the memory needed for the constraint solver + allocate(); +} + +// Allocate all the memory needed to solve the LCP problem +// The goal of this method is to avoid to free and allocate the memory +// each time the constraint solver is called but only if the we effectively +// need more memory. Therefore if for instance the number of constraints to +// be solved is smaller than the constraints capacity, we don't free and reallocate +// memory because we don't need to. The problem now is that the constraints capacity +// can grow indefinitely. Therefore we use a way to free and reallocate the memory +// if the average number of constraints currently solved is far less than the current +// constraints capacity +void ConstraintSolver::allocate() { + // If we need to allocate more memory for the bodies + if (nbBodies > bodiesCapacity || avBodiesCapacity < AV_PERCENT_TO_FREE * bodiesCapacity) { + freeMemory(true); + bodiesCapacity = nbBodies; + + Minv_sp = new Matrix[nbBodies]; + V1 = new Vector[nbBodies]; + Vconstraint = new Vector[nbBodies]; + Fext = new Vector[nbBodies]; + + avBodiesNumber = 0; + avBodiesCounter = 0; + } + + // If we need to allocate more memory for the constraints + if (nbConstraints > constraintsCapacity || constraintsCapacity < AV_PERCENT_TO_FREE * constraintsCapacity) { + freeMemory(false); + constraintsCapacity = nbConstraints; + + bodyMapping = new Body**[nbConstraints]; + J_sp = new Matrix*[nbConstraints]; + B_sp = new Matrix*[2]; + B_sp[0] = new Matrix[nbConstraints]; + B_sp[1] = new Matrix[nbConstraints]; + for (uint i=0; i 0) { + delete[] Minv_sp; + delete[] V1; + delete[] Vconstraint; + delete[] Fext; + } + else if (constraintsCapacity > 0) { // If we need to free the constraints memory + // Free the bodyMaping array + for (uint i=0; icomputeJacobian(1, J_sp[noConstraint][0]); + constraint->computeJacobian(2, J_sp[noConstraint][1]); + + // Fill in the body mapping matrix + bodyMapping[noConstraint][0] = constraint->getBody1(); + bodyMapping[noConstraint][1] = constraint->getBody2(); + + // Fill in the limit vectors for the constraint + lowerBounds.setValue(noConstraint, constraint->computeLowerBound()); + upperBounds.setValue(noConstraint, constraint->computeUpperBound()); + + // Fill in the error vector + errorValues.setValue(noConstraint, constraint->computeErrorValue()); + + // If it's a contact constraint + contact = dynamic_cast(constraint); + if (contact) { + // Get the lambda init value from the cache if exists + contactInfo = contactCache.getContactCachingInfo(contact->getBody1(), contact->getBody2(), contact->getPoint()); + if (contactInfo) { + // The last lambda init value was in the cache + lambdaInit.setValue(noConstraint, contactInfo->lambda); + } + else { + // The las lambda init value was not in the cache + lambdaInit.setValue(noConstraint, 0.0); + } + } + else { + // Set the lambda init value + lambdaInit.setValue(noConstraint, 0.0); + } + + nbAuxConstraints = constraint->getNbAuxConstraints(); + + // If the current constraint has auxiliary constraints + if (nbAuxConstraints > 0) { + + // For each auxiliary constraints + for (uint i=1; i<=nbAuxConstraints; i++) { + // Fill in the J_sp matrix + J_sp[noConstraint+i][0].changeSize(1, 6); + J_sp[noConstraint+i][1].changeSize(1, 6); + constraint->computeAuxJacobian(1, i, J_sp[noConstraint+i][0]); + constraint->computeAuxJacobian(2, i, J_sp[noConstraint+i][1]); + + // Fill in the body mapping matrix + bodyMapping[noConstraint+i][0] = constraint->getBody1(); + bodyMapping[noConstraint+i][1] = constraint->getBody2(); + + // Fill in the init lambda value for the constraint + lambdaInit.setValue(noConstraint+i, 0.0); + } + + // Fill in the limit vectors for the auxilirary constraints + constraint->computeAuxLowerBounds(noConstraint+1, lowerBounds); + constraint->computeAuxUpperBounds(noConstraint+1, upperBounds); + + // Fill in the errorValues vector for the auxiliary constraints + constraint->computeAuxErrorValues(noConstraint+1, errorValues); + } + + noConstraint += 1 + nbAuxConstraints; + } + + // For each current body that is implied in some constraint + RigidBody* rigidBody; + Body* body; + Vector v(6); + Vector f(6); + Matrix identity = Matrix::identity(3); + Matrix mInv(6,6); + uint b=0; + for (set::iterator it = constraintBodies.begin(); it != constraintBodies.end(); it++, b++) { + body = *it; + uint bodyNumber = bodyNumberMapping.at(body); + + // TODO : Use polymorphism and remove this downcasting + rigidBody = dynamic_cast(body); + assert(rigidBody != 0); + + // Compute the vector V1 with initial velocities values + v.fillInSubVector(0, rigidBody->getLinearVelocity()); + v.fillInSubVector(3, rigidBody->getAngularVelocity()); + V1[bodyNumber].changeSize(6); + V1[bodyNumber] = v; + + // Compute the vector Vconstraint with final constraint velocities + Vconstraint[bodyNumber].changeSize(6); + Vconstraint[bodyNumber].initWithValue(0.0); + + // Compute the vector with forces and torques values + f.fillInSubVector(0, rigidBody->getExternalForce()); + f.fillInSubVector(3, rigidBody->getExternalTorque()); + Fext[bodyNumber].changeSize(6); + Fext[bodyNumber] = f; + + // Compute the inverse sparse mass matrix + mInv.initWithValue(0.0); + if (rigidBody->getIsMotionEnabled()) { + mInv.fillInSubMatrix(0, 0, rigidBody->getMassInverse() * identity); + mInv.fillInSubMatrix(3, 3, rigidBody->getInertiaTensorInverseWorld()); + } + Minv_sp[bodyNumber].changeSize(6, 6); + Minv_sp[bodyNumber] = mInv; + } +} + +// Compute the vector b +void ConstraintSolver::computeVectorB(double dt) { + uint indexBody1, indexBody2; + double oneOverDT = 1.0/dt; + + b = errorValues * oneOverDT; + + for (uint c = 0; c(activeConstraints.at(c)); + if (contact) { + // Create a new ContactCachingInfo + contactInfo = new ContactCachingInfo(contact->getBody1(), contact->getBody2(), contact->getPoint(), lambda.getValue(noConstraint)); + + // Add it to the contact cache + contactCache.addContactCachingInfo(contactInfo); + } + + noConstraint += 1 + activeConstraints.at(c)->getNbAuxConstraints(); + } +} diff --git a/src/reactphysics3d/engine/ConstraintSolver.h b/src/reactphysics3d/engine/ConstraintSolver.h new file mode 100644 index 00000000..9865ca97 --- /dev/null +++ b/src/reactphysics3d/engine/ConstraintSolver.h @@ -0,0 +1,171 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef CONSTRAINTSOLVER_H +#define CONSTRAINTSOLVER_H + +// Libraries +#include "../typeDefinitions.h" +#include "../constraint/Constraint.h" +#include "../mathematics/lcp/LCPSolver.h" +#include "ContactCache.h" +#include "PhysicsWorld.h" +#include +#include + +// ReactPhysics3D namespace +namespace reactphysics3d { + +// Constants +const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when solving a LCP problem +const double AV_COUNTER_LIMIT = 500; // Maximum number value of the avBodiesCounter or avConstraintsCounter +const double AV_PERCENT_TO_FREE = 0.5; // We will free the memory if the current nb of bodies (or constraints) is + // less than AV_PERCENT_TO_FREE * bodiesCapacity (or constraintsCapacity). This + // is used to avoid to keep to much memory for a long time if the system doesn't + // need that memory. This value is between 0.0 and 1.0 + + /* ------------------------------------------------------------------- + Class ConstrainSolver : + This class represents the constraint solver. The goal is to + solve A constraint-base LCP problem. + ------------------------------------------------------------------- +*/ +class ConstraintSolver { + protected: + PhysicsWorld* physicsWorld; // Reference to the physics world + LCPSolver* lcpSolver; // LCP Solver + ContactCache contactCache; // Contact cache + std::vector activeConstraints; // Current active constraints in the physics world + uint nbConstraints; // Total number of constraints (with the auxiliary constraints) + uint nbBodies; // Current number of bodies in the physics world + uint constraintsCapacity; // Number of constraints that are currently allocated in memory in the solver + uint bodiesCapacity; // Number of bodies that are currently allocated in memory in the solver + uint avConstraintsCapacity; // Average constraint capacity + uint avBodiesCapacity; // Average bodies capacity + uint avBodiesNumber; // Total bodies number for average computation + uint avConstraintsNumber; // Total constraints number for average computation + uint avBodiesCounter; // Counter used to compute the average + uint avConstraintsCounter; + std::set constraintBodies; // Bodies that are implied in some constraint + std::map bodyNumberMapping; // Map a body pointer with its index number + Body*** bodyMapping; // 2-dimensional array that contains the mapping of body reference + // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains + // the pointer to the body that correspond to the 1x6 J_ij matrix in the + // J_sp matrix. A integer body index refers to its index in the "bodies" std::vector + Matrix** J_sp; // 2-dimensional array thar correspond to the sparse representation of the jacobian matrix of all constraints + // The dimension of this array is nbConstraints times 2. Each cell will contain + // a 1x6 matrix + Matrix** B_sp; // 2-dimensional array that correspond to a useful matrix in sparse representation + // The dimension of this array is 2 times nbConstraints. Each cell will contain + // a 6x1 matrix + Vector b; // Vector "b" of the LCP problem + Vector lambda; // Lambda vector of the LCP problem + Vector lambdaInit; // Lambda init vector for the LCP solver + Vector errorValues; // Error vector of all constraints + Vector lowerBounds; // Vector that contains the low limits for the variables of the LCP problem + Vector upperBounds; // Vector that contains the high limits for the variables of the LCP problem + Matrix* Minv_sp; // Sparse representation of the Matrix that contains information about mass and inertia of each body + // This is an array of size nbBodies that contains in each cell a 6x6 matrix + Vector* V1; // Array that contains for each body the Vector that contains linear and angular velocities + // Each cell contains a 6x1 vector with linear and angular velocities + Vector* Vconstraint; // Same kind of vector as V1 but contains the final constraint velocities + Vector* Fext; // Array that contains for each body the vector that contains external forces and torques + // Each cell contains a 6x1 vector with external force and torque. + void initialize(); // Initialize the constraint solver before each solving + void allocate(); // Allocate all the memory needed to solve the LCP problem + void fillInMatrices(); // Fill in all the matrices needed to solve the LCP problem + void computeVectorB(double dt); // Compute the vector b + void computeMatrixB_sp(); // Compute the matrix B_sp + void computeVectorVconstraint(double dt); // Compute the vector V2 + void updateContactCache(); // Clear and Fill in the contact cache with the new lambda values + void freeMemory(bool freeBodiesMemory); // Free some memory previously allocated for the constraint solver + + public: + ConstraintSolver(PhysicsWorld* world); // Constructor + virtual ~ConstraintSolver(); // Destructor + void solve(double dt); // Solve the current LCP problem + bool isConstrainedBody(Body* body) const; // Return true if the body is in at least one constraint + Vector3D getConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem + Vector3D getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem + void cleanup(); +}; + +// Return true if the body is in at least one constraint +inline bool ConstraintSolver::isConstrainedBody(Body* body) const { + if(constraintBodies.find(body) != constraintBodies.end()) { + return true; + } + return false; +} + +// Return the constrained linear velocity of a body after solving the LCP problem +inline Vector3D ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body) { + assert(isConstrainedBody(body)); + Vector vec = Vconstraint[bodyNumberMapping[body]].getSubVector(0, 3); + return Vector3D(vec.getValue(0), vec.getValue(1), vec.getValue(2)); + +} + +// Return the constrained angular velocity of a body after solving the LCP problem +inline Vector3D ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) { + assert(isConstrainedBody(body)); + Vector vec = Vconstraint[bodyNumberMapping[body]].getSubVector(3, 3); + return Vector3D(vec.getValue(0), vec.getValue(1), vec.getValue(2)); +} + +// Solve the current LCP problem +inline void ConstraintSolver::solve(double dt) { + // Allocate memory for the matrices + initialize(); + + // Fill-in all the matrices needed to solve the LCP problem + fillInMatrices(); + + // Compute the vector b + computeVectorB(dt); + + // Compute the matrix B + computeMatrixB_sp(); + + // Solve the LCP problem (computation of lambda) + lcpSolver->setLambdaInit(lambdaInit); + lcpSolver->solve(J_sp, B_sp, nbConstraints, nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda); + + // Update the contact chaching informations + updateContactCache(); + + // Compute the vector Vconstraint + computeVectorVconstraint(dt); +} + +// Cleanup of the constraint solver +inline void ConstraintSolver::cleanup() { + bodyNumberMapping.clear(); + constraintBodies.clear(); + activeConstraints.clear(); +} + +} // End of ReactPhysics3D namespace + +#endif \ No newline at end of file diff --git a/src/reactphysics3d/engine/ContactCache.cpp b/src/reactphysics3d/engine/ContactCache.cpp new file mode 100644 index 00000000..88b63463 --- /dev/null +++ b/src/reactphysics3d/engine/ContactCache.cpp @@ -0,0 +1,92 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "ContactCache.h" + +using namespace reactphysics3d; +using namespace std; + +// Constructor +ContactCache::ContactCache() { + +} + +// Destructor +ContactCache::~ContactCache() { + +} + +// Remove all the contact caching info of the cache +void ContactCache::clear() { + // Clear the cache + cache.clear(); +} + +// Add a new contact caching info in the cache +void ContactCache::addContactCachingInfo(ContactCachingInfo* info) { + // Check if there is already an entry for that pair of body in the cache + map, vector >::iterator entry = cache.find(make_pair(info->body1, info->body2)); + if (entry != cache.end()) { + (*entry).second.push_back(info); + } + else { + // Add a new entry in the cache + vector vec; + vec.push_back(info); + cache.insert(make_pair(make_pair(info->body1, info->body2), vec)); + } +} + +// Return the ContactCachingInfo (if exists) corresponding to the arguments +// If the contact pair between the two bodies in argument doesn't exist this or there is no ContactCachingInfo +// compatible (with approximatively the same position), this method returns NULL. +ContactCachingInfo* ContactCache::getContactCachingInfo(Body* body1, Body* body2, const Vector3D& position) { + // Check if there is an entry for that pair of body in the cache + map, vector >::iterator entry = cache.find(make_pair(body1, body2)); + if (entry != cache.end()) { + vector vec = (*entry).second; + assert((*entry).first.first == body1); + assert((*entry).first.second == body2); + + double posX = position.getX(); + double posY = position.getY(); + double posZ = position.getZ(); + + // Check among all the old contacts for this pair of body if there is an approximative match for the contact position + for(vector::iterator it = vec.begin(); it != vec.end(); it++) { + Vector3D& contactPos = (*it)->position; + if (posX <= contactPos.getX() + POSITION_TOLERANCE && posX >= contactPos.getX() - POSITION_TOLERANCE && + posY <= contactPos.getY() + POSITION_TOLERANCE && posY >= contactPos.getY() - POSITION_TOLERANCE && + posZ <= contactPos.getZ() + POSITION_TOLERANCE && posZ >= contactPos.getZ() - POSITION_TOLERANCE) { + // Return the ContactCachingInfo + return *it; + } + } + } + else { + return 0; + } +} + diff --git a/src/reactphysics3d/engine/ContactCache.h b/src/reactphysics3d/engine/ContactCache.h new file mode 100644 index 00000000..5455ba79 --- /dev/null +++ b/src/reactphysics3d/engine/ContactCache.h @@ -0,0 +1,63 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef CONTACTCACHE_H +#define CONTACTCACHE_H + +// Libraries +#include +#include +#include +#include "ContactCachingInfo.h" + +// ReactPhysics3D namespace +namespace reactphysics3d { + +// Constant +const double POSITION_TOLERANCE = 0.01 ; // Tolerance used to consider two similar positions to be considered as the same + +/* ------------------------------------------------------------------- + Class ContactCache : + This class is used to cache the contact in order to be able to + use the lambda values in the last time step in the next time step in the + constraint solver to improve the convergence rate of the PGS algorithm. + This class will cache the ContactCachingInfo objects at each time step. + ------------------------------------------------------------------- +*/ +class ContactCache { + private: + std::map, std::vector > cache; + + public: + ContactCache(); // Constructor + ~ContactCache(); // Destructor + void clear(); // Remove all the contact caching info of the cache + void addContactCachingInfo(ContactCachingInfo* contactCachingInfo); // Add a new contact caching info in the cache + ContactCachingInfo* getContactCachingInfo(Body* body1, Body* body2, const Vector3D& position); // Return the ContactCachingInfo (if exists) corresponding to the arguments +}; + +} // End of the ReactPhysics3D namespace + +#endif + diff --git a/src/reactphysics3d/engine/ContactCachingInfo.cpp b/src/reactphysics3d/engine/ContactCachingInfo.cpp new file mode 100644 index 00000000..680f24a2 --- /dev/null +++ b/src/reactphysics3d/engine/ContactCachingInfo.cpp @@ -0,0 +1,34 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "ContactCachingInfo.h" + +using namespace reactphysics3d; + +// Constructor +ContactCachingInfo::ContactCachingInfo(Body* body1, Body* body2, const Vector3D& position, double lambda) + : body1(body1), body2(body2), position(position), lambda(lambda) { + +} diff --git a/src/reactphysics3d/engine/ContactCachingInfo.h b/src/reactphysics3d/engine/ContactCachingInfo.h new file mode 100644 index 00000000..29e07662 --- /dev/null +++ b/src/reactphysics3d/engine/ContactCachingInfo.h @@ -0,0 +1,56 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef CONTACTCACHINGINFO_H +#define CONTACTCACHINGINFO_H + +// Libraries +#include "../body/OBB.h" + +// ReactPhysics3D namespace +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Structure ContactCachingInfo : + This structure contains informations used to cache the last lambda + value of each contact constraint. The last lambda value is used + as the init lambda value in the constraint solver in the next + time step to improve the convergence rate of the constraint solver. + ------------------------------------------------------------------- +*/ +struct ContactCachingInfo { + public: + // TODO : Use polymorphism here (change OBB into BoundingVolume to be more general) + Body* body1; // Body pointer of the first bounding volume + Body* body2; // Body pointer of the second bounding volume + Vector3D position; // Contact point + double lambda; // Last lambda value for the constraint + + ContactCachingInfo(Body* body1, Body* body2, const Vector3D& position, double lambda); // Constructor +}; + +} // End of the ReactPhysics3D namespace + +#endif + diff --git a/src/reactphysics3d/engine/PhysicsEngine.cpp b/src/reactphysics3d/engine/PhysicsEngine.cpp new file mode 100644 index 00000000..53f96f9f --- /dev/null +++ b/src/reactphysics3d/engine/PhysicsEngine.cpp @@ -0,0 +1,207 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "PhysicsEngine.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; +using namespace std; + +// Constructor +PhysicsEngine::PhysicsEngine(PhysicsWorld* world, double timeStep) throw (invalid_argument) + : world(world), timer(timeStep), collisionDetection(world), constraintSolver(world) { + // Check if the pointer to the world is not NULL + if (world == 0) { + // Throw an exception + throw invalid_argument("Error : The argument world to the PhysicsEngine constructor cannot be NULL"); + } + + // Check if the timeStep is positive + if (timeStep <= 0.0) { + // Throw an exception + throw invalid_argument("Error : The timeStep argument to the PhysicsEngine constructor have to be greater than zero"); + } +} + +// Destructor +PhysicsEngine::~PhysicsEngine() { + +} + +// Update the physics simulation +void PhysicsEngine::update() throw (logic_error) { + bool existCollision = false; + + // Check that the timer is running + if (timer.getIsRunning()) { + + // Compute the time since the last update() call and update the timer + timer.update(); + + // Apply the gravity force to all bodies + applyGravity(); + + // While the time accumulator is not empty + while(timer.isPossibleToTakeStep()) { + existCollision = false; + + // Compute the collision detection + if (collisionDetection.computeCollisionDetection()) { + existCollision = true; + + // Solve constraints + constraintSolver.solve(timer.getTimeStep()); + } + + // Update the timer + timer.nextStep(); + + // Update the position and orientation of each body + updateAllBodiesMotion(); + + // Cleanup of the constraint solver + if (existCollision) { + constraintSolver.cleanup(); + } + + // Clear the added and removed bodies from last update() method call + world->clearAddedAndRemovedBodies(); + } + + // Compute and set the interpolation factor to all the bodies + setInterpolationFactorToAllBodies(); + } + else { // Call to update() but the timer is not running + // Throw an exception + throw logic_error("Error : The PhysicsEngine::start() method have to be called before calling PhysicsEngine::update()"); + } +} + +// Compute the motion of all bodies and update their positions and orientations +// First this method compute the vector V2 = V_constraint + V_forces + V1 where +// V_constraint = dt * (M^-1 * J^T * lambda) and V_forces = dt * (M^-1 * F_ext) +// V2 is the final velocity after the timestep and V1 is the velocity before the +// timestep. +// After having computed the velocity V2, this method will update the position +// and orientation of each body. +// This method uses the semi-implicit Euler method to update the position and +// orientation of the body +void PhysicsEngine::updateAllBodiesMotion() { + double dt = timer.getTimeStep(); + Vector3D newLinearVelocity; + Vector3D newAngularVelocity; + + // For each body of thephysics world + for (vector::iterator it=world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); it++) { + + RigidBody* rigidBody = dynamic_cast(*it); + assert(rigidBody); + + // If the body is able to move + if (rigidBody->getIsMotionEnabled()) { + newLinearVelocity.setAllValues(0.0, 0.0, 0.0); + newAngularVelocity.setAllValues(0.0, 0.0, 0.0); + + // If it's a constrained body + if (constraintSolver.isConstrainedBody(*it)) { + // Get the constrained linear and angular velocities from the constraint solver + newLinearVelocity = constraintSolver.getConstrainedLinearVelocityOfBody(*it); + newAngularVelocity = constraintSolver.getConstrainedAngularVelocityOfBody(*it); + } + + // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the + // external forces and torques. + newLinearVelocity = newLinearVelocity + dt * rigidBody->getMassInverse() * rigidBody->getExternalForce(); + newAngularVelocity = newAngularVelocity + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); + + // Add the velocity V1 to the new velocity + newLinearVelocity = newLinearVelocity + rigidBody->getLinearVelocity(); + newAngularVelocity = newAngularVelocity + rigidBody->getAngularVelocity(); + + // Update the position and the orientation of the body according to the new velocity + updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity); + + // If the body state has changed, we have to update some informations in the rigid body + rigidBody->update(); + } + } +} + +// Update the position and orientation of a body +// Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new +// orientation of the body +void PhysicsEngine::updatePositionAndOrientationOfBody(Body* body, const Vector3D& newLinVelocity, const Vector3D& newAngVelocity) { + double dt = timer.getTimeStep(); + + RigidBody* rigidBody = dynamic_cast(body); + assert(rigidBody); + + // Update the old position and orientation of the body + rigidBody->updateOldPositionAndOrientation(); + + // Normalize the orientation quaternion + rigidBody->setOrientation(rigidBody->getOrientation().getUnit()); + + // Update the linear and angular velocity of the body + rigidBody->setLinearVelocity(newLinVelocity); + rigidBody->setAngularVelocity(newAngVelocity); + + // Update the position and the orientation of the body + rigidBody->setPosition(rigidBody->getPosition() + newLinVelocity * dt); + rigidBody->setOrientation(rigidBody->getOrientation() + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * rigidBody->getOrientation() * 0.5 * dt); +} + +// Compute and set the interpolation factor to all bodies +void PhysicsEngine::setInterpolationFactorToAllBodies() { + // Compute the interpolation factor + double factor = timer.computeInterpolationFactor(); + assert(factor >= 0.0 && factor <= 1.0); + + // Set the factor to all bodies + for (vector::iterator it=world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); it++) { + + RigidBody* rigidBody = dynamic_cast(*it); + assert(rigidBody); + + rigidBody->setInterpolationFactor(factor); + } +} + +// Apply the gravity force to all bodies of the physics world +void PhysicsEngine::applyGravity() { + + // For each body of the physics world + for (vector::iterator it=world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); it++) { + + RigidBody* rigidBody = dynamic_cast(*it); + assert(rigidBody); + + // If the gravity force is on + if(world->getIsGravityOn()) { + // Apply the current gravity force to the body + rigidBody->setExternalForce(rigidBody->getMass() * world->getGravity()); + } + } +} diff --git a/src/reactphysics3d/engine/PhysicsEngine.h b/src/reactphysics3d/engine/PhysicsEngine.h new file mode 100644 index 00000000..1b0b4831 --- /dev/null +++ b/src/reactphysics3d/engine/PhysicsEngine.h @@ -0,0 +1,78 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef PHYSICSENGINE_H +#define PHYSICSENGINE_H + +// Libraries +#include "PhysicsWorld.h" +#include "../collision/CollisionDetection.h" +#include "ConstraintSolver.h" +#include "../body/RigidBody.h" +#include "Timer.h" + +// Namespace ReactPhysics3D +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class PhysicsEngine : + This class represents the physics engine + of the library. + ------------------------------------------------------------------- +*/ +class PhysicsEngine { + protected : + PhysicsWorld* world; // Pointer to the physics world of the physics engine + Timer timer; // Timer of the physics engine + CollisionDetection collisionDetection; // Collision detection + ConstraintSolver constraintSolver; // Constraint solver + + void updateAllBodiesMotion(); // Compute the motion of all bodies and update their positions and orientations + void updatePositionAndOrientationOfBody(Body* body, const Vector3D& newLinVelocity, const Vector3D& newAngVelocity); // Update the position and orientation of a body + void setInterpolationFactorToAllBodies(); // Compute and set the interpolation factor to all bodies + void applyGravity(); // Apply the gravity force to all bodies + +public : + PhysicsEngine(PhysicsWorld* world, double timeStep) throw (std::invalid_argument); // Constructor + ~PhysicsEngine(); // Destructor + + void start(); // Start the physics simulation + void stop(); // Stop the physics simulation + void update() throw (std::logic_error); // Update the physics simulation +}; + +// --- Inline functions --- // + +// Start the physics simulation +inline void PhysicsEngine::start() { + timer.start(); +} + +inline void PhysicsEngine::stop() { + timer.stop(); +} + +} + +#endif diff --git a/src/reactphysics3d/engine/PhysicsWorld.cpp b/src/reactphysics3d/engine/PhysicsWorld.cpp new file mode 100644 index 00000000..8ad9fd83 --- /dev/null +++ b/src/reactphysics3d/engine/PhysicsWorld.cpp @@ -0,0 +1,72 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "PhysicsWorld.h" +#include + +// Namespaces +using namespace reactphysics3d; +using namespace std; + +// Constructor +PhysicsWorld::PhysicsWorld(const Vector3D& gravity) + : gravity(gravity), isGravityOn(true) { + +} + +// Destructor +PhysicsWorld::~PhysicsWorld() { + // Remove and free the memory of all constraints + removeAllConstraints(); +} + +// Remove all collision contacts constraints +void PhysicsWorld::removeAllContactConstraints() { + // For all constraints + for (vector::iterator it = constraints.begin(); it != constraints.end(); ) { + + // Try a downcasting + Contact* contact = dynamic_cast(*it); + + // If the constraint is a contact + if (contact != 0) { + // Delete the contact + delete (*it); + it = constraints.erase(it); + } + else { + ++it; + } + } +} + +// Remove all constraints in the physics world and also delete them (free their memory) +void PhysicsWorld::removeAllConstraints() { + for (vector::iterator it = constraints.begin(); it != constraints.end(); it++) { + delete *it; + } + constraints.clear(); +} + diff --git a/src/reactphysics3d/engine/PhysicsWorld.h b/src/reactphysics3d/engine/PhysicsWorld.h new file mode 100644 index 00000000..a281987a --- /dev/null +++ b/src/reactphysics3d/engine/PhysicsWorld.h @@ -0,0 +1,178 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef PHYSICSWORLD_H +#define PHYSICSWORLD_H + +// Libraries +#include +#include +#include "../mathematics/mathematics.h" +#include "../body/Body.h" +#include "../constraint/Constraint.h" +#include "../constraint/Contact.h" + +// Namespace reactphysics3d +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class PhysicsWorld : + This class represents the world of the + physics engine. The physics world contains all the bodies of the physics + engine. + ------------------------------------------------------------------- +*/ +class PhysicsWorld { + protected : + std::vector bodies; // list that contains all bodies of the physics world + std::vector addedBodies; // Added bodies since last update + std::vector removedBodies; // Removed bodies since last update + std::vector constraints; // List that contains all the current constraints + Vector3D gravity; // Gravity vector of the world + bool isGravityOn; // True if the gravity force is on + + public : + PhysicsWorld(const Vector3D& gravity); // Constructor + virtual ~PhysicsWorld(); // Destructor + + void addBody(Body* body); // Add a body to the physics world + void removeBody(Body const* const body); // Remove a body from the physics world + void clearAddedAndRemovedBodies(); // Clear the addedBodies and removedBodies sets + Vector3D getGravity() const; // Return the gravity vector of the world + bool getIsGravityOn() const; // Return if the gravity is on + void setIsGratityOn(bool isGravityOn); // Set the isGravityOn attribute + void addConstraint(Constraint* constraint); // Add a constraint + void removeConstraint(Constraint* constraint); // Remove a constraint + void removeAllContactConstraints(); // Remove all collision contacts constraints + void removeAllConstraints(); // Remove all constraints and delete them (free their memory) + std::vector::iterator getConstraintsBeginIterator(); // Return a start iterator on the constraint list + std::vector::iterator getConstraintsEndIterator(); // Return a end iterator on the constraint list + std::vector::iterator getBodiesBeginIterator(); // Return an iterator to the beginning of the bodies of the physics world + std::vector::iterator getBodiesEndIterator(); // Return an iterator to the end of the bodies of the physics world + std::vector& getAddedBodies(); // Return the added bodies since last update of the physics engine + std::vector& getRemovedBodies(); // Retrun the removed bodies since last update of the physics engine +}; + +// Add a body to the physics world +inline void PhysicsWorld::addBody(Body* body) { + std::vector::iterator it; + + assert(body); + it = std::find(bodies.begin(), bodies.end(), body); + assert(it == bodies.end()); + + // The body isn't already in the bodyList, therefore we add it to the list + bodies.push_back(body); + addedBodies.push_back(body); + it = std::find(removedBodies.begin(), removedBodies.end(), body); + if (it != removedBodies.end()) { + removedBodies.erase(it); + } +} + +// Remove a body from the physics world +inline void PhysicsWorld::removeBody(Body const* const body) { + std::vector::iterator it; + + assert(body); + it = std::find(bodies.begin(), bodies.end(), body); + assert(*it == body); + + // Remove the body + bodies.erase(it); + addedBodies.erase(it); + removedBodies.push_back(*it); +} + +// Add a constraint into the physics world +inline void PhysicsWorld::addConstraint(Constraint* constraint) { + assert(constraint != 0); + constraints.push_back(constraint); +} + +// Remove a constraint and free its memory +inline void PhysicsWorld::removeConstraint(Constraint* constraint) { + std::vector::iterator it; + + assert(constraint); + it = std::find(constraints.begin(), constraints.end(), constraint); + assert(*it == constraint); + delete *it; + constraints.erase(it); +} + +// Clear the addedBodies and removedBodies sets +inline void PhysicsWorld::clearAddedAndRemovedBodies() { + addedBodies.clear(); + removedBodies.clear(); +} + +// Return the gravity vector of the world +inline Vector3D PhysicsWorld::getGravity() const { + return gravity; +} + +// Return if the gravity is on +inline bool PhysicsWorld::getIsGravityOn() const { + return isGravityOn; +} + +// Set the isGravityOn attribute +inline void PhysicsWorld::setIsGratityOn(bool isGravityOn) { + this->isGravityOn = isGravityOn; +} + +// Return a start iterator on the constraint list +inline std::vector::iterator PhysicsWorld::getConstraintsBeginIterator() { + return constraints.begin(); +} + +// Return a end iterator on the constraint list +inline std::vector::iterator PhysicsWorld::getConstraintsEndIterator() { + return constraints.end(); +} + +// Return an iterator to the beginning of the bodies of the physics world +inline std::vector::iterator PhysicsWorld::getBodiesBeginIterator() { + return bodies.begin(); +} + +// Return an iterator to the end of the bodies of the physics world +inline std::vector::iterator PhysicsWorld::getBodiesEndIterator() { + return bodies.end(); +} + +// Return the added bodies since last update of the physics engine +inline std::vector& PhysicsWorld::getAddedBodies() { + return addedBodies; +} + +// Retrun the removed bodies since last update of the physics engine +inline std::vector& PhysicsWorld::getRemovedBodies() { + return removedBodies; +} + +} // End of the ReactPhysics3D namespace + + #endif \ No newline at end of file diff --git a/src/reactphysics3d/engine/Timer.cpp b/src/reactphysics3d/engine/Timer.cpp new file mode 100644 index 00000000..d5cf6ce3 --- /dev/null +++ b/src/reactphysics3d/engine/Timer.cpp @@ -0,0 +1,48 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "Timer.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +Timer::Timer(double timeStep) : timeStep(timeStep), isRunning(false) { + assert(timeStep > 0.0); +} + +// Destructor +Timer::~Timer() { + +} + + + + + + + + + diff --git a/src/reactphysics3d/engine/Timer.h b/src/reactphysics3d/engine/Timer.h new file mode 100644 index 00000000..d0535e08 --- /dev/null +++ b/src/reactphysics3d/engine/Timer.h @@ -0,0 +1,152 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef TIMER_H +#define TIMER_H + +// Libraries +#include +#include +#include +#include + +// Namespace ReactPhysics3D +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class Timer : + This class will take care of the time in the physics engine. + ------------------------------------------------------------------- +*/ +class Timer { + private : + double timeStep; // Timestep dt of the physics engine (timestep > 0.0) + long double time; // Current time of the physics engine + long double lastUpdateTime; // Last time the timer has been updated + long double deltaTime; // Time difference between the two last timer update() calls + double accumulator; // Used to fix the time step and avoid strange time effects + bool isRunning; // True if the timer is running + + public : + Timer(double timeStep); // Constructor + virtual ~Timer(); // Destructor + + double getTimeStep() const; // Return the timestep of the physics engine + void setTimeStep(double timeStep) throw(std::invalid_argument); // Set the timestep of the physics engine + long double getTime() const; // Return the current time + void start(); // Start the timer + void stop(); // Stop the timer + bool getIsRunning() const; // Return true if the timer is running + bool isPossibleToTakeStep() const; // True if it's possible to take a new step + void update(); // Compute the time since the last update() call and add it to the accumulator + void nextStep(); // Take a new step => update the timer by adding the timeStep value to the current time + double computeInterpolationFactor(); // Compute the interpolation factor +}; + +// --- Inline functions --- // + +// Return the timestep of the physics engine +inline double Timer::getTimeStep() const { + return timeStep; +} + +// Set the timestep of the physics engine +inline void Timer::setTimeStep(double timeStep) throw(std::invalid_argument) { + // Check if the timestep is different from zero + if (timeStep != 0.0) { + this->timeStep = timeStep; + } + else { + // We throw an exception + throw std::invalid_argument("Exception in Timer : the timestep has to be different from zero"); + } +} + +// Return the current time +inline long double Timer::getTime() const { + return time; +} + +// Return if the timer is running +inline bool Timer::getIsRunning() const { + return isRunning; +} + +// Start the timer +inline void Timer::start() { + if (!isRunning) { + // Initialize the lastUpdateTime with the current time in seconds + lastUpdateTime = std::clock() / double(CLOCKS_PER_SEC); + accumulator = 0.0; + isRunning = true; + } +} + +// Stop the timer +inline void Timer::stop() { + if (isRunning) { + isRunning = false; + } +} + +// True if it's possible to take a new step +inline bool Timer::isPossibleToTakeStep() const { + return (accumulator >= timeStep); +} + +// Take a new step => update the timer by adding the timeStep value to the current time +inline void Timer::nextStep() { + assert(isRunning); + + // Update the current time of the physics engine + time += timeStep; + + // Update the accumulator value + accumulator -= timeStep; +} + +// Compute the interpolation factor +inline double Timer::computeInterpolationFactor() { + return (accumulator / timeStep); +} + +// Compute the time since the last update() call and add it to the accumulator +inline void Timer::update() { + + // Compute the current time is seconds + long double currentTime = std::clock() / double(CLOCKS_PER_SEC); + + // Compute the delta display time between two display frames + deltaTime = currentTime - lastUpdateTime; + + // Update the current display time + lastUpdateTime = currentTime; + + // Update the accumulator value + accumulator += deltaTime; +} + +} // End of the ReactPhysics3D namespace + + #endif diff --git a/src/reactphysics3d/mathematics/Matrix.cpp b/src/reactphysics3d/mathematics/Matrix.cpp new file mode 100644 index 00000000..0eb6a657 --- /dev/null +++ b/src/reactphysics3d/mathematics/Matrix.cpp @@ -0,0 +1,549 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include +#include "Matrix.h" + +// Namespaces +using namespace reactphysics3d; + +// Constructor without argument +Matrix::Matrix() + :nbRow(0), nbColumn(0) { + array = 0; +} + +// Constructor of the class Matrix +Matrix::Matrix(int nbRow, int nbColumn) throw(std::invalid_argument) + :nbRow(nbRow),nbColumn(nbColumn) { + // Check the arguments + if (nbRow>0 && nbColumn>0) { + + // Create the two dimensional dynamic array + array = new double*[nbRow]; + + assert(array != 0); // Array pointer musn't be null + + for(int i=0; iarray; +} + +void Matrix::changeSize(uint newNbRows, uint newNbColumns) { + if (array) { + // Destruction of the dynamic array + for(int i=0; iarray; + } + + // Create the two dimensional dynamic array + array = new double*[newNbRows]; + + assert(array != 0); // Array pointer musn't be null + + for(int i=0; i nbRow || j+sizeColumns > nbColumn) { + // Throw an exception + throw std::invalid_argument("Error : The arguments are out of matrix bounds"); + } + + // Compute the sub-matrix + Matrix resultMatrix(sizeRows, sizeColumns); + for (unsigned int k=0; k 0) { + // Create a new matrix + Matrix identityMatrix(dimension,dimension); + + // Fill in the identity matrix + for(int i=0; i= subMatrix.nbColumn); + assert(nbColumn-colIndex >= subMatrix.nbColumn); + + // Fill in the sub-matrix + for (unsigned int i=0; igetValue(i,j) + matrix2.getValue(i,j)); + } + } + + // Return the sum matrix + return sumMatrix; + } + else { + // We throw an MathematicsException + throw MathematicsException("MathematicsException : Addition of the matrices isn't possible beacause the size of the matrices aren't the same"); + } +} + +// Definition of the operator - for the substraction of two matrices with references +Matrix Matrix::operator-(const Matrix& matrix2) const throw(MathematicsException) { + if (nbRow == matrix2.nbRow && nbColumn == matrix2.nbColumn) { + // Create a new matrix + Matrix sumMatrix(nbRow, nbColumn); + + // Substract the two matrices + for(int i=0; inbColumn; ++j) { + sumMatrix.setValue(i, j, this->getValue(i,j) - matrix2.getValue(i,j)); + } + } + + // Return the sum matrix + return sumMatrix; + } + else { + // We throw a MathematicsException + throw MathematicsException("MathematicsException : Substraction of the matrices isn't possible beacause the size of the matrices aren't the same"); + } +} + +// Overloaded operator * for the multiplication of the matrix with a number +Matrix Matrix::operator*(double nb) const { + // Creation of the result matrix + Matrix result(nbRow, nbColumn); + + // Multiplication of the matrix with the number + for(int i=0; isetValue(i,j, matrix2.getValue(i,j)); + } + } + } + + // Return a reference to the matrix + return *this; + } + else { + // Throw a MathematicsException + throw MathematicsException("MathematicsException : Assignment impossible because the size of the matrices aren't the same !"); + } +} + +// Overloaded operator for equality condition +bool Matrix::operator==(const Matrix& matrix2) const throw(MathematicsException) { + // Check if the matrices dimensions are compatible + if (nbRow == matrix2.nbRow && nbColumn == matrix2.nbColumn) { + for (int i=0; i +#include + +// TODO : Replace the "int" by "unsigned int" + +// ReactPhysics3D namespace +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class Matrix : + This class represents a matrix. + ------------------------------------------------------------------- +*/ +class Matrix { + private : + unsigned int nbRow; // Number of row in the matrix + unsigned int nbColumn; // Number of colum in the matrix + double** array; // Dynamic array that contains the values of the matrix + + public : + Matrix(); // Constructor without argument + Matrix(int nbRow, int nbColum) throw(std::invalid_argument); // Constructor of the class Matrix + Matrix(const Matrix& matrix); // Copy constructor of the class Matrix + Matrix(const Matrix3x3& matrix); // Conversion from Matrix3x3 + Matrix(const Vector& vector); // Conversion from Vector to Matrix + virtual ~Matrix(); // Destructor of the class Matrix + double getValue(int i, int j) const throw(std::invalid_argument); // Return a value in the matrix + void setValue(int i, int j, double value) throw(std::invalid_argument); // Set a value in the matrix + int getNbRow() const; // Return the number of row of the matrix + int getNbColumn() const; // Return the number of column of the matrix + Matrix getCofactor(int i, int j) const throw(std::invalid_argument); // Return the cofactor matrix by removing row i and column j + Matrix getTranspose() const; // Return the transposed matrixs + Matrix getInverse() const throw(MathematicsException); // Return the inverse of the matrix if there exists + double getDeterminant() const throw(MathematicsException); // Return the determinant of the matrix + double getTrace() const throw(MathematicsException); // Return the trace of a square matrix + Matrix getSubMatrix(unsigned int i, unsigned int j, + unsigned int nbRows, unsigned int nbColumns) const throw(std::invalid_argument); // Return a sub matrix of size of the current matrix + Vector getVector() const; + static Matrix identity(int dimension) throw(std::invalid_argument); // Return the identity matrix I of the given dimension + void fillInSubMatrix(unsigned int i, unsigned int j, const Matrix& subMatrix); // Fill in a sub-matrix of the current matrix with another matrix + void initWithValue(double value); // Initialize all the matrix with the given value + void display() const; // TO DELETE + void changeSize(uint nbRows, uint nbColumns); + + // --- Overloaded operators --- // + Matrix operator+(const Matrix& matrix2) const throw(MathematicsException); // Overloaded operator for addition + Matrix operator-(const Matrix& matrix2) const throw(MathematicsException); // Overloaded operator for substraction + Matrix operator*(double nb) const; // Overloaded operator for multiplication with a number + Matrix operator*(const Matrix& matrix2) const throw(MathematicsException); // Overloaded operator for multiplication with a matrix + Matrix operator*(const Vector& vector) const throw(MathematicsException); // Overloaded operator for multiplication with a vector + Matrix& operator=(const Matrix& matrix2) throw(MathematicsException); // Overloaded operator for assignment + bool operator==(const Matrix& matrix2) const throw(MathematicsException); // Overloaded operator for equality condition +}; + +// Function to get a value in the matrix (inline) +inline double Matrix::getValue(int i, int j) const throw(std::invalid_argument) { + if (0 <= i && i < nbRow && 0 <= j && j < nbColumn) { + // get the value in the matrix + return array[i][j]; + } + else { + // We Throw an out_of_range exception + throw std::invalid_argument("Exception : The index i or j is outside the matrix size !"); + } +} + +// Function to set a value in the matrix (inline) +inline void Matrix::setValue(int i, int j, double value) throw(std::invalid_argument) { + if (0 <= i && i < nbRow && 0 <= j && j < nbColumn) { + // Set the value in the matrix + this->array[i][j] = value; + } + else { + // We Throw an out_of_range exception + throw std::invalid_argument("Exception : The index i or j is outside the matrix size !"); + } +} + +// Function that return the number of row of the matrix (inline) +inline int Matrix::getNbRow() const { + return nbRow; +} + +// Function that return the number of colum of the matrix (inline) +inline int Matrix::getNbColumn() const { + return nbColumn; +} + +// Overloaded operator for multiplication between a number and a Matrix (inline) +inline Matrix operator*(double number, const Matrix& matrix) { + + // Return the result matrix + return matrix * number; +} + +} // End of the ReactPhysics3D namespace + +#endif diff --git a/src/reactphysics3d/mathematics/Matrix3x3.cpp b/src/reactphysics3d/mathematics/Matrix3x3.cpp new file mode 100644 index 00000000..2f78547f --- /dev/null +++ b/src/reactphysics3d/mathematics/Matrix3x3.cpp @@ -0,0 +1,141 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include +#include "Matrix3x3.h" + +// Namespaces +using namespace reactphysics3d; + +// Constructor of the class Matrix3x3 +Matrix3x3::Matrix3x3() { + // Initialize all values in the matrix to zero + setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); +} + +// Constructor with arguments +Matrix3x3::Matrix3x3(double a1, double a2, double a3, double b1, double b2, double b3, double c1, double c2, double c3) { + // Initialize the matrix with the values + setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3); +} + +// Copy-constructor +// TODO : Test if this copy-constructor is correct (check if the the copy matrix use +// the same memory place for its array) +Matrix3x3::Matrix3x3(const Matrix3x3& matrix2) { + // Copy the values in the matrix + setAllValues(matrix2.array[0][0], matrix2.array[0][1], matrix2.array[0][2], + matrix2.array[1][0], matrix2.array[1][1], matrix2.array[1][2], + matrix2.array[2][0], matrix2.array[2][1], matrix2.array[2][2]); +} + +// Destructor +Matrix3x3::~Matrix3x3() { + +} + +// Return the inverse matrix +Matrix3x3 Matrix3x3::getInverse() const throw(MathematicsException) { + // Compute the determinant of the matrix + double determinant = getDeterminant(); + + // Check if the determinant is equal to zero + if (determinant != 0) { + double invDeterminant = 1.0 / determinant; + Matrix3x3 tempMatrix; + + // Compute the inverse of the matrix + tempMatrix.setAllValues((array[1][1]*array[2][2]-array[2][1]*array[1][2]), -(array[1][0]*array[2][2]-array[2][0]*array[1][2]), (array[1][0]*array[2][1]-array[2][0]*array[1][1]), + -(array[0][1]*array[2][2]-array[2][1]*array[0][2]), (array[0][0]*array[2][2]-array[2][0]*array[0][2]), -(array[0][0]*array[2][1]-array[2][0]*array[0][1]), + (array[0][1]*array[1][2]-array[0][2]*array[1][1]), -(array[0][0]*array[1][2]-array[1][0]*array[0][2]), (array[0][0]*array[1][1]-array[0][1]*array[1][0])); + + // Return the inverse matrix + return (invDeterminant * tempMatrix.getTranspose()); + } + else { + // Throw an exception because the inverse of the matrix doesn't exist if the determinant is equal to zero + throw MathematicsException("MathematicsException : Impossible to compute the inverse of the matrix because the determinant is equal to zero"); + } +} + +// Return the 3x3 identity matrix +Matrix3x3 Matrix3x3::identity() { + // Return the isdentity matrix + return Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); +} + +// Overloaded operator for addition +Matrix3x3 Matrix3x3::operator+(const Matrix3x3& matrix2) const { + // Return the sum matrix + return Matrix3x3(array[0][0] + matrix2.array[0][0], array[0][1] + matrix2.array[0][1], array[0][2] + matrix2.array[0][2], + array[1][0] + matrix2.array[1][0], array[1][1] + matrix2.array[1][1], array[1][2] + matrix2.array[1][2], + array[2][0] + matrix2.array[2][0], array[2][1] + matrix2.array[2][1], array[2][2] + matrix2.array[2][2]); +} + +// Overloaded operator for substraction +Matrix3x3 Matrix3x3::operator-(const Matrix3x3& matrix2) const { + // Return the substraction matrix + return Matrix3x3(array[0][0] - matrix2.array[0][0], array[0][1] - matrix2.array[0][1], array[0][2] - matrix2.array[0][2], + array[1][0] - matrix2.array[1][0], array[1][1] - matrix2.array[1][1], array[1][2] - matrix2.array[1][2], + array[2][0] - matrix2.array[2][0], array[2][1] - matrix2.array[2][1], array[2][2] - matrix2.array[2][2]); +} + +// Overloaded operator for multiplication with a number +Matrix3x3 Matrix3x3::operator*(double nb) const { + // Return multiplied matrix + return Matrix3x3(array[0][0] * nb, array[0][1] * nb, array[0][2] * nb, + array[1][0] * nb, array[1][1] * nb, array[1][2] * nb, + array[2][0] * nb, array[2][1] * nb, array[2][2] * nb); +} + +// Overloaded operator for multiplication with a matrix +Matrix3x3 Matrix3x3::operator*(const Matrix3x3& matrix2) const { + // Compute and return the multiplication of the matrices + return Matrix3x3(array[0][0]*matrix2.array[0][0] + array[0][1]*matrix2.array[1][0] + array[0][2]*matrix2.array[2][0], + array[0][0]*matrix2.array[0][1] + array[0][1]*matrix2.array[1][1] + array[0][2]*matrix2.array[2][1], + array[0][0]*matrix2.array[0][2] + array[0][1]*matrix2.array[1][2] + array[0][2]*matrix2.array[2][2], + array[1][0]*matrix2.array[0][0] + array[1][1]*matrix2.array[1][0] + array[1][2]*matrix2.array[2][0], + array[1][0]*matrix2.array[0][1] + array[1][1]*matrix2.array[1][1] + array[1][2]*matrix2.array[2][1], + array[1][0]*matrix2.array[0][2] + array[1][1]*matrix2.array[1][2] + array[1][2]*matrix2.array[2][2], + array[2][0]*matrix2.array[0][0] + array[2][1]*matrix2.array[1][0] + array[2][2]*matrix2.array[2][0], + array[2][0]*matrix2.array[0][1] + array[2][1]*matrix2.array[1][1] + array[2][2]*matrix2.array[2][1], + array[2][0]*matrix2.array[0][2] + array[2][1]*matrix2.array[1][2] + array[2][2]*matrix2.array[2][2]); +} + +// Overloaded operator for assignment +Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix2) { + // Check for self-assignment + if (this != &matrix2) { + setAllValues(matrix2.array[0][0], matrix2.array[0][1], matrix2.array[0][2], + matrix2.array[1][0], matrix2.array[1][1], matrix2.array[1][2], + matrix2.array[2][0], matrix2.array[2][1], matrix2.array[2][2]); + } + + // Return a reference to the matrix + return *this; +} + + + diff --git a/src/reactphysics3d/mathematics/Matrix3x3.h b/src/reactphysics3d/mathematics/Matrix3x3.h new file mode 100644 index 00000000..2d62050f --- /dev/null +++ b/src/reactphysics3d/mathematics/Matrix3x3.h @@ -0,0 +1,161 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + + +#ifndef MATRIX3X3_H +#define MATRIX3X3_H + +// Libraries +#include "exceptions.h" +#include "Vector3D.h" + +// ReactPhysics3D namespace +namespace reactphysics3d { + + +/* ------------------------------------------------------------------- + Class Matrix3x3 : + This class represents a 3x3 matrix. + ------------------------------------------------------------------- +*/ +class Matrix3x3 { + private : + double array[3][3]; // Array with the values of the matrix + + public : + Matrix3x3(); // Constructor of the class Matrix3x3 + Matrix3x3(double a1, double a2, double a3, double b1, double b2, double b3, + double c1, double c2, double c3); // Constructor with arguments + Matrix3x3(const Matrix3x3& matrix); // Copy-constructor + virtual ~Matrix3x3(); // Destructor + + double getValue(int i, int j) const throw(std::invalid_argument); // Get a value in the matrix + void setValue(int i, int j, double value) throw(std::invalid_argument); // Set a value in the matrix + void setAllValues(double a1, double a2, double a3, double b1, double b2, double b3, + double c1, double c2, double c3); // Set all the values in the matrix + Matrix3x3 getTranspose() const; // Return the transpose matrix + double getDeterminant() const; // Return the determinant of the matrix + double getTrace() const; // Return the trace of the matrix + Matrix3x3 getInverse() const throw(MathematicsException); // Return the inverse matrix + static Matrix3x3 identity(); // Return the 3x3 identity matrix + + // --- Overloaded operators --- // + Matrix3x3 operator+(const Matrix3x3& matrix2) const; // Overloaded operator for addition + Matrix3x3 operator-(const Matrix3x3& matrix2) const ; // Overloaded operator for substraction + Matrix3x3 operator*(double nb) const; // Overloaded operator for multiplication with a number + Matrix3x3 operator*(const Matrix3x3& matrix2) const; // Overloaded operator for multiplication with a matrix + Vector3D operator*(const Vector3D& vector3d) const; // Overloaded operator for multiplication with a vector + Matrix3x3& operator=(const Matrix3x3& matrix2); // Overloaded operator for assignment + bool operator==(const Matrix3x3& matrix2) const; // Overloaded operator for equality condition +}; + + +// Method to get a value in the matrix (inline) +inline double Matrix3x3::getValue(int i, int j) const throw(std::invalid_argument) { + // Check the argument + if (i>=0 && i<3 && j>=0 && j<3) { + // Return the value + return array[i][j]; + } + else { + // Throw an exception because of the wrong argument + throw std::invalid_argument("Exception : The argument isn't in the bounds of the 3x3 matrix"); + } +} + +// Method to set a value in the matrix (inline) +inline void Matrix3x3::setValue(int i, int j, double value) throw(std::invalid_argument) { + // Check the argument + if (i>=0 && i<3 && j>=0 && j<3) { + // Set the value + array[i][j] = value; + } + else { + // Throw an exception because of the wrong argument + throw std::invalid_argument("Exception : The argument isn't in the bounds of the 3x3 matrix"); + } +} + +// Method to set all the values in the matrix +inline void Matrix3x3::setAllValues(double a1, double a2, double a3, double b1, double b2, double b3, + double c1, double c2, double c3) { + // Set all the values of the matrix + array[0][0] = a1; + array[0][1] = a2; + array[0][2] = a3; + + array[1][0] = b1; + array[1][1] = b2; + array[1][2] = b3; + + array[2][0] = c1; + array[2][1] = c2; + array[2][2] = c3; +} + +// Return the transpose matrix +inline Matrix3x3 Matrix3x3::getTranspose() const { + // Return the transpose matrix + return Matrix3x3(array[0][0], array[1][0], array[2][0], + array[0][1], array[1][1], array[2][1], + array[0][2], array[1][2], array[2][2]); +} + +// Return the determinant of the matrix +inline double Matrix3x3::getDeterminant() const { + // Compute and return the determinant of the matrix + return (array[0][0]*(array[1][1]*array[2][2]-array[2][1]*array[1][2]) - array[0][1]*(array[1][0]*array[2][2]-array[2][0]*array[1][2]) + + array[0][2]*(array[1][0]*array[2][1]-array[2][0]*array[1][1])); +} + +// Return the trace of the matrix +inline double Matrix3x3::getTrace() const { + // Compute and return the trace + return (array[0][0] + array[1][1] + array[2][2]); +} + +// Overloaded operator for multiplication between a number and a Matrix3x3 (inline) +inline Matrix3x3 operator*(double number, const Matrix3x3& matrix) { + // Return the multiplied matrix + return matrix * number; +} + +// Overloaded operator for multiplication with a vector +inline Vector3D Matrix3x3::operator*(const Vector3D& vector3d) const { + // Compute and return the result + return Vector3D(array[0][0]*vector3d.getX() + array[0][1]*vector3d.getY() + array[0][2]*vector3d.getZ(), + array[1][0]*vector3d.getX() + array[1][1]*vector3d.getY() + array[1][2]*vector3d.getZ(), + array[2][0]*vector3d.getX() + array[2][1]*vector3d.getY() + array[2][2]*vector3d.getZ()); +} + +// Overloaded operator for equality condition +inline bool Matrix3x3::operator==(const Matrix3x3& matrix2) const { + return (array[0][0] == matrix2.array[0][0] && array[0][1] == matrix2.array[0][1] && array[0][2] == matrix2.array[0][2] && + array[1][0] == matrix2.array[1][0] && array[1][1] == matrix2.array[1][1] && array[1][2] == matrix2.array[1][2] && + array[2][0] == matrix2.array[2][0] && array[2][1] == matrix2.array[2][1] && array[2][2] == matrix2.array[2][2]); +} + +} // End of the ReactPhysics3D namespace + +#endif diff --git a/src/reactphysics3d/mathematics/Quaternion.cpp b/src/reactphysics3d/mathematics/Quaternion.cpp new file mode 100755 index 00000000..4d67da93 --- /dev/null +++ b/src/reactphysics3d/mathematics/Quaternion.cpp @@ -0,0 +1,228 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "Quaternion.h" +#include "Vector3D.h" +#include + +// Namespace +using namespace reactphysics3d; + +// Constructor of the class +Quaternion::Quaternion() + :x(0.0), y(0.0), z(0.0), w(0.0) { + +} + +// Constructor with arguments +Quaternion::Quaternion(double x, double y, double z, double w) + :x(x), y(y), z(z), w(w) { + +} + +// Constructor with the component w and the vector v=(x y z) +Quaternion::Quaternion(double w, const Vector3D& v) + :x(v.getX()), y(v.getY()), z(v.getZ()), w(w) { + +} + +// Copy-constructor +Quaternion::Quaternion(const Quaternion& quaternion) + :x(quaternion.x), y(quaternion.y), z(quaternion.z), w(quaternion.w) { + +} + +// Create a unit quaternion from a rotation matrix +Quaternion::Quaternion(const Matrix3x3& matrix) { + + // Get the trace of the matrix + double trace = matrix.getTrace(); + + double array[3][3]; + for (int i=0; i<3; i++) { + for (int j=0; j<3; j++) { + array[i][j] = matrix.getValue(i, j); + } + } + + double r; + double s; + + if (trace < 0.0) { + if (array[1][1] > array[0][0]) { + if(array[2][2] > array[1][1]) { + r = sqrt(array[2][2] - array[0][0] - array[1][1] + 1.0); + s = 0.5 / r; + + // Compute the quaternion + x = (array[2][0] + array[0][2])*s; + y = (array[1][2] + array[2][1])*s; + z = 0.5*r; + w = (array[1][0] - array[0][1])*s; + } + else { + r = sqrt(array[1][1] - array[2][2] - array[0][0] + 1.0); + s = 0.5 / r; + + // Compute the quaternion + x = (array[0][1] + array[1][0])*s; + y = 0.5 * r; + z = (array[1][2] + array[2][1])*s; + w = (array[0][2] - array[2][0])*s; + } + } + else if (array[2][2] > array[0][0]) { + r = sqrt(array[2][2] - array[0][0] - array[1][1] + 1.0); + s = 0.5 / r; + + // Compute the quaternion + x = (array[2][0] + array[0][2])*s; + y = (array[1][2] + array[2][1])*s; + z = 0.5 * r; + w = (array[1][0] - array[0][1])*s; + } + else { + r = sqrt(array[0][0] - array[1][1] - array[2][2] + 1.0); + s = 0.5 / r; + + // Compute the quaternion + x = 0.5 * r; + y = (array[0][1] + array[1][0])*s; + z = (array[2][0] - array[0][2])*s; + w = (array[2][1] - array[1][2])*s; + } + } + else { + r = sqrt(trace + 1.0); + s = 0.5/r; + + // Compute the quaternion + x = (array[2][1]-array[1][2])*s; + y = (array[0][2]-array[2][0])*s; + z = (array[1][0]-array[0][1])*s; + w = 0.5 * r; + } +} + +// Destructor +Quaternion::~Quaternion() { + +} + +// Compute the rotation angle (in radians) and the 3D rotation axis +// This method is used to get the rotation angle (in radian) and the unit +// rotation axis of an orientation quaternion. +void Quaternion::getRotationAngleAxis(double& angle, Vector3D& axis) const { + Quaternion quaternion; + + // If the quaternion is unit + if (length() == 1.0) { + quaternion = *this; + } + else { + // We compute the unit quaternion + quaternion = getUnit(); + } + + // Compute the roation angle + angle = acos(quaternion.w) * 2.0; + + // Compute the 3D rotation axis + Vector3D rotationAxis(quaternion.x, quaternion.y, quaternion.z); + + // Normalize the rotation axis + rotationAxis = rotationAxis.getUnit(); + + // Set the rotation axis values + axis.setAllValues(rotationAxis.getX(), rotationAxis.getY(), rotationAxis.getZ()); +} + +// Return the orientation matrix corresponding to this quaternion +Matrix3x3 Quaternion::getMatrix() const { + + double nQ = x*x + y*y + z*z + w*w; + double s = 0.0; + + if (nQ > 0.0) { + s = 2.0/nQ; + } + + // Computations used for optimization (less multiplications) + double xs = x*s; + double ys = y*s; + double zs = z*s; + double wxs = w*xs; + double wys = w*ys; + double wzs = w*zs; + double xxs = x*xs; + double xys = x*ys; + double xzs = x*zs; + double yys = y*ys; + double yzs = y*zs; + double zzs = z*zs; + + // Create the matrix corresponding to the quaternion + return Matrix3x3(1.0-yys-zzs, xys-wzs, xzs + wys, + xys + wzs, 1.0-xxs-zzs, yzs-wxs, + xzs-wys, yzs + wxs, 1.0-xxs-yys); +} + +// Compute the spherical linear interpolation between two quaternions. +// The t argument has to be such that 0 <= t <= 1. This method is static. +Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, double t) { + assert(t >= 0.0 && t <= 1.0); + + double invert = 1.0; + + // Compute cos(theta) using the quaternion scalar product + double cosineTheta = quaternion1.scalarProduct(quaternion2); + + // Take care of the sign of cosineTheta + if (cosineTheta < 0.0) { + cosineTheta = -cosineTheta; + invert = -1.0; + } + + // Because of precision, if cos(theta) is nearly 1, therefore theta is nearly 0 and we can write + // sin((1-t)*theta) as (1-t) and sin(t*theta) as t + if(1-cosineTheta < EPSILON) { + return quaternion1 * (1.0-t) + quaternion2 * (t * invert); + } + + // Compute the theta angle + double theta = acos(cosineTheta); + + // Compute sin(theta) + double sineTheta = sin(theta); + + // Compute the two coefficients that are in the spherical linear interpolation formula + double coeff1 = sin((1.0-t)*theta) / sineTheta; + double coeff2 = sin(t*theta) / sineTheta * invert; + + // Compute and return the interpolated quaternion + return quaternion1 * coeff1 + quaternion2 * coeff2; +} + + diff --git a/src/reactphysics3d/mathematics/Quaternion.h b/src/reactphysics3d/mathematics/Quaternion.h new file mode 100644 index 00000000..068e98b2 --- /dev/null +++ b/src/reactphysics3d/mathematics/Quaternion.h @@ -0,0 +1,226 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef QUATERNION_H +#define QUATERNION_H + +// Libraries +#include +#include "Vector3D.h" +#include "Matrix3x3.h" +#include "exceptions.h" + +// ReactPhysics3D namespace +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class Quaternion : + This class represents a quaternion. We use the notation : + q = (x*i, y*j, z*k, w) to represent a quaternion. + ------------------------------------------------------------------- +*/ +class Quaternion { + private : + double x; // Component x of the quaternion + double y; // Component y of the quaternion + double z; // Component z of the quaternion + double w; // Component w of the quaternion + + public : + Quaternion(); // Constructor + Quaternion(double x, double y, double z, double w); // Constructor with arguments + Quaternion(double w, const Vector3D& v); // Constructor with the component w and the vector v=(x y z) + Quaternion(const Quaternion& quaternion); // Copy-constructor + Quaternion(const Matrix3x3& matrix); // Create a unit quaternion from a rotation matrix + ~Quaternion(); // Destructor + double getX() const; // Return the component x of the quaternion + double getY() const; // Return the component y of the quaternion + double getZ() const; // Return the component z of the quaternion + double getW() const; // Return the component w of the quaternion + void setX(double x); // Set the value x + void setY(double y); // Set the value y + void setZ(double z); // Set the value z + void setW(double w); // Set the value w + Vector3D vectorV() const; // Return the vector v=(x y z) of the quaternion + double length() const; // Return the length of the quaternion + Quaternion getUnit() const throw (MathematicsException); // Return the unit quaternion + Quaternion getConjugate() const; // Return the conjugate quaternion + Quaternion getInverse() const throw (MathematicsException); // Return the inverse of the quaternion + Matrix3x3 getMatrix() const; // Return the orientation matrix corresponding to this quaternion + double scalarProduct(const Quaternion& quaternion) const; // Scalar product between two quaternions + void getRotationAngleAxis(double& angle, Vector3D& axis) const; // Compute the rotation angle (in radians) and the axis + static Quaternion slerp(const Quaternion& quaternion1, + const Quaternion& quaternion2, double t); // Compute the spherical linear interpolation between two quaternions + + // --- Overloaded operators --- // + Quaternion operator+(const Quaternion& quaternion) const; // Overloaded operator for the addition + Quaternion operator-(const Quaternion& quaternion) const; // Overloaded operator for the substraction + Quaternion operator*(double nb) const; // Overloaded operator for the multiplication with a constant + Quaternion operator*(const Quaternion& quaternion) const; // Overloaded operator for the multiplication + Quaternion& operator=(const Quaternion& quaternion); // Overloaded operator for assignment + bool operator==(const Quaternion& quaternion) const; // Overloaded operator for equality condition +}; + +// --- Inline functions --- // + +// Get the value x (inline) +inline double Quaternion::getX() const { + return x; +} + +// Get the value y (inline) +inline double Quaternion::getY() const { + return y; +} + +// Get the value z (inline) +inline double Quaternion::getZ() const { + return z; +} + +// Get the value w (inline) +inline double Quaternion::getW() const { + return w; +} + +// Set the value x (inline) +inline void Quaternion::setX(double x) { + this->x = x; +} + +// Set the value y (inline) +inline void Quaternion::setY(double y) { + this->y = y; +} + +// Set the value z (inline) +inline void Quaternion::setZ(double z) { + this->z = z; +} + +// Set the value w (inline) +inline void Quaternion::setW(double w) { + this->w = w; +} + +// Return the vector v=(x y z) of the quaternion +inline Vector3D Quaternion::vectorV() const { + // Return the vector v + return Vector3D(x, y, z); +} + +// Return the length of the quaternion (inline) +inline double Quaternion::length() const { + return sqrt(x*x + y*y + z*z + w*w); +} + +// Return the unit quaternion +inline Quaternion Quaternion::getUnit() const throw(MathematicsException) { + double lengthQuaternion = length(); + + // Check if the length is not equal to zero + if (lengthQuaternion != 0.0) { + + // Compute and return the unit quaternion + return Quaternion(x/lengthQuaternion, y/lengthQuaternion, z/lengthQuaternion, w/lengthQuaternion); + } + else { + // Throw an exception because it's impossible to compute a unit quaternion if its length is equal to zero + throw MathematicsException("MathematicsException : Impossible to compute the unit quaternion if the length of the quaternion is zero"); + } +} + +// Return the conjugate of the quaternion (inline) +inline Quaternion Quaternion::getConjugate() const { + return Quaternion(-x, -y, -z, w); +} + +// Return the inverse of the quaternion (inline) +inline Quaternion Quaternion::getInverse() const throw(MathematicsException) { + double lengthQuaternion = length(); + lengthQuaternion = lengthQuaternion * lengthQuaternion; + + // Check if the length is not equal to zero + if (lengthQuaternion != 0.0) { + + // Compute and return the inverse quaternion + return Quaternion(-x/lengthQuaternion, -y/lengthQuaternion, -z/lengthQuaternion, w/lengthQuaternion); + } + else { + // Throw an exception because the inverse cannot be computed + throw MathematicsException("MathematicsException : Impossible to compute the inverse of the quaternion because it's length is zero"); + } +} + +// Scalar product between two quaternions +inline double Quaternion::scalarProduct(const Quaternion& quaternion) const { + return (x*quaternion.x + y*quaternion.y + z*quaternion.z + w*quaternion.w); +} + +// Overloaded operator for the addition of two quaternions +inline Quaternion Quaternion::operator+(const Quaternion& quaternion) const { + // Return the result quaternion + return Quaternion(x + quaternion.x, y + quaternion.y, z + quaternion.z, w + quaternion.w); +} + +// Overloaded operator for the substraction of two quaternions +inline Quaternion Quaternion::operator-(const Quaternion& quaternion) const { + // Return the result of the substraction + return Quaternion(x-quaternion.x, y - quaternion.y, z - quaternion.z, w - quaternion.w); +} + +// Overloaded operator for the multiplication with a constant +inline Quaternion Quaternion::operator*(double nb) const { + // Return the result + return Quaternion(nb*x, nb*y, nb*z, nb*w); +} + +// Overloaded operator for the multiplication of two quaternions +inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const { + // Return the result of the multiplication + return Quaternion(w*quaternion.w - vectorV().scalarProduct(quaternion.vectorV()), w*quaternion.vectorV()+quaternion.w*vectorV() + vectorV().crossProduct(quaternion.vectorV())); +} + +// Overloaded operator for the assignment +inline Quaternion& Quaternion::operator=(const Quaternion& quaternion) { + // Check for self-assignment + if (this != &quaternion) { + x = quaternion.x; + y = quaternion.y; + z = quaternion.z; + w = quaternion.w; + } + + // Return this quaternion + return *this; +} + +// Overloaded operator for equality condition +inline bool Quaternion::operator==(const Quaternion& quaternion) const { + return (x == quaternion.x && y == quaternion.y && z == quaternion.z && w == quaternion.w); +} + +} // End of the ReactPhysics3D namespace + +#endif diff --git a/src/reactphysics3d/mathematics/Vector.cpp b/src/reactphysics3d/mathematics/Vector.cpp new file mode 100644 index 00000000..76c23b79 --- /dev/null +++ b/src/reactphysics3d/mathematics/Vector.cpp @@ -0,0 +1,266 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "Vector.h" + +// Namespaces +using namespace reactphysics3d; + +// Constructor without argument +Vector::Vector() + :nbComponent(0) { + tab = 0; +} + +// Constructor of the class Vector +Vector::Vector(int n) throw(std::invalid_argument) { + // Check the argument + if (n > 0) { + // Create the array that contains the values of the vector + nbComponent = n; + tab = new double[nbComponent]; + + // TODO : Remove this initialization + // Fill the array with zero's value + for(int i=0; i +#include +#include + + +// ReactPhysics3D namespace +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class Vector : + This class represents a Vector. + ------------------------------------------------------------------- +*/ +class Vector { + private : + double* tab; // Array of the vector's components + uint nbComponent; // number of components in the vector + + public : + Vector(); // Constructor without argument + Vector(int n) throw(std::invalid_argument); // Constructor of the class Vector + Vector(const Vector& vector); // Copy-constructor of the class Vector + Vector(const Vector3D& vector3d); // Conversion from Vector3D to Vector + virtual ~Vector(); // Destructor of the class Vector + double getValue(int n) const throw(std::invalid_argument); // Get a component of the vector + void setValue(int n, double value) throw(std::invalid_argument); // Set the value of a component of the vector + int getNbComponent() const; // Get the number of components in the vector + void initWithValue(double value); // Init all the elements with a given value + double length() const; // Get the length of the vector + Vector getUnit() const throw(MathematicsException); // Return the corresponding unit vector + double scalarProduct(const Vector& vector) const throw(MathematicsException); // Scalar product of two vectors + Vector crossProduct(const Vector& vector) const throw(MathematicsException); // Cross product of two vectors (in 3D only) + void fillInSubVector(uint index, const Vector& subVector); // Replace a part of the current vector with another sub-vector + Vector getSubVector(uint index, uint nbElements) const throw(std::invalid_argument); // Return a sub-vector of the current vector + void setVector(const Vector& vector); + bool isUnit() const; // Return true if the vector is unit and false otherwise + void changeSize(uint newSize); + + // --- Overloaded operators --- // + Vector operator+(const Vector& vector) const throw(MathematicsException); // Overloaded operator for addition + Vector operator-(const Vector& vector) const throw(MathematicsException); // Overloaded operator for substraction + Vector operator*(double number) const; // Overloaded operator for multiplication with a number + Vector& operator=(const Vector& vector) throw(MathematicsException); // Overloaded operator for the assignement to a Vector + bool operator==(const Vector& vector) const throw(MathematicsException); // Overloaded operator for the equality condition +}; + + +// ------ Definition of inlines functions ------ // + +// Method to get the value of a component of the vector (inline) +inline double Vector::getValue(int n) const throw(std::invalid_argument) { + // Check the argument + if (n>=0 && n= 0 && n= subVector.nbComponent); + + // For each value of the sub-vector + for (uint i=0; i nbComponent) { + throw std::invalid_argument("Error : arguments are out of bounds"); + } + + // Compute the sub-vector + Vector subVector(nbElements); + for (uint i=0, j=index; i +#include +#include + +// Namespaces +using namespace reactphysics3d; + +// Constructor of the class Vector3D +Vector3D::Vector3D() + :x(0.0), y(0.0), z(0.0) { + +} + +// Constructor with arguments +Vector3D::Vector3D(double x, double y, double z) + :x(x), y(y), z(z) { + +} + +// Copy-constructor +Vector3D::Vector3D(const Vector3D& vector) + :x(vector.x), y(vector.y), z(vector.z) { + +} + +// Destructor +Vector3D::~Vector3D() { + +} + +// Return the corresponding unit vector +Vector3D Vector3D::getUnit() const throw(MathematicsException) { + double lengthVector = length(); + + // Check if the length is equal to zero + if (lengthVector != 0) { + // Compute and return the unit vector + double lengthInv = 1.0 / lengthVector; + return Vector3D(x * lengthInv, y * lengthInv, z*lengthInv); + } + else { + // Throw an exception because the length of the vector is zero + throw MathematicsException("MathematicsException : Impossible to compute the unit vector because the length of the vector is zero"); + } +} + +// Return two unit orthogonal vectors of the current vector +Vector3D Vector3D::getOneOrthogonalVector() const { + assert(!this->isZero()); + Vector3D unitVector = this->getUnit(); + + // Compute a first orthogonal vector + Vector3D vector1; + if (!approxEqual(x, 0.0)) { // If x != 0 + vector1.setY(x); + vector1.setZ((-2*x*y*z + 2*x*z)/(2*(z*z + x*x))); + vector1.setX((-x*y-z*vector1.getZ())/x); + } + else if (!approxEqual(y, 0.0)) { // If y != 0 + vector1.setZ(y); + vector1.setX((-2*x*y*z + 2*x*y)/(2*(y*y + x*x))); + vector1.setY((-z*y-x*vector1.getX())/y); + } + else if (!approxEqual(z, 0.0)) { // If z != 0 + vector1.setX(z); + vector1.setY((-2*x*y*z + 2*y*z)/(2*(z*z + y*y))); + vector1.setZ((-x*z-y*vector1.getY())/z); + } + + assert(vector1.isUnit()); + return vector1; +} + +// Overloaded operator for addition +Vector3D Vector3D::operator+(const Vector3D& vector) const { + // Compute and return the sum of the two vectors + return Vector3D(x + vector.x, y + vector.y, z + vector.z); +} + +// Overloaded operator for substraction +Vector3D Vector3D::operator-(const Vector3D& vector) const { + // Compute and return the substraction of the two vectors + return Vector3D(x - vector.x, y - vector.y, z - vector.z); +} + +// Overloaded operator for multiplication with a number +Vector3D Vector3D::operator*(double number) const { + // Compute and return the result + return Vector3D(x * number, y * number, z * number); +} + +// Overloaded operator for the assignement to a Vector +Vector3D& Vector3D::operator=(const Vector3D& vector) { + // Check for self-assignment + if (this != &vector) { + // Copy the vector + x = vector.x; + y = vector.y; + z = vector.z; + } + + // Return a reference to the vector + return *this; +} diff --git a/src/reactphysics3d/mathematics/Vector3D.h b/src/reactphysics3d/mathematics/Vector3D.h new file mode 100644 index 00000000..03b6e2c8 --- /dev/null +++ b/src/reactphysics3d/mathematics/Vector3D.h @@ -0,0 +1,194 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef VECTOR3D_H +#define VECTOR3D_H + +// Libraries +#include +#include "exceptions.h" +#include "mathematics_functions.h" + +// TODO : Remove the methods getX(), getY(), getZ(), setX(), setY(), setZ() and replace the attributes +// x, y and z by an array values[3] + + +// ReactPhysics3D namespace +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class Vector3D : + This classrepresents 3 dimensionnal vector in space. + ------------------------------------------------------------------- +*/ +class Vector3D { + private : + double x; // X component of the vector + double y; // Y component of the vector + double z; // Z component of the vector + + public : + Vector3D(); // Constructor of the class Vector3D + Vector3D(double x, double y, double z); // Constructor with arguments + Vector3D(const Vector3D& vector); // Copy-constructor + virtual ~Vector3D(); // Destructor + double getValue(int index) const throw(std::invalid_argument); // Get a component of the vector + void setValue(int index, double value) throw(std::invalid_argument); // Set a component of the vector + double getX() const; // Get the x component of the vector + double getY() const; // Get the y component of the vector + double getZ() const; // Get the z component of the vector + void setX(double x); // Set the x component of the vector + void setY(double y); // Set the y component of the vector + void setZ(double z); // Set the z component of the vector + void setAllValues(double x, double y, double z); // Set all the values of the vector + double length() const; // Return the lenght of the vector + Vector3D getUnit() const throw(MathematicsException); // Return the corresponding unit vector + bool isUnit() const; // Return true if the vector is unit and false otherwise + bool isZero() const; // Return true if the current vector is the zero vector + Vector3D getOpposite() const; // Return the vector in the opposite direction + Vector3D getOneOrthogonalVector() const; // Return one unit orthogonal vectors of the current vector + double scalarProduct(const Vector3D& vector) const; // Scalar product of two vectors + Vector3D crossProduct(const Vector3D& vector) const; // Cross product of two vectors + bool isParallelWith(const Vector3D& vector) const; // Return true if two vectors are parallel + + // --- Overloaded operators --- // + Vector3D operator+(const Vector3D& vector) const; // Overloaded operator for addition + Vector3D operator-(const Vector3D& vector) const ; // Overloaded operator for substraction + Vector3D operator*(double number) const; // Overloaded operator for multiplication with a number + Vector3D& operator=(const Vector3D& vector); // Overloaded operator for the assignement to a Vector + bool operator==(const Vector3D& vector) const; // Overloaded operator for the equality condition +}; + +// Get the x component of the vector (inline) +inline double Vector3D::getX() const { + return x; +} + +// Get the y component of the vector (inline) +inline double Vector3D::getY() const { + return y; +} + +// Get the z component of the vector (inline) +inline double Vector3D::getZ() const { + return z; +} + +// Set the x component of the vector (inline) +inline void Vector3D::setX(double x) { + this->x = x; +} + +// Set the y component of the vector (inline) +inline void Vector3D::setY(double y) { + this->y = y; +} + +// Set the z component of the vector (inline) +inline void Vector3D::setZ(double z) { + this->z = z; +} + +// Get a component of the vector +inline double Vector3D::getValue(int index) const throw(std::invalid_argument) { + switch(index) { + case 0: return x; + case 1: return y; + case 2: return z; + default: // Throw an exception because of the wrong argument + throw std::invalid_argument("The argument is outside the bounds of the Vector3D"); + } +} + +// Set a component of the vector +inline void Vector3D::setValue(int index, double value) throw(std::invalid_argument) { + switch(index) { + case 0: x = value; + case 1: y = value; + case 2: z = value; + default: // Throw an exception because of the wrong argument + throw std::invalid_argument("The argument is outside the bounds of the Vector3D"); + } +} + +// Set all the values of the vector (inline) +inline void Vector3D::setAllValues(double x, double y, double z) { + this->x = x; + this->y = y; + this->z = z; +} + +// Return the length of the vector (inline) +inline double Vector3D::length() const { + // Compute and return the length of the vector + return sqrt(x*x + y*y + z*z); +} + +// Return the vector in the opposite direction +inline Vector3D Vector3D::getOpposite() const { + return (Vector3D(0.0, 0.0, 0.0) - *this); +} + +// Scalar product of two vectors (inline) +inline double Vector3D::scalarProduct(const Vector3D& vector) const { + // Compute and return the result of the scalar product + return (x * vector.x + y * vector.y + z * vector.z); +} + +// Cross product of two vectors (inline) +inline Vector3D Vector3D::crossProduct(const Vector3D& vector) const { + // Compute and return the cross product + return Vector3D(y * vector.z - z * vector.y, z * vector.x - x * vector.z , x * vector.y - y * vector.x); +} + +// Return true if two vectors are parallel +inline bool Vector3D::isParallelWith(const Vector3D& vector) const { + double scalarProd = this->scalarProduct(vector); + return approxEqual(std::abs(scalarProd), length() * vector.length()); +} + +// Return true if the vector is unit and false otherwise +inline bool Vector3D::isUnit() const { + return approxEqual(x*x+y*y+z*z, 1.0); +} + +// Return true if the vector is the zero vector +inline bool Vector3D::isZero() const { + return approxEqual(x*x+y*y+z*z, 0.0); +} + +// Overloaded operator for multiplication between a number and a Vector3D (inline) +inline Vector3D operator * (double number, const Vector3D& vector) { + // Compute and return the result vector + return vector * number; +} + +// Overloaded operator for the equality condition +inline bool Vector3D::operator == (const Vector3D& vector) const { + return (x == vector.x && y == vector.y && z == vector.z); +} + +} // End of the ReactPhysics3D namespace + +#endif diff --git a/src/reactphysics3d/mathematics/constants.h b/src/reactphysics3d/mathematics/constants.h new file mode 100644 index 00000000..34a10429 --- /dev/null +++ b/src/reactphysics3d/mathematics/constants.h @@ -0,0 +1,37 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef CONSTANTS_H +#define CONSTANTS_H + +// Libraries +#include + +// Constants +const double EPSILON = 0.00001; +const double ONE_MINUS_EPSILON = 0.99999; +const double INFINITY_CONST = std::numeric_limits::infinity(); +const double PI = 3.14159265; // Pi constant + +#endif diff --git a/src/reactphysics3d/mathematics/exceptions.cpp b/src/reactphysics3d/mathematics/exceptions.cpp new file mode 100755 index 00000000..f18abb1d --- /dev/null +++ b/src/reactphysics3d/mathematics/exceptions.cpp @@ -0,0 +1,63 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +// Libraries +#include "exceptions.h" + +// Namespaces +using namespace reactphysics3d; + + +// Constructor of the MathematicsException class +MathematicsException::MathematicsException(const std::string& msg) + :std::runtime_error(msg) { + +} + +// Destructor of the MathException class +MathematicsException::~MathematicsException() throw() { + +} + +// Overriden exception base class method +const char* MathematicsException::what() const throw() { + return std::runtime_error::what(); +} + +// Constructor of the DivisionByZeroException class +DivisionByZeroException::DivisionByZeroException(const std::string& msg) + :MathematicsException(msg) { + +} + +// Destructor of the DivisionByZeroException class +DivisionByZeroException::~DivisionByZeroException() throw() { + +} + +// Overriden exception base class method +const char* DivisionByZeroException::what() const throw() { + return MathematicsException::what(); +} + diff --git a/src/reactphysics3d/mathematics/exceptions.h b/src/reactphysics3d/mathematics/exceptions.h new file mode 100755 index 00000000..827a0f1c --- /dev/null +++ b/src/reactphysics3d/mathematics/exceptions.h @@ -0,0 +1,57 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef EXCEPTIONS_H +#define EXCEPTIONS_H + +// Libraries +#include + +// ReactPhysics3D namespace +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Exception class for the mathematics library + ------------------------------------------------------------------- +*/ + +// Class MathematicsException +class MathematicsException : public std::runtime_error { + public: + MathematicsException(const std::string& msg="MathException"); // Constructor + virtual ~MathematicsException() throw(); // Destructor + virtual const char* what() const throw(); // Overriding the base exception method +}; + +// Class DivisionByZeroException +class DivisionByZeroException : public MathematicsException { + public: + DivisionByZeroException(const std::string& msg="DivisionByZeroException : Division by zero !"); // Constructor + virtual ~DivisionByZeroException() throw(); // Destructor + virtual const char* what() const throw(); // Overriding the base exception method +}; + +} // End of the ReactPhysics3D namespace + +#endif diff --git a/src/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.cpp b/src/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.cpp new file mode 100644 index 00000000..d16bef68 --- /dev/null +++ b/src/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.cpp @@ -0,0 +1,101 @@ +/**************************************************************************** +* Copyright (C) 2009 Daniel Chappuis * +**************************************************************************** +* This file is part of ReactPhysics3D. * +* * +* ReactPhysics3D is free software: you can redistribute it and/or modify * +* it under the terms of the GNU Lesser General Public License as published * +* by the Free Software Foundation, either version 3 of the License, or * +* (at your option) any later version. * +* * +* ReactPhysics3D is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU Lesser General Public License for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with ReactPhysics3D. If not, see . * +***************************************************************************/ + +// Libraries +#include "LCPProjectedGaussSeidel.h" +#include + +using namespace reactphysics3d; + +// Constructor +LCPProjectedGaussSeidel::LCPProjectedGaussSeidel(uint maxIterations) + :LCPSolver(maxIterations) { + +} + +// Destructor +LCPProjectedGaussSeidel::~LCPProjectedGaussSeidel() { + +} + +// Solve a LCP problem using the Projected-Gauss-Seidel algorithm +// This method outputs the result in the lambda vector +void LCPProjectedGaussSeidel::solve(Matrix** J_sp, Matrix** B_sp, uint nbConstraints, + uint nbBodies, Body*** const bodyMapping, std::map bodyNumberMapping, + const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const { + + lambda = lambdaInit; + + double* d = new double[nbConstraints]; // TODO : Avoid those kind of memory allocation here for optimization (allocate once in the object) + uint indexBody1, indexBody2; + double deltaLambda; + double lambdaTemp; + uint i, iter; + Vector* a = new Vector[nbBodies]; // Array that contains nbBodies vector of dimension 6x1 + for (i=0; i bodyNumberMapping, + Vector* const a, uint nbBodies) const { + uint i; + uint indexBody1, indexBody2; + + // Init the vector a with zero values + for (i=0; i. * +***************************************************************************/ + +#ifndef LCPPROJECTEDGAUSSSEIDEL_H +#define LCPPROJECTEDGAUSSSEIDEL_H + +// Libraries +#include "LCPSolver.h" +#include + +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class LCPProjectedGaussSeidel : + This class implements the Projected-Gauss-Seidel (PGS) + algorithm in order to solve a LCP problem. This class inherits + from the LCPSolver class. + ------------------------------------------------------------------- +*/ +class LCPProjectedGaussSeidel : public LCPSolver { + protected: + + void computeVectorA(const Vector& lambda, uint nbConstraints, Body*** const bodyMapping, + Matrix** B_sp, std::map bodyNumberMapping, + Vector* const a, uint nbBodies) const ; // Compute the vector a used in the solve() method + + public: + LCPProjectedGaussSeidel(uint maxIterations); // Constructor + virtual ~LCPProjectedGaussSeidel(); // Destructor + virtual void solve(Matrix** J_sp, Matrix** B_sp, uint nbConstraints, + uint nbBodies, Body*** const bodyMapping, std::map bodyNumberMapping, + const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const; // Solve a LCP problem using Projected-Gauss-Seidel algorithm // Set the initial value for lambda +}; + +} // End of the ReactPhysics3D namespace + +#endif diff --git a/src/reactphysics3d/mathematics/lcp/LCPSolver.cpp b/src/reactphysics3d/mathematics/lcp/LCPSolver.cpp new file mode 100644 index 00000000..63483131 --- /dev/null +++ b/src/reactphysics3d/mathematics/lcp/LCPSolver.cpp @@ -0,0 +1,34 @@ +/**************************************************************************** +* Copyright (C) 2009 Daniel Chappuis * +**************************************************************************** +* This file is part of ReactPhysics3D. * +* * +* ReactPhysics3D is free software: you can redistribute it and/or modify * +* it under the terms of the GNU Lesser General Public License as published * +* by the Free Software Foundation, either version 3 of the License, or * +* (at your option) any later version. * +* * +* ReactPhysics3D is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU Lesser General Public License for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with ReactPhysics3D. If not, see . * +***************************************************************************/ + +// Libraries +#include "LCPSolver.h" + +using namespace reactphysics3d; + +// Constructor +LCPSolver::LCPSolver(uint maxIterations) + : maxIterations(maxIterations) { + +} + +// Destructor +LCPSolver::~LCPSolver() { + +} diff --git a/src/reactphysics3d/mathematics/lcp/LCPSolver.h b/src/reactphysics3d/mathematics/lcp/LCPSolver.h new file mode 100644 index 00000000..6d375da9 --- /dev/null +++ b/src/reactphysics3d/mathematics/lcp/LCPSolver.h @@ -0,0 +1,84 @@ +/**************************************************************************** +* Copyright (C) 2009 Daniel Chappuis * +**************************************************************************** +* This file is part of ReactPhysics3D. * +* * +* ReactPhysics3D is free software: you can redistribute it and/or modify * +* it under the terms of the GNU Lesser General Public License as published * +* by the Free Software Foundation, either version 3 of the License, or * +* (at your option) any later version. * +* * +* ReactPhysics3D is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU Lesser General Public License for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with ReactPhysics3D. If not, see . * +***************************************************************************/ + +#ifndef LCPSOLVER_H +#define LCPSOLVER_H + +// Libraries +#include +#include +#include "../Vector.h" +#include "../Matrix.h" +#include "../../body/Body.h" +#include "../../typeDefinitions.h" + +// ReactPhysics3D namespace +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class LCPSolver : + This abstract class represents an algorithm to solve a Linear + Complementary Problem (LCP). Given a matrix "A=J*B", a vector "b", + a vector "lowLimit" of lower limits and a vector "highLimits" of + upper limits. The goal is to find a vector "lambda" such that: + + w = Ax - b + lowLimits <= lambda <= highLimits + + and one of the thre following conditions holds : + + lambda_i = lowLimits_i, w_i >= 0 + lambda_i = highLimits_i, w_i >= 0 + lowLimits_i < lambda_i < highLimits_i, w_i = 0 + + Note that the matrix A is givent by the two matrices J and B with A=J*B. + But only their sparse representations "J_sp" and "B_sp" are passed in + arguments to solve() to be more efficient. + ------------------------------------------------------------------- +*/ +class LCPSolver { + protected: + uint maxIterations; // Maximum number of iterations + Vector lambdaInit; // Initial value for lambda at the beginning of the algorithm + + public: + LCPSolver(uint maxIterations); // Constructor + virtual ~LCPSolver(); // Destructor + virtual void solve(Matrix** J_sp, Matrix** B_sp, uint nbConstraints, + uint nbBodies, Body*** const bodyMapping, std::map bodyNumberMapping, + const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const=0; // Solve a LCP problem + void setLambdaInit(const Vector& lambdaInit); // Set the initial lambda vector + void setMaxIterations(uint maxIterations); // Set the maximum number of iterations +}; + +// Set the initial lambda vector +inline void LCPSolver::setLambdaInit(const Vector& lambdaInit) { + this->lambdaInit.changeSize(lambdaInit.getNbComponent()); + this->lambdaInit = lambdaInit; +} + +// Set the maximum number of iterations +inline void LCPSolver::setMaxIterations(uint maxIterations) { + assert(maxIterations > 0); + this->maxIterations = maxIterations; +} + +} // End of the ReactPhysics3D namespace + +#endif diff --git a/src/reactphysics3d/mathematics/mathematics.h b/src/reactphysics3d/mathematics/mathematics.h new file mode 100644 index 00000000..b00ae03e --- /dev/null +++ b/src/reactphysics3d/mathematics/mathematics.h @@ -0,0 +1,450 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef MATHEMATICS_H +#define MATHEMATICS_H + +// Libraries +#include "Matrix.h" +#include "Matrix3x3.h" +#include "Quaternion.h" +#include "Vector.h" +#include "Vector3D.h" +#include "constants.h" +#include "exceptions.h" +#include "mathematics_functions.h" +#include +#include +#include +#include + +// ReactPhysics3D namespace +namespace reactphysics3d { + +// ---------- Mathematics functions ---------- // + +// Rotate a vector according to a rotation quaternion. +// The function returns the vector rotated according to the quaternion in argument +inline reactphysics3d::Vector3D rotateVectorWithQuaternion(const reactphysics3d::Vector3D& vector, const reactphysics3d::Quaternion& quaternion) { + // Convert the vector into a quaternion + reactphysics3d::Quaternion vectorQuaternion(0, vector); + + // Compute the quaternion rotation result + reactphysics3d::Quaternion quaternionResult = (quaternion * vectorQuaternion) * quaternion.getInverse(); + + // Convert the result quaternion into a vector + return quaternionResult.vectorV(); +} + +// Given two lines (given by the points "point1", "point2" and the vectors "d1" and "d2" that are not parallel, this method returns the values +// "alpha" and "beta" such that the two points P1 and P2 are the two closest point between the two lines and such that +// P1 = point1 + alpha * d1 +// P2 = point2 + beta * d2 +inline void closestPointsBetweenTwoLines(const reactphysics3d::Vector3D& point1, const reactphysics3d::Vector3D& d1, const reactphysics3d::Vector3D& point2, + const reactphysics3d::Vector3D& d2, double* alpha, double* beta) { + + reactphysics3d::Vector3D r = point1 - point2; + double a = d1.scalarProduct(d1); + double b = d1.scalarProduct(d2); + double c = d1.scalarProduct(r); + double e = d2.scalarProduct(d2); + double f = d2.scalarProduct(r); + double d = a*e-b*b; + + // The two lines must not be parallel + assert(!reactphysics3d::approxEqual(d, 0.0)); + + // Compute the "alpha" and "beta" values + *alpha = (b*f -c*e)/d; + *beta = (a*f-b*c)/d; +} + +// This method returns true if the point "P" is on the segment between "segPointA" and "segPointB" and return false otherwise +inline bool isPointOnSegment(const reactphysics3d::Vector3D& segPointA, const reactphysics3d::Vector3D& segPointB, const reactphysics3d::Vector3D& P) { + + // Check if the point P is on the line between "segPointA" and "segPointB" + reactphysics3d::Vector3D d = segPointB - segPointA; + reactphysics3d::Vector3D dP = P - segPointA; + if (!d.isParallelWith(dP)) { + return false; + } + + // Compute the length of the segment + double segmentLength = d.length(); + + // Compute the distance from point "P" to points "segPointA" and "segPointB" + double distA = dP.length(); + double distB = (P - segPointB).length(); + + // If one of the "distA" and "distB" is greather than the length of the segment, then P is not on the segment + if (distA > segmentLength || distB > segmentLength) { + return false; + } + + // Otherwise, the point P is on the segment + return true; +} + + +// Given two lines in 3D that intersect, this method returns the intersection point between the two lines. +// The first line is given by the point "p1" and the vector "d1", the second line is given by the point "p2" and the vector "d2". +inline reactphysics3d::Vector3D computeLinesIntersection(const reactphysics3d::Vector3D& p1, const reactphysics3d::Vector3D& d1, + const reactphysics3d::Vector3D& p2, const reactphysics3d::Vector3D& d2) { + // Computes the two closest points on the lines + double alpha, beta; + closestPointsBetweenTwoLines(p1, d1, p2, d2, &alpha, &beta); + reactphysics3d::Vector3D point1 = p1 + alpha * d1; + reactphysics3d::Vector3D point2 = p2 + beta * d2; + + // The two points must be very close + //assert((point1-point2).length() <= 0.1); + + // Return the intersection point (halfway between "point1" and "point2") + return 0.5 * (point1 + point2); +} + + +// Given two segments in 3D that are not parallel and that intersect, this method computes the intersection point between the two segments. +// This method returns the intersection point. +inline reactphysics3d::Vector3D computeNonParallelSegmentsIntersection(const reactphysics3d::Vector3D& seg1PointA, const reactphysics3d::Vector3D& seg1PointB, + const reactphysics3d::Vector3D& seg2PointA, const reactphysics3d::Vector3D& seg2PointB) { + // Determine the lines of both segments + reactphysics3d::Vector3D d1 = seg1PointB - seg1PointA; + reactphysics3d::Vector3D d2 = seg2PointB - seg2PointA; + + // The segments must not be parallel + assert(!d1.isParallelWith(d2)); + + // Compute the closet points between the two lines + double alpha, beta; + closestPointsBetweenTwoLines(seg1PointA, d1, seg2PointA, d2, &alpha, &beta); + reactphysics3d::Vector3D point1 = seg1PointA + alpha * d1; + reactphysics3d::Vector3D point2 = seg2PointA + beta * d2; + + // The closest points have to be on the segments, otherwise there is no intersection between the segments + assert(isPointOnSegment(seg1PointA, seg1PointB, point1)); + assert(isPointOnSegment(seg2PointA, seg2PointB, point2)); + + // If the two closest point aren't very close, there is no intersection between the segments + reactphysics3d::Vector3D d = point2 - point1; + assert(d.length() <= EPSILON); + + // They are very close so we return the intersection point (halfway between "point1" and "point2" + return 0.5 * (point1 + point2); +} + + +// Move a set of points by a given vector. +// The method returns a set of points moved by the given vector. +inline std::vector movePoints(const std::vector& points, const reactphysics3d::Vector3D& vector) { + std::vector result; + + // For each point of the set + for (unsigned int i=0; i projectPointsOntoPlane(const std::vector& points, const reactphysics3d::Vector3D& A, + const reactphysics3d::Vector3D& normal) { + assert(normal.length() != 0.0); + + std::vector projectedPoints; + reactphysics3d::Vector3D n = normal.getUnit(); + + // For each point of the set + for (unsigned int i=0; i rectangle) { + assert(rectangle.size() == 4); + double distPSegment1 = computeDistanceBetweenPointAndLine(P, rectangle[0], rectangle[1] - rectangle[0]); + double distPSegment2 = computeDistanceBetweenPointAndLine(P, rectangle[1], rectangle[2] - rectangle[1]); + double distPSegment3 = computeDistanceBetweenPointAndLine(P, rectangle[2], rectangle[3] - rectangle[2]); + double distPSegment4 = computeDistanceBetweenPointAndLine(P, rectangle[3], rectangle[0] - rectangle[3]); + double distSegment1Segment3 = computeDistanceBetweenPointAndLine(rectangle[0], rectangle[3], rectangle[3] - rectangle[2]); + double distSegment2Segment4 = computeDistanceBetweenPointAndLine(rectangle[1], rectangle[3], rectangle[0] - rectangle[3]); + Vector3D resultPoint; + + // Check if P is between the lines of the first pair of parallel segments of the rectangle + if (distPSegment1 <= distSegment1Segment3 && distPSegment3 <= distSegment1Segment3) { + // Find among segments 2 and 4 which one is the nearest + if (distPSegment2 <= distPSegment4) { // Segment 2 is the nearest + // We compute the projection of the point P onto the segment 2 + resultPoint = computeOrthogonalProjectionOfPointOntoALine(P, rectangle[1], rectangle[2] - rectangle[1]); + } + else { // Segment 4 is the nearest + // We compute the projection of the point P onto the segment 4 + resultPoint = computeOrthogonalProjectionOfPointOntoALine(P, rectangle[3], rectangle[0] - rectangle[3]); + } + } + // Check if P is between the lines of the second pair of parallel segments of the rectangle + else if (distPSegment2 <= distSegment2Segment4 && distPSegment4 <= distSegment2Segment4) { + // Find among segments 1 and 3 which one is the nearest + if (distPSegment1 <= distPSegment3) { // Segment 1 is the nearest + // We compute the projection of the point P onto the segment 1 + resultPoint = computeOrthogonalProjectionOfPointOntoALine(P, rectangle[0], rectangle[1] - rectangle[0]); + } + else { // Segment 3 is the nearest + // We compute the projection of the point P onto the segment 3 + resultPoint = computeOrthogonalProjectionOfPointOntoALine(P, rectangle[2], rectangle[3] - rectangle[2]); + } + } + else if (distPSegment4 <= distPSegment2) { + if (distPSegment1 <= distPSegment3) { // The point P is in the corner of point rectangle[0] + // Return the corner of the rectangle + return rectangle[0]; + } + else { // The point P is in the corner of point rectangle[3] + // Return the corner of the rectangle + return rectangle[3]; + } + } + else { + if (distPSegment1 <= distPSegment3) { // The point P is in the corner of point rectangle[1] + // Return the corner of the rectangle + return rectangle[1]; + } + else { // The point P is in the corner of point rectangle[2] + // Return the corner of the rectangle + return rectangle[2]; + } + } + + // Return the result point + return resultPoint; +} + +// Compute the intersection between two parallel segments (the first segment is between the points "seg1PointA" and "seg1PointB" and the second +// segment is between the points "seg2PointA" and "seg2PointB"). The result is the segment intersection (represented by the points "resultPointA" +// and "resultPointB". Because the two given segments don't have to be on the same exact line, the result intersection segment will a segment +// halway between the first and the second given segments. +inline void computeParallelSegmentsIntersection(const reactphysics3d::Vector3D& seg1PointA, const reactphysics3d::Vector3D& seg1PointB, + const reactphysics3d::Vector3D& seg2PointA, const reactphysics3d::Vector3D& seg2PointB, + reactphysics3d::Vector3D& resultPointA, reactphysics3d::Vector3D& resultPointB) { + // Compute the segment vectors + reactphysics3d::Vector3D d1 = seg1PointB - seg1PointA; + reactphysics3d::Vector3D d2 = seg2PointB - seg2PointA; + + // The two segments should be parallel + assert(d1.isParallelWith(d2)); + + // Compute the projection of the two points of the second segment onto the vector of segment 1 + double projSeg2PointA = d1.getUnit().scalarProduct(seg2PointA - seg1PointA); + double projSeg2PointB = d1.getUnit().scalarProduct(seg2PointB - seg1PointA); + + // The projections intervals should intersect + assert(!(projSeg2PointA < 0.0 && projSeg2PointB < 0.0)); + assert(!(projSeg2PointA > d1.length() && projSeg2PointB > d1.length())); + + // Compute the vector "v" from a point on the line 1 to the orthogonal point of the line 2 + reactphysics3d::Vector3D point = computeOrthogonalProjectionOfPointOntoALine(seg2PointA, seg1PointA, d1); + reactphysics3d::Vector3D v = seg2PointA - point; + + // Return the segment intersection according to the configuration of two projection intervals + if (projSeg2PointA >= 0 && projSeg2PointA <= d1.length() && projSeg2PointB >= d1.length()) { + // Move the contact points halfway between the two segments + resultPointA = seg2PointA - 0.5 * v; + resultPointB = seg1PointB + 0.5 * v; + } + else if (projSeg2PointA <= 0 && projSeg2PointB >= 0 && projSeg2PointB <= d1.length()) { + // Move the contact points halfway between the two segments + resultPointA = seg1PointA + 0.5 * v; + resultPointB = seg2PointB - 0.5 * v; + } + else if (projSeg2PointA <= 0 && projSeg2PointB >= d1.length()) { + // Move the contact points halfway between the two segments + resultPointA = seg1PointA + 0.5 * v; + resultPointB = seg1PointB + 0.5 * v; + } + else if (projSeg2PointA <= d1.length() && projSeg2PointB <= d1.length()) { + // Move the contact points halfway between the two segments + resultPointA = seg2PointA - 0.5 * v; + resultPointB = seg2PointB - 0.5 * v; + } +} + +// This method clip a 3D segment with 3D rectangle polygon. The segment and the rectangle are asssumed to be on the same plane. We +// also assume that the segment is not completely outside the clipping rectangle. +// The segment is given by the two vertices in "segment" and the rectangle is given by the ordered vertices in "clipRectangle". +// This method returns the clipped segment. +inline std::vector clipSegmentWithRectangleInPlane(const std::vector& segment, const std::vector clipRectangle) { + double const epsilon = 0.01; + + assert(segment.size() == 2); + assert(clipRectangle.size() == 4); + + std::vector inputSegment = segment; + std::vector outputSegment; + + // For each edge of the clip rectangle + for (unsigned int i=0; i<4; ++i) { + outputSegment.clear(); + + // Current clipped segment + //assert(inputSegment.size() == 2); + reactphysics3d::Vector3D S = inputSegment[0]; + reactphysics3d::Vector3D P = inputSegment[1]; + + // Edge of the clip rectangle + reactphysics3d::Vector3D A = clipRectangle[i]; + reactphysics3d::Vector3D B = clipRectangle[ (i+1) % 4]; + reactphysics3d::Vector3D planeNormal = clipRectangle[(i+2) % 4] - clipRectangle[(i+1) % 4]; + + // If the point P is inside the clip plane + if (planeNormal.scalarProduct(P-A) >= 0.0 - epsilon) { + // If the point S is inside the clip plane + if (planeNormal.scalarProduct(S-A) >= 0.0 - epsilon) { + outputSegment.push_back(P); + outputSegment.push_back(S); + } + else { // P is inside and S is outside the clip plane + // Compute the intersection point between the segment SP and the clip plane + reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A); + + outputSegment.push_back(P); + outputSegment.push_back(intersectPoint); + } + } + else if (planeNormal.scalarProduct(S-A) > 0.0 - epsilon) { // P is outside and S is inside the clip plane + // Compute the intersection point between the segment SP and the clip plane + reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A); + + outputSegment.push_back(S); + outputSegment.push_back(intersectPoint); + } + + inputSegment = outputSegment; + } + + // Return the clipped segment + return outputSegment; +} + +// This method uses the Sutherland-Hodgman clipping algorithm to clip a subject polygon (given by the ordered 3D vertices in "subjectPolygon") using +// a rectangle polygon (given by the ordered 3D vertices in "clipRectangle"). The subject polygon and the clip rectangle are in 3D but we assumed that +// they are on a same plane in 3D. The method returns the ordered 3D vertices of the subject polygon clipped using the rectangle polygon. +inline std::vector clipPolygonWithRectangleInPlane(const std::vector& subjectPolygon, const std::vector& clipRectangle) { + double const epsilon = 0.1; + assert(clipRectangle.size() == 4); + + std::vector outputPolygon; + std::vector inputPolygon = subjectPolygon; + + // For each edge of the clip rectangle + for (unsigned int i=0; i<4; ++i) { + outputPolygon.clear(); + + // Each edge defines a clip plane. The clip plane is define by a point on this plane (a vertice of the current edge) and + // a plane normal (because we are using a clip rectangle, the plane normal is the next edge of the clip rectangle). + reactphysics3d::Vector3D planeNormal = clipRectangle[(i+2) % 4] - clipRectangle[(i+1) % 4]; + reactphysics3d::Vector3D A = clipRectangle[i]; // Segment AB is the current segment of the "clipRectangle" + reactphysics3d::Vector3D B = clipRectangle[(i+1) % 4]; + reactphysics3d::Vector3D S = inputPolygon[0]; + + // For each vertex of the subject polygon + for (unsigned int j=0; j= 0.0 - epsilon) { + // If the point S is also inside the clip plane + if (planeNormal.scalarProduct(S-A) >= 0.0 - epsilon) { + outputPolygon.push_back(P); + } + else { // If the point S is outside the clip plane + // Compute the intersection point between the segment SP and the clip plane + reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A); + + outputPolygon.push_back(intersectPoint); + outputPolygon.push_back(P); + } + } + else if (planeNormal.scalarProduct(S-A) > 0.0) { + // Compute the intersection point between the segment SP and the clip plane + reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A); + + outputPolygon.push_back(intersectPoint); + } + S = P; + } + inputPolygon = outputPolygon; + } + + // Return the clipped polygon + return outputPolygon; +} + +// Compute the intersection point between a line and a plane in 3D space. There must be an intersection, therefore the +// the lineVector must not be orthogonal to the planeNormal. +inline reactphysics3d::Vector3D intersectLineWithPlane(const reactphysics3d::Vector3D& linePoint, const reactphysics3d::Vector3D& lineVector, + const reactphysics3d::Vector3D& planePoint, const reactphysics3d::Vector3D& planeNormal) { + assert(!approxEqual(lineVector.scalarProduct(planeNormal), 0.0)); + + // The plane is represented by the equation planeNormal dot X = d where X is a point of the plane + double d = planeNormal.scalarProduct(planePoint); + + // Compute the parameter t + double t = (d - planeNormal.scalarProduct(linePoint)) / planeNormal.scalarProduct(lineVector); + + // Compute the intersection point + return linePoint + lineVector * t; +} + + +} // End of the ReactPhysics3D namespace + + +#endif diff --git a/src/reactphysics3d/mathematics/mathematics_functions.h b/src/reactphysics3d/mathematics/mathematics_functions.h new file mode 100644 index 00000000..2a45a0fd --- /dev/null +++ b/src/reactphysics3d/mathematics/mathematics_functions.h @@ -0,0 +1,47 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef MATHEMATICS_FUNCTIONS_H +#define MATHEMATICS_FUNCTIONS_H + +// Libraries +#include "constants.h" + +// ReactPhysics3D namespace +namespace reactphysics3d { + +// ---------- Mathematics functions ---------- // + +// function to test if two real numbers are (almost) equal +// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON] +inline bool approxEqual(double a, double b) { + double difference = a - b; + return (difference < EPSILON && difference > -EPSILON); +} + +} // End of ReactPhysics3D namespace + + + +#endif diff --git a/src/reactphysics3d/reactphysics3d.h b/src/reactphysics3d/reactphysics3d.h new file mode 100644 index 00000000..ef412baf --- /dev/null +++ b/src/reactphysics3d/reactphysics3d.h @@ -0,0 +1,53 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + + +/******************************************************************************** +* ReactPhysics3D * +* Version 0.01 * +* http://code.google.com/p/reactphysics3d/ * +* Daniel Chappuis * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_H +#define REACTPHYSICS3D_H + +// Libraries +#include "body/RigidBody.h" +#include "engine/PhysicsWorld.h" +#include "engine/PhysicsEngine.h" +#include "body/BoundingVolume.h" +#include "body/OBB.h" +#include "body/AABB.h" + +// TODO : Use using namespace std in every possible cpp files to increase readability + +// TODO : Check for memory management (RigidBody must free for BoundingVolume it has, ...) + +// Alias to the ReactPhysics3D namespace +namespace rp3d = reactphysics3d; + +// TODO : Replace in all files of the project "unsigned int" by "uint" (see typeDefinitions.h" + +#endif diff --git a/src/reactphysics3d/testing/reactphysics3dTestSuite.cpp b/src/reactphysics3d/testing/reactphysics3dTestSuite.cpp new file mode 100644 index 00000000..3b9b9f9c --- /dev/null +++ b/src/reactphysics3d/testing/reactphysics3dTestSuite.cpp @@ -0,0 +1,61 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +// Libraries +#include +#include "TestSuite/Suite.h" +#include "testing_mathematics/MathematicsTest.h" +#include "testing_mathematics/VectorTest.h" +#include "testing_mathematics/Vector3DTest.h" +#include "testing_mathematics/MatrixTest.h" +#include "testing_mathematics/Matrix3x3Test.h" +#include "testing_mathematics/QuaternionTest.h" +#include "testing_physics/TimeTest.h" +#include "testing_physics/KilogramTest.h" + +// Namespaces +using namespace std; +using namespace TestSuite; + +// Main function +int main() { + + // ReactPhysics3D TestSuite + Suite reactphysics3DTestSuite("ReactPhysics3D TestSuite"); + + // Mathematics tests + reactphysics3DTestSuite.addTest(new MathematicsTest); + reactphysics3DTestSuite.addTest(new VectorTest); + reactphysics3DTestSuite.addTest(new Vector3DTest); + reactphysics3DTestSuite.addTest(new MatrixTest); + reactphysics3DTestSuite.addTest(new Matrix3x3Test); + reactphysics3DTestSuite.addTest(new QuaternionTest); + + // Physics tests + reactphysics3DTestSuite.addTest(new TimeTest); + reactphysics3DTestSuite.addTest(new KilogramTest); + + // Run the ReactPhysics3D TestSuite and display the report + reactphysics3DTestSuite.run(); + long nbFailures = reactphysics3DTestSuite.report(); + reactphysics3DTestSuite.free(); + return nbFailures; + double inPause; + cin >> inPause; +} diff --git a/src/reactphysics3d/testing/testing_mathematics/MathematicsTest.h b/src/reactphysics3d/testing/testing_mathematics/MathematicsTest.h new file mode 100644 index 00000000..2182b826 --- /dev/null +++ b/src/reactphysics3d/testing/testing_mathematics/MathematicsTest.h @@ -0,0 +1,63 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +#ifndef MATHEMATICSTEST_H +#define MATHEMATICSTEST_H + +// Libraries +#include "../TestSuite/Test.h" +#include "../../mathematics/mathematics.h" +#include +#include +#include + +// Namespaces +using namespace reactphysics3d; + +// Class MathematicsTest +class MathematicsTest : public TestSuite::Test { + private : + + public : + + // Constructor + MathematicsTest() { + + } + + // Run method of the Test + void run() { + testEqual(); + } + + // Test the equal() method + void testEqual() { + double number1 = 19.13417; + double number2 = 19.13417 + EPSILON/2.0; + double number3 = 19.13417 + 2*EPSILON; + + test_(equal(number1, number2)); + test_(equal(number2, number1)); + test_(!equal(number1, number3)); + test_(!equal(number3, number1)); + } + +}; + +#endif diff --git a/src/reactphysics3d/testing/testing_mathematics/Matrix3x3Test.h b/src/reactphysics3d/testing/testing_mathematics/Matrix3x3Test.h new file mode 100644 index 00000000..25b486db --- /dev/null +++ b/src/reactphysics3d/testing/testing_mathematics/Matrix3x3Test.h @@ -0,0 +1,417 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +#ifndef MATRIX3DTEST_H +#define MATRIX3DTEST_H + +// Libraries +#include "../TestSuite/Test.h" +#include "../../mathematics/Matrix3x3.h" +#include +#include +#include + +// Namespaces +using namespace reactphysics3d; + +// Class MatrixTest +class Matrix3x3Test : public TestSuite::Test { + private : + Matrix3x3 matrix1; + Matrix3x3 matrix2; + public : + + // Constructor + Matrix3x3Test() { + matrix1.setAllValues(3, 7, -5, 13, -1, 2, 6, 5, 9); + matrix2.setAllValues(-13, 8, 2, 5, -25, 11, -7, 6, 21); + } + + // Run method of the Test + void run() { + testConstructors(); + testGetValue(); + testSetValue(); + testSetAllValues() ; + testGetTranspose(); + testGetInverse(); + testGetDeterminant(); + testGetTrace(); + testGetQuaternion(); + testIdentityMatrix(); + testOperatorAddition(); + testOperatorSubstraction(); + testOperatorMultiplicationWithConstant(); + testOperatorMultiplicationWithMatrix(); + testOperatorMultiplicationWithVector(); + testOperatorAssignment(); + testOperatorEquality(); + } + + // Test the constructors + void testConstructors() { + + // Constructor without argument + Matrix3x3 matrix; // This shouldn't throw an exception + test_(matrix.getValue(0,0) == 0); + test_(matrix.getValue(1,2) == 0); + test_(matrix.getValue(0,2) == 0); + + // Constructor with arguments + Matrix3x3 matrix3(1, 2, 3, 4, 5, 6, 7, 8, 9); + test_(matrix3.getValue(0,0) == 1); + test_(matrix3.getValue(0,1) == 2); + test_(matrix3.getValue(0,2) == 3); + test_(matrix3.getValue(1,0) == 4); + test_(matrix3.getValue(1,1) == 5); + test_(matrix3.getValue(1,2) == 6); + test_(matrix3.getValue(2,0) == 7); + test_(matrix3.getValue(2,1) == 8); + test_(matrix3.getValue(2,2) == 9); + + // Copy-constructor + Matrix3x3 matrix4 = matrix3; + test_(matrix4.getValue(0,0) == 1); + test_(matrix4.getValue(0,1) == 2); + test_(matrix4.getValue(0,2) == 3); + test_(matrix4.getValue(1,0) == 4); + test_(matrix4.getValue(1,1) == 5); + test_(matrix4.getValue(1,2) == 6); + test_(matrix4.getValue(2,0) == 7); + test_(matrix4.getValue(2,1) == 8); + test_(matrix4.getValue(2,2) == 9); + + // Conversion-constructor (Quaternion --> Matrix3x3) + + // Rotation matrix of a rotation of 180 degrees around x axis + Matrix3x3 rotation1(1, 0, 0, 0, -1, 0, 0, 0, -1); + Quaternion quaternion1(1, 0, 0, 0); + Matrix3x3 matrix5(quaternion1); // Convert the quaternion into a matrix + test_(matrix5 == rotation1); // Check if the matrix result and the rotation matrix are the same + + // Rotation matrix of a rotation of 180 degrees around y axis + Matrix3x3 rotation2(-1, 0, 0, 0, 1, 0, 0, 0, -1); + Quaternion quaternion2(0, 1, 0, 0); + Matrix3x3 matrix6(quaternion2); // Convert the quaternion into a matrix + test_(matrix6 == rotation2); // Check if the matrix result and the rotation matrix are the same + + // Rotation matrix of a rotation of 180 degrees around z axis + Matrix3x3 rotation3(-1, 0, 0, 0, -1, 0, 0, 0, 1); + Quaternion quaternion3(0, 0, 1, 0); + Matrix3x3 matrix7(quaternion3); // Convert the quaternion into a matrix + test_(matrix7 == rotation3); // Check if the matrix result and the rotation matrix are the same + } + + // Test getValue() + void testGetValue() { + // Try a valid getValue() + try { + test_(matrix1.getValue(0, 0) == 3); // This shouldn't throw an exception + test_(matrix1.getValue(1, 0) == 13); // This shouldn't throw an exception + test_(matrix1.getValue(1, 2) == 2); // This shouldn't throw an exception + test_(matrix1.getValue(1, 1) == -1); // This shouldn't throw an exception + succeed_(); // Succeed if no exceptions have been thrown + } + catch(std::invalid_argument& ex) { + fail_("Valid getValue() call throws an exception"); // Failed if an exception has been thrown + } + + // Try an invalid getValue() call + try { + matrix1.getValue(-1, 0); // This should throw an exception + fail_("Invalid getValue() call undetected"); + } + catch(std::invalid_argument& ex) { + succeed_(); // Succeed if an exception has been thrown + } + + // Try an invalid getValue() call + try { + matrix1.getValue(0, 3); // This should throw an exception + fail_("Invalid getValue() call undetected"); + } + catch(std::invalid_argument& ex) { + succeed_(); // Succeed if an exception has been thrown + } + } + + // Test setValue() + void testSetValue() { + + Matrix3x3 matrix; + + // Try a valid setValue() + try { + matrix.setValue(0, 0, 18); // This shouldn't throw an exception + matrix.setValue(0, 2, -6); // This shouldn't throw an exception + matrix.setValue(1, 0, -44); // This shouldn't throw an exception + matrix.setValue(1, 2, 21); // This shouldn't throw an exception + matrix.setValue(1, 1, 5); // This shouldn't throw an exception + test_(matrix.getValue(0, 0) == 18); + test_(matrix.getValue(0, 2) == -6); + test_(matrix.getValue(1, 0) == -44); + test_(matrix.getValue(1, 2) == 21); + test_(matrix.getValue(1, 1) == 5); + succeed_(); // Succeed if no exceptions have been thrown + } + catch(std::invalid_argument& ex) { + fail_("Valid setValue() call throws an exception"); // Failed if an exception has been thrown + } + + // Try an invalid setValue() call + try { + matrix.setValue(-1, 0, 42); // This should throw an exception + fail_("Invalid setValue() call undetected"); // Failed if no exception have been thrown + } + catch(std::invalid_argument& ex) { + succeed_(); // Succeed if an exception has been thrown + } + + // Try an invalid setValue() call + try { + matrix1.setValue(0, 3, 53); // This should throw an exception + fail_("Invalid setValue() call undetected"); // Failed if no exception have been thrown + } + catch(std::invalid_argument& ex) { + succeed_(); // Succeed if an exception has been thrown + } + } + + // Test setAllValues() + void testSetAllValues() { + Matrix3x3 matrix; + matrix.setAllValues(1,2,3,4,5,6,7,8,9); + test_(matrix.getValue(0,0) == 1); + test_(matrix.getValue(0,1) == 2); + test_(matrix.getValue(0,2) == 3); + test_(matrix.getValue(1,0) == 4); + test_(matrix.getValue(1,1) == 5); + test_(matrix.getValue(1,2) == 6); + test_(matrix.getValue(2,0) == 7); + test_(matrix.getValue(2,1) == 8); + test_(matrix.getValue(2,2) == 9); + } + + // Test getTranspose() + void testGetTranspose() { + // Get the transpose of matrix1 + Matrix3x3 matrix = matrix1.getTranspose(); + + // Test the transpose matrix + test_(matrix.getValue(0, 0) == 3); + test_(matrix.getValue(0, 1) == 13); + test_(matrix.getValue(0, 2) == 6); + test_(matrix.getValue(1, 0) == 7); + test_(matrix.getValue(1, 1) == -1); + test_(matrix.getValue(1, 2) == 5); + test_(matrix.getValue(2, 0) == -5); + test_(matrix.getValue(2, 1) == 2); + test_(matrix.getValue(2, 2) == 9); + } + + // Test getInverse() + void testGetInverse() { + + // Construct a 3x3 matrix + Matrix3x3 matrix(0, 1, 2, 1, 0, 3, 4, -3, 8); + + // Try to inverse a invertible matrix + try { + Matrix3x3 result = matrix.getInverse(); // This shouldn't thrown an exception + test_(result.getValue(0, 0) == -4.5); + test_(result.getValue(0, 1) == 7); + test_(result.getValue(0, 2) == -3.0/2.0); + test_(result.getValue(1, 0) == -2); + test_(result.getValue(1, 1) == 4); + test_(result.getValue(1, 2) == -1); + test_(result.getValue(2, 0) == 3.0/2.0); + test_(result.getValue(2, 1) == -2); + test_(result.getValue(2, 2) == 1.0/2.0); + succeed_(); // Succeed if no exceptions have been thrown + } + catch(MathematicsException& ex) { + fail_("Valid getInverse() call throws an exception"); // Failed if an exception has been thrown + } + + // Try to inverse a square non-invertible matrix (determinant equal to zero) + try { + Matrix3x3 matrix4; + matrix4.getInverse(); // This should throw an exception + fail_("Invalid getInverse() call undetected (non-invertible matrix)"); // Failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // Succeed if an exception has been thrown + } + } + + // Test getDeterminant() + void testGetDeterminant() { + Matrix3x3 matrix; + test_(matrix.getDeterminant() == 0); + test_(matrix1.getDeterminant() == -1147); + test_(matrix2.getDeterminant() == 5937); + } + + // Test getTrace() + void testGetTrace() { + Matrix3x3 matrix; + test_(matrix.getTrace() == 0); + test_(matrix1.getTrace() == 11); + test_(matrix2.getTrace() == -17); + } + + // Test getQuaternion() + void testGetQuaternion() { + // Rotation matrix of a rotation of 180 degrees around x axis + Matrix3x3 rotation1(1, 0, 0, 0, -1, 0, 0, 0, -1); + + // Convert the matrix into a quaternion + Quaternion quaternion1 = rotation1.getQuaternion(); + test_(quaternion1.getX() == 1); + test_(quaternion1.getY() == 0); + test_(quaternion1.getZ() == 0); + test_(quaternion1.getW() == 0); + + // Rotation matrix of a rotation of 180 degrees around y axis + Matrix3x3 rotation2(-1, 0, 0, 0, 1, 0, 0, 0, -1); + + // Convert the matrix into a quaternion + Quaternion quaternion2 = rotation2.getQuaternion(); + test_(quaternion2.getX() == 0); + test_(quaternion2.getY() == 1); + test_(quaternion2.getZ() == 0); + test_(quaternion2.getW() == 0); + + // Rotation matrix of a rotation of 180 degrees around z axis + Matrix3x3 rotation3(-1, 0, 0, 0, -1, 0, 0, 0, 1); + + // Convert the matrix into a quaternion + Quaternion quaternion3 = rotation3.getQuaternion(); + test_(quaternion3.getX() == 0); + test_(quaternion3.getY() == 0); + test_(quaternion3.getZ() == 1); + test_(quaternion3.getW() == 0); + } + + // Test identityMatrix() + void testIdentityMatrix() { + Matrix3x3 matrix = Matrix3x3::identity(); + test_(matrix.getValue(0, 0) == 1); + test_(matrix.getValue(0, 1) == 0); + test_(matrix.getValue(0, 2) == 0); + test_(matrix.getValue(1, 0) == 0); + test_(matrix.getValue(1, 1) == 1); + test_(matrix.getValue(1, 2) == 0); + test_(matrix.getValue(2, 0) == 0); + test_(matrix.getValue(2, 1) == 0); + test_(matrix.getValue(2, 2) == 1); + } + + // Test operator+() + void testOperatorAddition() { + Matrix3x3 result = matrix1 + matrix2; + test_(result.getValue(0,0) == -10); + test_(result.getValue(0,1) == 15); + test_(result.getValue(0,2) == -3); + test_(result.getValue(1,0) == 18); + test_(result.getValue(1,1) == -26); + test_(result.getValue(1,2) == 13); + test_(result.getValue(2,0) == -1); + test_(result.getValue(2,1) == 11); + test_(result.getValue(2,2) == 30); + } + + // Test operator-() + void testOperatorSubstraction() { + Matrix3x3 result = matrix1 - matrix2; + test_(result.getValue(0,0) == 16); + test_(result.getValue(0,1) == -1); + test_(result.getValue(0,2) == -7); + test_(result.getValue(1,0) == 8); + test_(result.getValue(1,1) == 24); + test_(result.getValue(1,2) == -9); + test_(result.getValue(2,0) == 13); + test_(result.getValue(2,1) == -1); + test_(result.getValue(2,2) == -12); + } + + // Test operator* (multiplication with a constant number) + void testOperatorMultiplicationWithConstant() { + Matrix3x3 result = matrix1 * 2; + test_(result.getValue(0,0) == 6); + test_(result.getValue(0,1) == 14); + test_(result.getValue(0,2) == -10); + test_(result.getValue(1,0) == 26); + test_(result.getValue(1,1) == -2); + test_(result.getValue(1,2) == 4); + test_(result.getValue(2,0) == 12); + test_(result.getValue(2,1) == 10); + test_(result.getValue(2,2) == 18); + } + + // Test operator* (multiplication with matrix) + void testOperatorMultiplicationWithMatrix() { + Matrix3x3 result = matrix1 * matrix2; + test_(result.getValue(0,0) == 31); + test_(result.getValue(0,1) == -181); + test_(result.getValue(0,2) == -22); + test_(result.getValue(1,0) == -188); + test_(result.getValue(1,1) == 141); + test_(result.getValue(1,2) == 57); + test_(result.getValue(2,0) == -116); + test_(result.getValue(2,1) == -23); + test_(result.getValue(2,2) == 256); + } + + void testOperatorMultiplicationWithVector() { + Vector3D vector(4,7,3); + Vector3D result = matrix1 * vector; + test_(result.getX() == 46); + test_(result.getY() == 51); + test_(result.getZ() == 86); + } + + // Test operator=() + void testOperatorAssignment() { + Matrix3x3 matrix; + matrix = matrix1; + test_(matrix.getValue(0,0) == 3); + test_(matrix.getValue(0,1) == 7); + test_(matrix.getValue(0,2) == -5); + test_(matrix.getValue(1,0) == 13); + test_(matrix.getValue(1,1) == -1); + test_(matrix.getValue(1,2) == 2); + test_(matrix.getValue(2,0) == 6); + test_(matrix.getValue(2,1) == 5); + test_(matrix.getValue(2,2) == 9); + } + + // Test operator==() + void testOperatorEquality() { + Matrix3x3 matrix(3, 7, -5, 13, -1, 2, 6, 5, 9); + test_(matrix == matrix1); + test_(matrix1 == matrix); + test_(matrix == matrix); + matrix.setValue(1,1, 100); + test_(!(matrix == matrix1)); + test_(!(matrix1 == matrix)); + } +}; + +#endif diff --git a/src/reactphysics3d/testing/testing_mathematics/MatrixTest.h b/src/reactphysics3d/testing/testing_mathematics/MatrixTest.h new file mode 100644 index 00000000..0b684472 --- /dev/null +++ b/src/reactphysics3d/testing/testing_mathematics/MatrixTest.h @@ -0,0 +1,648 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +#ifndef MATRIXTEST_H +#define MATRIXTEST_H + +// Libraries +#include "../TestSuite/Test.h" +#include "../../mathematics/Matrix.h" +#include + +// Namespaces +using namespace reactphysics3d; + +// Class MatrixTest +class MatrixTest : public TestSuite::Test { + private : + Matrix matrix1; + Matrix matrix2; + Matrix matrix3; + public : + + // Constructor + MatrixTest() : matrix1(2,3), matrix2(2,3), matrix3(3,2) { + matrix1.setValue(0, 0, 4); + matrix1.setValue(0, 1, 5); + matrix1.setValue(0, 2, 7); + matrix1.setValue(1, 0, 2); + matrix1.setValue(1, 1, 3); + matrix1.setValue(1, 2, -4); + + matrix2.setValue(0, 0, -12); + matrix2.setValue(0, 1, 3); + matrix2.setValue(0, 2, 16); + matrix2.setValue(1, 0, -7); + matrix2.setValue(1, 1, 4); + matrix2.setValue(1, 2, 6); + + matrix3.setValue(0, 0, -4); + matrix3.setValue(0, 1, -2); + matrix3.setValue(1, 0, 7); + matrix3.setValue(1, 1, 9); + matrix3.setValue(2, 0, 18); + matrix3.setValue(2, 1, 33); + } + + // Run method of the Test + void run() { + testConstructors(); + testGetValue(); + testSetValue(); + testGetNbRow(); + testGetNbColumn(); + testGetCofactor(); + testGetTranspose(); + testGetInverse(); + testGetDeterminant(); + testGetTrace(); + testIdentityMatrix(); + testOperatorAddition(); + testOperatorSubstraction(); + testOperatorMultiplicationWithConstant(); + testOperatorMultiplicationWithMatrix(); + testOperatorAssignment(); + testOperatorEquality(); + } + + // Test the constructors + void testConstructors() { + + // Try a valid constructor call + try { + // Constructor + Matrix matrix(4,6); // This shouldn't throw an exception + test_(matrix.getNbRow() == 4); + test_(matrix.getNbColumn() == 6); + test_(matrix.getValue(0,0) == 0); + succeed_(); // Succeed if no exceptions have been thrown + } + catch(std::invalid_argument& ex) { + fail_("Valid constructor call throws an exception"); // Failed if an exception has been thrown + } + + // Try an invalid constructor call + try { + // Constructor + Matrix matrix(-2,6); // This should throw an exception + fail_("Invalid constructor call undetected (argument -2) "); // Failed if no exception have been thrown + } + catch(std::invalid_argument& ex) { + succeed_(); // Succeed if an exception has been thrown + } + + // Try an invalid constructor call + try { + // Constructor + Matrix matrix(3,0); // This should throw an exception + fail_("Invalid constructor call undetected (argument 0)"); // Failed if no exception have been thrown + } + catch(std::invalid_argument& ex) { + succeed_(); // Succeed if an exception has been thrown + } + + // Copy-constructor + Matrix matrix4 = matrix1; + test_(matrix4.getNbRow() == 2); + test_(matrix4.getNbColumn() == 3); + test_(matrix4.getValue(0, 0) == 4); + test_(matrix4.getValue(0, 1) == 5); + test_(matrix4.getValue(0, 2) == 7); + test_(matrix4.getValue(1, 0) == 2); + test_(matrix4.getValue(1, 1) == 3); + test_(matrix4.getValue(1, 2) == -4); + } + + // Test getValue() + void testGetValue() { + // Try a valid getValue() + try { + test_(matrix1.getValue(0, 0) == 4); // This shouldn't throw an exception + test_(matrix1.getValue(0, 2) == 7); // This shouldn't throw an exception + test_(matrix1.getValue(1, 0) == 2); // This shouldn't throw an exception + test_(matrix1.getValue(1, 2) == -4); // This shouldn't throw an exception + test_(matrix1.getValue(1, 1) == 3); // This shouldn't throw an exception + succeed_(); // Succeed if no exceptions have been thrown + } + catch(std::invalid_argument& ex) { + fail_("Valid getValue() call throws an exception"); // Failed if an exception has been thrown + } + + // Try an invalid getValue() call + try { + matrix1.getValue(-1, 0); // This should throw an exception + fail_("Invalid getValue() call undetected"); + } + catch(std::invalid_argument& ex) { + succeed_(); // Succeed if an exception has been thrown + } + + // Try an invalid getValue() call + try { + matrix1.getValue(0, 3); // This should throw an exception + fail_("Invalid getValue() call undetected"); + } + catch(std::invalid_argument& ex) { + succeed_(); // Succeed if an exception has been thrown + } + } + + // Test setValue() + void testSetValue() { + + Matrix matrix(2,3); + + // Try a valid setValue() + try { + matrix.setValue(0, 0, 18); // This shouldn't throw an exception + matrix.setValue(0, 2, -6); // This shouldn't throw an exception + matrix.setValue(1, 0, -44); // This shouldn't throw an exception + matrix.setValue(1, 2, 21); // This shouldn't throw an exception + matrix.setValue(1, 1, 5); // This shouldn't throw an exception + test_(matrix.getValue(0, 0) == 18); + test_(matrix.getValue(0, 2) == -6); + test_(matrix.getValue(1, 0) == -44); + test_(matrix.getValue(1, 2) == 21); + test_(matrix.getValue(1, 1) == 5); + succeed_(); // Succeed if no exceptions have been thrown + } + catch(std::invalid_argument& ex) { + fail_("Valid setValue() call throws an exception"); // Failed if an exception has been thrown + } + + // Try an invalid setValue() call + try { + matrix.setValue(-1, 0, 42); // This should throw an exception + fail_("Invalid setValue() call undetected"); // Failed if no exception have been thrown + } + catch(std::invalid_argument& ex) { + succeed_(); // Succeed if an exception has been thrown + } + + // Try an invalid setValue() call + try { + matrix1.setValue(0, 3, 53); // This should throw an exception + fail_("Invalid setValue() call undetected"); // Failed if no exception have been thrown + } + catch(std::invalid_argument& ex) { + succeed_(); // Succeed if an exception has been thrown + } + } + + // Test getNbRow() + void testGetNbRow() { + test_(matrix1.getNbRow() == 2); + test_(matrix3.getNbRow() == 3); + } + + // Test getNbColumn() + void testGetNbColumn() { + test_(matrix1.getNbColumn() == 3); + test_(matrix3.getNbColumn() == 2); + } + + // Test getCofactor() + void testGetCofactor() { + + // Try a valid getCofactor() + try { + Matrix matrix = matrix1.getCofactor(0,1); // This shouldn't throw an exception + + test_(matrix.getNbRow() == 1); + test_(matrix.getNbColumn() == 2); + test_(matrix.getValue(0, 0) == 2); + test_(matrix.getValue(0, 1) == -4); + + Matrix matrix4 = matrix1.getCofactor(1,2); // This shouldn't throw an exception + + test_(matrix4.getNbRow() == 1); + test_(matrix4.getNbColumn() == 2); + test_(matrix4.getValue(0, 0) == 4); + test_(matrix4.getValue(0, 1) == 5); + succeed_(); // Succeed if no exceptions have been thrown + } + catch(std::invalid_argument& ex) { + fail_("Valid getCofactor() call throws an exception"); // Failed if an exception has been thrown + } + + // Try an invalid getCofactor() call + try { + matrix1.getCofactor(-1,0); // This should throw an exception + fail_("Invalid getCofactor() call undetected"); // Failed if no exception have been thrown + } + catch(std::invalid_argument& ex) { + succeed_(); // Succeed if an exception has been thrown + } + + // Try an invalid getCofactor() call + try { + matrix1.getCofactor(0,3); // This should throw an exception + fail_("Invalid getCofactor() call undetected"); // Failed if no exception have been thrown + } + catch(std::invalid_argument& ex) { + succeed_(); // Succeed if an exception has been thrown + } + } + + // Test getTranspose() + void testGetTranspose() { + // Get the transpose of matrix1 + Matrix matrix = matrix1.getTranspose(); + + // Test the transpose matrix + test_(matrix.getNbRow() == 3); + test_(matrix.getNbColumn() == 2); + test_(matrix.getValue(0, 0) == 4); + test_(matrix.getValue(0, 1) == 2); + test_(matrix.getValue(1, 0) == 5); + test_(matrix.getValue(1, 1) == 3); + test_(matrix.getValue(2, 0) == 7); + test_(matrix.getValue(2, 1) == -4); + } + + // Test getInverse() + void testGetInverse() { + + // Construct a 3x3 matrix + Matrix matrix(3,3); + matrix.setValue(0, 0, 0); + matrix.setValue(0, 1, 1); + matrix.setValue(0, 2, 2); + matrix.setValue(1, 0, 1); + matrix.setValue(1, 1, 0); + matrix.setValue(1, 2, 3); + matrix.setValue(2, 0, 4); + matrix.setValue(2, 1, -3); + matrix.setValue(2, 2, 8); + + // Try to inverse a invertible matrix + try { + Matrix result = matrix.getInverse(); // This shouldn't thrown an exception + test_(result.getValue(0, 0) == -4.5); + test_(result.getValue(0, 1) == 7); + test_(result.getValue(0, 2) == -3.0/2.0); + test_(result.getValue(1, 0) == -2); + test_(result.getValue(1, 1) == 4); + test_(result.getValue(1, 2) == -1); + test_(result.getValue(2, 0) == 3.0/2.0); + test_(result.getValue(2, 1) == -2); + test_(result.getValue(2, 2) == 1.0/2.0); + succeed_(); // Succeed if no exceptions have been thrown + } + catch(MathematicsException& ex) { + fail_("Valid getInverse() call throws an exception"); // Failed if an exception has been thrown + } + + // Try to inverse a non-square matrix + try { + matrix1.getInverse(); // This should throw an exception + fail_("Invalid getInverse() call undetected (non-square matrix)"); // Failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // Succeed if an exception has been thrown + } + + // Try to inverse a square non-invertible matrix (determinant equal to zero) + try { + Matrix matrix4(2,2); + matrix4.setValue(0, 0, 3); + matrix4.setValue(0, 1, 2); + matrix4.setValue(1, 0, 3); + matrix4.setValue(1, 1, 2); + matrix4.getInverse(); // This should throw an exception + fail_("Invalid getInverse() call undetected (non-invertible matrix)"); // Failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // Succeed if an exception has been thrown + } + } + + // Test getDeterminant() + void testGetDeterminant() { + + // Try to compute the determinant of a square matrix + try { + Matrix matrix(2,2); + test_(matrix.getDeterminant() == 0); // This shouldn't throw an exception + matrix.setValue(0,0, 4); + matrix.setValue(0,1, -9); + matrix.setValue(1,0, 0); + matrix.setValue(1,1, 5); + test_(matrix.getDeterminant() == 20); // This shouldn't throw an exception + matrix.setValue(0,0, 6); + matrix.setValue(0,1, -9); + matrix.setValue(1,0, -4); + matrix.setValue(1,1, 6); + test_(matrix.getDeterminant() == 0); // This shouldn't throw an exception + succeed_(); // Succeed if no exceptions have been thrown + } + catch(MathematicsException& ex) { + fail_("Valid getDeterminant() call throws an exception"); // Failed if an exception has been thrown + } + + // Try to compute the determinant of a non-square matrix + try { + Matrix matrix5(2,8); + matrix5.setValue(0, 2, 3); + matrix5.setValue(1, 1, 2); + matrix5.getDeterminant(); // This should throw an exception + fail_("getDeterminant() call with a non-square matrix undetected"); // Failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // Succeed if an exception has been thrown + } + } + + // Test getTrace() + void testGetTrace() { + + // Try to compute the trace of a square matrix + try { + // Construct a 3x3 matrix + Matrix matrix(3,3); + matrix.setValue(0, 0, -2); + matrix.setValue(0, 1, 1); + matrix.setValue(0, 2, 2); + matrix.setValue(1, 0, 1); + matrix.setValue(1, 1, 5); + matrix.setValue(1, 2, 3); + matrix.setValue(2, 0, 4); + matrix.setValue(2, 1, -3); + matrix.setValue(2, 2, 8); + test_(matrix.getTrace() == 11); // This shouldn't throw an exception + succeed_(); // Succeed if no exceptions have been thrown + } + catch(MathematicsException& ex) { + fail_("Valid getTrace() call throws an exception"); // Failed if an exception has been thrown + } + + // Try to compute the trace of a non-square matrix + try { + Matrix matrix5(2,8); + matrix5.getTrace(); // This should throw an exception + fail_("getTrace() call with a non-square matrix undetected"); // Failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // Succeed if an exception has been thrown + } + } + + // Test identityMatrix() + void testIdentityMatrix() { + + + // Try to compute a valid identity matrix + try { + Matrix matrix = Matrix::identity(2); // This shouldn't throw an exception + test_(matrix.getNbRow() == 2); + test_(matrix.getNbColumn() == 2); + test_(matrix.getValue(0, 0) == 1); + test_(matrix.getValue(0, 1) == 0); + test_(matrix.getValue(1, 0) == 0); + test_(matrix.getValue(1, 1) == 1); + succeed_(); // Succeed if no exceptions have been thrown + } + catch(std::invalid_argument& ex) { + fail_("Valid identity() call throws an exception"); // Failed if an exception has been thrown + } + + // Try to compute an invalid identity matrix + try { + Matrix matrix5 = Matrix::identity(0); // This should throw an exception + fail_("Invalid identity() call (argument 0) undetected"); // Failed if no exception have been thrown + } + catch(std::invalid_argument& ex) { + succeed_(); // Succeed if an exception has been thrown + } + + // Try to compute an invalid identity matrix + try { + Matrix matrix5 = Matrix::identity(-1); // This should throw an exception + fail_("Invalid identity() call (argument -1) undetected"); // Failed if no exception have been thrown + } + catch(std::invalid_argument& ex) { + succeed_(); // Succeed if an exception has been thrown + } + } + + // Test operator+() + void testOperatorAddition() { + + // Try to compute a valid addition + try { + Matrix result = matrix1 + matrix2; // This shouldn't throw an exception + test_(result.getNbRow() == 2); + test_(result.getNbColumn() == 3); + test_(result.getValue(0,0) == -8); + test_(result.getValue(0,1) == 8); + test_(result.getValue(0,2) == 23); + test_(result.getValue(1,0) == -5); + test_(result.getValue(1,1) == 7); + test_(result.getValue(1,2) == 2); + succeed_(); // Succeed if no exceptions have been thrown + } + catch(MathematicsException& ex) { + fail_("Valid matrix addition throws an exception"); // Failed if an exception has been thrown + } + + // Try to compute an invalid addition + try { + Matrix matrix5(2,4); + matrix1 + matrix5; // This should throw an exception + fail_("Invalid matrix addition undetected"); // Failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // Succeed if an exception has been thrown + } + + // Try to compute an invalid addition + try { + Matrix matrix5(1,3); + matrix1 + matrix5; // This should throw an exception + fail_("Invalid matrix addition undetected"); // Failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // Succeed if an exception has been thrown + } + } + + // Test operator-() + void testOperatorSubstraction() { + + // Try to compute a valid substraction + try { + Matrix result = matrix1 - matrix2; // This shouldn't throw an exception + test_(result.getNbRow() == 2); + test_(result.getNbColumn() == 3); + test_(result.getValue(0,0) == 16); + test_(result.getValue(0,1) == 2); + test_(result.getValue(0,2) == -9); + test_(result.getValue(1,0) == 9); + test_(result.getValue(1,1) == -1); + test_(result.getValue(1,2) == -10); + succeed_(); // Succeed if no exceptions have been thrown + } + catch(MathematicsException& ex) { + fail_("Valid matrix substraction throws an exception"); // Failed if an exception has been thrown + } + + // Try to compute an invalid substraction + try { + Matrix matrix5(2,4); + matrix1 - matrix5; // This should throw an exception + fail_("Invalid matrix substraction undetected"); // Failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // Succeed if an exception has been thrown + } + + // Try to compute an invalid substraction + try { + Matrix matrix5(1,3); + matrix1 - matrix5; // This should throw an exception + fail_("Invalid matrix substraction undetected"); // Failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // Succeed if an exception has been thrown + } + } + + // Test operator* (multiplication with a constant number) + void testOperatorMultiplicationWithConstant() { + Matrix matrix = matrix1 * 2; + test_(matrix.getNbRow() == 2); + test_(matrix.getNbColumn() == 3); + test_(matrix.getValue(0,0) == 8); + test_(matrix.getValue(0,1) == 10); + test_(matrix.getValue(0,2) == 14); + test_(matrix.getValue(1,0) == 4); + test_(matrix.getValue(1,1) == 6); + test_(matrix.getValue(1,2) == -8); + } + + // Test operator* (multiplication with matrix) + void testOperatorMultiplicationWithMatrix() { + + // Try to compute a valid multiplication + try { + Matrix result = matrix1 * matrix3; // This shouldn't throw an exception + test_(result.getNbRow() == 2); + test_(result.getNbColumn() == 2); + test_(result.getValue(0,0) == 145); + test_(result.getValue(0,1) == 268); + test_(result.getValue(1,0) == -59); + test_(result.getValue(1,1) == -109); + succeed_(); // Succeed if no exceptions have been thrown + } + catch(MathematicsException& ex) { + fail_("Valid matrix multiplication throws an exception"); // Failed if an exception has been thrown + } + + // Try to compute an invalid multiplication + try { + Matrix matrix5(1,3); + matrix1 * matrix5; // This should throw an exception + fail_("Invalid matrix substraction undetected"); // Failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // Succeed if an exception has been thrown + } + + // Try to compute an invalid multiplication + try { + Matrix matrix5(2,2); + matrix1 * matrix5; // This should throw an exception + fail_("Invalid matrix substraction undetected"); // Failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // Succeed if an exception has been thrown + } + } + + // Test operator=() + void testOperatorAssignment() { + + // Try to compute a valid assignment + try { + Matrix matrix(2,3); + matrix = matrix1; // This shouldn't throw an exception + test_(matrix.getValue(0, 0) == 4); + test_(matrix.getValue(0, 2) == 7); + test_(matrix.getValue(1, 0) == 2); + test_(matrix.getValue(1, 2) == -4); + test_(matrix.getValue(1, 1) == 3); + succeed_(); // Succeed if no exceptions have been thrown + } + catch(MathematicsException& ex) { + fail_("Valid matrix assignment throws an exception"); // Failed if an exception has been thrown + } + + // Try to compute an invalid assignment + try { + Matrix matrix(2,2); + matrix = matrix1; // This should throw an exception + fail_("Invalid matrix assignment undetected"); // Failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // Succeed if an exception has been thrown + } + } + + // Test operator==() + void testOperatorEquality() { + + + // Try to test a valid equality + try { + Matrix matrix(2,3); + matrix.setValue(0, 0, 4); + matrix.setValue(0, 1, 5); + matrix.setValue(0, 2, 7); + matrix.setValue(1, 0, 2); + matrix.setValue(1, 1, 3); + matrix.setValue(1, 2, -4); + test_(matrix == matrix1); // This shouldn't throw an exception + test_(matrix1 == matrix); // This shouldn't throw an exception + test_(matrix == matrix); // This shouldn't throw an exception + matrix.setValue(1,1, 5); + test_(!(matrix == matrix1)); // This shouldn't throw an exception + test_(!(matrix1 == matrix)); // This shouldn't throw an exception + succeed_(); // Succeed if no exceptions have been thrown + } + catch(MathematicsException& ex) { + fail_("Valid matrix equality test throws an exception"); // Failed if an exception has been thrown + } + + // Try to test a invalid equality + try { + Matrix matrix(2,2); + matrix == matrix1; // This should throw an exception + fail_("Invalid matrix assignment undetected"); // Failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // Succeed if an exception has been thrown + } + } + + + +}; + +#endif + diff --git a/src/reactphysics3d/testing/testing_mathematics/QuaternionTest.h b/src/reactphysics3d/testing/testing_mathematics/QuaternionTest.h new file mode 100644 index 00000000..113278fc --- /dev/null +++ b/src/reactphysics3d/testing/testing_mathematics/QuaternionTest.h @@ -0,0 +1,350 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +#ifndef QUATERNIONTEST_H +#define QUATERNIONTEST_H + +// Libraries +#include "../TestSuite/Test.h" +#include "../../mathematics/Quaternion.h" +#include "../../mathematics/constants.h" +#include +#include + +// Namespaces +using namespace reactphysics3d; + +// Class MatrixTest +class QuaternionTest : public TestSuite::Test { + private : + Quaternion quaternion1; + Quaternion quaternion2; + public : + + // Constructor + QuaternionTest() : quaternion1(2,3,4,5), quaternion2(6,7,8,9) { + + } + + // Run method of the Test + void run() { + testConstructors(); + testVectorV(); + testLength(); + testGetUnit(); + testGetConjugate(); + testGetInverse(); + testScalarProduct(); + testgetRotationAngleAxis(); + testSlerp(); + testOperatorAddition(); + testOperatorSubstraction(); + testOperatorMultiplicationWithConstant(); + testOperatorMultiplicationWithQuaternion(); + testOperatorAssignment(); + testOperatorEquality(); + } + + // Test the constructors + void testConstructors() { + // Constructor without argument + Quaternion quaternion; + test_(quaternion.getX() == 0); + test_(quaternion.getY() == 0); + test_(quaternion.getZ() == 0); + test_(quaternion.getW() == 0); + + // Constructor with argument + Quaternion quaternion3(1,2,3,4); + test_(quaternion3.getX() == 1); + test_(quaternion3.getY() == 2); + test_(quaternion3.getZ() == 3); + test_(quaternion3.getW() == 4); + + // Constructor with vector + Vector3D vector(2,3,4); + Quaternion quaternion4(5, vector); + test_(quaternion4.getX() == 2); + test_(quaternion4.getY() == 3); + test_(quaternion4.getZ() == 4); + test_(quaternion4.getW() == 5); + + // Copy-constructor + Quaternion quaternion5 = quaternion3; + test_(quaternion5.getX() == 1); + test_(quaternion5.getY() == 2); + test_(quaternion5.getZ() == 3); + test_(quaternion5.getW() == 4); + } + + // Test getX() + void testGetX() { + test_(quaternion1.getX() == 2); + test_(quaternion2.getX() == 6); + } + + // Test getY() + void testGetY() { + test_(quaternion1.getY() == 3); + test_(quaternion2.getY() == 7); + } + + // Test getZ() + void testGetZ() { + test_(quaternion1.getZ() == 4); + test_(quaternion2.getZ() == 8); + } + + // Test getW() + void testGetW() { + test_(quaternion1.getW() == 5); + test_(quaternion2.getW() == 9); + } + + // Test setX() + void testSetX() { + Quaternion quaternion; + quaternion.setX(3); + test_(quaternion.getX() == 3); + test_(quaternion.getY() == 0); + test_(quaternion.getZ() == 0); + test_(quaternion.getW() == 0); + } + + // Test setY() + void testSetY() { + Quaternion quaternion; + quaternion.setY(3); + test_(quaternion.getX() == 0); + test_(quaternion.getY() == 3); + test_(quaternion.getZ() == 0); + test_(quaternion.getW() == 0); + } + + // Test setZ() + void testSetZ() { + Quaternion quaternion; + quaternion.setZ(3); + test_(quaternion.getX() == 0); + test_(quaternion.getY() == 0); + test_(quaternion.getZ() == 3); + test_(quaternion.getW() == 0); + } + + // Test setW() + void testSetW() { + Quaternion quaternion; + quaternion.setW(3); + test_(quaternion.getX() == 0); + test_(quaternion.getY() == 0); + test_(quaternion.getZ() == 0); + test_(quaternion.getW() == 3); + } + + // Test vectorV() + void testVectorV() { + Vector3D vector1(2,3,4); + Vector3D vector2(6,7,8); + + Vector3D vectorTest1 = quaternion1.vectorV(); + Vector3D vectorTest2 = quaternion2.vectorV(); + test_(vectorTest1 == vector1); + test_(vectorTest2 == vector2); + } + + // Test length() + void testLength() { + Quaternion quaternion; + test_(quaternion.length() == 0); + + Quaternion quaternion3(3, 4, 0, 0); + test_(quaternion3.length() == 5); + + Quaternion quaternion4(0, 4, 3, 0); + test_(quaternion4.length() == 5); + + Quaternion quaternion5(0, 0, 3, 4); + test_(quaternion5.length() == 5); + } + + // Test getUnit() + void testGetUnit() { + // Try to compute a valid unit quaternion + try { + Quaternion quaternion(3, 4, 0, 0); + Quaternion unit = quaternion.getUnit(); // This shouldn't throw an exception + test_(unit.getX() == 3.0/5.0); + test_(unit.getY() == 4.0/5.0); + test_(unit.getZ() == 0); + test_(unit.getW() == 0); + + Quaternion quaternion3(0, 0, 4, 3); + Quaternion unit2 = quaternion3.getUnit(); // This shouldn't throw an exception + test_(unit2.getX() == 0); + test_(unit2.getY() == 0); + test_(unit2.getZ() == 4.0/5.0); + test_(unit2.getW() == 3.0/5.0); + succeed_(); // Succeed if no exception have been thrown + } + catch(MathematicsException& ex) { + fail_("Valid getUnit() call throw an exception"); // Failed if an exception has been thrown + } + + // Try to compute an invalid unit quaternion + try { + Quaternion quaternion(0, 0, 0, 0); + quaternion.getUnit(); // This should throw an exception + fail_("Invalid getUnit() call undetected"); // Failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // Succeed if an exception has been thrown + } + } + + // Test getConjugate() + void testGetConjugate() { + Quaternion conjugate1 = quaternion1.getConjugate(); + test_(conjugate1.getX() == -2); + test_(conjugate1.getY() == -3); + test_(conjugate1.getZ() == -4); + test_(conjugate1.getW() == 5); + + Quaternion conjugate2 = quaternion2.getConjugate(); + test_(conjugate2.getX() == -6); + test_(conjugate2.getY() == -7); + test_(conjugate2.getZ() == -8); + test_(conjugate2.getW() == 9); + } + + // Test getInverse() + void testGetInverse() { + // Try to compute a valid inverse quaternion + try { + Quaternion quaternion(3, 4, 0, 0); + Quaternion inverse = quaternion.getInverse(); // This shouldn't throw an exception + test_(inverse.getX() == -3.0/25.0); + test_(inverse.getY() == -4.0/25.0); + test_(inverse.getZ() == 0); + test_(inverse.getW() == 0); + + Quaternion quaternion3(0, 0, 4, 3); + Quaternion inverse2 = quaternion3.getInverse(); // This shouldn't throw an exception + test_(inverse2.getX() == 0); + test_(inverse2.getY() == 0); + test_(inverse2.getZ() == -4.0/25.0); + test_(inverse2.getW() == 3.0/25.0); + succeed_(); // Succeed if no exception have been thrown + } + catch(MathematicsException& ex) { + fail_("Valid getInverse() call throw an exception"); // Failed if an exception has been thrown + } + + // Try to compute an invalid unit quaternion + try { + Quaternion quaternion(0, 0, 0, 0); + quaternion.getInverse(); // This should throw an exception + fail_("Invalid getInverse() call undetected"); // Failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // Succeed if an exception has been thrown + } + } + + // Test the scalarProduct() method + void testScalarProduct() { + double result = quaternion1.scalarProduct(quaternion2); + test_(result == 110.0); + } + + // Test the getRotationAngleAxis() method + void testgetRotationAngleAxis() { + Quaternion quaternion(1.0, 2.0, 3.0, 0.0); + double invAxisLength = 1.0/sqrt(1.0 + 2.0*2.0 + 3.0*3.0); + double angle; + Vector3D axis; + + quaternion.getRotationAngleAxis(angle, axis); + test_(equal(angle, PI)); + test_(equal(axis.getX(), 1.0*invAxisLength)); + test_(equal(axis.getY(), 2.0*invAxisLength)); + test_(equal(axis.getZ(), 3.0*invAxisLength)); + } + + // Test the slerp() method + void testSlerp() { + // TODO : Test the Quaternion::slerp() method + } + + // Test operator+() + void testOperatorAddition() { + Quaternion result = quaternion1 + quaternion2; + test_(result.getX() == 8); + test_(result.getY() == 10); + test_(result.getZ() == 12); + test_(result.getW() == 14); + } + + // Test operator-() + void testOperatorSubstraction() { + Quaternion result = quaternion1 - quaternion2; + test_(result.getX() == -4); + test_(result.getY() == -4); + test_(result.getZ() == -4); + test_(result.getW() == -4); + } + + // Test operator* (multiplication with a constant number) + void testOperatorMultiplicationWithConstant() { + Quaternion result = quaternion1 * 3; + test_(result.getX() == 6); + test_(result.getY() == 9); + test_(result.getZ() == 12); + test_(result.getW() == 15); + } + + // Test operator* (multiplication with quaternion) + void testOperatorMultiplicationWithQuaternion() { + Quaternion result = quaternion1 * quaternion2; + test_(result.getX() == 44); + test_(result.getY() == 70); + test_(result.getZ() == 72); + test_(result.getW() == -20); + } + + // Test operator=() + void testOperatorAssignment() { + Quaternion quaternion; + quaternion = quaternion1; + test_(quaternion.getX() == 2); + test_(quaternion.getY() == 3); + test_(quaternion.getZ() == 4); + test_(quaternion.getW() == 5); + } + + // Test operator==() + void testOperatorEquality() { + Quaternion quaternion(2,3,4,5); + test_(quaternion == quaternion1); + test_(quaternion1 == quaternion); + test_(!(quaternion2 == quaternion1)); + test_(!(quaternion1 == quaternion2)); + } +}; + +#endif diff --git a/src/reactphysics3d/testing/testing_mathematics/Vector3DTest.h b/src/reactphysics3d/testing/testing_mathematics/Vector3DTest.h new file mode 100644 index 00000000..2100c600 --- /dev/null +++ b/src/reactphysics3d/testing/testing_mathematics/Vector3DTest.h @@ -0,0 +1,272 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +#ifndef VECTOR3DTEST_H +#define VECTOR3DTEST_H + +// Libraries +#include "../TestSuite/Test.h" +#include "../../mathematics/Vector3D.h" +#include + +// Namespaces +using namespace reactphysics3d; + +// Class Vector3DTest +class Vector3DTest : public TestSuite::Test { + private : + Vector3D vector1; + Vector3D vector2; + public : + + // Constructor + Vector3DTest() : vector1(1,2,3), vector2(-3,5,7) { + + } + + // Run method of the Test + void run() { + testConstructors(); + testGetX(); + testGetY(); + testGetZ(); + testSetX(); + testSetY(); + testSetZ(); + testSetAllValues(); + testLength(); + testGetUnit(); + testScalarProduct(); + testCrossProduct(); + testOperatorAddition(); + testOperatorSubstraction(); + testOperatorConstantMultiplications(); + testOperatorAssignment(); + } + + // Test the constructors + void testConstructors() { + + // Constructor without arguments + Vector3D vector; + test_(vector.getX() == 0); + test_(vector.getY() == 0); + test_(vector.getZ() == 0); + + // Constructor with arguments + Vector3D vector3(4, -67, 21); + test_(vector3.getX() == 4); + test_(vector3.getY() == -67); + test_(vector3.getZ() == 21); + + // Copy-constructor + Vector3D vector4 = vector1; + test_(vector4.getX() == 1); + test_(vector4.getY() == 2); + test_(vector4.getZ() == 3); + } + + // Test getX() + void testGetX() { + test_(vector1.getX() == 1); + } + + // Test getY() + void testGetY() { + test_(vector1.getY() == 2); + } + + // Test getZ() + void testGetZ() { + test_(vector1.getZ() == 3); + } + + // Test setX() + void testSetX() { + Vector3D vector(5, 6, 7); + vector.setX(8); + test_(vector.getX() == 8); + test_(vector.getY() == 6); + test_(vector.getZ() == 7); + } + + // Test setY() + void testSetY() { + Vector3D vector(5, 6, 7); + vector.setY(8); + test_(vector.getX() == 5); + test_(vector.getY() == 8); + test_(vector.getZ() == 7); + } + + // Test setZ() + void testSetZ() { + Vector3D vector(5, 6, 7); + vector.setZ(8); + test_(vector.getX() == 5); + test_(vector.getY() == 6); + test_(vector.getZ() == 8); + } + + + // Test setAllValues() + void testSetAllValues() { + Vector3D vector(5, 6, 7); + vector1.setAllValues(4,3,9); + test_(vector1.getX() == 4); + test_(vector1.getY() == 3); + test_(vector1.getZ() == 9); + } + + // Test length() + void testLength() { + Vector3D vector3; + test_(vector3.length() == 0); + vector3.setAllValues(3, 4, 0); + test_(vector3.length() == 5); + vector3.setAllValues(0, -3, 4); + test_(vector3.length() == 5); + } + + // Test getUnit() + void testGetUnit() { + + Vector3D vector3(-23, 0, 0); + test_(vector3.getUnit().length() == 1); + test_(vector3.getUnit().getX() == -1); + test_(vector3.getUnit().getY() == 0); + test_(vector3.getUnit().getZ() == 0); + + vector3.setAllValues(0, 6, 0); + test_(vector3.getUnit().length() == 1); + test_(vector3.getUnit().getX() == 0); + test_(vector3.getUnit().getY() == 1); + test_(vector3.getUnit().getZ() == 0); + + vector3.setAllValues(0, 0, 13); + test_(vector3.getUnit().length() == 1); + test_(vector3.getUnit().getX() == 0); + test_(vector3.getUnit().getY() == 0); + test_(vector3.getUnit().getZ() == 1); + + vector3.setAllValues(0, 0, 0); // Vector of length equal to zero + try { + vector3.getUnit(); // This should throw an exception + fail_("getUnit() with a vector of length equals to zero undetected"); // The test failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // The test succeed if an exception has been thrown + } + } + + // Test scalarProduct() + void testScalarProduct() { + Vector3D vector3(2, -3, 5); + Vector3D vector4(7, 4, 6); + + // Test the scalar product result + test_(vector3.scalarProduct(vector4) == 32); + } + + // Test crossProduct() + void testCrossProduct() { + Vector3D vector3(4, -5, 2); + Vector3D vector4(3, 2, 6); + + // Compute the cross product + Vector3D result = vector3.crossProduct(vector4); + + // Test the result + test_(result.getX() == -34); + test_(result.getY() == -18); + test_(result.getZ() == 23); + } + + // Test operator+() + void testOperatorAddition() { + Vector3D vector3(4, -5, 2); + Vector3D vector4(3, 2, 6); + + // Compute the sum + Vector3D result = vector3 + vector4; + + // Test the result + test_(result.getX() == 7); + test_(result.getY() == -3); + test_(result.getZ() == 8); + } + + // Test operator-() + void testOperatorSubstraction() { + Vector3D vector3(4, -5, 2); + Vector3D vector4(3, 2, 6); + + // Compute the substraction + Vector3D result = vector3 - vector4; + + // Test the result + test_(result.getX() == 1); + test_(result.getY() == -7); + test_(result.getZ() == -4); + } + + // Test operator*() (with a constant number) + void testOperatorConstantMultiplications() { + Vector3D vector3(4, -5, 2); + + // Compute the multiplication + Vector3D result = vector3 * 5; + + // Test the result + test_(result.getX() == 20); + test_(result.getY() == -25); + test_(result.getZ() == 10); + } + + // Test operator=() + void testOperatorAssignment() { + Vector3D vector3(4, -5, 2); + + // Assignment + Vector3D result; + result = vector3; + + // Test the result + test_(result.getX() == 4); + test_(result.getY() == -5); + test_(result.getZ() == 2); + } + + // Test operator==() + void testOperatorEquality() { + Vector3D vector3(4, -5, 2); + Vector3D vector4(4, -5, 2); + Vector3D vector5(3, -5, -2); + + // Test the equality + test_(vector3 == vector4); + test_(vector4 == vector3); + test_(vector3 == vector3); + test_(!(vector3 == vector5)); + test_(!(vector5 == vector3)); + } +}; + +#endif + diff --git a/src/reactphysics3d/testing/testing_mathematics/VectorTest.h b/src/reactphysics3d/testing/testing_mathematics/VectorTest.h new file mode 100644 index 00000000..ca8aa1ab --- /dev/null +++ b/src/reactphysics3d/testing/testing_mathematics/VectorTest.h @@ -0,0 +1,450 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +#ifndef VECTORTEST_H +#define VECTORTEST_H + +// Libraries +#include "../TestSuite/Test.h" +#include "../../mathematics/Vector.h" +#include + +// Namespaces +using namespace reactphysics3d; + +// Class VectorTest +class VectorTest : public TestSuite::Test { + private : + Vector vect2; // Vector of dimension 2 + Vector vect4; // Vector of dimension 4 + public : + + // Constructor + VectorTest() : vect2(2), vect4(4) { + vect2.setValue(0, 3); + vect2.setValue(1, 5); + + vect4.setValue(0, 1); + vect4.setValue(1, -5); + vect4.setValue(2, 10); + vect4.setValue(3, 3); + } + + // Run method of the Test + void run() { + testConstructors(); + testGetValue(); + testSetValue(); + testGetNbComponent(); + testLength(); + testGetUnit(); + testScalarProduct(); + testCrossProduct(); + testOperatorAddition(); + testOperatorSubstraction(); + testOperatorConstantMultiplications(); + testOperatorAssignment(); + } + + // Test the constructors + void testConstructors() { + + // Try a valid constructor call + try { + // Constructor + Vector vector(3); // This shouldn't throw an exception + test_(vector.getValue(0) == 0); + test_(vector.getValue(1) == 0); + test_(vector.getValue(2) == 0); + succeed_(); // Test succeed if no exception have been thrown + } + catch(std::invalid_argument& ex) { + fail_("Valid constructor call throws an exception"); // Test failed if an exception has been thrown + } + + // Try a invalid constructor call + try { + // Constructor + Vector vector3(-1); // This should throw an exception + fail_("Invalid constructor (argument -1) call undetected"); // Test failed if no exception have been thrown + } + catch(std::invalid_argument& ex) { + succeed_(); // Test succeed if an exception has been thrown + } + + // Try a invalid constructor call + try { + // Constructor + Vector vector4(0); // This should throw an exception + fail_("Invalid constructor call (argument 0) undetected"); // Test failed if no exception have been thrown + } + catch(std::invalid_argument& ex) { + succeed_(); // Test succeed if an exception has been thrown + } + + // Copy-Constructor + Vector vector5 = vect2; + test_(vector5.getValue(0) == 3); + test_(vector5.getValue(1) == 5); + } + + // Test getValue() + void testGetValue() { + test_(vect2.getValue(0) == 3); + test_(vect2.getValue(1) == 5); + + // Try to get an invalid value + try { + vect2.getValue(-1); // This should throw an exception + fail_("Invalid getValue(-1) undetected"); // The test failed if no exception have been thrown + } + catch(std::invalid_argument& ex) { + succeed_(); // The test succeed if an exception has been thrown + } + + // Try to get an invalid value + try { + vect2.getValue(2); // This should throw an exception + fail_("Invalid getValue(2) undetected"); // The test failed if no exception have been thrown + } + catch(std::invalid_argument& ex) { + succeed_(); // The test succeed if an exception has been thrown + } + } + + // Test setValue() + void testSetValue() { + Vector vector(5); + + // Try to set valid values + try { + vector.setValue(0, 5); + vector.setValue(3, -1); + vector.setValue(4, 14); + test_(vector.getValue(0) == 5); + test_(vector.getValue(3) == -1); + test_(vector.getValue(4) == 14); + succeed_(); // The test succeed if an exception have been thrown + } + catch(std::invalid_argument& ex) { + fail_("Valid setValue() throws an exception"); // The failed if an exception has been thrown + } + + // Try to set an invalid value + try { + vector.setValue(-1, 4); // This should throw an exception + fail_("Invalid setValue(-1,4) undetected"); // The test failed if no exception have been thrown + } + catch(std::invalid_argument& ex) { + succeed_(); // The test succeed if an exception has been thrown + } + + // Try to set an invalid value + try { + vector.setValue(5, 2); // This should throw an exception + fail_("Invalid setValue(5,2) undetected"); // The test failed if no exception have been thrown + } + catch(std::invalid_argument& ex) { + succeed_(); // The test succeed if an exception has been thrown + } + } + + // Test getNbComponent() + void testGetNbComponent() { + test_(vect2.getNbComponent() == 2); + test_(vect4.getNbComponent() == 4); + } + + // Test length() + void testLength() { + Vector vector1(2); + test_(vector1.length() == 0); + vector1.setValue(0, 4); + test_(vector1.length() == 4); + vector1.setValue(1, -3); + test_(vector1.length() == 5); + } + + // Test getUnit() + void testGetUnit() { + Vector vector1(3); + vector1.setValue(0, 3); + test_(vector1.getUnit().length() == 1); + test_(vector1.getUnit().getValue(0) == 1); + test_(vector1.getUnit().getValue(1) == 0); + test_(vector1.getUnit().getValue(2) == 0); + + Vector vector2(8); + vector2.setValue(2, 54); + test_(vector2.getUnit().length() == 1); + test_(vector2.getUnit().getValue(0) == 0); + test_(vector2.getUnit().getValue(1) == 0); + test_(vector2.getUnit().getValue(2) == 1); + + Vector vector3(7); // Vector of length equal to zero + try { + vector3.getUnit(); // This should throw an exception + fail_("getUnit() with a vector of length equals to zero undetected"); // The test failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // The test succeed if an exception has been thrown + } + } + + // Test scalarProduct() + void testScalarProduct() { + Vector vector1(2); + vector1.setValue(0, 4); + vector1.setValue(1, -5); + + Vector vector2(2); + vector2.setValue(0, 3); + vector2.setValue(1, 2); + + // Try to compute a valid scalar product + try { + test_(vector1.scalarProduct(vector2) == 2); + succeed_(); // The test succeed if no exception have been thrown + } + catch(MathematicsException& ex) { + fail_("scalarProduct() thrown an exception during a valid scalar product computation"); // The test failed if an exception has been thrown + } + + // Try to compute a invalid scalar product + Vector vector3(5); + try { + vector1.scalarProduct(vector3); // This should throw an exception + fail_("Invalid dimensions in scalarProduct() undetected"); // The test failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // The test succeed if an exception has been thrown + } + } + + // Test crossProduct() + void testCrossProduct() { + Vector vector1(3); + vector1.setValue(0, 4); + vector1.setValue(1, -5); + vector1.setValue(2, 2); + + Vector vector2(3); + vector2.setValue(0, 3); + vector2.setValue(1, 2); + vector2.setValue(2, 6); + + // Try to compute a valid scalar product + try { + Vector result = vector1.crossProduct(vector2); + test_(result.getValue(0) == -34); + test_(result.getValue(1) == -18); + test_(result.getValue(2) == 23); + succeed_(); // The test succeed if no exception have been thrown + } + catch(MathematicsException& ex) { + fail_("crossProduct() thrown an exception during a valid cross product computation"); // The test failed if an exception has been thrown + } + + // Try to compute a invalid cross product + Vector vector3(5); + try { + vector1.crossProduct(vector3); // This should throw an exception + fail_("Invalid dimensions in crossProduct() undetected"); // The test failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // The test succeed if an exception has been thrown + } + } + + // Test operator+() + void testOperatorAddition() { + Vector vector1(4); + vector1.setValue(0, 3); + vector1.setValue(3, -2); + + Vector vector2(4); + vector2.setValue(0, 9); + vector2.setValue(2, 6); + vector2.setValue(3, 9); + + // Try to compute a valid sum (two vectors with the same dimensions) + try { + // Compute the sum + Vector sum = vector1 + vector2; // This shouldn't throw an exception + + test_(sum.getValue(0) == 12); + test_(sum.getValue(1) == 0); + test_(sum.getValue(2) == 6); + test_(sum.getValue(3) == 7); + succeed_(); // The test succeed if no exception have been thrown + } + catch(MathematicsException& ex) { + fail_("A valid sum of two vectors throws an excception"); // The test failed if an exception has been thrown + } + + // Try to compute an invalid sum (vectors with different dimensions) + Vector vector3(3); + Vector vector4(5); + try { + // Compute the sum + Vector sum = vector3 + vector4; // This should throw an exception + fail_("An invalid sum of two vectors undetected"); // The test failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // The test succeed if an exception has been thrown + } + } + + // Test operator-() + void testOperatorSubstraction() { + Vector vector1(4); + vector1.setValue(0, 3); + vector1.setValue(3, -2); + + Vector vector2(4); + vector2.setValue(0, 9); + vector2.setValue(2, 6); + vector2.setValue(3, 9); + + // Try to compute a valid subtraction (two vectors with the same dimensions) + try { + // Compute the substraction + Vector sub = vector1 - vector2; // This shouldn't throw an exception + + test_(sub.getValue(0) == -6); + test_(sub.getValue(1) == 0); + test_(sub.getValue(2) == -6); + test_(sub.getValue(3) == -11); + succeed_(); // The test succeed if no exception have been thrown + } + catch(MathematicsException& ex) { + fail_("A valid subtraction of two vectors throws an excception"); // The test failed if an exception has been thrown + } + + // Try to compute an invalid substraction (vectors with different dimensions) + Vector vector3(3); + Vector vector4(5); + try { + // Compute the substraction + Vector sub = vector3 - vector4; // This should throw an exception + fail_("An invalid substraction of two vectors undetected"); // The test failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // The test succeed if an exception has been thrown + } + } + + // Test operator*() (with a constant number) + void testOperatorConstantMultiplications() { + Vector vector(4); + vector.setValue(0, 3); + vector.setValue(3, -2); + + // Compute the multiplication + Vector sum = vector * 3.0; + test_(sum.getValue(0) == 9); + test_(sum.getValue(1) == 0); + test_(sum.getValue(2) == 0); + test_(sum.getValue(3) == -6); + } + + // Test operator=() + void testOperatorAssignment() { + Vector vector1(2); + vector1.setValue(0, 4); + vector1.setValue(1, 7); + + Vector vector2(8); + + + // Try to compute a valid assignment (two vectors with the same dimensions) + try { + Vector vector(2); + vector = vector1; // This shouldn't throw an exception + test_(vector == vector1); + + vector = vector; // This shouldn't throw an exception + succeed_(); // The test succeed if no exception have been thrown + } + catch(MathematicsException& ex) { + fail_("A valid vector assignment throws an excception"); // The test failed if an exception has been thrown + } + + // Try to compute an invalid assignment (vectors with different dimensions) + try { + Vector vector3(2); + vector3 = vector2; // This should throw an exception + fail_("An invalid vector assignment undetected"); // The test failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // The test succeed if an exception has been thrown + } + + // Try to compute an invalid assignment (vectors with different dimensions) + try { + Vector vector3(2); + vector2 = vector3; // This should throw an exception + fail_("An invalid vector assignment undetected"); // The test failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // The test succeed if an exception has been thrown + } + } + + // Test operator==() + void testOperatorEquality() { + Vector vector1(2); + vector1.setValue(0, 4); + vector1.setValue(1, 7); + + Vector vector2(2); + vector2.setValue(0, 4); + vector2.setValue(1, 7); + + Vector vector3(2); + vector3.setValue(0, 5); + vector3.setValue(1, 7); + + Vector vector4(8); + + // Try to test a valid equality (two vectors with the same dimensions) + try { + test_(vector1 == vector2); // This shouldn't throw an exception + test_(vector2 == vector1); // This shouldn't throw an exception + test_(vector1 == vector1); // This shouldn't throw an exception + test_(!(vector1 == vector3)); // This shouldn't throw an exception + test_(!(vector3 == vector1)); // This shouldn't throw an exception + succeed_(); // The test succeed if no exception have been thrown + } + catch(MathematicsException& ex) { + fail_("A valid vector equality test throws an excception"); // The test failed if an exception has been thrown + } + + // Try to test an invalid equality (vectors with different dimensions) + try { + vector4 == vector1; // This should throw an exception + fail_("An invalid equality test of two vectors undetected"); // The test failed if no exception have been thrown + } + catch(MathematicsException& ex) { + succeed_(); // The test succeed if an exception has been thrown + } + } +}; + +#endif diff --git a/src/reactphysics3d/testing/testing_physics/KilogramTest.h b/src/reactphysics3d/testing/testing_physics/KilogramTest.h new file mode 100755 index 00000000..003e351f --- /dev/null +++ b/src/reactphysics3d/testing/testing_physics/KilogramTest.h @@ -0,0 +1,130 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +#ifndef KILOGRAMTEST_H +#define KILOGRAMTEST_H + +// Libraries +#include "../TestSuite/Test.h" +#include "../../physics/Kilogram.h" +#include + +// Namespaces +using namespace reactphysics3d; + +// Class KilogramTest +class KilogramTest : public TestSuite::Test { + private : + Kilogram mass1; + Kilogram mass2; + public : + + // Constructor + KilogramTest() : mass1(5.0), mass2(10.0) { + + } + + // Run method of the Test + void run() { + testConstructors(); + testGetValue(); + testSetValue(); + } + + // Test the constructors + void testConstructors() { + // Try valid constructors calls + try { + Kilogram mass(30.4); // This should'n throw an exception + Kilogram mass3(mass); // This should'n throw an exception + Kilogram mass4(0.0); // This should'n throw an exception + Kilogram mass5; + test_(mass.getValue() == 30.4); + test_(mass3.getValue() == 30.4); + test_(mass4.getValue() == 0.0); + test_(mass5.getValue() == 0.0); + succeed_(); + } + catch(std::invalid_argument& ex) { + fail_("Valid constructor call throws an exception"); + } + + // Try an invalid constructor call + try{ + Kilogram mass4(-0.1); // This should throw an exception + fail_("Invalid constructors calls didn't throw an exception"); + } + catch(std::invalid_argument& ex) { + succeed_(); + } + + // Try an invalid constructor call + try{ + Kilogram mass4(-10.); // This should throw an exception + fail_("Invalid constructors calls didn't throw an exception"); + } + catch(std::invalid_argument& ex) { + succeed_(); + } + } + + // Test the method getValue() + void testGetValue() { + test_(mass1.getValue() == 5.0); + test_(mass2.getValue() == 10.0); + } + + // Test the method setValue() + void testSetValue() { + Kilogram mass(10.0); + + // Try a valid setValue() call + try { + mass.setValue(0.0); // This should'n throw an exception + test_(mass.getValue() == 0.0); + mass.setValue(43.0); // This should'n throw an exception + test_(mass.getValue() == 43.0); + succeed_(); + } + catch(std::invalid_argument& ex) { + fail_("Valid setValue() call throw an exception"); + } + + // Try an invalid setValue() call + try { + mass.setValue(-0.1); // This should throw an exception + fail_("Invalid setValue() call didn't throw an exception"); + } + catch(std::invalid_argument& ex) { + succeed_(); + } + + // Try an invalid setValue() call + try { + mass.setValue(-40.0); // This should throw an exception + fail_("Invalid setValue() call didn't throw an exception"); + } + catch(std::invalid_argument& ex) { + succeed_(); + } + } + +}; + +#endif diff --git a/src/reactphysics3d/testing/testing_physics/TimeTest.h b/src/reactphysics3d/testing/testing_physics/TimeTest.h new file mode 100755 index 00000000..722bd619 --- /dev/null +++ b/src/reactphysics3d/testing/testing_physics/TimeTest.h @@ -0,0 +1,189 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +#ifndef TIMETEST_H +#define TIMETEST_H + +// Libraries +#include "../TestSuite/Test.h" +#include "../../physics/Time.h" +#include + +// Namespaces +using namespace reactphysics3d; + +// Class TimeTest +class TimeTest : public TestSuite::Test { + private : + Time time1; + Time time2; + public : + + // Constructor + TimeTest() : time1(5.0), time2(10.0) { + + } + + // Run method of the Test + void run() { + testConstructors(); + testGetValue(); + testSetValue(); + testOperatorAddition(); + testOperatorSubstraction(); + testOperatorMultiplication(); + } + + // Test the constructors + void testConstructors() { + // Try valid constructors calls + try { + Time time(30.4); // This should'n throw an exception + Time time3(time); // This should'n throw an exception + Time time4(0.0); // This should'n throw an exception + Time time5; + test_(time.getValue() == 30.4); + test_(time3.getValue() == 30.4); + test_(time4.getValue() == 0.0); + test_(time5.getValue() == 0.0); + succeed_(); + } + catch(std::invalid_argument& ex) { + fail_("Valid constructor call throws an exception"); + } + + // Try an invalid constructor call + try{ + Time time4(-0.1); // This should throw an exception + fail_("Invalid constructors calls didn't throw an exception"); + } + catch(std::invalid_argument& ex) { + succeed_(); + } + + // Try an invalid constructor call + try{ + Time time4(-10.); // This should throw an exception + fail_("Invalid constructors calls didn't throw an exception"); + } + catch(std::invalid_argument& ex) { + succeed_(); + } + } + + // Test the method getValue() + void testGetValue() { + test_(time1.getValue() == 5.0); + test_(time2.getValue() == 10.0); + } + + // Test the method setValue() + void testSetValue() { + Time time(10.0); + + // Try a valid setValue() call + try { + time.setValue(0.0); // This should'n throw an exception + test_(time.getValue() == 0.0); + time.setValue(43.0); // This should'n throw an exception + test_(time.getValue() == 43.0); + succeed_(); + } + catch(std::invalid_argument& ex) { + fail_("Valid setValue() call throw an exception"); + } + + // Try an invalid setValue() call + try { + time.setValue(-0.1); // This should throw an exception + fail_("Invalid setValue() call didn't throw an exception"); + } + catch(std::invalid_argument& ex) { + succeed_(); + } + + // Try an invalid setValue() call + try { + time.setValue(-40.0); // This should throw an exception + fail_("Invalid setValue() call didn't throw an exception"); + } + catch(std::invalid_argument& ex) { + succeed_(); + } + } + + // Test the overloaded addition operator + void testOperatorAddition() { + Time result; + result = time1 + time2; + test_(result.getValue() == 15.0); + } + + // Test the overloaded substraction operator + void testOperatorSubstraction() { + Time result; + + // Try a valid substraction + try { + + result = time2 - time1; // This should'n throw an exception + test_(result.getValue() == 5.0); + succeed_(); + } + catch(std::invalid_argument& ex) { + fail_("Valid call to substraction operator throw an exception"); + } + + // try an invalid substraction + try { + result = time1 - time2; + fail_("Invalid call to substraction didn't throw an exception"); + } + catch(std::invalid_argument& ex) { + succeed_(); + } + } + + // Test the overloaded multiplication operator + void testOperatorMultiplication() { + Time result; + + // Try a valid substraction + try { + result = time1 * 3.0; // This should'n throw an exception + test_(result.getValue() == 15.0); + succeed_(); + } + catch(std::invalid_argument& ex) { + fail_("Valid call to multiplication operator throw an exception"); + } + + // try an invalid substraction + try { + result = time1 * (-3.0); + fail_("Invalid call to multiplication didn't throw an exception"); + } + catch(std::invalid_argument& ex) { + succeed_(); + } + } + +}; + +#endif diff --git a/src/reactphysics3d/typeDefinitions.h b/src/reactphysics3d/typeDefinitions.h new file mode 100644 index 00000000..1e956970 --- /dev/null +++ b/src/reactphysics3d/typeDefinitions.h @@ -0,0 +1,32 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 Daniel Chappuis * +********************************************************************************* +* * +* Permission is hereby granted, free of charge, to any person obtaining a copy * +* of this software and associated documentation files (the "Software"), to deal * +* in the Software without restriction, including without limitation the rights * +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * +* copies of the Software, and to permit persons to whom the Software is * +* furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * +* THE SOFTWARE. * +********************************************************************************/ + +#ifndef TYPEDEFINITIONS_H +#define TYPEDEFINITIONS_H + +// Type definitions +typedef unsigned int uint; + +#endif + diff --git a/src/typeDefinitions.h b/src/typeDefinitions.h new file mode 100644 index 00000000..2aca7f1c --- /dev/null +++ b/src/typeDefinitions.h @@ -0,0 +1,27 @@ +/*************************************************************************** +* Copyright (C) 2009 Daniel Chappuis * +**************************************************************************** +* This file is part of ReactPhysics3D. * +* * +* ReactPhysics3D is free software: you can redistribute it and/or modify * +* it under the terms of the GNU Lesser General Public License as published * +* by the Free Software Foundation, either version 3 of the License, or * +* (at your option) any later version. * +* * +* ReactPhysics3D is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU Lesser General Public License for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with ReactPhysics3D. If not, see . * +***************************************************************************/ + +#ifndef TYPEDEFINITIONS_H +#define TYPEDEFINITIONS_H + +// Type definitions +typedef unsigned int uint; + +#endif +