From 572d64845c1e0b072f62e87553855abec3cb7c00 Mon Sep 17 00:00:00 2001 From: Edouard DUPIN Date: Thu, 15 Jun 2017 21:27:15 +0200 Subject: [PATCH] [DEV] remove all math from here and use etk versions --- ephysics/body/Body.h | 4 +- ephysics/body/CollisionBody.cpp | 17 +- ephysics/body/CollisionBody.h | 73 +- ephysics/body/RigidBody.cpp | 294 +++-- ephysics/body/RigidBody.h | 607 ++++------ ephysics/collision/CollisionDetection.h | 4 +- ephysics/collision/CollisionShapeInfo.h | 6 +- ephysics/collision/ContactManifold.cpp | 44 +- ephysics/collision/ContactManifold.h | 42 +- ephysics/collision/ContactManifoldSet.cpp | 30 +- ephysics/collision/ContactManifoldSet.h | 2 +- ephysics/collision/ProxyShape.cpp | 14 +- ephysics/collision/ProxyShape.h | 26 +- ephysics/collision/RaycastInfo.h | 6 +- .../broadphase/BroadPhaseAlgorithm.cpp | 2 +- .../broadphase/BroadPhaseAlgorithm.h | 2 +- .../collision/broadphase/DynamicAABBTree.cpp | 351 +++--- .../collision/broadphase/DynamicAABBTree.h | 36 +- .../narrowphase/ConcaveVsConvexAlgorithm.cpp | 36 +- .../narrowphase/ConcaveVsConvexAlgorithm.h | 20 +- .../narrowphase/EPA/EPAAlgorithm.cpp | 58 +- .../collision/narrowphase/EPA/EPAAlgorithm.h | 10 +- .../collision/narrowphase/EPA/EdgeEPA.cpp | 2 +- ephysics/collision/narrowphase/EPA/EdgeEPA.h | 2 +- .../collision/narrowphase/EPA/TriangleEPA.cpp | 12 +- .../collision/narrowphase/EPA/TriangleEPA.h | 24 +- .../narrowphase/EPA/TrianglesStore.h | 4 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 96 +- .../collision/narrowphase/GJK/GJKAlgorithm.h | 8 +- .../collision/narrowphase/GJK/Simplex.cpp | 28 +- ephysics/collision/narrowphase/GJK/Simplex.h | 24 +- .../narrowphase/SphereVsSphereAlgorithm.cpp | 22 +- ephysics/collision/shapes/AABB.cpp | 157 +-- ephysics/collision/shapes/AABB.h | 80 +- ephysics/collision/shapes/BoxShape.cpp | 42 +- ephysics/collision/shapes/BoxShape.h | 44 +- ephysics/collision/shapes/CapsuleShape.cpp | 108 +- ephysics/collision/shapes/CapsuleShape.h | 52 +- ephysics/collision/shapes/CollisionShape.cpp | 20 +- ephysics/collision/shapes/CollisionShape.h | 26 +- .../collision/shapes/ConcaveMeshShape.cpp | 32 +- ephysics/collision/shapes/ConcaveMeshShape.h | 18 +- ephysics/collision/shapes/ConcaveShape.h | 6 +- ephysics/collision/shapes/ConeShape.cpp | 88 +- ephysics/collision/shapes/ConeShape.h | 50 +- ephysics/collision/shapes/ConvexMeshShape.cpp | 58 +- ephysics/collision/shapes/ConvexMeshShape.h | 66 +- ephysics/collision/shapes/ConvexShape.cpp | 16 +- ephysics/collision/shapes/ConvexShape.h | 10 +- ephysics/collision/shapes/CylinderShape.cpp | 99 +- ephysics/collision/shapes/CylinderShape.h | 53 +- .../collision/shapes/HeightFieldShape.cpp | 148 +-- ephysics/collision/shapes/HeightFieldShape.h | 121 +- ephysics/collision/shapes/SphereShape.cpp | 18 +- ephysics/collision/shapes/SphereShape.h | 58 +- ephysics/collision/shapes/TriangleShape.cpp | 34 +- ephysics/collision/shapes/TriangleShape.h | 74 +- ephysics/configuration.h | 6 +- ephysics/constraint/BallAndSocketJoint.cpp | 92 +- ephysics/constraint/BallAndSocketJoint.h | 22 +- ephysics/constraint/ContactPoint.cpp | 4 +- ephysics/constraint/ContactPoint.h | 78 +- ephysics/constraint/FixedJoint.cpp | 134 +-- ephysics/constraint/FixedJoint.h | 30 +- ephysics/constraint/HingeJoint.cpp | 220 ++-- ephysics/constraint/HingeJoint.h | 56 +- ephysics/constraint/SliderJoint.cpp | 244 ++-- ephysics/constraint/SliderJoint.h | 62 +- ephysics/engine/CollisionWorld.cpp | 4 +- ephysics/engine/CollisionWorld.h | 2 +- ephysics/engine/ConstraintSolver.h | 24 +- ephysics/engine/ContactSolver.cpp | 206 ++-- ephysics/engine/ContactSolver.h | 94 +- ephysics/engine/DynamicsWorld.cpp | 52 +- ephysics/engine/DynamicsWorld.h | 40 +- ephysics/engine/Impulse.h | 12 +- ephysics/engine/Material.h | 4 +- ephysics/engine/OverlappingPair.h | 10 +- ephysics/mathematics/Matrix2x2.cpp | 68 -- ephysics/mathematics/Matrix2x2.h | 297 ----- ephysics/mathematics/Matrix3x3.cpp | 83 -- ephysics/mathematics/Matrix3x3.h | 347 ------ ephysics/mathematics/Quaternion.cpp | 241 ---- ephysics/mathematics/Quaternion.h | 330 ------ ephysics/mathematics/Ray.h | 8 +- ephysics/mathematics/Transform.cpp | 39 - ephysics/mathematics/Transform.h | 205 ---- ephysics/mathematics/Vector2.cpp | 54 - ephysics/mathematics/Vector2.h | 323 ------ ephysics/mathematics/Vector3.cpp | 67 -- ephysics/mathematics/Vector3.h | 363 ------ ephysics/mathematics/mathematics.h | 47 +- .../mathematics/mathematics_functions.cpp | 20 +- ephysics/mathematics/mathematics_functions.h | 10 +- lutin_ephysics.py | 12 - test/main.cpp | 24 +- test/tests/collision/TestAABB.h | 280 ++--- test/tests/collision/TestCollisionWorld.h | 34 +- test/tests/collision/TestDynamicAABBTree.h | 130 +-- test/tests/collision/TestPointInside.h | 1026 ++++++++--------- test/tests/collision/TestRaycast.h | 794 ++++++------- .../mathematics/TestMathematicsFunctions.h | 8 +- test/tests/mathematics/TestMatrix2x2.h | 142 +-- test/tests/mathematics/TestMatrix3x3.h | 160 +-- test/tests/mathematics/TestQuaternion.h | 228 ++-- test/tests/mathematics/TestTransform.h | 138 +-- test/tests/mathematics/TestVector2.h | 184 +-- test/tests/mathematics/TestVector3.h | 226 ++-- tools/testbed/common/Box.cpp | 66 +- tools/testbed/common/Box.h | 14 +- tools/testbed/common/Capsule.cpp | 54 +- tools/testbed/common/Capsule.h | 16 +- tools/testbed/common/ConcaveMesh.cpp | 58 +- tools/testbed/common/ConcaveMesh.h | 16 +- tools/testbed/common/Cone.cpp | 54 +- tools/testbed/common/Cone.h | 16 +- tools/testbed/common/ConvexMesh.cpp | 58 +- tools/testbed/common/ConvexMesh.h | 16 +- tools/testbed/common/Cylinder.cpp | 54 +- tools/testbed/common/Cylinder.h | 16 +- tools/testbed/common/Dumbbell.cpp | 74 +- tools/testbed/common/Dumbbell.h | 16 +- tools/testbed/common/HeightField.cpp | 74 +- tools/testbed/common/HeightField.h | 18 +- tools/testbed/common/Line.cpp | 8 +- tools/testbed/common/Line.h | 14 +- tools/testbed/common/PhysicsObject.cpp | 10 +- tools/testbed/common/PhysicsObject.h | 8 +- tools/testbed/common/Sphere.cpp | 54 +- tools/testbed/common/Sphere.h | 16 +- tools/testbed/common/VisualContactPoint.cpp | 8 +- tools/testbed/common/VisualContactPoint.h | 2 +- tools/testbed/opengl-framework/src/Camera.cpp | 16 +- tools/testbed/opengl-framework/src/Camera.h | 10 +- tools/testbed/opengl-framework/src/Light.h | 2 +- tools/testbed/opengl-framework/src/Mesh.cpp | 52 +- tools/testbed/opengl-framework/src/Mesh.h | 62 +- .../opengl-framework/src/MeshReaderWriter.cpp | 46 +- tools/testbed/opengl-framework/src/Object3D.h | 40 +- tools/testbed/opengl-framework/src/Shader.h | 26 +- .../opengl-framework/src/Texture2D.cpp | 14 +- .../testbed/opengl-framework/src/Texture2D.h | 8 +- .../src/TextureReaderWriter.cpp | 4 +- .../opengl-framework/src/maths/Matrix3.h | 30 +- .../opengl-framework/src/maths/Matrix4.h | 72 +- .../opengl-framework/src/maths/Vector2.h | 58 +- .../opengl-framework/src/maths/Vector3.h | 74 +- .../opengl-framework/src/maths/Vector4.h | 26 +- .../opengl-framework/src/openglframework.h | 4 +- .../collisionshapes/CollisionShapesScene.cpp | 108 +- .../collisionshapes/CollisionShapesScene.h | 4 +- .../scenes/concavemesh/ConcaveMeshScene.cpp | 28 +- tools/testbed/scenes/cubes/CubesScene.cpp | 32 +- tools/testbed/scenes/cubes/CubesScene.h | 4 +- .../scenes/heightfield/HeightFieldScene.cpp | 28 +- tools/testbed/scenes/joints/JointsScene.cpp | 132 +-- tools/testbed/scenes/joints/JointsScene.h | 4 +- tools/testbed/scenes/raycast/RaycastScene.cpp | 36 +- tools/testbed/scenes/raycast/RaycastScene.h | 12 +- tools/testbed/src/Gui.cpp | 18 +- tools/testbed/src/Scene.cpp | 14 +- tools/testbed/src/Scene.h | 14 +- tools/testbed/src/SceneDemo.cpp | 20 +- tools/testbed/src/TestbedApplication.cpp | 18 +- tools/testbed/src/TestbedApplication.h | 12 +- 165 files changed, 4914 insertions(+), 7522 deletions(-) delete mode 100644 ephysics/mathematics/Matrix2x2.cpp delete mode 100644 ephysics/mathematics/Matrix2x2.h delete mode 100644 ephysics/mathematics/Matrix3x3.cpp delete mode 100644 ephysics/mathematics/Matrix3x3.h delete mode 100644 ephysics/mathematics/Quaternion.cpp delete mode 100644 ephysics/mathematics/Quaternion.h delete mode 100644 ephysics/mathematics/Transform.cpp delete mode 100644 ephysics/mathematics/Transform.h delete mode 100644 ephysics/mathematics/Vector2.cpp delete mode 100644 ephysics/mathematics/Vector2.h delete mode 100644 ephysics/mathematics/Vector3.cpp delete mode 100644 ephysics/mathematics/Vector3.h diff --git a/ephysics/body/Body.h b/ephysics/body/Body.h index 0e17958..88e5fbf 100644 --- a/ephysics/body/Body.h +++ b/ephysics/body/Body.h @@ -170,11 +170,11 @@ inline void Body::setIsActive(bool isActive) { inline void Body::setIsSleeping(bool isSleeping) { if (isSleeping) { - m_sleepTime = float(0.0); + m_sleepTime = 0.0f; } else { if (m_isSleeping) { - m_sleepTime = float(0.0); + m_sleepTime = 0.0f; } } diff --git a/ephysics/body/CollisionBody.cpp b/ephysics/body/CollisionBody.cpp index ced90c5..0ee0136 100644 --- a/ephysics/body/CollisionBody.cpp +++ b/ephysics/body/CollisionBody.cpp @@ -18,7 +18,7 @@ using namespace reactphysics3d; * @param world The physics world where the body is created * @param id ID of the body */ -CollisionBody::CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id) +CollisionBody::CollisionBody(const etk::Transform3D& transform, CollisionWorld& world, bodyindex id) : Body(id), m_type(DYNAMIC), m_transform(transform), m_proxyCollisionShapes(NULL), m_numberCollisionShapes(0), m_contactManifoldsList(NULL), m_world(world) { @@ -32,6 +32,15 @@ CollisionBody::~CollisionBody() { removeAllCollisionShapes(); } +inline void CollisionBody::setType(BodyType _type) { + m_type = _type; + if (m_type == STATIC) { + // Update the broad-phase state of the body + updateBroadPhaseState(); + } +} + + // Add a collision shape to the body. Note that you can share a collision // shape between several bodies using the same collision shape instance to // when you add the shape to the different bodies. Do not forget to delete @@ -48,7 +57,7 @@ CollisionBody::~CollisionBody() { * the new collision shape you have added. */ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape, - const Transform& transform) { + const etk::Transform3D& transform) { // Create a new proxy collision shape to attach the collision shape to the body ProxyShape* proxyShape = new (m_world.m_memoryAllocator.allocate( @@ -188,7 +197,7 @@ void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool fo proxyShape->getCollisionShape()->computeAABB(aabb, m_transform * proxyShape->getLocalToBodyTransform()); // Update the broad-phase state for the proxy collision shape - m_world.m_collisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert); + m_world.m_collisionDetection.updateProxyCollisionShape(proxyShape, aabb, vec3(0, 0, 0), forceReinsert); } // Set whether or not the body is active @@ -267,7 +276,7 @@ int32_t CollisionBody::resetIsAlreadyInIslandAndCountManifolds() { * @param worldPoint The point to test (in world-space coordinates) * @return True if the point is inside the body */ -bool CollisionBody::testPointInside(const Vector3& worldPoint) const { +bool CollisionBody::testPointInside(const vec3& worldPoint) const { // For each collision shape of the body for (ProxyShape* shape = m_proxyCollisionShapes; shape != NULL; shape = shape->m_next) { diff --git a/ephysics/body/CollisionBody.h b/ephysics/body/CollisionBody.h index b0a8737..b8aeed8 100644 --- a/ephysics/body/CollisionBody.h +++ b/ephysics/body/CollisionBody.h @@ -9,7 +9,7 @@ #include #include #include -#include +#include #include #include #include @@ -50,7 +50,7 @@ class CollisionBody : public Body { BodyType m_type; /// Position and orientation of the body - Transform m_transform; + etk::Transform3D m_transform; /// First element of the linked list of proxy collision shapes of this body ProxyShape* m_proxyCollisionShapes; @@ -96,29 +96,40 @@ class CollisionBody : public Body { // -------------------- Methods -------------------- // /// Constructor - CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id); + CollisionBody(const etk::Transform3D& transform, CollisionWorld& world, bodyindex id); /// Destructor virtual ~CollisionBody(); /// Return the type of the body BodyType getType() const; - - /// Set the type of the body - void setType(BodyType type); + /** + * @brief Set the type of the body + * The type of the body can either STATIC, KINEMATIC or DYNAMIC as described bellow: + * STATIC : A static body has infinite mass, zero velocity but the position can be + * changed manually. A static body does not collide with other static or kinematic bodies. + * KINEMATIC : A kinematic body has infinite mass, the velocity can be changed manually and its + * position is computed by the physics engine. A kinematic body does not collide with + * other static or kinematic bodies. + * DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its + * position is determined by the physics engine. A dynamic body can collide with other + * dynamic, static or kinematic bodies. + * @param[in] type The type of the body (STATIC, KINEMATIC, DYNAMIC) + */ + virtual void setType(BodyType _type); /// Set whether or not the body is active virtual void setIsActive(bool isActive); /// Return the current position and orientation - const Transform& getTransform() const; + const etk::Transform3D& getTransform() const; /// Set the current position and orientation - virtual void setTransform(const Transform& transform); + virtual void setTransform(const etk::Transform3D& transform); /// Add a collision shape to the body. virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape, - const Transform& transform); + const etk::Transform3D& transform); /// Remove a collision shape from the body virtual void removeCollisionShape(const ProxyShape* proxyShape); @@ -127,7 +138,7 @@ class CollisionBody : public Body { const ContactManifoldListElement* getContactManifoldsList() const; /// Return true if a point is inside the collision body - bool testPointInside(const Vector3& worldPoint) const; + bool testPointInside(const vec3& worldPoint) const; /// Raycast method with feedback information bool raycast(const Ray& ray, RaycastInfo& raycastInfo); @@ -142,16 +153,16 @@ class CollisionBody : public Body { const ProxyShape* getProxyShapesList() const; /// Return the world-space coordinates of a point given the local-space coordinates of the body - Vector3 getWorldPoint(const Vector3& localPoint) const; + vec3 getWorldPoint(const vec3& localPoint) const; /// Return the world-space vector of a vector given in local-space coordinates of the body - Vector3 getWorldVector(const Vector3& localVector) const; + vec3 getWorldVector(const vec3& localVector) const; /// Return the body local-space coordinates of a point given in the world-space coordinates - Vector3 getLocalPoint(const Vector3& worldPoint) const; + vec3 getLocalPoint(const vec3& worldPoint) const; /// Return the body local-space coordinates of a vector given in the world-space coordinates - Vector3 getLocalVector(const Vector3& worldVector) const; + vec3 getLocalVector(const vec3& worldVector) const; // -------------------- Friendship -------------------- // @@ -171,35 +182,13 @@ inline BodyType CollisionBody::getType() const { return m_type; } -// Set the type of the body -/// The type of the body can either STATIC, KINEMATIC or DYNAMIC as described bellow: -/// STATIC : A static body has infinite mass, zero velocity but the position can be -/// changed manually. A static body does not collide with other static or kinematic bodies. -/// KINEMATIC : A kinematic body has infinite mass, the velocity can be changed manually and its -/// position is computed by the physics engine. A kinematic body does not collide with -/// other static or kinematic bodies. -/// DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its -/// position is determined by the physics engine. A dynamic body can collide with other -/// dynamic, static or kinematic bodies. -/** - * @param type The type of the body (STATIC, KINEMATIC, DYNAMIC) - */ -inline void CollisionBody::setType(BodyType type) { - m_type = type; - - if (m_type == STATIC) { - - // Update the broad-phase state of the body - updateBroadPhaseState(); - } -} // Return the current position and orientation /** * @return The current transformation of the body that transforms the local-space * of the body int32_to world-space */ -inline const Transform& CollisionBody::getTransform() const { +inline const etk::Transform3D& CollisionBody::getTransform() const { return m_transform; } @@ -208,7 +197,7 @@ inline const Transform& CollisionBody::getTransform() const { * @param transform The transformation of the body that transforms the local-space * of the body int32_to world-space */ -inline void CollisionBody::setTransform(const Transform& transform) { +inline void CollisionBody::setTransform(const etk::Transform3D& transform) { // Update the transform of the body m_transform = transform; @@ -249,7 +238,7 @@ inline const ProxyShape* CollisionBody::getProxyShapesList() const { * @param localPoint A point in the local-space coordinates of the body * @return The point in world-space coordinates */ -inline Vector3 CollisionBody::getWorldPoint(const Vector3& localPoint) const { +inline vec3 CollisionBody::getWorldPoint(const vec3& localPoint) const { return m_transform * localPoint; } @@ -258,7 +247,7 @@ inline Vector3 CollisionBody::getWorldPoint(const Vector3& localPoint) const { * @param localVector A vector in the local-space coordinates of the body * @return The vector in world-space coordinates */ -inline Vector3 CollisionBody::getWorldVector(const Vector3& localVector) const { +inline vec3 CollisionBody::getWorldVector(const vec3& localVector) const { return m_transform.getOrientation() * localVector; } @@ -267,7 +256,7 @@ inline Vector3 CollisionBody::getWorldVector(const Vector3& localVector) const { * @param worldPoint A point in world-space coordinates * @return The point in the local-space coordinates of the body */ -inline Vector3 CollisionBody::getLocalPoint(const Vector3& worldPoint) const { +inline vec3 CollisionBody::getLocalPoint(const vec3& worldPoint) const { return m_transform.getInverse() * worldPoint; } @@ -276,7 +265,7 @@ inline Vector3 CollisionBody::getLocalPoint(const Vector3& worldPoint) const { * @param worldVector A vector in world-space coordinates * @return The vector in the local-space coordinates of the body */ -inline Vector3 CollisionBody::getLocalVector(const Vector3& worldVector) const { +inline vec3 CollisionBody::getLocalVector(const vec3& worldVector) const { return m_transform.getOrientation().getInverse() * worldVector; } diff --git a/ephysics/body/RigidBody.cpp b/ephysics/body/RigidBody.cpp index 98625d9..5588087 100644 --- a/ephysics/body/RigidBody.cpp +++ b/ephysics/body/RigidBody.cpp @@ -13,20 +13,15 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -// Constructor -/** -* @param transform The transformation of the body -* @param world The world where the body has been added -* @param id The ID of the body -*/ -RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id) - : CollisionBody(transform, world, id), m_initMass(float(1.0)), + +RigidBody::RigidBody(const etk::Transform3D& transform, CollisionWorld& world, bodyindex id) + : CollisionBody(transform, world, id), m_initMass(1.0f), m_centerOfMassLocal(0, 0, 0), m_centerOfMassWorld(transform.getPosition()), - m_isGravityEnabled(true), m_linearDamping(float(0.0)), m_angularDamping(float(0.0)), + m_isGravityEnabled(true), m_linearDamping(0.0f), m_angularDamping(float(0.0)), m_jointsList(NULL) { // Compute the inverse mass - m_massInverse = float(1.0) / m_initMass; + m_massInverse = 1.0f / m_initMass; } // Destructor @@ -34,63 +29,34 @@ RigidBody::~RigidBody() { assert(m_jointsList == NULL); } -// Set the type of the body -/// The type of the body can either STATIC, KINEMATIC or DYNAMIC as described bellow: -/// STATIC : A static body has infinite mass, zero velocity but the position can be -/// changed manually. A static body does not collide with other static or kinematic bodies. -/// KINEMATIC : A kinematic body has infinite mass, the velocity can be changed manually and its -/// position is computed by the physics engine. A kinematic body does not collide with -/// other static or kinematic bodies. -/// DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its -/// position is determined by the physics engine. A dynamic body can collide with other -/// dynamic, static or kinematic bodies. -/** - * @param type The type of the body (STATIC, KINEMATIC, DYNAMIC) - */ -void RigidBody::setType(BodyType type) { - if (m_type == type) return; - - CollisionBody::setType(type); - - // Recompute the total mass, center of mass and inertia tensor +void RigidBody::setType(BodyType _type) { + if (m_type == _type) { + return; + } + CollisionBody::setType(_type); recomputeMassInformation(); - - // If it is a static body if (m_type == STATIC) { - // Reset the velocity to zero - m_linearVelocity.setToZero(); - m_angularVelocity.setToZero(); + m_linearVelocity.setZero(); + m_angularVelocity.setZero(); } - - // If it is a static or a kinematic body - if (m_type == STATIC || m_type == KINEMATIC) { - + if ( m_type == STATIC + || m_type == KINEMATIC) { // Reset the inverse mass and inverse inertia tensor to zero - m_massInverse = float(0.0); - m_inertiaTensorLocal.setToZero(); - m_inertiaTensorLocalInverse.setToZero(); - - } - else { // If it is a dynamic body - m_massInverse = float(1.0) / m_initMass; + m_massInverse = 0.0f; + m_inertiaTensorLocal.setZero(); + m_inertiaTensorLocalInverse.setZero(); + } else { + m_massInverse = 1.0f / m_initMass; m_inertiaTensorLocalInverse = m_inertiaTensorLocal.getInverse(); } - - // Awake the body setIsSleeping(false); - - // Remove all the contacts with this body resetContactManifoldsList(); - - // Ask the broad-phase to test again the collision shapes of the body for collision - // detection (as if the body has moved) + // Ask the broad-phase to test again the collision shapes of the body for collision detection (as if the body has moved) askForBroadPhaseCollisionCheck(); - - // Reset the force and torque on the body - m_externalForce.setToZero(); - m_externalTorque.setToZero(); + m_externalForce.setZero(); + m_externalTorque.setZero(); } // Set the local inertia tensor of the body (in local-space coordinates) @@ -98,7 +64,7 @@ void RigidBody::setType(BodyType type) { * @param inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space * coordinates */ -void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) { +void RigidBody::setInertiaTensorLocal(const etk::Matrix3x3& inertiaTensorLocal) { if (m_type != DYNAMIC) return; @@ -113,11 +79,11 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) { * @param centerOfMassLocal The center of mass of the body in local-space * coordinates */ -void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) { +void RigidBody::setCenterOfMassLocal(const vec3& centerOfMassLocal) { if (m_type != DYNAMIC) return; - const Vector3 oldCenterOfMass = m_centerOfMassWorld; + const vec3 oldCenterOfMass = m_centerOfMassWorld; m_centerOfMassLocal = centerOfMassLocal; // Compute the center of mass in world-space coordinates @@ -127,31 +93,23 @@ void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) { m_linearVelocity += m_angularVelocity.cross(m_centerOfMassWorld - oldCenterOfMass); } -// Set the mass of the rigid body -/** - * @param mass The mass (in kilograms) of the body - */ -void RigidBody::setMass(float mass) { - if (m_type != DYNAMIC) return; - - m_initMass = mass; - - if (m_initMass > float(0.0)) { - m_massInverse = float(1.0) / m_initMass; +void RigidBody::setMass(float _mass) { + if (m_type != DYNAMIC) { + return; } - else { - m_initMass = float(1.0); - m_massInverse = float(1.0); + m_initMass = _mass; + if (m_initMass > 0.0f) { + m_massInverse = 1.0f / m_initMass; + } else { + m_initMass = 1.0f; + m_massInverse = 1.0f; } } -// Remove a joint from the joints list void RigidBody::removeJointFrom_jointsList(MemoryAllocator& memoryAllocator, const Joint* joint) { - - assert(joint != NULL); - assert(m_jointsList != NULL); - + assert(joint != nullptr); + assert(m_jointsList != nullptr); // Remove the joint from the linked list of the joints of the first body if (m_jointsList->joint == joint) { // If the first element is the one to remove JointListElement* elementToRemove = m_jointsList; @@ -161,7 +119,7 @@ void RigidBody::removeJointFrom_jointsList(MemoryAllocator& memoryAllocator, con } else { // If the element to remove is not the first one in the list JointListElement* currentElement = m_jointsList; - while (currentElement->next != NULL) { + while (currentElement->next != nullptr) { if (currentElement->next->joint == joint) { JointListElement* elementToRemove = currentElement->next; currentElement->next = elementToRemove->next; @@ -191,10 +149,10 @@ void RigidBody::removeJointFrom_jointsList(MemoryAllocator& memoryAllocator, con * the new collision shape you have added. */ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape, - const Transform& transform, + const etk::Transform3D& transform, float mass) { - assert(mass > float(0.0)); + assert(mass > 0.0f); // Create a new proxy collision shape to attach the collision shape to the body ProxyShape* proxyShape = new (m_world.m_memoryAllocator.allocate( @@ -235,154 +193,120 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape, * @param proxyShape The pointer of the proxy shape you want to remove */ void RigidBody::removeCollisionShape(const ProxyShape* proxyShape) { - - // Remove the collision shape CollisionBody::removeCollisionShape(proxyShape); - - // Recompute the total mass, center of mass and inertia tensor recomputeMassInformation(); } -// Set the linear velocity of the rigid body. -/** - * @param linearVelocity Linear velocity vector of the body - */ -void RigidBody::setLinearVelocity(const Vector3& linearVelocity) { - // If it is a static body, we do nothing - if (m_type == STATIC) return; - - // Update the linear velocity of the current body state - m_linearVelocity = linearVelocity; - - // If the linear velocity is not zero, awake the body - if (m_linearVelocity.lengthSquare() > float(0.0)) { +void RigidBody::setLinearVelocity(const vec3& _linearVelocity) { + if (m_type == STATIC) { + return; + } + m_linearVelocity = _linearVelocity; + if (m_linearVelocity.length2() > 0.0f) { setIsSleeping(false); } } -// Set the angular velocity. -/** -* @param angularVelocity The angular velocity vector of the body -*/ -void RigidBody::setAngularVelocity(const Vector3& angularVelocity) { - // If it is a static body, we do nothing - if (m_type == STATIC) return; - - // Set the angular velocity - m_angularVelocity = angularVelocity; - - // If the velocity is not zero, awake the body - if (m_angularVelocity.lengthSquare() > float(0.0)) { +void RigidBody::setAngularVelocity(const vec3& _angularVelocity) { + if (m_type == STATIC) { + return; + } + m_angularVelocity = _angularVelocity; + if (m_angularVelocity.length2() > 0.0f) { setIsSleeping(false); } } -// Set the current position and orientation -/** - * @param transform The transformation of the body that transforms the local-space - * of the body int32_to world-space - */ -void RigidBody::setTransform(const Transform& transform) { +void RigidBody::setIsSleeping(bool _isSleeping) { + if (_isSleeping) { + m_linearVelocity.setZero(); + m_angularVelocity.setZero(); + m_externalForce.setZero(); + m_externalTorque.setZero(); + } + Body::setIsSleeping(_isSleeping); +} - // Update the transform of the body - m_transform = transform; - - const Vector3 oldCenterOfMass = m_centerOfMassWorld; +void RigidBody::setTransform(const etk::Transform3D& _transform) { + m_transform = _transform; + const vec3 oldCenterOfMass = m_centerOfMassWorld; // Compute the new center of mass in world-space coordinates m_centerOfMassWorld = m_transform * m_centerOfMassLocal; - // Update the linear velocity of the center of mass m_linearVelocity += m_angularVelocity.cross(m_centerOfMassWorld - oldCenterOfMass); - - // Update the broad-phase state of the body updateBroadPhaseState(); } // Recompute the center of mass, total mass and inertia tensor of the body using all // the collision shapes attached to the body. void RigidBody::recomputeMassInformation() { - - m_initMass = float(0.0); - m_massInverse = float(0.0); - m_inertiaTensorLocal.setToZero(); - m_inertiaTensorLocalInverse.setToZero(); - m_centerOfMassLocal.setToZero(); - + m_initMass = 0.0f; + m_massInverse = 0.0f; + m_inertiaTensorLocal.setZero(); + m_inertiaTensorLocalInverse.setZero(); + m_centerOfMassLocal.setZero(); // If it is STATIC or KINEMATIC body if (m_type == STATIC || m_type == KINEMATIC) { m_centerOfMassWorld = m_transform.getPosition(); return; } - assert(m_type == DYNAMIC); - // Compute the total mass of the body for (ProxyShape* shape = m_proxyCollisionShapes; shape != NULL; shape = shape->m_next) { m_initMass += shape->getMass(); m_centerOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass(); } - - if (m_initMass > float(0.0)) { - m_massInverse = float(1.0) / m_initMass; + if (m_initMass > 0.0f) { + m_massInverse = 1.0f / m_initMass; + } else { + m_initMass = 1.0f; + m_massInverse = 1.0f; } - else { - m_initMass = float(1.0); - m_massInverse = float(1.0); - } - // Compute the center of mass - const Vector3 oldCenterOfMass = m_centerOfMassWorld; + const vec3 oldCenterOfMass = m_centerOfMassWorld; m_centerOfMassLocal *= m_massInverse; m_centerOfMassWorld = m_transform * m_centerOfMassLocal; - // Compute the total mass and inertia tensor using all the collision shapes - for (ProxyShape* shape = m_proxyCollisionShapes; shape != NULL; shape = shape->m_next) { - + for (ProxyShape* shape = m_proxyCollisionShapes; shape != nullptr; shape = shape->m_next) { // Get the inertia tensor of the collision shape in its local-space - Matrix3x3 inertiaTensor; + etk::Matrix3x3 inertiaTensor; shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass()); - // Convert the collision shape inertia tensor int32_to the local-space of the body - const Transform& shapeTransform = shape->getLocalToBodyTransform(); - Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix(); + const etk::Transform3D& shapeTransform = shape->getLocalToBodyTransform(); + etk::Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix(); inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose(); - // Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape // center int32_to a inertia tensor w.r.t to the body origin. - Vector3 offset = shapeTransform.getPosition() - m_centerOfMassLocal; - float offsetSquare = offset.lengthSquare(); - Matrix3x3 offsetMatrix; - offsetMatrix[0].setAllValues(offsetSquare, float(0.0), float(0.0)); - offsetMatrix[1].setAllValues(float(0.0), offsetSquare, float(0.0)); - offsetMatrix[2].setAllValues(float(0.0), float(0.0), offsetSquare); - offsetMatrix[0] += offset * (-offset.x); - offsetMatrix[1] += offset * (-offset.y); - offsetMatrix[2] += offset * (-offset.z); + vec3 offset = shapeTransform.getPosition() - m_centerOfMassLocal; + float offsetSquare = offset.length2(); + vec3 off1 = offset * (-offset.x()); + vec3 off2 = offset * (-offset.y()); + vec3 off3 = offset * (-offset.z()); + etk::Matrix3x3 offsetMatrix(off1.x()+offsetSquare, off1.y(), off1.z(), + off2.x(), off2.y()+offsetSquare, off2.z(), + off3.x(), off3.y(), off3.z()+offsetSquare); offsetMatrix *= shape->getMass(); - m_inertiaTensorLocal += inertiaTensor + offsetMatrix; } - // Compute the local inverse inertia tensor m_inertiaTensorLocalInverse = m_inertiaTensorLocal.getInverse(); - // Update the linear velocity of the center of mass m_linearVelocity += m_angularVelocity.cross(m_centerOfMassWorld - oldCenterOfMass); } -// Update the broad-phase state for this body (because it has moved for instance) + void RigidBody::updateBroadPhaseState() const { PROFILE("RigidBody::updateBroadPhaseState()"); DynamicsWorld& world = static_cast(m_world); - const Vector3 displacement = world.m_timeStep * m_linearVelocity; + const vec3 displacement = world.m_timeStep * m_linearVelocity; // For all the proxy collision shapes of the body - for (ProxyShape* shape = m_proxyCollisionShapes; shape != NULL; shape = shape->m_next) { + for (ProxyShape* shape = m_proxyCollisionShapes; shape != nullptr; shape = shape->m_next) { // Recompute the world-space AABB of the collision shape AABB aabb; @@ -393,3 +317,43 @@ void RigidBody::updateBroadPhaseState() const { } } + +void RigidBody::applyForceToCenterOfMass(const vec3& _force) { + // If it is not a dynamic body, we do nothing + if (m_type != DYNAMIC) { + return; + } + // Awake the body if it was sleeping + if (m_isSleeping) { + setIsSleeping(false); + } + // Add the force + m_externalForce += _force; +} + +void RigidBody::applyForce(const vec3& _force, const vec3& _point) { + // If it is not a dynamic body, we do nothing + if (m_type != DYNAMIC) { + return; + } + // Awake the body if it was sleeping + if (m_isSleeping) { + setIsSleeping(false); + } + // Add the force and torque + m_externalForce += _force; + m_externalTorque += (_point - m_centerOfMassWorld).cross(_force); +} + +void RigidBody::applyTorque(const vec3& _torque) { + // If it is not a dynamic body, we do nothing + if (m_type != DYNAMIC) { + return; + } + // Awake the body if it was sleeping + if (m_isSleeping) { + setIsSleeping(false); + } + // Add the torque + m_externalTorque += _torque; +} \ No newline at end of file diff --git a/ephysics/body/RigidBody.h b/ephysics/body/RigidBody.h index f2071e8..b3c9e47 100644 --- a/ephysics/body/RigidBody.h +++ b/ephysics/body/RigidBody.h @@ -5,14 +5,12 @@ */ #pragma once -// Libraries #include #include #include #include #include -/// Namespace reactphysics3d namespace reactphysics3d { // Class declarations @@ -20,177 +18,259 @@ struct JointListElement; class Joint; class DynamicsWorld; -// Class RigidBody /** - * This class represents a rigid body of the physics + * @brief This class represents a rigid body of the physics * engine. A rigid body is a non-deformable body that * has a constant mass. This class inherits from the * CollisionBody class. - */ + */ class RigidBody : public CollisionBody { - protected : - - // -------------------- Attributes -------------------- // - - /// Intial mass of the body - float m_initMass; - - /// Center of mass of the body in local-space coordinates. - /// The center of mass can therefore be different from the body origin - Vector3 m_centerOfMassLocal; - - /// Center of mass of the body in world-space coordinates - Vector3 m_centerOfMassWorld; - - /// Linear velocity of the body - Vector3 m_linearVelocity; - - /// Angular velocity of the body - Vector3 m_angularVelocity; - - /// Current external force on the body - Vector3 m_externalForce; - - /// Current external torque on the body - Vector3 m_externalTorque; - - /// Local inertia tensor of the body (in local-space) with respect to the - /// center of mass of the body - Matrix3x3 m_inertiaTensorLocal; - - /// Inverse of the inertia tensor of the body - Matrix3x3 m_inertiaTensorLocalInverse; - - /// Inverse of the mass of the body - float m_massInverse; - - /// True if the gravity needs to be applied to this rigid body - bool m_isGravityEnabled; - - /// Material properties of the rigid body - Material m_material; - - /// Linear velocity damping factor - float m_linearDamping; - - /// Angular velocity damping factor - float m_angularDamping; - - /// First element of the linked list of joints involving this body - JointListElement* m_jointsList; - - // -------------------- Methods -------------------- // - + float m_initMass; //!< Intial mass of the body + vec3 m_centerOfMassLocal; //!< Center of mass of the body in local-space coordinates. The center of mass can therefore be different from the body origin + vec3 m_centerOfMassWorld; //!< Center of mass of the body in world-space coordinates + vec3 m_linearVelocity; //!< Linear velocity of the body + vec3 m_angularVelocity; //!< Angular velocity of the body + vec3 m_externalForce; //!< Current external force on the body + vec3 m_externalTorque; //!< Current external torque on the body + etk::Matrix3x3 m_inertiaTensorLocal; //!< Local inertia tensor of the body (in local-space) with respect to the center of mass of the body + etk::Matrix3x3 m_inertiaTensorLocalInverse; //!< Inverse of the inertia tensor of the body + float m_massInverse; //!< Inverse of the mass of the body + bool m_isGravityEnabled; //!< True if the gravity needs to be applied to this rigid body + Material m_material; //!< Material properties of the rigid body + float m_linearDamping; //!< Linear velocity damping factor + float m_angularDamping; //!< Angular velocity damping factor + JointListElement* m_jointsList; //!< First element of the linked list of joints involving this body /// Private copy-constructor RigidBody(const RigidBody& body); /// Private assignment operator RigidBody& operator=(const RigidBody& body); - /// Remove a joint from the joints list - void removeJointFrom_jointsList(MemoryAllocator& memoryAllocator, const Joint* joint); - - /// Update the transform of the body after a change of the center of mass - void updateTransformWithCenterOfMass(); - - /// Update the broad-phase state for this body (because it has moved for instance) + /** + * @brief Remove a joint from the joints list + */ + void removeJointFrom_jointsList(MemoryAllocator& _memoryAllocator, const Joint* _joint); + /** + * @brief Update the transform of the body after a change of the center of mass + */ + void updateTransformWithCenterOfMass() { + // Translate the body according to the translation of the center of mass position + m_transform.setPosition(m_centerOfMassWorld - m_transform.getOrientation() * m_centerOfMassLocal); + } + /** + * @brief Update the broad-phase state for this body (because it has moved for instance) + */ virtual void updateBroadPhaseState() const; - public : - - // -------------------- Methods -------------------- // - - /// Constructor - RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id); + /** + * @brief Constructor + * @param transform The transformation of the body + * @param world The world where the body has been added + * @param id The ID of the body + */ + RigidBody(const etk::Transform3D& transform, CollisionWorld& world, bodyindex id); /// Destructor virtual ~RigidBody(); - - /// Set the type of the body (static, kinematic or dynamic) - void setType(BodyType type); - - /// Set the current position and orientation - virtual void setTransform(const Transform& transform); - - /// Return the mass of the body - float getMass() const; - - /// Return the linear velocity - Vector3 getLinearVelocity() const; - - /// Set the linear velocity of the body. - void setLinearVelocity(const Vector3& linearVelocity); - - /// Return the angular velocity - Vector3 getAngularVelocity() const; - - /// Set the angular velocity. - void setAngularVelocity(const Vector3& angularVelocity); - - /// Set the variable to know whether or not the body is sleeping - virtual void setIsSleeping(bool isSleeping); - - /// Return the local inertia tensor of the body (in body coordinates) - const Matrix3x3& getInertiaTensorLocal() const; + void setType(BodyType _type); // TODO: override + /** + * @brief Set the current position and orientation + * @param[in] _transform The transformation of the body that transforms the local-space of the body int32_to world-space + */ + virtual void setTransform(const etk::Transform3D& _transform); + /** + * @brief Get the mass of the body + * @return The mass (in kilograms) of the body + */ + float getMass() const { + return m_initMass; + } + /** + * @brief Get the linear velocity + * @return The linear velocity vector of the body + */ + vec3 getLinearVelocity() const { + return m_linearVelocity; + } + /** + * @brief Set the linear velocity of the rigid body. + * @param[in] _linearVelocity Linear velocity vector of the body + */ + void setLinearVelocity(const vec3& _linearVelocity); + /** + * @brief Get the angular velocity of the body + * @return The angular velocity vector of the body + */ + vec3 getAngularVelocity() const { + return m_angularVelocity; + } + /** + * @brief Set the angular velocity. + * @param[in] _angularVelocity The angular velocity vector of the body + */ + void setAngularVelocity(const vec3& _angularVelocity); + /** + * @brief Set the variable to know whether or not the body is sleeping + * @param[in] _isSleeping New sleeping state of the body + */ + virtual void setIsSleeping(bool _isSleeping); + /** + * @brief Get the local inertia tensor of the body (in local-space coordinates) + * @return The 3x3 inertia tensor matrix of the body + */ + const etk::Matrix3x3& getInertiaTensorLocal() const { + return m_inertiaTensorLocal; + } /// Set the local inertia tensor of the body (in body coordinates) - void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal); + void setInertiaTensorLocal(const etk::Matrix3x3& inertiaTensorLocal); /// Set the local center of mass of the body (in local-space coordinates) - void setCenterOfMassLocal(const Vector3& centerOfMassLocal); + void setCenterOfMassLocal(const vec3& centerOfMassLocal); + /** + * @brief Set the mass of the rigid body + * @param[in] _mass The mass (in kilograms) of the body + */ + void setMass(float _mass); - /// Set the mass of the rigid body - void setMass(float mass); - - /// Return the inertia tensor in world coordinates. - Matrix3x3 getInertiaTensorWorld() const; - - /// Return the inverse of the inertia tensor in world coordinates. - Matrix3x3 getInertiaTensorInverseWorld() const; - - /// Return true if the gravity needs to be applied to this rigid body - bool isGravityEnabled() const; - - /// Set the variable to know if the gravity is applied to this rigid body - void enableGravity(bool isEnabled); - - /// Return a reference to the material properties of the rigid body - Material& getMaterial(); - - /// Set a new material for this rigid body - void setMaterial(const Material& material); - - /// Return the linear velocity damping factor - float getLinearDamping() const; - - /// Set the linear damping factor - void setLinearDamping(float linearDamping); - - /// Return the angular velocity damping factor - float getAngularDamping() const; - - /// Set the angular damping factor - void setAngularDamping(float angularDamping); - - /// Return the first element of the linked list of joints involving this body - const JointListElement* getJointsList() const; - - /// Return the first element of the linked list of joints involving this body - JointListElement* getJointsList(); - - /// Apply an external force to the body at its center of mass. - void applyForceToCenterOfMass(const Vector3& force); - - /// Apply an external force to the body at a given point (in world-space coordinates). - void applyForce(const Vector3& force, const Vector3& point); - - /// Apply an external torque to the body. - void applyTorque(const Vector3& torque); + /** + * @brief Get the inertia tensor in world coordinates. + * The inertia tensor I_w in world coordinates is computed + * with the local inertia tensor I_b in body coordinates + * by I_w = R * I_b * R^T + * where R is the rotation matrix (and R^T its transpose) of + * the current orientation quaternion of the body + * @return The 3x3 inertia tensor matrix of the body in world-space coordinates + */ + etk::Matrix3x3 getInertiaTensorWorld() const { + // Compute and return the inertia tensor in world coordinates + return m_transform.getOrientation().getMatrix() * m_inertiaTensorLocal * + m_transform.getOrientation().getMatrix().getTranspose(); + } + + /** + * @brief Get the inverse of the inertia tensor in world coordinates. + * The inertia tensor I_w in world coordinates is computed with the + * local inverse inertia tensor I_b^-1 in body coordinates + * by I_w = R * I_b^-1 * R^T + * where R is the rotation matrix (and R^T its transpose) of the + * current orientation quaternion of the body + * @return The 3x3 inverse inertia tensor matrix of the body in world-space coordinates + */ + etk::Matrix3x3 getInertiaTensorInverseWorld() const { + // TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE + // INVERSE WORLD TENSOR IN THE CLASS AND UPLDATE IT WHEN THE ORIENTATION OF THE BODY CHANGES + // Compute and return the inertia tensor in world coordinates + return m_transform.getOrientation().getMatrix() * m_inertiaTensorLocalInverse * + m_transform.getOrientation().getMatrix().getTranspose(); + } + /** + * @brief get the need of gravity appling to this rigid body + * @return True if the gravity is applied to the body + */ + bool isGravityEnabled() const { + return m_isGravityEnabled; + } + /** + * @brief Set the variable to know if the gravity is applied to this rigid body + * @param[in] _isEnabled True if you want the gravity to be applied to this body + */ + void enableGravity(bool _isEnabled) { + m_isGravityEnabled = _isEnabled; + } + /** + * @brief get a reference to the material properties of the rigid body + * @return A reference to the material of the body + */ + Material& getMaterial() { + return m_material; + } + /** + * @brief Set a new material for this rigid body + * @param[in] _material The material you want to set to the body + */ + void setMaterial(const Material& _material) { + m_material = _material; + } + /** + * @brief Get the linear velocity damping factor + * @return The linear damping factor of this body + */ + float getLinearDamping() const { + return m_linearDamping; + } + /** + * @brief Set the linear damping factor. This is the ratio of the linear velocity that the body will lose every at seconds of simulation. + * @param[in] _linearDamping The linear damping factor of this body + */ + void setLinearDamping(float _linearDamping) { + assert(_linearDamping >= 0.0f); + m_linearDamping = _linearDamping; + } + /** + * @brief Get the angular velocity damping factor + * @return The angular damping factor of this body + */ + float getAngularDamping() const { + return m_angularDamping; + } + /** + * @brief Set the angular damping factor. This is the ratio of the angular velocity that the body will lose at every seconds of simulation. + * @param[in] _angularDamping The angular damping factor of this body + */ + void setAngularDamping(float _angularDamping) { + assert(_angularDamping >= 0.0f); + m_angularDamping = _angularDamping; + } + /** + * @brief Get the first element of the linked list of joints involving this body + * @return The first element of the linked-list of all the joints involving this body + */ + const JointListElement* getJointsList() const { + return m_jointsList; + } + /** + * @brief Get the first element of the linked list of joints involving this body + * @return The first element of the linked-list of all the joints involving this body + */ + JointListElement* getJointsList() { + return m_jointsList; + } + /** + * @brief Apply an external force to the body at its center of mass. + * If the body is sleeping, calling this method will wake it up. Note that the + * force will we added to the sum of the applied forces and that this sum will be + * reset to zero at the end of each call of the DynamicsWorld::update() method. + * You can only apply a force to a dynamic body otherwise, this method will do nothing. + * @param[in] _force The external force to apply on the center of mass of the body + */ + void applyForceToCenterOfMass(const vec3& _force); + /** + * @brief Apply an external force to the body at a given point (in world-space coordinates). + * If the point is not at the center of mass of the body, it will also + * generate some torque and therefore, change the angular velocity of the body. + * If the body is sleeping, calling this method will wake it up. Note that the + * force will we added to the sum of the applied forces and that this sum will be + * reset to zero at the end of each call of the DynamicsWorld::update() method. + * You can only apply a force to a dynamic body otherwise, this method will do nothing. + * @param[in] _force The force to apply on the body + * @param[in] _point The point where the force is applied (in world-space coordinates) + */ + void applyForce(const vec3& _force, const vec3& _point); + /** + * @brief Apply an external torque to the body. + * If the body is sleeping, calling this method will wake it up. Note that the + * force will we added to the sum of the applied torques and that this sum will be + * reset to zero at the end of each call of the DynamicsWorld::update() method. + * You can only apply a force to a dynamic body otherwise, this method will do nothing. + * @param[in] _torque The external torque to apply on the body + */ + void applyTorque(const vec3& _torque); /// Add a collision shape to the body. virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape, - const Transform& transform, + const etk::Transform3D& transform, float mass); /// Remove a collision shape from the body @@ -200,8 +280,6 @@ class RigidBody : public CollisionBody { /// the collision shapes attached to the body. void recomputeMassInformation(); - // -------------------- Friendship -------------------- // - friend class DynamicsWorld; friend class ContactSolver; friend class BallAndSocketJoint; @@ -210,247 +288,18 @@ class RigidBody : public CollisionBody { friend class FixedJoint; }; -// Method that return the mass of the body -/** - * @return The mass (in kilograms) of the body - */ -inline float RigidBody::getMass() const { - return m_initMass; -} -// Return the linear velocity -/** - * @return The linear velocity vector of the body - */ -inline Vector3 RigidBody::getLinearVelocity() const { - return m_linearVelocity; -} -// Return the angular velocity of the body -/** - * @return The angular velocity vector of the body - */ -inline Vector3 RigidBody::getAngularVelocity() const { - return m_angularVelocity; -} -// Return the local inertia tensor of the body (in local-space coordinates) -/** - * @return The 3x3 inertia tensor matrix of the body (in local-space coordinates) - */ -inline const Matrix3x3& RigidBody::getInertiaTensorLocal() const { - return m_inertiaTensorLocal; -} -// Return the inertia tensor in world coordinates. -/// The inertia tensor I_w in world coordinates is computed -/// with the local inertia tensor I_b in body coordinates -/// by I_w = R * I_b * R^T -/// where R is the rotation matrix (and R^T its transpose) of -/// the current orientation quaternion of the body -/** - * @return The 3x3 inertia tensor matrix of the body in world-space coordinates - */ -inline Matrix3x3 RigidBody::getInertiaTensorWorld() const { - // Compute and return the inertia tensor in world coordinates - return m_transform.getOrientation().getMatrix() * m_inertiaTensorLocal * - m_transform.getOrientation().getMatrix().getTranspose(); -} -// Return the inverse of the inertia tensor in world coordinates. -/// The inertia tensor I_w in world coordinates is computed with the -/// local inverse inertia tensor I_b^-1 in body coordinates -/// by I_w = R * I_b^-1 * R^T -/// where R is the rotation matrix (and R^T its transpose) of the -/// current orientation quaternion of the body -/** - * @return The 3x3 inverse inertia tensor matrix of the body in world-space - * coordinates - */ -inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const { - // TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE - // INVERSE WORLD TENSOR IN THE CLASS AND UPLDATE IT WHEN THE ORIENTATION OF THE BODY CHANGES - // Compute and return the inertia tensor in world coordinates - return m_transform.getOrientation().getMatrix() * m_inertiaTensorLocalInverse * - m_transform.getOrientation().getMatrix().getTranspose(); -} -// Return true if the gravity needs to be applied to this rigid body -/** - * @return True if the gravity is applied to the body - */ -inline bool RigidBody::isGravityEnabled() const { - return m_isGravityEnabled; -} -// Set the variable to know if the gravity is applied to this rigid body -/** - * @param isEnabled True if you want the gravity to be applied to this body - */ -inline void RigidBody::enableGravity(bool isEnabled) { - m_isGravityEnabled = isEnabled; -} -// Return a reference to the material properties of the rigid body -/** - * @return A reference to the material of the body - */ -inline Material& RigidBody::getMaterial() { - return m_material; -} -// Set a new material for this rigid body -/** - * @param material The material you want to set to the body - */ -inline void RigidBody::setMaterial(const Material& material) { - m_material = material; -} - -// Return the linear velocity damping factor -/** - * @return The linear damping factor of this body - */ -inline float RigidBody::getLinearDamping() const { - return m_linearDamping; -} - -// Set the linear damping factor. This is the ratio of the linear velocity -// that the body will lose every at seconds of simulation. -/** - * @param linearDamping The linear damping factor of this body - */ -inline void RigidBody::setLinearDamping(float linearDamping) { - assert(linearDamping >= float(0.0)); - m_linearDamping = linearDamping; -} - -// Return the angular velocity damping factor -/** - * @return The angular damping factor of this body - */ -inline float RigidBody::getAngularDamping() const { - return m_angularDamping; -} - -// Set the angular damping factor. This is the ratio of the angular velocity -// that the body will lose at every seconds of simulation. -/** - * @param angularDamping The angular damping factor of this body - */ -inline void RigidBody::setAngularDamping(float angularDamping) { - assert(angularDamping >= float(0.0)); - m_angularDamping = angularDamping; -} - -// Return the first element of the linked list of joints involving this body -/** - * @return The first element of the linked-list of all the joints involving this body - */ -inline const JointListElement* RigidBody::getJointsList() const { - return m_jointsList; -} - -// Return the first element of the linked list of joints involving this body -/** - * @return The first element of the linked-list of all the joints involving this body - */ -inline JointListElement* RigidBody::getJointsList() { - return m_jointsList; -} - -// Set the variable to know whether or not the body is sleeping -inline void RigidBody::setIsSleeping(bool isSleeping) { - - if (isSleeping) { - m_linearVelocity.setToZero(); - m_angularVelocity.setToZero(); - m_externalForce.setToZero(); - m_externalTorque.setToZero(); - } - - Body::setIsSleeping(isSleeping); -} - -// Apply an external force to the body at its center of mass. -/// If the body is sleeping, calling this method will wake it up. Note that the -/// force will we added to the sum of the applied forces and that this sum will be -/// reset to zero at the end of each call of the DynamicsWorld::update() method. -/// You can only apply a force to a dynamic body otherwise, this method will do nothing. -/** - * @param force The external force to apply on the center of mass of the body - */ -inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) { - - // If it is not a dynamic body, we do nothing - if (m_type != DYNAMIC) return; - - // Awake the body if it was sleeping - if (m_isSleeping) { - setIsSleeping(false); - } - - // Add the force - m_externalForce += force; -} - -// Apply an external force to the body at a given point (in world-space coordinates). -/// If the point is not at the center of mass of the body, it will also -/// generate some torque and therefore, change the angular velocity of the body. -/// If the body is sleeping, calling this method will wake it up. Note that the -/// force will we added to the sum of the applied forces and that this sum will be -/// reset to zero at the end of each call of the DynamicsWorld::update() method. -/// You can only apply a force to a dynamic body otherwise, this method will do nothing. -/** - * @param force The force to apply on the body - * @param point The point where the force is applied (in world-space coordinates) - */ -inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) { - - // If it is not a dynamic body, we do nothing - if (m_type != DYNAMIC) return; - - // Awake the body if it was sleeping - if (m_isSleeping) { - setIsSleeping(false); - } - - // Add the force and torque - m_externalForce += force; - m_externalTorque += (point - m_centerOfMassWorld).cross(force); -} - -// Apply an external torque to the body. -/// If the body is sleeping, calling this method will wake it up. Note that the -/// force will we added to the sum of the applied torques and that this sum will be -/// reset to zero at the end of each call of the DynamicsWorld::update() method. -/// You can only apply a force to a dynamic body otherwise, this method will do nothing. -/** - * @param torque The external torque to apply on the body - */ -inline void RigidBody::applyTorque(const Vector3& torque) { - - // If it is not a dynamic body, we do nothing - if (m_type != DYNAMIC) return; - - // Awake the body if it was sleeping - if (m_isSleeping) { - setIsSleeping(false); - } - - // Add the torque - m_externalTorque += torque; -} - -/// Update the transform of the body after a change of the center of mass -inline void RigidBody::updateTransformWithCenterOfMass() { - - // Translate the body according to the translation of the center of mass position - m_transform.setPosition(m_centerOfMassWorld - m_transform.getOrientation() * m_centerOfMassLocal); -} } diff --git a/ephysics/collision/CollisionDetection.h b/ephysics/collision/CollisionDetection.h index 9f728cc..a161740 100644 --- a/ephysics/collision/CollisionDetection.h +++ b/ephysics/collision/CollisionDetection.h @@ -145,7 +145,7 @@ class CollisionDetection : public NarrowPhaseCallback { /// Update a proxy collision shape (that has moved for instance) void updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb, - const Vector3& displacement = Vector3(0, 0, 0), bool forceReinsert = false); + const vec3& displacement = vec3(0, 0, 0), bool forceReinsert = false); /// Add a pair of bodies that cannot collide with each other void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2); @@ -257,7 +257,7 @@ inline void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape // Update a proxy collision shape (that has moved for instance) inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb, - const Vector3& displacement, bool forceReinsert) { + const vec3& displacement, bool forceReinsert) { m_broadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement); } diff --git a/ephysics/collision/CollisionShapeInfo.h b/ephysics/collision/CollisionShapeInfo.h index e149510..2b48e93 100644 --- a/ephysics/collision/CollisionShapeInfo.h +++ b/ephysics/collision/CollisionShapeInfo.h @@ -31,15 +31,15 @@ struct CollisionShapeInfo { /// Pointer to the collision shape const CollisionShape* collisionShape; - /// Transform that maps from collision shape local-space to world-space - Transform shapeToWorldTransform; + /// etk::Transform3D that maps from collision shape local-space to world-space + etk::Transform3D shapeToWorldTransform; /// Cached collision data of the proxy shape void** cachedCollisionData; /// Constructor CollisionShapeInfo(ProxyShape* proxyCollisionShape, const CollisionShape* shape, - const Transform& shapeLocalToWorldTransform, OverlappingPair* pair, + const etk::Transform3D& shapeLocalToWorldTransform, OverlappingPair* pair, void** cachedData) : overlappingPair(pair), proxyShape(proxyCollisionShape), collisionShape(shape), shapeToWorldTransform(shapeLocalToWorldTransform), diff --git a/ephysics/collision/ContactManifold.cpp b/ephysics/collision/ContactManifold.cpp index 1b3bf08..c962a45 100644 --- a/ephysics/collision/ContactManifold.cpp +++ b/ephysics/collision/ContactManifold.cpp @@ -35,7 +35,7 @@ void ContactManifold::addContactPoint(ContactPoint* contact) { // Check if the new point point does not correspond to a same contact point // already in the manifold. float distance = (m_contactPoints[i]->getWorldPointOnBody1() - - contact->getWorldPointOnBody1()).lengthSquare(); + contact->getWorldPointOnBody1()).length2(); if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) { // Delete the new contact @@ -86,7 +86,7 @@ void ContactManifold::removeContactPoint(uint32_t index) { /// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also /// the contacts with a too large distance between the contact points in the plane orthogonal to the /// contact normal. -void ContactManifold::update(const Transform& transform1, const Transform& transform2) { +void ContactManifold::update(const etk::Transform3D& transform1, const etk::Transform3D& transform2) { if (m_nbContactPoints == 0) return; @@ -117,13 +117,13 @@ void ContactManifold::update(const Transform& transform1, const Transform& trans else { // Compute the distance of the two contact points in the plane // orthogonal to the contact normal - Vector3 projOfPoint1 = m_contactPoints[i]->getWorldPointOnBody1() + + vec3 projOfPoint1 = m_contactPoints[i]->getWorldPointOnBody1() + m_contactPoints[i]->getNormal() * distanceNormal; - Vector3 projDifference = m_contactPoints[i]->getWorldPointOnBody2() - projOfPoint1; + vec3 projDifference = m_contactPoints[i]->getWorldPointOnBody2() - projOfPoint1; // If the orthogonal distance is larger than the valid distance // threshold, we remove the contact - if (projDifference.lengthSquare() > squarePersistentContactThreshold) { + if (projDifference.length2() > squarePersistentContactThreshold) { removeContactPoint(i); } } @@ -162,7 +162,7 @@ int32_t ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) /// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore, /// this is only a guess that is faster to compute. This idea comes from the Bullet Physics library /// by Erwin Coumans (http://wwww.bulletphysics.org). -int32_t ContactManifold::getIndexToRemove(int32_t indexMaxPenetration, const Vector3& newPoint) const { +int32_t ContactManifold::getIndexToRemove(int32_t indexMaxPenetration, const vec3& newPoint) const { assert(m_nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); @@ -173,35 +173,35 @@ int32_t ContactManifold::getIndexToRemove(int32_t indexMaxPenetration, const Vec if (indexMaxPenetration != 0) { // Compute the area - Vector3 vector1 = newPoint - m_contactPoints[1]->getLocalPointOnBody1(); - Vector3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - + vec3 vector1 = newPoint - m_contactPoints[1]->getLocalPointOnBody1(); + vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - m_contactPoints[2]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area0 = crossProduct.lengthSquare(); + vec3 crossProduct = vector1.cross(vector2); + area0 = crossProduct.length2(); } if (indexMaxPenetration != 1) { // Compute the area - Vector3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1(); - Vector3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - + vec3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1(); + vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - m_contactPoints[2]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area1 = crossProduct.lengthSquare(); + vec3 crossProduct = vector1.cross(vector2); + area1 = crossProduct.length2(); } if (indexMaxPenetration != 2) { // Compute the area - Vector3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1(); - Vector3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - + vec3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1(); + vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - m_contactPoints[1]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area2 = crossProduct.lengthSquare(); + vec3 crossProduct = vector1.cross(vector2); + area2 = crossProduct.length2(); } if (indexMaxPenetration != 3) { // Compute the area - Vector3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1(); - Vector3 vector2 = m_contactPoints[2]->getLocalPointOnBody1() - + vec3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1(); + vec3 vector2 = m_contactPoints[2]->getLocalPointOnBody1() - m_contactPoints[1]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area3 = crossProduct.lengthSquare(); + vec3 crossProduct = vector1.cross(vector2); + area3 = crossProduct.length2(); } // Return the index of the contact to remove diff --git a/ephysics/collision/ContactManifold.h b/ephysics/collision/ContactManifold.h index e950755..b63b2cc 100644 --- a/ephysics/collision/ContactManifold.h +++ b/ephysics/collision/ContactManifold.h @@ -84,10 +84,10 @@ class ContactManifold { uint32_t m_nbContactPoints; /// First friction vector of the contact manifold - Vector3 m_frictionVector1; + vec3 m_frictionVector1; /// Second friction vector of the contact manifold - Vector3 m_frictionVector2; + vec3 m_frictionvec2; /// First friction constraint accumulated impulse float m_frictionImpulse1; @@ -99,7 +99,7 @@ class ContactManifold { float m_frictionTwistImpulse; /// Accumulated rolling resistance impulse - Vector3 m_rollingResistanceImpulse; + vec3 m_rollingResistanceImpulse; /// True if the contact manifold has already been added int32_to an island bool m_isAlreadyInIsland; @@ -122,7 +122,7 @@ class ContactManifold { int32_t getIndexOfDeepestPenetration(ContactPoint* newContact) const; /// Return the index that will be removed. - int32_t getIndexToRemove(int32_t indexMaxPenetration, const Vector3& newPoint) const; + int32_t getIndexToRemove(int32_t indexMaxPenetration, const vec3& newPoint) const; /// Remove a contact point from the manifold void removeContactPoint(uint32_t index); @@ -160,7 +160,7 @@ class ContactManifold { void addContactPoint(ContactPoint* contact); /// Update the contact manifold. - void update(const Transform& transform1, const Transform& transform2); + void update(const etk::Transform3D& transform1, const etk::Transform3D& transform2); /// Clear the contact manifold void clear(); @@ -169,16 +169,16 @@ class ContactManifold { uint32_t getNbContactPoints() const; /// Return the first friction vector at the center of the contact manifold - const Vector3& getFrictionVector1() const; + const vec3& getFrictionVector1() const; /// set the first friction vector at the center of the contact manifold - void setFrictionVector1(const Vector3& m_frictionVector1); + void setFrictionVector1(const vec3& m_frictionVector1); /// Return the second friction vector at the center of the contact manifold - const Vector3& getFrictionVector2() const; + const vec3& getFrictionvec2() const; /// set the second friction vector at the center of the contact manifold - void setFrictionVector2(const Vector3& m_frictionVector2); + void setFrictionvec2(const vec3& m_frictionvec2); /// Return the first friction accumulated impulse float getFrictionImpulse1() const; @@ -199,13 +199,13 @@ class ContactManifold { void setFrictionTwistImpulse(float frictionTwistImpulse); /// Set the accumulated rolling resistance impulse - void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse); + void setRollingResistanceImpulse(const vec3& rollingResistanceImpulse); /// Return a contact point of the manifold ContactPoint* getContactPoint(uint32_t index) const; /// Return the normalized averaged normal vector - Vector3 getAverageContactNormal() const; + vec3 getAverageContactNormal() const; /// Return the largest depth of all the contact points float getLargestContactDepth() const; @@ -248,23 +248,23 @@ inline uint32_t ContactManifold::getNbContactPoints() const { } // Return the first friction vector at the center of the contact manifold -inline const Vector3& ContactManifold::getFrictionVector1() const { +inline const vec3& ContactManifold::getFrictionVector1() const { return m_frictionVector1; } // set the first friction vector at the center of the contact manifold -inline void ContactManifold::setFrictionVector1(const Vector3& frictionVector1) { +inline void ContactManifold::setFrictionVector1(const vec3& frictionVector1) { m_frictionVector1 = frictionVector1; } // Return the second friction vector at the center of the contact manifold -inline const Vector3& ContactManifold::getFrictionVector2() const { - return m_frictionVector2; +inline const vec3& ContactManifold::getFrictionvec2() const { + return m_frictionvec2; } // set the second friction vector at the center of the contact manifold -inline void ContactManifold::setFrictionVector2(const Vector3& frictionVector2) { - m_frictionVector2 = frictionVector2; +inline void ContactManifold::setFrictionvec2(const vec3& frictionvec2) { + m_frictionvec2 = frictionvec2; } // Return the first friction accumulated impulse @@ -298,7 +298,7 @@ inline void ContactManifold::setFrictionTwistImpulse(float frictionTwistImpulse) } // Set the accumulated rolling resistance impulse -inline void ContactManifold::setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse) { +inline void ContactManifold::setRollingResistanceImpulse(const vec3& rollingResistanceImpulse) { m_rollingResistanceImpulse = rollingResistanceImpulse; } @@ -314,14 +314,14 @@ inline bool ContactManifold::isAlreadyInIsland() const { } // Return the normalized averaged normal vector -inline Vector3 ContactManifold::getAverageContactNormal() const { - Vector3 averageNormal; +inline vec3 ContactManifold::getAverageContactNormal() const { + vec3 averageNormal; for (uint32_t i=0; igetNormal(); } - return averageNormal.getUnit(); + return averageNormal.safeNormalized(); } // Return the largest depth of all the contact points diff --git a/ephysics/collision/ContactManifoldSet.cpp b/ephysics/collision/ContactManifoldSet.cpp index c982441..0bc4383 100644 --- a/ephysics/collision/ContactManifoldSet.cpp +++ b/ephysics/collision/ContactManifoldSet.cpp @@ -129,29 +129,29 @@ int32_t ContactManifoldSet::selectManifoldWithSimilarNormal(int16_t normalDirect // Map the normal vector int32_to a cubemap face bucket (a face contains 4x4 buckets) // Each face of the cube is divided int32_to 4x4 buckets. This method maps the // normal vector int32_to of the of the bucket and returns a unique Id for the bucket -int16_t ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) const { +int16_t ContactManifoldSet::computeCubemapNormalId(const vec3& normal) const { - assert(normal.lengthSquare() > MACHINE_EPSILON); + assert(normal.length2() > MACHINE_EPSILON); int32_t faceNo; float u, v; - float max = max3(fabs(normal.x), fabs(normal.y), fabs(normal.z)); - Vector3 normalScaled = normal / max; + float max = max3(fabs(normal.x()), fabs(normal.y()), fabs(normal.z())); + vec3 normalScaled = normal / max; - if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) { - faceNo = normalScaled.x > 0 ? 0 : 1; - u = normalScaled.y; - v = normalScaled.z; + if (normalScaled.x() >= normalScaled.y() && normalScaled.x() >= normalScaled.z()) { + faceNo = normalScaled.x() > 0 ? 0 : 1; + u = normalScaled.y(); + v = normalScaled.z(); } - else if (normalScaled.y >= normalScaled.x && normalScaled.y >= normalScaled.z) { - faceNo = normalScaled.y > 0 ? 2 : 3; - u = normalScaled.x; - v = normalScaled.z; + else if (normalScaled.y() >= normalScaled.x() && normalScaled.y() >= normalScaled.z()) { + faceNo = normalScaled.y() > 0 ? 2 : 3; + u = normalScaled.x(); + v = normalScaled.z(); } else { - faceNo = normalScaled.z > 0 ? 4 : 5; - u = normalScaled.x; - v = normalScaled.y; + faceNo = normalScaled.z() > 0 ? 4 : 5; + u = normalScaled.x(); + v = normalScaled.y(); } int32_t indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); diff --git a/ephysics/collision/ContactManifoldSet.h b/ephysics/collision/ContactManifoldSet.h index c04b6e6..67c833c 100644 --- a/ephysics/collision/ContactManifoldSet.h +++ b/ephysics/collision/ContactManifoldSet.h @@ -59,7 +59,7 @@ class ContactManifoldSet { // Map the normal vector int32_to a cubemap face bucket (a face contains 4x4 buckets) // Each face of the cube is divided int32_to 4x4 buckets. This method maps the // normal vector int32_to of the of the bucket and returns a unique Id for the bucket - int16_t computeCubemapNormalId(const Vector3& normal) const; + int16_t computeCubemapNormalId(const vec3& normal) const; public: diff --git a/ephysics/collision/ProxyShape.cpp b/ephysics/collision/ProxyShape.cpp index 91c8aba..89ef332 100644 --- a/ephysics/collision/ProxyShape.cpp +++ b/ephysics/collision/ProxyShape.cpp @@ -13,10 +13,10 @@ using namespace reactphysics3d; /** * @param body Pointer to the parent body * @param shape Pointer to the collision shape - * @param transform Transformation from collision shape local-space to body local-space + * @param transform etk::Transform3Dation from collision shape local-space to body local-space * @param mass Mass of the collision shape (in kilograms) */ -ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, float mass) +ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const etk::Transform3D& transform, float mass) :m_body(body), m_collisionShape(shape), m_localToBodyTransform(transform), m_mass(mass), m_next(NULL), m_broadPhaseID(-1), m_cachedCollisionData(NULL), m_userData(NULL), m_collisionCategoryBits(0x0001), m_collideWithMaskBits(0xFFFF) { @@ -37,9 +37,9 @@ ProxyShape::~ProxyShape() { * @param worldPoint Point to test in world-space coordinates * @return True if the point is inside the collision shape */ -bool ProxyShape::testPointInside(const Vector3& worldPoint) { - const Transform localToWorld = m_body->getTransform() * m_localToBodyTransform; - const Vector3 localPoint = localToWorld.getInverse() * worldPoint; +bool ProxyShape::testPointInside(const vec3& worldPoint) { + const etk::Transform3D localToWorld = m_body->getTransform() * m_localToBodyTransform; + const vec3 localPoint = localToWorld.getInverse() * worldPoint; return m_collisionShape->testPointInside(localPoint, this); } @@ -56,8 +56,8 @@ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) { if (!m_body->isActive()) return false; // Convert the ray int32_to the local-space of the collision shape - const Transform localToWorldTransform = getLocalToWorldTransform(); - const Transform worldToLocalTransform = localToWorldTransform.getInverse(); + const etk::Transform3D localToWorldTransform = getLocalToWorldTransform(); + const etk::Transform3D worldToLocalTransform = localToWorldTransform.getInverse(); Ray rayLocal(worldToLocalTransform * ray.point1, worldToLocalTransform * ray.point2, ray.maxFraction); diff --git a/ephysics/collision/ProxyShape.h b/ephysics/collision/ProxyShape.h index 30d98d5..9633843 100644 --- a/ephysics/collision/ProxyShape.h +++ b/ephysics/collision/ProxyShape.h @@ -34,7 +34,7 @@ class ProxyShape { CollisionShape* m_collisionShape; /// Local-space to parent body-space transform (does not change over time) - Transform m_localToBodyTransform; + etk::Transform3D m_localToBodyTransform; /// Mass (in kilogramms) of the corresponding collision shape float m_mass; @@ -78,7 +78,7 @@ class ProxyShape { /// Constructor ProxyShape(CollisionBody* body, CollisionShape* shape, - const Transform& transform, float mass); + const etk::Transform3D& transform, float mass); /// Destructor virtual ~ProxyShape(); @@ -99,16 +99,16 @@ class ProxyShape { void setUserData(void* userData); /// Return the local to parent body transform - const Transform& getLocalToBodyTransform() const; + const etk::Transform3D& getLocalToBodyTransform() const; /// Set the local to parent body transform - void setLocalToBodyTransform(const Transform& transform); + void setLocalToBodyTransform(const etk::Transform3D& transform); /// Return the local to world transform - const Transform getLocalToWorldTransform() const; + const etk::Transform3D getLocalToWorldTransform() const; /// Return true if a point is inside the collision shape - bool testPointInside(const Vector3& worldPoint); + bool testPointInside(const vec3& worldPoint); /// Raycast method with feedback information bool raycast(const Ray& ray, RaycastInfo& raycastInfo); @@ -135,10 +135,10 @@ class ProxyShape { void** getCachedCollisionData(); /// Return the local scaling vector of the collision shape - Vector3 getLocalScaling() const; + vec3 getLocalScaling() const; /// Set the local scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + virtual void setLocalScaling(const vec3& scaling); // -------------------- Friendship -------------------- // @@ -206,12 +206,12 @@ inline void ProxyShape::setUserData(void* userData) { * @return The transformation that transforms the local-space of the collision shape * to the local-space of the parent body */ -inline const Transform& ProxyShape::getLocalToBodyTransform() const { +inline const etk::Transform3D& ProxyShape::getLocalToBodyTransform() const { return m_localToBodyTransform; } // Set the local to parent body transform -inline void ProxyShape::setLocalToBodyTransform(const Transform& transform) { +inline void ProxyShape::setLocalToBodyTransform(const etk::Transform3D& transform) { m_localToBodyTransform = transform; @@ -226,7 +226,7 @@ inline void ProxyShape::setLocalToBodyTransform(const Transform& transform) { * @return The transformation that transforms the local-space of the collision * shape to the world-space */ -inline const Transform ProxyShape::getLocalToWorldTransform() const { +inline const etk::Transform3D ProxyShape::getLocalToWorldTransform() const { return m_body->m_transform * m_localToBodyTransform; } @@ -282,7 +282,7 @@ inline void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBit /** * @return The local scaling vector */ -inline Vector3 ProxyShape::getLocalScaling() const { +inline vec3 ProxyShape::getLocalScaling() const { return m_collisionShape->getScaling(); } @@ -290,7 +290,7 @@ inline Vector3 ProxyShape::getLocalScaling() const { /** * @param scaling The new local scaling vector */ -inline void ProxyShape::setLocalScaling(const Vector3& scaling) { +inline void ProxyShape::setLocalScaling(const vec3& scaling) { // Set the local scaling of the collision shape m_collisionShape->setLocalScaling(scaling); diff --git a/ephysics/collision/RaycastInfo.h b/ephysics/collision/RaycastInfo.h index 93d7c85..76a75fb 100644 --- a/ephysics/collision/RaycastInfo.h +++ b/ephysics/collision/RaycastInfo.h @@ -6,7 +6,7 @@ #pragma once // Libraries -#include +#include #include /// ReactPhysics3D namespace @@ -38,10 +38,10 @@ struct RaycastInfo { // -------------------- Attributes -------------------- // /// Hit point in world-space coordinates - Vector3 worldPoint; + vec3 worldPoint; /// Surface normal at hit point in world-space coordinates - Vector3 worldNormal; + vec3 worldNormal; /// Fraction distance of the hit point between point1 and point2 of the ray /// The hit point "p" is such that p = point1 + hitFraction * (point2 - point1) diff --git a/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp b/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp index f3d4a98..0acc76d 100644 --- a/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp @@ -124,7 +124,7 @@ void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) { // Notify the broad-phase that a collision shape has moved and need to be updated void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb, - const Vector3& displacement, bool forceReinsert) { + const vec3& displacement, bool forceReinsert) { int32_t broadPhaseID = proxyShape->m_broadPhaseID; diff --git a/ephysics/collision/broadphase/BroadPhaseAlgorithm.h b/ephysics/collision/broadphase/BroadPhaseAlgorithm.h index 115ee8b..6434400 100644 --- a/ephysics/collision/broadphase/BroadPhaseAlgorithm.h +++ b/ephysics/collision/broadphase/BroadPhaseAlgorithm.h @@ -165,7 +165,7 @@ class BroadPhaseAlgorithm { /// Notify the broad-phase that a collision shape has moved and need to be updated void updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb, - const Vector3& displacement, bool forceReinsert = false); + const vec3& displacement, bool forceReinsert = false); /// Add a collision shape in the array of shapes that have moved in the last simulation step /// and that need to be tested again for broad-phase overlapping. diff --git a/ephysics/collision/broadphase/DynamicAABBTree.cpp b/ephysics/collision/broadphase/DynamicAABBTree.cpp index dd8765e..bf6b616 100644 --- a/ephysics/collision/broadphase/DynamicAABBTree.cpp +++ b/ephysics/collision/broadphase/DynamicAABBTree.cpp @@ -16,7 +16,7 @@ using namespace reactphysics3d; const int32_t TreeNode::NULL_TREE_NODE = -1; // Constructor -DynamicAABBTree::DynamicAABBTree(float extraAABBGap) : mExtraAABBGap(extraAABBGap) { +DynamicAABBTree::DynamicAABBTree(float extraAABBGap) : m_extraAABBGap(extraAABBGap) { init(); } @@ -25,36 +25,36 @@ DynamicAABBTree::DynamicAABBTree(float extraAABBGap) : mExtraAABBGap(extraAABBGa DynamicAABBTree::~DynamicAABBTree() { // Free the allocated memory for the nodes - free(mNodes); + free(m_nodes); } // Initialize the tree void DynamicAABBTree::init() { m_rootNodeID = TreeNode::NULL_TREE_NODE; - mNbNodes = 0; - mNbAllocatedNodes = 8; + m_numberNodes = 0; + m_numberAllocatedNodes = 8; // Allocate memory for the nodes of the tree - mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode)); - assert(mNodes); - memset(mNodes, 0, mNbAllocatedNodes * sizeof(TreeNode)); + m_nodes = (TreeNode*) malloc(m_numberAllocatedNodes * sizeof(TreeNode)); + assert(m_nodes); + memset(m_nodes, 0, m_numberAllocatedNodes * sizeof(TreeNode)); // Initialize the allocated nodes - for (int32_t i=0; i 0); - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - assert(mNodes[nodeID].height >= 0); - mNodes[nodeID].nextNodeID = mFreeNodeID; - mNodes[nodeID].height = -1; - mFreeNodeID = nodeID; - mNbNodes--; + assert(m_numberNodes > 0); + assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes); + assert(m_nodes[nodeID].height >= 0); + m_nodes[nodeID].nextNodeID = m_freeNodeID; + m_nodes[nodeID].height = -1; + m_freeNodeID = nodeID; + m_numberNodes--; } // Internally add an object int32_to the tree @@ -115,16 +115,16 @@ int32_t DynamicAABBTree::addObjectInternal(const AABB& aabb) { int32_t nodeID = allocateNode(); // Create the fat aabb to use in the tree - const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap); - mNodes[nodeID].aabb.setMin(aabb.getMin() - gap); - mNodes[nodeID].aabb.setMax(aabb.getMax() + gap); + const vec3 gap(m_extraAABBGap, m_extraAABBGap, m_extraAABBGap); + m_nodes[nodeID].aabb.setMin(aabb.getMin() - gap); + m_nodes[nodeID].aabb.setMax(aabb.getMax() + gap); // Set the height of the node in the tree - mNodes[nodeID].height = 0; + m_nodes[nodeID].height = 0; // Insert the new leaf node in the tree insertLeafNode(nodeID); - assert(mNodes[nodeID].isLeaf()); + assert(m_nodes[nodeID].isLeaf()); assert(nodeID >= 0); @@ -135,8 +135,8 @@ int32_t DynamicAABBTree::addObjectInternal(const AABB& aabb) { // Remove an object from the tree void DynamicAABBTree::removeObject(int32_t nodeID) { - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - assert(mNodes[nodeID].isLeaf()); + assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes); + assert(m_nodes[nodeID].isLeaf()); // Remove the node from the tree removeLeafNode(nodeID); @@ -150,16 +150,16 @@ void DynamicAABBTree::removeObject(int32_t nodeID) { /// argument is the linear velocity of the AABB multiplied by the elapsed time between two /// frames. If the "forceReinsert" parameter is true, we force a removal and reinsertion of the node /// (this can be useful if the shape AABB has become much smaller than the previous one for instance). -bool DynamicAABBTree::updateObject(int32_t nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert) { +bool DynamicAABBTree::updateObject(int32_t nodeID, const AABB& newAABB, const vec3& displacement, bool forceReinsert) { PROFILE("DynamicAABBTree::updateObject()"); - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - assert(mNodes[nodeID].isLeaf()); - assert(mNodes[nodeID].height >= 0); + assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes); + assert(m_nodes[nodeID].isLeaf()); + assert(m_nodes[nodeID].height >= 0); // If the new AABB is still inside the fat AABB of the node - if (!forceReinsert && mNodes[nodeID].aabb.contains(newAABB)) { + if (!forceReinsert && m_nodes[nodeID].aabb.contains(newAABB)) { return false; } @@ -167,32 +167,29 @@ bool DynamicAABBTree::updateObject(int32_t nodeID, const AABB& newAABB, const Ve removeLeafNode(nodeID); // Compute the fat AABB by inflating the AABB with a constant gap - mNodes[nodeID].aabb = newAABB; - const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap); - mNodes[nodeID].aabb.m_minCoordinates -= gap; - mNodes[nodeID].aabb.m_maxCoordinates += gap; + m_nodes[nodeID].aabb = newAABB; + const vec3 gap(m_extraAABBGap, m_extraAABBGap, m_extraAABBGap); + m_nodes[nodeID].aabb.m_minCoordinates -= gap; + m_nodes[nodeID].aabb.m_maxCoordinates += gap; // Inflate the fat AABB in direction of the linear motion of the AABB - if (displacement.x < float(0.0)) { - mNodes[nodeID].aabb.m_minCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x; + if (displacement.x() < 0.0f) { + m_nodes[nodeID].aabb.m_minCoordinates.setX(m_nodes[nodeID].aabb.m_minCoordinates.x() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x()); + } else { + m_nodes[nodeID].aabb.m_maxCoordinates.setY(m_nodes[nodeID].aabb.m_maxCoordinates.x() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x()); } - else { - mNodes[nodeID].aabb.m_maxCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x; + if (displacement.y() < 0.0f) { + m_nodes[nodeID].aabb.m_minCoordinates.setY(m_nodes[nodeID].aabb.m_minCoordinates.y() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y()); + } else { + m_nodes[nodeID].aabb.m_maxCoordinates.setY(m_nodes[nodeID].aabb.m_maxCoordinates.y() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y()); } - if (displacement.y < float(0.0)) { - mNodes[nodeID].aabb.m_minCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y; - } - else { - mNodes[nodeID].aabb.m_maxCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y; - } - if (displacement.z < float(0.0)) { - mNodes[nodeID].aabb.m_minCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z; - } - else { - mNodes[nodeID].aabb.m_maxCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z; + if (displacement.z() < 0.0f) { + m_nodes[nodeID].aabb.m_minCoordinates.setZ(m_nodes[nodeID].aabb.m_minCoordinates.z() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z()); + } else { + m_nodes[nodeID].aabb.m_maxCoordinates.setZ(m_nodes[nodeID].aabb.m_maxCoordinates.z() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z()); } - assert(mNodes[nodeID].aabb.contains(newAABB)); + assert(m_nodes[nodeID].aabb.contains(newAABB)); // Reinsert the node int32_to the tree insertLeafNode(nodeID); @@ -208,24 +205,24 @@ void DynamicAABBTree::insertLeafNode(int32_t nodeID) { // If the tree is empty if (m_rootNodeID == TreeNode::NULL_TREE_NODE) { m_rootNodeID = nodeID; - mNodes[m_rootNodeID].parentID = TreeNode::NULL_TREE_NODE; + m_nodes[m_rootNodeID].parentID = TreeNode::NULL_TREE_NODE; return; } assert(m_rootNodeID != TreeNode::NULL_TREE_NODE); // Find the best sibling node for the new node - AABB newNodeAABB = mNodes[nodeID].aabb; + AABB newNodeAABB = m_nodes[nodeID].aabb; int32_t currentNodeID = m_rootNodeID; - while (!mNodes[currentNodeID].isLeaf()) { + while (!m_nodes[currentNodeID].isLeaf()) { - int32_t leftChild = mNodes[currentNodeID].children[0]; - int32_t rightChild = mNodes[currentNodeID].children[1]; + int32_t leftChild = m_nodes[currentNodeID].children[0]; + int32_t rightChild = m_nodes[currentNodeID].children[1]; // Compute the merged AABB - float volumeAABB = mNodes[currentNodeID].aabb.getVolume(); + float volumeAABB = m_nodes[currentNodeID].aabb.getVolume(); AABB mergedAABBs; - mergedAABBs.mergeTwoAABBs(mNodes[currentNodeID].aabb, newNodeAABB); + mergedAABBs.mergeTwoAABBs(m_nodes[currentNodeID].aabb, newNodeAABB); float mergedVolume = mergedAABBs.getVolume(); // Compute the cost of making the current node the sibbling of the new node @@ -237,24 +234,24 @@ void DynamicAABBTree::insertLeafNode(int32_t nodeID) { // Compute the cost of descending int32_to the left child float costLeft; AABB currentAndLeftAABB; - currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, mNodes[leftChild].aabb); - if (mNodes[leftChild].isLeaf()) { // If the left child is a leaf + currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, m_nodes[leftChild].aabb); + if (m_nodes[leftChild].isLeaf()) { // If the left child is a leaf costLeft = currentAndLeftAABB.getVolume() + costI; } else { - float leftChildVolume = mNodes[leftChild].aabb.getVolume(); + float leftChildVolume = m_nodes[leftChild].aabb.getVolume(); costLeft = costI + currentAndLeftAABB.getVolume() - leftChildVolume; } // Compute the cost of descending int32_to the right child float costRight; AABB currentAndRightAABB; - currentAndRightAABB.mergeTwoAABBs(newNodeAABB, mNodes[rightChild].aabb); - if (mNodes[rightChild].isLeaf()) { // If the right child is a leaf + currentAndRightAABB.mergeTwoAABBs(newNodeAABB, m_nodes[rightChild].aabb); + if (m_nodes[rightChild].isLeaf()) { // If the right child is a leaf costRight = currentAndRightAABB.getVolume() + costI; } else { - float rightChildVolume = mNodes[rightChild].aabb.getVolume(); + float rightChildVolume = m_nodes[rightChild].aabb.getVolume(); costRight = costI + currentAndRightAABB.getVolume() - rightChildVolume; } @@ -274,69 +271,69 @@ void DynamicAABBTree::insertLeafNode(int32_t nodeID) { int32_t siblingNode = currentNodeID; // Create a new parent for the new node and the sibling node - int32_t oldParentNode = mNodes[siblingNode].parentID; + int32_t oldParentNode = m_nodes[siblingNode].parentID; int32_t newParentNode = allocateNode(); - mNodes[newParentNode].parentID = oldParentNode; - mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB); - mNodes[newParentNode].height = mNodes[siblingNode].height + 1; - assert(mNodes[newParentNode].height > 0); + m_nodes[newParentNode].parentID = oldParentNode; + m_nodes[newParentNode].aabb.mergeTwoAABBs(m_nodes[siblingNode].aabb, newNodeAABB); + m_nodes[newParentNode].height = m_nodes[siblingNode].height + 1; + assert(m_nodes[newParentNode].height > 0); // If the sibling node was not the root node if (oldParentNode != TreeNode::NULL_TREE_NODE) { - assert(!mNodes[oldParentNode].isLeaf()); - if (mNodes[oldParentNode].children[0] == siblingNode) { - mNodes[oldParentNode].children[0] = newParentNode; + assert(!m_nodes[oldParentNode].isLeaf()); + if (m_nodes[oldParentNode].children[0] == siblingNode) { + m_nodes[oldParentNode].children[0] = newParentNode; } else { - mNodes[oldParentNode].children[1] = newParentNode; + m_nodes[oldParentNode].children[1] = newParentNode; } - mNodes[newParentNode].children[0] = siblingNode; - mNodes[newParentNode].children[1] = nodeID; - mNodes[siblingNode].parentID = newParentNode; - mNodes[nodeID].parentID = newParentNode; + m_nodes[newParentNode].children[0] = siblingNode; + m_nodes[newParentNode].children[1] = nodeID; + m_nodes[siblingNode].parentID = newParentNode; + m_nodes[nodeID].parentID = newParentNode; } else { // If the sibling node was the root node - mNodes[newParentNode].children[0] = siblingNode; - mNodes[newParentNode].children[1] = nodeID; - mNodes[siblingNode].parentID = newParentNode; - mNodes[nodeID].parentID = newParentNode; + m_nodes[newParentNode].children[0] = siblingNode; + m_nodes[newParentNode].children[1] = nodeID; + m_nodes[siblingNode].parentID = newParentNode; + m_nodes[nodeID].parentID = newParentNode; m_rootNodeID = newParentNode; } // Move up in the tree to change the AABBs that have changed - currentNodeID = mNodes[nodeID].parentID; - assert(!mNodes[currentNodeID].isLeaf()); + currentNodeID = m_nodes[nodeID].parentID; + assert(!m_nodes[currentNodeID].isLeaf()); while (currentNodeID != TreeNode::NULL_TREE_NODE) { // Balance the sub-tree of the current node if it is not balanced currentNodeID = balanceSubTreeAtNode(currentNodeID); - assert(mNodes[nodeID].isLeaf()); + assert(m_nodes[nodeID].isLeaf()); - assert(!mNodes[currentNodeID].isLeaf()); - int32_t leftChild = mNodes[currentNodeID].children[0]; - int32_t rightChild = mNodes[currentNodeID].children[1]; + assert(!m_nodes[currentNodeID].isLeaf()); + int32_t leftChild = m_nodes[currentNodeID].children[0]; + int32_t rightChild = m_nodes[currentNodeID].children[1]; assert(leftChild != TreeNode::NULL_TREE_NODE); assert(rightChild != TreeNode::NULL_TREE_NODE); // Recompute the height of the node in the tree - mNodes[currentNodeID].height = std::max(mNodes[leftChild].height, - mNodes[rightChild].height) + 1; - assert(mNodes[currentNodeID].height > 0); + m_nodes[currentNodeID].height = std::max(m_nodes[leftChild].height, + m_nodes[rightChild].height) + 1; + assert(m_nodes[currentNodeID].height > 0); // Recompute the AABB of the node - mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb); + m_nodes[currentNodeID].aabb.mergeTwoAABBs(m_nodes[leftChild].aabb, m_nodes[rightChild].aabb); - currentNodeID = mNodes[currentNodeID].parentID; + currentNodeID = m_nodes[currentNodeID].parentID; } - assert(mNodes[nodeID].isLeaf()); + assert(m_nodes[nodeID].isLeaf()); } // Remove a leaf node from the tree void DynamicAABBTree::removeLeafNode(int32_t nodeID) { - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - assert(mNodes[nodeID].isLeaf()); + assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes); + assert(m_nodes[nodeID].isLeaf()); // If we are removing the root node (root node is a leaf in this case) if (m_rootNodeID == nodeID) { @@ -344,28 +341,28 @@ void DynamicAABBTree::removeLeafNode(int32_t nodeID) { return; } - int32_t parentNodeID = mNodes[nodeID].parentID; - int32_t grandParentNodeID = mNodes[parentNodeID].parentID; + int32_t parentNodeID = m_nodes[nodeID].parentID; + int32_t grandParentNodeID = m_nodes[parentNodeID].parentID; int32_t siblingNodeID; - if (mNodes[parentNodeID].children[0] == nodeID) { - siblingNodeID = mNodes[parentNodeID].children[1]; + if (m_nodes[parentNodeID].children[0] == nodeID) { + siblingNodeID = m_nodes[parentNodeID].children[1]; } else { - siblingNodeID = mNodes[parentNodeID].children[0]; + siblingNodeID = m_nodes[parentNodeID].children[0]; } // If the parent of the node to remove is not the root node if (grandParentNodeID != TreeNode::NULL_TREE_NODE) { // Destroy the parent node - if (mNodes[grandParentNodeID].children[0] == parentNodeID) { - mNodes[grandParentNodeID].children[0] = siblingNodeID; + if (m_nodes[grandParentNodeID].children[0] == parentNodeID) { + m_nodes[grandParentNodeID].children[0] = siblingNodeID; } else { - assert(mNodes[grandParentNodeID].children[1] == parentNodeID); - mNodes[grandParentNodeID].children[1] = siblingNodeID; + assert(m_nodes[grandParentNodeID].children[1] == parentNodeID); + m_nodes[grandParentNodeID].children[1] = siblingNodeID; } - mNodes[siblingNodeID].parentID = grandParentNodeID; + m_nodes[siblingNodeID].parentID = grandParentNodeID; releaseNode(parentNodeID); // Now, we need to recompute the AABBs of the node on the path back to the root @@ -376,27 +373,27 @@ void DynamicAABBTree::removeLeafNode(int32_t nodeID) { // Balance the current sub-tree if necessary currentNodeID = balanceSubTreeAtNode(currentNodeID); - assert(!mNodes[currentNodeID].isLeaf()); + assert(!m_nodes[currentNodeID].isLeaf()); // Get the two children of the current node - int32_t leftChildID = mNodes[currentNodeID].children[0]; - int32_t rightChildID = mNodes[currentNodeID].children[1]; + int32_t leftChildID = m_nodes[currentNodeID].children[0]; + int32_t rightChildID = m_nodes[currentNodeID].children[1]; // Recompute the AABB and the height of the current node - mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChildID].aabb, - mNodes[rightChildID].aabb); - mNodes[currentNodeID].height = std::max(mNodes[leftChildID].height, - mNodes[rightChildID].height) + 1; - assert(mNodes[currentNodeID].height > 0); + m_nodes[currentNodeID].aabb.mergeTwoAABBs(m_nodes[leftChildID].aabb, + m_nodes[rightChildID].aabb); + m_nodes[currentNodeID].height = std::max(m_nodes[leftChildID].height, + m_nodes[rightChildID].height) + 1; + assert(m_nodes[currentNodeID].height > 0); - currentNodeID = mNodes[currentNodeID].parentID; + currentNodeID = m_nodes[currentNodeID].parentID; } } else { // If the parent of the node to remove is the root node // The sibling node becomes the new root node m_rootNodeID = siblingNodeID; - mNodes[siblingNodeID].parentID = TreeNode::NULL_TREE_NODE; + m_nodes[siblingNodeID].parentID = TreeNode::NULL_TREE_NODE; releaseNode(parentNodeID); } } @@ -408,7 +405,7 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) { assert(nodeID != TreeNode::NULL_TREE_NODE); - TreeNode* nodeA = mNodes + nodeID; + TreeNode* nodeA = m_nodes + nodeID; // If the node is a leaf or the height of A's sub-tree is less than 2 if (nodeA->isLeaf() || nodeA->height < 2) { @@ -420,10 +417,10 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) { // Get the two children nodes int32_t nodeBID = nodeA->children[0]; int32_t nodeCID = nodeA->children[1]; - assert(nodeBID >= 0 && nodeBID < mNbAllocatedNodes); - assert(nodeCID >= 0 && nodeCID < mNbAllocatedNodes); - TreeNode* nodeB = mNodes + nodeBID; - TreeNode* nodeC = mNodes + nodeCID; + assert(nodeBID >= 0 && nodeBID < m_numberAllocatedNodes); + assert(nodeCID >= 0 && nodeCID < m_numberAllocatedNodes); + TreeNode* nodeB = m_nodes + nodeBID; + TreeNode* nodeC = m_nodes + nodeCID; // Compute the factor of the left and right sub-trees int32_t balanceFactor = nodeC->height - nodeB->height; @@ -435,10 +432,10 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) { int32_t nodeFID = nodeC->children[0]; int32_t nodeGID = nodeC->children[1]; - assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes); - assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes); - TreeNode* nodeF = mNodes + nodeFID; - TreeNode* nodeG = mNodes + nodeGID; + assert(nodeFID >= 0 && nodeFID < m_numberAllocatedNodes); + assert(nodeGID >= 0 && nodeGID < m_numberAllocatedNodes); + TreeNode* nodeF = m_nodes + nodeFID; + TreeNode* nodeG = m_nodes + nodeGID; nodeC->children[0] = nodeID; nodeC->parentID = nodeA->parentID; @@ -446,12 +443,12 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) { if (nodeC->parentID != TreeNode::NULL_TREE_NODE) { - if (mNodes[nodeC->parentID].children[0] == nodeID) { - mNodes[nodeC->parentID].children[0] = nodeCID; + if (m_nodes[nodeC->parentID].children[0] == nodeID) { + m_nodes[nodeC->parentID].children[0] = nodeCID; } else { - assert(mNodes[nodeC->parentID].children[1] == nodeID); - mNodes[nodeC->parentID].children[1] = nodeCID; + assert(m_nodes[nodeC->parentID].children[1] == nodeID); + m_nodes[nodeC->parentID].children[1] = nodeCID; } } else { @@ -505,10 +502,10 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) { int32_t nodeFID = nodeB->children[0]; int32_t nodeGID = nodeB->children[1]; - assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes); - assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes); - TreeNode* nodeF = mNodes + nodeFID; - TreeNode* nodeG = mNodes + nodeGID; + assert(nodeFID >= 0 && nodeFID < m_numberAllocatedNodes); + assert(nodeGID >= 0 && nodeGID < m_numberAllocatedNodes); + TreeNode* nodeF = m_nodes + nodeFID; + TreeNode* nodeG = m_nodes + nodeGID; nodeB->children[0] = nodeID; nodeB->parentID = nodeA->parentID; @@ -516,12 +513,12 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) { if (nodeB->parentID != TreeNode::NULL_TREE_NODE) { - if (mNodes[nodeB->parentID].children[0] == nodeID) { - mNodes[nodeB->parentID].children[0] = nodeBID; + if (m_nodes[nodeB->parentID].children[0] == nodeID) { + m_nodes[nodeB->parentID].children[0] = nodeBID; } else { - assert(mNodes[nodeB->parentID].children[1] == nodeID); - mNodes[nodeB->parentID].children[1] = nodeBID; + assert(m_nodes[nodeB->parentID].children[1] == nodeID); + m_nodes[nodeB->parentID].children[1] = nodeBID; } } else { @@ -590,7 +587,7 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) continue; // Get the corresponding node - const TreeNode* nodeToVisit = mNodes + nodeIDToVisit; + const TreeNode* nodeToVisit = m_nodes + nodeIDToVisit; // If the AABB in parameter overlaps with the AABB of the node to visit if (aabb.testCollision(nodeToVisit->aabb)) { @@ -632,7 +629,7 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &ca if (nodeID == TreeNode::NULL_TREE_NODE) continue; // Get the corresponding node - const TreeNode* node = mNodes + nodeID; + const TreeNode* node = m_nodes + nodeID; Ray rayTemp(ray.point1, ray.point2, maxFraction); @@ -647,12 +644,12 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &ca // If the user returned a hitFraction of zero, it means that // the raycasting should stop here - if (hitFraction == float(0.0)) { + if (hitFraction == 0.0f) { return; } // If the user returned a positive fraction - if (hitFraction > float(0.0)) { + if (hitFraction > 0.0f) { // We update the maxFraction value and the ray // AABB using the new maximum fraction @@ -682,16 +679,16 @@ void DynamicAABBTree::check() const { checkNode(m_rootNodeID); int32_t nbFreeNodes = 0; - int32_t freeNodeID = mFreeNodeID; + int32_t freeNodeID = m_freeNodeID; // Check the free nodes while(freeNodeID != TreeNode::NULL_TREE_NODE) { - assert(0 <= freeNodeID && freeNodeID < mNbAllocatedNodes); - freeNodeID = mNodes[freeNodeID].nextNodeID; + assert(0 <= freeNodeID && freeNodeID < m_numberAllocatedNodes); + freeNodeID = m_nodes[freeNodeID].nextNodeID; nbFreeNodes++; } - assert(mNbNodes + nbFreeNodes == mNbAllocatedNodes); + assert(m_numberNodes + nbFreeNodes == m_numberAllocatedNodes); } // Check if the node structure is valid (for debugging purpose) @@ -701,11 +698,11 @@ void DynamicAABBTree::checkNode(int32_t nodeID) const { // If it is the root if (nodeID == m_rootNodeID) { - assert(mNodes[nodeID].parentID == TreeNode::NULL_TREE_NODE); + assert(m_nodes[nodeID].parentID == TreeNode::NULL_TREE_NODE); } // Get the children nodes - TreeNode* pNode = mNodes + nodeID; + TreeNode* pNode = m_nodes + nodeID; assert(!pNode->isLeaf()); int32_t leftChild = pNode->children[0]; int32_t rightChild = pNode->children[1]; @@ -724,22 +721,22 @@ void DynamicAABBTree::checkNode(int32_t nodeID) const { else { // Check that the children node IDs are valid - assert(0 <= leftChild && leftChild < mNbAllocatedNodes); - assert(0 <= rightChild && rightChild < mNbAllocatedNodes); + assert(0 <= leftChild && leftChild < m_numberAllocatedNodes); + assert(0 <= rightChild && rightChild < m_numberAllocatedNodes); // Check that the children nodes have the correct parent node - assert(mNodes[leftChild].parentID == nodeID); - assert(mNodes[rightChild].parentID == nodeID); + assert(m_nodes[leftChild].parentID == nodeID); + assert(m_nodes[rightChild].parentID == nodeID); // Check the height of node - int32_t height = 1 + std::max(mNodes[leftChild].height, mNodes[rightChild].height); - assert(mNodes[nodeID].height == height); + int32_t height = 1 + std::max(m_nodes[leftChild].height, m_nodes[rightChild].height); + assert(m_nodes[nodeID].height == height); // Check the AABB of the node AABB aabb; - aabb.mergeTwoAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb); - assert(aabb.getMin() == mNodes[nodeID].aabb.getMin()); - assert(aabb.getMax() == mNodes[nodeID].aabb.getMax()); + aabb.mergeTwoAABBs(m_nodes[leftChild].aabb, m_nodes[rightChild].aabb); + assert(aabb.getMin() == m_nodes[nodeID].aabb.getMin()); + assert(aabb.getMax() == m_nodes[nodeID].aabb.getMax()); // Recursively check the children nodes checkNode(leftChild); @@ -754,8 +751,8 @@ int32_t DynamicAABBTree::computeHeight() { // Compute the height of a given node in the tree int32_t DynamicAABBTree::computeHeight(int32_t nodeID) { - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - TreeNode* node = mNodes + nodeID; + assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes); + TreeNode* node = m_nodes + nodeID; // If the node is a leaf, its height is zero if (node->isLeaf()) { diff --git a/ephysics/collision/broadphase/DynamicAABBTree.h b/ephysics/collision/broadphase/DynamicAABBTree.h index 44163d2..a7b4a95 100644 --- a/ephysics/collision/broadphase/DynamicAABBTree.h +++ b/ephysics/collision/broadphase/DynamicAABBTree.h @@ -113,23 +113,23 @@ class DynamicAABBTree { // -------------------- Attributes -------------------- // /// Pointer to the memory location of the nodes of the tree - TreeNode* mNodes; + TreeNode* m_nodes; /// ID of the root node of the tree int32_t m_rootNodeID; /// ID of the first node of the list of free (allocated) nodes in the tree that we can use - int32_t mFreeNodeID; + int32_t m_freeNodeID; /// Number of allocated nodes in the tree - int32_t mNbAllocatedNodes; + int32_t m_numberAllocatedNodes; /// Number of nodes in the tree - int32_t mNbNodes; + int32_t m_numberNodes; /// Extra AABB Gap used to allow the collision shape to move a little bit /// without triggering a large modification of the tree which can be costly - float mExtraAABBGap; + float m_extraAABBGap; // -------------------- Methods -------------------- // @@ -172,7 +172,7 @@ class DynamicAABBTree { // -------------------- Methods -------------------- // /// Constructor - DynamicAABBTree(float extraAABBGap = float(0.0)); + DynamicAABBTree(float extraAABBGap = 0.0f); /// Destructor virtual ~DynamicAABBTree(); @@ -187,7 +187,7 @@ class DynamicAABBTree { void removeObject(int32_t nodeID); /// Update the dynamic tree after an object has moved. - bool updateObject(int32_t nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert = false); + bool updateObject(int32_t nodeID, const AABB& newAABB, const vec3& displacement, bool forceReinsert = false); /// Return the fat AABB corresponding to a given node ID const AABB& getFatAABB(int32_t nodeID) const; @@ -222,22 +222,22 @@ inline bool TreeNode::isLeaf() const { // Return the fat AABB corresponding to a given node ID inline const AABB& DynamicAABBTree::getFatAABB(int32_t nodeID) const { - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - return mNodes[nodeID].aabb; + assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes); + return m_nodes[nodeID].aabb; } // Return the pointer to the data array of a given leaf node of the tree inline int32_t* DynamicAABBTree::getNodeDataInt(int32_t nodeID) const { - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - assert(mNodes[nodeID].isLeaf()); - return mNodes[nodeID].dataInt; + assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes); + assert(m_nodes[nodeID].isLeaf()); + return m_nodes[nodeID].dataInt; } // Return the pointer to the data pointer of a given leaf node of the tree inline void* DynamicAABBTree::getNodeDataPointer(int32_t nodeID) const { - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - assert(mNodes[nodeID].isLeaf()); - return mNodes[nodeID].dataPointer; + assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes); + assert(m_nodes[nodeID].isLeaf()); + return m_nodes[nodeID].dataPointer; } // Return the root AABB of the tree @@ -251,8 +251,8 @@ inline int32_t DynamicAABBTree::addObject(const AABB& aabb, int32_t data1, int32 int32_t nodeId = addObjectInternal(aabb); - mNodes[nodeId].dataInt[0] = data1; - mNodes[nodeId].dataInt[1] = data2; + m_nodes[nodeId].dataInt[0] = data1; + m_nodes[nodeId].dataInt[1] = data2; return nodeId; } @@ -263,7 +263,7 @@ inline int32_t DynamicAABBTree::addObject(const AABB& aabb, void* data) { int32_t nodeId = addObjectInternal(aabb); - mNodes[nodeId].dataPointer = data; + m_nodes[nodeId].dataPointer = data; return nodeId; } diff --git a/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp b/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp index 18205d7..3b60766 100644 --- a/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp +++ b/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp @@ -85,7 +85,7 @@ void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Inf } // Test collision between a triangle and the convex mesh shape -void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) { +void ConvexVsTriangleCallback::testTriangle(const vec3* trianglePoints) { // Create a triangle collision shape float margin = mConcaveShape->getTriangleMargin(); @@ -120,7 +120,7 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl NarrowPhaseCallback* narrowPhaseCallback) { // Set with the triangle vertices already processed to void further contacts with same triangle - std::unordered_multimap processTriangleVertices; + std::unordered_multimap processTriangleVertices; // Sort the list of narrow-phase contacts according to their penetration depth std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare()); @@ -130,7 +130,7 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { const SmoothMeshContactInfo info = *it; - const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; + const vec3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; // Compute the barycentric coordinates of the point in the triangle float u, v, w; @@ -149,7 +149,7 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl // If it is a vertex contact if (nbZeros == 2) { - Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]); + vec3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]); // Check that this triangle vertex has not been processed yet if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) { @@ -160,8 +160,8 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl } else if (nbZeros == 1) { // If it is an edge contact - Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]); - Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]); + vec3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]); + vec3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]); // Check that this triangle edge has not been processed yet if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) && @@ -188,12 +188,12 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl } // We use the triangle normal as the contact normal - Vector3 a = info.triangleVertices[1] - info.triangleVertices[0]; - Vector3 b = info.triangleVertices[2] - info.triangleVertices[0]; - Vector3 localNormal = a.cross(b); + vec3 a = info.triangleVertices[1] - info.triangleVertices[0]; + vec3 b = info.triangleVertices[2] - info.triangleVertices[0]; + vec3 localNormal = a.cross(b); newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal; - Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; - Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint; + vec3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; + vec3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint; newContactInfo.normal.normalize(); if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) { newContactInfo.normal = -newContactInfo.normal; @@ -202,13 +202,13 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl // We recompute the contact point on the second body with the new normal as described in // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and // Dirk Gregorius) to avoid adding torque - Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse(); + etk::Transform3D worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse(); if (info.isFirstShapeTriangle) { - Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal; + vec3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal; newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint; } else { - Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal; + vec3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal; newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint; } @@ -225,13 +225,13 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl } // Return true if the vertex is in the set of already processed vertices -bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap& processTriangleVertices, const Vector3& vertex) const { +bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap& processTriangleVertices, const vec3& vertex) const { - int32_t key = int32_t(vertex.x * vertex.y * vertex.z); + int32_t key = int32_t(vertex.x() * vertex.y() * vertex.z()); auto range = processTriangleVertices.equal_range(key); for (auto it = range.first; it != range.second; ++it) { - if (vertex.x == it->second.x && vertex.y == it->second.y && vertex.z == it->second.z) return true; + if (vertex.x() == it->second.x() && vertex.y() == it->second.y() && vertex.z() == it->second.z()) return true; } return false; @@ -240,7 +240,7 @@ bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multi // Called by a narrow-phase collision algorithm when a new contact has been found void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) { - Vector3 triangleVertices[3]; + vec3 triangleVertices[3]; bool isFirstShapeTriangle; // If the collision shape 1 is the triangle diff --git a/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.h b/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.h index b42cccc..af0bedc 100644 --- a/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.h +++ b/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.h @@ -83,7 +83,7 @@ class ConvexVsTriangleCallback : public TriangleCallback { } /// Test collision between a triangle and the convex mesh shape - virtual void testTriangle(const Vector3* trianglePoints); + virtual void testTriangle(const vec3* trianglePoints); }; // Class SmoothMeshContactInfo @@ -97,11 +97,11 @@ class SmoothMeshContactInfo { ContactPointInfo contactInfo; bool isFirstShapeTriangle; - Vector3 triangleVertices[3]; + vec3 triangleVertices[3]; /// Constructor - SmoothMeshContactInfo(const ContactPointInfo& contact, bool firstShapeTriangle, const Vector3& trianglePoint1, - const Vector3& trianglePoint2, const Vector3& trianglePoint3) + SmoothMeshContactInfo(const ContactPointInfo& contact, bool firstShapeTriangle, const vec3& trianglePoint1, + const vec3& trianglePoint2, const vec3& trianglePoint3) : contactInfo(contact) { isFirstShapeTriangle = firstShapeTriangle; triangleVertices[0] = trianglePoint1; @@ -179,12 +179,12 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm { NarrowPhaseCallback* narrowPhaseCallback); /// Add a triangle vertex int32_to the set of processed triangles - void addProcessedVertex(std::unordered_multimap& processTriangleVertices, - const Vector3& vertex); + void addProcessedVertex(std::unordered_multimap& processTriangleVertices, + const vec3& vertex); /// Return true if the vertex is in the set of already processed vertices - bool hasVertexBeenProcessed(const std::unordered_multimap& processTriangleVertices, - const Vector3& vertex) const; + bool hasVertexBeenProcessed(const std::unordered_multimap& processTriangleVertices, + const vec3& vertex) const; public : @@ -203,8 +203,8 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm { }; // Add a triangle vertex int32_to the set of processed triangles -inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap& processTriangleVertices, const Vector3& vertex) { - processTriangleVertices.insert(std::make_pair(int32_t(vertex.x * vertex.y * vertex.z), vertex)); +inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap& processTriangleVertices, const vec3& vertex) { + processTriangleVertices.insert(std::make_pair(int32_t(vertex.x() * vertex.y() * vertex.z()), vertex)); } } diff --git a/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp b/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp index c3e93f4..43e9282 100644 --- a/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ b/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp @@ -26,29 +26,29 @@ EPAAlgorithm::~EPAAlgorithm() { // Decide if the origin is in the tetrahedron. /// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of /// the vertex that is wrong if the origin is not in the tetrahedron -int32_t EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, - const Vector3& p3, const Vector3& p4) const { +int32_t EPAAlgorithm::isOriginInTetrahedron(const vec3& p1, const vec3& p2, + const vec3& p3, const vec3& p4) const { // Check vertex 1 - Vector3 normal1 = (p2-p1).cross(p3-p1); + vec3 normal1 = (p2-p1).cross(p3-p1); if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) { return 4; } // Check vertex 2 - Vector3 normal2 = (p4-p2).cross(p3-p2); + vec3 normal2 = (p4-p2).cross(p3-p2); if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) { return 1; } // Check vertex 3 - Vector3 normal3 = (p4-p3).cross(p1-p3); + vec3 normal3 = (p4-p3).cross(p1-p3); if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) { return 2; } // Check vertex 4 - Vector3 normal4 = (p2-p4).cross(p1-p4); + vec3 normal4 = (p2-p4).cross(p1-p4); if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) { return 3; } @@ -65,10 +65,10 @@ int32_t EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2 /// the correct penetration depth void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex, CollisionShapeInfo shape1Info, - const Transform& transform1, + const etk::Transform3D& transform1, CollisionShapeInfo shape2Info, - const Transform& transform2, - Vector3& v, + const etk::Transform3D& transform2, + vec3& v, NarrowPhaseCallback* narrowPhaseCallback) { PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()"); @@ -82,20 +82,20 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape2CachedCollisionData = shape2Info.cachedCollisionData; - Vector3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates - Vector3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates - Vector3 points[MAX_SUPPORT_POINTS]; // Current points + vec3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates + vec3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates + vec3 points[MAX_SUPPORT_POINTS]; // Current points TrianglesStore triangleStore; // Store the triangles TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face // candidate of the EPA algorithm - // Transform a point from local space of body 2 to local + // etk::Transform3D a point from local space of body 2 to local // space of body 1 (the GJK algorithm is done in local space of body 1) - Transform body2Tobody1 = transform1.getInverse() * transform2; + etk::Transform3D body2Tobody1 = transform1.getInverse() * transform2; // Matrix that transform a direction from local // space of body 1 int32_to local space of body 2 - Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * + etk::Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * transform1.getOrientation(); // Get the simplex computed previously by the GJK algorithm @@ -130,22 +130,22 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple // v1, v2 and v3. // Direction of the segment - Vector3 d = (points[1] - points[0]).getUnit(); + vec3 d = (points[1] - points[0]).safeNormalized(); // Choose the coordinate axis from the minimal absolute component of the vector d - int32_t minAxis = d.getAbsoluteVector().getMinAxis(); + int32_t minAxis = d.absolute().getMinAxis(); // Compute sin(60) - const float sin60 = float(sqrt(3.0)) * float(0.5); + const float sin60 = float(sqrt(3.0)) * 0.5f; // Create a rotation quaternion to rotate the vector v1 to get the vectors // v2 and v3 - Quaternion rotationQuat(d.x * sin60, d.y * sin60, d.z * sin60, 0.5); + etk::Quaternion rotationQuat(d.x() * sin60, d.y() * sin60, d.z() * sin60, 0.5); // Compute the vector v1, v2, v3 - Vector3 v1 = d.cross(Vector3(minAxis == 0, minAxis == 1, minAxis == 2)); - Vector3 v2 = rotationQuat * v1; - Vector3 v3 = rotationQuat * v2; + vec3 v1 = d.cross(vec3(minAxis == 0, minAxis == 1, minAxis == 2)); + vec3 v2 = rotationQuat * v1; + vec3 v3 = rotationQuat * v2; // Compute the support point in the direction of v1 suppPointsA[2] = shape1->getLocalSupportPointWithMargin(v1, shape1CachedCollisionData); @@ -256,9 +256,9 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple // tetrahedron that contains the origin. // Compute the normal of the triangle - Vector3 v1 = points[1] - points[0]; - Vector3 v2 = points[2] - points[0]; - Vector3 n = v1.cross(v2); + vec3 v1 = points[1] - points[0]; + vec3 v2 = points[2] - points[0]; + vec3 n = v1.cross(v2); // Compute the two new vertices to obtain a hexahedron suppPointsA[3] = shape1->getLocalSupportPointWithMargin(n, shape1CachedCollisionData); @@ -404,13 +404,13 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple // Compute the contact info v = transform1.getOrientation() * triangle->getClosestPoint(); - Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA); - Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB); - Vector3 normal = v.getUnit(); + vec3 pALocal = triangle->computeClosestPointOfObject(suppPointsA); + vec3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB); + vec3 normal = v.safeNormalized(); float penetrationDepth = v.length(); assert(penetrationDepth > 0.0); - if (normal.lengthSquare() < MACHINE_EPSILON) return; + if (normal.length2() < MACHINE_EPSILON) return; // Create the contact info object ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, diff --git a/ephysics/collision/narrowphase/EPA/EPAAlgorithm.h b/ephysics/collision/narrowphase/EPA/EPAAlgorithm.h index ad7e07a..e773b1b 100644 --- a/ephysics/collision/narrowphase/EPA/EPAAlgorithm.h +++ b/ephysics/collision/narrowphase/EPA/EPAAlgorithm.h @@ -85,8 +85,8 @@ class EPAAlgorithm { float upperBoundSquarePenDepth); /// Decide if the origin is in the tetrahedron. - int32_t isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, - const Vector3& p3, const Vector3& p4) const; + int32_t isOriginInTetrahedron(const vec3& p1, const vec3& p2, + const vec3& p3, const vec3& p4) const; public: @@ -104,10 +104,10 @@ class EPAAlgorithm { /// Compute the penetration depth with EPA algorithm. void computePenetrationDepthAndContactPoints(const Simplex& simplex, CollisionShapeInfo shape1Info, - const Transform& transform1, + const etk::Transform3D& transform1, CollisionShapeInfo shape2Info, - const Transform& transform2, - Vector3& v, + const etk::Transform3D& transform2, + vec3& v, NarrowPhaseCallback* narrowPhaseCallback); }; diff --git a/ephysics/collision/narrowphase/EPA/EdgeEPA.cpp b/ephysics/collision/narrowphase/EPA/EdgeEPA.cpp index 1dd2fdc..fe1d704 100644 --- a/ephysics/collision/narrowphase/EPA/EdgeEPA.cpp +++ b/ephysics/collision/narrowphase/EPA/EdgeEPA.cpp @@ -47,7 +47,7 @@ uint32_t EdgeEPA::getTargetVertexIndex() const { } // Execute the recursive silhouette algorithm from this edge -bool EdgeEPA::computeSilhouette(const Vector3* vertices, uint32_t indexNewVertex, +bool EdgeEPA::computeSilhouette(const vec3* vertices, uint32_t indexNewVertex, TrianglesStore& triangleStore) { // If the edge has not already been visited if (!mOwnerTriangle->getIsObsolete()) { diff --git a/ephysics/collision/narrowphase/EPA/EdgeEPA.h b/ephysics/collision/narrowphase/EPA/EdgeEPA.h index fb42910..da001d2 100644 --- a/ephysics/collision/narrowphase/EPA/EdgeEPA.h +++ b/ephysics/collision/narrowphase/EPA/EdgeEPA.h @@ -62,7 +62,7 @@ class EdgeEPA { uint32_t getTargetVertexIndex() const; /// Execute the recursive silhouette algorithm from this edge - bool computeSilhouette(const Vector3* vertices, uint32_t index, TrianglesStore& triangleStore); + bool computeSilhouette(const vec3* vertices, uint32_t index, TrianglesStore& triangleStore); /// Assignment operator EdgeEPA& operator=(const EdgeEPA& edge); diff --git a/ephysics/collision/narrowphase/EPA/TriangleEPA.cpp b/ephysics/collision/narrowphase/EPA/TriangleEPA.cpp index d87825f..d75e437 100644 --- a/ephysics/collision/narrowphase/EPA/TriangleEPA.cpp +++ b/ephysics/collision/narrowphase/EPA/TriangleEPA.cpp @@ -31,11 +31,11 @@ TriangleEPA::~TriangleEPA() { } // Compute the point v closest to the origin of this triangle -bool TriangleEPA::computeClosestPoint(const Vector3* vertices) { - const Vector3& p0 = vertices[mIndicesVertices[0]]; +bool TriangleEPA::computeClosestPoint(const vec3* vertices) { + const vec3& p0 = vertices[mIndicesVertices[0]]; - Vector3 v1 = vertices[mIndicesVertices[1]] - p0; - Vector3 v2 = vertices[mIndicesVertices[2]] - p0; + vec3 v1 = vertices[mIndicesVertices[1]] - p0; + vec3 v2 = vertices[mIndicesVertices[2]] - p0; float v1Dotv1 = v1.dot(v1); float v1Dotv2 = v1.dot(v2); float v2Dotv2 = v2.dot(v2); @@ -52,7 +52,7 @@ bool TriangleEPA::computeClosestPoint(const Vector3* vertices) { // If the determinant is positive if (mDet > 0.0) { // Compute the closest point v - mClosestPoint = p0 + float(1.0) / mDet * (mLambda1 * v1 + mLambda2 * v2); + mClosestPoint = p0 + 1.0f / mDet * (mLambda1 * v1 + mLambda2 * v2); // Compute the square distance of closest point to the origin mDistSquare = mClosestPoint.dot(mClosestPoint); @@ -101,7 +101,7 @@ void reactphysics3d::halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1) { /// face from the new vertex, computes the silhouette and create the new faces from the new vertex in /// order that we always have a convex polytope. The faces visible from the new vertex are set /// obselete and will not be considered as being a candidate face in the future. -bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint32_t indexNewVertex, +bool TriangleEPA::computeSilhouette(const vec3* vertices, uint32_t indexNewVertex, TrianglesStore& triangleStore) { uint32_t first = triangleStore.getNbTriangles(); diff --git a/ephysics/collision/narrowphase/EPA/TriangleEPA.h b/ephysics/collision/narrowphase/EPA/TriangleEPA.h index 773469e..119e7e8 100644 --- a/ephysics/collision/narrowphase/EPA/TriangleEPA.h +++ b/ephysics/collision/narrowphase/EPA/TriangleEPA.h @@ -42,7 +42,7 @@ class TriangleEPA { float mDet; /// Point v closest to the origin on the affine hull of the triangle - Vector3 mClosestPoint; + vec3 mClosestPoint; /// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 float mLambda1; @@ -90,22 +90,22 @@ class TriangleEPA { bool getIsObsolete() const; /// Return the point closest to the origin - const Vector3& getClosestPoint() const; + const vec3& getClosestPoint() const; // Return true if the closest point on affine hull is inside the triangle bool isClosestPointInternalToTriangle() const; /// Return true if the triangle is visible from a given vertex - bool isVisibleFromVertex(const Vector3* vertices, uint32_t index) const; + bool isVisibleFromVertex(const vec3* vertices, uint32_t index) const; /// Compute the point v closest to the origin of this triangle - bool computeClosestPoint(const Vector3* vertices); + bool computeClosestPoint(const vec3* vertices); /// Compute the point of an object closest to the origin - Vector3 computeClosestPointOfObject(const Vector3* supportPointsOfObject) const; + vec3 computeClosestPointOfObject(const vec3* supportPointsOfObject) const; /// Execute the recursive silhouette algorithm from this triangle face. - bool computeSilhouette(const Vector3* vertices, uint32_t index, TrianglesStore& triangleStore); + bool computeSilhouette(const vec3* vertices, uint32_t index, TrianglesStore& triangleStore); /// Access operator uint32_t operator[](int32_t i) const; @@ -145,7 +145,7 @@ inline bool TriangleEPA::getIsObsolete() const { } // Return the point closest to the origin -inline const Vector3& TriangleEPA::getClosestPoint() const { +inline const vec3& TriangleEPA::getClosestPoint() const { return mClosestPoint; } @@ -155,15 +155,15 @@ inline bool TriangleEPA::isClosestPointInternalToTriangle() const { } // Return true if the triangle is visible from a given vertex -inline bool TriangleEPA::isVisibleFromVertex(const Vector3* vertices, uint32_t index) const { - Vector3 closestToVert = vertices[index] - mClosestPoint; +inline bool TriangleEPA::isVisibleFromVertex(const vec3* vertices, uint32_t index) const { + vec3 closestToVert = vertices[index] - mClosestPoint; return (mClosestPoint.dot(closestToVert) > 0.0); } // Compute the point of an object closest to the origin -inline Vector3 TriangleEPA::computeClosestPointOfObject(const Vector3* supportPointsOfObject) const{ - const Vector3& p0 = supportPointsOfObject[mIndicesVertices[0]]; - return p0 + float(1.0)/mDet * (mLambda1 * (supportPointsOfObject[mIndicesVertices[1]] - p0) + +inline vec3 TriangleEPA::computeClosestPointOfObject(const vec3* supportPointsOfObject) const{ + const vec3& p0 = supportPointsOfObject[mIndicesVertices[0]]; + return p0 + 1.0f/mDet * (mLambda1 * (supportPointsOfObject[mIndicesVertices[1]] - p0) + mLambda2 * (supportPointsOfObject[mIndicesVertices[2]] - p0)); } diff --git a/ephysics/collision/narrowphase/EPA/TrianglesStore.h b/ephysics/collision/narrowphase/EPA/TrianglesStore.h index 0dd5d1e..6dbaa6d 100644 --- a/ephysics/collision/narrowphase/EPA/TrianglesStore.h +++ b/ephysics/collision/narrowphase/EPA/TrianglesStore.h @@ -64,7 +64,7 @@ class TrianglesStore { TriangleEPA& last(); /// Create a new triangle - TriangleEPA* newTriangle(const Vector3* vertices, uint32_t v0, uint32_t v1, uint32_t v2); + TriangleEPA* newTriangle(const vec3* vertices, uint32_t v0, uint32_t v1, uint32_t v2); /// Access operator TriangleEPA& operator[](int32_t i); @@ -92,7 +92,7 @@ inline TriangleEPA& TrianglesStore::last() { } // Create a new triangle -inline TriangleEPA* TrianglesStore::newTriangle(const Vector3* vertices, +inline TriangleEPA* TrianglesStore::newTriangle(const vec3* vertices, uint32_t v0,uint32_t v1, uint32_t v2) { TriangleEPA* newTriangle = NULL; diff --git a/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp b/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp index f853a22..6640076 100644 --- a/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -43,11 +43,11 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, PROFILE("GJKAlgorithm::testCollision()"); - Vector3 suppA; // Support point of object A - Vector3 suppB; // Support point of object B - Vector3 w; // Support point of Minkowski difference A-B - Vector3 pA; // Closest point of object A - Vector3 pB; // Closest point of object B + vec3 suppA; // Support point of object A + vec3 suppB; // Support point of object B + vec3 w; // Support point of Minkowski difference A-B + vec3 pA; // Closest point of object A + vec3 pB; // Closest point of object B float vDotw; float prevDistSquare; @@ -61,16 +61,16 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, void** shape2CachedCollisionData = shape2Info.cachedCollisionData; // Get the local-space to world-space transforms - const Transform transform1 = shape1Info.shapeToWorldTransform; - const Transform transform2 = shape2Info.shapeToWorldTransform; + const etk::Transform3D transform1 = shape1Info.shapeToWorldTransform; + const etk::Transform3D transform2 = shape2Info.shapeToWorldTransform; - // Transform a point from local space of body 2 to local + // etk::Transform3D a point from local space of body 2 to local // space of body 1 (the GJK algorithm is done in local space of body 1) - Transform body2Tobody1 = transform1.getInverse() * transform2; + etk::Transform3D body2Tobody1 = transform1.getInverse() * transform2; // Matrix that transform a direction from local // space of body 1 int32_to local space of body 2 - Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * + etk::Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix(); // Initialize the margin (sum of margins of both objects) @@ -82,7 +82,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, Simplex simplex; // Get the previous point V (last cached separating axis) - Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis(); + vec3 v = mCurrentOverlappingPair->getCachedSeparatingAxis(); // Initialize the upper bound for the square distance float distSquare = DECIMAL_LARGEST; @@ -123,7 +123,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); // Compute the contact info - Vector3 normal = transform1.getOrientation() * (-v.getUnit()); + vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); float penetrationDepth = margin - dist; // Reject the contact if the penetration depth is negative (due too numerical errors) @@ -156,7 +156,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); // Compute the contact info - Vector3 normal = transform1.getOrientation() * (-v.getUnit()); + vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); float penetrationDepth = margin - dist; // Reject the contact if the penetration depth is negative (due too numerical errors) @@ -187,7 +187,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); // Compute the contact info - Vector3 normal = transform1.getOrientation() * (-v.getUnit()); + vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); float penetrationDepth = margin - dist; // Reject the contact if the penetration depth is negative (due too numerical errors) @@ -205,14 +205,14 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, // Store and update the squared distance of the closest point prevDistSquare = distSquare; - distSquare = v.lengthSquare(); + distSquare = v.length2(); // If the distance to the closest point doesn't improve a lot if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { simplex.backupClosestPointInSimplex(v); // Get the new squared distance - distSquare = v.lengthSquare(); + distSquare = v.length2(); // Compute the closet points of both objects (without the margins) simplex.computeClosestPointsOfAandB(pA, pB); @@ -225,7 +225,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); // Compute the contact info - Vector3 normal = transform1.getOrientation() * (-v.getUnit()); + vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); float penetrationDepth = margin - dist; // Reject the contact if the penetration depth is negative (due too numerical errors) @@ -257,17 +257,17 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, /// a polytope must exist. Then, we give that polytope to the EPA algorithm to /// compute the correct penetration depth and contact points of the enlarged objects. void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info, - const Transform& transform1, + const etk::Transform3D& transform1, const CollisionShapeInfo& shape2Info, - const Transform& transform2, + const etk::Transform3D& transform2, NarrowPhaseCallback* narrowPhaseCallback, - Vector3& v) { + vec3& v) { PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()"); Simplex simplex; - Vector3 suppA; - Vector3 suppB; - Vector3 w; + vec3 suppA; + vec3 suppB; + vec3 w; float vDotw; float distSquare = DECIMAL_LARGEST; float prevDistSquare; @@ -281,12 +281,12 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape2CachedCollisionData = shape2Info.cachedCollisionData; - // Transform a point from local space of body 2 to local space + // etk::Transform3D a point from local space of body 2 to local space // of body 1 (the GJK algorithm is done in local space of body 1) - Transform body2ToBody1 = transform1.getInverse() * transform2; + etk::Transform3D body2ToBody1 = transform1.getInverse() * transform2; // Matrix that transform a direction from local space of body 1 int32_to local space of body 2 - Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * + etk::Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix(); do { @@ -319,7 +319,7 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap // Store and update the square distance prevDistSquare = distSquare; - distSquare = v.lengthSquare(); + distSquare = v.length2(); if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { return; @@ -337,10 +337,10 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap } // Use the GJK Algorithm to find if a point is inside a convex collision shape -bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) { +bool GJKAlgorithm::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) { - Vector3 suppA; // Support point of object A - Vector3 w; // Support point of Minkowski difference A-B + vec3 suppA; // Support point of object A + vec3 w; // Support point of Minkowski difference A-B float prevDistSquare; assert(proxyShape->getCollisionShape()->isConvex()); @@ -350,13 +350,13 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); // Support point of object B (object B is a single point) - const Vector3 suppB(localPoint); + const vec3 suppB(localPoint); // Create a simplex set Simplex simplex; // Initial supporting direction - Vector3 v(1, 1, 1); + vec3 v(1, 1, 1); // Initialize the upper bound for the square distance float distSquare = DECIMAL_LARGEST; @@ -387,7 +387,7 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS // Store and update the squared distance of the closest point prevDistSquare = distSquare; - distSquare = v.lengthSquare(); + distSquare = v.length2(); // If the distance to the closest point doesn't improve a lot if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { @@ -413,29 +413,29 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); - Vector3 suppA; // Current lower bound point on the ray (starting at ray's origin) - Vector3 suppB; // Support point on the collision shape + vec3 suppA; // Current lower bound point on the ray (starting at ray's origin) + vec3 suppB; // Support point on the collision shape const float machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON; const float epsilon = float(0.0001); // Convert the ray origin and direction int32_to the local-space of the collision shape - Vector3 rayDirection = ray.point2 - ray.point1; + vec3 rayDirection = ray.point2 - ray.point1; // If the points of the segment are two close, return no hit - if (rayDirection.lengthSquare() < machineEpsilonSquare) return false; + if (rayDirection.length2() < machineEpsilonSquare) return false; - Vector3 w; + vec3 w; // Create a simplex set Simplex simplex; - Vector3 n(float(0.0), float(0.0), float(0.0)); - float lambda = float(0.0); + vec3 n(0.0f, float(0.0), float(0.0)); + float lambda = 0.0f; suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin) suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData); - Vector3 v = suppA - suppB; + vec3 v = suppA - suppB; float vDotW, vDotR; - float distSquare = v.lengthSquare(); + float distSquare = v.length2(); int32_t nbIterations = 0; // GJK Algorithm loop @@ -472,10 +472,10 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& // Compute the closest point if (simplex.computeClosestPoint(v)) { - distSquare = v.lengthSquare(); + distSquare = v.length2(); } else { - distSquare = float(0.0); + distSquare = 0.0f; } // If the current lower bound distance is larger than the maximum raycasting distance @@ -488,8 +488,8 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& if (lambda < MACHINE_EPSILON) return false; // Compute the closet points of both objects (without the margins) - Vector3 pointA; - Vector3 pointB; + vec3 pointA; + vec3 pointB; simplex.computeClosestPointsOfAandB(pointA, pointB); // A raycast hit has been found, we fill in the raycast info @@ -498,11 +498,11 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; - if (n.lengthSquare() >= machineEpsilonSquare) { // The normal vector is valid + if (n.length2() >= machineEpsilonSquare) { // The normal vector is valid raycastInfo.worldNormal = n; } else { // Degenerated normal vector, we return a zero normal vector - raycastInfo.worldNormal = Vector3(float(0), float(0), float(0)); + raycastInfo.worldNormal = vec3(float(0), float(0), float(0)); } return true; diff --git a/ephysics/collision/narrowphase/GJK/GJKAlgorithm.h b/ephysics/collision/narrowphase/GJK/GJKAlgorithm.h index 7ea1809..3716a32 100644 --- a/ephysics/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/ephysics/collision/narrowphase/GJK/GJKAlgorithm.h @@ -55,11 +55,11 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { /// Compute the penetration depth for enlarged objects. void computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info, - const Transform& transform1, + const etk::Transform3D& transform1, const CollisionShapeInfo& shape2Info, - const Transform& transform2, + const etk::Transform3D& transform2, NarrowPhaseCallback* narrowPhaseCallback, - Vector3& v); + vec3& v); public : @@ -81,7 +81,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { NarrowPhaseCallback* narrowPhaseCallback); /// Use the GJK Algorithm to find if a point is inside a convex collision shape - bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape); + bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape); /// Ray casting algorithm agains a convex collision shape using the GJK Algorithm bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo); diff --git a/ephysics/collision/narrowphase/GJK/Simplex.cpp b/ephysics/collision/narrowphase/GJK/Simplex.cpp index a6721e9..1c5606c 100644 --- a/ephysics/collision/narrowphase/GJK/Simplex.cpp +++ b/ephysics/collision/narrowphase/GJK/Simplex.cpp @@ -25,7 +25,7 @@ Simplex::~Simplex() { /// suppPointA : support point of object A in a direction -v /// suppPointB : support point of object B in a direction v /// point : support point of object (A-B) => point = suppPointA - suppPointB -void Simplex::addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB) { +void Simplex::addPoint(const vec3& point, const vec3& suppPointA, const vec3& suppPointB) { assert(!isFull()); mLastFound = 0; @@ -57,7 +57,7 @@ void Simplex::addPoint(const Vector3& point, const Vector3& suppPointA, const Ve } // Return true if the point is in the simplex -bool Simplex::isPointInSimplex(const Vector3& point) const { +bool Simplex::isPointInSimplex(const vec3& point) const { int32_t i; Bits bit; @@ -93,8 +93,8 @@ void Simplex::updateCache() { } // Return the points of the simplex -uint32_t Simplex::getSimplex(Vector3* suppPointsA, Vector3* suppPointsB, - Vector3* points) const { +uint32_t Simplex::getSimplex(vec3* suppPointsA, vec3* suppPointsB, + vec3* points) const { uint32_t nbVertices = 0; int32_t i; Bits bit; @@ -272,10 +272,10 @@ bool Simplex::isValidSubset(Bits subset) const { /// pA = sum(lambda_i * a_i) where "a_i" are the support points of object A /// pB = sum(lambda_i * b_i) where "b_i" are the support points of object B /// with lambda_i = deltaX_i / deltaX -void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const { +void Simplex::computeClosestPointsOfAandB(vec3& pA, vec3& pB) const { float deltaX = 0.0; - pA.setAllValues(0.0, 0.0, 0.0); - pB.setAllValues(0.0, 0.0, 0.0); + pA.setValue(0.0, 0.0, 0.0); + pB.setValue(0.0, 0.0, 0.0); int32_t i; Bits bit; @@ -290,7 +290,7 @@ void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const { } assert(deltaX > 0.0); - float factor = float(1.0) / deltaX; + float factor = 1.0f / deltaX; pA *= factor; pB *= factor; } @@ -299,7 +299,7 @@ void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const { /// This method executes the Jonhnson's algorithm for computing the point /// "v" of simplex that is closest to the origin. The method returns true /// if a closest point has been found. -bool Simplex::computeClosestPoint(Vector3& v) { +bool Simplex::computeClosestPoint(vec3& v) { Bits subset; // For each possible simplex set @@ -326,13 +326,13 @@ bool Simplex::computeClosestPoint(Vector3& v) { } // Backup the closest point -void Simplex::backupClosestPointInSimplex(Vector3& v) { +void Simplex::backupClosestPointInSimplex(vec3& v) { float minDistSquare = DECIMAL_LARGEST; Bits bit; for (bit = mAllBits; bit != 0x0; bit--) { if (isSubset(bit, mAllBits) && isProperSubset(bit)) { - Vector3 u = computeClosestPointForSubset(bit); + vec3 u = computeClosestPointForSubset(bit); float distSquare = u.dot(u); if (distSquare < minDistSquare) { minDistSquare = distSquare; @@ -345,8 +345,8 @@ void Simplex::backupClosestPointInSimplex(Vector3& v) { // Return the closest point "v" in the convex hull of the points in the subset // represented by the bits "subset" -Vector3 Simplex::computeClosestPointForSubset(Bits subset) { - Vector3 v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i]) +vec3 Simplex::computeClosestPointForSubset(Bits subset) { + vec3 v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i]) mMaxLengthSquare = 0.0; float deltaX = 0.0; // deltaX = sum of all det[subset][i] int32_t i; @@ -371,5 +371,5 @@ Vector3 Simplex::computeClosestPointForSubset(Bits subset) { assert(deltaX > 0.0); // Return the closet point "v" in the convex hull for the given subset - return (float(1.0) / deltaX) * v; + return (1.0f / deltaX) * v; } diff --git a/ephysics/collision/narrowphase/GJK/Simplex.h b/ephysics/collision/narrowphase/GJK/Simplex.h index e7208cb..64b8288 100644 --- a/ephysics/collision/narrowphase/GJK/Simplex.h +++ b/ephysics/collision/narrowphase/GJK/Simplex.h @@ -31,7 +31,7 @@ class Simplex { // -------------------- Attributes -------------------- // /// Current points - Vector3 mPoints[4]; + vec3 mPoints[4]; /// pointsLengthSquare[i] = (points[i].length)^2 float mPointsLengthSquare[4]; @@ -40,13 +40,13 @@ class Simplex { float mMaxLengthSquare; /// Support points of object A in local coordinates - Vector3 mSuppPointsA[4]; + vec3 mSuppPointsA[4]; /// Support points of object B in local coordinates - Vector3 mSuppPointsB[4]; + vec3 mSuppPointsB[4]; /// diff[i][j] contains points[i] - points[j] - Vector3 mDiffLength[4][4]; + vec3 mDiffLength[4][4]; /// Cached determinant values float mDet[16][4]; @@ -94,7 +94,7 @@ class Simplex { void computeDeterminants(); /// Return the closest point "v" in the convex hull of a subset of points - Vector3 computeClosestPointForSubset(Bits subset); + vec3 computeClosestPointForSubset(Bits subset); public: @@ -113,29 +113,29 @@ class Simplex { bool isEmpty() const; /// Return the points of the simplex - uint32_t getSimplex(Vector3* mSuppPointsA, Vector3* mSuppPointsB, - Vector3* mPoints) const; + uint32_t getSimplex(vec3* mSuppPointsA, vec3* mSuppPointsB, + vec3* mPoints) const; /// Return the maximum squared length of a point float getMaxLengthSquareOfAPoint() const; /// Add a new support point of (A-B) int32_to the simplex. - void addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB); + void addPoint(const vec3& point, const vec3& suppPointA, const vec3& suppPointB); /// Return true if the point is in the simplex - bool isPointInSimplex(const Vector3& point) const; + bool isPointInSimplex(const vec3& point) const; /// Return true if the set is affinely dependent bool isAffinelyDependent() const; /// Backup the closest point - void backupClosestPointInSimplex(Vector3& point); + void backupClosestPointInSimplex(vec3& point); /// Compute the closest points "pA" and "pB" of object A and B. - void computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const; + void computeClosestPointsOfAandB(vec3& pA, vec3& pB) const; /// Compute the closest point to the origin of the current simplex. - bool computeClosestPoint(Vector3& v); + bool computeClosestPoint(vec3& v); }; // Return true if some bits of "a" overlap with bits of "b" diff --git a/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 1558864..8b929ce 100644 --- a/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -29,26 +29,26 @@ void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info const SphereShape* sphereShape1 = static_cast(shape1Info.collisionShape); const SphereShape* sphereShape2 = static_cast(shape2Info.collisionShape); // Get the local-space to world-space transforms - const Transform& transform1 = shape1Info.shapeToWorldTransform; - const Transform& transform2 = shape2Info.shapeToWorldTransform; + const etk::Transform3D& transform1 = shape1Info.shapeToWorldTransform; + const etk::Transform3D& transform2 = shape2Info.shapeToWorldTransform; // Compute the distance between the centers - Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); - float squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare(); + vec3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); + float squaredDistanceBetweenCenters = vectorBetweenCenters.length2(); // Compute the sum of the radius float sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius(); // If the sphere collision shapes int32_tersect if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) { - Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); - Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); - Vector3 int32_tersectionOnBody1 = sphereShape1->getRadius() * - centerSphere2InBody1LocalSpace.getUnit(); - Vector3 int32_tersectionOnBody2 = sphereShape2->getRadius() * - centerSphere1InBody2LocalSpace.getUnit(); + vec3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); + vec3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); + vec3 int32_tersectionOnBody1 = sphereShape1->getRadius() * + centerSphere2InBody1LocalSpace.safeNormalized(); + vec3 int32_tersectionOnBody2 = sphereShape2->getRadius() * + centerSphere1InBody2LocalSpace.safeNormalized(); float penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); // Create the contact info object ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, - shape2Info.collisionShape, vectorBetweenCenters.getUnit(), penetrationDepth, + shape2Info.collisionShape, vectorBetweenCenters.safeNormalized(), penetrationDepth, int32_tersectionOnBody1, int32_tersectionOnBody2); // Notify about the new contact narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); diff --git a/ephysics/collision/shapes/AABB.cpp b/ephysics/collision/shapes/AABB.cpp index a18f67e..24eb4ee 100644 --- a/ephysics/collision/shapes/AABB.cpp +++ b/ephysics/collision/shapes/AABB.cpp @@ -19,15 +19,17 @@ AABB::AABB() { } // Constructor -AABB::AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates) - :m_minCoordinates(minCoordinates), m_maxCoordinates(maxCoordinates) { - +AABB::AABB(const vec3& minCoordinates, const vec3& maxCoordinates): + m_minCoordinates(minCoordinates), + m_maxCoordinates(maxCoordinates) { + } // Copy-constructor -AABB::AABB(const AABB& aabb) - : m_minCoordinates(aabb.m_minCoordinates), m_maxCoordinates(aabb.m_maxCoordinates) { - +AABB::AABB(const AABB& aabb): + m_minCoordinates(aabb.m_minCoordinates), + m_maxCoordinates(aabb.m_maxCoordinates) { + } // Destructor @@ -37,62 +39,76 @@ AABB::~AABB() { // Merge the AABB in parameter with the current one void AABB::mergeWithAABB(const AABB& aabb) { - m_minCoordinates.x = std::min(m_minCoordinates.x, aabb.m_minCoordinates.x); - m_minCoordinates.y = std::min(m_minCoordinates.y, aabb.m_minCoordinates.y); - m_minCoordinates.z = std::min(m_minCoordinates.z, aabb.m_minCoordinates.z); - - m_maxCoordinates.x = std::max(m_maxCoordinates.x, aabb.m_maxCoordinates.x); - m_maxCoordinates.y = std::max(m_maxCoordinates.y, aabb.m_maxCoordinates.y); - m_maxCoordinates.z = std::max(m_maxCoordinates.z, aabb.m_maxCoordinates.z); + m_minCoordinates.setX(std::min(m_minCoordinates.x(), aabb.m_minCoordinates.x())); + m_minCoordinates.setY(std::min(m_minCoordinates.y(), aabb.m_minCoordinates.y())); + m_minCoordinates.setZ(std::min(m_minCoordinates.z(), aabb.m_minCoordinates.z())); + m_maxCoordinates.setX(std::max(m_maxCoordinates.x(), aabb.m_maxCoordinates.x())); + m_maxCoordinates.setY(std::max(m_maxCoordinates.y(), aabb.m_maxCoordinates.y())); + m_maxCoordinates.setZ(std::max(m_maxCoordinates.z(), aabb.m_maxCoordinates.z())); } // Replace the current AABB with a new AABB that is the union of two AABBs in parameters void AABB::mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2) { - m_minCoordinates.x = std::min(aabb1.m_minCoordinates.x, aabb2.m_minCoordinates.x); - m_minCoordinates.y = std::min(aabb1.m_minCoordinates.y, aabb2.m_minCoordinates.y); - m_minCoordinates.z = std::min(aabb1.m_minCoordinates.z, aabb2.m_minCoordinates.z); - - m_maxCoordinates.x = std::max(aabb1.m_maxCoordinates.x, aabb2.m_maxCoordinates.x); - m_maxCoordinates.y = std::max(aabb1.m_maxCoordinates.y, aabb2.m_maxCoordinates.y); - m_maxCoordinates.z = std::max(aabb1.m_maxCoordinates.z, aabb2.m_maxCoordinates.z); + m_minCoordinates.setX(std::min(aabb1.m_minCoordinates.x(), aabb2.m_minCoordinates.x())); + m_minCoordinates.setY(std::min(aabb1.m_minCoordinates.y(), aabb2.m_minCoordinates.y())); + m_minCoordinates.setZ(std::min(aabb1.m_minCoordinates.z(), aabb2.m_minCoordinates.z())); + m_maxCoordinates.setX(std::max(aabb1.m_maxCoordinates.x(), aabb2.m_maxCoordinates.x())); + m_maxCoordinates.setY(std::max(aabb1.m_maxCoordinates.y(), aabb2.m_maxCoordinates.y())); + m_maxCoordinates.setZ(std::max(aabb1.m_maxCoordinates.z(), aabb2.m_maxCoordinates.z())); } // Return true if the current AABB contains the AABB given in parameter bool AABB::contains(const AABB& aabb) const { - bool isInside = true; - isInside = isInside && m_minCoordinates.x <= aabb.m_minCoordinates.x; - isInside = isInside && m_minCoordinates.y <= aabb.m_minCoordinates.y; - isInside = isInside && m_minCoordinates.z <= aabb.m_minCoordinates.z; - - isInside = isInside && m_maxCoordinates.x >= aabb.m_maxCoordinates.x; - isInside = isInside && m_maxCoordinates.y >= aabb.m_maxCoordinates.y; - isInside = isInside && m_maxCoordinates.z >= aabb.m_maxCoordinates.z; + isInside = isInside && m_minCoordinates.x() <= aabb.m_minCoordinates.x(); + isInside = isInside && m_minCoordinates.y() <= aabb.m_minCoordinates.y(); + isInside = isInside && m_minCoordinates.z() <= aabb.m_minCoordinates.z(); + isInside = isInside && m_maxCoordinates.x() >= aabb.m_maxCoordinates.x(); + isInside = isInside && m_maxCoordinates.y() >= aabb.m_maxCoordinates.y(); + isInside = isInside && m_maxCoordinates.z() >= aabb.m_maxCoordinates.z(); return isInside; } // Create and return an AABB for a triangle -AABB AABB::createAABBForTriangle(const Vector3* trianglePoints) { - - Vector3 minCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z); - Vector3 maxCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z); - - if (trianglePoints[1].x < minCoords.x) minCoords.x = trianglePoints[1].x; - if (trianglePoints[1].y < minCoords.y) minCoords.y = trianglePoints[1].y; - if (trianglePoints[1].z < minCoords.z) minCoords.z = trianglePoints[1].z; - - if (trianglePoints[2].x < minCoords.x) minCoords.x = trianglePoints[2].x; - if (trianglePoints[2].y < minCoords.y) minCoords.y = trianglePoints[2].y; - if (trianglePoints[2].z < minCoords.z) minCoords.z = trianglePoints[2].z; - - if (trianglePoints[1].x > maxCoords.x) maxCoords.x = trianglePoints[1].x; - if (trianglePoints[1].y > maxCoords.y) maxCoords.y = trianglePoints[1].y; - if (trianglePoints[1].z > maxCoords.z) maxCoords.z = trianglePoints[1].z; - - if (trianglePoints[2].x > maxCoords.x) maxCoords.x = trianglePoints[2].x; - if (trianglePoints[2].y > maxCoords.y) maxCoords.y = trianglePoints[2].y; - if (trianglePoints[2].z > maxCoords.z) maxCoords.z = trianglePoints[2].z; - +AABB AABB::createAABBForTriangle(const vec3* trianglePoints) { + vec3 minCoords(trianglePoints[0].x(), trianglePoints[0].y(), trianglePoints[0].z()); + vec3 maxCoords(trianglePoints[0].x(), trianglePoints[0].y(), trianglePoints[0].z()); + if (trianglePoints[1].x() < minCoords.x()) { + minCoords.setX(trianglePoints[1].x()); + } + if (trianglePoints[1].y() < minCoords.y()) { + minCoords.setY(trianglePoints[1].y()); + } + if (trianglePoints[1].z() < minCoords.z()) { + minCoords.setZ(trianglePoints[1].z()); + } + if (trianglePoints[2].x() < minCoords.x()) { + minCoords.setX(trianglePoints[2].x()); + } + if (trianglePoints[2].y() < minCoords.y()) { + minCoords.setY(trianglePoints[2].y()); + } + if (trianglePoints[2].z() < minCoords.z()) { + minCoords.setZ(trianglePoints[2].z()); + } + if (trianglePoints[1].x() > maxCoords.x()) { + maxCoords.setX(trianglePoints[1].x()); + } + if (trianglePoints[1].y() > maxCoords.y()) { + maxCoords.setY(trianglePoints[1].y()); + } + if (trianglePoints[1].z() > maxCoords.z()) { + maxCoords.setZ(trianglePoints[1].z()); + } + if (trianglePoints[2].x() > maxCoords.x()) { + maxCoords.setX(trianglePoints[2].x()); + } + if (trianglePoints[2].y() > maxCoords.y()) { + maxCoords.setY(trianglePoints[2].y()); + } + if (trianglePoints[2].z() > maxCoords.z()) { + maxCoords.setZ(trianglePoints[2].z()); + } return AABB(minCoords, maxCoords); } @@ -100,33 +116,40 @@ AABB AABB::createAABBForTriangle(const Vector3* trianglePoints) { /// This method use the line vs AABB raycasting technique described in /// Real-time Collision Detection by Christer Ericson. bool AABB::testRayIntersect(const Ray& ray) const { - - const Vector3 point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1); - const Vector3 e = m_maxCoordinates - m_minCoordinates; - const Vector3 d = point2 - ray.point1; - const Vector3 m = ray.point1 + point2 - m_minCoordinates - m_maxCoordinates; - + const vec3 point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1); + const vec3 e = m_maxCoordinates - m_minCoordinates; + const vec3 d = point2 - ray.point1; + const vec3 m = ray.point1 + point2 - m_minCoordinates - m_maxCoordinates; // Test if the AABB face normals are separating axis - float adx = std::abs(d.x); - if (std::abs(m.x) > e.x + adx) return false; - float ady = std::abs(d.y); - if (std::abs(m.y) > e.y + ady) return false; - float adz = std::abs(d.z); - if (std::abs(m.z) > e.z + adz) return false; - + float adx = std::abs(d.x()); + if (std::abs(m.x()) > e.x() + adx) { + return false; + } + float ady = std::abs(d.y()); + if (std::abs(m.y()) > e.y() + ady) { + return false; + } + float adz = std::abs(d.z()); + if (std::abs(m.z()) > e.z() + adz) { + return false; + } // Add in an epsilon term to counteract arithmetic errors when segment is // (near) parallel to a coordinate axis (see text for detail) const float epsilon = 0.00001; adx += epsilon; ady += epsilon; adz += epsilon; - // Test if the cross products between face normals and ray direction are // separating axis - if (std::abs(m.y * d.z - m.z * d.y) > e.y * adz + e.z * ady) return false; - if (std::abs(m.z * d.x - m.x * d.z) > e.x * adz + e.z * adx) return false; - if (std::abs(m.x * d.y - m.y * d.x) > e.x * ady + e.y * adx) return false; - + if (std::abs(m.y() * d.z() - m.z() * d.y()) > e.y() * adz + e.z() * ady) { + return false; + } + if (std::abs(m.z() * d.x() - m.x() * d.z()) > e.x() * adz + e.z() * adx) { + return false; + } + if (std::abs(m.x() * d.y() - m.y() * d.x()) > e.x() * ady + e.y() * adx) { + return false; + } // No separating axis has been found return true; } diff --git a/ephysics/collision/shapes/AABB.h b/ephysics/collision/shapes/AABB.h index 77e064f..d7e84b5 100644 --- a/ephysics/collision/shapes/AABB.h +++ b/ephysics/collision/shapes/AABB.h @@ -25,10 +25,10 @@ class AABB { // -------------------- Attributes -------------------- // /// Minimum world coordinates of the AABB on the x,y and z axis - Vector3 m_minCoordinates; + vec3 m_minCoordinates; /// Maximum world coordinates of the AABB on the x,y and z axis - Vector3 m_maxCoordinates; + vec3 m_maxCoordinates; public : @@ -38,7 +38,7 @@ class AABB { AABB(); /// Constructor - AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates); + AABB(const vec3& minCoordinates, const vec3& maxCoordinates); /// Copy-constructor AABB(const AABB& aabb); @@ -47,22 +47,22 @@ class AABB { ~AABB(); /// Return the center point - Vector3 getCenter() const; + vec3 getCenter() const; /// Return the minimum coordinates of the AABB - const Vector3& getMin() const; + const vec3& getMin() const; /// Set the minimum coordinates of the AABB - void setMin(const Vector3& min); + void setMin(const vec3& min); /// Return the maximum coordinates of the AABB - const Vector3& getMax() const; + const vec3& getMax() const; /// Set the maximum coordinates of the AABB - void setMax(const Vector3& max); + void setMax(const vec3& max); /// Return the size of the AABB in the three dimension x, y and z - Vector3 getExtent() const; + vec3 getExtent() const; /// Inflate each side of the AABB by a given size void inflate(float dx, float dy, float dz); @@ -83,16 +83,16 @@ class AABB { bool contains(const AABB& aabb) const; /// Return true if a point is inside the AABB - bool contains(const Vector3& point) const; + bool contains(const vec3& point) const; /// Return true if the AABB of a triangle int32_tersects the AABB - bool testCollisionTriangleAABB(const Vector3* trianglePoints) const; + bool testCollisionTriangleAABB(const vec3* trianglePoints) const; /// Return true if the ray int32_tersects the AABB bool testRayIntersect(const Ray& ray) const; /// Create and return an AABB for a triangle - static AABB createAABBForTriangle(const Vector3* trianglePoints); + static AABB createAABBForTriangle(const vec3* trianglePoints); /// Assignment operator AABB& operator=(const AABB& aabb); @@ -103,79 +103,79 @@ class AABB { }; // Return the center point of the AABB in world coordinates -inline Vector3 AABB::getCenter() const { - return (m_minCoordinates + m_maxCoordinates) * float(0.5); +inline vec3 AABB::getCenter() const { + return (m_minCoordinates + m_maxCoordinates) * 0.5f; } // Return the minimum coordinates of the AABB -inline const Vector3& AABB::getMin() const { +inline const vec3& AABB::getMin() const { return m_minCoordinates; } // Set the minimum coordinates of the AABB -inline void AABB::setMin(const Vector3& min) { +inline void AABB::setMin(const vec3& min) { m_minCoordinates = min; } // Return the maximum coordinates of the AABB -inline const Vector3& AABB::getMax() const { +inline const vec3& AABB::getMax() const { return m_maxCoordinates; } // Set the maximum coordinates of the AABB -inline void AABB::setMax(const Vector3& max) { +inline void AABB::setMax(const vec3& max) { m_maxCoordinates = max; } // Return the size of the AABB in the three dimension x, y and z -inline Vector3 AABB::getExtent() const { +inline vec3 AABB::getExtent() const { return m_maxCoordinates - m_minCoordinates; } // Inflate each side of the AABB by a given size inline void AABB::inflate(float dx, float dy, float dz) { - m_maxCoordinates += Vector3(dx, dy, dz); - m_minCoordinates -= Vector3(dx, dy, dz); + m_maxCoordinates += vec3(dx, dy, dz); + m_minCoordinates -= vec3(dx, dy, dz); } // Return true if the current AABB is overlapping with the AABB in argument. /// Two AABBs overlap if they overlap in the three x, y and z axis at the same time inline bool AABB::testCollision(const AABB& aabb) const { - if (m_maxCoordinates.x < aabb.m_minCoordinates.x || - aabb.m_maxCoordinates.x < m_minCoordinates.x) return false; - if (m_maxCoordinates.y < aabb.m_minCoordinates.y || - aabb.m_maxCoordinates.y < m_minCoordinates.y) return false; - if (m_maxCoordinates.z < aabb.m_minCoordinates.z|| - aabb.m_maxCoordinates.z < m_minCoordinates.z) return false; + if (m_maxCoordinates.x() < aabb.m_minCoordinates.x() || + aabb.m_maxCoordinates.x() < m_minCoordinates.x()) return false; + if (m_maxCoordinates.y() < aabb.m_minCoordinates.y() || + aabb.m_maxCoordinates.y() < m_minCoordinates.y()) return false; + if (m_maxCoordinates.z() < aabb.m_minCoordinates.z()|| + aabb.m_maxCoordinates.z() < m_minCoordinates.z()) return false; return true; } // Return the volume of the AABB inline float AABB::getVolume() const { - const Vector3 diff = m_maxCoordinates - m_minCoordinates; - return (diff.x * diff.y * diff.z); + const vec3 diff = m_maxCoordinates - m_minCoordinates; + return (diff.x() * diff.y() * diff.z()); } // Return true if the AABB of a triangle int32_tersects the AABB -inline bool AABB::testCollisionTriangleAABB(const Vector3* trianglePoints) const { +inline bool AABB::testCollisionTriangleAABB(const vec3* trianglePoints) const { - if (min3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) > m_maxCoordinates.x) return false; - if (min3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) > m_maxCoordinates.y) return false; - if (min3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) > m_maxCoordinates.z) return false; + if (min3(trianglePoints[0].x(), trianglePoints[1].x(), trianglePoints[2].x()) > m_maxCoordinates.x()) return false; + if (min3(trianglePoints[0].y(), trianglePoints[1].y(), trianglePoints[2].y()) > m_maxCoordinates.y()) return false; + if (min3(trianglePoints[0].z(), trianglePoints[1].z(), trianglePoints[2].z()) > m_maxCoordinates.z()) return false; - if (max3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) < m_minCoordinates.x) return false; - if (max3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) < m_minCoordinates.y) return false; - if (max3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) < m_minCoordinates.z) return false; + if (max3(trianglePoints[0].x(), trianglePoints[1].x(), trianglePoints[2].x()) < m_minCoordinates.x()) return false; + if (max3(trianglePoints[0].y(), trianglePoints[1].y(), trianglePoints[2].y()) < m_minCoordinates.y()) return false; + if (max3(trianglePoints[0].z(), trianglePoints[1].z(), trianglePoints[2].z()) < m_minCoordinates.z()) return false; return true; } // Return true if a point is inside the AABB -inline bool AABB::contains(const Vector3& point) const { +inline bool AABB::contains(const vec3& point) const { - return (point.x >= m_minCoordinates.x - MACHINE_EPSILON && point.x <= m_maxCoordinates.x + MACHINE_EPSILON && - point.y >= m_minCoordinates.y - MACHINE_EPSILON && point.y <= m_maxCoordinates.y + MACHINE_EPSILON && - point.z >= m_minCoordinates.z - MACHINE_EPSILON && point.z <= m_maxCoordinates.z + MACHINE_EPSILON); + return (point.x() >= m_minCoordinates.x() - MACHINE_EPSILON && point.x() <= m_maxCoordinates.x() + MACHINE_EPSILON && + point.y() >= m_minCoordinates.y() - MACHINE_EPSILON && point.y() <= m_maxCoordinates.y() + MACHINE_EPSILON && + point.z() >= m_minCoordinates.z() - MACHINE_EPSILON && point.z() <= m_maxCoordinates.z() + MACHINE_EPSILON); } // Assignment operator diff --git a/ephysics/collision/shapes/BoxShape.cpp b/ephysics/collision/shapes/BoxShape.cpp index 31e4049..4a71586 100644 --- a/ephysics/collision/shapes/BoxShape.cpp +++ b/ephysics/collision/shapes/BoxShape.cpp @@ -18,11 +18,11 @@ using namespace reactphysics3d; * @param extent The vector with the three extents of the box (in meters) * @param margin The collision margin (in meters) around the collision shape */ -BoxShape::BoxShape(const Vector3& extent, float margin) - : ConvexShape(BOX, margin), m_extent(extent - Vector3(margin, margin, margin)) { - assert(extent.x > float(0.0) && extent.x > margin); - assert(extent.y > float(0.0) && extent.y > margin); - assert(extent.z > float(0.0) && extent.z > margin); +BoxShape::BoxShape(const vec3& extent, float margin) + : ConvexShape(BOX, margin), m_extent(extent - vec3(margin, margin, margin)) { + assert(extent.x() > 0.0f && extent.x() > margin); + assert(extent.y() > 0.0f && extent.y() > margin); + assert(extent.z() > 0.0f && extent.z() > margin); } // Destructor @@ -36,13 +36,13 @@ BoxShape::~BoxShape() { * coordinates * @param mass Mass to use to compute the inertia tensor of the collision shape */ -void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { - float factor = (float(1.0) / float(3.0)) * mass; - Vector3 realExtent = m_extent + Vector3(mMargin, mMargin, mMargin); - float xSquare = realExtent.x * realExtent.x; - float ySquare = realExtent.y * realExtent.y; - float zSquare = realExtent.z * realExtent.z; - tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0, +void BoxShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { + float factor = (1.0f / float(3.0)) * mass; + vec3 realExtent = m_extent + vec3(m_margin, m_margin, m_margin); + float xSquare = realExtent.x() * realExtent.x(); + float ySquare = realExtent.y() * realExtent.y(); + float zSquare = realExtent.z() * realExtent.z(); + tensor.setValue(factor * (ySquare + zSquare), 0.0, 0.0, 0.0, factor * (xSquare + zSquare), 0.0, 0.0, 0.0, factor * (xSquare + ySquare)); } @@ -50,11 +50,11 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { // Raycast method with feedback information bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - Vector3 rayDirection = ray.point2 - ray.point1; + vec3 rayDirection = ray.point2 - ray.point1; float tMin = DECIMAL_SMALLEST; float tMax = DECIMAL_LARGEST; - Vector3 normalDirection(float(0), float(0), float(0)); - Vector3 currentNormal; + vec3 normalDirection(float(0), float(0), float(0)); + vec3 currentNormal; // For each of the three slabs for (int32_t i=0; i<3; i++) { @@ -68,12 +68,12 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro else { // Compute the int32_tersection of the ray with the near and far plane of the slab - float oneOverD = float(1.0) / rayDirection[i]; + float oneOverD = 1.0f / rayDirection[i]; float t1 = (-m_extent[i] - ray.point1[i]) * oneOverD; float t2 = (m_extent[i] - ray.point1[i]) * oneOverD; - currentNormal[0] = (i == 0) ? -m_extent[i] : float(0.0); - currentNormal[1] = (i == 1) ? -m_extent[i] : float(0.0); - currentNormal[2] = (i == 2) ? -m_extent[i] : float(0.0); + currentNormal[0] = (i == 0) ? -m_extent[i] : 0.0f; + currentNormal[1] = (i == 1) ? -m_extent[i] : 0.0f; + currentNormal[2] = (i == 2) ? -m_extent[i] : 0.0f; // Swap t1 and t2 if need so that t1 is int32_tersection with near plane and // t2 with far plane @@ -98,10 +98,10 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro } // If tMin is negative, we return no hit - if (tMin < float(0.0) || tMin > ray.maxFraction) return false; + if (tMin < 0.0f || tMin > ray.maxFraction) return false; // The ray int32_tersects the three slabs, we compute the hit point - Vector3 localHitPoint = ray.point1 + tMin * rayDirection; + vec3 localHitPoint = ray.point1 + tMin * rayDirection; raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; diff --git a/ephysics/collision/shapes/BoxShape.h b/ephysics/collision/shapes/BoxShape.h index 64085ad..7647f1f 100644 --- a/ephysics/collision/shapes/BoxShape.h +++ b/ephysics/collision/shapes/BoxShape.h @@ -36,7 +36,7 @@ class BoxShape : public ConvexShape { // -------------------- Attributes -------------------- // /// Extent sizes of the box in the x, y and z direction - Vector3 m_extent; + vec3 m_extent; // -------------------- Methods -------------------- // @@ -47,11 +47,11 @@ class BoxShape : public ConvexShape { BoxShape& operator=(const BoxShape& shape); /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, + virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, void** cachedCollisionData) const; /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; + virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; @@ -64,36 +64,36 @@ class BoxShape : public ConvexShape { // -------------------- Methods -------------------- // /// Constructor - BoxShape(const Vector3& extent, float margin = OBJECT_MARGIN); + BoxShape(const vec3& extent, float margin = OBJECT_MARGIN); /// Destructor virtual ~BoxShape(); /// Return the extents of the box - Vector3 getExtent() const; + vec3 getExtent() const; /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + virtual void setLocalScaling(const vec3& scaling); /// Return the local bounds of the shape in x, y and z directions - virtual void getLocalBounds(Vector3& min, Vector3& max) const; + virtual void getLocalBounds(vec3& min, vec3& max) const; /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const; + virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; }; // Return the extents of the box /** * @return The vector with the three extents of the box shape (in meters) */ -inline Vector3 BoxShape::getExtent() const { - return m_extent + Vector3(mMargin, mMargin, mMargin); +inline vec3 BoxShape::getExtent() const { + return m_extent + vec3(m_margin, m_margin, m_margin); } // Set the scaling vector of the collision shape -inline void BoxShape::setLocalScaling(const Vector3& scaling) { +inline void BoxShape::setLocalScaling(const vec3& scaling) { - m_extent = (m_extent / mScaling) * scaling; + m_extent = (m_extent / m_scaling) * scaling; CollisionShape::setLocalScaling(scaling); } @@ -104,10 +104,10 @@ inline void BoxShape::setLocalScaling(const Vector3& scaling) { * @param min The minimum bounds of the shape in local-space coordinates * @param max The maximum bounds of the shape in local-space coordinates */ -inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const { +inline void BoxShape::getLocalBounds(vec3& min, vec3& max) const { // Maximum bounds - max = m_extent + Vector3(mMargin, mMargin, mMargin); + max = m_extent + vec3(m_margin, m_margin, m_margin); // Minimum bounds min = -max; @@ -119,19 +119,19 @@ inline size_t BoxShape::getSizeInBytes() const { } // Return a local support point in a given direction without the objec margin -inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction, +inline vec3 BoxShape::getLocalSupportPointWithoutMargin(const vec3& direction, void** cachedCollisionData) const { - return Vector3(direction.x < 0.0 ? -m_extent.x : m_extent.x, - direction.y < 0.0 ? -m_extent.y : m_extent.y, - direction.z < 0.0 ? -m_extent.z : m_extent.z); + return vec3(direction.x() < 0.0 ? -m_extent.x() : m_extent.x(), + direction.y() < 0.0 ? -m_extent.y() : m_extent.y(), + direction.z() < 0.0 ? -m_extent.z() : m_extent.z()); } // Return true if a point is inside the collision shape -inline bool BoxShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { - return (localPoint.x < m_extent[0] && localPoint.x > -m_extent[0] && - localPoint.y < m_extent[1] && localPoint.y > -m_extent[1] && - localPoint.z < m_extent[2] && localPoint.z > -m_extent[2]); +inline bool BoxShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { + return (localPoint.x() < m_extent[0] && localPoint.x() > -m_extent[0] && + localPoint.y() < m_extent[1] && localPoint.y() > -m_extent[1] && + localPoint.z() < m_extent[2] && localPoint.z() > -m_extent[2]); } } diff --git a/ephysics/collision/shapes/CapsuleShape.cpp b/ephysics/collision/shapes/CapsuleShape.cpp index 49a02c7..dfe2195 100644 --- a/ephysics/collision/shapes/CapsuleShape.cpp +++ b/ephysics/collision/shapes/CapsuleShape.cpp @@ -18,9 +18,9 @@ using namespace reactphysics3d; * @param height The height of the capsule (in meters) */ CapsuleShape::CapsuleShape(float radius, float height) - : ConvexShape(CAPSULE, radius), mHalfHeight(height * float(0.5)) { - assert(radius > float(0.0)); - assert(height > float(0.0)); + : ConvexShape(CAPSULE, radius), m_halfHeight(height * 0.5f) { + assert(radius > 0.0f); + assert(height > 0.0f); } // Destructor @@ -34,38 +34,38 @@ CapsuleShape::~CapsuleShape() { * coordinates * @param mass Mass to use to compute the inertia tensor of the collision shape */ -void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { +void CapsuleShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { // The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1 - float height = mHalfHeight + mHalfHeight; - float radiusSquare = mMargin * mMargin; + float height = m_halfHeight + m_halfHeight; + float radiusSquare = m_margin * m_margin; float heightSquare = height * height; float radiusSquareDouble = radiusSquare + radiusSquare; - float factor1 = float(2.0) * mMargin / (float(4.0) * mMargin + float(3.0) * height); - float factor2 = float(3.0) * height / (float(4.0) * mMargin + float(3.0) * height); + float factor1 = float(2.0) * m_margin / (float(4.0) * m_margin + float(3.0) * height); + float factor2 = float(3.0) * height / (float(4.0) * m_margin + float(3.0) * height); float sum1 = float(0.4) * radiusSquareDouble; - float sum2 = float(0.75) * height * mMargin + float(0.5) * heightSquare; + float sum2 = float(0.75) * height * m_margin + 0.5f * heightSquare; float sum3 = float(0.25) * radiusSquare + float(1.0 / 12.0) * heightSquare; float IxxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3; float Iyy = factor1 * mass * sum1 + factor2 * mass * float(0.25) * radiusSquareDouble; - tensor.setAllValues(IxxAndzz, 0.0, 0.0, + tensor.setValue(IxxAndzz, 0.0, 0.0, 0.0, Iyy, 0.0, 0.0, 0.0, IxxAndzz); } // Return true if a point is inside the collision shape -bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { +bool CapsuleShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { - const float diffYCenterSphere1 = localPoint.y - mHalfHeight; - const float diffYCenterSphere2 = localPoint.y + mHalfHeight; - const float xSquare = localPoint.x * localPoint.x; - const float zSquare = localPoint.z * localPoint.z; - const float squareRadius = mMargin * mMargin; + const float diffYCenterSphere1 = localPoint.y() - m_halfHeight; + const float diffYCenterSphere2 = localPoint.y() + m_halfHeight; + const float xSquare = localPoint.x() * localPoint.x(); + const float zSquare = localPoint.z() * localPoint.z(); + const float squareRadius = m_margin * m_margin; // Return true if the point is inside the cylinder or one of the two spheres of the capsule return ((xSquare + zSquare) < squareRadius && - localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) || + localPoint.y() < m_halfHeight && localPoint.y() > -m_halfHeight) || (xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius || (xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius; } @@ -73,13 +73,13 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS // Raycast method with feedback information bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - const Vector3 n = ray.point2 - ray.point1; + const vec3 n = ray.point2 - ray.point1; const float epsilon = float(0.01); - Vector3 p(float(0), -mHalfHeight, float(0)); - Vector3 q(float(0), mHalfHeight, float(0)); - Vector3 d = q - p; - Vector3 m = ray.point1 - p; + vec3 p(float(0), -m_halfHeight, float(0)); + vec3 q(float(0), m_halfHeight, float(0)); + vec3 d = q - p; + vec3 m = ray.point1 - p; float t; float mDotD = m.dot(d); @@ -87,38 +87,38 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* float dDotD = d.dot(d); // Test if the segment is outside the cylinder - float vec1DotD = (ray.point1 - Vector3(float(0.0), -mHalfHeight - mMargin, float(0.0))).dot(d); - if (vec1DotD < float(0.0) && vec1DotD + nDotD < float(0.0)) return false; - float ddotDExtraCaps = float(2.0) * mMargin * d.y; + float vec1DotD = (ray.point1 - vec3(0.0f, -m_halfHeight - m_margin, float(0.0))).dot(d); + if (vec1DotD < 0.0f && vec1DotD + nDotD < float(0.0)) return false; + float ddotDExtraCaps = float(2.0) * m_margin * d.y(); if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) return false; float nDotN = n.dot(n); float mDotN = m.dot(n); float a = dDotD * nDotN - nDotD * nDotD; - float k = m.dot(m) - mMargin * mMargin; + float k = m.dot(m) - m_margin * m_margin; float c = dDotD * k - mDotD * mDotD; // If the ray is parallel to the capsule axis if (std::abs(a) < epsilon) { // If the origin is outside the surface of the capusle's cylinder, we return no hit - if (c > float(0.0)) return false; + if (c > 0.0f) return false; // Here we know that the segment int32_tersect an endcap of the capsule // If the ray int32_tersects with the "p" endcap of the capsule - if (mDotD < float(0.0)) { + if (mDotD < 0.0f) { // Check int32_tersection between the ray and the "p" sphere endcap of the capsule - Vector3 hitLocalPoint; + vec3 hitLocalPoint; float hitFraction; if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = hitFraction; raycastInfo.worldPoint = hitLocalPoint; - Vector3 normalDirection = hitLocalPoint - p; + vec3 normalDirection = hitLocalPoint - p; raycastInfo.worldNormal = normalDirection; return true; @@ -129,14 +129,14 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* else if (mDotD > dDotD) { // If the ray int32_tersects with the "q" endcap of the cylinder // Check int32_tersection between the ray and the "q" sphere endcap of the capsule - Vector3 hitLocalPoint; + vec3 hitLocalPoint; float hitFraction; if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = hitFraction; raycastInfo.worldPoint = hitLocalPoint; - Vector3 normalDirection = hitLocalPoint - q; + vec3 normalDirection = hitLocalPoint - q; raycastInfo.worldNormal = normalDirection; return true; @@ -152,24 +152,24 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* float discriminant = b * b - a * c; // If the discriminant is negative, no real roots and therfore, no hit - if (discriminant < float(0.0)) return false; + if (discriminant < 0.0f) return false; // Compute the smallest root (first int32_tersection along the ray) float t0 = t = (-b - std::sqrt(discriminant)) / a; // If the int32_tersection is outside the finite cylinder of the capsule on "p" endcap side float value = mDotD + t * nDotD; - if (value < float(0.0)) { + if (value < 0.0f) { // Check int32_tersection between the ray and the "p" sphere endcap of the capsule - Vector3 hitLocalPoint; + vec3 hitLocalPoint; float hitFraction; if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = hitFraction; raycastInfo.worldPoint = hitLocalPoint; - Vector3 normalDirection = hitLocalPoint - p; + vec3 normalDirection = hitLocalPoint - p; raycastInfo.worldNormal = normalDirection; return true; @@ -180,14 +180,14 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* else if (value > dDotD) { // If the int32_tersection is outside the finite cylinder on the "q" side // Check int32_tersection between the ray and the "q" sphere endcap of the capsule - Vector3 hitLocalPoint; + vec3 hitLocalPoint; float hitFraction; if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = hitFraction; raycastInfo.worldPoint = hitLocalPoint; - Vector3 normalDirection = hitLocalPoint - q; + vec3 normalDirection = hitLocalPoint - q; raycastInfo.worldNormal = normalDirection; return true; @@ -200,52 +200,52 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* // If the int32_tersection is behind the origin of the ray or beyond the maximum // raycasting distance, we return no hit - if (t < float(0.0) || t > ray.maxFraction) return false; + if (t < 0.0f || t > ray.maxFraction) return false; // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; + vec3 localHitPoint = ray.point1 + t * n; raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; raycastInfo.worldPoint = localHitPoint; - Vector3 v = localHitPoint - p; - Vector3 w = (v.dot(d) / d.lengthSquare()) * d; - Vector3 normalDirection = (localHitPoint - (p + w)).getUnit(); + vec3 v = localHitPoint - p; + vec3 w = (v.dot(d) / d.length2()) * d; + vec3 normalDirection = (localHitPoint - (p + w)).safeNormalized(); raycastInfo.worldNormal = normalDirection; return true; } // Raycasting method between a ray one of the two spheres end cap of the capsule -bool CapsuleShape::raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2, - const Vector3& sphereCenter, float maxFraction, - Vector3& hitLocalPoint, float& hitFraction) const { +bool CapsuleShape::raycastWithSphereEndCap(const vec3& point1, const vec3& point2, + const vec3& sphereCenter, float maxFraction, + vec3& hitLocalPoint, float& hitFraction) const { - const Vector3 m = point1 - sphereCenter; - float c = m.dot(m) - mMargin * mMargin; + const vec3 m = point1 - sphereCenter; + float c = m.dot(m) - m_margin * m_margin; // If the origin of the ray is inside the sphere, we return no int32_tersection - if (c < float(0.0)) return false; + if (c < 0.0f) return false; - const Vector3 rayDirection = point2 - point1; + const vec3 rayDirection = point2 - point1; float b = m.dot(rayDirection); // If the origin of the ray is outside the sphere and the ray // is pointing away from the sphere, there is no int32_tersection - if (b > float(0.0)) return false; + if (b > 0.0f) return false; - float raySquareLength = rayDirection.lengthSquare(); + float raySquareLength = rayDirection.length2(); // Compute the discriminant of the quadratic equation float discriminant = b * b - raySquareLength * c; // If the discriminant is negative or the ray length is very small, there is no int32_tersection - if (discriminant < float(0.0) || raySquareLength < MACHINE_EPSILON) return false; + if (discriminant < 0.0f || raySquareLength < MACHINE_EPSILON) return false; // Compute the solution "t" closest to the origin float t = -b - std::sqrt(discriminant); - assert(t >= float(0.0)); + assert(t >= 0.0f); // If the hit point is withing the segment ray fraction if (t < maxFraction * raySquareLength) { diff --git a/ephysics/collision/shapes/CapsuleShape.h b/ephysics/collision/shapes/CapsuleShape.h index 1fa995e..50bda94 100644 --- a/ephysics/collision/shapes/CapsuleShape.h +++ b/ephysics/collision/shapes/CapsuleShape.h @@ -30,7 +30,7 @@ class CapsuleShape : public ConvexShape { // -------------------- Attributes -------------------- // /// Half height of the capsule (height = distance between the centers of the two spheres) - float mHalfHeight; + float m_halfHeight; // -------------------- Methods -------------------- // @@ -41,19 +41,19 @@ class CapsuleShape : public ConvexShape { CapsuleShape& operator=(const CapsuleShape& shape); /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, + virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, void** cachedCollisionData) const; /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; + virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; /// Raycasting method between a ray one of the two spheres end cap of the capsule - bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2, - const Vector3& sphereCenter, float maxFraction, - Vector3& hitLocalPoint, float& hitFraction) const; + bool raycastWithSphereEndCap(const vec3& point1, const vec3& point2, + const vec3& sphereCenter, float maxFraction, + vec3& hitLocalPoint, float& hitFraction) const; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const; @@ -75,13 +75,13 @@ class CapsuleShape : public ConvexShape { float getHeight() const; /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + virtual void setLocalScaling(const vec3& scaling); /// Return the local bounds of the shape in x, y and z directions - virtual void getLocalBounds(Vector3& min, Vector3& max) const; + virtual void getLocalBounds(vec3& min, vec3& max) const; /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const; + virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; }; // Get the radius of the capsule @@ -89,7 +89,7 @@ class CapsuleShape : public ConvexShape { * @return The radius of the capsule shape (in meters) */ inline float CapsuleShape::getRadius() const { - return mMargin; + return m_margin; } // Return the height of the capsule @@ -97,14 +97,14 @@ inline float CapsuleShape::getRadius() const { * @return The height of the capsule shape (in meters) */ inline float CapsuleShape::getHeight() const { - return mHalfHeight + mHalfHeight; + return m_halfHeight + m_halfHeight; } // Set the scaling vector of the collision shape -inline void CapsuleShape::setLocalScaling(const Vector3& scaling) { +inline void CapsuleShape::setLocalScaling(const vec3& scaling) { - mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y; - mMargin = (mMargin / mScaling.x) * scaling.x; + m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y(); + m_margin = (m_margin / m_scaling.x()) * scaling.x(); CollisionShape::setLocalScaling(scaling); } @@ -120,17 +120,17 @@ inline size_t CapsuleShape::getSizeInBytes() const { * @param min The minimum bounds of the shape in local-space coordinates * @param max The maximum bounds of the shape in local-space coordinates */ -inline void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const { +inline void CapsuleShape::getLocalBounds(vec3& min, vec3& max) const { // Maximum bounds - max.x = mMargin; - max.y = mHalfHeight + mMargin; - max.z = mMargin; + max.setX(m_margin); + max.setY(m_halfHeight + m_margin); + max.setZ(m_margin); // Minimum bounds - min.x = -mMargin; - min.y = -max.y; - min.z = min.x; + min.setX(-m_margin); + min.setY(-max.y()); + min.setZ(min.x()); } // Return a local support point in a given direction without the object margin. @@ -140,21 +140,21 @@ inline void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const { /// Therefore, in this method, we compute the support points of both top and bottom spheres of /// the capsule and return the point with the maximum dot product with the direction vector. Note /// that the object margin is implicitly the radius and height of the capsule. -inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction, +inline vec3 CapsuleShape::getLocalSupportPointWithoutMargin(const vec3& direction, void** cachedCollisionData) const { // Support point top sphere - float dotProductTop = mHalfHeight * direction.y; + float dotProductTop = m_halfHeight * direction.y(); // Support point bottom sphere - float dotProductBottom = -mHalfHeight * direction.y; + float dotProductBottom = -m_halfHeight * direction.y(); // Return the point with the maximum dot product if (dotProductTop > dotProductBottom) { - return Vector3(0, mHalfHeight, 0); + return vec3(0, m_halfHeight, 0); } else { - return Vector3(0, -mHalfHeight, 0); + return vec3(0, -m_halfHeight, 0); } } diff --git a/ephysics/collision/shapes/CollisionShape.cpp b/ephysics/collision/shapes/CollisionShape.cpp index d60c201..c258582 100644 --- a/ephysics/collision/shapes/CollisionShape.cpp +++ b/ephysics/collision/shapes/CollisionShape.cpp @@ -13,7 +13,7 @@ using namespace reactphysics3d; // Constructor -CollisionShape::CollisionShape(CollisionShapeType type) : m_type(type), mScaling(1.0, 1.0, 1.0) { +CollisionShape::CollisionShape(CollisionShapeType type) : m_type(type), m_scaling(1.0, 1.0, 1.0) { } @@ -26,29 +26,29 @@ CollisionShape::~CollisionShape() { /** * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape * computed in world-space coordinates - * @param transform Transform used to compute the AABB of the collision shape + * @param transform etk::Transform3D used to compute the AABB of the collision shape */ -void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const { +void CollisionShape::computeAABB(AABB& aabb, const etk::Transform3D& transform) const { PROFILE("CollisionShape::computeAABB()"); // Get the local bounds in x,y and z direction - Vector3 minBounds; - Vector3 maxBounds; + vec3 minBounds; + vec3 maxBounds; getLocalBounds(minBounds, maxBounds); // Rotate the local bounds according to the orientation of the body - Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsoluteMatrix(); - Vector3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds), + etk::Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsolute(); + vec3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds), worldAxis.getColumn(1).dot(minBounds), worldAxis.getColumn(2).dot(minBounds)); - Vector3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds), + vec3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds), worldAxis.getColumn(1).dot(maxBounds), worldAxis.getColumn(2).dot(maxBounds)); // Compute the minimum and maximum coordinates of the rotated extents - Vector3 minCoordinates = transform.getPosition() + worldMinBounds; - Vector3 maxCoordinates = transform.getPosition() + worldMaxBounds; + vec3 minCoordinates = transform.getPosition() + worldMinBounds; + vec3 maxCoordinates = transform.getPosition() + worldMaxBounds; // Update the AABB with the new minimum and maximum coordinates aabb.setMin(minCoordinates); diff --git a/ephysics/collision/shapes/CollisionShape.h b/ephysics/collision/shapes/CollisionShape.h index c0b02fa..e411303 100644 --- a/ephysics/collision/shapes/CollisionShape.h +++ b/ephysics/collision/shapes/CollisionShape.h @@ -8,8 +8,8 @@ // Libraries #include #include -#include -#include +#include +#include #include #include #include @@ -42,7 +42,7 @@ class CollisionShape { CollisionShapeType m_type; /// Scaling vector of the collision shape - Vector3 mScaling; + vec3 m_scaling; // -------------------- Methods -------------------- // @@ -53,7 +53,7 @@ class CollisionShape { CollisionShape& operator=(const CollisionShape& shape); /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0; + virtual bool testPointInside(const vec3& worldPoint, ProxyShape* proxyShape) const=0; /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const=0; @@ -78,19 +78,19 @@ class CollisionShape { virtual bool isConvex() const=0; /// Return the local bounds of the shape in x, y and z directions - virtual void getLocalBounds(Vector3& min, Vector3& max) const=0; + virtual void getLocalBounds(vec3& min, vec3& max) const=0; /// Return the scaling vector of the collision shape - Vector3 getScaling() const; + vec3 getScaling() const; /// Set the local scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + virtual void setLocalScaling(const vec3& scaling); /// Return the local inertia tensor of the collision shapes - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const=0; + virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const=0; /// Compute the world-space AABB of the collision shape given a transform - virtual void computeAABB(AABB& aabb, const Transform& transform) const; + virtual void computeAABB(AABB& aabb, const etk::Transform3D& transform) const; /// Return true if the collision shape type is a convex shape static bool isConvex(CollisionShapeType shapeType); @@ -119,13 +119,13 @@ inline bool CollisionShape::isConvex(CollisionShapeType shapeType) { } // Return the scaling vector of the collision shape -inline Vector3 CollisionShape::getScaling() const { - return mScaling; +inline vec3 CollisionShape::getScaling() const { + return m_scaling; } // Set the scaling vector of the collision shape -inline void CollisionShape::setLocalScaling(const Vector3& scaling) { - mScaling = scaling; +inline void CollisionShape::setLocalScaling(const vec3& scaling) { + m_scaling = scaling; } // Return the maximum number of contact manifolds allowed in an overlapping diff --git a/ephysics/collision/shapes/ConcaveMeshShape.cpp b/ephysics/collision/shapes/ConcaveMeshShape.cpp index e9c39e7..7d06450 100644 --- a/ephysics/collision/shapes/ConcaveMeshShape.cpp +++ b/ephysics/collision/shapes/ConcaveMeshShape.cpp @@ -40,7 +40,7 @@ void ConcaveMeshShape::initBVHTree() { // For each triangle of the concave mesh for (uint32_t triangleIndex=0; triangleIndexgetNbTriangles(); triangleIndex++) { void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride); - Vector3 trianglePoints[3]; + vec3 trianglePoints[3]; // For each vertex of the triangle for (int32_t k=0; k < 3; k++) { // Get the index of the current vertex in the triangle @@ -55,14 +55,14 @@ void ConcaveMeshShape::initBVHTree() { // Get the vertices components of the triangle if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) { const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride); - trianglePoints[k][0] = float(vertices[0]) * mScaling.x; - trianglePoints[k][1] = float(vertices[1]) * mScaling.y; - trianglePoints[k][2] = float(vertices[2]) * mScaling.z; + trianglePoints[k][0] = float(vertices[0]) * m_scaling.x(); + trianglePoints[k][1] = float(vertices[1]) * m_scaling.y(); + trianglePoints[k][2] = float(vertices[2]) * m_scaling.z(); } else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) { const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride); - trianglePoints[k][0] = float(vertices[0]) * mScaling.x; - trianglePoints[k][1] = float(vertices[1]) * mScaling.y; - trianglePoints[k][2] = float(vertices[2]) * mScaling.z; + trianglePoints[k][0] = float(vertices[0]) * m_scaling.x(); + trianglePoints[k][1] = float(vertices[1]) * m_scaling.y(); + trianglePoints[k][2] = float(vertices[2]) * m_scaling.z(); } else { assert(false); } @@ -78,7 +78,7 @@ void ConcaveMeshShape::initBVHTree() { // Return the three vertices coordinates (in the array outTriangleVertices) of a triangle // given the start vertex index pointer of the triangle -void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32_t subPart, int32_t triangleIndex, Vector3* outTriangleVertices) const { +void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32_t subPart, int32_t triangleIndex, vec3* outTriangleVertices) const { // Get the triangle vertex array of the current sub-part TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(subPart); if (triangleVertexArray == nullptr) { @@ -106,14 +106,14 @@ void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32_t subPart, int3 // Get the vertices components of the triangle if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) { const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride); - outTriangleVertices[k][0] = float(vertices[0]) * mScaling.x; - outTriangleVertices[k][1] = float(vertices[1]) * mScaling.y; - outTriangleVertices[k][2] = float(vertices[2]) * mScaling.z; + outTriangleVertices[k][0] = float(vertices[0]) * m_scaling.x(); + outTriangleVertices[k][1] = float(vertices[1]) * m_scaling.y(); + outTriangleVertices[k][2] = float(vertices[2]) * m_scaling.z(); } else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) { const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride); - outTriangleVertices[k][0] = float(vertices[0]) * mScaling.x; - outTriangleVertices[k][1] = float(vertices[1]) * mScaling.y; - outTriangleVertices[k][2] = float(vertices[2]) * mScaling.z; + outTriangleVertices[k][0] = float(vertices[0]) * m_scaling.x(); + outTriangleVertices[k][1] = float(vertices[1]) * m_scaling.y(); + outTriangleVertices[k][2] = float(vertices[2]) * m_scaling.z(); } else { assert(false); } @@ -158,7 +158,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { // Get the node data (triangle index and mesh subpart index) int32_t* data = m_dynamicAABBTree.getNodeDataInt(*it); // Get the triangle vertices for this node from the concave mesh shape - Vector3 trianglePoints[3]; + vec3 trianglePoints[3]; m_concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints); // Create a triangle collision shape float margin = m_concaveMeshShape.getTriangleMargin(); @@ -169,7 +169,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { bool isTriangleHit = triangleShape.raycast(m_ray, raycastInfo, m_proxyShape); // If the ray hit the collision shape if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) { - assert(raycastInfo.hitFraction >= float(0.0)); + assert(raycastInfo.hitFraction >= 0.0f); m_raycastInfo.body = raycastInfo.body; m_raycastInfo.proxyShape = raycastInfo.proxyShape; m_raycastInfo.hitFraction = raycastInfo.hitFraction; diff --git a/ephysics/collision/shapes/ConcaveMeshShape.h b/ephysics/collision/shapes/ConcaveMeshShape.h index 5f93ca9..c500a4d 100644 --- a/ephysics/collision/shapes/ConcaveMeshShape.h +++ b/ephysics/collision/shapes/ConcaveMeshShape.h @@ -117,7 +117,7 @@ class ConcaveMeshShape : public ConcaveShape { /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle /// given the start vertex index pointer of the triangle. void getTriangleVerticesWithIndexPointer(int32_t subPart, int32_t triangleIndex, - Vector3* outTriangleVertices) const; + vec3* outTriangleVertices) const; public: @@ -128,13 +128,13 @@ class ConcaveMeshShape : public ConcaveShape { ~ConcaveMeshShape(); /// Return the local bounds of the shape in x, y and z directions. - virtual void getLocalBounds(Vector3& min, Vector3& max) const; + virtual void getLocalBounds(vec3& min, vec3& max) const; /// Set the local scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + virtual void setLocalScaling(const vec3& scaling); /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const; + virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; /// Use a callback method on all triangles of the concave shape inside a given AABB virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const; @@ -156,7 +156,7 @@ inline size_t ConcaveMeshShape::getSizeInBytes() const { * @param min The minimum bounds of the shape in local-space coordinates * @param max The maximum bounds of the shape in local-space coordinates */ -inline void ConcaveMeshShape::getLocalBounds(Vector3& min, Vector3& max) const { +inline void ConcaveMeshShape::getLocalBounds(vec3& min, vec3& max) const { // Get the AABB of the whole tree AABB treeAABB = m_dynamicAABBTree.getRootAABB(); @@ -166,7 +166,7 @@ inline void ConcaveMeshShape::getLocalBounds(Vector3& min, Vector3& max) const { } // Set the local scaling vector of the collision shape -inline void ConcaveMeshShape::setLocalScaling(const Vector3& scaling) { +inline void ConcaveMeshShape::setLocalScaling(const vec3& scaling) { CollisionShape::setLocalScaling(scaling); @@ -183,13 +183,13 @@ inline void ConcaveMeshShape::setLocalScaling(const Vector3& scaling) { * coordinates * @param mass Mass to use to compute the inertia tensor of the collision shape */ -inline void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { +inline void ConcaveMeshShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { // Default inertia tensor // Note that this is not very realistic for a concave triangle mesh. // However, in most cases, it will only be used static bodies and therefore, // the inertia tensor is not used. - tensor.setAllValues(mass, 0, 0, + tensor.setValue(mass, 0, 0, 0, mass, 0, 0, 0, mass); } @@ -202,7 +202,7 @@ inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int32_t nod int32_t* data = m_dynamicAABBTree.getNodeDataInt(nodeId); // Get the triangle vertices for this node from the concave mesh shape - Vector3 trianglePoints[3]; + vec3 trianglePoints[3]; m_concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints); // Call the callback to test narrow-phase collision with this triangle diff --git a/ephysics/collision/shapes/ConcaveShape.h b/ephysics/collision/shapes/ConcaveShape.h index 621b567..06470d0 100644 --- a/ephysics/collision/shapes/ConcaveShape.h +++ b/ephysics/collision/shapes/ConcaveShape.h @@ -23,7 +23,7 @@ class TriangleCallback { virtual ~TriangleCallback() = default; /// Report a triangle - virtual void testTriangle(const Vector3* trianglePoints)=0; + virtual void testTriangle(const vec3* trianglePoints)=0; }; @@ -57,7 +57,7 @@ class ConcaveShape : public CollisionShape { ConcaveShape& operator=(const ConcaveShape& shape); /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; + virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; public : @@ -102,7 +102,7 @@ inline bool ConcaveShape::isConvex() const { } // Return true if a point is inside the collision shape -inline bool ConcaveShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { +inline bool ConcaveShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { return false; } diff --git a/ephysics/collision/shapes/ConeShape.cpp b/ephysics/collision/shapes/ConeShape.cpp index 068a0bd..9da6154 100644 --- a/ephysics/collision/shapes/ConeShape.cpp +++ b/ephysics/collision/shapes/ConeShape.cpp @@ -19,9 +19,9 @@ using namespace reactphysics3d; * @param margin Collision margin (in meters) around the collision shape */ ConeShape::ConeShape(float radius, float height, float margin) - : ConvexShape(CONE, margin), mRadius(radius), mHalfHeight(height * float(0.5)) { - assert(mRadius > float(0.0)); - assert(mHalfHeight > float(0.0)); + : ConvexShape(CONE, margin), mRadius(radius), m_halfHeight(height * 0.5f) { + assert(mRadius > 0.0f); + assert(m_halfHeight > 0.0f); // Compute the sine of the semi-angle at the apex point mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height)); @@ -33,24 +33,24 @@ ConeShape::~ConeShape() { } // Return a local support point in a given direction without the object margin -Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction, +vec3 ConeShape::getLocalSupportPointWithoutMargin(const vec3& direction, void** cachedCollisionData) const { - const Vector3& v = direction; + const vec3& v = direction; float sinThetaTimesLengthV = mSinTheta * v.length(); - Vector3 supportPoint; + vec3 supportPoint; - if (v.y > sinThetaTimesLengthV) { - supportPoint = Vector3(0.0, mHalfHeight, 0.0); + if (v.y() > sinThetaTimesLengthV) { + supportPoint = vec3(0.0, m_halfHeight, 0.0); } else { - float projectedLength = sqrt(v.x * v.x + v.z * v.z); + float projectedLength = sqrt(v.x() * v.x() + v.z() * v.z()); if (projectedLength > MACHINE_EPSILON) { float d = mRadius / projectedLength; - supportPoint = Vector3(v.x * d, -mHalfHeight, v.z * d); + supportPoint = vec3(v.x() * d, -m_halfHeight, v.z() * d); } else { - supportPoint = Vector3(0.0, -mHalfHeight, 0.0); + supportPoint = vec3(0.0, -m_halfHeight, 0.0); } } @@ -63,33 +63,33 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction, // http://www.geometrictools.com/Documentation/IntersectionLineCone.pdf bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - const Vector3 r = ray.point2 - ray.point1; + const vec3 r = ray.point2 - ray.point1; const float epsilon = float(0.00001); - Vector3 V(0, mHalfHeight, 0); - Vector3 centerBase(0, -mHalfHeight, 0); - Vector3 axis(0, float(-1.0), 0); - float heightSquare = float(4.0) * mHalfHeight * mHalfHeight; + vec3 V(0, m_halfHeight, 0); + vec3 centerBase(0, -m_halfHeight, 0); + vec3 axis(0, float(-1.0), 0); + float heightSquare = float(4.0) * m_halfHeight * m_halfHeight; float cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius); - float factor = float(1.0) - cosThetaSquare; - Vector3 delta = ray.point1 - V; - float c0 = -cosThetaSquare * delta.x * delta.x + factor * delta.y * delta.y - - cosThetaSquare * delta.z * delta.z; - float c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z; - float c2 = -cosThetaSquare * r.x * r.x + factor * r.y * r.y - cosThetaSquare * r.z * r.z; + float factor = 1.0f - cosThetaSquare; + vec3 delta = ray.point1 - V; + float c0 = -cosThetaSquare * delta.x() * delta.x() + factor * delta.y() * delta.y() - + cosThetaSquare * delta.z() * delta.z(); + float c1 = -cosThetaSquare * delta.x() * r.x() + factor * delta.y() * r.y() - cosThetaSquare * delta.z() * r.z(); + float c2 = -cosThetaSquare * r.x() * r.x() + factor * r.y() * r.y() - cosThetaSquare * r.z() * r.z(); float tHit[] = {float(-1.0), float(-1.0), float(-1.0)}; - Vector3 localHitPoint[3]; - Vector3 localNormal[3]; + vec3 localHitPoint[3]; + vec3 localNormal[3]; // If c2 is different from zero if (std::abs(c2) > MACHINE_EPSILON) { float gamma = c1 * c1 - c0 * c2; // If there is no real roots in the quadratic equation - if (gamma < float(0.0)) { + if (gamma < 0.0f) { return false; } - else if (gamma > float(0.0)) { // The equation has two real roots + else if (gamma > 0.0f) { // The equation has two real roots // Compute two int32_tersections float sqrRoot = std::sqrt(gamma); @@ -124,31 +124,31 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr localHitPoint[1] = ray.point1 + tHit[1] * r; // Only keep hit points in one side of the double cone (the cone we are int32_terested in) - if (axis.dot(localHitPoint[0] - V) < float(0.0)) { + if (axis.dot(localHitPoint[0] - V) < 0.0f) { tHit[0] = float(-1.0); } - if (axis.dot(localHitPoint[1] - V) < float(0.0)) { + if (axis.dot(localHitPoint[1] - V) < 0.0f) { tHit[1] = float(-1.0); } // Only keep hit points that are within the correct height of the cone - if (localHitPoint[0].y < float(-mHalfHeight)) { + if (localHitPoint[0].y() < float(-m_halfHeight)) { tHit[0] = float(-1.0); } - if (localHitPoint[1].y < float(-mHalfHeight)) { + if (localHitPoint[1].y() < float(-m_halfHeight)) { tHit[1] = float(-1.0); } // If the ray is in direction of the base plane of the cone - if (r.y > epsilon) { + if (r.y() > epsilon) { // Compute the int32_tersection with the base plane of the cone - tHit[2] = (-ray.point1.y - mHalfHeight) / (r.y); + tHit[2] = (-ray.point1.y() - m_halfHeight) / (r.y()); // Only keep this int32_tersection if it is inside the cone radius localHitPoint[2] = ray.point1 + tHit[2] * r; - if ((localHitPoint[2] - centerBase).lengthSquare() > mRadius * mRadius) { + if ((localHitPoint[2] - centerBase).length2() > mRadius * mRadius) { tHit[2] = float(-1.0); } @@ -160,7 +160,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr int32_t hitIndex = -1; float t = DECIMAL_LARGEST; for (int32_t i=0; i<3; i++) { - if (tHit[i] < float(0.0)) continue; + if (tHit[i] < 0.0f) continue; if (tHit[i] < t) { hitIndex = i; t = tHit[hitIndex]; @@ -172,17 +172,17 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr // Compute the normal direction for hit against side of the cone if (hitIndex != 2) { - float h = float(2.0) * mHalfHeight; - float value1 = (localHitPoint[hitIndex].x * localHitPoint[hitIndex].x + - localHitPoint[hitIndex].z * localHitPoint[hitIndex].z); + float h = float(2.0) * m_halfHeight; + float value1 = (localHitPoint[hitIndex].x() * localHitPoint[hitIndex].x() + + localHitPoint[hitIndex].z() * localHitPoint[hitIndex].z()); float rOverH = mRadius / h; - float value2 = float(1.0) + rOverH * rOverH; - float factor = float(1.0) / std::sqrt(value1 * value2); - float x = localHitPoint[hitIndex].x * factor; - float z = localHitPoint[hitIndex].z * factor; - localNormal[hitIndex].x = x; - localNormal[hitIndex].y = std::sqrt(x * x + z * z) * rOverH; - localNormal[hitIndex].z = z; + float value2 = 1.0f + rOverH * rOverH; + float factor = 1.0f / std::sqrt(value1 * value2); + float x = localHitPoint[hitIndex].x() * factor; + float z = localHitPoint[hitIndex].z() * factor; + localNormal[hitIndex].setX(x); + localNormal[hitIndex].setY(std::sqrt(x * x + z * z) * rOverH); + localNormal[hitIndex].setZ(z); } raycastInfo.body = proxyShape->getBody(); diff --git a/ephysics/collision/shapes/ConeShape.h b/ephysics/collision/shapes/ConeShape.h index 522e3c5..7b49599 100644 --- a/ephysics/collision/shapes/ConeShape.h +++ b/ephysics/collision/shapes/ConeShape.h @@ -38,7 +38,7 @@ class ConeShape : public ConvexShape { float mRadius; /// Half height of the cone - float mHalfHeight; + float m_halfHeight; /// sine of the semi angle at the apex point float mSinTheta; @@ -52,11 +52,11 @@ class ConeShape : public ConvexShape { ConeShape& operator=(const ConeShape& shape); /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, + virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, void** cachedCollisionData) const; /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; + virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; @@ -81,13 +81,13 @@ class ConeShape : public ConvexShape { float getHeight() const; /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + virtual void setLocalScaling(const vec3& scaling); /// Return the local bounds of the shape in x, y and z directions - virtual void getLocalBounds(Vector3& min, Vector3& max) const; + virtual void getLocalBounds(vec3& min, vec3& max) const; /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const; + virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; }; // Return the radius @@ -103,14 +103,14 @@ inline float ConeShape::getRadius() const { * @return Height of the cone (in meters) */ inline float ConeShape::getHeight() const { - return float(2.0) * mHalfHeight; + return float(2.0) * m_halfHeight; } // Set the scaling vector of the collision shape -inline void ConeShape::setLocalScaling(const Vector3& scaling) { +inline void ConeShape::setLocalScaling(const vec3& scaling) { - mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y; - mRadius = (mRadius / mScaling.x) * scaling.x; + m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y(); + mRadius = (mRadius / m_scaling.x()) * scaling.x(); CollisionShape::setLocalScaling(scaling); } @@ -125,17 +125,17 @@ inline size_t ConeShape::getSizeInBytes() const { * @param min The minimum bounds of the shape in local-space coordinates * @param max The maximum bounds of the shape in local-space coordinates */ -inline void ConeShape::getLocalBounds(Vector3& min, Vector3& max) const { +inline void ConeShape::getLocalBounds(vec3& min, vec3& max) const { // Maximum bounds - max.x = mRadius + mMargin; - max.y = mHalfHeight + mMargin; - max.z = max.x; + max.setX(mRadius + m_margin); + max.setY(m_halfHeight + m_margin); + max.setZ(max.x()); // Minimum bounds - min.x = -max.x; - min.y = -max.y; - min.z = min.x; + min.setX(-max.x()); + min.setY(-max.y()); + min.setZ(min.x()); } // Return the local inertia tensor of the collision shape @@ -144,20 +144,20 @@ inline void ConeShape::getLocalBounds(Vector3& min, Vector3& max) const { * coordinates * @param mass Mass to use to compute the inertia tensor of the collision shape */ -inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { +inline void ConeShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { float rSquare = mRadius * mRadius; - float diagXZ = float(0.15) * mass * (rSquare + mHalfHeight); - tensor.setAllValues(diagXZ, 0.0, 0.0, + float diagXZ = float(0.15) * mass * (rSquare + m_halfHeight); + tensor.setValue(diagXZ, 0.0, 0.0, 0.0, float(0.3) * mass * rSquare, 0.0, 0.0, 0.0, diagXZ); } // Return true if a point is inside the collision shape -inline bool ConeShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { - const float radiusHeight = mRadius * (-localPoint.y + mHalfHeight) / - (mHalfHeight * float(2.0)); - return (localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) && - (localPoint.x * localPoint.x + localPoint.z * localPoint.z < radiusHeight *radiusHeight); +inline bool ConeShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { + const float radiusHeight = mRadius * (-localPoint.y() + m_halfHeight) / + (m_halfHeight * float(2.0)); + return (localPoint.y() < m_halfHeight && localPoint.y() > -m_halfHeight) && + (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z() < radiusHeight *radiusHeight); } } diff --git a/ephysics/collision/shapes/ConvexMeshShape.cpp b/ephysics/collision/shapes/ConvexMeshShape.cpp index 4d89597..04b424a 100644 --- a/ephysics/collision/shapes/ConvexMeshShape.cpp +++ b/ephysics/collision/shapes/ConvexMeshShape.cpp @@ -30,7 +30,7 @@ ConvexMeshShape::ConvexMeshShape(const float* arrayVertices, uint32_t nbVertices // Copy all the vertices int32_to the int32_ternal array for (uint32_t i=0; i= float(0.0)); + assert(maxDotProduct >= 0.0f); // Return the vertex with the largest dot product in the support direction - return m_vertices[indexMaxDotProduct] * mScaling; + return m_vertices[indexMaxDotProduct] * m_scaling; } } @@ -217,33 +217,43 @@ void ConvexMeshShape::recalculateBounds() { // TODO : Only works if the local origin is inside the mesh // => Make it more robust (init with first vertex of mesh instead) - m_minBounds.setToZero(); - m_maxBounds.setToZero(); + m_minBounds.setZero(); + m_maxBounds.setZero(); // For each vertex of the mesh for (uint32_t i=0; i m_maxBounds.x) m_maxBounds.x = m_vertices[i].x; - if (m_vertices[i].x < m_minBounds.x) m_minBounds.x = m_vertices[i].x; + if (m_vertices[i].x() > m_maxBounds.x()) { + m_maxBounds.setX(m_vertices[i].x()); + } + if (m_vertices[i].x() < m_minBounds.x()) { + m_minBounds.setX(m_vertices[i].x()); + } + if (m_vertices[i].y() > m_maxBounds.y()) { + m_maxBounds.setY(m_vertices[i].y()); + } + if (m_vertices[i].y() < m_minBounds.y()) { + m_minBounds.setY(m_vertices[i].y()); + } - if (m_vertices[i].y > m_maxBounds.y) m_maxBounds.y = m_vertices[i].y; - if (m_vertices[i].y < m_minBounds.y) m_minBounds.y = m_vertices[i].y; - - if (m_vertices[i].z > m_maxBounds.z) m_maxBounds.z = m_vertices[i].z; - if (m_vertices[i].z < m_minBounds.z) m_minBounds.z = m_vertices[i].z; + if (m_vertices[i].z() > m_maxBounds.z()) { + m_maxBounds.setZ(m_vertices[i].z()); + } + if (m_vertices[i].z() < m_minBounds.z()) { + m_minBounds.setZ(m_vertices[i].z()); + } } // Apply the local scaling factor - m_maxBounds = m_maxBounds * mScaling; - m_minBounds = m_minBounds * mScaling; + m_maxBounds = m_maxBounds * m_scaling; + m_minBounds = m_minBounds * m_scaling; // Add the object margin to the bounds - m_maxBounds += Vector3(mMargin, mMargin, mMargin); - m_minBounds -= Vector3(mMargin, mMargin, mMargin); + m_maxBounds += vec3(m_margin, m_margin, m_margin); + m_minBounds -= vec3(m_margin, m_margin, m_margin); } // Raycast method with feedback information bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - return proxyShape->m_body->m_world.m_collisionDetection.m_narrowPhaseGJKAlgorithm.raycast( - ray, proxyShape, raycastInfo); + return proxyShape->m_body->m_world.m_collisionDetection.m_narrowPhaseGJKAlgorithm.raycast(ray, proxyShape, raycastInfo); } diff --git a/ephysics/collision/shapes/ConvexMeshShape.h b/ephysics/collision/shapes/ConvexMeshShape.h index b15377f..4d0b467 100644 --- a/ephysics/collision/shapes/ConvexMeshShape.h +++ b/ephysics/collision/shapes/ConvexMeshShape.h @@ -44,16 +44,16 @@ class ConvexMeshShape : public ConvexShape { // -------------------- Attributes -------------------- // /// Array with the vertices of the mesh - std::vector m_vertices; + std::vector m_vertices; /// Number of vertices in the mesh uint32_t m_numberVertices; /// Mesh minimum bounds in the three local x, y and z directions - Vector3 m_minBounds; + vec3 m_minBounds; /// Mesh maximum bounds in the three local x, y and z directions - Vector3 m_maxBounds; + vec3 m_maxBounds; /// True if the shape contains the edges of the convex mesh in order to /// make the collision detection faster @@ -74,14 +74,14 @@ class ConvexMeshShape : public ConvexShape { void recalculateBounds(); /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + virtual void setLocalScaling(const vec3& scaling); /// Return a local support point in a given direction without the object margin. - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, + virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, void** cachedCollisionData) const; /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; + virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; @@ -108,13 +108,13 @@ class ConvexMeshShape : public ConvexShape { virtual ~ConvexMeshShape(); /// Return the local bounds of the shape in x, y and z directions - virtual void getLocalBounds(Vector3& min, Vector3& max) const; + virtual void getLocalBounds(vec3& min, vec3& max) const; /// Return the local inertia tensor of the collision shape. - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const; + virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; /// Add a vertex int32_to the convex mesh - void addVertex(const Vector3& vertex); + void addVertex(const vec3& vertex); /// Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge. void addEdge(uint32_t v1, uint32_t v2); @@ -128,7 +128,7 @@ class ConvexMeshShape : public ConvexShape { }; /// Set the scaling vector of the collision shape -inline void ConvexMeshShape::setLocalScaling(const Vector3& scaling) { +inline void ConvexMeshShape::setLocalScaling(const vec3& scaling) { ConvexShape::setLocalScaling(scaling); recalculateBounds(); } @@ -143,7 +143,7 @@ inline size_t ConvexMeshShape::getSizeInBytes() const { * @param min The minimum bounds of the shape in local-space coordinates * @param max The maximum bounds of the shape in local-space coordinates */ -inline void ConvexMeshShape::getLocalBounds(Vector3& min, Vector3& max) const { +inline void ConvexMeshShape::getLocalBounds(vec3& min, vec3& max) const { min = m_minBounds; max = m_maxBounds; } @@ -156,14 +156,14 @@ inline void ConvexMeshShape::getLocalBounds(Vector3& min, Vector3& max) const { * coordinates * @param mass Mass to use to compute the inertia tensor of the collision shape */ -inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { - float factor = (float(1.0) / float(3.0)) * mass; - Vector3 realExtent = float(0.5) * (m_maxBounds - m_minBounds); - assert(realExtent.x > 0 && realExtent.y > 0 && realExtent.z > 0); - float xSquare = realExtent.x * realExtent.x; - float ySquare = realExtent.y * realExtent.y; - float zSquare = realExtent.z * realExtent.z; - tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0, +inline void ConvexMeshShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { + float factor = (1.0f / float(3.0)) * mass; + vec3 realExtent = 0.5f * (m_maxBounds - m_minBounds); + assert(realExtent.x() > 0 && realExtent.y() > 0 && realExtent.z() > 0); + float xSquare = realExtent.x() * realExtent.x(); + float ySquare = realExtent.y() * realExtent.y(); + float zSquare = realExtent.z() * realExtent.z(); + tensor.setValue(factor * (ySquare + zSquare), 0.0, 0.0, 0.0, factor * (xSquare + zSquare), 0.0, 0.0, 0.0, factor * (xSquare + ySquare)); } @@ -172,19 +172,31 @@ inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, float /** * @param vertex Vertex to be added */ -inline void ConvexMeshShape::addVertex(const Vector3& vertex) { +inline void ConvexMeshShape::addVertex(const vec3& vertex) { // Add the vertex in to vertices array m_vertices.push_back(vertex); m_numberVertices++; // Update the bounds of the mesh - if (vertex.x * mScaling.x > m_maxBounds.x) m_maxBounds.x = vertex.x * mScaling.x; - if (vertex.x * mScaling.x < m_minBounds.x) m_minBounds.x = vertex.x * mScaling.x; - if (vertex.y * mScaling.y > m_maxBounds.y) m_maxBounds.y = vertex.y * mScaling.y; - if (vertex.y * mScaling.y < m_minBounds.y) m_minBounds.y = vertex.y * mScaling.y; - if (vertex.z * mScaling.z > m_maxBounds.z) m_maxBounds.z = vertex.z * mScaling.z; - if (vertex.z * mScaling.z < m_minBounds.z) m_minBounds.z = vertex.z * mScaling.z; + if (vertex.x() * m_scaling.x() > m_maxBounds.x()) { + m_maxBounds.setX(vertex.x() * m_scaling.x()); + } + if (vertex.x() * m_scaling.x() < m_minBounds.x()) { + m_minBounds.setX(vertex.x() * m_scaling.x()); + } + if (vertex.y() * m_scaling.y() > m_maxBounds.y()) { + m_maxBounds.setY(vertex.y() * m_scaling.y()); + } + if (vertex.y() * m_scaling.y() < m_minBounds.y()) { + m_minBounds.setY(vertex.y() * m_scaling.y()); + } + if (vertex.z() * m_scaling.z() > m_maxBounds.z()) { + m_maxBounds.setZ(vertex.z() * m_scaling.z()); + } + if (vertex.z() * m_scaling.z() < m_minBounds.z()) { + m_minBounds.setZ(vertex.z() * m_scaling.z()); + } } // Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge. @@ -231,7 +243,7 @@ inline void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) { } // Return true if a point is inside the collision shape -inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint, +inline bool ConvexMeshShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { // Use the GJK algorithm to test if the point is inside the convex mesh diff --git a/ephysics/collision/shapes/ConvexShape.cpp b/ephysics/collision/shapes/ConvexShape.cpp index 5a2c8f0..a243109 100644 --- a/ephysics/collision/shapes/ConvexShape.cpp +++ b/ephysics/collision/shapes/ConvexShape.cpp @@ -13,7 +13,7 @@ using namespace reactphysics3d; // Constructor ConvexShape::ConvexShape(CollisionShapeType type, float margin) - : CollisionShape(type), mMargin(margin) { + : CollisionShape(type), m_margin(margin) { } @@ -23,20 +23,20 @@ ConvexShape::~ConvexShape() { } // Return a local support point in a given direction with the object margin -Vector3 ConvexShape::getLocalSupportPointWithMargin(const Vector3& direction, +vec3 ConvexShape::getLocalSupportPointWithMargin(const vec3& direction, void** cachedCollisionData) const { // Get the support point without margin - Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData); + vec3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData); - if (mMargin != float(0.0)) { + if (m_margin != 0.0f) { // Add the margin to the support point - Vector3 unitVec(0.0, -1.0, 0.0); - if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) { - unitVec = direction.getUnit(); + vec3 unitVec(0.0, -1.0, 0.0); + if (direction.length2() > MACHINE_EPSILON * MACHINE_EPSILON) { + unitVec = direction.safeNormalized(); } - supportPoint += unitVec * mMargin; + supportPoint += unitVec * m_margin; } return supportPoint; diff --git a/ephysics/collision/shapes/ConvexShape.h b/ephysics/collision/shapes/ConvexShape.h index fd876da..9341e00 100644 --- a/ephysics/collision/shapes/ConvexShape.h +++ b/ephysics/collision/shapes/ConvexShape.h @@ -23,7 +23,7 @@ class ConvexShape : public CollisionShape { // -------------------- Attributes -------------------- // /// Margin used for the GJK collision detection algorithm - float mMargin; + float m_margin; // -------------------- Methods -------------------- // @@ -34,15 +34,15 @@ class ConvexShape : public CollisionShape { ConvexShape& operator=(const ConvexShape& shape); // Return a local support point in a given direction with the object margin - Vector3 getLocalSupportPointWithMargin(const Vector3& direction, + vec3 getLocalSupportPointWithMargin(const vec3& direction, void** cachedCollisionData) const; /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, + virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, void** cachedCollisionData) const=0; /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0; + virtual bool testPointInside(const vec3& worldPoint, ProxyShape* proxyShape) const=0; public : @@ -76,7 +76,7 @@ inline bool ConvexShape::isConvex() const { * @return The margin (in meters) around the collision shape */ inline float ConvexShape::getMargin() const { - return mMargin; + return m_margin; } } diff --git a/ephysics/collision/shapes/CylinderShape.cpp b/ephysics/collision/shapes/CylinderShape.cpp index 6fab781..a8f1faf 100644 --- a/ephysics/collision/shapes/CylinderShape.cpp +++ b/ephysics/collision/shapes/CylinderShape.cpp @@ -19,9 +19,9 @@ using namespace reactphysics3d; */ CylinderShape::CylinderShape(float radius, float height, float margin) : ConvexShape(CYLINDER, margin), mRadius(radius), - mHalfHeight(height/float(2.0)) { - assert(radius > float(0.0)); - assert(height > float(0.0)); + m_halfHeight(height/float(2.0)) { + assert(radius > 0.0f); + assert(height > 0.0f); } // Destructor @@ -30,24 +30,25 @@ CylinderShape::~CylinderShape() { } // Return a local support point in a given direction without the object margin -Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { - - Vector3 supportPoint(0.0, 0.0, 0.0); - float uDotv = direction.y; - Vector3 w(direction.x, 0.0, direction.z); - float lengthW = sqrt(direction.x * direction.x + direction.z * direction.z); - +vec3 CylinderShape::getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const { + vec3 supportPoint(0.0, 0.0, 0.0); + float uDotv = _direction.y(); + vec3 w(_direction.x(), 0.0, _direction.z()); + float lengthW = sqrt(_direction.x() * _direction.x() + _direction.z() * _direction.z()); if (lengthW > MACHINE_EPSILON) { - if (uDotv < 0.0) supportPoint.y = -mHalfHeight; - else supportPoint.y = mHalfHeight; + if (uDotv < 0.0) { + supportPoint.setY(-m_halfHeight); + } else { + supportPoint.setY(m_halfHeight); + } supportPoint += (mRadius / lengthW) * w; + } else { + if (uDotv < 0.0) { + supportPoint.setY(-m_halfHeight); + } else { + supportPoint.setY(m_halfHeight); + } } - else { - if (uDotv < 0.0) supportPoint.y = -mHalfHeight; - else supportPoint.y = mHalfHeight; - } - return supportPoint; } @@ -56,13 +57,13 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio /// Morgan Kaufmann. bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - const Vector3 n = ray.point2 - ray.point1; + const vec3 n = ray.point2 - ray.point1; const float epsilon = float(0.01); - Vector3 p(float(0), -mHalfHeight, float(0)); - Vector3 q(float(0), mHalfHeight, float(0)); - Vector3 d = q - p; - Vector3 m = ray.point1 - p; + vec3 p(float(0), -m_halfHeight, float(0)); + vec3 q(float(0), m_halfHeight, float(0)); + vec3 d = q - p; + vec3 m = ray.point1 - p; float t; float mDotD = m.dot(d); @@ -70,7 +71,7 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape float dDotD = d.dot(d); // Test if the segment is outside the cylinder - if (mDotD < float(0.0) && mDotD + nDotD < float(0.0)) return false; + if (mDotD < 0.0f && mDotD + nDotD < float(0.0)) return false; if (mDotD > dDotD && mDotD + nDotD > dDotD) return false; float nDotN = n.dot(n); @@ -84,26 +85,26 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape if (std::abs(a) < epsilon) { // If the origin is outside the surface of the cylinder, we return no hit - if (c > float(0.0)) return false; + if (c > 0.0f) return false; // Here we know that the segment int32_tersect an endcap of the cylinder // If the ray int32_tersects with the "p" endcap of the cylinder - if (mDotD < float(0.0)) { + if (mDotD < 0.0f) { t = -mDotN / nDotN; // If the int32_tersection is behind the origin of the ray or beyond the maximum // raycasting distance, we return no hit - if (t < float(0.0) || t > ray.maxFraction) return false; + if (t < 0.0f || t > ray.maxFraction) return false; // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; + vec3 localHitPoint = ray.point1 + t * n; raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; raycastInfo.worldPoint = localHitPoint; - Vector3 normalDirection(0, float(-1), 0); + vec3 normalDirection(0, float(-1), 0); raycastInfo.worldNormal = normalDirection; return true; @@ -114,15 +115,15 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape // If the int32_tersection is behind the origin of the ray or beyond the maximum // raycasting distance, we return no hit - if (t < float(0.0) || t > ray.maxFraction) return false; + if (t < 0.0f || t > ray.maxFraction) return false; // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; + vec3 localHitPoint = ray.point1 + t * n; raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; raycastInfo.worldPoint = localHitPoint; - Vector3 normalDirection(0, float(1.0), 0); + vec3 normalDirection(0, 1.0f, 0); raycastInfo.worldNormal = normalDirection; return true; @@ -135,35 +136,35 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape float discriminant = b * b - a * c; // If the discriminant is negative, no real roots and therfore, no hit - if (discriminant < float(0.0)) return false; + if (discriminant < 0.0f) return false; // Compute the smallest root (first int32_tersection along the ray) float t0 = t = (-b - std::sqrt(discriminant)) / a; // If the int32_tersection is outside the cylinder on "p" endcap side float value = mDotD + t * nDotD; - if (value < float(0.0)) { + if (value < 0.0f) { // If the ray is pointing away from the "p" endcap, we return no hit - if (nDotD <= float(0.0)) return false; + if (nDotD <= 0.0f) return false; // Compute the int32_tersection against the "p" endcap (int32_tersection agains whole plane) t = -mDotD / nDotD; // Keep the int32_tersection if the it is inside the cylinder radius - if (k + t * (float(2.0) * mDotN + t) > float(0.0)) return false; + if (k + t * (float(2.0) * mDotN + t) > 0.0f) return false; // If the int32_tersection is behind the origin of the ray or beyond the maximum // raycasting distance, we return no hit - if (t < float(0.0) || t > ray.maxFraction) return false; + if (t < 0.0f || t > ray.maxFraction) return false; // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; + vec3 localHitPoint = ray.point1 + t * n; raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; raycastInfo.worldPoint = localHitPoint; - Vector3 normalDirection(0, float(-1.0), 0); + vec3 normalDirection(0, float(-1.0), 0); raycastInfo.worldNormal = normalDirection; return true; @@ -171,26 +172,26 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape else if (value > dDotD) { // If the int32_tersection is outside the cylinder on the "q" side // If the ray is pointing away from the "q" endcap, we return no hit - if (nDotD >= float(0.0)) return false; + if (nDotD >= 0.0f) return false; // Compute the int32_tersection against the "q" endcap (int32_tersection against whole plane) t = (dDotD - mDotD) / nDotD; // Keep the int32_tersection if it is inside the cylinder radius if (k + dDotD - float(2.0) * mDotD + t * (float(2.0) * (mDotN - nDotD) + t) > - float(0.0)) return false; + 0.0f) return false; // If the int32_tersection is behind the origin of the ray or beyond the maximum // raycasting distance, we return no hit - if (t < float(0.0) || t > ray.maxFraction) return false; + if (t < 0.0f || t > ray.maxFraction) return false; // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; + vec3 localHitPoint = ray.point1 + t * n; raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; raycastInfo.worldPoint = localHitPoint; - Vector3 normalDirection(0, float(1.0), 0); + vec3 normalDirection(0, 1.0f, 0); raycastInfo.worldNormal = normalDirection; return true; @@ -200,17 +201,17 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape // If the int32_tersection is behind the origin of the ray or beyond the maximum // raycasting distance, we return no hit - if (t < float(0.0) || t > ray.maxFraction) return false; + if (t < 0.0f || t > ray.maxFraction) return false; // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; + vec3 localHitPoint = ray.point1 + t * n; raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; raycastInfo.worldPoint = localHitPoint; - Vector3 v = localHitPoint - p; - Vector3 w = (v.dot(d) / d.lengthSquare()) * d; - Vector3 normalDirection = (localHitPoint - (p + w)); + vec3 v = localHitPoint - p; + vec3 w = (v.dot(d) / d.length2()) * d; + vec3 normalDirection = (localHitPoint - (p + w)); raycastInfo.worldNormal = normalDirection; return true; diff --git a/ephysics/collision/shapes/CylinderShape.h b/ephysics/collision/shapes/CylinderShape.h index b5e425c..0e970d4 100644 --- a/ephysics/collision/shapes/CylinderShape.h +++ b/ephysics/collision/shapes/CylinderShape.h @@ -38,7 +38,7 @@ class CylinderShape : public ConvexShape { float mRadius; /// Half height of the cylinder - float mHalfHeight; + float m_halfHeight; // -------------------- Methods -------------------- // @@ -49,11 +49,11 @@ class CylinderShape : public ConvexShape { CylinderShape& operator=(const CylinderShape& shape); /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, + virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, void** cachedCollisionData) const; /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; + virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; @@ -78,13 +78,13 @@ class CylinderShape : public ConvexShape { float getHeight() const; /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + virtual void setLocalScaling(const vec3& scaling); /// Return the local bounds of the shape in x, y and z directions - virtual void getLocalBounds(Vector3& min, Vector3& max) const; + virtual void getLocalBounds(vec3& min, vec3& max) const; /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const; + virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; }; // Return the radius @@ -100,14 +100,14 @@ inline float CylinderShape::getRadius() const { * @return Height of the cylinder (in meters) */ inline float CylinderShape::getHeight() const { - return mHalfHeight + mHalfHeight; + return m_halfHeight + m_halfHeight; } // Set the scaling vector of the collision shape -inline void CylinderShape::setLocalScaling(const Vector3& scaling) { +inline void CylinderShape::setLocalScaling(const vec3& scaling) { - mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y; - mRadius = (mRadius / mScaling.x) * scaling.x; + m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y(); + mRadius = (mRadius / m_scaling.x()) * scaling.x(); CollisionShape::setLocalScaling(scaling); } @@ -122,17 +122,15 @@ inline size_t CylinderShape::getSizeInBytes() const { * @param min The minimum bounds of the shape in local-space coordinates * @param max The maximum bounds of the shape in local-space coordinates */ -inline void CylinderShape::getLocalBounds(Vector3& min, Vector3& max) const { - +inline void CylinderShape::getLocalBounds(vec3& min, vec3& max) const { // Maximum bounds - max.x = mRadius + mMargin; - max.y = mHalfHeight + mMargin; - max.z = max.x; - + max.setX(mRadius + m_margin); + max.setY(m_halfHeight + m_margin); + max.setZ(max.x()); // Minimum bounds - min.x = -max.x; - min.y = -max.y; - min.z = min.x; + min.setX(-max.x()); + min.setY(-max.y()); + min.setZ(min.x()); } // Return the local inertia tensor of the cylinder @@ -141,18 +139,19 @@ inline void CylinderShape::getLocalBounds(Vector3& min, Vector3& max) const { * coordinates * @param mass Mass to use to compute the inertia tensor of the collision shape */ -inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { - float height = float(2.0) * mHalfHeight; - float diag = (float(1.0) / float(12.0)) * mass * (3 * mRadius * mRadius + height * height); - tensor.setAllValues(diag, 0.0, 0.0, 0.0, - float(0.5) * mass * mRadius * mRadius, 0.0, +inline void CylinderShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { + float height = float(2.0) * m_halfHeight; + float diag = (1.0f / float(12.0)) * mass * (3 * mRadius * mRadius + height * height); + tensor.setValue(diag, 0.0, 0.0, 0.0, + 0.5f * mass * mRadius * mRadius, 0.0, 0.0, 0.0, diag); } // Return true if a point is inside the collision shape -inline bool CylinderShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const{ - return ((localPoint.x * localPoint.x + localPoint.z * localPoint.z) < mRadius * mRadius && - localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight); +inline bool CylinderShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const{ + return ( (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z()) < mRadius * mRadius + && localPoint.y() < m_halfHeight + && localPoint.y() > -m_halfHeight); } } diff --git a/ephysics/collision/shapes/HeightFieldShape.cpp b/ephysics/collision/shapes/HeightFieldShape.cpp index a7ef27a..c5cb8db 100644 --- a/ephysics/collision/shapes/HeightFieldShape.cpp +++ b/ephysics/collision/shapes/HeightFieldShape.cpp @@ -23,35 +23,35 @@ using namespace reactphysics3d; HeightFieldShape::HeightFieldShape(int32_t nbGridColumns, int32_t nbGridRows, float minHeight, float maxHeight, const void* heightFieldData, HeightDataType dataType, int32_t upAxis, float int32_tegerHeightScale) - : ConcaveShape(HEIGHTFIELD), mNbColumns(nbGridColumns), mNbRows(nbGridRows), - mWidth(nbGridColumns - 1), mLength(nbGridRows - 1), mMinHeight(minHeight), - mMaxHeight(maxHeight), mUpAxis(upAxis), mIntegerHeightScale(int32_tegerHeightScale), - mHeightDataType(dataType) { + : ConcaveShape(HEIGHTFIELD), m_numberColumns(nbGridColumns), m_numberRows(nbGridRows), + m_width(nbGridColumns - 1), m_length(nbGridRows - 1), m_minHeight(minHeight), + m_maxHeight(maxHeight), m_upAxis(upAxis), m_integerHeightScale(int32_tegerHeightScale), + m_heightDataType(dataType) { assert(nbGridColumns >= 2); assert(nbGridRows >= 2); - assert(mWidth >= 1); - assert(mLength >= 1); + assert(m_width >= 1); + assert(m_length >= 1); assert(minHeight <= maxHeight); assert(upAxis == 0 || upAxis == 1 || upAxis == 2); - mHeightFieldData = heightFieldData; + m_heightFieldData = heightFieldData; - float halfHeight = (mMaxHeight - mMinHeight) * float(0.5); + float halfHeight = (m_maxHeight - m_minHeight) * 0.5f; assert(halfHeight >= 0); // Compute the local AABB of the height field - if (mUpAxis == 0) { - mAABB.setMin(Vector3(-halfHeight, -mWidth * float(0.5), -mLength * float(0.5))); - mAABB.setMax(Vector3(halfHeight, mWidth * float(0.5), mLength* float(0.5))); + if (m_upAxis == 0) { + m_AABB.setMin(vec3(-halfHeight, -m_width * 0.5f, -m_length * float(0.5))); + m_AABB.setMax(vec3(halfHeight, m_width * 0.5f, m_length* float(0.5))); } - else if (mUpAxis == 1) { - mAABB.setMin(Vector3(-mWidth * float(0.5), -halfHeight, -mLength * float(0.5))); - mAABB.setMax(Vector3(mWidth * float(0.5), halfHeight, mLength * float(0.5))); + else if (m_upAxis == 1) { + m_AABB.setMin(vec3(-m_width * 0.5f, -halfHeight, -m_length * float(0.5))); + m_AABB.setMax(vec3(m_width * 0.5f, halfHeight, m_length * float(0.5))); } - else if (mUpAxis == 2) { - mAABB.setMin(Vector3(-mWidth * float(0.5), -mLength * float(0.5), -halfHeight)); - mAABB.setMax(Vector3(mWidth * float(0.5), mLength * float(0.5), halfHeight)); + else if (m_upAxis == 2) { + m_AABB.setMin(vec3(-m_width * 0.5f, -m_length * float(0.5), -halfHeight)); + m_AABB.setMax(vec3(m_width * 0.5f, m_length * float(0.5), halfHeight)); } } @@ -66,9 +66,9 @@ HeightFieldShape::~HeightFieldShape() { * @param min The minimum bounds of the shape in local-space coordinates * @param max The maximum bounds of the shape in local-space coordinates */ -void HeightFieldShape::getLocalBounds(Vector3& min, Vector3& max) const { - min = mAABB.getMin() * mScaling; - max = mAABB.getMax() * mScaling; +void HeightFieldShape::getLocalBounds(vec3& min, vec3& max) const { + min = m_AABB.getMin() * m_scaling; + max = m_AABB.getMax() * m_scaling; } // Test collision with the triangles of the height field shape. The idea is to use the AABB @@ -78,7 +78,7 @@ void HeightFieldShape::getLocalBounds(Vector3& min, Vector3& max) const { void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const { // Compute the non-scaled AABB - Vector3 inverseScaling(float(1.0) / mScaling.x, float(1.0) / mScaling.y, float(1.0) / mScaling.z); + vec3 inverseScaling(1.0f / m_scaling.x(), 1.0f / m_scaling.y(), float(1.0) / m_scaling.z()); AABB aabb(localAABB.getMin() * inverseScaling, localAABB.getMax() * inverseScaling); // Compute the int32_teger grid coordinates inside the area we need to test for collision @@ -91,41 +91,41 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& int32_t iMax = 0; int32_t jMin = 0; int32_t jMax = 0; - switch(mUpAxis) { - case 0 : iMin = clamp(minGridCoords[1], 0, mNbColumns - 1); - iMax = clamp(maxGridCoords[1], 0, mNbColumns - 1); - jMin = clamp(minGridCoords[2], 0, mNbRows - 1); - jMax = clamp(maxGridCoords[2], 0, mNbRows - 1); + switch(m_upAxis) { + case 0 : iMin = clamp(minGridCoords[1], 0, m_numberColumns - 1); + iMax = clamp(maxGridCoords[1], 0, m_numberColumns - 1); + jMin = clamp(minGridCoords[2], 0, m_numberRows - 1); + jMax = clamp(maxGridCoords[2], 0, m_numberRows - 1); break; - case 1 : iMin = clamp(minGridCoords[0], 0, mNbColumns - 1); - iMax = clamp(maxGridCoords[0], 0, mNbColumns - 1); - jMin = clamp(minGridCoords[2], 0, mNbRows - 1); - jMax = clamp(maxGridCoords[2], 0, mNbRows - 1); + case 1 : iMin = clamp(minGridCoords[0], 0, m_numberColumns - 1); + iMax = clamp(maxGridCoords[0], 0, m_numberColumns - 1); + jMin = clamp(minGridCoords[2], 0, m_numberRows - 1); + jMax = clamp(maxGridCoords[2], 0, m_numberRows - 1); break; - case 2 : iMin = clamp(minGridCoords[0], 0, mNbColumns - 1); - iMax = clamp(maxGridCoords[0], 0, mNbColumns - 1); - jMin = clamp(minGridCoords[1], 0, mNbRows - 1); - jMax = clamp(maxGridCoords[1], 0, mNbRows - 1); + case 2 : iMin = clamp(minGridCoords[0], 0, m_numberColumns - 1); + iMax = clamp(maxGridCoords[0], 0, m_numberColumns - 1); + jMin = clamp(minGridCoords[1], 0, m_numberRows - 1); + jMax = clamp(maxGridCoords[1], 0, m_numberRows - 1); break; } - assert(iMin >= 0 && iMin < mNbColumns); - assert(iMax >= 0 && iMax < mNbColumns); - assert(jMin >= 0 && jMin < mNbRows); - assert(jMax >= 0 && jMax < mNbRows); + assert(iMin >= 0 && iMin < m_numberColumns); + assert(iMax >= 0 && iMax < m_numberColumns); + assert(jMin >= 0 && jMin < m_numberRows); + assert(jMax >= 0 && jMax < m_numberRows); // For each sub-grid points (except the last ones one each dimension) for (int32_t i = iMin; i < iMax; i++) { for (int32_t j = jMin; j < jMax; j++) { // Compute the four point of the current quad - Vector3 p1 = getVertexAt(i, j); - Vector3 p2 = getVertexAt(i, j + 1); - Vector3 p3 = getVertexAt(i + 1, j); - Vector3 p4 = getVertexAt(i + 1, j + 1); + vec3 p1 = getVertexAt(i, j); + vec3 p2 = getVertexAt(i, j + 1); + vec3 p3 = getVertexAt(i + 1, j); + vec3 p4 = getVertexAt(i + 1, j + 1); // Generate the first triangle for the current grid rectangle - Vector3 trianglePoints[3] = {p1, p2, p3}; + vec3 trianglePoints[3] = {p1, p2, p3}; // Test collision against the first triangle callback.testTriangle(trianglePoints); @@ -146,29 +146,29 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& void HeightFieldShape::computeMinMaxGridCoordinates(int32_t* minCoords, int32_t* maxCoords, const AABB& aabbToCollide) const { // Clamp the min/max coords of the AABB to collide inside the height field AABB - Vector3 minPoint = Vector3::max(aabbToCollide.getMin(), mAABB.getMin()); - minPoint = Vector3::min(minPoint, mAABB.getMax()); + vec3 minPoint = etk::max(aabbToCollide.getMin(), m_AABB.getMin()); + minPoint = etk::min(minPoint, m_AABB.getMax()); - Vector3 maxPoint = Vector3::min(aabbToCollide.getMax(), mAABB.getMax()); - maxPoint = Vector3::max(maxPoint, mAABB.getMin()); + vec3 maxPoint = etk::min(aabbToCollide.getMax(), m_AABB.getMax()); + maxPoint = etk::max(maxPoint, m_AABB.getMin()); // Translate the min/max points such that the we compute grid points from [0 ... mNbWidthGridPoints] - // and from [0 ... mNbLengthGridPoints] because the AABB coordinates range are [-mWdith/2 ... mWidth/2] - // and [-mLength/2 ... mLength/2] - const Vector3 translateVec = mAABB.getExtent() * float(0.5); + // and from [0 ... mNbLengthGridPoints] because the AABB coordinates range are [-mWdith/2 ... m_width/2] + // and [-m_length/2 ... m_length/2] + const vec3 translateVec = m_AABB.getExtent() * 0.5f; minPoint += translateVec; maxPoint += translateVec; // Convert the floating min/max coords of the AABB int32_to closest int32_teger // grid values (note that we use the closest grid coordinate that is out // of the AABB) - minCoords[0] = computeIntegerGridValue(minPoint.x) - 1; - minCoords[1] = computeIntegerGridValue(minPoint.y) - 1; - minCoords[2] = computeIntegerGridValue(minPoint.z) - 1; + minCoords[0] = computeIntegerGridValue(minPoint.x()) - 1; + minCoords[1] = computeIntegerGridValue(minPoint.y()) - 1; + minCoords[2] = computeIntegerGridValue(minPoint.z()) - 1; - maxCoords[0] = computeIntegerGridValue(maxPoint.x) + 1; - maxCoords[1] = computeIntegerGridValue(maxPoint.y) + 1; - maxCoords[2] = computeIntegerGridValue(maxPoint.z) + 1; + maxCoords[0] = computeIntegerGridValue(maxPoint.x()) + 1; + maxCoords[1] = computeIntegerGridValue(maxPoint.y()) + 1; + maxCoords[2] = computeIntegerGridValue(maxPoint.z()) + 1; } // Raycast method with feedback information @@ -184,8 +184,8 @@ bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this); // Compute the AABB for the ray - const Vector3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1); - const AABB rayAABB(Vector3::min(ray.point1, rayEnd), Vector3::max(ray.point1, rayEnd)); + const vec3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1); + const AABB rayAABB(etk::min(ray.point1, rayEnd), etk::max(ray.point1, rayEnd)); testAllTriangles(triangleCallback, rayAABB); @@ -193,46 +193,46 @@ bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh } // Return the vertex (local-coordinates) of the height field at a given (x,y) position -Vector3 HeightFieldShape::getVertexAt(int32_t x, int32_t y) const { +vec3 HeightFieldShape::getVertexAt(int32_t x, int32_t y) const { // Get the height value const float height = getHeightAt(x, y); // Height values origin - const float heightOrigin = -(mMaxHeight - mMinHeight) * float(0.5) - mMinHeight; + const float heightOrigin = -(m_maxHeight - m_minHeight) * 0.5f - m_minHeight; - Vector3 vertex; - switch (mUpAxis) { - case 0: vertex = Vector3(heightOrigin + height, -mWidth * float(0.5) + x, -mLength * float(0.5) + y); + vec3 vertex; + switch (m_upAxis) { + case 0: vertex = vec3(heightOrigin + height, -m_width * 0.5f + x, -m_length * float(0.5) + y); break; - case 1: vertex = Vector3(-mWidth * float(0.5) + x, heightOrigin + height, -mLength * float(0.5) + y); + case 1: vertex = vec3(-m_width * 0.5f + x, heightOrigin + height, -m_length * float(0.5) + y); break; - case 2: vertex = Vector3(-mWidth * float(0.5) + x, -mLength * float(0.5) + y, heightOrigin + height); + case 2: vertex = vec3(-m_width * 0.5f + x, -m_length * float(0.5) + y, heightOrigin + height); break; default: assert(false); } - assert(mAABB.contains(vertex)); + assert(m_AABB.contains(vertex)); - return vertex * mScaling; + return vertex * m_scaling; } // Raycast test between a ray and a triangle of the heightfield -void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) { +void TriangleOverlapCallback::testTriangle(const vec3* trianglePoints) { // Create a triangle collision shape - float margin = mHeightFieldShape.getTriangleMargin(); + float margin = m_heightFieldShape.getTriangleMargin(); TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); - triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType()); + triangleShape.setRaycastTestType(m_heightFieldShape.getRaycastTestType()); // Ray casting test against the collision shape RaycastInfo raycastInfo; bool isTriangleHit = triangleShape.raycast(m_ray, raycastInfo, m_proxyShape); // If the ray hit the collision shape - if (isTriangleHit && raycastInfo.hitFraction <= mSmallestHitFraction) { + if (isTriangleHit && raycastInfo.hitFraction <= m_smallestHitFraction) { - assert(raycastInfo.hitFraction >= float(0.0)); + assert(raycastInfo.hitFraction >= 0.0f); m_raycastInfo.body = raycastInfo.body; m_raycastInfo.proxyShape = raycastInfo.proxyShape; @@ -242,7 +242,7 @@ void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) { m_raycastInfo.meshSubpart = -1; m_raycastInfo.triangleIndex = -1; - mSmallestHitFraction = raycastInfo.hitFraction; - mIsHit = true; + m_smallestHitFraction = raycastInfo.hitFraction; + m_isHit = true; } } diff --git a/ephysics/collision/shapes/HeightFieldShape.h b/ephysics/collision/shapes/HeightFieldShape.h index d37bbb9..e8b2f90 100644 --- a/ephysics/collision/shapes/HeightFieldShape.h +++ b/ephysics/collision/shapes/HeightFieldShape.h @@ -11,45 +11,40 @@ #include namespace reactphysics3d { + class HeightFieldShape; + /** + * @brief This class is used for testing AABB and triangle overlap for raycasting + */ + class TriangleOverlapCallback : public TriangleCallback { + protected: + const Ray& m_ray; + ProxyShape* m_proxyShape; + RaycastInfo& m_raycastInfo; + bool m_isHit; + float m_smallestHitFraction; + const HeightFieldShape& m_heightFieldShape; + public: + TriangleOverlapCallback(const Ray& _ray, + ProxyShape* _proxyShape, + RaycastInfo& _raycastInfo, + const HeightFieldShape& _heightFieldShape): + m_ray(_ray), + m_proxyShape(_proxyShape), + m_raycastInfo(_raycastInfo), + m_heightFieldShape(_heightFieldShape) { + m_isHit = false; + m_smallestHitFraction = m_ray.maxFraction; + } + bool getIsHit() const { + return m_isHit; + } + /// Raycast test between a ray and a triangle of the heightfield + virtual void testTriangle(const vec3* _trianglePoints); + }; -class HeightFieldShape; -// Class TriangleOverlapCallback /** - * This class is used for testing AABB and triangle overlap for raycasting - */ -class TriangleOverlapCallback : public TriangleCallback { - - protected: - - const Ray& m_ray; - ProxyShape* m_proxyShape; - RaycastInfo& m_raycastInfo; - bool mIsHit; - float mSmallestHitFraction; - const HeightFieldShape& mHeightFieldShape; - - public: - - // Constructor - TriangleOverlapCallback(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo, - const HeightFieldShape& heightFieldShape) - : m_ray(ray), m_proxyShape(proxyShape), m_raycastInfo(raycastInfo), - mHeightFieldShape (heightFieldShape) { - mIsHit = false; - mSmallestHitFraction = m_ray.maxFraction; - } - - bool getIsHit() const {return mIsHit;} - - /// Raycast test between a ray and a triangle of the heightfield - virtual void testTriangle(const Vector3* trianglePoints); -}; - - -// Class HeightFieldShape -/** - * This class represents a static height field that can be used to represent + * @brief This class represents a static height field that can be used to represent * a terrain. The height field is made of a grid with rows and columns with a * height value at each grid point. Note that the height values are not copied int32_to the shape * but are shared instead. The height values can be of type int32_teger, float or double. @@ -70,37 +65,37 @@ class HeightFieldShape : public ConcaveShape { // -------------------- Attributes -------------------- // /// Number of columns in the grid of the height field - int32_t mNbColumns; + int32_t m_numberColumns; /// Number of rows in the grid of the height field - int32_t mNbRows; + int32_t m_numberRows; /// Height field width - float mWidth; + float m_width; /// Height field length - float mLength; + float m_length; /// Minimum height of the height field - float mMinHeight; + float m_minHeight; /// Maximum height of the height field - float mMaxHeight; + float m_maxHeight; /// Up axis direction (0 => x, 1 => y, 2 => z) - int32_t mUpAxis; + int32_t m_upAxis; /// Height values scale for height field with int32_teger height values - float mIntegerHeightScale; + float m_integerHeightScale; /// Data type of the height values - HeightDataType mHeightDataType; + HeightDataType m_heightDataType; /// Array of data with all the height values of the height field - const void* mHeightFieldData; + const void* m_heightFieldData; /// Local AABB of the height field (without scaling) - AABB mAABB; + AABB m_AABB; // -------------------- Methods -------------------- // @@ -122,10 +117,10 @@ class HeightFieldShape : public ConcaveShape { /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle /// given the start vertex index pointer of the triangle. void getTriangleVerticesWithIndexPointer(int32_t subPart, int32_t triangleIndex, - Vector3* outTriangleVertices) const; + vec3* outTriangleVertices) const; /// Return the vertex (local-coordinates) of the height field at a given (x,y) position - Vector3 getVertexAt(int32_t x, int32_t y) const; + vec3 getVertexAt(int32_t x, int32_t y) const; /// Return the height of a given (x,y) point in the height field float getHeightAt(int32_t x, int32_t y) const; @@ -156,13 +151,13 @@ class HeightFieldShape : public ConcaveShape { HeightDataType getHeightDataType() const; /// Return the local bounds of the shape in x, y and z directions. - virtual void getLocalBounds(Vector3& min, Vector3& max) const; + virtual void getLocalBounds(vec3& min, vec3& max) const; /// Set the local scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + virtual void setLocalScaling(const vec3& scaling); /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const; + virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; /// Use a callback method on all triangles of the concave shape inside a given AABB virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const; @@ -175,17 +170,17 @@ class HeightFieldShape : public ConcaveShape { // Return the number of rows in the height field inline int32_t HeightFieldShape::getNbRows() const { - return mNbRows; + return m_numberRows; } // Return the number of columns in the height field inline int32_t HeightFieldShape::getNbColumns() const { - return mNbColumns; + return m_numberColumns; } // Return the type of height value in the height field inline HeightFieldShape::HeightDataType HeightFieldShape::getHeightDataType() const { - return mHeightDataType; + return m_heightDataType; } // Return the number of bytes used by the collision shape @@ -194,24 +189,24 @@ inline size_t HeightFieldShape::getSizeInBytes() const { } // Set the local scaling vector of the collision shape -inline void HeightFieldShape::setLocalScaling(const Vector3& scaling) { +inline void HeightFieldShape::setLocalScaling(const vec3& scaling) { CollisionShape::setLocalScaling(scaling); } // Return the height of a given (x,y) point in the height field inline float HeightFieldShape::getHeightAt(int32_t x, int32_t y) const { - switch(mHeightDataType) { - case HEIGHT_FLOAT_TYPE : return ((float*)mHeightFieldData)[y * mNbColumns + x]; - case HEIGHT_DOUBLE_TYPE : return ((double*)mHeightFieldData)[y * mNbColumns + x]; - case HEIGHT_INT_TYPE : return ((int32_t*)mHeightFieldData)[y * mNbColumns + x] * mIntegerHeightScale; + switch(m_heightDataType) { + case HEIGHT_FLOAT_TYPE : return ((float*)m_heightFieldData)[y * m_numberColumns + x]; + case HEIGHT_DOUBLE_TYPE : return ((double*)m_heightFieldData)[y * m_numberColumns + x]; + case HEIGHT_INT_TYPE : return ((int32_t*)m_heightFieldData)[y * m_numberColumns + x] * m_integerHeightScale; default: assert(false); return 0; } } // Return the closest inside int32_teger grid value of a given floating grid value inline int32_t HeightFieldShape::computeIntegerGridValue(float value) const { - return (value < float(0.0)) ? value - float(0.5) : value + float(0.5); + return (value < 0.0f) ? value - 0.5f : value + float(0.5); } // Return the local inertia tensor @@ -220,13 +215,13 @@ inline int32_t HeightFieldShape::computeIntegerGridValue(float value) const { * coordinates * @param mass Mass to use to compute the inertia tensor of the collision shape */ -inline void HeightFieldShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { +inline void HeightFieldShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { // Default inertia tensor // Note that this is not very realistic for a concave triangle mesh. // However, in most cases, it will only be used static bodies and therefore, // the inertia tensor is not used. - tensor.setAllValues(mass, 0, 0, + tensor.setValue(mass, 0, 0, 0, mass, 0, 0, 0, mass); } diff --git a/ephysics/collision/shapes/SphereShape.cpp b/ephysics/collision/shapes/SphereShape.cpp index 5638fbc..b95006b 100644 --- a/ephysics/collision/shapes/SphereShape.cpp +++ b/ephysics/collision/shapes/SphereShape.cpp @@ -17,7 +17,7 @@ using namespace reactphysics3d; * @param radius Radius of the sphere (in meters) */ SphereShape::SphereShape(float radius) : ConvexShape(SPHERE, radius) { - assert(radius > float(0.0)); + assert(radius > 0.0f); } // Destructor @@ -28,31 +28,31 @@ SphereShape::~SphereShape() { // Raycast method with feedback information bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - const Vector3 m = ray.point1; - float c = m.dot(m) - mMargin * mMargin; + const vec3 m = ray.point1; + float c = m.dot(m) - m_margin * m_margin; // If the origin of the ray is inside the sphere, we return no int32_tersection - if (c < float(0.0)) return false; + if (c < 0.0f) return false; - const Vector3 rayDirection = ray.point2 - ray.point1; + const vec3 rayDirection = ray.point2 - ray.point1; float b = m.dot(rayDirection); // If the origin of the ray is outside the sphere and the ray // is pointing away from the sphere, there is no int32_tersection - if (b > float(0.0)) return false; + if (b > 0.0f) return false; - float raySquareLength = rayDirection.lengthSquare(); + float raySquareLength = rayDirection.length2(); // Compute the discriminant of the quadratic equation float discriminant = b * b - raySquareLength * c; // If the discriminant is negative or the ray length is very small, there is no int32_tersection - if (discriminant < float(0.0) || raySquareLength < MACHINE_EPSILON) return false; + if (discriminant < 0.0f || raySquareLength < MACHINE_EPSILON) return false; // Compute the solution "t" closest to the origin float t = -b - std::sqrt(discriminant); - assert(t >= float(0.0)); + assert(t >= 0.0f); // If the hit point is withing the segment ray fraction if (t < ray.maxFraction * raySquareLength) { diff --git a/ephysics/collision/shapes/SphereShape.h b/ephysics/collision/shapes/SphereShape.h index 7071612..31bf490 100644 --- a/ephysics/collision/shapes/SphereShape.h +++ b/ephysics/collision/shapes/SphereShape.h @@ -37,11 +37,11 @@ class SphereShape : public ConvexShape { SphereShape& operator=(const SphereShape& shape); /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, + virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, void** cachedCollisionData) const; /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; + virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; @@ -63,16 +63,16 @@ class SphereShape : public ConvexShape { float getRadius() const; /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + virtual void setLocalScaling(const vec3& scaling); /// Return the local bounds of the shape in x, y and z directions. - virtual void getLocalBounds(Vector3& min, Vector3& max) const; + virtual void getLocalBounds(vec3& min, vec3& max) const; /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const; + virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; /// Update the AABB of a body using its collision shape - virtual void computeAABB(AABB& aabb, const Transform& transform) const; + virtual void computeAABB(AABB& aabb, const etk::Transform3D& transform) const; }; // Get the radius of the sphere @@ -80,13 +80,13 @@ class SphereShape : public ConvexShape { * @return Radius of the sphere (in meters) */ inline float SphereShape::getRadius() const { - return mMargin; + return m_margin; } // Set the scaling vector of the collision shape -inline void SphereShape::setLocalScaling(const Vector3& scaling) { +inline void SphereShape::setLocalScaling(const vec3& scaling) { - mMargin = (mMargin / mScaling.x) * scaling.x; + m_margin = (m_margin / m_scaling.x()) * scaling.x(); CollisionShape::setLocalScaling(scaling); } @@ -97,11 +97,11 @@ inline size_t SphereShape::getSizeInBytes() const { } // Return a local support point in a given direction without the object margin -inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction, +inline vec3 SphereShape::getLocalSupportPointWithoutMargin(const vec3& direction, void** cachedCollisionData) const { // Return the center of the sphere (the radius is taken int32_to account in the object margin) - return Vector3(0.0, 0.0, 0.0); + return vec3(0.0, 0.0, 0.0); } // Return the local bounds of the shape in x, y and z directions. @@ -110,17 +110,15 @@ inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& dir * @param min The minimum bounds of the shape in local-space coordinates * @param max The maximum bounds of the shape in local-space coordinates */ -inline void SphereShape::getLocalBounds(Vector3& min, Vector3& max) const { - +inline void SphereShape::getLocalBounds(vec3& min, vec3& max) const { // Maximum bounds - max.x = mMargin; - max.y = mMargin; - max.z = mMargin; - + max.setX(m_margin); + max.setY(m_margin); + max.setZ(m_margin); // Minimum bounds - min.x = -mMargin; - min.y = min.x; - min.z = min.x; + min.setX(-m_margin); + min.setY(min.x()); + min.setZ(min.x()); } // Return the local inertia tensor of the sphere @@ -129,23 +127,23 @@ inline void SphereShape::getLocalBounds(Vector3& min, Vector3& max) const { * coordinates * @param mass Mass to use to compute the inertia tensor of the collision shape */ -inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { - float diag = float(0.4) * mass * mMargin * mMargin; - tensor.setAllValues(diag, 0.0, 0.0, - 0.0, diag, 0.0, - 0.0, 0.0, diag); +inline void SphereShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { + float diag = float(0.4) * mass * m_margin * m_margin; + tensor.setValue(diag, 0.0, 0.0, + 0.0, diag, 0.0, + 0.0, 0.0, diag); } // Update the AABB of a body using its collision shape /** * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape * computed in world-space coordinates - * @param transform Transform used to compute the AABB of the collision shape + * @param transform etk::Transform3D used to compute the AABB of the collision shape */ -inline void SphereShape::computeAABB(AABB& aabb, const Transform& transform) const { +inline void SphereShape::computeAABB(AABB& aabb, const etk::Transform3D& transform) const { // Get the local extents in x,y and z direction - Vector3 extents(mMargin, mMargin, mMargin); + vec3 extents(m_margin, m_margin, m_margin); // Update the AABB with the new minimum and maximum coordinates aabb.setMin(transform.getPosition() - extents); @@ -153,8 +151,8 @@ inline void SphereShape::computeAABB(AABB& aabb, const Transform& transform) con } // Return true if a point is inside the collision shape -inline bool SphereShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { - return (localPoint.lengthSquare() < mMargin * mMargin); +inline bool SphereShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { + return (localPoint.length2() < m_margin * m_margin); } } diff --git a/ephysics/collision/shapes/TriangleShape.cpp b/ephysics/collision/shapes/TriangleShape.cpp index c4119dd..ecac90f 100644 --- a/ephysics/collision/shapes/TriangleShape.cpp +++ b/ephysics/collision/shapes/TriangleShape.cpp @@ -20,7 +20,7 @@ using namespace reactphysics3d; * @param point3 Third point of the triangle * @param margin The collision margin (in meters) around the collision shape */ -TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, float margin) +TriangleShape::TriangleShape(const vec3& point1, const vec3& point2, const vec3& point3, float margin) : ConvexShape(TRIANGLE, margin) { mPoints[0] = point1; mPoints[1] = point2; @@ -40,28 +40,28 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape PROFILE("TriangleShape::raycast()"); - const Vector3 pq = ray.point2 - ray.point1; - const Vector3 pa = mPoints[0] - ray.point1; - const Vector3 pb = mPoints[1] - ray.point1; - const Vector3 pc = mPoints[2] - ray.point1; + const vec3 pq = ray.point2 - ray.point1; + const vec3 pa = mPoints[0] - ray.point1; + const vec3 pb = mPoints[1] - ray.point1; + const vec3 pc = mPoints[2] - ray.point1; // Test if the line PQ is inside the eges BC, CA and AB. We use the triple // product for this test. - const Vector3 m = pq.cross(pc); + const vec3 m = pq.cross(pc); float u = pb.dot(m); if (m_raycastTestType == FRONT) { - if (u < float(0.0)) return false; + if (u < 0.0f) return false; } else if (m_raycastTestType == BACK) { - if (u > float(0.0)) return false; + if (u > 0.0f) return false; } float v = -pa.dot(m); if (m_raycastTestType == FRONT) { - if (v < float(0.0)) return false; + if (v < 0.0f) return false; } else if (m_raycastTestType == BACK) { - if (v > float(0.0)) return false; + if (v > 0.0f) return false; } else if (m_raycastTestType == FRONT_AND_BACK) { if (!sameSign(u, v)) return false; @@ -69,10 +69,10 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape float w = pa.dot(pq.cross(pb)); if (m_raycastTestType == FRONT) { - if (w < float(0.0)) return false; + if (w < 0.0f) return false; } else if (m_raycastTestType == BACK) { - if (w > float(0.0)) return false; + if (w > 0.0f) return false; } else if (m_raycastTestType == FRONT_AND_BACK) { if (!sameSign(u, w)) return false; @@ -83,19 +83,19 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape // Compute the barycentric coordinates (u, v, w) to determine the // int32_tersection point R, R = u * a + v * b + w * c - float denom = float(1.0) / (u + v + w); + float denom = 1.0f / (u + v + w); u *= denom; v *= denom; w *= denom; // Compute the local hit point using the barycentric coordinates - const Vector3 localHitPoint = u * mPoints[0] + v * mPoints[1] + w * mPoints[2]; + const vec3 localHitPoint = u * mPoints[0] + v * mPoints[1] + w * mPoints[2]; const float hitFraction = (localHitPoint - ray.point1).length() / pq.length(); - if (hitFraction < float(0.0) || hitFraction > ray.maxFraction) return false; + if (hitFraction < 0.0f || hitFraction > ray.maxFraction) return false; - Vector3 localHitNormal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]); - if (localHitNormal.dot(pq) > float(0.0)) localHitNormal = -localHitNormal; + vec3 localHitNormal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]); + if (localHitNormal.dot(pq) > 0.0f) localHitNormal = -localHitNormal; raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; diff --git a/ephysics/collision/shapes/TriangleShape.h b/ephysics/collision/shapes/TriangleShape.h index 05f318c..57b151f 100644 --- a/ephysics/collision/shapes/TriangleShape.h +++ b/ephysics/collision/shapes/TriangleShape.h @@ -37,7 +37,7 @@ class TriangleShape : public ConvexShape { // -------------------- Attribute -------------------- // /// Three points of the triangle - Vector3 mPoints[3]; + vec3 mPoints[3]; /// Raycast test type for the triangle (front, back, front-back) TriangleRaycastSide m_raycastTestType; @@ -51,11 +51,11 @@ class TriangleShape : public ConvexShape { TriangleShape& operator=(const TriangleShape& shape); /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, + virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, void** cachedCollisionData) const; /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; + virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; @@ -68,23 +68,23 @@ class TriangleShape : public ConvexShape { // -------------------- Methods -------------------- // /// Constructor - TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, + TriangleShape(const vec3& point1, const vec3& point2, const vec3& point3, float margin = OBJECT_MARGIN); /// Destructor virtual ~TriangleShape(); /// Return the local bounds of the shape in x, y and z directions. - virtual void getLocalBounds(Vector3& min, Vector3& max) const; + virtual void getLocalBounds(vec3& min, vec3& max) const; /// Set the local scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + virtual void setLocalScaling(const vec3& scaling); /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const; + virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; /// Update the AABB of a body using its collision shape - virtual void computeAABB(AABB& aabb, const Transform& transform) const; + virtual void computeAABB(AABB& aabb, const etk::Transform3D& transform) const; /// Return the raycast test type (front, back, front-back) TriangleRaycastSide getRaycastTestType() const; @@ -93,7 +93,7 @@ class TriangleShape : public ConvexShape { void setRaycastTestType(TriangleRaycastSide testType); /// Return the coordinates of a given vertex of the triangle - Vector3 getVertex(int32_t index) const; + vec3 getVertex(int32_t index) const; // ---------- Friendship ---------- // @@ -107,9 +107,9 @@ inline size_t TriangleShape::getSizeInBytes() const { } // Return a local support point in a given direction without the object margin -inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction, +inline vec3 TriangleShape::getLocalSupportPointWithoutMargin(const vec3& direction, void** cachedCollisionData) const { - Vector3 dotProducts(direction.dot(mPoints[0]), direction.dot(mPoints[1]), direction.dot(mPoints[2])); + vec3 dotProducts(direction.dot(mPoints[0]), direction.dot(mPoints[1]), direction.dot(mPoints[2])); return mPoints[dotProducts.getMaxAxis()]; } @@ -119,24 +119,24 @@ inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& d * @param min The minimum bounds of the shape in local-space coordinates * @param max The maximum bounds of the shape in local-space coordinates */ -inline void TriangleShape::getLocalBounds(Vector3& min, Vector3& max) const { +inline void TriangleShape::getLocalBounds(vec3& min, vec3& max) const { - const Vector3 xAxis(mPoints[0].x, mPoints[1].x, mPoints[2].x); - const Vector3 yAxis(mPoints[0].y, mPoints[1].y, mPoints[2].y); - const Vector3 zAxis(mPoints[0].z, mPoints[1].z, mPoints[2].z); - min.setAllValues(xAxis.getMinValue(), yAxis.getMinValue(), zAxis.getMinValue()); - max.setAllValues(xAxis.getMaxValue(), yAxis.getMaxValue(), zAxis.getMaxValue()); + const vec3 xAxis(mPoints[0].x(), mPoints[1].x(), mPoints[2].x()); + const vec3 yAxis(mPoints[0].y(), mPoints[1].y(), mPoints[2].y()); + const vec3 zAxis(mPoints[0].z(), mPoints[1].z(), mPoints[2].z()); + min.setValue(xAxis.getMin(), yAxis.getMin(), zAxis.getMin()); + max.setValue(xAxis.getMax(), yAxis.getMax(), zAxis.getMax()); - min -= Vector3(mMargin, mMargin, mMargin); - max += Vector3(mMargin, mMargin, mMargin); + min -= vec3(m_margin, m_margin, m_margin); + max += vec3(m_margin, m_margin, m_margin); } // Set the local scaling vector of the collision shape -inline void TriangleShape::setLocalScaling(const Vector3& scaling) { +inline void TriangleShape::setLocalScaling(const vec3& scaling) { - mPoints[0] = (mPoints[0] / mScaling) * scaling; - mPoints[1] = (mPoints[1] / mScaling) * scaling; - mPoints[2] = (mPoints[2] / mScaling) * scaling; + mPoints[0] = (mPoints[0] / m_scaling) * scaling; + mPoints[1] = (mPoints[1] / m_scaling) * scaling; + mPoints[2] = (mPoints[2] / m_scaling) * scaling; CollisionShape::setLocalScaling(scaling); } @@ -147,31 +147,31 @@ inline void TriangleShape::setLocalScaling(const Vector3& scaling) { * coordinates * @param mass Mass to use to compute the inertia tensor of the collision shape */ -inline void TriangleShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { - tensor.setToZero(); +inline void TriangleShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { + tensor.setZero(); } // Update the AABB of a body using its collision shape /** * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape * computed in world-space coordinates - * @param transform Transform used to compute the AABB of the collision shape + * @param transform etk::Transform3D used to compute the AABB of the collision shape */ -inline void TriangleShape::computeAABB(AABB& aabb, const Transform& transform) const { +inline void TriangleShape::computeAABB(AABB& aabb, const etk::Transform3D& transform) const { - const Vector3 worldPoint1 = transform * mPoints[0]; - const Vector3 worldPoint2 = transform * mPoints[1]; - const Vector3 worldPoint3 = transform * mPoints[2]; + const vec3 worldPoint1 = transform * mPoints[0]; + const vec3 worldPoint2 = transform * mPoints[1]; + const vec3 worldPoint3 = transform * mPoints[2]; - const Vector3 xAxis(worldPoint1.x, worldPoint2.x, worldPoint3.x); - const Vector3 yAxis(worldPoint1.y, worldPoint2.y, worldPoint3.y); - const Vector3 zAxis(worldPoint1.z, worldPoint2.z, worldPoint3.z); - aabb.setMin(Vector3(xAxis.getMinValue(), yAxis.getMinValue(), zAxis.getMinValue())); - aabb.setMax(Vector3(xAxis.getMaxValue(), yAxis.getMaxValue(), zAxis.getMaxValue())); + const vec3 xAxis(worldPoint1.x(), worldPoint2.x(), worldPoint3.x()); + const vec3 yAxis(worldPoint1.y(), worldPoint2.y(), worldPoint3.y()); + const vec3 zAxis(worldPoint1.z(), worldPoint2.z(), worldPoint3.z()); + aabb.setMin(vec3(xAxis.getMin(), yAxis.getMin(), zAxis.getMin())); + aabb.setMax(vec3(xAxis.getMax(), yAxis.getMax(), zAxis.getMax())); } // Return true if a point is inside the collision shape -inline bool TriangleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { +inline bool TriangleShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { return false; } @@ -192,7 +192,7 @@ inline void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) { /** * @param index Index (0 to 2) of a vertex of the triangle */ -inline Vector3 TriangleShape::getVertex(int32_t index) const { +inline vec3 TriangleShape::getVertex(int32_t index) const { assert(index >= 0 && index < 3); return mPoints[index]; } diff --git a/ephysics/configuration.h b/ephysics/configuration.h index 73519b2..dad7a20 100644 --- a/ephysics/configuration.h +++ b/ephysics/configuration.h @@ -53,10 +53,10 @@ namespace reactphysics3d { const float DEFAULT_FRICTION_COEFFICIENT = float(0.3); /// Default bounciness factor for a rigid body - const float DEFAULT_BOUNCINESS = float(0.5); + const float DEFAULT_BOUNCINESS = 0.5f; /// Default rolling resistance - const float DEFAULT_ROLLING_RESISTANCE = float(0.0); + const float DEFAULT_ROLLING_RESISTANCE = 0.0f; /// True if the spleeping technique is enabled const bool SPLEEPING_ENABLED = true; @@ -68,7 +68,7 @@ namespace reactphysics3d { const float PERSISTENT_CONTACT_DIST_THRESHOLD = float(0.03); /// Velocity threshold for contact velocity restitution - const float RESTITUTION_VELOCITY_THRESHOLD = float(1.0); + const float RESTITUTION_VELOCITY_THRESHOLD = 1.0f; /// Number of iterations when solving the velocity constraints of the Sequential Impulse technique const uint32_t DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 10; diff --git a/ephysics/constraint/BallAndSocketJoint.cpp b/ephysics/constraint/BallAndSocketJoint.cpp index 4364b8e..f4779ed 100644 --- a/ephysics/constraint/BallAndSocketJoint.cpp +++ b/ephysics/constraint/BallAndSocketJoint.cpp @@ -15,7 +15,7 @@ const float BallAndSocketJoint::BETA = float(0.2); // Constructor BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo) - : Joint(jointInfo), m_impulse(Vector3(0, 0, 0)) { + : Joint(jointInfo), m_impulse(vec3(0, 0, 0)) { // Compute the local-space anchor point for each body m_localAnchorPointBody1 = m_body1->getTransform().getInverse() * jointInfo.m_anchorPointWorldSpace; @@ -35,10 +35,10 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS m_indexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(m_body2)->second; // Get the bodies center of mass and orientations - const Vector3& x1 = m_body1->m_centerOfMassWorld; - const Vector3& x2 = m_body2->m_centerOfMassWorld; - const Quaternion& orientationBody1 = m_body1->getTransform().getOrientation(); - const Quaternion& orientationBody2 = m_body2->getTransform().getOrientation(); + const vec3& x1 = m_body1->m_centerOfMassWorld; + const vec3& x2 = m_body2->m_centerOfMassWorld; + const etk::Quaternion& orientationBody1 = m_body1->getTransform().getOrientation(); + const etk::Quaternion& orientationBody2 = m_body2->getTransform().getOrientation(); // Get the inertia tensor of bodies m_i1 = m_body1->getInertiaTensorInverseWorld(); @@ -49,25 +49,25 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS m_r2World = orientationBody2 * m_localAnchorPointBody2; // Compute the corresponding skew-symmetric matrices - Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World); - Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World); + etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World); + etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World); // Compute the matrix K=JM^-1J^t (3x3 matrix) float inverseMassBodies = m_body1->m_massInverse + m_body2->m_massInverse; - Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, + etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose(); // Compute the inverse mass matrix K^-1 - m_inverseMassMatrix.setToZero(); + m_inverseMassMatrix.setZero(); if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { m_inverseMassMatrix = massMatrix.getInverse(); } // Compute the bias "b" of the constraint - m_biasVector.setToZero(); + m_biasVector.setZero(); if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { float biasFactor = (BETA / constraintSolverData.timeStep); m_biasVector = biasFactor * (x2 + m_r2World - x1 - m_r1World); @@ -77,7 +77,7 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS if (!constraintSolverData.isWarmStartingActive) { // Reset the accumulated impulse - m_impulse.setToZero(); + m_impulse.setZero(); } } @@ -85,21 +85,21 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[m_indexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[m_indexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[m_indexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[m_indexBody2]; + vec3& v1 = constraintSolverData.linearVelocities[m_indexBody1]; + vec3& v2 = constraintSolverData.linearVelocities[m_indexBody2]; + vec3& w1 = constraintSolverData.angularVelocities[m_indexBody1]; + vec3& w2 = constraintSolverData.angularVelocities[m_indexBody2]; // Compute the impulse P=J^T * lambda for the body 1 - const Vector3 linearImpulseBody1 = -m_impulse; - const Vector3 angularImpulseBody1 = m_impulse.cross(m_r1World); + const vec3 linearImpulseBody1 = -m_impulse; + const vec3 angularImpulseBody1 = m_impulse.cross(m_r1World); // Apply the impulse to the body 1 v1 += m_body1->m_massInverse * linearImpulseBody1; w1 += m_i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the body 2 - const Vector3 angularImpulseBody2 = -m_impulse.cross(m_r2World); + const vec3 angularImpulseBody2 = -m_impulse.cross(m_r2World); // Apply the impulse to the body to the body 2 v2 += m_body2->m_massInverse * m_impulse; @@ -110,28 +110,28 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[m_indexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[m_indexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[m_indexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[m_indexBody2]; + vec3& v1 = constraintSolverData.linearVelocities[m_indexBody1]; + vec3& v2 = constraintSolverData.linearVelocities[m_indexBody2]; + vec3& w1 = constraintSolverData.angularVelocities[m_indexBody1]; + vec3& w2 = constraintSolverData.angularVelocities[m_indexBody2]; // Compute J*v - const Vector3 Jv = v2 + w2.cross(m_r2World) - v1 - w1.cross(m_r1World); + const vec3 Jv = v2 + w2.cross(m_r2World) - v1 - w1.cross(m_r1World); // Compute the Lagrange multiplier lambda - const Vector3 deltaLambda = m_inverseMassMatrix * (-Jv - m_biasVector); + const vec3 deltaLambda = m_inverseMassMatrix * (-Jv - m_biasVector); m_impulse += deltaLambda; // Compute the impulse P=J^T * lambda for the body 1 - const Vector3 linearImpulseBody1 = -deltaLambda; - const Vector3 angularImpulseBody1 = deltaLambda.cross(m_r1World); + const vec3 linearImpulseBody1 = -deltaLambda; + const vec3 angularImpulseBody1 = deltaLambda.cross(m_r1World); // Apply the impulse to the body 1 v1 += m_body1->m_massInverse * linearImpulseBody1; w1 += m_i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the body 2 - const Vector3 angularImpulseBody2 = -deltaLambda.cross(m_r2World); + const vec3 angularImpulseBody2 = -deltaLambda.cross(m_r2World); // Apply the impulse to the body 2 v2 += m_body2->m_massInverse * deltaLambda; @@ -146,10 +146,10 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con if (m_positionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies center of mass and orientations - Vector3& x1 = constraintSolverData.positions[m_indexBody1]; - Vector3& x2 = constraintSolverData.positions[m_indexBody2]; - Quaternion& q1 = constraintSolverData.orientations[m_indexBody1]; - Quaternion& q2 = constraintSolverData.orientations[m_indexBody2]; + vec3& x1 = constraintSolverData.positions[m_indexBody1]; + vec3& x2 = constraintSolverData.positions[m_indexBody2]; + etk::Quaternion& q1 = constraintSolverData.orientations[m_indexBody1]; + etk::Quaternion& q2 = constraintSolverData.orientations[m_indexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies float inverseMassBody1 = m_body1->m_massInverse; @@ -164,52 +164,52 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con m_r2World = q2 * m_localAnchorPointBody2; // Compute the corresponding skew-symmetric matrices - Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World); - Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World); + etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World); + etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World); // Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints float inverseMassBodies = inverseMassBody1 + inverseMassBody2; - Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, + etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose(); - m_inverseMassMatrix.setToZero(); + m_inverseMassMatrix.setZero(); if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { m_inverseMassMatrix = massMatrix.getInverse(); } // Compute the constraint error (value of the C(x) function) - const Vector3 constraintError = (x2 + m_r2World - x1 - m_r1World); + const vec3 constraintError = (x2 + m_r2World - x1 - m_r1World); // Compute the Lagrange multiplier lambda // TODO : Do not solve the system by computing the inverse each time and multiplying with the // right-hand side vector but instead use a method to directly solve the linear system. - const Vector3 lambda = m_inverseMassMatrix * (-constraintError); + const vec3 lambda = m_inverseMassMatrix * (-constraintError); // Compute the impulse of body 1 - const Vector3 linearImpulseBody1 = -lambda; - const Vector3 angularImpulseBody1 = lambda.cross(m_r1World); + const vec3 linearImpulseBody1 = -lambda; + const vec3 angularImpulseBody1 = lambda.cross(m_r1World); // Compute the pseudo velocity of body 1 - const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - const Vector3 w1 = m_i1 * angularImpulseBody1; + const vec3 v1 = inverseMassBody1 * linearImpulseBody1; + const vec3 w1 = m_i1 * angularImpulseBody1; // Update the body center of mass and orientation of body 1 x1 += v1; - q1 += Quaternion(0, w1) * q1 * float(0.5); + q1 += etk::Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse of body 2 - const Vector3 angularImpulseBody2 = -lambda.cross(m_r2World); + const vec3 angularImpulseBody2 = -lambda.cross(m_r2World); // Compute the pseudo velocity of body 2 - const Vector3 v2 = inverseMassBody2 * lambda; - const Vector3 w2 = m_i2 * angularImpulseBody2; + const vec3 v2 = inverseMassBody2 * lambda; + const vec3 w2 = m_i2 * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; - q2 += Quaternion(0, w2) * q2 * float(0.5); + q2 += etk::Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); } diff --git a/ephysics/constraint/BallAndSocketJoint.h b/ephysics/constraint/BallAndSocketJoint.h index 32eba3b..2e4b48a 100644 --- a/ephysics/constraint/BallAndSocketJoint.h +++ b/ephysics/constraint/BallAndSocketJoint.h @@ -23,7 +23,7 @@ struct BallAndSocketJointInfo : public JointInfo { // -------------------- Attributes -------------------- // /// Anchor point (in world-space coordinates) - Vector3 m_anchorPointWorldSpace; + vec3 m_anchorPointWorldSpace; /// Constructor /** @@ -33,7 +33,7 @@ struct BallAndSocketJointInfo : public JointInfo { * coordinates */ BallAndSocketJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const Vector3& initAnchorPointWorldSpace) + const vec3& initAnchorPointWorldSpace) : JointInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT), m_anchorPointWorldSpace(initAnchorPointWorldSpace) {} }; @@ -56,31 +56,31 @@ class BallAndSocketJoint : public Joint { // -------------------- Attributes -------------------- // /// Anchor point of body 1 (in local-space coordinates of body 1) - Vector3 m_localAnchorPointBody1; + vec3 m_localAnchorPointBody1; /// Anchor point of body 2 (in local-space coordinates of body 2) - Vector3 m_localAnchorPointBody2; + vec3 m_localAnchorPointBody2; /// Vector from center of body 2 to anchor point in world-space - Vector3 m_r1World; + vec3 m_r1World; /// Vector from center of body 2 to anchor point in world-space - Vector3 m_r2World; + vec3 m_r2World; /// Inertia tensor of body 1 (in world-space coordinates) - Matrix3x3 m_i1; + etk::Matrix3x3 m_i1; /// Inertia tensor of body 2 (in world-space coordinates) - Matrix3x3 m_i2; + etk::Matrix3x3 m_i2; /// Bias vector for the constraint - Vector3 m_biasVector; + vec3 m_biasVector; /// Inverse mass matrix K=JM^-1J^-t of the constraint - Matrix3x3 m_inverseMassMatrix; + etk::Matrix3x3 m_inverseMassMatrix; /// Accumulated impulse - Vector3 m_impulse; + vec3 m_impulse; // -------------------- Methods -------------------- // diff --git a/ephysics/constraint/ContactPoint.cpp b/ephysics/constraint/ContactPoint.cpp index 66e16b9..2a0da6a 100644 --- a/ephysics/constraint/ContactPoint.cpp +++ b/ephysics/constraint/ContactPoint.cpp @@ -26,8 +26,8 @@ ContactPoint::ContactPoint(const ContactPointInfo& contactInfo) contactInfo.localPoint2), mIsRestingContact(false) { - m_frictionVectors[0] = Vector3(0, 0, 0); - m_frictionVectors[1] = Vector3(0, 0, 0); + m_frictionVectors[0] = vec3(0, 0, 0); + m_frictionVectors[1] = vec3(0, 0, 0); assert(mPenetrationDepth > 0.0); diff --git a/ephysics/constraint/ContactPoint.h b/ephysics/constraint/ContactPoint.h index d1a0582..2e0bdf4 100644 --- a/ephysics/constraint/ContactPoint.h +++ b/ephysics/constraint/ContactPoint.h @@ -45,23 +45,23 @@ struct ContactPointInfo { const CollisionShape* collisionShape2; /// Normalized normal vector of the collision contact in world space - Vector3 normal; + vec3 normal; /// Penetration depth of the contact float penetrationDepth; /// Contact point of body 1 in local space of body 1 - Vector3 localPoint1; + vec3 localPoint1; /// Contact point of body 2 in local space of body 2 - Vector3 localPoint2; + vec3 localPoint2; // -------------------- Methods -------------------- // /// Constructor ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, const CollisionShape* collShape1, - const CollisionShape* collShape2, const Vector3& normal, float penetrationDepth, - const Vector3& localPoint1, const Vector3& localPoint2) + const CollisionShape* collShape2, const vec3& normal, float penetrationDepth, + const vec3& localPoint1, const vec3& localPoint2) : shape1(proxyShape1), shape2(proxyShape2), collisionShape1(collShape1), collisionShape2(collShape2), normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1), localPoint2(localPoint2) { @@ -87,28 +87,28 @@ class ContactPoint { CollisionBody* m_body2; /// Normalized normal vector of the contact (from body1 toward body2) in world space - const Vector3 mNormal; + const vec3 mNormal; /// Penetration depth float mPenetrationDepth; /// Contact point on body 1 in local space of body 1 - const Vector3 mLocalPointOnBody1; + const vec3 mLocalPointOnBody1; /// Contact point on body 2 in local space of body 2 - const Vector3 mLocalPointOnBody2; + const vec3 mLocalPointOnBody2; /// Contact point on body 1 in world space - Vector3 m_worldPointOnBody1; + vec3 m_worldPointOnBody1; /// Contact point on body 2 in world space - Vector3 m_worldPointOnBody2; + vec3 m_worldPointOnBody2; /// True if the contact is a resting contact (exists for more than one time step) bool mIsRestingContact; /// Two orthogonal vectors that span the tangential friction plane - Vector3 m_frictionVectors[2]; + vec3 m_frictionVectors[2]; /// Cached penetration impulse float mPenetrationImpulse; @@ -120,7 +120,7 @@ class ContactPoint { float m_frictionImpulse2; /// Cached rolling resistance impulse - Vector3 m_rollingResistanceImpulse; + vec3 m_rollingResistanceImpulse; // -------------------- Methods -------------------- // @@ -147,22 +147,22 @@ class ContactPoint { CollisionBody* getBody2() const; /// Return the normal vector of the contact - Vector3 getNormal() const; + vec3 getNormal() const; /// Set the penetration depth of the contact void setPenetrationDepth(float penetrationDepth); /// Return the contact local point on body 1 - Vector3 getLocalPointOnBody1() const; + vec3 getLocalPointOnBody1() const; /// Return the contact local point on body 2 - Vector3 getLocalPointOnBody2() const; + vec3 getLocalPointOnBody2() const; /// Return the contact world point on body 1 - Vector3 getWorldPointOnBody1() const; + vec3 getWorldPointOnBody1() const; /// Return the contact world point on body 2 - Vector3 getWorldPointOnBody2() const; + vec3 getWorldPointOnBody2() const; /// Return the cached penetration impulse float getPenetrationImpulse() const; @@ -174,7 +174,7 @@ class ContactPoint { float getFrictionImpulse2() const; /// Return the cached rolling resistance impulse - Vector3 getRollingResistanceImpulse() const; + vec3 getRollingResistanceImpulse() const; /// Set the cached penetration impulse void setPenetrationImpulse(float impulse); @@ -186,13 +186,13 @@ class ContactPoint { void setFrictionImpulse2(float impulse); /// Set the cached rolling resistance impulse - void setRollingResistanceImpulse(const Vector3& impulse); + void setRollingResistanceImpulse(const vec3& impulse); /// Set the contact world point on body 1 - void setWorldPointOnBody1(const Vector3& worldPoint); + void setWorldPointOnBody1(const vec3& worldPoint); /// Set the contact world point on body 2 - void setWorldPointOnBody2(const Vector3& worldPoint); + void setWorldPointOnBody2(const vec3& worldPoint); /// Return true if the contact is a resting contact bool getIsRestingContact() const; @@ -201,16 +201,16 @@ class ContactPoint { void setIsRestingContact(bool isRestingContact); /// Get the first friction vector - Vector3 getFrictionVector1() const; + vec3 getFrictionVector1() const; /// Set the first friction vector - void setFrictionVector1(const Vector3& frictionVector1); + void setFrictionVector1(const vec3& frictionVector1); /// Get the second friction vector - Vector3 getFrictionVector2() const; + vec3 getFrictionvec2() const; /// Set the second friction vector - void setFrictionVector2(const Vector3& frictionVector2); + void setFrictionvec2(const vec3& frictionvec2); /// Return the penetration depth float getPenetrationDepth() const; @@ -230,7 +230,7 @@ inline CollisionBody* ContactPoint::getBody2() const { } // Return the normal vector of the contact -inline Vector3 ContactPoint::getNormal() const { +inline vec3 ContactPoint::getNormal() const { return mNormal; } @@ -240,22 +240,22 @@ inline void ContactPoint::setPenetrationDepth(float penetrationDepth) { } // Return the contact point on body 1 -inline Vector3 ContactPoint::getLocalPointOnBody1() const { +inline vec3 ContactPoint::getLocalPointOnBody1() const { return mLocalPointOnBody1; } // Return the contact point on body 2 -inline Vector3 ContactPoint::getLocalPointOnBody2() const { +inline vec3 ContactPoint::getLocalPointOnBody2() const { return mLocalPointOnBody2; } // Return the contact world point on body 1 -inline Vector3 ContactPoint::getWorldPointOnBody1() const { +inline vec3 ContactPoint::getWorldPointOnBody1() const { return m_worldPointOnBody1; } // Return the contact world point on body 2 -inline Vector3 ContactPoint::getWorldPointOnBody2() const { +inline vec3 ContactPoint::getWorldPointOnBody2() const { return m_worldPointOnBody2; } @@ -275,7 +275,7 @@ inline float ContactPoint::getFrictionImpulse2() const { } // Return the cached rolling resistance impulse -inline Vector3 ContactPoint::getRollingResistanceImpulse() const { +inline vec3 ContactPoint::getRollingResistanceImpulse() const { return m_rollingResistanceImpulse; } @@ -295,17 +295,17 @@ inline void ContactPoint::setFrictionImpulse2(float impulse) { } // Set the cached rolling resistance impulse -inline void ContactPoint::setRollingResistanceImpulse(const Vector3& impulse) { +inline void ContactPoint::setRollingResistanceImpulse(const vec3& impulse) { m_rollingResistanceImpulse = impulse; } // Set the contact world point on body 1 -inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) { +inline void ContactPoint::setWorldPointOnBody1(const vec3& worldPoint) { m_worldPointOnBody1 = worldPoint; } // Set the contact world point on body 2 -inline void ContactPoint::setWorldPointOnBody2(const Vector3& worldPoint) { +inline void ContactPoint::setWorldPointOnBody2(const vec3& worldPoint) { m_worldPointOnBody2 = worldPoint; } @@ -320,23 +320,23 @@ inline void ContactPoint::setIsRestingContact(bool isRestingContact) { } // Get the first friction vector -inline Vector3 ContactPoint::getFrictionVector1() const { +inline vec3 ContactPoint::getFrictionVector1() const { return m_frictionVectors[0]; } // Set the first friction vector -inline void ContactPoint::setFrictionVector1(const Vector3& frictionVector1) { +inline void ContactPoint::setFrictionVector1(const vec3& frictionVector1) { m_frictionVectors[0] = frictionVector1; } // Get the second friction vector -inline Vector3 ContactPoint::getFrictionVector2() const { +inline vec3 ContactPoint::getFrictionvec2() const { return m_frictionVectors[1]; } // Set the second friction vector -inline void ContactPoint::setFrictionVector2(const Vector3& frictionVector2) { - m_frictionVectors[1] = frictionVector2; +inline void ContactPoint::setFrictionvec2(const vec3& frictionvec2) { + m_frictionVectors[1] = frictionvec2; } // Return the penetration depth of the contact diff --git a/ephysics/constraint/FixedJoint.cpp b/ephysics/constraint/FixedJoint.cpp index d3be150..d78f9eb 100644 --- a/ephysics/constraint/FixedJoint.cpp +++ b/ephysics/constraint/FixedJoint.cpp @@ -18,8 +18,8 @@ FixedJoint::FixedJoint(const FixedJointInfo& jointInfo) : Joint(jointInfo), m_impulseTranslation(0, 0, 0), m_impulseRotation(0, 0, 0) { // Compute the local-space anchor point for each body - const Transform& transform1 = m_body1->getTransform(); - const Transform& transform2 = m_body2->getTransform(); + const etk::Transform3D& transform1 = m_body1->getTransform(); + const etk::Transform3D& transform2 = m_body2->getTransform(); m_localAnchorPointBody1 = transform1.getInverse() * jointInfo.m_anchorPointWorldSpace; m_localAnchorPointBody2 = transform2.getInverse() * jointInfo.m_anchorPointWorldSpace; @@ -43,10 +43,10 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat m_indexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(m_body2)->second; // Get the bodies positions and orientations - const Vector3& x1 = m_body1->m_centerOfMassWorld; - const Vector3& x2 = m_body2->m_centerOfMassWorld; - const Quaternion& orientationBody1 = m_body1->getTransform().getOrientation(); - const Quaternion& orientationBody2 = m_body2->getTransform().getOrientation(); + const vec3& x1 = m_body1->m_centerOfMassWorld; + const vec3& x2 = m_body2->m_centerOfMassWorld; + const etk::Quaternion& orientationBody1 = m_body1->getTransform().getOrientation(); + const etk::Quaternion& orientationBody2 = m_body2->getTransform().getOrientation(); // Get the inertia tensor of bodies m_i1 = m_body1->getInertiaTensorInverseWorld(); @@ -57,26 +57,26 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat m_r2World = orientationBody2 * m_localAnchorPointBody2; // Compute the corresponding skew-symmetric matrices - Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World); - Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World); + etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World); + etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World); // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints float inverseMassBodies = m_body1->m_massInverse + m_body2->m_massInverse; - Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, - 0, inverseMassBodies, 0, - 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose(); + etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0, + 0, inverseMassBodies, 0, + 0, 0, inverseMassBodies) + + skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() + + skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose(); // Compute the inverse mass matrix K^-1 for the 3 translation constraints - m_inverseMassMatrixTranslation.setToZero(); + m_inverseMassMatrixTranslation.setZero(); if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { m_inverseMassMatrixTranslation = massMatrix.getInverse(); } // Compute the bias "b" of the constraint for the 3 translation constraints float biasFactor = (BETA / constraintSolverData.timeStep); - m_biasTranslation.setToZero(); + m_biasTranslation.setZero(); if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { m_biasTranslation = biasFactor * (x2 + m_r2World - x1 - m_r1World); } @@ -89,11 +89,11 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat } // Compute the bias "b" for the 3 rotation constraints - m_biasRotation.setToZero(); + m_biasRotation.setZero(); if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { - Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); + etk::Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); currentOrientationDifference.normalize(); - const Quaternion qError = currentOrientationDifference * m_initOrientationDifferenceInv; + const etk::Quaternion qError = currentOrientationDifference * m_initOrientationDifferenceInv; m_biasRotation = biasFactor * float(2.0) * qError.getVectorV(); } @@ -101,8 +101,8 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat if (!constraintSolverData.isWarmStartingActive) { // Reset the accumulated impulses - m_impulseTranslation.setToZero(); - m_impulseRotation.setToZero(); + m_impulseTranslation.setZero(); + m_impulseRotation.setZero(); } } @@ -110,18 +110,18 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[m_indexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[m_indexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[m_indexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[m_indexBody2]; + vec3& v1 = constraintSolverData.linearVelocities[m_indexBody1]; + vec3& v2 = constraintSolverData.linearVelocities[m_indexBody2]; + vec3& w1 = constraintSolverData.angularVelocities[m_indexBody1]; + vec3& w2 = constraintSolverData.angularVelocities[m_indexBody2]; // Get the inverse mass of the bodies const float inverseMassBody1 = m_body1->m_massInverse; const float inverseMassBody2 = m_body2->m_massInverse; // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1 - Vector3 linearImpulseBody1 = -m_impulseTranslation; - Vector3 angularImpulseBody1 = m_impulseTranslation.cross(m_r1World); + vec3 linearImpulseBody1 = -m_impulseTranslation; + vec3 angularImpulseBody1 = m_impulseTranslation.cross(m_r1World); // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1 angularImpulseBody1 += -m_impulseRotation; @@ -131,7 +131,7 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) { w1 += m_i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 2 - Vector3 angularImpulseBody2 = -m_impulseTranslation.cross(m_r2World); + vec3 angularImpulseBody2 = -m_impulseTranslation.cross(m_r2World); // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 2 angularImpulseBody2 += m_impulseRotation; @@ -145,10 +145,10 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) { void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[m_indexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[m_indexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[m_indexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[m_indexBody2]; + vec3& v1 = constraintSolverData.linearVelocities[m_indexBody1]; + vec3& v2 = constraintSolverData.linearVelocities[m_indexBody2]; + vec3& w1 = constraintSolverData.angularVelocities[m_indexBody1]; + vec3& w2 = constraintSolverData.angularVelocities[m_indexBody2]; // Get the inverse mass of the bodies float inverseMassBody1 = m_body1->m_massInverse; @@ -157,23 +157,23 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS // --------------- Translation Constraints --------------- // // Compute J*v for the 3 translation constraints - const Vector3 JvTranslation = v2 + w2.cross(m_r2World) - v1 - w1.cross(m_r1World); + const vec3 JvTranslation = v2 + w2.cross(m_r2World) - v1 - w1.cross(m_r1World); // Compute the Lagrange multiplier lambda - const Vector3 deltaLambda = m_inverseMassMatrixTranslation * + const vec3 deltaLambda = m_inverseMassMatrixTranslation * (-JvTranslation - m_biasTranslation); m_impulseTranslation += deltaLambda; // Compute the impulse P=J^T * lambda for body 1 - const Vector3 linearImpulseBody1 = -deltaLambda; - Vector3 angularImpulseBody1 = deltaLambda.cross(m_r1World); + const vec3 linearImpulseBody1 = -deltaLambda; + vec3 angularImpulseBody1 = deltaLambda.cross(m_r1World); // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; w1 += m_i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for body 2 - const Vector3 angularImpulseBody2 = -deltaLambda.cross(m_r2World); + const vec3 angularImpulseBody2 = -deltaLambda.cross(m_r2World); // Apply the impulse to the body 2 v2 += inverseMassBody2 * deltaLambda; @@ -182,10 +182,10 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS // --------------- Rotation Constraints --------------- // // Compute J*v for the 3 rotation constraints - const Vector3 JvRotation = w2 - w1; + const vec3 JvRotation = w2 - w1; // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector3 deltaLambda2 = m_inverseMassMatrixRotation * (-JvRotation - m_biasRotation); + vec3 deltaLambda2 = m_inverseMassMatrixRotation * (-JvRotation - m_biasRotation); m_impulseRotation += deltaLambda2; // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1 @@ -206,10 +206,10 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS if (m_positionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies positions and orientations - Vector3& x1 = constraintSolverData.positions[m_indexBody1]; - Vector3& x2 = constraintSolverData.positions[m_indexBody2]; - Quaternion& q1 = constraintSolverData.orientations[m_indexBody1]; - Quaternion& q2 = constraintSolverData.orientations[m_indexBody2]; + vec3& x1 = constraintSolverData.positions[m_indexBody1]; + vec3& x2 = constraintSolverData.positions[m_indexBody2]; + etk::Quaternion& q1 = constraintSolverData.orientations[m_indexBody1]; + etk::Quaternion& q2 = constraintSolverData.orientations[m_indexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies float inverseMassBody1 = m_body1->m_massInverse; @@ -224,52 +224,52 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS m_r2World = q2 * m_localAnchorPointBody2; // Compute the corresponding skew-symmetric matrices - Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World); - Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World); + etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World); + etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World); // --------------- Translation Constraints --------------- // // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints float inverseMassBodies = m_body1->m_massInverse + m_body2->m_massInverse; - Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, - 0, inverseMassBodies, 0, - 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose(); - m_inverseMassMatrixTranslation.setToZero(); + etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0, + 0, inverseMassBodies, 0, + 0, 0, inverseMassBodies) + + skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() + + skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose(); + m_inverseMassMatrixTranslation.setZero(); if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { m_inverseMassMatrixTranslation = massMatrix.getInverse(); } // Compute position error for the 3 translation constraints - const Vector3 errorTranslation = x2 + m_r2World - x1 - m_r1World; + const vec3 errorTranslation = x2 + m_r2World - x1 - m_r1World; // Compute the Lagrange multiplier lambda - const Vector3 lambdaTranslation = m_inverseMassMatrixTranslation * (-errorTranslation); + const vec3 lambdaTranslation = m_inverseMassMatrixTranslation * (-errorTranslation); // Compute the impulse of body 1 - Vector3 linearImpulseBody1 = -lambdaTranslation; - Vector3 angularImpulseBody1 = lambdaTranslation.cross(m_r1World); + vec3 linearImpulseBody1 = -lambdaTranslation; + vec3 angularImpulseBody1 = lambdaTranslation.cross(m_r1World); // Compute the pseudo velocity of body 1 - const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - Vector3 w1 = m_i1 * angularImpulseBody1; + const vec3 v1 = inverseMassBody1 * linearImpulseBody1; + vec3 w1 = m_i1 * angularImpulseBody1; // Update the body position/orientation of body 1 x1 += v1; - q1 += Quaternion(0, w1) * q1 * float(0.5); + q1 += etk::Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse of body 2 - Vector3 angularImpulseBody2 = -lambdaTranslation.cross(m_r2World); + vec3 angularImpulseBody2 = -lambdaTranslation.cross(m_r2World); // Compute the pseudo velocity of body 2 - const Vector3 v2 = inverseMassBody2 * lambdaTranslation; - Vector3 w2 = m_i2 * angularImpulseBody2; + const vec3 v2 = inverseMassBody2 * lambdaTranslation; + vec3 w2 = m_i2 * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; - q2 += Quaternion(0, w2) * q2 * float(0.5); + q2 += etk::Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); // --------------- Rotation Constraints --------------- // @@ -282,13 +282,13 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS } // Compute the position error for the 3 rotation constraints - Quaternion currentOrientationDifference = q2 * q1.getInverse(); + etk::Quaternion currentOrientationDifference = q2 * q1.getInverse(); currentOrientationDifference.normalize(); - const Quaternion qError = currentOrientationDifference * m_initOrientationDifferenceInv; - const Vector3 errorRotation = float(2.0) * qError.getVectorV(); + const etk::Quaternion qError = currentOrientationDifference * m_initOrientationDifferenceInv; + const vec3 errorRotation = float(2.0) * qError.getVectorV(); // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector3 lambdaRotation = m_inverseMassMatrixRotation * (-errorRotation); + vec3 lambdaRotation = m_inverseMassMatrixRotation * (-errorRotation); // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 angularImpulseBody1 = -lambdaRotation; @@ -297,14 +297,14 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS w1 = m_i1 * angularImpulseBody1; // Update the body position/orientation of body 1 - q1 += Quaternion(0, w1) * q1 * float(0.5); + q1 += etk::Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the pseudo velocity of body 2 w2 = m_i2 * lambdaRotation; // Update the body position/orientation of body 2 - q2 += Quaternion(0, w2) * q2 * float(0.5); + q2 += etk::Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); } diff --git a/ephysics/constraint/FixedJoint.h b/ephysics/constraint/FixedJoint.h index 505be7b..e618c47 100644 --- a/ephysics/constraint/FixedJoint.h +++ b/ephysics/constraint/FixedJoint.h @@ -23,7 +23,7 @@ struct FixedJointInfo : public JointInfo { // -------------------- Attributes -------------------- // /// Anchor point (in world-space coordinates) - Vector3 m_anchorPointWorldSpace; + vec3 m_anchorPointWorldSpace; /// Constructor /** @@ -33,7 +33,7 @@ struct FixedJointInfo : public JointInfo { * world-space coordinates */ FixedJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const Vector3& initAnchorPointWorldSpace) + const vec3& initAnchorPointWorldSpace) : JointInfo(rigidBody1, rigidBody2, FIXEDJOINT), m_anchorPointWorldSpace(initAnchorPointWorldSpace){} }; @@ -55,43 +55,43 @@ class FixedJoint : public Joint { // -------------------- Attributes -------------------- // /// Anchor point of body 1 (in local-space coordinates of body 1) - Vector3 m_localAnchorPointBody1; + vec3 m_localAnchorPointBody1; /// Anchor point of body 2 (in local-space coordinates of body 2) - Vector3 m_localAnchorPointBody2; + vec3 m_localAnchorPointBody2; /// Vector from center of body 2 to anchor point in world-space - Vector3 m_r1World; + vec3 m_r1World; /// Vector from center of body 2 to anchor point in world-space - Vector3 m_r2World; + vec3 m_r2World; /// Inertia tensor of body 1 (in world-space coordinates) - Matrix3x3 m_i1; + etk::Matrix3x3 m_i1; /// Inertia tensor of body 2 (in world-space coordinates) - Matrix3x3 m_i2; + etk::Matrix3x3 m_i2; /// Accumulated impulse for the 3 translation constraints - Vector3 m_impulseTranslation; + vec3 m_impulseTranslation; /// Accumulate impulse for the 3 rotation constraints - Vector3 m_impulseRotation; + vec3 m_impulseRotation; /// Inverse mass matrix K=JM^-1J^-t of the 3 translation constraints (3x3 matrix) - Matrix3x3 m_inverseMassMatrixTranslation; + etk::Matrix3x3 m_inverseMassMatrixTranslation; /// Inverse mass matrix K=JM^-1J^-t of the 3 rotation constraints (3x3 matrix) - Matrix3x3 m_inverseMassMatrixRotation; + etk::Matrix3x3 m_inverseMassMatrixRotation; /// Bias vector for the 3 translation constraints - Vector3 m_biasTranslation; + vec3 m_biasTranslation; /// Bias vector for the 3 rotation constraints - Vector3 m_biasRotation; + vec3 m_biasRotation; /// Inverse of the initial orientation difference between the two bodies - Quaternion m_initOrientationDifferenceInv; + etk::Quaternion m_initOrientationDifferenceInv; // -------------------- Methods -------------------- // diff --git a/ephysics/constraint/HingeJoint.cpp b/ephysics/constraint/HingeJoint.cpp index a31ffbf..72830ce 100644 --- a/ephysics/constraint/HingeJoint.cpp +++ b/ephysics/constraint/HingeJoint.cpp @@ -27,8 +27,8 @@ HingeJoint::HingeJoint(const HingeJointInfo& jointInfo) assert(mUpperLimit >= 0 && mUpperLimit <= 2.0 * PI); // Compute the local-space anchor point for each body - Transform transform1 = m_body1->getTransform(); - Transform transform2 = m_body2->getTransform(); + etk::Transform3D transform1 = m_body1->getTransform(); + etk::Transform3D transform2 = m_body2->getTransform(); m_localAnchorPointBody1 = transform1.getInverse() * jointInfo.m_anchorPointWorldSpace; m_localAnchorPointBody2 = transform2.getInverse() * jointInfo.m_anchorPointWorldSpace; @@ -58,10 +58,10 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat m_indexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(m_body2)->second; // Get the bodies positions and orientations - const Vector3& x1 = m_body1->m_centerOfMassWorld; - const Vector3& x2 = m_body2->m_centerOfMassWorld; - const Quaternion& orientationBody1 = m_body1->getTransform().getOrientation(); - const Quaternion& orientationBody2 = m_body2->getTransform().getOrientation(); + const vec3& x1 = m_body1->m_centerOfMassWorld; + const vec3& x2 = m_body2->m_centerOfMassWorld; + const etk::Quaternion& orientationBody1 = m_body1->getTransform().getOrientation(); + const etk::Quaternion& orientationBody2 = m_body2->getTransform().getOrientation(); // Get the inertia tensor of bodies m_i1 = m_body1->getInertiaTensorInverseWorld(); @@ -90,42 +90,42 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Compute vectors needed in the Jacobian mA1 = orientationBody1 * mHingeLocalAxisBody1; - Vector3 a2 = orientationBody2 * mHingeLocalAxisBody2; + vec3 a2 = orientationBody2 * mHingeLocalAxisBody2; mA1.normalize(); a2.normalize(); - const Vector3 b2 = a2.getOneUnitOrthogonalVector(); - const Vector3 c2 = a2.cross(b2); + const vec3 b2 = a2.getOneUnitOrthogonalVector(); + const vec3 c2 = a2.cross(b2); mB2CrossA1 = b2.cross(mA1); mC2CrossA1 = c2.cross(mA1); // Compute the corresponding skew-symmetric matrices - Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World); - Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World); + etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World); + etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World); // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix) float inverseMassBodies = m_body1->m_massInverse + m_body2->m_massInverse; - Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, + etk::Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose(); - m_inverseMassMatrixTranslation.setToZero(); + m_inverseMassMatrixTranslation.setZero(); if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { m_inverseMassMatrixTranslation = massMatrix.getInverse(); } // Compute the bias "b" of the translation constraints - mBTranslation.setToZero(); + mBTranslation.setZero(); float biasFactor = (BETA / constraintSolverData.timeStep); if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { mBTranslation = biasFactor * (x2 + m_r2World - x1 - m_r1World); } // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix) - Vector3 I1B2CrossA1 = m_i1 * mB2CrossA1; - Vector3 I1C2CrossA1 = m_i1 * mC2CrossA1; - Vector3 I2B2CrossA1 = m_i2 * mB2CrossA1; - Vector3 I2C2CrossA1 = m_i2 * mC2CrossA1; + vec3 I1B2CrossA1 = m_i1 * mB2CrossA1; + vec3 I1C2CrossA1 = m_i1 * mC2CrossA1; + vec3 I2B2CrossA1 = m_i2 * mB2CrossA1; + vec3 I2C2CrossA1 = m_i2 * mC2CrossA1; const float el11 = mB2CrossA1.dot(I1B2CrossA1) + mB2CrossA1.dot(I2B2CrossA1); const float el12 = mB2CrossA1.dot(I1C2CrossA1) + @@ -134,24 +134,24 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat mC2CrossA1.dot(I2B2CrossA1); const float el22 = mC2CrossA1.dot(I1C2CrossA1) + mC2CrossA1.dot(I2C2CrossA1); - const Matrix2x2 matrixKRotation(el11, el12, el21, el22); - m_inverseMassMatrixRotation.setToZero(); + const etk::Matrix2x2 matrixKRotation(el11, el12, el21, el22); + m_inverseMassMatrixRotation.setZero(); if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { m_inverseMassMatrixRotation = matrixKRotation.getInverse(); } // Compute the bias "b" of the rotation constraints - mBRotation.setToZero(); + mBRotation.setZero(); if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBRotation = biasFactor * Vector2(mA1.dot(b2), mA1.dot(c2)); + mBRotation = biasFactor * vec2(mA1.dot(b2), mA1.dot(c2)); } // If warm-starting is not enabled if (!constraintSolverData.isWarmStartingActive) { // Reset all the accumulated impulses - m_impulseTranslation.setToZero(); - m_impulseRotation.setToZero(); + m_impulseTranslation.setZero(); + m_impulseRotation.setZero(); m_impulseLowerLimit = 0.0; m_impulseUpperLimit = 0.0; m_impulseMotor = 0.0; @@ -163,7 +163,7 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix) m_inverseMassMatrixLimitMotor = mA1.dot(m_i1 * mA1) + mA1.dot(m_i2 * mA1); m_inverseMassMatrixLimitMotor = (m_inverseMassMatrixLimitMotor > 0.0) ? - float(1.0) / m_inverseMassMatrixLimitMotor : float(0.0); + 1.0f / m_inverseMassMatrixLimitMotor : 0.0f; if (mIsLimitEnabled) { @@ -186,27 +186,27 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[m_indexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[m_indexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[m_indexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[m_indexBody2]; + vec3& v1 = constraintSolverData.linearVelocities[m_indexBody1]; + vec3& v2 = constraintSolverData.linearVelocities[m_indexBody2]; + vec3& w1 = constraintSolverData.angularVelocities[m_indexBody1]; + vec3& w2 = constraintSolverData.angularVelocities[m_indexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies const float inverseMassBody1 = m_body1->m_massInverse; const float inverseMassBody2 = m_body2->m_massInverse; // Compute the impulse P=J^T * lambda for the 2 rotation constraints - Vector3 rotationImpulse = -mB2CrossA1 * m_impulseRotation.x - mC2CrossA1 * m_impulseRotation.y; + vec3 rotationImpulse = -mB2CrossA1 * m_impulseRotation.x() - mC2CrossA1 * m_impulseRotation.y(); // Compute the impulse P=J^T * lambda for the lower and upper limits constraints - const Vector3 limitsImpulse = (m_impulseUpperLimit - m_impulseLowerLimit) * mA1; + const vec3 limitsImpulse = (m_impulseUpperLimit - m_impulseLowerLimit) * mA1; // Compute the impulse P=J^T * lambda for the motor constraint - const Vector3 motorImpulse = -m_impulseMotor * mA1; + const vec3 motorImpulse = -m_impulseMotor * mA1; // Compute the impulse P=J^T * lambda for the 3 translation constraints of body 1 - Vector3 linearImpulseBody1 = -m_impulseTranslation; - Vector3 angularImpulseBody1 = m_impulseTranslation.cross(m_r1World); + vec3 linearImpulseBody1 = -m_impulseTranslation; + vec3 angularImpulseBody1 = m_impulseTranslation.cross(m_r1World); // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1 angularImpulseBody1 += rotationImpulse; @@ -222,7 +222,7 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { w1 += m_i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 3 translation constraints of body 2 - Vector3 angularImpulseBody2 = -m_impulseTranslation.cross(m_r2World); + vec3 angularImpulseBody2 = -m_impulseTranslation.cross(m_r2World); // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2 angularImpulseBody2 += -rotationImpulse; @@ -242,10 +242,10 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[m_indexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[m_indexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[m_indexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[m_indexBody2]; + vec3& v1 = constraintSolverData.linearVelocities[m_indexBody1]; + vec3& v2 = constraintSolverData.linearVelocities[m_indexBody2]; + vec3& w1 = constraintSolverData.angularVelocities[m_indexBody1]; + vec3& w2 = constraintSolverData.angularVelocities[m_indexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies float inverseMassBody1 = m_body1->m_massInverse; @@ -254,23 +254,23 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS // --------------- Translation Constraints --------------- // // Compute J*v - const Vector3 JvTranslation = v2 + w2.cross(m_r2World) - v1 - w1.cross(m_r1World); + const vec3 JvTranslation = v2 + w2.cross(m_r2World) - v1 - w1.cross(m_r1World); // Compute the Lagrange multiplier lambda - const Vector3 deltaLambdaTranslation = m_inverseMassMatrixTranslation * + const vec3 deltaLambdaTranslation = m_inverseMassMatrixTranslation * (-JvTranslation - mBTranslation); m_impulseTranslation += deltaLambdaTranslation; // Compute the impulse P=J^T * lambda of body 1 - const Vector3 linearImpulseBody1 = -deltaLambdaTranslation; - Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(m_r1World); + const vec3 linearImpulseBody1 = -deltaLambdaTranslation; + vec3 angularImpulseBody1 = deltaLambdaTranslation.cross(m_r1World); // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; w1 += m_i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda of body 2 - Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(m_r2World); + vec3 angularImpulseBody2 = -deltaLambdaTranslation.cross(m_r2World); // Apply the impulse to the body 2 v2 += inverseMassBody2 * deltaLambdaTranslation; @@ -279,23 +279,23 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS // --------------- Rotation Constraints --------------- // // Compute J*v for the 2 rotation constraints - const Vector2 JvRotation(-mB2CrossA1.dot(w1) + mB2CrossA1.dot(w2), + const vec2 JvRotation(-mB2CrossA1.dot(w1) + mB2CrossA1.dot(w2), -mC2CrossA1.dot(w1) + mC2CrossA1.dot(w2)); // Compute the Lagrange multiplier lambda for the 2 rotation constraints - Vector2 deltaLambdaRotation = m_inverseMassMatrixRotation * (-JvRotation - mBRotation); + vec2 deltaLambdaRotation = m_inverseMassMatrixRotation * (-JvRotation - mBRotation); m_impulseRotation += deltaLambdaRotation; // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1 - angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x - - mC2CrossA1 * deltaLambdaRotation.y; + angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x() - + mC2CrossA1 * deltaLambdaRotation.y(); // Apply the impulse to the body 1 w1 += m_i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2 - angularImpulseBody2 = mB2CrossA1 * deltaLambdaRotation.x + - mC2CrossA1 * deltaLambdaRotation.y; + angularImpulseBody2 = mB2CrossA1 * deltaLambdaRotation.x() + + mC2CrossA1 * deltaLambdaRotation.y(); // Apply the impulse to the body 2 w2 += m_i2 * angularImpulseBody2; @@ -313,17 +313,17 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS // Compute the Lagrange multiplier lambda for the lower limit constraint float deltaLambdaLower = m_inverseMassMatrixLimitMotor * (-JvLowerLimit -mBLowerLimit); float lambdaTemp = m_impulseLowerLimit; - m_impulseLowerLimit = std::max(m_impulseLowerLimit + deltaLambdaLower, float(0.0)); + m_impulseLowerLimit = std::max(m_impulseLowerLimit + deltaLambdaLower, 0.0f); deltaLambdaLower = m_impulseLowerLimit - lambdaTemp; // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 - const Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1; + const vec3 angularImpulseBody1 = -deltaLambdaLower * mA1; // Apply the impulse to the body 1 w1 += m_i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 - const Vector3 angularImpulseBody2 = deltaLambdaLower * mA1; + const vec3 angularImpulseBody2 = deltaLambdaLower * mA1; // Apply the impulse to the body 2 w2 += m_i2 * angularImpulseBody2; @@ -338,17 +338,17 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS // Compute the Lagrange multiplier lambda for the upper limit constraint float deltaLambdaUpper = m_inverseMassMatrixLimitMotor * (-JvUpperLimit -mBUpperLimit); float lambdaTemp = m_impulseUpperLimit; - m_impulseUpperLimit = std::max(m_impulseUpperLimit + deltaLambdaUpper, float(0.0)); + m_impulseUpperLimit = std::max(m_impulseUpperLimit + deltaLambdaUpper, 0.0f); deltaLambdaUpper = m_impulseUpperLimit - lambdaTemp; // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 - const Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1; + const vec3 angularImpulseBody1 = deltaLambdaUpper * mA1; // Apply the impulse to the body 1 w1 += m_i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 - const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mA1; + const vec3 angularImpulseBody2 = -deltaLambdaUpper * mA1; // Apply the impulse to the body 2 w2 += m_i2 * angularImpulseBody2; @@ -371,13 +371,13 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS deltaLambdaMotor = m_impulseMotor - lambdaTemp; // Compute the impulse P=J^T * lambda for the motor of body 1 - const Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1; + const vec3 angularImpulseBody1 = -deltaLambdaMotor * mA1; // Apply the impulse to the body 1 w1 += m_i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the motor of body 2 - const Vector3 angularImpulseBody2 = deltaLambdaMotor * mA1; + const vec3 angularImpulseBody2 = deltaLambdaMotor * mA1; // Apply the impulse to the body 2 w2 += m_i2 * angularImpulseBody2; @@ -392,10 +392,10 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS if (m_positionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies positions and orientations - Vector3& x1 = constraintSolverData.positions[m_indexBody1]; - Vector3& x2 = constraintSolverData.positions[m_indexBody2]; - Quaternion& q1 = constraintSolverData.orientations[m_indexBody1]; - Quaternion& q2 = constraintSolverData.orientations[m_indexBody2]; + vec3& x1 = constraintSolverData.positions[m_indexBody1]; + vec3& x2 = constraintSolverData.positions[m_indexBody2]; + etk::Quaternion& q1 = constraintSolverData.orientations[m_indexBody1]; + etk::Quaternion& q2 = constraintSolverData.orientations[m_indexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies float inverseMassBody1 = m_body1->m_massInverse; @@ -420,70 +420,70 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // Compute vectors needed in the Jacobian mA1 = q1 * mHingeLocalAxisBody1; - Vector3 a2 = q2 * mHingeLocalAxisBody2; + vec3 a2 = q2 * mHingeLocalAxisBody2; mA1.normalize(); a2.normalize(); - const Vector3 b2 = a2.getOneUnitOrthogonalVector(); - const Vector3 c2 = a2.cross(b2); + const vec3 b2 = a2.getOneUnitOrthogonalVector(); + const vec3 c2 = a2.cross(b2); mB2CrossA1 = b2.cross(mA1); mC2CrossA1 = c2.cross(mA1); // Compute the corresponding skew-symmetric matrices - Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World); - Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World); + etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World); + etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World); // --------------- Translation Constraints --------------- // // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints float inverseMassBodies = m_body1->m_massInverse + m_body2->m_massInverse; - Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, + etk::Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose(); - m_inverseMassMatrixTranslation.setToZero(); + m_inverseMassMatrixTranslation.setZero(); if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { m_inverseMassMatrixTranslation = massMatrix.getInverse(); } // Compute position error for the 3 translation constraints - const Vector3 errorTranslation = x2 + m_r2World - x1 - m_r1World; + const vec3 errorTranslation = x2 + m_r2World - x1 - m_r1World; // Compute the Lagrange multiplier lambda - const Vector3 lambdaTranslation = m_inverseMassMatrixTranslation * (-errorTranslation); + const vec3 lambdaTranslation = m_inverseMassMatrixTranslation * (-errorTranslation); // Compute the impulse of body 1 - Vector3 linearImpulseBody1 = -lambdaTranslation; - Vector3 angularImpulseBody1 = lambdaTranslation.cross(m_r1World); + vec3 linearImpulseBody1 = -lambdaTranslation; + vec3 angularImpulseBody1 = lambdaTranslation.cross(m_r1World); // Compute the pseudo velocity of body 1 - const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - Vector3 w1 = m_i1 * angularImpulseBody1; + const vec3 v1 = inverseMassBody1 * linearImpulseBody1; + vec3 w1 = m_i1 * angularImpulseBody1; // Update the body position/orientation of body 1 x1 += v1; - q1 += Quaternion(0, w1) * q1 * float(0.5); + q1 += etk::Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse of body 2 - Vector3 angularImpulseBody2 = -lambdaTranslation.cross(m_r2World); + vec3 angularImpulseBody2 = -lambdaTranslation.cross(m_r2World); // Compute the pseudo velocity of body 2 - const Vector3 v2 = inverseMassBody2 * lambdaTranslation; - Vector3 w2 = m_i2 * angularImpulseBody2; + const vec3 v2 = inverseMassBody2 * lambdaTranslation; + vec3 w2 = m_i2 * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; - q2 += Quaternion(0, w2) * q2 * float(0.5); + q2 += etk::Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); // --------------- Rotation Constraints --------------- // // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix) - Vector3 I1B2CrossA1 = m_i1 * mB2CrossA1; - Vector3 I1C2CrossA1 = m_i1 * mC2CrossA1; - Vector3 I2B2CrossA1 = m_i2 * mB2CrossA1; - Vector3 I2C2CrossA1 = m_i2 * mC2CrossA1; + vec3 I1B2CrossA1 = m_i1 * mB2CrossA1; + vec3 I1C2CrossA1 = m_i1 * mC2CrossA1; + vec3 I2B2CrossA1 = m_i2 * mB2CrossA1; + vec3 I2C2CrossA1 = m_i2 * mC2CrossA1; const float el11 = mB2CrossA1.dot(I1B2CrossA1) + mB2CrossA1.dot(I2B2CrossA1); const float el12 = mB2CrossA1.dot(I1C2CrossA1) + @@ -492,36 +492,36 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS mC2CrossA1.dot(I2B2CrossA1); const float el22 = mC2CrossA1.dot(I1C2CrossA1) + mC2CrossA1.dot(I2C2CrossA1); - const Matrix2x2 matrixKRotation(el11, el12, el21, el22); - m_inverseMassMatrixRotation.setToZero(); + const etk::Matrix2x2 matrixKRotation(el11, el12, el21, el22); + m_inverseMassMatrixRotation.setZero(); if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { m_inverseMassMatrixRotation = matrixKRotation.getInverse(); } // Compute the position error for the 3 rotation constraints - const Vector2 errorRotation = Vector2(mA1.dot(b2), mA1.dot(c2)); + const vec2 errorRotation = vec2(mA1.dot(b2), mA1.dot(c2)); // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector2 lambdaRotation = m_inverseMassMatrixRotation * (-errorRotation); + vec2 lambdaRotation = m_inverseMassMatrixRotation * (-errorRotation); // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 - angularImpulseBody1 = -mB2CrossA1 * lambdaRotation.x - mC2CrossA1 * lambdaRotation.y; + angularImpulseBody1 = -mB2CrossA1 * lambdaRotation.x() - mC2CrossA1 * lambdaRotation.y(); // Compute the pseudo velocity of body 1 w1 = m_i1 * angularImpulseBody1; // Update the body position/orientation of body 1 - q1 += Quaternion(0, w1) * q1 * float(0.5); + q1 += etk::Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse of body 2 - angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x + mC2CrossA1 * lambdaRotation.y; + angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x() + mC2CrossA1 * lambdaRotation.y(); // Compute the pseudo velocity of body 2 w2 = m_i2 * angularImpulseBody2; // Update the body position/orientation of body 2 - q2 += Quaternion(0, w2) * q2 * float(0.5); + q2 += etk::Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); // --------------- Limits Constraints --------------- // @@ -533,7 +533,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) m_inverseMassMatrixLimitMotor = mA1.dot(m_i1 * mA1) + mA1.dot(m_i2 * mA1); m_inverseMassMatrixLimitMotor = (m_inverseMassMatrixLimitMotor > 0.0) ? - float(1.0) / m_inverseMassMatrixLimitMotor : float(0.0); + 1.0f / m_inverseMassMatrixLimitMotor : 0.0f; } // If the lower limit is violated @@ -543,23 +543,23 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS float lambdaLowerLimit = m_inverseMassMatrixLimitMotor * (-lowerLimitError ); // Compute the impulse P=J^T * lambda of body 1 - const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1; + const vec3 angularImpulseBody1 = -lambdaLowerLimit * mA1; // Compute the pseudo velocity of body 1 - const Vector3 w1 = m_i1 * angularImpulseBody1; + const vec3 w1 = m_i1 * angularImpulseBody1; // Update the body position/orientation of body 1 - q1 += Quaternion(0, w1) * q1 * float(0.5); + q1 += etk::Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse P=J^T * lambda of body 2 - const Vector3 angularImpulseBody2 = lambdaLowerLimit * mA1; + const vec3 angularImpulseBody2 = lambdaLowerLimit * mA1; // Compute the pseudo velocity of body 2 - const Vector3 w2 = m_i2 * angularImpulseBody2; + const vec3 w2 = m_i2 * angularImpulseBody2; // Update the body position/orientation of body 2 - q2 += Quaternion(0, w2) * q2 * float(0.5); + q2 += etk::Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); } @@ -570,23 +570,23 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS float lambdaUpperLimit = m_inverseMassMatrixLimitMotor * (-upperLimitError); // Compute the impulse P=J^T * lambda of body 1 - const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1; + const vec3 angularImpulseBody1 = lambdaUpperLimit * mA1; // Compute the pseudo velocity of body 1 - const Vector3 w1 = m_i1 * angularImpulseBody1; + const vec3 w1 = m_i1 * angularImpulseBody1; // Update the body position/orientation of body 1 - q1 += Quaternion(0, w1) * q1 * float(0.5); + q1 += etk::Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse P=J^T * lambda of body 2 - const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mA1; + const vec3 angularImpulseBody2 = -lambdaUpperLimit * mA1; // Compute the pseudo velocity of body 2 - const Vector3 w2 = m_i2 * angularImpulseBody2; + const vec3 w2 = m_i2 * angularImpulseBody2; // Update the body position/orientation of body 2 - q2 += Quaternion(0, w2) * q2 * float(0.5); + q2 += etk::Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); } } @@ -742,17 +742,17 @@ float HingeJoint::computeCorrespondingAngleNearLimits(float inputAngle, float lo } // Compute the current angle around the hinge axis -float HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1, - const Quaternion& orientationBody2) { +float HingeJoint::computeCurrentHingeAngle(const etk::Quaternion& orientationBody1, + const etk::Quaternion& orientationBody2) { float hingeAngle; // Compute the current orientation difference between the two bodies - Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse(); + etk::Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse(); currentOrientationDiff.normalize(); // Compute the relative rotation considering the initial orientation difference - Quaternion relativeRotation = currentOrientationDiff * m_initOrientationDifferenceInv; + etk::Quaternion relativeRotation = currentOrientationDiff * m_initOrientationDifferenceInv; relativeRotation.normalize(); // A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit @@ -769,7 +769,7 @@ float HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1, float dotProduct = relativeRotation.getVectorV().dot(mA1); // If the relative rotation axis and the hinge axis are pointing the same direction - if (dotProduct >= float(0.0)) { + if (dotProduct >= 0.0f) { hingeAngle = float(2.0) * std::atan2(sinHalfAngleAbs, cosHalfAngle); } else { diff --git a/ephysics/constraint/HingeJoint.h b/ephysics/constraint/HingeJoint.h index 6c4cd8a..681206c 100644 --- a/ephysics/constraint/HingeJoint.h +++ b/ephysics/constraint/HingeJoint.h @@ -23,10 +23,10 @@ struct HingeJointInfo : public JointInfo { // -------------------- Attributes -------------------- // /// Anchor point (in world-space coordinates) - Vector3 m_anchorPointWorldSpace; + vec3 m_anchorPointWorldSpace; /// Hinge rotation axis (in world-space coordinates) - Vector3 rotationAxisWorld; + vec3 rotationAxisWorld; /// True if the hinge joint limits are enabled bool isLimitEnabled; @@ -59,8 +59,8 @@ struct HingeJointInfo : public JointInfo { * coordinates */ HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const Vector3& initAnchorPointWorldSpace, - const Vector3& initRotationAxisWorld) + const vec3& initAnchorPointWorldSpace, + const vec3& initRotationAxisWorld) : JointInfo(rigidBody1, rigidBody2, HINGEJOINT), m_anchorPointWorldSpace(initAnchorPointWorldSpace), rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(false), @@ -77,8 +77,8 @@ struct HingeJointInfo : public JointInfo { * @param initMaxAngleLimit The initial maximum limit angle (in radian) */ HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const Vector3& initAnchorPointWorldSpace, - const Vector3& initRotationAxisWorld, + const vec3& initAnchorPointWorldSpace, + const vec3& initRotationAxisWorld, float initMinAngleLimit, float initMaxAngleLimit) : JointInfo(rigidBody1, rigidBody2, HINGEJOINT), m_anchorPointWorldSpace(initAnchorPointWorldSpace), @@ -99,8 +99,8 @@ struct HingeJointInfo : public JointInfo { * @param initMaxMotorTorque The initial maximum motor torque (in Newtons) */ HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const Vector3& initAnchorPointWorldSpace, - const Vector3& initRotationAxisWorld, + const vec3& initAnchorPointWorldSpace, + const vec3& initRotationAxisWorld, float initMinAngleLimit, float initMaxAngleLimit, float initMotorSpeed, float initMaxMotorTorque) : JointInfo(rigidBody1, rigidBody2, HINGEJOINT), @@ -129,43 +129,43 @@ class HingeJoint : public Joint { // -------------------- Attributes -------------------- // /// Anchor point of body 1 (in local-space coordinates of body 1) - Vector3 m_localAnchorPointBody1; + vec3 m_localAnchorPointBody1; /// Anchor point of body 2 (in local-space coordinates of body 2) - Vector3 m_localAnchorPointBody2; + vec3 m_localAnchorPointBody2; /// Hinge rotation axis (in local-space coordinates of body 1) - Vector3 mHingeLocalAxisBody1; + vec3 mHingeLocalAxisBody1; /// Hinge rotation axis (in local-space coordiantes of body 2) - Vector3 mHingeLocalAxisBody2; + vec3 mHingeLocalAxisBody2; /// Inertia tensor of body 1 (in world-space coordinates) - Matrix3x3 m_i1; + etk::Matrix3x3 m_i1; /// Inertia tensor of body 2 (in world-space coordinates) - Matrix3x3 m_i2; + etk::Matrix3x3 m_i2; /// Hinge rotation axis (in world-space coordinates) computed from body 1 - Vector3 mA1; + vec3 mA1; /// Vector from center of body 2 to anchor point in world-space - Vector3 m_r1World; + vec3 m_r1World; /// Vector from center of body 2 to anchor point in world-space - Vector3 m_r2World; + vec3 m_r2World; /// Cross product of vector b2 and a1 - Vector3 mB2CrossA1; + vec3 mB2CrossA1; /// Cross product of vector c2 and a1; - Vector3 mC2CrossA1; + vec3 mC2CrossA1; /// Impulse for the 3 translation constraints - Vector3 m_impulseTranslation; + vec3 m_impulseTranslation; /// Impulse for the 2 rotation constraints - Vector2 m_impulseRotation; + vec2 m_impulseRotation; /// Accumulated impulse for the lower limit constraint float m_impulseLowerLimit; @@ -177,10 +177,10 @@ class HingeJoint : public Joint { float m_impulseMotor; /// Inverse mass matrix K=JM^-1J^t for the 3 translation constraints - Matrix3x3 m_inverseMassMatrixTranslation; + etk::Matrix3x3 m_inverseMassMatrixTranslation; /// Inverse mass matrix K=JM^-1J^t for the 2 rotation constraints - Matrix2x2 m_inverseMassMatrixRotation; + etk::Matrix2x2 m_inverseMassMatrixRotation; /// Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) float m_inverseMassMatrixLimitMotor; @@ -189,10 +189,10 @@ class HingeJoint : public Joint { float m_inverseMassMatrixMotor; /// Bias vector for the error correction for the translation constraints - Vector3 mBTranslation; + vec3 mBTranslation; /// Bias vector for the error correction for the rotation constraints - Vector2 mBRotation; + vec2 mBRotation; /// Bias of the lower limit constraint float mBLowerLimit; @@ -201,7 +201,7 @@ class HingeJoint : public Joint { float mBUpperLimit; /// Inverse of the initial orientation difference between the bodies - Quaternion m_initOrientationDifferenceInv; + etk::Quaternion m_initOrientationDifferenceInv; /// True if the joint limits are enabled bool mIsLimitEnabled; @@ -249,8 +249,8 @@ class HingeJoint : public Joint { float upperLimitAngle) const; /// Compute the current angle around the hinge axis - float computeCurrentHingeAngle(const Quaternion& orientationBody1, - const Quaternion& orientationBody2); + float computeCurrentHingeAngle(const etk::Quaternion& orientationBody1, + const etk::Quaternion& orientationBody2); /// Return the number of bytes used by the joint virtual size_t getSizeInBytes() const; diff --git a/ephysics/constraint/SliderJoint.cpp b/ephysics/constraint/SliderJoint.cpp index ba06f73..896fbb6 100644 --- a/ephysics/constraint/SliderJoint.cpp +++ b/ephysics/constraint/SliderJoint.cpp @@ -27,8 +27,8 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) assert(mMaxMotorForce >= 0.0); // Compute the local-space anchor point for each body - const Transform& transform1 = m_body1->getTransform(); - const Transform& transform2 = m_body2->getTransform(); + const etk::Transform3D& transform1 = m_body1->getTransform(); + const etk::Transform3D& transform2 = m_body2->getTransform(); m_localAnchorPointBody1 = transform1.getInverse() * jointInfo.m_anchorPointWorldSpace; m_localAnchorPointBody2 = transform2.getInverse() * jointInfo.m_anchorPointWorldSpace; @@ -57,10 +57,10 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa m_indexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(m_body2)->second; // Get the bodies positions and orientations - const Vector3& x1 = m_body1->m_centerOfMassWorld; - const Vector3& x2 = m_body2->m_centerOfMassWorld; - const Quaternion& orientationBody1 = m_body1->getTransform().getOrientation(); - const Quaternion& orientationBody2 = m_body2->getTransform().getOrientation(); + const vec3& x1 = m_body1->m_centerOfMassWorld; + const vec3& x2 = m_body2->m_centerOfMassWorld; + const etk::Quaternion& orientationBody1 = m_body1->getTransform().getOrientation(); + const etk::Quaternion& orientationBody2 = m_body2->getTransform().getOrientation(); // Get the inertia tensor of bodies m_i1 = m_body1->getInertiaTensorInverseWorld(); @@ -71,7 +71,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mR2 = orientationBody2 * m_localAnchorPointBody2; // Compute the vector u (difference between anchor points) - const Vector3 u = x2 + mR2 - x1 - mR1; + const vec3 u = x2 + mR2 - x1 - mR1; // Compute the two orthogonal vectors to the slider axis in world-space mSliderAxisWorld = orientationBody1 * mSliderAxisBody1; @@ -98,7 +98,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mR2CrossN1 = mR2.cross(mN1); mR2CrossN2 = mR2.cross(mN2); mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld); - const Vector3 r1PlusU = mR1 + u; + const vec3 r1PlusU = mR1 + u; mR1PlusUCrossN1 = (r1PlusU).cross(mN1); mR1PlusUCrossN2 = (r1PlusU).cross(mN2); mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld); @@ -106,10 +106,10 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // constraints (2x2 matrix) float sumInverseMass = m_body1->m_massInverse + m_body2->m_massInverse; - Vector3 I1R1PlusUCrossN1 = m_i1 * mR1PlusUCrossN1; - Vector3 I1R1PlusUCrossN2 = m_i1 * mR1PlusUCrossN2; - Vector3 I2R2CrossN1 = m_i2 * mR2CrossN1; - Vector3 I2R2CrossN2 = m_i2 * mR2CrossN2; + vec3 I1R1PlusUCrossN1 = m_i1 * mR1PlusUCrossN1; + vec3 I1R1PlusUCrossN2 = m_i1 * mR1PlusUCrossN2; + vec3 I2R2CrossN1 = m_i2 * mR2CrossN1; + vec3 I2R2CrossN2 = m_i2 * mR2CrossN2; const float el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + mR2CrossN1.dot(I2R2CrossN1); const float el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + @@ -118,18 +118,18 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mR2CrossN2.dot(I2R2CrossN1); const float el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + mR2CrossN2.dot(I2R2CrossN2); - Matrix2x2 matrixKTranslation(el11, el12, el21, el22); - m_inverseMassMatrixTranslationConstraint.setToZero(); + etk::Matrix2x2 matrixKTranslation(el11, el12, el21, el22); + m_inverseMassMatrixTranslationConstraint.setZero(); if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { m_inverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); } // Compute the bias "b" of the translation constraint - mBTranslation.setToZero(); + mBTranslation.setZero(); float biasFactor = (BETA / constraintSolverData.timeStep); if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBTranslation.x = u.dot(mN1); - mBTranslation.y = u.dot(mN2); + mBTranslation.x() = u.dot(mN1); + mBTranslation.y() = u.dot(mN2); mBTranslation *= biasFactor; } @@ -141,11 +141,11 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa } // Compute the bias "b" of the rotation constraint - mBRotation.setToZero(); + mBRotation.setZero(); if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { - Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); + etk::Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); currentOrientationDifference.normalize(); - const Quaternion qError = currentOrientationDifference * m_initOrientationDifferenceInv; + const etk::Quaternion qError = currentOrientationDifference * m_initOrientationDifferenceInv; mBRotation = biasFactor * float(2.0) * qError.getVectorV(); } @@ -157,7 +157,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mR1PlusUCrossSliderAxis.dot(m_i1 * mR1PlusUCrossSliderAxis) + mR2CrossSliderAxis.dot(m_i2 * mR2CrossSliderAxis); m_inverseMassMatrixLimit = (m_inverseMassMatrixLimit > 0.0) ? - float(1.0) / m_inverseMassMatrixLimit : float(0.0); + 1.0f / m_inverseMassMatrixLimit : 0.0f; // Compute the bias "b" of the lower limit constraint mBLowerLimit = 0.0; @@ -178,15 +178,15 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix) m_inverseMassMatrixMotor = m_body1->m_massInverse + m_body2->m_massInverse; m_inverseMassMatrixMotor = (m_inverseMassMatrixMotor > 0.0) ? - float(1.0) / m_inverseMassMatrixMotor : float(0.0); + 1.0f / m_inverseMassMatrixMotor : 0.0f; } // If warm-starting is not enabled if (!constraintSolverData.isWarmStartingActive) { // Reset all the accumulated impulses - m_impulseTranslation.setToZero(); - m_impulseRotation.setToZero(); + m_impulseTranslation.setZero(); + m_impulseRotation.setZero(); m_impulseLowerLimit = 0.0; m_impulseUpperLimit = 0.0; m_impulseMotor = 0.0; @@ -197,10 +197,10 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[m_indexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[m_indexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[m_indexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[m_indexBody2]; + vec3& v1 = constraintSolverData.linearVelocities[m_indexBody1]; + vec3& v2 = constraintSolverData.linearVelocities[m_indexBody2]; + vec3& w1 = constraintSolverData.angularVelocities[m_indexBody1]; + vec3& w2 = constraintSolverData.angularVelocities[m_indexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies const float inverseMassBody1 = m_body1->m_massInverse; @@ -208,15 +208,15 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1 float impulseLimits = m_impulseUpperLimit - m_impulseLowerLimit; - Vector3 linearImpulseLimits = impulseLimits * mSliderAxisWorld; + vec3 linearImpulseLimits = impulseLimits * mSliderAxisWorld; // Compute the impulse P=J^T * lambda for the motor constraint of body 1 - Vector3 impulseMotor = m_impulseMotor * mSliderAxisWorld; + vec3 impulseMotor = m_impulseMotor * mSliderAxisWorld; // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 - Vector3 linearImpulseBody1 = -mN1 * m_impulseTranslation.x - mN2 * m_impulseTranslation.y; - Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * m_impulseTranslation.x - - mR1PlusUCrossN2 * m_impulseTranslation.y; + vec3 linearImpulseBody1 = -mN1 * m_impulseTranslation.x() - mN2 * m_impulseTranslation.y(); + vec3 angularImpulseBody1 = -mR1PlusUCrossN1 * m_impulseTranslation.x() - + mR1PlusUCrossN2 * m_impulseTranslation.y(); // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 angularImpulseBody1 += -m_impulseRotation; @@ -233,9 +233,9 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { w1 += m_i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 - Vector3 linearImpulseBody2 = mN1 * m_impulseTranslation.x + mN2 * m_impulseTranslation.y; - Vector3 angularImpulseBody2 = mR2CrossN1 * m_impulseTranslation.x + - mR2CrossN2 * m_impulseTranslation.y; + vec3 linearImpulseBody2 = mN1 * m_impulseTranslation.x() + mN2 * m_impulseTranslation.y(); + vec3 angularImpulseBody2 = mR2CrossN1 * m_impulseTranslation.x() + + mR2CrossN2 * m_impulseTranslation.y(); // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 angularImpulseBody2 += m_impulseRotation; @@ -256,10 +256,10 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[m_indexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[m_indexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[m_indexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[m_indexBody2]; + vec3& v1 = constraintSolverData.linearVelocities[m_indexBody1]; + vec3& v2 = constraintSolverData.linearVelocities[m_indexBody2]; + vec3& w1 = constraintSolverData.angularVelocities[m_indexBody1]; + vec3& w2 = constraintSolverData.angularVelocities[m_indexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies float inverseMassBody1 = m_body1->m_massInverse; @@ -272,24 +272,24 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint mN1.dot(v2) + w2.dot(mR2CrossN1); const float el2 = -mN2.dot(v1) - w1.dot(mR1PlusUCrossN2) + mN2.dot(v2) + w2.dot(mR2CrossN2); - const Vector2 JvTranslation(el1, el2); + const vec2 JvTranslation(el1, el2); // Compute the Lagrange multiplier lambda for the 2 translation constraints - Vector2 deltaLambda = m_inverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation); + vec2 deltaLambda = m_inverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation); m_impulseTranslation += deltaLambda; // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 - const Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y; - Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x - - mR1PlusUCrossN2 * deltaLambda.y; + const vec3 linearImpulseBody1 = -mN1 * deltaLambda.x() - mN2 * deltaLambda.y(); + vec3 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x() - + mR1PlusUCrossN2 * deltaLambda.y(); // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; w1 += m_i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 - const Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y; - Vector3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x + mR2CrossN2 * deltaLambda.y; + const vec3 linearImpulseBody2 = mN1 * deltaLambda.x() + mN2 * deltaLambda.y(); + vec3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x() + mR2CrossN2 * deltaLambda.y(); // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; @@ -298,10 +298,10 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // --------------- Rotation Constraints --------------- // // Compute J*v for the 3 rotation constraints - const Vector3 JvRotation = w2 - w1; + const vec3 JvRotation = w2 - w1; // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector3 deltaLambda2 = m_inverseMassMatrixRotationConstraint * (-JvRotation - mBRotation); + vec3 deltaLambda2 = m_inverseMassMatrixRotationConstraint * (-JvRotation - mBRotation); m_impulseRotation += deltaLambda2; // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 @@ -330,20 +330,20 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // Compute the Lagrange multiplier lambda for the lower limit constraint float deltaLambdaLower = m_inverseMassMatrixLimit * (-JvLowerLimit -mBLowerLimit); float lambdaTemp = m_impulseLowerLimit; - m_impulseLowerLimit = std::max(m_impulseLowerLimit + deltaLambdaLower, float(0.0)); + m_impulseLowerLimit = std::max(m_impulseLowerLimit + deltaLambdaLower, 0.0f); deltaLambdaLower = m_impulseLowerLimit - lambdaTemp; // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 - const Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld; - const Vector3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis; + const vec3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld; + const vec3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis; // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; w1 += m_i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 - const Vector3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld; - const Vector3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis; + const vec3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld; + const vec3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis; // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; @@ -360,20 +360,20 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // Compute the Lagrange multiplier lambda for the upper limit constraint float deltaLambdaUpper = m_inverseMassMatrixLimit * (-JvUpperLimit -mBUpperLimit); float lambdaTemp = m_impulseUpperLimit; - m_impulseUpperLimit = std::max(m_impulseUpperLimit + deltaLambdaUpper, float(0.0)); + m_impulseUpperLimit = std::max(m_impulseUpperLimit + deltaLambdaUpper, 0.0f); deltaLambdaUpper = m_impulseUpperLimit - lambdaTemp; // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 - const Vector3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld; - const Vector3 angularImpulseBody1 = deltaLambdaUpper * mR1PlusUCrossSliderAxis; + const vec3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld; + const vec3 angularImpulseBody1 = deltaLambdaUpper * mR1PlusUCrossSliderAxis; // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; w1 += m_i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 - const Vector3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld; - const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mR2CrossSliderAxis; + const vec3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld; + const vec3 angularImpulseBody2 = -deltaLambdaUpper * mR2CrossSliderAxis; // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; @@ -396,13 +396,13 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint deltaLambdaMotor = m_impulseMotor - lambdaTemp; // Compute the impulse P=J^T * lambda for the motor of body 1 - const Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld; + const vec3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld; // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; // Compute the impulse P=J^T * lambda for the motor of body 2 - const Vector3 linearImpulseBody2 = -deltaLambdaMotor * mSliderAxisWorld; + const vec3 linearImpulseBody2 = -deltaLambdaMotor * mSliderAxisWorld; // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; @@ -417,10 +417,10 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint if (m_positionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies positions and orientations - Vector3& x1 = constraintSolverData.positions[m_indexBody1]; - Vector3& x2 = constraintSolverData.positions[m_indexBody2]; - Quaternion& q1 = constraintSolverData.orientations[m_indexBody1]; - Quaternion& q2 = constraintSolverData.orientations[m_indexBody2]; + vec3& x1 = constraintSolverData.positions[m_indexBody1]; + vec3& x2 = constraintSolverData.positions[m_indexBody2]; + etk::Quaternion& q1 = constraintSolverData.orientations[m_indexBody1]; + etk::Quaternion& q2 = constraintSolverData.orientations[m_indexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies float inverseMassBody1 = m_body1->m_massInverse; @@ -435,7 +435,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint mR2 = q2 * m_localAnchorPointBody2; // Compute the vector u (difference between anchor points) - const Vector3 u = x2 + mR2 - x1 - mR1; + const vec3 u = x2 + mR2 - x1 - mR1; // Compute the two orthogonal vectors to the slider axis in world-space mSliderAxisWorld = q1 * mSliderAxisBody1; @@ -454,7 +454,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint mR2CrossN1 = mR2.cross(mN1); mR2CrossN2 = mR2.cross(mN2); mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld); - const Vector3 r1PlusU = mR1 + u; + const vec3 r1PlusU = mR1 + u; mR1PlusUCrossN1 = (r1PlusU).cross(mN1); mR1PlusUCrossN2 = (r1PlusU).cross(mN2); mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld); @@ -464,10 +464,10 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // constraints (2x2 matrix) float sumInverseMass = m_body1->m_massInverse + m_body2->m_massInverse; - Vector3 I1R1PlusUCrossN1 = m_i1 * mR1PlusUCrossN1; - Vector3 I1R1PlusUCrossN2 = m_i1 * mR1PlusUCrossN2; - Vector3 I2R2CrossN1 = m_i2 * mR2CrossN1; - Vector3 I2R2CrossN2 = m_i2 * mR2CrossN2; + vec3 I1R1PlusUCrossN1 = m_i1 * mR1PlusUCrossN1; + vec3 I1R1PlusUCrossN2 = m_i1 * mR1PlusUCrossN2; + vec3 I2R2CrossN1 = m_i2 * mR2CrossN1; + vec3 I2R2CrossN2 = m_i2 * mR2CrossN2; const float el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + mR2CrossN1.dot(I2R2CrossN1); const float el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + @@ -476,44 +476,44 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint mR2CrossN2.dot(I2R2CrossN1); const float el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + mR2CrossN2.dot(I2R2CrossN2); - Matrix2x2 matrixKTranslation(el11, el12, el21, el22); - m_inverseMassMatrixTranslationConstraint.setToZero(); + etk::Matrix2x2 matrixKTranslation(el11, el12, el21, el22); + m_inverseMassMatrixTranslationConstraint.setZero(); if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { m_inverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); } // Compute the position error for the 2 translation constraints - const Vector2 translationError(u.dot(mN1), u.dot(mN2)); + const vec2 translationError(u.dot(mN1), u.dot(mN2)); // Compute the Lagrange multiplier lambda for the 2 translation constraints - Vector2 lambdaTranslation = m_inverseMassMatrixTranslationConstraint * (-translationError); + vec2 lambdaTranslation = m_inverseMassMatrixTranslationConstraint * (-translationError); // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 - const Vector3 linearImpulseBody1 = -mN1 * lambdaTranslation.x - mN2 * lambdaTranslation.y; - Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * lambdaTranslation.x - - mR1PlusUCrossN2 * lambdaTranslation.y; + const vec3 linearImpulseBody1 = -mN1 * lambdaTranslation.x() - mN2 * lambdaTranslation.y(); + vec3 angularImpulseBody1 = -mR1PlusUCrossN1 * lambdaTranslation.x() - + mR1PlusUCrossN2 * lambdaTranslation.y(); // Apply the impulse to the body 1 - const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - Vector3 w1 = m_i1 * angularImpulseBody1; + const vec3 v1 = inverseMassBody1 * linearImpulseBody1; + vec3 w1 = m_i1 * angularImpulseBody1; // Update the body position/orientation of body 1 x1 += v1; - q1 += Quaternion(0, w1) * q1 * float(0.5); + q1 += etk::Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 - const Vector3 linearImpulseBody2 = mN1 * lambdaTranslation.x + mN2 * lambdaTranslation.y; - Vector3 angularImpulseBody2 = mR2CrossN1 * lambdaTranslation.x + - mR2CrossN2 * lambdaTranslation.y; + const vec3 linearImpulseBody2 = mN1 * lambdaTranslation.x() + mN2 * lambdaTranslation.y(); + vec3 angularImpulseBody2 = mR2CrossN1 * lambdaTranslation.x() + + mR2CrossN2 * lambdaTranslation.y(); // Apply the impulse to the body 2 - const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; - Vector3 w2 = m_i2 * angularImpulseBody2; + const vec3 v2 = inverseMassBody2 * linearImpulseBody2; + vec3 w2 = m_i2 * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; - q2 += Quaternion(0, w2) * q2 * float(0.5); + q2 += etk::Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); // --------------- Rotation Constraints --------------- // @@ -526,13 +526,13 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint } // Compute the position error for the 3 rotation constraints - Quaternion currentOrientationDifference = q2 * q1.getInverse(); + etk::Quaternion currentOrientationDifference = q2 * q1.getInverse(); currentOrientationDifference.normalize(); - const Quaternion qError = currentOrientationDifference * m_initOrientationDifferenceInv; - const Vector3 errorRotation = float(2.0) * qError.getVectorV(); + const etk::Quaternion qError = currentOrientationDifference * m_initOrientationDifferenceInv; + const vec3 errorRotation = float(2.0) * qError.getVectorV(); // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector3 lambdaRotation = m_inverseMassMatrixRotationConstraint * (-errorRotation); + vec3 lambdaRotation = m_inverseMassMatrixRotationConstraint * (-errorRotation); // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 angularImpulseBody1 = -lambdaRotation; @@ -541,7 +541,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint w1 = m_i1 * angularImpulseBody1; // Update the body position/orientation of body 1 - q1 += Quaternion(0, w1) * q1 * float(0.5); + q1 += etk::Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 @@ -551,7 +551,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint w2 = m_i2 * angularImpulseBody2; // Update the body position/orientation of body 2 - q2 += Quaternion(0, w2) * q2 * float(0.5); + q2 += etk::Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); // --------------- Limits Constraints --------------- // @@ -565,7 +565,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint mR1PlusUCrossSliderAxis.dot(m_i1 * mR1PlusUCrossSliderAxis) + mR2CrossSliderAxis.dot(m_i2 * mR2CrossSliderAxis); m_inverseMassMatrixLimit = (m_inverseMassMatrixLimit > 0.0) ? - float(1.0) / m_inverseMassMatrixLimit : float(0.0); + 1.0f / m_inverseMassMatrixLimit : 0.0f; } // If the lower limit is violated @@ -575,29 +575,29 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint float lambdaLowerLimit = m_inverseMassMatrixLimit * (-lowerLimitError); // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 - const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld; - const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mR1PlusUCrossSliderAxis; + const vec3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld; + const vec3 angularImpulseBody1 = -lambdaLowerLimit * mR1PlusUCrossSliderAxis; // Apply the impulse to the body 1 - const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - const Vector3 w1 = m_i1 * angularImpulseBody1; + const vec3 v1 = inverseMassBody1 * linearImpulseBody1; + const vec3 w1 = m_i1 * angularImpulseBody1; // Update the body position/orientation of body 1 x1 += v1; - q1 += Quaternion(0, w1) * q1 * float(0.5); + q1 += etk::Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 - const Vector3 linearImpulseBody2 = lambdaLowerLimit * mSliderAxisWorld; - const Vector3 angularImpulseBody2 = lambdaLowerLimit * mR2CrossSliderAxis; + const vec3 linearImpulseBody2 = lambdaLowerLimit * mSliderAxisWorld; + const vec3 angularImpulseBody2 = lambdaLowerLimit * mR2CrossSliderAxis; // Apply the impulse to the body 2 - const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; - const Vector3 w2 = m_i2 * angularImpulseBody2; + const vec3 v2 = inverseMassBody2 * linearImpulseBody2; + const vec3 w2 = m_i2 * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; - q2 += Quaternion(0, w2) * q2 * float(0.5); + q2 += etk::Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); } @@ -608,29 +608,29 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint float lambdaUpperLimit = m_inverseMassMatrixLimit * (-upperLimitError); // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 - const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld; - const Vector3 angularImpulseBody1 = lambdaUpperLimit * mR1PlusUCrossSliderAxis; + const vec3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld; + const vec3 angularImpulseBody1 = lambdaUpperLimit * mR1PlusUCrossSliderAxis; // Apply the impulse to the body 1 - const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - const Vector3 w1 = m_i1 * angularImpulseBody1; + const vec3 v1 = inverseMassBody1 * linearImpulseBody1; + const vec3 w1 = m_i1 * angularImpulseBody1; // Update the body position/orientation of body 1 x1 += v1; - q1 += Quaternion(0, w1) * q1 * float(0.5); + q1 += etk::Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 - const Vector3 linearImpulseBody2 = -lambdaUpperLimit * mSliderAxisWorld; - const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mR2CrossSliderAxis; + const vec3 linearImpulseBody2 = -lambdaUpperLimit * mSliderAxisWorld; + const vec3 angularImpulseBody2 = -lambdaUpperLimit * mR2CrossSliderAxis; // Apply the impulse to the body 2 - const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; - const Vector3 w2 = m_i2 * angularImpulseBody2; + const vec3 v2 = inverseMassBody2 * linearImpulseBody2; + const vec3 w2 = m_i2 * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; - q2 += Quaternion(0, w2) * q2 * float(0.5); + q2 += etk::Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); } } @@ -676,20 +676,20 @@ float SliderJoint::getTranslation() const { // TODO : Check if we need to compare rigid body position or center of mass here // Get the bodies positions and orientations - const Vector3& x1 = m_body1->getTransform().getPosition(); - const Vector3& x2 = m_body2->getTransform().getPosition(); - const Quaternion& q1 = m_body1->getTransform().getOrientation(); - const Quaternion& q2 = m_body2->getTransform().getOrientation(); + const vec3& x1 = m_body1->getTransform().getPosition(); + const vec3& x2 = m_body2->getTransform().getPosition(); + const etk::Quaternion& q1 = m_body1->getTransform().getOrientation(); + const etk::Quaternion& q2 = m_body2->getTransform().getOrientation(); // Compute the two anchor points in world-space coordinates - const Vector3 anchorBody1 = x1 + q1 * m_localAnchorPointBody1; - const Vector3 anchorBody2 = x2 + q2 * m_localAnchorPointBody2; + const vec3 anchorBody1 = x1 + q1 * m_localAnchorPointBody1; + const vec3 anchorBody2 = x2 + q2 * m_localAnchorPointBody2; // Compute the vector u (difference between anchor points) - const Vector3 u = anchorBody2 - anchorBody1; + const vec3 u = anchorBody2 - anchorBody1; // Compute the slider axis in world-space - Vector3 sliderAxisWorld = q1 * mSliderAxisBody1; + vec3 sliderAxisWorld = q1 * mSliderAxisBody1; sliderAxisWorld.normalize(); // Compute and return the translation value diff --git a/ephysics/constraint/SliderJoint.h b/ephysics/constraint/SliderJoint.h index d9342e6..d8b9a6f 100644 --- a/ephysics/constraint/SliderJoint.h +++ b/ephysics/constraint/SliderJoint.h @@ -23,10 +23,10 @@ struct SliderJointInfo : public JointInfo { // -------------------- Attributes -------------------- // /// Anchor point (in world-space coordinates) - Vector3 m_anchorPointWorldSpace; + vec3 m_anchorPointWorldSpace; /// Slider axis (in world-space coordinates) - Vector3 sliderAxisWorldSpace; + vec3 sliderAxisWorldSpace; /// True if the slider limits are enabled bool isLimitEnabled; @@ -54,8 +54,8 @@ struct SliderJointInfo : public JointInfo { * @param initSliderAxisWorldSpace The initial slider axis in world-space */ SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const Vector3& initAnchorPointWorldSpace, - const Vector3& initSliderAxisWorldSpace) + const vec3& initAnchorPointWorldSpace, + const vec3& initSliderAxisWorldSpace) : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), m_anchorPointWorldSpace(initAnchorPointWorldSpace), sliderAxisWorldSpace(initSliderAxisWorldSpace), @@ -72,8 +72,8 @@ struct SliderJointInfo : public JointInfo { * @param initMaxTranslationLimit The initial maximum translation limit (in meters) */ SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const Vector3& initAnchorPointWorldSpace, - const Vector3& initSliderAxisWorldSpace, + const vec3& initAnchorPointWorldSpace, + const vec3& initSliderAxisWorldSpace, float initMinTranslationLimit, float initMaxTranslationLimit) : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), m_anchorPointWorldSpace(initAnchorPointWorldSpace), @@ -95,8 +95,8 @@ struct SliderJointInfo : public JointInfo { * @param initMaxMotorForce The initial maximum motor force of the joint (in Newtons x meters) */ SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const Vector3& initAnchorPointWorldSpace, - const Vector3& initSliderAxisWorldSpace, + const vec3& initAnchorPointWorldSpace, + const vec3& initSliderAxisWorldSpace, float initMinTranslationLimit, float initMaxTranslationLimit, float initMotorSpeed, float initMaxMotorForce) : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), @@ -126,58 +126,58 @@ class SliderJoint : public Joint { // -------------------- Attributes -------------------- // /// Anchor point of body 1 (in local-space coordinates of body 1) - Vector3 m_localAnchorPointBody1; + vec3 m_localAnchorPointBody1; /// Anchor point of body 2 (in local-space coordinates of body 2) - Vector3 m_localAnchorPointBody2; + vec3 m_localAnchorPointBody2; /// Slider axis (in local-space coordinates of body 1) - Vector3 mSliderAxisBody1; + vec3 mSliderAxisBody1; /// Inertia tensor of body 1 (in world-space coordinates) - Matrix3x3 m_i1; + etk::Matrix3x3 m_i1; /// Inertia tensor of body 2 (in world-space coordinates) - Matrix3x3 m_i2; + etk::Matrix3x3 m_i2; /// Inverse of the initial orientation difference between the two bodies - Quaternion m_initOrientationDifferenceInv; + etk::Quaternion m_initOrientationDifferenceInv; /// First vector orthogonal to the slider axis local-space of body 1 - Vector3 mN1; + vec3 mN1; /// Second vector orthogonal to the slider axis and mN1 in local-space of body 1 - Vector3 mN2; + vec3 mN2; /// Vector r1 in world-space coordinates - Vector3 mR1; + vec3 mR1; /// Vector r2 in world-space coordinates - Vector3 mR2; + vec3 mR2; /// Cross product of r2 and n1 - Vector3 mR2CrossN1; + vec3 mR2CrossN1; /// Cross product of r2 and n2 - Vector3 mR2CrossN2; + vec3 mR2CrossN2; /// Cross product of r2 and the slider axis - Vector3 mR2CrossSliderAxis; + vec3 mR2CrossSliderAxis; /// Cross product of vector (r1 + u) and n1 - Vector3 mR1PlusUCrossN1; + vec3 mR1PlusUCrossN1; /// Cross product of vector (r1 + u) and n2 - Vector3 mR1PlusUCrossN2; + vec3 mR1PlusUCrossN2; /// Cross product of vector (r1 + u) and the slider axis - Vector3 mR1PlusUCrossSliderAxis; + vec3 mR1PlusUCrossSliderAxis; /// Bias of the 2 translation constraints - Vector2 mBTranslation; + vec2 mBTranslation; /// Bias of the 3 rotation constraints - Vector3 mBRotation; + vec3 mBRotation; /// Bias of the lower limit constraint float mBLowerLimit; @@ -186,10 +186,10 @@ class SliderJoint : public Joint { float mBUpperLimit; /// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix) - Matrix2x2 m_inverseMassMatrixTranslationConstraint; + etk::Matrix2x2 m_inverseMassMatrixTranslationConstraint; /// Inverse of mass matrix K=JM^-1J^t for the rotation constraint (3x3 matrix) - Matrix3x3 m_inverseMassMatrixRotationConstraint; + etk::Matrix3x3 m_inverseMassMatrixRotationConstraint; /// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix) float m_inverseMassMatrixLimit; @@ -198,10 +198,10 @@ class SliderJoint : public Joint { float m_inverseMassMatrixMotor; /// Accumulated impulse for the 2 translation constraints - Vector2 m_impulseTranslation; + vec2 m_impulseTranslation; /// Accumulated impulse for the 3 rotation constraints - Vector3 m_impulseRotation; + vec3 m_impulseRotation; /// Accumulated impulse for the lower limit constraint float m_impulseLowerLimit; @@ -219,7 +219,7 @@ class SliderJoint : public Joint { bool mIsMotorEnabled; /// Slider axis in world-space coordinates - Vector3 mSliderAxisWorld; + vec3 mSliderAxisWorld; /// Lower limit (minimum translation distance) float mLowerLimit; diff --git a/ephysics/engine/CollisionWorld.cpp b/ephysics/engine/CollisionWorld.cpp index 0669896..04924b8 100644 --- a/ephysics/engine/CollisionWorld.cpp +++ b/ephysics/engine/CollisionWorld.cpp @@ -35,10 +35,10 @@ CollisionWorld::~CollisionWorld() { // Create a collision body and add it to the world /** - * @param transform Transformation mapping the local-space of the body to world-space + * @param transform etk::Transform3Dation mapping the local-space of the body to world-space * @return A pointer to the body that has been created in the world */ -CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) { +CollisionBody* CollisionWorld::createCollisionBody(const etk::Transform3D& transform) { // Get the next available body ID bodyindex bodyID = computeNextAvailableBodyID(); diff --git a/ephysics/engine/CollisionWorld.h b/ephysics/engine/CollisionWorld.h index a50d187..fd50d67 100644 --- a/ephysics/engine/CollisionWorld.h +++ b/ephysics/engine/CollisionWorld.h @@ -88,7 +88,7 @@ class CollisionWorld { std::set::iterator getBodiesEndIterator(); /// Create a collision body - CollisionBody* createCollisionBody(const Transform& transform); + CollisionBody* createCollisionBody(const etk::Transform3D& transform); /// Destroy a collision body void destroyCollisionBody(CollisionBody* collisionBody); diff --git a/ephysics/engine/ConstraintSolver.h b/ephysics/engine/ConstraintSolver.h index 03338d3..e3e722f 100644 --- a/ephysics/engine/ConstraintSolver.h +++ b/ephysics/engine/ConstraintSolver.h @@ -28,16 +28,16 @@ struct ConstraintSolverData { float timeStep; /// Array with the bodies linear velocities - Vector3* linearVelocities; + vec3* linearVelocities; /// Array with the bodies angular velocities - Vector3* angularVelocities; + vec3* angularVelocities; /// Reference to the bodies positions - Vector3* positions; + vec3* positions; /// Reference to the bodies orientations - Quaternion* orientations; + etk::Quaternion* orientations; /// Reference to the map that associates rigid body to their index /// in the constrained velocities array @@ -170,17 +170,17 @@ class ConstraintSolver { void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive); /// Set the constrained velocities arrays - void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, - Vector3* constrainedAngularVelocities); + void setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities, + vec3* constrainedAngularVelocities); /// Set the constrained positions/orientations arrays - void setConstrainedPositionsArrays(Vector3* constrainedPositions, - Quaternion* constrainedOrientations); + void setConstrainedPositionsArrays(vec3* constrainedPositions, + etk::Quaternion* constrainedOrientations); }; // Set the constrained velocities arrays -inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, - Vector3* constrainedAngularVelocities) { +inline void ConstraintSolver::setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities, + vec3* constrainedAngularVelocities) { assert(constrainedLinearVelocities != NULL); assert(constrainedAngularVelocities != NULL); m_constraintSolverData.linearVelocities = constrainedLinearVelocities; @@ -188,8 +188,8 @@ inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constraine } // Set the constrained positions/orientations arrays -inline void ConstraintSolver::setConstrainedPositionsArrays(Vector3* constrainedPositions, - Quaternion* constrainedOrientations) { +inline void ConstraintSolver::setConstrainedPositionsArrays(vec3* constrainedPositions, + etk::Quaternion* constrainedOrientations) { assert(constrainedPositions != NULL); assert(constrainedOrientations != NULL); m_constraintSolverData.positions = constrainedPositions; diff --git a/ephysics/engine/ContactSolver.cpp b/ephysics/engine/ContactSolver.cpp index c396012..3485998 100644 --- a/ephysics/engine/ContactSolver.cpp +++ b/ephysics/engine/ContactSolver.cpp @@ -70,8 +70,8 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { assert(body2 != NULL); // Get the position of the two bodies - const Vector3& x1 = body1->m_centerOfMassWorld; - const Vector3& x2 = body2->m_centerOfMassWorld; + const vec3& x1 = body1->m_centerOfMassWorld; + const vec3& x2 = body2->m_centerOfMassWorld; // Initialize the int32_ternal contact manifold structure using the external // contact manifold @@ -91,8 +91,8 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { // If we solve the friction constraints at the center of the contact manifold if (mIsSolveFrictionAtContactManifoldCenterActive) { - int32_ternalManifold.frictionPointBody1 = Vector3::zero(); - int32_ternalManifold.frictionPointBody2 = Vector3::zero(); + int32_ternalManifold.frictionPointBody1 = vec3::zero(); + int32_ternalManifold.frictionPointBody2 = vec3::zero(); } // For each contact point of the contact manifold @@ -104,8 +104,8 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { ContactPoint* externalContact = externalManifold->getContactPoint(c); // Get the contact point on the two bodies - Vector3 p1 = externalContact->getWorldPointOnBody1(); - Vector3 p2 = externalContact->getWorldPointOnBody2(); + vec3 p1 = externalContact->getWorldPointOnBody1(); + vec3 p2 = externalContact->getWorldPointOnBody2(); contactPoint.externalContact = externalContact; contactPoint.normal = externalContact->getNormal(); @@ -115,11 +115,11 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { contactPoint.isRestingContact = externalContact->getIsRestingContact(); externalContact->setIsRestingContact(true); contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1(); - contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2(); + contactPoint.oldFrictionvec2 = externalContact->getFrictionvec2(); contactPoint.penetrationImpulse = 0.0; contactPoint.friction1Impulse = 0.0; contactPoint.friction2Impulse = 0.0; - contactPoint.rollingResistanceImpulse = Vector3::zero(); + contactPoint.rollingResistanceImpulse = vec3::zero(); // If we solve the friction constraints at the center of the contact manifold if (mIsSolveFrictionAtContactManifoldCenterActive) { @@ -136,7 +136,7 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { int32_ternalManifold.r1Friction = int32_ternalManifold.frictionPointBody1 - x1; int32_ternalManifold.r2Friction = int32_ternalManifold.frictionPointBody2 - x2; int32_ternalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1(); - int32_ternalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2(); + int32_ternalManifold.oldFrictionvec2 = externalManifold->getFrictionvec2(); // If warm starting is active if (mIsWarmStartingActive) { @@ -152,7 +152,7 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { int32_ternalManifold.friction1Impulse = 0.0; int32_ternalManifold.friction2Impulse = 0.0; int32_ternalManifold.frictionTwistImpulse = 0.0; - int32_ternalManifold.rollingResistanceImpulse = Vector3(0, 0, 0); + int32_ternalManifold.rollingResistanceImpulse = vec3(0, 0, 0); } } } @@ -170,19 +170,19 @@ void ContactSolver::initializeContactConstraints() { ContactManifoldSolver& manifold = mContactConstraints[c]; // Get the inertia tensors of both bodies - Matrix3x3& I1 = manifold.inverseInertiaTensorBody1; - Matrix3x3& I2 = manifold.inverseInertiaTensorBody2; + etk::Matrix3x3& I1 = manifold.inverseInertiaTensorBody1; + etk::Matrix3x3& I2 = manifold.inverseInertiaTensorBody2; // If we solve the friction constraints at the center of the contact manifold if (mIsSolveFrictionAtContactManifoldCenterActive) { - manifold.normal = Vector3(0.0, 0.0, 0.0); + manifold.normal = vec3(0.0, 0.0, 0.0); } // Get the velocities of the bodies - const Vector3& v1 = mLinearVelocities[manifold.indexBody1]; - const Vector3& w1 = mAngularVelocities[manifold.indexBody1]; - const Vector3& v2 = mLinearVelocities[manifold.indexBody2]; - const Vector3& w2 = mAngularVelocities[manifold.indexBody2]; + const vec3& v1 = mLinearVelocities[manifold.indexBody1]; + const vec3& w1 = mAngularVelocities[manifold.indexBody1]; + const vec3& v2 = mLinearVelocities[manifold.indexBody2]; + const vec3& w2 = mAngularVelocities[manifold.indexBody2]; // For each contact point constraint for (uint32_t i=0; i 0.0 ? contactPoint.inversePenetrationMass = float(1.0) / + massPenetration > 0.0 ? contactPoint.inversePenetrationMass = 1.0f / massPenetration : - float(0.0); + 0.0f; // If we do not solve the friction constraints at the center of the contact manifold if (!mIsSolveFrictionAtContactManifoldCenterActive) { @@ -211,9 +211,9 @@ void ContactSolver::initializeContactConstraints() { computeFrictionVectors(deltaV, contactPoint); contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1); - contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionVector2); + contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionvec2); contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1); - contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionVector2); + contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionvec2); // Compute the inverse mass matrix K for the friction // constraints at each contact point @@ -224,15 +224,15 @@ void ContactSolver::initializeContactConstraints() { contactPoint.frictionVector1); float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot( - contactPoint.frictionVector2) + + contactPoint.frictionvec2) + ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot( - contactPoint.frictionVector2); - friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = float(1.0) / + contactPoint.frictionvec2); + friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = 1.0f / friction1Mass : - float(0.0); - friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = float(1.0) / + 0.0f; + friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = 1.0f / friction2Mass : - float(0.0); + 0.0f; } // Compute the restitution velocity bias "b". We compute this here instead @@ -265,7 +265,7 @@ void ContactSolver::initializeContactConstraints() { } // Compute the inverse K matrix for the rolling resistance constraint - manifold.inverseRollingResistance.setToZero(); + manifold.inverseRollingResistance.setZero(); if (manifold.rollingResistanceFactor > 0 && (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) { manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1 + manifold.inverseInertiaTensorBody2; manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse(); @@ -276,7 +276,7 @@ void ContactSolver::initializeContactConstraints() { manifold.normal.normalize(); - Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) - + vec3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) - v1 - w1.cross(manifold.r1Friction); // Compute the friction vectors @@ -285,9 +285,9 @@ void ContactSolver::initializeContactConstraints() { // Compute the inverse mass matrix K for the friction constraints at the center of // the contact manifold manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1); - manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2); + manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionvec2); manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1); - manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2); + manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionvec2); float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot( manifold.frictionVector1) + @@ -295,20 +295,20 @@ void ContactSolver::initializeContactConstraints() { manifold.frictionVector1); float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot( - manifold.frictionVector2) + + manifold.frictionvec2) + ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot( - manifold.frictionVector2); + manifold.frictionvec2); float frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 * manifold.normal) + manifold.normal.dot(manifold.inverseInertiaTensorBody2 * manifold.normal); - friction1Mass > 0.0 ? manifold.inverseFriction1Mass = float(1.0)/friction1Mass - : float(0.0); - friction2Mass > 0.0 ? manifold.inverseFriction2Mass = float(1.0)/friction2Mass - : float(0.0); - frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = float(1.0) / + friction1Mass > 0.0 ? manifold.inverseFriction1Mass = 1.0f/friction1Mass + : 0.0f; + friction2Mass > 0.0 ? manifold.inverseFriction2Mass = 1.0f/friction2Mass + : 0.0f; + frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = 1.0f / frictionTwistMass : - float(0.0); + 0.0f; } } } @@ -352,14 +352,14 @@ void ContactSolver::warmStart() { // Project the old friction impulses (with old friction vectors) int32_to // the new friction vectors to get the new friction impulses - Vector3 oldFrictionImpulse = contactPoint.friction1Impulse * + vec3 oldFrictionImpulse = contactPoint.friction1Impulse * contactPoint.oldFrictionVector1 + contactPoint.friction2Impulse * - contactPoint.oldFrictionVector2; + contactPoint.oldFrictionvec2; contactPoint.friction1Impulse = oldFrictionImpulse.dot( contactPoint.frictionVector1); contactPoint.friction2Impulse = oldFrictionImpulse.dot( - contactPoint.frictionVector2); + contactPoint.frictionvec2); // --------- Friction 1 --------- // @@ -384,8 +384,8 @@ void ContactSolver::warmStart() { if (contactManifold.rollingResistanceFactor > 0) { // Compute the impulse P = J^T * lambda - const Impulse impulseRollingResistance(Vector3::zero(), -contactPoint.rollingResistanceImpulse, - Vector3::zero(), contactPoint.rollingResistanceImpulse); + const Impulse impulseRollingResistance(vec3::zero(), -contactPoint.rollingResistanceImpulse, + vec3::zero(), contactPoint.rollingResistanceImpulse); // Apply the impulses to the bodies of the constraint applyImpulse(impulseRollingResistance, contactManifold); @@ -398,7 +398,7 @@ void ContactSolver::warmStart() { contactPoint.penetrationImpulse = 0.0; contactPoint.friction1Impulse = 0.0; contactPoint.friction2Impulse = 0.0; - contactPoint.rollingResistanceImpulse = Vector3::zero(); + contactPoint.rollingResistanceImpulse = vec3::zero(); } } @@ -408,25 +408,25 @@ void ContactSolver::warmStart() { // Project the old friction impulses (with old friction vectors) int32_to the new friction // vectors to get the new friction impulses - Vector3 oldFrictionImpulse = contactManifold.friction1Impulse * + vec3 oldFrictionImpulse = contactManifold.friction1Impulse * contactManifold.oldFrictionVector1 + contactManifold.friction2Impulse * - contactManifold.oldFrictionVector2; + contactManifold.oldFrictionvec2; contactManifold.friction1Impulse = oldFrictionImpulse.dot( contactManifold.frictionVector1); contactManifold.friction2Impulse = oldFrictionImpulse.dot( - contactManifold.frictionVector2); + contactManifold.frictionvec2); // ------ First friction constraint at the center of the contact manifold ------ // // Compute the impulse P = J^T * lambda - Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * + vec3 linearImpulseBody1 = -contactManifold.frictionVector1 * contactManifold.friction1Impulse; - Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * + vec3 angularImpulseBody1 = -contactManifold.r1CrossT1 * contactManifold.friction1Impulse; - Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * + vec3 linearImpulseBody2 = contactManifold.frictionVector1 * contactManifold.friction1Impulse; - Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * + vec3 angularImpulseBody2 = contactManifold.r2CrossT1 * contactManifold.friction1Impulse; const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); @@ -437,11 +437,11 @@ void ContactSolver::warmStart() { // ------ Second friction constraint at the center of the contact manifold ----- // // Compute the impulse P = J^T * lambda - linearImpulseBody1 = -contactManifold.frictionVector2 * + linearImpulseBody1 = -contactManifold.frictionvec2 * contactManifold.friction2Impulse; angularImpulseBody1 = -contactManifold.r1CrossT2 * contactManifold.friction2Impulse; - linearImpulseBody2 = contactManifold.frictionVector2 * + linearImpulseBody2 = contactManifold.frictionvec2 * contactManifold.friction2Impulse; angularImpulseBody2 = contactManifold.r2CrossT2 * contactManifold.friction2Impulse; @@ -454,9 +454,9 @@ void ContactSolver::warmStart() { // ------ Twist friction constraint at the center of the contact manifold ------ // // Compute the impulse P = J^T * lambda - linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); + linearImpulseBody1 = vec3(0.0, 0.0, 0.0); angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse; - linearImpulseBody2 = Vector3(0.0, 0.0, 0.0); + linearImpulseBody2 = vec3(0.0, 0.0, 0.0); angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse; const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); @@ -469,8 +469,8 @@ void ContactSolver::warmStart() { // Compute the impulse P = J^T * lambda angularImpulseBody1 = -contactManifold.rollingResistanceImpulse; angularImpulseBody2 = contactManifold.rollingResistanceImpulse; - const Impulse impulseRollingResistance(Vector3::zero(), angularImpulseBody1, - Vector3::zero(), angularImpulseBody2); + const Impulse impulseRollingResistance(vec3::zero(), angularImpulseBody1, + vec3::zero(), angularImpulseBody2); // Apply the impulses to the bodies of the constraint applyImpulse(impulseRollingResistance, contactManifold); @@ -481,7 +481,7 @@ void ContactSolver::warmStart() { contactManifold.friction1Impulse = 0.0; contactManifold.friction2Impulse = 0.0; contactManifold.frictionTwistImpulse = 0.0; - contactManifold.rollingResistanceImpulse = Vector3::zero(); + contactManifold.rollingResistanceImpulse = vec3::zero(); } } } @@ -502,10 +502,10 @@ void ContactSolver::solve() { float sumPenetrationImpulse = 0.0; // Get the constrained velocities - const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1]; - const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1]; - const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2]; - const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2]; + const vec3& v1 = mLinearVelocities[contactManifold.indexBody1]; + const vec3& w1 = mAngularVelocities[contactManifold.indexBody1]; + const vec3& v2 = mLinearVelocities[contactManifold.indexBody2]; + const vec3& w2 = mAngularVelocities[contactManifold.indexBody2]; for (uint32_t i=0; i 0) { // Compute J*v - const Vector3 JvRolling = w2 - w1; + const vec3 JvRolling = w2 - w1; // Compute the Lagrange multiplier lambda - Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); + vec3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); float rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse; - Vector3 lambdaTempRolling = contactPoint.rollingResistanceImpulse; + vec3 lambdaTempRolling = contactPoint.rollingResistanceImpulse; contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse + deltaLambdaRolling, rollingLimit); deltaLambdaRolling = contactPoint.rollingResistanceImpulse - lambdaTempRolling; // Compute the impulse P=J^T * lambda - const Impulse impulseRolling(Vector3::zero(), -deltaLambdaRolling, - Vector3::zero(), deltaLambdaRolling); + const Impulse impulseRolling(vec3::zero(), -deltaLambdaRolling, + vec3::zero(), deltaLambdaRolling); // Apply the impulses to the bodies of the constraint applyImpulse(impulseRolling, contactManifold); @@ -655,7 +655,7 @@ void ContactSolver::solve() { // ------ First friction constraint at the center of the contact manifol ------ // // Compute J*v - Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction) + vec3 deltaV = v2 + w2.cross(contactManifold.r2Friction) - v1 - w1.cross(contactManifold.r1Friction); float Jv = deltaV.dot(contactManifold.frictionVector1); @@ -669,10 +669,10 @@ void ContactSolver::solve() { deltaLambda = contactManifold.friction1Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda - Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda; - Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda; - Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda; - Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda; + vec3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda; + vec3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda; + vec3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda; + vec3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda; const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); @@ -684,7 +684,7 @@ void ContactSolver::solve() { // Compute J*v deltaV = v2 + w2.cross(contactManifold.r2Friction) - v1 - w1.cross(contactManifold.r1Friction); - Jv = deltaV.dot(contactManifold.frictionVector2); + Jv = deltaV.dot(contactManifold.frictionvec2); // Compute the Lagrange multiplier lambda deltaLambda = -Jv * contactManifold.inverseFriction2Mass; @@ -696,9 +696,9 @@ void ContactSolver::solve() { deltaLambda = contactManifold.friction2Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda - linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda; + linearImpulseBody1 = -contactManifold.frictionvec2 * deltaLambda; angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda; - linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda; + linearImpulseBody2 = contactManifold.frictionvec2 * deltaLambda; angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda; const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); @@ -721,9 +721,9 @@ void ContactSolver::solve() { deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp; // Compute the impulse P=J^T * lambda - linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); + linearImpulseBody1 = vec3(0.0, 0.0, 0.0); angularImpulseBody1 = -contactManifold.normal * deltaLambda; - linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);; + linearImpulseBody2 = vec3(0.0, 0.0, 0.0);; angularImpulseBody2 = contactManifold.normal * deltaLambda; const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); @@ -736,12 +736,12 @@ void ContactSolver::solve() { if (contactManifold.rollingResistanceFactor > 0) { // Compute J*v - const Vector3 JvRolling = w2 - w1; + const vec3 JvRolling = w2 - w1; // Compute the Lagrange multiplier lambda - Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); + vec3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); float rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse; - Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse; + vec3 lambdaTempRolling = contactManifold.rollingResistanceImpulse; contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse + deltaLambdaRolling, rollingLimit); deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling; @@ -749,8 +749,8 @@ void ContactSolver::solve() { // Compute the impulse P=J^T * lambda angularImpulseBody1 = -deltaLambdaRolling; angularImpulseBody2 = deltaLambdaRolling; - const Impulse impulseRolling(Vector3::zero(), angularImpulseBody1, - Vector3::zero(), angularImpulseBody2); + const Impulse impulseRolling(vec3::zero(), angularImpulseBody1, + vec3::zero(), angularImpulseBody2); // Apply the impulses to the bodies of the constraint applyImpulse(impulseRolling, contactManifold); @@ -778,7 +778,7 @@ void ContactSolver::storeImpulses() { contactPoint.externalContact->setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse); contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1); - contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2); + contactPoint.externalContact->setFrictionvec2(contactPoint.frictionvec2); } manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse); @@ -786,7 +786,7 @@ void ContactSolver::storeImpulses() { manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse); manifold.externalContactManifold->setRollingResistanceImpulse(manifold.rollingResistanceImpulse); manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1); - manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2); + manifold.externalContactManifold->setFrictionvec2(manifold.frictionvec2); } } @@ -826,14 +826,14 @@ void ContactSolver::applySplitImpulse(const Impulse& impulse, // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane // for a contact point. The two vectors have to be such that : t1 x t2 = contactNormal. -void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, +void ContactSolver::computeFrictionVectors(const vec3& deltaVelocity, ContactPointSolver& contactPoint) const { assert(contactPoint.normal.length() > 0.0); // Compute the velocity difference vector in the tangential plane - Vector3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal; - Vector3 tangentVelocity = deltaVelocity - normalVelocity; + vec3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal; + vec3 tangentVelocity = deltaVelocity - normalVelocity; // If the velocty difference in the tangential plane is not zero float lengthTangenVelocity = tangentVelocity.length(); @@ -851,19 +851,19 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, // The second friction vector is computed by the cross product of the firs // friction vector and the contact normal - contactPoint.frictionVector2 =contactPoint.normal.cross(contactPoint.frictionVector1).getUnit(); + contactPoint.frictionvec2 =contactPoint.normal.cross(contactPoint.frictionVector1).safeNormalized(); } // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane // for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal. -void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, +void ContactSolver::computeFrictionVectors(const vec3& deltaVelocity, ContactManifoldSolver& contact) const { assert(contact.normal.length() > 0.0); // Compute the velocity difference vector in the tangential plane - Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal; - Vector3 tangentVelocity = deltaVelocity - normalVelocity; + vec3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal; + vec3 tangentVelocity = deltaVelocity - normalVelocity; // If the velocty difference in the tangential plane is not zero float lengthTangenVelocity = tangentVelocity.length(); @@ -881,7 +881,7 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, // The second friction vector is computed by the cross product of the firs // friction vector and the contact normal - contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit(); + contact.frictionvec2 = contact.normal.cross(contact.frictionVector1).safeNormalized(); } // Clean up the constraint solver diff --git a/ephysics/engine/ContactSolver.h b/ephysics/engine/ContactSolver.h index 4079fd5..f68f292 100644 --- a/ephysics/engine/ContactSolver.h +++ b/ephysics/engine/ContactSolver.h @@ -112,46 +112,46 @@ class ContactSolver { float penetrationSplitImpulse; /// Accumulated rolling resistance impulse - Vector3 rollingResistanceImpulse; + vec3 rollingResistanceImpulse; /// Normal vector of the contact - Vector3 normal; + vec3 normal; /// First friction vector in the tangent plane - Vector3 frictionVector1; + vec3 frictionVector1; /// Second friction vector in the tangent plane - Vector3 frictionVector2; + vec3 frictionvec2; /// Old first friction vector in the tangent plane - Vector3 oldFrictionVector1; + vec3 oldFrictionVector1; /// Old second friction vector in the tangent plane - Vector3 oldFrictionVector2; + vec3 oldFrictionvec2; /// Vector from the body 1 center to the contact point - Vector3 r1; + vec3 r1; /// Vector from the body 2 center to the contact point - Vector3 r2; + vec3 r2; /// Cross product of r1 with 1st friction vector - Vector3 r1CrossT1; + vec3 r1CrossT1; /// Cross product of r1 with 2nd friction vector - Vector3 r1CrossT2; + vec3 r1CrossT2; /// Cross product of r2 with 1st friction vector - Vector3 r2CrossT1; + vec3 r2CrossT1; /// Cross product of r2 with 2nd friction vector - Vector3 r2CrossT2; + vec3 r2CrossT2; /// Cross product of r1 with the contact normal - Vector3 r1CrossN; + vec3 r1CrossN; /// Cross product of r2 with the contact normal - Vector3 r2CrossN; + vec3 r2CrossN; /// Penetration depth float penetrationDepth; @@ -195,10 +195,10 @@ class ContactSolver { float massInverseBody2; /// Inverse inertia tensor of body 1 - Matrix3x3 inverseInertiaTensorBody1; + etk::Matrix3x3 inverseInertiaTensorBody1; /// Inverse inertia tensor of body 2 - Matrix3x3 inverseInertiaTensorBody2; + etk::Matrix3x3 inverseInertiaTensorBody2; /// Contact point constraints ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD]; @@ -227,31 +227,31 @@ class ContactSolver { // - Variables used when friction constraints are apply at the center of the manifold-// /// Average normal vector of the contact manifold - Vector3 normal; + vec3 normal; /// Point on body 1 where to apply the friction constraints - Vector3 frictionPointBody1; + vec3 frictionPointBody1; /// Point on body 2 where to apply the friction constraints - Vector3 frictionPointBody2; + vec3 frictionPointBody2; /// R1 vector for the friction constraints - Vector3 r1Friction; + vec3 r1Friction; /// R2 vector for the friction constraints - Vector3 r2Friction; + vec3 r2Friction; /// Cross product of r1 with 1st friction vector - Vector3 r1CrossT1; + vec3 r1CrossT1; /// Cross product of r1 with 2nd friction vector - Vector3 r1CrossT2; + vec3 r1CrossT2; /// Cross product of r2 with 1st friction vector - Vector3 r2CrossT1; + vec3 r2CrossT1; /// Cross product of r2 with 2nd friction vector - Vector3 r2CrossT2; + vec3 r2CrossT2; /// Matrix K for the first friction constraint float inverseFriction1Mass; @@ -263,19 +263,19 @@ class ContactSolver { float inverseTwistFrictionMass; /// Matrix K for the rolling resistance constraint - Matrix3x3 inverseRollingResistance; + etk::Matrix3x3 inverseRollingResistance; /// First friction direction at contact manifold center - Vector3 frictionVector1; + vec3 frictionVector1; /// Second friction direction at contact manifold center - Vector3 frictionVector2; + vec3 frictionvec2; /// Old 1st friction direction at contact manifold center - Vector3 oldFrictionVector1; + vec3 oldFrictionVector1; /// Old 2nd friction direction at contact manifold center - Vector3 oldFrictionVector2; + vec3 oldFrictionvec2; /// First friction direction impulse at manifold center float friction1Impulse; @@ -287,7 +287,7 @@ class ContactSolver { float frictionTwistImpulse; /// Rolling resistance impulse - Vector3 rollingResistanceImpulse; + vec3 rollingResistanceImpulse; }; // -------------------- Constants --------------------- // @@ -304,10 +304,10 @@ class ContactSolver { // -------------------- Attributes -------------------- // /// Split linear velocities for the position contact solver (split impulse) - Vector3* m_splitLinearVelocities; + vec3* m_splitLinearVelocities; /// Split angular velocities for the position contact solver (split impulse) - Vector3* m_splitAngularVelocities; + vec3* m_splitAngularVelocities; /// Current time step float m_timeStep; @@ -319,10 +319,10 @@ class ContactSolver { uint32_t m_numberContactManifolds; /// Array of linear velocities - Vector3* mLinearVelocities; + vec3* mLinearVelocities; /// Array of angular velocities - Vector3* mAngularVelocities; + vec3* mAngularVelocities; /// Reference to the map of rigid body to their index in the constrained velocities array const std::map& m_mapBodyToConstrainedVelocityIndex; @@ -363,13 +363,13 @@ class ContactSolver { /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction /// plane for a contact point. The two vectors have to be /// such that : t1 x t2 = contactNormal. - void computeFrictionVectors(const Vector3& deltaVelocity, + void computeFrictionVectors(const vec3& deltaVelocity, ContactPointSolver &contactPoint) const; /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction /// plane for a contact manifold. The two vectors have to be /// such that : t1 x t2 = contactNormal. - void computeFrictionVectors(const Vector3& deltaVelocity, + void computeFrictionVectors(const vec3& deltaVelocity, ContactManifoldSolver& contactPoint) const; /// Compute a penetration constraint impulse @@ -398,12 +398,12 @@ class ContactSolver { void initializeForIsland(float dt, Island* island); /// Set the split velocities arrays - void setSplitVelocitiesArrays(Vector3* splitLinearVelocities, - Vector3* splitAngularVelocities); + void setSplitVelocitiesArrays(vec3* splitLinearVelocities, + vec3* splitAngularVelocities); /// Set the constrained velocities arrays - void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, - Vector3* constrainedAngularVelocities); + void setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities, + vec3* constrainedAngularVelocities); /// Warm start the solver. void warmStart(); @@ -430,8 +430,8 @@ class ContactSolver { }; // Set the split velocities arrays -inline void ContactSolver::setSplitVelocitiesArrays(Vector3* splitLinearVelocities, - Vector3* splitAngularVelocities) { +inline void ContactSolver::setSplitVelocitiesArrays(vec3* splitLinearVelocities, + vec3* splitAngularVelocities) { assert(splitLinearVelocities != NULL); assert(splitAngularVelocities != NULL); m_splitLinearVelocities = splitLinearVelocities; @@ -439,8 +439,8 @@ inline void ContactSolver::setSplitVelocitiesArrays(Vector3* splitLinearVelociti } // Set the constrained velocities arrays -inline void ContactSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, - Vector3* constrainedAngularVelocities) { +inline void ContactSolver::setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities, + vec3* constrainedAngularVelocities) { assert(constrainedLinearVelocities != NULL); assert(constrainedAngularVelocities != NULL); mLinearVelocities = constrainedLinearVelocities; @@ -509,9 +509,9 @@ inline const Impulse ContactSolver::computeFriction1Impulse(float deltaLambda, inline const Impulse ContactSolver::computeFriction2Impulse(float deltaLambda, const ContactPointSolver& contactPoint) const { - return Impulse(-contactPoint.frictionVector2 * deltaLambda, + return Impulse(-contactPoint.frictionvec2 * deltaLambda, -contactPoint.r1CrossT2 * deltaLambda, - contactPoint.frictionVector2 * deltaLambda, + contactPoint.frictionvec2 * deltaLambda, contactPoint.r2CrossT2 * deltaLambda); } diff --git a/ephysics/engine/DynamicsWorld.cpp b/ephysics/engine/DynamicsWorld.cpp index 7aca004..678821b 100644 --- a/ephysics/engine/DynamicsWorld.cpp +++ b/ephysics/engine/DynamicsWorld.cpp @@ -19,7 +19,7 @@ using namespace std; /** * @param gravity Gravity vector in the world (in meters per second squared) */ -DynamicsWorld::DynamicsWorld(const Vector3 &gravity) +DynamicsWorld::DynamicsWorld(const vec3 &gravity) : CollisionWorld(), m_contactSolver(m_mapBodyToConstrainedVelocityIndex), m_constraintSolver(m_mapBodyToConstrainedVelocityIndex), @@ -168,8 +168,8 @@ void DynamicsWorld::int32_tegrateRigidBodiesPositions() { // Get the constrained velocity uint32_t indexArray = m_mapBodyToConstrainedVelocityIndex.find(bodies[b])->second; - Vector3 newLinVelocity = m_constrainedLinearVelocities[indexArray]; - Vector3 newAngVelocity = m_constrainedAngularVelocities[indexArray]; + vec3 newLinVelocity = m_constrainedLinearVelocities[indexArray]; + vec3 newAngVelocity = m_constrainedAngularVelocities[indexArray]; // Add the split impulse velocity from Contact Solver (only used // to update the position) @@ -180,14 +180,14 @@ void DynamicsWorld::int32_tegrateRigidBodiesPositions() { } // Get current position and orientation of the body - const Vector3& currentPosition = bodies[b]->m_centerOfMassWorld; - const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation(); + const vec3& currentPosition = bodies[b]->m_centerOfMassWorld; + const etk::Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation(); // Update the new constrained position and orientation of the body m_constrainedPositions[indexArray] = currentPosition + newLinVelocity * m_timeStep; m_constrainedOrientations[indexArray] = currentOrientation + - Quaternion(0, newAngVelocity) * - currentOrientation * float(0.5) * m_timeStep; + etk::Quaternion(0, newAngVelocity) * + currentOrientation * 0.5f * m_timeStep; } } } @@ -215,7 +215,7 @@ void DynamicsWorld::updateBodiesState() { bodies[b]->m_centerOfMassWorld = m_constrainedPositions[index]; // Update the orientation of the body - bodies[b]->m_transform.setOrientation(m_constrainedOrientations[index].getUnit()); + bodies[b]->m_transform.setOrientation(m_constrainedOrientations[index].safeNormalized()); // Update the transform of the body (using the new center of mass and new orientation) bodies[b]->updateTransformWithCenterOfMass(); @@ -238,12 +238,12 @@ void DynamicsWorld::initVelocityArrays() { } m_numberBodiesCapacity = nbBodies; // TODO : Use better memory allocation here - m_splitLinearVelocities = new Vector3[m_numberBodiesCapacity]; - m_splitAngularVelocities = new Vector3[m_numberBodiesCapacity]; - m_constrainedLinearVelocities = new Vector3[m_numberBodiesCapacity]; - m_constrainedAngularVelocities = new Vector3[m_numberBodiesCapacity]; - m_constrainedPositions = new Vector3[m_numberBodiesCapacity]; - m_constrainedOrientations = new Quaternion[m_numberBodiesCapacity]; + m_splitLinearVelocities = new vec3[m_numberBodiesCapacity]; + m_splitAngularVelocities = new vec3[m_numberBodiesCapacity]; + m_constrainedLinearVelocities = new vec3[m_numberBodiesCapacity]; + m_constrainedAngularVelocities = new vec3[m_numberBodiesCapacity]; + m_constrainedPositions = new vec3[m_numberBodiesCapacity]; + m_constrainedOrientations = new etk::Quaternion[m_numberBodiesCapacity]; assert(m_splitLinearVelocities != NULL); assert(m_splitAngularVelocities != NULL); assert(m_constrainedLinearVelocities != NULL); @@ -254,8 +254,8 @@ void DynamicsWorld::initVelocityArrays() { // Reset the velocities arrays for (uint32_t i=0; isecond; - assert(m_splitLinearVelocities[indexBody] == Vector3(0, 0, 0)); - assert(m_splitAngularVelocities[indexBody] == Vector3(0, 0, 0)); + assert(m_splitLinearVelocities[indexBody] == vec3(0, 0, 0)); + assert(m_splitAngularVelocities[indexBody] == vec3(0, 0, 0)); // Integrate the external force to get the new velocity of the body m_constrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() + @@ -326,8 +326,8 @@ void DynamicsWorld::int32_tegrateRigidBodiesVelocities() { // => v2 = v1 * (1 - c * dt) float linDampingFactor = bodies[b]->getLinearDamping(); float angDampingFactor = bodies[b]->getAngularDamping(); - float linearDamping = pow(float(1.0) - linDampingFactor, m_timeStep); - float angularDamping = pow(float(1.0) - angDampingFactor, m_timeStep); + float linearDamping = pow(1.0f - linDampingFactor, m_timeStep); + float angularDamping = pow(1.0f - angDampingFactor, m_timeStep); m_constrainedLinearVelocities[indexBody] *= linearDamping; m_constrainedAngularVelocities[indexBody] *= angularDamping; @@ -422,10 +422,10 @@ void DynamicsWorld::solvePositionCorrection() { // Create a rigid body int32_to the physics world /** - * @param transform Transformation from body local-space to world-space + * @param transform etk::Transform3Dation from body local-space to world-space * @return A pointer to the body that has been created in the world */ -RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) { +RigidBody* DynamicsWorld::createRigidBody(const etk::Transform3D& transform) { // Compute the body ID bodyindex bodyID = computeNextAvailableBodyID(); @@ -794,13 +794,13 @@ void DynamicsWorld::updateSleepingBodies() { if (bodies[b]->getType() == STATIC) continue; // If the body is velocity is large enough to stay awake - if (bodies[b]->getLinearVelocity().lengthSquare() > sleepLinearVelocitySquare || - bodies[b]->getAngularVelocity().lengthSquare() > sleepAngularVelocitySquare || + if (bodies[b]->getLinearVelocity().length2() > sleepLinearVelocitySquare || + bodies[b]->getAngularVelocity().length2() > sleepAngularVelocitySquare || !bodies[b]->isAllowedToSleep()) { // Reset the sleep time of the body - bodies[b]->m_sleepTime = float(0.0); - minSleepTime = float(0.0); + bodies[b]->m_sleepTime = 0.0f; + minSleepTime = 0.0f; } else { // If the body velocity is bellow the sleeping velocity threshold diff --git a/ephysics/engine/DynamicsWorld.h b/ephysics/engine/DynamicsWorld.h index 084d438..7163c79 100644 --- a/ephysics/engine/DynamicsWorld.h +++ b/ephysics/engine/DynamicsWorld.h @@ -51,7 +51,7 @@ class DynamicsWorld : public CollisionWorld { std::set m_joints; /// Gravity vector of the world - Vector3 m_gravity; + vec3 m_gravity; /// Current frame time step (in seconds) float m_timeStep; @@ -61,23 +61,23 @@ class DynamicsWorld : public CollisionWorld { /// Array of constrained linear velocities (state of the linear velocities /// after solving the constraints) - Vector3* m_constrainedLinearVelocities; + vec3* m_constrainedLinearVelocities; /// Array of constrained angular velocities (state of the angular velocities /// after solving the constraints) - Vector3* m_constrainedAngularVelocities; + vec3* m_constrainedAngularVelocities; /// Split linear velocities for the position contact solver (split impulse) - Vector3* m_splitLinearVelocities; + vec3* m_splitLinearVelocities; /// Split angular velocities for the position contact solver (split impulse) - Vector3* m_splitAngularVelocities; + vec3* m_splitAngularVelocities; /// Array of constrained rigid bodies position (for position error correction) - Vector3* m_constrainedPositions; + vec3* m_constrainedPositions; /// Array of constrained rigid bodies orientation (for position error correction) - Quaternion* m_constrainedOrientations; + etk::Quaternion* m_constrainedOrientations; /// Map body to their index in the constrained velocities array std::map m_mapBodyToConstrainedVelocityIndex; @@ -122,8 +122,8 @@ class DynamicsWorld : public CollisionWorld { void resetBodiesForceAndTorque(); /// Update the position and orientation of a body - void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity, - Vector3 newAngVelocity); + void updatePositionAndOrientationOfBody(RigidBody* body, vec3 newLinVelocity, + vec3 newAngVelocity); /// Compute and set the int32_terpolation factor to all bodies void setInterpolationFactorToAllBodies(); @@ -160,7 +160,7 @@ class DynamicsWorld : public CollisionWorld { // -------------------- Methods -------------------- // /// Constructor - DynamicsWorld(const Vector3& m_gravity); + DynamicsWorld(const vec3& m_gravity); /// Destructor virtual ~DynamicsWorld(); @@ -191,7 +191,7 @@ class DynamicsWorld : public CollisionWorld { void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); /// Create a rigid body int32_to the physics world. - RigidBody* createRigidBody(const Transform& transform); + RigidBody* createRigidBody(const etk::Transform3D& transform); /// Destroy a rigid body and all the joints which it belongs void destroyRigidBody(RigidBody* rigidBody); @@ -203,10 +203,10 @@ class DynamicsWorld : public CollisionWorld { void destroyJoint(Joint* joint); /// Return the gravity vector of the world - Vector3 getGravity() const; + vec3 getGravity() const; /// Set the gravity vector of the world - void setGravity(Vector3& gravity); + void setGravity(vec3& gravity); /// Return if the gravity is on bool isGravityEnabled() const; @@ -290,8 +290,8 @@ inline void DynamicsWorld::resetBodiesForceAndTorque() { // For each body of the world std::set::iterator it; for (it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) { - (*it)->m_externalForce.setToZero(); - (*it)->m_externalTorque.setToZero(); + (*it)->m_externalForce.setZero(); + (*it)->m_externalTorque.setZero(); } } @@ -363,7 +363,7 @@ inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool /** * @return The current gravity vector (in meter per seconds squared) */ -inline Vector3 DynamicsWorld::getGravity() const { +inline vec3 DynamicsWorld::getGravity() const { return m_gravity; } @@ -371,7 +371,7 @@ inline Vector3 DynamicsWorld::getGravity() const { /** * @param gravity The gravity vector (in meter per seconds squared) */ -inline void DynamicsWorld::setGravity(Vector3& gravity) { +inline void DynamicsWorld::setGravity(vec3& gravity) { m_gravity = gravity; } @@ -448,7 +448,7 @@ inline float DynamicsWorld::getSleepLinearVelocity() const { * @param sleepLinearVelocity The sleep linear velocity (in meters per second) */ inline void DynamicsWorld::setSleepLinearVelocity(float sleepLinearVelocity) { - assert(sleepLinearVelocity >= float(0.0)); + assert(sleepLinearVelocity >= 0.0f); m_sleepLinearVelocity = sleepLinearVelocity; } @@ -468,7 +468,7 @@ inline float DynamicsWorld::getSleepAngularVelocity() const { * @param sleepAngularVelocity The sleep angular velocity (in radian per second) */ inline void DynamicsWorld::setSleepAngularVelocity(float sleepAngularVelocity) { - assert(sleepAngularVelocity >= float(0.0)); + assert(sleepAngularVelocity >= 0.0f); m_sleepAngularVelocity = sleepAngularVelocity; } @@ -486,7 +486,7 @@ inline float DynamicsWorld::getTimeBeforeSleep() const { * @param timeBeforeSleep Time a body is required to stay still before sleeping (in seconds) */ inline void DynamicsWorld::setTimeBeforeSleep(float timeBeforeSleep) { - assert(timeBeforeSleep >= float(0.0)); + assert(timeBeforeSleep >= 0.0f); m_timeBeforeSleep = timeBeforeSleep; } diff --git a/ephysics/engine/Impulse.h b/ephysics/engine/Impulse.h index f257760..8be1fe7 100644 --- a/ephysics/engine/Impulse.h +++ b/ephysics/engine/Impulse.h @@ -28,22 +28,22 @@ struct Impulse { // -------------------- Attributes -------------------- // /// Linear impulse applied to the first body - const Vector3 linearImpulseBody1; + const vec3 linearImpulseBody1; /// Angular impulse applied to the first body - const Vector3 angularImpulseBody1; + const vec3 angularImpulseBody1; /// Linear impulse applied to the second body - const Vector3 linearImpulseBody2; + const vec3 linearImpulseBody2; /// Angular impulse applied to the second body - const Vector3 angularImpulseBody2; + const vec3 angularImpulseBody2; // -------------------- Methods -------------------- // /// Constructor - Impulse(const Vector3& initLinearImpulseBody1, const Vector3& initAngularImpulseBody1, - const Vector3& initLinearImpulseBody2, const Vector3& initAngularImpulseBody2) + Impulse(const vec3& initLinearImpulseBody1, const vec3& initAngularImpulseBody1, + const vec3& initLinearImpulseBody2, const vec3& initAngularImpulseBody2) : linearImpulseBody1(initLinearImpulseBody1), angularImpulseBody1(initAngularImpulseBody1), linearImpulseBody2(initLinearImpulseBody2), diff --git a/ephysics/engine/Material.h b/ephysics/engine/Material.h index 5a69fac..ed2b16f 100644 --- a/ephysics/engine/Material.h +++ b/ephysics/engine/Material.h @@ -82,7 +82,7 @@ inline float Material::getBounciness() const { * @param bounciness Bounciness factor (between 0 and 1) where 1 is very bouncy */ inline void Material::setBounciness(float bounciness) { - assert(bounciness >= float(0.0) && bounciness <= float(1.0)); + assert(bounciness >= 0.0f && bounciness <= 1.0f); m_bounciness = bounciness; } @@ -101,7 +101,7 @@ inline float Material::getFrictionCoefficient() const { * @param frictionCoefficient Friction coefficient (positive value) */ inline void Material::setFrictionCoefficient(float frictionCoefficient) { - assert(frictionCoefficient >= float(0.0)); + assert(frictionCoefficient >= 0.0f); m_frictionCoefficient = frictionCoefficient; } diff --git a/ephysics/engine/OverlappingPair.h b/ephysics/engine/OverlappingPair.h index ffe71f9..a094654 100644 --- a/ephysics/engine/OverlappingPair.h +++ b/ephysics/engine/OverlappingPair.h @@ -34,7 +34,7 @@ class OverlappingPair { ContactManifoldSet m_contactManifoldSet; /// Cached previous separating axis - Vector3 m_cachedSeparatingAxis; + vec3 m_cachedSeparatingAxis; // -------------------- Methods -------------------- // @@ -68,10 +68,10 @@ class OverlappingPair { void update(); /// Return the cached separating axis - Vector3 getCachedSeparatingAxis() const; + vec3 getCachedSeparatingAxis() const; /// Set the cached separating axis - void setCachedSeparatingAxis(const Vector3& axis); + void setCachedSeparatingAxis(const vec3& axis); /// Return the number of contacts in the cache uint32_t getNbContactPoints() const; @@ -114,12 +114,12 @@ inline void OverlappingPair::update() { } // Return the cached separating axis -inline Vector3 OverlappingPair::getCachedSeparatingAxis() const { +inline vec3 OverlappingPair::getCachedSeparatingAxis() const { return m_cachedSeparatingAxis; } // Set the cached separating axis -inline void OverlappingPair::setCachedSeparatingAxis(const Vector3& axis) { +inline void OverlappingPair::setCachedSeparatingAxis(const vec3& axis) { m_cachedSeparatingAxis = axis; } diff --git a/ephysics/mathematics/Matrix2x2.cpp b/ephysics/mathematics/Matrix2x2.cpp deleted file mode 100644 index f760530..0000000 --- a/ephysics/mathematics/Matrix2x2.cpp +++ /dev/null @@ -1,68 +0,0 @@ -/** @file - * @author Daniel Chappuis - * @copyright 2010-2016 Daniel Chappuis - * @license BSD 3 clauses (see license file) - */ - -// Libraries -#include - -using namespace reactphysics3d; - -// Constructor of the class Matrix2x2 -Matrix2x2::Matrix2x2() { - - // Initialize all values in the matrix to zero - setAllValues(0.0, 0.0, 0.0, 0.0); -} - -// Constructor -Matrix2x2::Matrix2x2(float value) { - setAllValues(value, value, value, value); -} - -// Constructor with arguments -Matrix2x2::Matrix2x2(float a1, float a2, float b1, float b2) { - - // Initialize the matrix with the values - setAllValues(a1, a2, b1, b2); -} - -// Destructor -Matrix2x2::~Matrix2x2() { - -} - -// Copy-constructor -Matrix2x2::Matrix2x2(const Matrix2x2& matrix) { - setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], - matrix.mRows[1][0], matrix.mRows[1][1]); -} - -// Assignment operator -Matrix2x2& Matrix2x2::operator=(const Matrix2x2& matrix) { - - // Check for self-assignment - if (&matrix != this) { - setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], - matrix.mRows[1][0], matrix.mRows[1][1]); - } - return *this; -} - -// Return the inverse matrix -Matrix2x2 Matrix2x2::getInverse() const { - - // Compute the determinant of the matrix - float determinant = getDeterminant(); - - // Check if the determinant is equal to zero - assert(std::abs(determinant) > MACHINE_EPSILON); - - float invDeterminant = float(1.0) / determinant; - - Matrix2x2 tempMatrix(mRows[1][1], -mRows[0][1], -mRows[1][0], mRows[0][0]); - - // Return the inverse matrix - return (invDeterminant * tempMatrix); -} diff --git a/ephysics/mathematics/Matrix2x2.h b/ephysics/mathematics/Matrix2x2.h deleted file mode 100644 index 5570916..0000000 --- a/ephysics/mathematics/Matrix2x2.h +++ /dev/null @@ -1,297 +0,0 @@ -/** @file - * @author Daniel Chappuis - * @copyright 2010-2016 Daniel Chappuis - * @license BSD 3 clauses (see license file) - */ -#pragma once - -// Libraries -#include -#include - -/// ReactPhysics3D namespace -namespace reactphysics3d { - -// Class Matrix2x2 -/** - * This class represents a 2x2 matrix. - */ -class Matrix2x2 { - - private : - - // -------------------- Attributes -------------------- // - - /// Rows of the matrix; - Vector2 mRows[2]; - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - Matrix2x2(); - - /// Constructor - Matrix2x2(float value); - - /// Constructor - Matrix2x2(float a1, float a2, float b1, float b2); - - /// Destructor - ~Matrix2x2(); - - /// Copy-constructor - Matrix2x2(const Matrix2x2& matrix); - - /// Assignment operator - Matrix2x2& operator=(const Matrix2x2& matrix); - - /// Set all the values in the matrix - void setAllValues(float a1, float a2, float b1, float b2); - - /// Set the matrix to zero - void setToZero(); - - /// Return a column - Vector2 getColumn(int32_t i) const; - - /// Return a row - Vector2 getRow(int32_t i) const; - - /// Return the transpose matrix - Matrix2x2 getTranspose() const; - - /// Return the determinant of the matrix - float getDeterminant() const; - - /// Return the trace of the matrix - float getTrace() const; - - /// Return the inverse matrix - Matrix2x2 getInverse() const; - - /// Return the matrix with absolute values - Matrix2x2 getAbsoluteMatrix() const; - - /// Set the matrix to the identity matrix - void setToIdentity(); - - /// Return the 2x2 identity matrix - static Matrix2x2 identity(); - - /// Return the 2x2 zero matrix - static Matrix2x2 zero(); - - /// Overloaded operator for addition - friend Matrix2x2 operator+(const Matrix2x2& matrix1, const Matrix2x2& matrix2); - - /// Overloaded operator for substraction - friend Matrix2x2 operator-(const Matrix2x2& matrix1, const Matrix2x2& matrix2); - - /// Overloaded operator for the negative of the matrix - friend Matrix2x2 operator-(const Matrix2x2& matrix); - - /// Overloaded operator for multiplication with a number - friend Matrix2x2 operator*(float nb, const Matrix2x2& matrix); - - /// Overloaded operator for multiplication with a matrix - friend Matrix2x2 operator*(const Matrix2x2& matrix, float nb); - - /// Overloaded operator for matrix multiplication - friend Matrix2x2 operator*(const Matrix2x2& matrix1, const Matrix2x2& matrix2); - - /// Overloaded operator for multiplication with a vector - friend Vector2 operator*(const Matrix2x2& matrix, const Vector2& vector); - - /// Overloaded operator for equality condition - bool operator==(const Matrix2x2& matrix) const; - - /// Overloaded operator for the is different condition - bool operator!= (const Matrix2x2& matrix) const; - - /// Overloaded operator for addition with assignment - Matrix2x2& operator+=(const Matrix2x2& matrix); - - /// Overloaded operator for substraction with assignment - Matrix2x2& operator-=(const Matrix2x2& matrix); - - /// Overloaded operator for multiplication with a number with assignment - Matrix2x2& operator*=(float nb); - - /// Overloaded operator to read element of the matrix. - const Vector2& operator[](int32_t row) const; - - /// Overloaded operator to read/write element of the matrix. - Vector2& operator[](int32_t row); -}; - -// Method to set all the values in the matrix -inline void Matrix2x2::setAllValues(float a1, float a2, - float b1, float b2) { - mRows[0][0] = a1; mRows[0][1] = a2; - mRows[1][0] = b1; mRows[1][1] = b2; -} - -// Set the matrix to zero -inline void Matrix2x2::setToZero() { - mRows[0].setToZero(); - mRows[1].setToZero(); -} - -// Return a column -inline Vector2 Matrix2x2::getColumn(int32_t i) const { - assert(i>= 0 && i<2); - return Vector2(mRows[0][i], mRows[1][i]); -} - -// Return a row -inline Vector2 Matrix2x2::getRow(int32_t i) const { - assert(i>= 0 && i<2); - return mRows[i]; -} - -// Return the transpose matrix -inline Matrix2x2 Matrix2x2::getTranspose() const { - - // Return the transpose matrix - return Matrix2x2(mRows[0][0], mRows[1][0], - mRows[0][1], mRows[1][1]); -} - -// Return the determinant of the matrix -inline float Matrix2x2::getDeterminant() const { - - // Compute and return the determinant of the matrix - return mRows[0][0] * mRows[1][1] - mRows[1][0] * mRows[0][1]; -} - -// Return the trace of the matrix -inline float Matrix2x2::getTrace() const { - - // Compute and return the trace - return (mRows[0][0] + mRows[1][1]); -} - -// Set the matrix to the identity matrix -inline void Matrix2x2::setToIdentity() { - mRows[0][0] = 1.0; mRows[0][1] = 0.0; - mRows[1][0] = 0.0; mRows[1][1] = 1.0; -} - -// Return the 2x2 identity matrix -inline Matrix2x2 Matrix2x2::identity() { - - // Return the isdentity matrix - return Matrix2x2(1.0, 0.0, 0.0, 1.0); -} - -// Return the 2x2 zero matrix -inline Matrix2x2 Matrix2x2::zero() { - return Matrix2x2(0.0, 0.0, 0.0, 0.0); -} - -// Return the matrix with absolute values -inline Matrix2x2 Matrix2x2::getAbsoluteMatrix() const { - return Matrix2x2(fabs(mRows[0][0]), fabs(mRows[0][1]), - fabs(mRows[1][0]), fabs(mRows[1][1])); -} - -// Overloaded operator for addition -inline Matrix2x2 operator+(const Matrix2x2& matrix1, const Matrix2x2& matrix2) { - return Matrix2x2(matrix1.mRows[0][0] + matrix2.mRows[0][0], - matrix1.mRows[0][1] + matrix2.mRows[0][1], - matrix1.mRows[1][0] + matrix2.mRows[1][0], - matrix1.mRows[1][1] + matrix2.mRows[1][1]); -} - -// Overloaded operator for substraction -inline Matrix2x2 operator-(const Matrix2x2& matrix1, const Matrix2x2& matrix2) { - return Matrix2x2(matrix1.mRows[0][0] - matrix2.mRows[0][0], - matrix1.mRows[0][1] - matrix2.mRows[0][1], - matrix1.mRows[1][0] - matrix2.mRows[1][0], - matrix1.mRows[1][1] - matrix2.mRows[1][1]); -} - -// Overloaded operator for the negative of the matrix -inline Matrix2x2 operator-(const Matrix2x2& matrix) { - return Matrix2x2(-matrix.mRows[0][0], -matrix.mRows[0][1], - -matrix.mRows[1][0], -matrix.mRows[1][1]); -} - -// Overloaded operator for multiplication with a number -inline Matrix2x2 operator*(float nb, const Matrix2x2& matrix) { - return Matrix2x2(matrix.mRows[0][0] * nb, matrix.mRows[0][1] * nb, - matrix.mRows[1][0] * nb, matrix.mRows[1][1] * nb); -} - -// Overloaded operator for multiplication with a matrix -inline Matrix2x2 operator*(const Matrix2x2& matrix, float nb) { - return nb * matrix; -} - -// Overloaded operator for matrix multiplication -inline Matrix2x2 operator*(const Matrix2x2& matrix1, const Matrix2x2& matrix2) { - return Matrix2x2(matrix1.mRows[0][0] * matrix2.mRows[0][0] + matrix1.mRows[0][1] * - matrix2.mRows[1][0], - matrix1.mRows[0][0] * matrix2.mRows[0][1] + matrix1.mRows[0][1] * - matrix2.mRows[1][1], - matrix1.mRows[1][0] * matrix2.mRows[0][0] + matrix1.mRows[1][1] * - matrix2.mRows[1][0], - matrix1.mRows[1][0] * matrix2.mRows[0][1] + matrix1.mRows[1][1] * - matrix2.mRows[1][1]); -} - -// Overloaded operator for multiplication with a vector -inline Vector2 operator*(const Matrix2x2& matrix, const Vector2& vector) { - return Vector2(matrix.mRows[0][0]*vector.x + matrix.mRows[0][1]*vector.y, - matrix.mRows[1][0]*vector.x + matrix.mRows[1][1]*vector.y); -} - -// Overloaded operator for equality condition -inline bool Matrix2x2::operator==(const Matrix2x2& matrix) const { - return (mRows[0][0] == matrix.mRows[0][0] && mRows[0][1] == matrix.mRows[0][1] && - mRows[1][0] == matrix.mRows[1][0] && mRows[1][1] == matrix.mRows[1][1]); -} - -// Overloaded operator for the is different condition -inline bool Matrix2x2::operator!= (const Matrix2x2& matrix) const { - return !(*this == matrix); -} - -// Overloaded operator for addition with assignment -inline Matrix2x2& Matrix2x2::operator+=(const Matrix2x2& matrix) { - mRows[0][0] += matrix.mRows[0][0]; mRows[0][1] += matrix.mRows[0][1]; - mRows[1][0] += matrix.mRows[1][0]; mRows[1][1] += matrix.mRows[1][1]; - return *this; -} - -// Overloaded operator for substraction with assignment -inline Matrix2x2& Matrix2x2::operator-=(const Matrix2x2& matrix) { - mRows[0][0] -= matrix.mRows[0][0]; mRows[0][1] -= matrix.mRows[0][1]; - mRows[1][0] -= matrix.mRows[1][0]; mRows[1][1] -= matrix.mRows[1][1]; - return *this; -} - -// Overloaded operator for multiplication with a number with assignment -inline Matrix2x2& Matrix2x2::operator*=(float nb) { - mRows[0][0] *= nb; mRows[0][1] *= nb; - mRows[1][0] *= nb; mRows[1][1] *= nb; - return *this; -} - -// Overloaded operator to return a row of the matrix. -/// This operator is also used to access a matrix value using the syntax -/// matrix[row][col]. -inline const Vector2& Matrix2x2::operator[](int32_t row) const { - return mRows[row]; -} - -// Overloaded operator to return a row of the matrix. -/// This operator is also used to access a matrix value using the syntax -/// matrix[row][col]. -inline Vector2& Matrix2x2::operator[](int32_t row) { - return mRows[row]; -} - -} diff --git a/ephysics/mathematics/Matrix3x3.cpp b/ephysics/mathematics/Matrix3x3.cpp deleted file mode 100644 index dc6a0f6..0000000 --- a/ephysics/mathematics/Matrix3x3.cpp +++ /dev/null @@ -1,83 +0,0 @@ -/** @file - * @author Daniel Chappuis - * @copyright 2010-2016 Daniel Chappuis - * @license BSD 3 clauses (see license file) - */ - -// Libraries -#include -#include - -// Namespaces -using namespace reactphysics3d; - -// Constructor of the class Matrix3x3 -Matrix3x3::Matrix3x3() { - // Initialize all values in the matrix to zero - setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); -} - -// Constructor -Matrix3x3::Matrix3x3(float value) { - setAllValues(value, value, value, value, value, value, value, value, value); -} - -// Constructor with arguments -Matrix3x3::Matrix3x3(float a1, float a2, float a3, - float b1, float b2, float b3, - float c1, float c2, float c3) { - // Initialize the matrix with the values - setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3); -} - -// Destructor -Matrix3x3::~Matrix3x3() { - -} - -// Copy-constructor -Matrix3x3::Matrix3x3(const Matrix3x3& matrix) { - setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[0][2], - matrix.mRows[1][0], matrix.mRows[1][1], matrix.mRows[1][2], - matrix.mRows[2][0], matrix.mRows[2][1], matrix.mRows[2][2]); -} - -// Assignment operator -Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix) { - - // Check for self-assignment - if (&matrix != this) { - setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[0][2], - matrix.mRows[1][0], matrix.mRows[1][1], matrix.mRows[1][2], - matrix.mRows[2][0], matrix.mRows[2][1], matrix.mRows[2][2]); - } - return *this; -} - -// Return the inverse matrix -Matrix3x3 Matrix3x3::getInverse() const { - - // Compute the determinant of the matrix - float determinant = getDeterminant(); - - // Check if the determinant is equal to zero - assert(std::abs(determinant) > MACHINE_EPSILON); - - float invDeterminant = float(1.0) / determinant; - - Matrix3x3 tempMatrix((mRows[1][1]*mRows[2][2]-mRows[2][1]*mRows[1][2]), - -(mRows[0][1]*mRows[2][2]-mRows[2][1]*mRows[0][2]), - (mRows[0][1]*mRows[1][2]-mRows[0][2]*mRows[1][1]), - -(mRows[1][0]*mRows[2][2]-mRows[2][0]*mRows[1][2]), - (mRows[0][0]*mRows[2][2]-mRows[2][0]*mRows[0][2]), - -(mRows[0][0]*mRows[1][2]-mRows[1][0]*mRows[0][2]), - (mRows[1][0]*mRows[2][1]-mRows[2][0]*mRows[1][1]), - -(mRows[0][0]*mRows[2][1]-mRows[2][0]*mRows[0][1]), - (mRows[0][0]*mRows[1][1]-mRows[0][1]*mRows[1][0])); - - // Return the inverse matrix - return (invDeterminant * tempMatrix); -} - - - diff --git a/ephysics/mathematics/Matrix3x3.h b/ephysics/mathematics/Matrix3x3.h deleted file mode 100644 index f3c7c74..0000000 --- a/ephysics/mathematics/Matrix3x3.h +++ /dev/null @@ -1,347 +0,0 @@ -/** @file - * @author Daniel Chappuis - * @copyright 2010-2016 Daniel Chappuis - * @license BSD 3 clauses (see license file) - */ -#pragma once - -// Libraries -#include -#include - -/// ReactPhysics3D namespace -namespace reactphysics3d { - - -// Class Matrix3x3 -/** - * This class represents a 3x3 matrix. - */ -class Matrix3x3 { - - private : - - // -------------------- Attributes -------------------- // - - /// Rows of the matrix; - Vector3 mRows[3]; - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - Matrix3x3(); - - /// Constructor - Matrix3x3(float value); - - /// Constructor - Matrix3x3(float a1, float a2, float a3, float b1, float b2, float b3, - float c1, float c2, float c3); - - /// Destructor - virtual ~Matrix3x3(); - - /// Copy-constructor - Matrix3x3(const Matrix3x3& matrix); - - /// Assignment operator - Matrix3x3& operator=(const Matrix3x3& matrix); - - /// Set all the values in the matrix - void setAllValues(float a1, float a2, float a3, float b1, float b2, float b3, - float c1, float c2, float c3); - - /// Set the matrix to zero - void setToZero(); - - /// Return a column - Vector3 getColumn(int32_t i) const; - - /// Return a row - Vector3 getRow(int32_t i) const; - - /// Return the transpose matrix - Matrix3x3 getTranspose() const; - - /// Return the determinant of the matrix - float getDeterminant() const; - - /// Return the trace of the matrix - float getTrace() const; - - /// Return the inverse matrix - Matrix3x3 getInverse() const; - - /// Return the matrix with absolute values - Matrix3x3 getAbsoluteMatrix() const; - - /// Set the matrix to the identity matrix - void setToIdentity(); - - /// Return the 3x3 identity matrix - static Matrix3x3 identity(); - - /// Return the 3x3 zero matrix - static Matrix3x3 zero(); - - /// Return a skew-symmetric matrix using a given vector that can be used - /// to compute cross product with another vector using matrix multiplication - static Matrix3x3 computeSkewSymmetricMatrixForCrossProduct(const Vector3& vector); - - /// Overloaded operator for addition - friend Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2); - - /// Overloaded operator for substraction - friend Matrix3x3 operator-(const Matrix3x3& matrix1, const Matrix3x3& matrix2); - - /// Overloaded operator for the negative of the matrix - friend Matrix3x3 operator-(const Matrix3x3& matrix); - - /// Overloaded operator for multiplication with a number - friend Matrix3x3 operator*(float nb, const Matrix3x3& matrix); - - /// Overloaded operator for multiplication with a matrix - friend Matrix3x3 operator*(const Matrix3x3& matrix, float nb); - - /// Overloaded operator for matrix multiplication - friend Matrix3x3 operator*(const Matrix3x3& matrix1, const Matrix3x3& matrix2); - - /// Overloaded operator for multiplication with a vector - friend Vector3 operator*(const Matrix3x3& matrix, const Vector3& vector); - - /// Overloaded operator for equality condition - bool operator==(const Matrix3x3& matrix) const; - - /// Overloaded operator for the is different condition - bool operator!= (const Matrix3x3& matrix) const; - - /// Overloaded operator for addition with assignment - Matrix3x3& operator+=(const Matrix3x3& matrix); - - /// Overloaded operator for substraction with assignment - Matrix3x3& operator-=(const Matrix3x3& matrix); - - /// Overloaded operator for multiplication with a number with assignment - Matrix3x3& operator*=(float nb); - - /// Overloaded operator to read element of the matrix. - const Vector3& operator[](int32_t row) const; - - /// Overloaded operator to read/write element of the matrix. - Vector3& operator[](int32_t row); -}; - -// Method to set all the values in the matrix -inline void Matrix3x3::setAllValues(float a1, float a2, float a3, - float b1, float b2, float b3, - float c1, float c2, float c3) { - mRows[0][0] = a1; mRows[0][1] = a2; mRows[0][2] = a3; - mRows[1][0] = b1; mRows[1][1] = b2; mRows[1][2] = b3; - mRows[2][0] = c1; mRows[2][1] = c2; mRows[2][2] = c3; -} - -// Set the matrix to zero -inline void Matrix3x3::setToZero() { - mRows[0].setToZero(); - mRows[1].setToZero(); - mRows[2].setToZero(); -} - -// Return a column -inline Vector3 Matrix3x3::getColumn(int32_t i) const { - assert(i>= 0 && i<3); - return Vector3(mRows[0][i], mRows[1][i], mRows[2][i]); -} - -// Return a row -inline Vector3 Matrix3x3::getRow(int32_t i) const { - assert(i>= 0 && i<3); - return mRows[i]; -} - -// Return the transpose matrix -inline Matrix3x3 Matrix3x3::getTranspose() const { - - // Return the transpose matrix - return Matrix3x3(mRows[0][0], mRows[1][0], mRows[2][0], - mRows[0][1], mRows[1][1], mRows[2][1], - mRows[0][2], mRows[1][2], mRows[2][2]); -} - -// Return the determinant of the matrix -inline float Matrix3x3::getDeterminant() const { - - // Compute and return the determinant of the matrix - return (mRows[0][0]*(mRows[1][1]*mRows[2][2]-mRows[2][1]*mRows[1][2]) - - mRows[0][1]*(mRows[1][0]*mRows[2][2]-mRows[2][0]*mRows[1][2]) + - mRows[0][2]*(mRows[1][0]*mRows[2][1]-mRows[2][0]*mRows[1][1])); -} - -// Return the trace of the matrix -inline float Matrix3x3::getTrace() const { - - // Compute and return the trace - return (mRows[0][0] + mRows[1][1] + mRows[2][2]); -} - -// Set the matrix to the identity matrix -inline void Matrix3x3::setToIdentity() { - mRows[0][0] = 1.0; mRows[0][1] = 0.0; mRows[0][2] = 0.0; - mRows[1][0] = 0.0; mRows[1][1] = 1.0; mRows[1][2] = 0.0; - mRows[2][0] = 0.0; mRows[2][1] = 0.0; mRows[2][2] = 1.0; -} - -// Return the 3x3 identity matrix -inline Matrix3x3 Matrix3x3::identity() { - return Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); -} - -// Return the 3x3 zero matrix -inline Matrix3x3 Matrix3x3::zero() { - return Matrix3x3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); -} - -// Return a skew-symmetric matrix using a given vector that can be used -// to compute cross product with another vector using matrix multiplication -inline Matrix3x3 Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(const Vector3& vector) { - return Matrix3x3(0, -vector.z, vector.y, vector.z, 0, -vector.x, -vector.y, vector.x, 0); -} - -// Return the matrix with absolute values -inline Matrix3x3 Matrix3x3::getAbsoluteMatrix() const { - return Matrix3x3(fabs(mRows[0][0]), fabs(mRows[0][1]), fabs(mRows[0][2]), - fabs(mRows[1][0]), fabs(mRows[1][1]), fabs(mRows[1][2]), - fabs(mRows[2][0]), fabs(mRows[2][1]), fabs(mRows[2][2])); -} - -// Overloaded operator for addition -inline Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2) { - return Matrix3x3(matrix1.mRows[0][0] + matrix2.mRows[0][0], matrix1.mRows[0][1] + - matrix2.mRows[0][1], matrix1.mRows[0][2] + matrix2.mRows[0][2], - matrix1.mRows[1][0] + matrix2.mRows[1][0], matrix1.mRows[1][1] + - matrix2.mRows[1][1], matrix1.mRows[1][2] + matrix2.mRows[1][2], - matrix1.mRows[2][0] + matrix2.mRows[2][0], matrix1.mRows[2][1] + - matrix2.mRows[2][1], matrix1.mRows[2][2] + matrix2.mRows[2][2]); -} - -// Overloaded operator for substraction -inline Matrix3x3 operator-(const Matrix3x3& matrix1, const Matrix3x3& matrix2) { - return Matrix3x3(matrix1.mRows[0][0] - matrix2.mRows[0][0], matrix1.mRows[0][1] - - matrix2.mRows[0][1], matrix1.mRows[0][2] - matrix2.mRows[0][2], - matrix1.mRows[1][0] - matrix2.mRows[1][0], matrix1.mRows[1][1] - - matrix2.mRows[1][1], matrix1.mRows[1][2] - matrix2.mRows[1][2], - matrix1.mRows[2][0] - matrix2.mRows[2][0], matrix1.mRows[2][1] - - matrix2.mRows[2][1], matrix1.mRows[2][2] - matrix2.mRows[2][2]); -} - -// Overloaded operator for the negative of the matrix -inline Matrix3x3 operator-(const Matrix3x3& matrix) { - return Matrix3x3(-matrix.mRows[0][0], -matrix.mRows[0][1], -matrix.mRows[0][2], - -matrix.mRows[1][0], -matrix.mRows[1][1], -matrix.mRows[1][2], - -matrix.mRows[2][0], -matrix.mRows[2][1], -matrix.mRows[2][2]); -} - -// Overloaded operator for multiplication with a number -inline Matrix3x3 operator*(float nb, const Matrix3x3& matrix) { - return Matrix3x3(matrix.mRows[0][0] * nb, matrix.mRows[0][1] * nb, matrix.mRows[0][2] * nb, - matrix.mRows[1][0] * nb, matrix.mRows[1][1] * nb, matrix.mRows[1][2] * nb, - matrix.mRows[2][0] * nb, matrix.mRows[2][1] * nb, matrix.mRows[2][2] * nb); -} - -// Overloaded operator for multiplication with a matrix -inline Matrix3x3 operator*(const Matrix3x3& matrix, float nb) { - return nb * matrix; -} - -// Overloaded operator for matrix multiplication -inline Matrix3x3 operator*(const Matrix3x3& matrix1, const Matrix3x3& matrix2) { - return Matrix3x3(matrix1.mRows[0][0]*matrix2.mRows[0][0] + matrix1.mRows[0][1] * - matrix2.mRows[1][0] + matrix1.mRows[0][2]*matrix2.mRows[2][0], - matrix1.mRows[0][0]*matrix2.mRows[0][1] + matrix1.mRows[0][1] * - matrix2.mRows[1][1] + matrix1.mRows[0][2]*matrix2.mRows[2][1], - matrix1.mRows[0][0]*matrix2.mRows[0][2] + matrix1.mRows[0][1] * - matrix2.mRows[1][2] + matrix1.mRows[0][2]*matrix2.mRows[2][2], - matrix1.mRows[1][0]*matrix2.mRows[0][0] + matrix1.mRows[1][1] * - matrix2.mRows[1][0] + matrix1.mRows[1][2]*matrix2.mRows[2][0], - matrix1.mRows[1][0]*matrix2.mRows[0][1] + matrix1.mRows[1][1] * - matrix2.mRows[1][1] + matrix1.mRows[1][2]*matrix2.mRows[2][1], - matrix1.mRows[1][0]*matrix2.mRows[0][2] + matrix1.mRows[1][1] * - matrix2.mRows[1][2] + matrix1.mRows[1][2]*matrix2.mRows[2][2], - matrix1.mRows[2][0]*matrix2.mRows[0][0] + matrix1.mRows[2][1] * - matrix2.mRows[1][0] + matrix1.mRows[2][2]*matrix2.mRows[2][0], - matrix1.mRows[2][0]*matrix2.mRows[0][1] + matrix1.mRows[2][1] * - matrix2.mRows[1][1] + matrix1.mRows[2][2]*matrix2.mRows[2][1], - matrix1.mRows[2][0]*matrix2.mRows[0][2] + matrix1.mRows[2][1] * - matrix2.mRows[1][2] + matrix1.mRows[2][2]*matrix2.mRows[2][2]); -} - -// Overloaded operator for multiplication with a vector -inline Vector3 operator*(const Matrix3x3& matrix, const Vector3& vector) { - return Vector3(matrix.mRows[0][0]*vector.x + matrix.mRows[0][1]*vector.y + - matrix.mRows[0][2]*vector.z, - matrix.mRows[1][0]*vector.x + matrix.mRows[1][1]*vector.y + - matrix.mRows[1][2]*vector.z, - matrix.mRows[2][0]*vector.x + matrix.mRows[2][1]*vector.y + - matrix.mRows[2][2]*vector.z); -} - -// Overloaded operator for equality condition -inline bool Matrix3x3::operator==(const Matrix3x3& matrix) const { - return (mRows[0][0] == matrix.mRows[0][0] && mRows[0][1] == matrix.mRows[0][1] && - mRows[0][2] == matrix.mRows[0][2] && - mRows[1][0] == matrix.mRows[1][0] && mRows[1][1] == matrix.mRows[1][1] && - mRows[1][2] == matrix.mRows[1][2] && - mRows[2][0] == matrix.mRows[2][0] && mRows[2][1] == matrix.mRows[2][1] && - mRows[2][2] == matrix.mRows[2][2]); -} - -// Overloaded operator for the is different condition -inline bool Matrix3x3::operator!= (const Matrix3x3& matrix) const { - return !(*this == matrix); -} - -// Overloaded operator for addition with assignment -inline Matrix3x3& Matrix3x3::operator+=(const Matrix3x3& matrix) { - mRows[0][0] += matrix.mRows[0][0]; mRows[0][1] += matrix.mRows[0][1]; - mRows[0][2] += matrix.mRows[0][2]; mRows[1][0] += matrix.mRows[1][0]; - mRows[1][1] += matrix.mRows[1][1]; mRows[1][2] += matrix.mRows[1][2]; - mRows[2][0] += matrix.mRows[2][0]; mRows[2][1] += matrix.mRows[2][1]; - mRows[2][2] += matrix.mRows[2][2]; - return *this; -} - -// Overloaded operator for substraction with assignment -inline Matrix3x3& Matrix3x3::operator-=(const Matrix3x3& matrix) { - mRows[0][0] -= matrix.mRows[0][0]; mRows[0][1] -= matrix.mRows[0][1]; - mRows[0][2] -= matrix.mRows[0][2]; mRows[1][0] -= matrix.mRows[1][0]; - mRows[1][1] -= matrix.mRows[1][1]; mRows[1][2] -= matrix.mRows[1][2]; - mRows[2][0] -= matrix.mRows[2][0]; mRows[2][1] -= matrix.mRows[2][1]; - mRows[2][2] -= matrix.mRows[2][2]; - return *this; -} - -// Overloaded operator for multiplication with a number with assignment -inline Matrix3x3& Matrix3x3::operator*=(float nb) { - mRows[0][0] *= nb; mRows[0][1] *= nb; mRows[0][2] *= nb; - mRows[1][0] *= nb; mRows[1][1] *= nb; mRows[1][2] *= nb; - mRows[2][0] *= nb; mRows[2][1] *= nb; mRows[2][2] *= nb; - return *this; -} - -// Overloaded operator to return a row of the matrix. -/// This operator is also used to access a matrix value using the syntax -/// matrix[row][col]. -inline const Vector3& Matrix3x3::operator[](int32_t row) const { - return mRows[row]; -} - -// Overloaded operator to return a row of the matrix. -/// This operator is also used to access a matrix value using the syntax -/// matrix[row][col]. -inline Vector3& Matrix3x3::operator[](int32_t row) { - return mRows[row]; -} - -} diff --git a/ephysics/mathematics/Quaternion.cpp b/ephysics/mathematics/Quaternion.cpp deleted file mode 100644 index 81d9c20..0000000 --- a/ephysics/mathematics/Quaternion.cpp +++ /dev/null @@ -1,241 +0,0 @@ -/** @file - * @author Daniel Chappuis - * @copyright 2010-2016 Daniel Chappuis - * @license BSD 3 clauses (see license file) - */ - -// Libraries -#include -#include -#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(float newX, float newY, float newZ, float newW) - :x(newX), y(newY), z(newZ), w(newW) { - -} - -// Constructor with the component w and the vector v=(x y z) -Quaternion::Quaternion(float newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), w(newW) { - -} - -// Constructor which convert Euler angles (in radians) to a quaternion -Quaternion::Quaternion(float angleX, float angleY, float angleZ) { - initWithEulerAngles(angleX, angleY, angleZ); -} - -// Constructor which convert Euler angles (in radians) to a quaternion -Quaternion::Quaternion(const Vector3& eulerAngles) { - initWithEulerAngles(eulerAngles.x, eulerAngles.y, eulerAngles.z); -} - -// Copy-constructor -Quaternion::Quaternion(const Quaternion& quaternion) - :x(quaternion.x), y(quaternion.y), z(quaternion.z), w(quaternion.w) { - -} - -// Create a unit quaternion from a rotation matrix -Quaternion::Quaternion(const Matrix3x3& matrix) { - - // Get the trace of the matrix - float trace = matrix.getTrace(); - - float r; - float s; - - if (trace < 0.0) { - if (matrix[1][1] > matrix[0][0]) { - if(matrix[2][2] > matrix[1][1]) { - r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + float(1.0)); - s = float(0.5) / r; - - // Compute the quaternion - x = (matrix[2][0] + matrix[0][2]) * s; - y = (matrix[1][2] + matrix[2][1]) * s; - z = float(0.5) * r; - w = (matrix[1][0] - matrix[0][1]) * s; - } - else { - r = sqrt(matrix[1][1] - matrix[2][2] - matrix[0][0] + float(1.0)); - s = float(0.5) / r; - - // Compute the quaternion - x = (matrix[0][1] + matrix[1][0]) * s; - y = float(0.5) * r; - z = (matrix[1][2] + matrix[2][1]) * s; - w = (matrix[0][2] - matrix[2][0]) * s; - } - } - else if (matrix[2][2] > matrix[0][0]) { - r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + float(1.0)); - s = float(0.5) / r; - - // Compute the quaternion - x = (matrix[2][0] + matrix[0][2]) * s; - y = (matrix[1][2] + matrix[2][1]) * s; - z = float(0.5) * r; - w = (matrix[1][0] - matrix[0][1]) * s; - } - else { - r = sqrt(matrix[0][0] - matrix[1][1] - matrix[2][2] + float(1.0)); - s = float(0.5) / r; - - // Compute the quaternion - x = float(0.5) * r; - y = (matrix[0][1] + matrix[1][0]) * s; - z = (matrix[2][0] - matrix[0][2]) * s; - w = (matrix[2][1] - matrix[1][2]) * s; - } - } - else { - r = sqrt(trace + float(1.0)); - s = float(0.5) / r; - - // Compute the quaternion - x = (matrix[2][1] - matrix[1][2]) * s; - y = (matrix[0][2] - matrix[2][0]) * s; - z = (matrix[1][0] - matrix[0][1]) * s; - w = float(0.5) * r; - } -} - -// Destructor -Quaternion::~Quaternion() { - -} - -// Compute the rotation angle (in radians) and the rotation axis -/// This method is used to get the rotation angle (in radian) and the unit -/// rotation axis of an orientation quaternion. -void Quaternion::getRotationAngleAxis(float& angle, Vector3& axis) const { - Quaternion quaternion; - - // If the quaternion is unit - if (length() == 1.0) { - quaternion = *this; - } - else { - // We compute the unit quaternion - quaternion = getUnit(); - } - - // Compute the roation angle - angle = acos(quaternion.w) * float(2.0); - - // Compute the 3D rotation axis - Vector3 rotationAxis(quaternion.x, quaternion.y, quaternion.z); - - // Normalize the rotation axis - rotationAxis = rotationAxis.getUnit(); - - // Set the rotation axis values - axis.setAllValues(rotationAxis.x, rotationAxis.y, rotationAxis.z); -} - -// Return the orientation matrix corresponding to this quaternion -Matrix3x3 Quaternion::getMatrix() const { - - float nQ = x*x + y*y + z*z + w*w; - float s = 0.0; - - if (nQ > 0.0) { - s = float(2.0) / nQ; - } - - // Computations used for optimization (less multiplications) - float xs = x*s; - float ys = y*s; - float zs = z*s; - float wxs = w*xs; - float wys = w*ys; - float wzs = w*zs; - float xxs = x*xs; - float xys = x*ys; - float xzs = x*zs; - float yys = y*ys; - float yzs = y*zs; - float zzs = z*zs; - - // Create the matrix corresponding to the quaternion - return Matrix3x3(float(1.0) - yys - zzs, xys-wzs, xzs + wys, - xys + wzs, float(1.0) - xxs - zzs, yzs-wxs, - xzs-wys, yzs + wxs, float(1.0) - xxs - yys); -} - -// Compute the spherical linear int32_terpolation between two quaternions. -/// The t argument has to be such that 0 <= t <= 1. This method is static. -Quaternion Quaternion::slerp(const Quaternion& quaternion1, - const Quaternion& quaternion2, float t) { - assert(t >= 0.0 && t <= 1.0); - - float invert = 1.0; - - // Compute cos(theta) using the quaternion scalar product - float cosineTheta = quaternion1.dot(quaternion2); - - // Take care of the sign of cosineTheta - if (cosineTheta < 0.0) { - cosineTheta = -cosineTheta; - invert = -1.0; - } - - // Because of precision, if cos(theta) is nearly 1, - // therefore theta is nearly 0 and we can write - // sin((1-t)*theta) as (1-t) and sin(t*theta) as t - const float epsilon = float(0.00001); - if(1-cosineTheta < epsilon) { - return quaternion1 * (float(1.0)-t) + quaternion2 * (t * invert); - } - - // Compute the theta angle - float theta = acos(cosineTheta); - - // Compute sin(theta) - float sineTheta = sin(theta); - - // Compute the two coefficients that are in the spherical linear int32_terpolation formula - float coeff1 = sin((float(1.0)-t)*theta) / sineTheta; - float coeff2 = sin(t*theta) / sineTheta * invert; - - // Compute and return the int32_terpolated quaternion - return quaternion1 * coeff1 + quaternion2 * coeff2; -} - -// Initialize the quaternion using Euler angles -void Quaternion::initWithEulerAngles(float angleX, float angleY, float angleZ) { - - float angle = angleX * float(0.5); - const float sinX = std::sin(angle); - const float cosX = std::cos(angle); - - angle = angleY * float(0.5); - const float sinY = std::sin(angle); - const float cosY = std::cos(angle); - - angle = angleZ * float(0.5); - const float sinZ = std::sin(angle); - const float cosZ = std::cos(angle); - - const float cosYcosZ = cosY * cosZ; - const float sinYcosZ = sinY * cosZ; - const float cosYsinZ = cosY * sinZ; - const float sinYsinZ = sinY * sinZ; - - x = sinX * cosYcosZ - cosX * sinYsinZ; - y = cosX * sinYcosZ + sinX * cosYsinZ; - z = cosX * cosYsinZ - sinX * sinYcosZ; - w = cosX * cosYcosZ + sinX * sinYsinZ; - - // Normalize the quaternion - normalize(); -} diff --git a/ephysics/mathematics/Quaternion.h b/ephysics/mathematics/Quaternion.h deleted file mode 100644 index b4b279f..0000000 --- a/ephysics/mathematics/Quaternion.h +++ /dev/null @@ -1,330 +0,0 @@ -/** @file - * @author Daniel Chappuis - * @copyright 2010-2016 Daniel Chappuis - * @license BSD 3 clauses (see license file) - */ -#pragma once - -// Libraries -#include -#include -#include - - -/// ReactPhysics3D namespace -namespace reactphysics3d { - -// Class Quaternion -/** - * This class represents a quaternion. We use the notation : - * q = (x*i, y*j, z*k, w) to represent a quaternion. - */ -struct Quaternion { - - public : - - // -------------------- Attributes -------------------- // - - /// Component x - float x; - - /// Component y - float y; - - /// Component z - float z; - - /// Component w - float w; - - // -------------------- Methods -------------------- // - - /// Constructor - Quaternion(); - - /// Constructor with arguments - Quaternion(float newX, float newY, float newZ, float newW); - - /// Constructor with the component w and the vector v=(x y z) - Quaternion(float newW, const Vector3& v); - - /// Constructor which convert Euler angles (in radians) to a quaternion - Quaternion(float angleX, float angleY, float angleZ); - - /// Constructor which convert Euler angles (in radians) to a quaternion - Quaternion(const Vector3& eulerAngles); - - /// Copy-constructor - Quaternion(const Quaternion& quaternion); - - /// Create a unit quaternion from a rotation matrix - Quaternion(const Matrix3x3& matrix); - - /// Destructor - ~Quaternion(); - - /// Set all the values - void setAllValues(float newX, float newY, float newZ, float newW); - - /// Set the quaternion to zero - void setToZero(); - - /// Set to the identity quaternion - void setToIdentity(); - - /// Return the vector v=(x y z) of the quaternion - Vector3 getVectorV() const; - - /// Return the length of the quaternion - float length() const; - - /// Return the square of the length of the quaternion - float lengthSquare() const; - - /// Normalize the quaternion - void normalize(); - - /// Inverse the quaternion - void inverse(); - - /// Return the unit quaternion - Quaternion getUnit() const; - - /// Return the conjugate quaternion - Quaternion getConjugate() const; - - /// Return the inverse of the quaternion - Quaternion getInverse() const; - - /// Return the orientation matrix corresponding to this quaternion - Matrix3x3 getMatrix() const; - - /// Return the identity quaternion - static Quaternion identity(); - - /// Dot product between two quaternions - float dot(const Quaternion& quaternion) const; - - /// Compute the rotation angle (in radians) and the rotation axis - void getRotationAngleAxis(float& angle, Vector3& axis) const; - - /// Compute the spherical linear int32_terpolation between two quaternions - static Quaternion slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, - float t); - - /// Overloaded operator for the addition - Quaternion operator+(const Quaternion& quaternion) const; - - /// Overloaded operator for the substraction - Quaternion operator-(const Quaternion& quaternion) const; - - /// Overloaded operator for addition with assignment - Quaternion& operator+=(const Quaternion& quaternion); - - /// Overloaded operator for substraction with assignment - Quaternion& operator-=(const Quaternion& quaternion); - - /// Overloaded operator for the multiplication with a constant - Quaternion operator*(float nb) const; - - /// Overloaded operator for the multiplication - Quaternion operator*(const Quaternion& quaternion) const; - - /// Overloaded operator for the multiplication with a vector - Vector3 operator*(const Vector3& point) const; - - /// Overloaded operator for assignment - Quaternion& operator=(const Quaternion& quaternion); - - /// Overloaded operator for equality condition - bool operator==(const Quaternion& quaternion) const; - - private: - - /// Initialize the quaternion using Euler angles - void initWithEulerAngles(float angleX, float angleY, float angleZ); -}; - -/// Set all the values -inline void Quaternion::setAllValues(float newX, float newY, float newZ, float newW) { - x = newX; - y = newY; - z = newZ; - w = newW; -} - -/// Set the quaternion to zero -inline void Quaternion::setToZero() { - x = 0; - y = 0; - z = 0; - w = 0; -} - -// Set to the identity quaternion -inline void Quaternion::setToIdentity() { - x = 0; - y = 0; - z = 0; - w = 1; -} - -// Return the vector v=(x y z) of the quaternion -inline Vector3 Quaternion::getVectorV() const { - - // Return the vector v - return Vector3(x, y, z); -} - -// Return the length of the quaternion (inline) -inline float Quaternion::length() const { - return sqrt(x*x + y*y + z*z + w*w); -} - -// Return the square of the length of the quaternion -inline float Quaternion::lengthSquare() const { - return x*x + y*y + z*z + w*w; -} - -// Normalize the quaternion -inline void Quaternion::normalize() { - - float l = length(); - - // Check if the length is not equal to zero - assert (l > MACHINE_EPSILON); - - x /= l; - y /= l; - z /= l; - w /= l; -} - -// Inverse the quaternion -inline void Quaternion::inverse() { - - // Get the square length of the quaternion - float lengthSquareQuaternion = lengthSquare(); - - assert (lengthSquareQuaternion > MACHINE_EPSILON); - - // Compute and return the inverse quaternion - x /= -lengthSquareQuaternion; - y /= -lengthSquareQuaternion; - z /= -lengthSquareQuaternion; - w /= lengthSquareQuaternion; -} - -// Return the unit quaternion -inline Quaternion Quaternion::getUnit() const { - float lengthQuaternion = length(); - - // Check if the length is not equal to zero - assert (lengthQuaternion > MACHINE_EPSILON); - - // Compute and return the unit quaternion - return Quaternion(x / lengthQuaternion, y / lengthQuaternion, - z / lengthQuaternion, w / lengthQuaternion); -} - -// Return the identity quaternion -inline Quaternion Quaternion::identity() { - return Quaternion(0.0, 0.0, 0.0, 1.0); -} - -// Return the conjugate of the quaternion (inline) -inline Quaternion Quaternion::getConjugate() const { - return Quaternion(-x, -y, -z, w); -} - -// Return the inverse of the quaternion (inline) -inline Quaternion Quaternion::getInverse() const { - - float lengthSquareQuaternion = lengthSquare(); - - assert (lengthSquareQuaternion > MACHINE_EPSILON); - - // Compute and return the inverse quaternion - return Quaternion(-x / lengthSquareQuaternion, -y / lengthSquareQuaternion, - -z / lengthSquareQuaternion, w / lengthSquareQuaternion); -} - -// Scalar product between two quaternions -inline float Quaternion::dot(const Quaternion& quaternion) const { - return (x*quaternion.x + y*quaternion.y + z*quaternion.z + w*quaternion.w); -} - -// Overloaded operator for the addition of two quaternions -inline Quaternion Quaternion::operator+(const Quaternion& quaternion) const { - - // Return the result quaternion - return Quaternion(x + quaternion.x, y + quaternion.y, z + quaternion.z, w + quaternion.w); -} - -// Overloaded operator for the substraction of two quaternions -inline Quaternion Quaternion::operator-(const Quaternion& quaternion) const { - - // Return the result of the substraction - return Quaternion(x - quaternion.x, y - quaternion.y, z - quaternion.z, w - quaternion.w); -} - -// Overloaded operator for addition with assignment -inline Quaternion& Quaternion::operator+=(const Quaternion& quaternion) { - x += quaternion.x; - y += quaternion.y; - z += quaternion.z; - w += quaternion.w; - return *this; -} - -// Overloaded operator for substraction with assignment -inline Quaternion& Quaternion::operator-=(const Quaternion& quaternion) { - x -= quaternion.x; - y -= quaternion.y; - z -= quaternion.z; - w -= quaternion.w; - return *this; -} - -// Overloaded operator for the multiplication with a constant -inline Quaternion Quaternion::operator*(float nb) const { - return Quaternion(nb * x, nb * y, nb * z, nb * w); -} - -// Overloaded operator for the multiplication of two quaternions -inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const { - return Quaternion(w * quaternion.w - getVectorV().dot(quaternion.getVectorV()), - w * quaternion.getVectorV() + quaternion.w * getVectorV() + - getVectorV().cross(quaternion.getVectorV())); -} - -// Overloaded operator for the multiplication with a vector. -/// This methods rotates a point given the rotation of a quaternion. -inline Vector3 Quaternion::operator*(const Vector3& point) const { - Quaternion p(point.x, point.y, point.z, 0.0); - return (((*this) * p) * getConjugate()).getVectorV(); -} - -// Overloaded operator for the assignment -inline Quaternion& Quaternion::operator=(const Quaternion& quaternion) { - - // Check for self-assignment - if (this != &quaternion) { - x = quaternion.x; - y = quaternion.y; - z = quaternion.z; - w = quaternion.w; - } - - // Return this quaternion - return *this; -} - -// Overloaded operator for equality condition -inline bool Quaternion::operator==(const Quaternion& quaternion) const { - return (x == quaternion.x && y == quaternion.y && - z == quaternion.z && w == quaternion.w); -} - -} - diff --git a/ephysics/mathematics/Ray.h b/ephysics/mathematics/Ray.h index ed52124..9efc821 100644 --- a/ephysics/mathematics/Ray.h +++ b/ephysics/mathematics/Ray.h @@ -6,7 +6,7 @@ #pragma once // Libraries -#include +#include /// ReactPhysics3D namespace namespace reactphysics3d { @@ -24,10 +24,10 @@ struct Ray { // -------------------- Attributes -------------------- // /// First point of the ray (origin) - Vector3 point1; + vec3 point1; /// Second point of the ray - Vector3 point2; + vec3 point2; /// Maximum fraction value float maxFraction; @@ -35,7 +35,7 @@ struct Ray { // -------------------- Methods -------------------- // /// Constructor with arguments - Ray(const Vector3& p1, const Vector3& p2, float maxFrac = float(1.0)) + Ray(const vec3& p1, const vec3& p2, float maxFrac = 1.0f) : point1(p1), point2(p2), maxFraction(maxFrac) { } diff --git a/ephysics/mathematics/Transform.cpp b/ephysics/mathematics/Transform.cpp deleted file mode 100644 index b6a01e9..0000000 --- a/ephysics/mathematics/Transform.cpp +++ /dev/null @@ -1,39 +0,0 @@ -/** @file - * @author Daniel Chappuis - * @copyright 2010-2016 Daniel Chappuis - * @license BSD 3 clauses (see license file) - */ - -// Libraries -#include - -// Namespaces -using namespace reactphysics3d; - -// Constructor -Transform::Transform() : m_position(Vector3(0.0, 0.0, 0.0)), m_orientation(Quaternion::identity()) { - -} - -// Constructor -Transform::Transform(const Vector3& position, const Matrix3x3& orientation) - : m_position(position), m_orientation(Quaternion(orientation)) { - -} - -// Constructor -Transform::Transform(const Vector3& position, const Quaternion& orientation) - : m_position(position), m_orientation(orientation) { - -} - -// Copy-constructor -Transform::Transform(const Transform& transform) - : m_position(transform.m_position), m_orientation(transform.m_orientation) { - -} - -// Destructor -Transform::~Transform() { - -} diff --git a/ephysics/mathematics/Transform.h b/ephysics/mathematics/Transform.h deleted file mode 100644 index 2db3281..0000000 --- a/ephysics/mathematics/Transform.h +++ /dev/null @@ -1,205 +0,0 @@ -/** @file - * @author Daniel Chappuis - * @copyright 2010-2016 Daniel Chappuis - * @license BSD 3 clauses (see license file) - */ -#pragma once - -// Libraries -#include -#include -#include - -// ReactPhysiscs3D namespace -namespace reactphysics3d { - -// Class Transform -/** - * This class represents a position and an orientation in 3D. It can - * also be seen as representing a translation and a rotation. - */ -class Transform { - - private : - - // -------------------- Attributes -------------------- // - - /// Position - Vector3 m_position; - - /// Orientation - Quaternion m_orientation; - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - Transform(); - - /// Constructor - Transform(const Vector3& position, const Matrix3x3& orientation); - - /// Constructor - Transform(const Vector3& position, const Quaternion& orientation); - - /// Destructor - ~Transform(); - - /// Copy-constructor - Transform(const Transform& transform); - - /// Return the origin of the transform - const Vector3& getPosition() const; - - /// Set the origin of the transform - void setPosition(const Vector3& position); - - /// Return the orientation quaternion - const Quaternion& getOrientation() const; - - /// Set the rotation quaternion - void setOrientation(const Quaternion& orientation); - - /// Set the transform to the identity transform - void setToIdentity(); - - /// Set the transform from an OpenGL transform matrix - void setFromOpenGL(float* openglMatrix); - - /// Get the OpenGL matrix of the transform - void getOpenGLMatrix(float* openglMatrix) const; - - /// Return the inverse of the transform - Transform getInverse() const; - - /// Return an int32_terpolated transform - static Transform int32_terpolateTransforms(const Transform& oldTransform, - const Transform& newTransform, - float int32_terpolationFactor); - - /// Return the identity transform - static Transform identity(); - - /// Return the transformed vector - Vector3 operator*(const Vector3& vector) const; - - /// Operator of multiplication of a transform with another one - Transform operator*(const Transform& transform2) const; - - /// Return true if the two transforms are equal - bool operator==(const Transform& transform2) const; - - /// Return true if the two transforms are different - bool operator!=(const Transform& transform2) const; - - /// Assignment operator - Transform& operator=(const Transform& transform); -}; - -// Return the position of the transform -inline const Vector3& Transform::getPosition() const { - return m_position; -} - -// Set the origin of the transform -inline void Transform::setPosition(const Vector3& position) { - m_position = position; -} - -// Return the rotation matrix -inline const Quaternion& Transform::getOrientation() const { - return m_orientation; -} - -// Set the rotation matrix of the transform -inline void Transform::setOrientation(const Quaternion& orientation) { - m_orientation = orientation; -} - -// Set the transform to the identity transform -inline void Transform::setToIdentity() { - m_position = Vector3(0.0, 0.0, 0.0); - m_orientation = Quaternion::identity(); -} - -// Set the transform from an OpenGL transform matrix -inline void Transform::setFromOpenGL(float* openglMatrix) { - Matrix3x3 matrix(openglMatrix[0], openglMatrix[4], openglMatrix[8], - openglMatrix[1], openglMatrix[5], openglMatrix[9], - openglMatrix[2], openglMatrix[6], openglMatrix[10]); - m_orientation = Quaternion(matrix); - m_position.setAllValues(openglMatrix[12], openglMatrix[13], openglMatrix[14]); -} - -// Get the OpenGL matrix of the transform -inline void Transform::getOpenGLMatrix(float* openglMatrix) const { - const Matrix3x3& matrix = m_orientation.getMatrix(); - openglMatrix[0] = matrix[0][0]; openglMatrix[1] = matrix[1][0]; - openglMatrix[2] = matrix[2][0]; openglMatrix[3] = 0.0; - openglMatrix[4] = matrix[0][1]; openglMatrix[5] = matrix[1][1]; - openglMatrix[6] = matrix[2][1]; openglMatrix[7] = 0.0; - openglMatrix[8] = matrix[0][2]; openglMatrix[9] = matrix[1][2]; - openglMatrix[10] = matrix[2][2]; openglMatrix[11] = 0.0; - openglMatrix[12] = m_position.x; openglMatrix[13] = m_position.y; - openglMatrix[14] = m_position.z; openglMatrix[15] = 1.0; -} - -// Return the inverse of the transform -inline Transform Transform::getInverse() const { - const Quaternion& invQuaternion = m_orientation.getInverse(); - Matrix3x3 invMatrix = invQuaternion.getMatrix(); - return Transform(invMatrix * (-m_position), invQuaternion); -} - -// Return an int32_terpolated transform -inline Transform Transform::int32_terpolateTransforms(const Transform& oldTransform, - const Transform& newTransform, - float int32_terpolationFactor) { - - Vector3 int32_terPosition = oldTransform.m_position * (float(1.0) - int32_terpolationFactor) + - newTransform.m_position * int32_terpolationFactor; - - Quaternion int32_terOrientation = Quaternion::slerp(oldTransform.m_orientation, - newTransform.m_orientation, - int32_terpolationFactor); - - return Transform(int32_terPosition, int32_terOrientation); -} - -// Return the identity transform -inline Transform Transform::identity() { - return Transform(Vector3(0, 0, 0), Quaternion::identity()); -} - -// Return the transformed vector -inline Vector3 Transform::operator*(const Vector3& vector) const { - return (m_orientation.getMatrix() * vector) + m_position; -} - -// Operator of multiplication of a transform with another one -inline Transform Transform::operator*(const Transform& transform2) const { - return Transform(m_position + m_orientation.getMatrix() * transform2.m_position, - m_orientation * transform2.m_orientation); -} - -// Return true if the two transforms are equal -inline bool Transform::operator==(const Transform& transform2) const { - return (m_position == transform2.m_position) && (m_orientation == transform2.m_orientation); -} - -// Return true if the two transforms are different -inline bool Transform::operator!=(const Transform& transform2) const { - return !(*this == transform2); -} - -// Assignment operator -inline Transform& Transform::operator=(const Transform& transform) { - if (&transform != this) { - m_position = transform.m_position; - m_orientation = transform.m_orientation; - } - return *this; -} - -} diff --git a/ephysics/mathematics/Vector2.cpp b/ephysics/mathematics/Vector2.cpp deleted file mode 100644 index 9fdb871..0000000 --- a/ephysics/mathematics/Vector2.cpp +++ /dev/null @@ -1,54 +0,0 @@ -/** @file - * @author Daniel Chappuis - * @copyright 2010-2016 Daniel Chappuis - * @license BSD 3 clauses (see license file) - */ - -// Libraries -#include -#include - -// Namespaces -using namespace reactphysics3d; - -// Constructor -Vector2::Vector2() : x(0.0), y(0.0) { - -} - -// Constructor with arguments -Vector2::Vector2(float newX, float newY) : x(newX), y(newY) { - -} - -// Copy-constructor -Vector2::Vector2(const Vector2& vector) : x(vector.x), y(vector.y) { - -} - -// Destructor -Vector2::~Vector2() { - -} - -// Return the corresponding unit vector -Vector2 Vector2::getUnit() const { - float lengthVector = length(); - - if (lengthVector < MACHINE_EPSILON) { - return *this; - } - - // Compute and return the unit vector - float lengthInv = float(1.0) / lengthVector; - return Vector2(x * lengthInv, y * lengthInv); -} - -// Return one unit orthogonal vector of the current vector -Vector2 Vector2::getOneUnitOrthogonalVector() const { - - float l = length(); - assert(l > MACHINE_EPSILON); - - return Vector2(-y / l, x / l); -} diff --git a/ephysics/mathematics/Vector2.h b/ephysics/mathematics/Vector2.h deleted file mode 100644 index 210aa13..0000000 --- a/ephysics/mathematics/Vector2.h +++ /dev/null @@ -1,323 +0,0 @@ -/** @file - * @author Daniel Chappuis - * @copyright 2010-2016 Daniel Chappuis - * @license BSD 3 clauses (see license file) - */ -#pragma once - -// Libraries -#include -#include -#include - - - -/// ReactPhysics3D namespace -namespace reactphysics3d { - -// Class Vector2 -/** - * This class represents a 2D vector. - */ -struct Vector2 { - - public: - - // -------------------- Attributes -------------------- // - - /// Component x - float x; - - /// Component y - float y; - - // -------------------- Methods -------------------- // - - /// Constructor of the class Vector3D - Vector2(); - - /// Constructor with arguments - Vector2(float newX, float newY); - - /// Copy-constructor - Vector2(const Vector2& vector); - - /// Destructor - ~Vector2(); - - /// Set all the values of the vector - void setAllValues(float newX, float newY); - - /// Set the vector to zero - void setToZero(); - - /// Return the length of the vector - float length() const; - - /// Return the square of the length of the vector - float lengthSquare() const; - - /// Return the corresponding unit vector - Vector2 getUnit() const; - - /// Return one unit orthogonal vector of the current vector - Vector2 getOneUnitOrthogonalVector() const; - - /// Return true if the vector is unit and false otherwise - bool isUnit() const; - - /// Return true if the current vector is the zero vector - bool isZero() const; - - /// Dot product of two vectors - float dot(const Vector2& vector) const; - - /// Normalize the vector - void normalize(); - - /// Return the corresponding absolute value vector - Vector2 getAbsoluteVector() const; - - /// Return the axis with the minimal value - int32_t getMinAxis() const; - - /// Return the axis with the maximal value - int32_t getMaxAxis() const; - - /// Overloaded operator for the equality condition - bool operator== (const Vector2& vector) const; - - /// Overloaded operator for the is different condition - bool operator!= (const Vector2& vector) const; - - /// Overloaded operator for addition with assignment - Vector2& operator+=(const Vector2& vector); - - /// Overloaded operator for substraction with assignment - Vector2& operator-=(const Vector2& vector); - - /// Overloaded operator for multiplication with a number with assignment - Vector2& operator*=(float number); - - /// Overloaded operator for division by a number with assignment - Vector2& operator/=(float number); - - /// Overloaded operator for value access - float& operator[] (int32_t index); - - /// Overloaded operator for value access - const float& operator[] (int32_t index) const; - - /// Overloaded operator - Vector2& operator=(const Vector2& vector); - - /// Overloaded less than operator for ordering to be used inside std::set for instance - bool operator<(const Vector2& vector) const; - - /// Return a vector taking the minimum components of two vectors - static Vector2 min(const Vector2& vector1, const Vector2& vector2); - - /// Return a vector taking the maximum components of two vectors - static Vector2 max(const Vector2& vector1, const Vector2& vector2); - - /// Return the zero vector - static Vector2 zero(); - - // -------------------- Friends -------------------- // - - friend Vector2 operator+(const Vector2& vector1, const Vector2& vector2); - friend Vector2 operator-(const Vector2& vector1, const Vector2& vector2); - friend Vector2 operator-(const Vector2& vector); - friend Vector2 operator*(const Vector2& vector, float number); - friend Vector2 operator*(float number, const Vector2& vector); - friend Vector2 operator*(const Vector2& vector1, const Vector2& vector2); - friend Vector2 operator/(const Vector2& vector, float number); - friend Vector2 operator/(const Vector2& vector1, const Vector2& vector2); -}; - -// Set the vector to zero -inline void Vector2::setToZero() { - x = 0; - y = 0; -} - -// Set all the values of the vector -inline void Vector2::setAllValues(float newX, float newY) { - x = newX; - y = newY; -} - -// Return the length of the vector -inline float Vector2::length() const { - return sqrt(x*x + y*y); -} - -// Return the square of the length of the vector -inline float Vector2::lengthSquare() const { - return x*x + y*y; -} - -// Scalar product of two vectors (inline) -inline float Vector2::dot(const Vector2& vector) const { - return (x*vector.x + y*vector.y); -} - -// Normalize the vector -inline void Vector2::normalize() { - float l = length(); - if (l < MACHINE_EPSILON) { - return; - } - x /= l; - y /= l; -} - -// Return the corresponding absolute value vector -inline Vector2 Vector2::getAbsoluteVector() const { - return Vector2(std::abs(x), std::abs(y)); -} - -// Return the axis with the minimal value -inline int32_t Vector2::getMinAxis() const { - return (x < y ? 0 : 1); -} - -// Return the axis with the maximal value -inline int32_t Vector2::getMaxAxis() const { - return (x < y ? 1 : 0); -} - -// Return true if the vector is unit and false otherwise -inline bool Vector2::isUnit() const { - return approxEqual(lengthSquare(), 1.0); -} - -// Return true if the vector is the zero vector -inline bool Vector2::isZero() const { - return approxEqual(lengthSquare(), 0.0); -} - -// Overloaded operator for the equality condition -inline bool Vector2::operator== (const Vector2& vector) const { - return (x == vector.x && y == vector.y); -} - -// Overloaded operator for the is different condition -inline bool Vector2::operator!= (const Vector2& vector) const { - return !(*this == vector); -} - -// Overloaded operator for addition with assignment -inline Vector2& Vector2::operator+=(const Vector2& vector) { - x += vector.x; - y += vector.y; - return *this; -} - -// Overloaded operator for substraction with assignment -inline Vector2& Vector2::operator-=(const Vector2& vector) { - x -= vector.x; - y -= vector.y; - return *this; -} - -// Overloaded operator for multiplication with a number with assignment -inline Vector2& Vector2::operator*=(float number) { - x *= number; - y *= number; - return *this; -} - -// Overloaded operator for division by a number with assignment -inline Vector2& Vector2::operator/=(float number) { - assert(number > std::numeric_limits::epsilon()); - x /= number; - y /= number; - return *this; -} - -// Overloaded operator for value access -inline float& Vector2::operator[] (int32_t index) { - return (&x)[index]; -} - -// Overloaded operator for value access -inline const float& Vector2::operator[] (int32_t index) const { - return (&x)[index]; -} - -// Overloaded operator for addition -inline Vector2 operator+(const Vector2& vector1, const Vector2& vector2) { - return Vector2(vector1.x + vector2.x, vector1.y + vector2.y); -} - -// Overloaded operator for substraction -inline Vector2 operator-(const Vector2& vector1, const Vector2& vector2) { - return Vector2(vector1.x - vector2.x, vector1.y - vector2.y); -} - -// Overloaded operator for the negative of a vector -inline Vector2 operator-(const Vector2& vector) { - return Vector2(-vector.x, -vector.y); -} - -// Overloaded operator for multiplication with a number -inline Vector2 operator*(const Vector2& vector, float number) { - return Vector2(number * vector.x, number * vector.y); -} - -// Overloaded operator for multiplication of two vectors -inline Vector2 operator*(const Vector2& vector1, const Vector2& vector2) { - return Vector2(vector1.x * vector2.x, vector1.y * vector2.y); -} - -// Overloaded operator for division by a number -inline Vector2 operator/(const Vector2& vector, float number) { - assert(number > MACHINE_EPSILON); - return Vector2(vector.x / number, vector.y / number); -} - -// Overload operator for division between two vectors -inline Vector2 operator/(const Vector2& vector1, const Vector2& vector2) { - assert(vector2.x > MACHINE_EPSILON); - assert(vector2.y > MACHINE_EPSILON); - return Vector2(vector1.x / vector2.x, vector1.y / vector2.y); -} - -// Overloaded operator for multiplication with a number -inline Vector2 operator*(float number, const Vector2& vector) { - return vector * number; -} - -// Assignment operator -inline Vector2& Vector2::operator=(const Vector2& vector) { - if (&vector != this) { - x = vector.x; - y = vector.y; - } - return *this; -} - -// Overloaded less than operator for ordering to be used inside std::set for instance -inline bool Vector2::operator<(const Vector2& vector) const { - return (x == vector.x ? y < vector.y : x < vector.x); -} - -// Return a vector taking the minimum components of two vectors -inline Vector2 Vector2::min(const Vector2& vector1, const Vector2& vector2) { - return Vector2(std::min(vector1.x, vector2.x), - std::min(vector1.y, vector2.y)); -} - -// Return a vector taking the maximum components of two vectors -inline Vector2 Vector2::max(const Vector2& vector1, const Vector2& vector2) { - return Vector2(std::max(vector1.x, vector2.x), - std::max(vector1.y, vector2.y)); -} - -// Return the zero vector -inline Vector2 Vector2::zero() { - return Vector2(0, 0); -} - -} diff --git a/ephysics/mathematics/Vector3.cpp b/ephysics/mathematics/Vector3.cpp deleted file mode 100644 index 1f0505f..0000000 --- a/ephysics/mathematics/Vector3.cpp +++ /dev/null @@ -1,67 +0,0 @@ -/** @file - * @author Daniel Chappuis - * @copyright 2010-2016 Daniel Chappuis - * @license BSD 3 clauses (see license file) - */ - -// Libraries -#include -#include -#include - -// Namespaces -using namespace reactphysics3d; - -// Constructor of the class Vector3D -Vector3::Vector3() : x(0.0), y(0.0), z(0.0) { - -} - -// Constructor with arguments -Vector3::Vector3(float newX, float newY, float newZ) : x(newX), y(newY), z(newZ) { - -} - -// Copy-constructor -Vector3::Vector3(const Vector3& vector) : x(vector.x), y(vector.y), z(vector.z) { - -} - -// Destructor -Vector3::~Vector3() { - -} - -// Return the corresponding unit vector -Vector3 Vector3::getUnit() const { - float lengthVector = length(); - - if (lengthVector < MACHINE_EPSILON) { - return *this; - } - - // Compute and return the unit vector - float lengthInv = float(1.0) / lengthVector; - return Vector3(x * lengthInv, y * lengthInv, z * lengthInv); -} - -// Return one unit orthogonal vector of the current vector -Vector3 Vector3::getOneUnitOrthogonalVector() const { - - assert(length() > MACHINE_EPSILON); - - // Get the minimum element of the vector - Vector3 vectorAbs(fabs(x), fabs(y), fabs(z)); - int32_t minElement = vectorAbs.getMinAxis(); - - if (minElement == 0) { - return Vector3(0.0, -z, y) / sqrt(y*y + z*z); - } - else if (minElement == 1) { - return Vector3(-z, 0.0, x) / sqrt(x*x + z*z); - } - else { - return Vector3(-y, x, 0.0) / sqrt(x*x + y*y); - } - -} diff --git a/ephysics/mathematics/Vector3.h b/ephysics/mathematics/Vector3.h deleted file mode 100644 index 0ecfa72..0000000 --- a/ephysics/mathematics/Vector3.h +++ /dev/null @@ -1,363 +0,0 @@ -/** @file - * @author Daniel Chappuis - * @copyright 2010-2016 Daniel Chappuis - * @license BSD 3 clauses (see license file) - */ -#pragma once - -// Libraries -#include -#include -#include - - - -/// ReactPhysics3D namespace -namespace reactphysics3d { - -// Class Vector3 -/** - * This class represents a 3D vector. - */ -struct Vector3 { - - public: - - // -------------------- Attributes -------------------- // - - /// Component x - float x; - - /// Component y - float y; - - /// Component z - float z; - - // -------------------- Methods -------------------- // - - /// Constructor of the class Vector3D - Vector3(); - - /// Constructor with arguments - Vector3(float newX, float newY, float newZ); - - /// Copy-constructor - Vector3(const Vector3& vector); - - /// Destructor - ~Vector3(); - - /// Set all the values of the vector - void setAllValues(float newX, float newY, float newZ); - - /// Set the vector to zero - void setToZero(); - - /// Return the length of the vector - float length() const; - - /// Return the square of the length of the vector - float lengthSquare() const; - - /// Return the corresponding unit vector - Vector3 getUnit() const; - - /// Return one unit orthogonal vector of the current vector - Vector3 getOneUnitOrthogonalVector() const; - - /// Return true if the vector is unit and false otherwise - bool isUnit() const; - - /// Return true if the current vector is the zero vector - bool isZero() const; - - /// Dot product of two vectors - float dot(const Vector3& vector) const; - - /// Cross product of two vectors - Vector3 cross(const Vector3& vector) const; - - /// Normalize the vector - void normalize(); - - /// Return the corresponding absolute value vector - Vector3 getAbsoluteVector() const; - - /// Return the axis with the minimal value - int32_t getMinAxis() const; - - /// Return the axis with the maximal value - int32_t getMaxAxis() const; - - /// Return the minimum value among the three components of a vector - float getMinValue() const; - - /// Return the maximum value among the three components of a vector - float getMaxValue() const; - - /// Overloaded operator for the equality condition - bool operator== (const Vector3& vector) const; - - /// Overloaded operator for the is different condition - bool operator!= (const Vector3& vector) const; - - /// Overloaded operator for addition with assignment - Vector3& operator+=(const Vector3& vector); - - /// Overloaded operator for substraction with assignment - Vector3& operator-=(const Vector3& vector); - - /// Overloaded operator for multiplication with a number with assignment - Vector3& operator*=(float number); - - /// Overloaded operator for division by a number with assignment - Vector3& operator/=(float number); - - /// Overloaded operator for value access - float& operator[] (int32_t index); - - /// Overloaded operator for value access - const float& operator[] (int32_t index) const; - - /// Overloaded operator - Vector3& operator=(const Vector3& vector); - - /// Overloaded less than operator for ordering to be used inside std::set for instance - bool operator<(const Vector3& vector) const; - - /// Return a vector taking the minimum components of two vectors - static Vector3 min(const Vector3& vector1, const Vector3& vector2); - - /// Return a vector taking the maximum components of two vectors - static Vector3 max(const Vector3& vector1, const Vector3& vector2); - - /// Return the zero vector - static Vector3 zero(); - - // -------------------- Friends -------------------- // - - friend Vector3 operator+(const Vector3& vector1, const Vector3& vector2); - friend Vector3 operator-(const Vector3& vector1, const Vector3& vector2); - friend Vector3 operator-(const Vector3& vector); - friend Vector3 operator*(const Vector3& vector, float number); - friend Vector3 operator*(float number, const Vector3& vector); - friend Vector3 operator*(const Vector3& vector1, const Vector3& vector2); - friend Vector3 operator/(const Vector3& vector, float number); - friend Vector3 operator/(const Vector3& vector1, const Vector3& vector2); -}; - -// Set the vector to zero -inline void Vector3::setToZero() { - x = 0; - y = 0; - z = 0; -} - -// Set all the values of the vector -inline void Vector3::setAllValues(float newX, float newY, float newZ) { - x = newX; - y = newY; - z = newZ; -} - -// Return the length of the vector -inline float Vector3::length() const { - return sqrt(x*x + y*y + z*z); -} - -// Return the square of the length of the vector -inline float Vector3::lengthSquare() const { - return x*x + y*y + z*z; -} - -// Scalar product of two vectors (inline) -inline float Vector3::dot(const Vector3& vector) const { - return (x*vector.x + y*vector.y + z*vector.z); -} - -// Cross product of two vectors (inline) -inline Vector3 Vector3::cross(const Vector3& vector) const { - return Vector3(y * vector.z - z * vector.y, - z * vector.x - x * vector.z, - x * vector.y - y * vector.x); -} - -// Normalize the vector -inline void Vector3::normalize() { - float l = length(); - if (l < MACHINE_EPSILON) { - return; - } - x /= l; - y /= l; - z /= l; -} - -// Return the corresponding absolute value vector -inline Vector3 Vector3::getAbsoluteVector() const { - return Vector3(std::abs(x), std::abs(y), std::abs(z)); -} - -// Return the axis with the minimal value -inline int32_t Vector3::getMinAxis() const { - return (x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2)); -} - -// Return the axis with the maximal value -inline int32_t Vector3::getMaxAxis() const { - return (x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0)); -} - -// Return true if the vector is unit and false otherwise -inline bool Vector3::isUnit() const { - return approxEqual(lengthSquare(), 1.0); -} - -// Return true if the vector is the zero vector -inline bool Vector3::isZero() const { - return approxEqual(lengthSquare(), 0.0); -} - -// Overloaded operator for the equality condition -inline bool Vector3::operator== (const Vector3& vector) const { - return (x == vector.x && y == vector.y && z == vector.z); -} - -// Overloaded operator for the is different condition -inline bool Vector3::operator!= (const Vector3& vector) const { - return !(*this == vector); -} - -// Overloaded operator for addition with assignment -inline Vector3& Vector3::operator+=(const Vector3& vector) { - x += vector.x; - y += vector.y; - z += vector.z; - return *this; -} - -// Overloaded operator for substraction with assignment -inline Vector3& Vector3::operator-=(const Vector3& vector) { - x -= vector.x; - y -= vector.y; - z -= vector.z; - return *this; -} - -// Overloaded operator for multiplication with a number with assignment -inline Vector3& Vector3::operator*=(float number) { - x *= number; - y *= number; - z *= number; - return *this; -} - -// Overloaded operator for division by a number with assignment -inline Vector3& Vector3::operator/=(float number) { - assert(number > std::numeric_limits::epsilon()); - x /= number; - y /= number; - z /= number; - return *this; -} - -// Overloaded operator for value access -inline float& Vector3::operator[] (int32_t index) { - return (&x)[index]; -} - -// Overloaded operator for value access -inline const float& Vector3::operator[] (int32_t index) const { - return (&x)[index]; -} - -// Overloaded operator for addition -inline Vector3 operator+(const Vector3& vector1, const Vector3& vector2) { - return Vector3(vector1.x + vector2.x, vector1.y + vector2.y, vector1.z + vector2.z); -} - -// Overloaded operator for substraction -inline Vector3 operator-(const Vector3& vector1, const Vector3& vector2) { - return Vector3(vector1.x - vector2.x, vector1.y - vector2.y, vector1.z - vector2.z); -} - -// Overloaded operator for the negative of a vector -inline Vector3 operator-(const Vector3& vector) { - return Vector3(-vector.x, -vector.y, -vector.z); -} - -// Overloaded operator for multiplication with a number -inline Vector3 operator*(const Vector3& vector, float number) { - return Vector3(number * vector.x, number * vector.y, number * vector.z); -} - -// Overloaded operator for division by a number -inline Vector3 operator/(const Vector3& vector, float number) { - assert(number > MACHINE_EPSILON); - return Vector3(vector.x / number, vector.y / number, vector.z / number); -} - -// Overload operator for division between two vectors -inline Vector3 operator/(const Vector3& vector1, const Vector3& vector2) { - assert(vector2.x > MACHINE_EPSILON); - assert(vector2.y > MACHINE_EPSILON); - assert(vector2.z > MACHINE_EPSILON); - return Vector3(vector1.x / vector2.x, vector1.y / vector2.y, vector1.z / vector2.z); -} - -// Overloaded operator for multiplication with a number -inline Vector3 operator*(float number, const Vector3& vector) { - return vector * number; -} - -// Overload operator for multiplication between two vectors -inline Vector3 operator*(const Vector3& vector1, const Vector3& vector2) { - return Vector3(vector1.x * vector2.x, vector1.y * vector2.y, vector1.z * vector2.z); -} - -// Assignment operator -inline Vector3& Vector3::operator=(const Vector3& vector) { - if (&vector != this) { - x = vector.x; - y = vector.y; - z = vector.z; - } - return *this; -} - -// Overloaded less than operator for ordering to be used inside std::set for instance -inline bool Vector3::operator<(const Vector3& vector) const { - return (x == vector.x ? (y == vector.y ? z < vector.z : y < vector.y) : x < vector.x); -} - -// Return a vector taking the minimum components of two vectors -inline Vector3 Vector3::min(const Vector3& vector1, const Vector3& vector2) { - return Vector3(std::min(vector1.x, vector2.x), - std::min(vector1.y, vector2.y), - std::min(vector1.z, vector2.z)); -} - -// Return a vector taking the maximum components of two vectors -inline Vector3 Vector3::max(const Vector3& vector1, const Vector3& vector2) { - return Vector3(std::max(vector1.x, vector2.x), - std::max(vector1.y, vector2.y), - std::max(vector1.z, vector2.z)); -} - -// Return the minimum value among the three components of a vector -inline float Vector3::getMinValue() const { - return std::min(std::min(x, y), z); -} - -// Return the maximum value among the three components of a vector -inline float Vector3::getMaxValue() const { - return std::max(std::max(x, y), z); -} - -// Return the zero vector -inline Vector3 Vector3::zero() { - return Vector3(0, 0, 0); -} - -} diff --git a/ephysics/mathematics/mathematics.h b/ephysics/mathematics/mathematics.h index ee03cbf..2b7f9c7 100644 --- a/ephysics/mathematics/mathematics.h +++ b/ephysics/mathematics/mathematics.h @@ -1,38 +1,17 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_MATHEMATICS_H -#define REACTPHYSICS3D_MATHEMATICS_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -40,5 +19,3 @@ #include #include #include - -#endif diff --git a/ephysics/mathematics/mathematics_functions.cpp b/ephysics/mathematics/mathematics_functions.cpp index 228a324..e8e4ad2 100644 --- a/ephysics/mathematics/mathematics_functions.cpp +++ b/ephysics/mathematics/mathematics_functions.cpp @@ -6,18 +6,18 @@ // Libraries #include -#include +#include using namespace reactphysics3d; /// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) /// This method uses the technique described in the book Real-Time collision detection by /// Christer Ericson. -void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c, - const Vector3& p, float& u, float& v, float& w) { - const Vector3 v0 = b - a; - const Vector3 v1 = c - a; - const Vector3 v2 = p - a; +void reactphysics3d::computeBarycentricCoordinatesInTriangle(const vec3& a, const vec3& b, const vec3& c, + const vec3& p, float& u, float& v, float& w) { + const vec3 v0 = b - a; + const vec3 v1 = c - a; + const vec3 v2 = p - a; float d00 = v0.dot(v0); float d01 = v0.dot(v1); @@ -28,13 +28,13 @@ void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, c float denom = d00 * d11 - d01 * d01; v = (d11 * d20 - d01 * d21) / denom; w = (d00 * d21 - d01 * d20) / denom; - u = float(1.0) - v - w; + u = 1.0f - v - w; } // Clamp a vector such that it is no longer than a given maximum length -Vector3 reactphysics3d::clamp(const Vector3& vector, float maxLength) { - if (vector.lengthSquare() > maxLength * maxLength) { - return vector.getUnit() * maxLength; +vec3 reactphysics3d::clamp(const vec3& vector, float maxLength) { + if (vector.length2() > maxLength * maxLength) { + return vector.safeNormalized() * maxLength; } return vector; } diff --git a/ephysics/mathematics/mathematics_functions.h b/ephysics/mathematics/mathematics_functions.h index 9d56381..7ee39da 100644 --- a/ephysics/mathematics/mathematics_functions.h +++ b/ephysics/mathematics/mathematics_functions.h @@ -15,8 +15,6 @@ /// ReactPhysics3D namespace namespace reactphysics3d { -struct Vector3; - // ---------- Mathematics functions ---------- // /// Function to test if two real numbers are (almost) equal @@ -51,14 +49,14 @@ inline float max3(float a, float b, float c) { /// Return true if two values have the same sign inline bool sameSign(float a, float b) { - return a * b >= float(0.0); + return a * b >= 0.0f; } /// Clamp a vector such that it is no longer than a given maximum length -Vector3 clamp(const Vector3& vector, float maxLength); +vec3 clamp(const vec3& vector, float maxLength); /// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) -void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c, - const Vector3& p, float& u, float& v, float& w); +void computeBarycentricCoordinatesInTriangle(const vec3& a, const vec3& b, const vec3& c, + const vec3& p, float& u, float& v, float& w); } diff --git a/lutin_ephysics.py b/lutin_ephysics.py index 49ced20..ce777bf 100644 --- a/lutin_ephysics.py +++ b/lutin_ephysics.py @@ -70,13 +70,7 @@ def configure(target, my_module): 'ephysics/body/RigidBody.cpp', 'ephysics/body/Body.cpp', 'ephysics/body/CollisionBody.cpp', - 'ephysics/mathematics/Vector2.cpp', - 'ephysics/mathematics/Vector3.cpp', - 'ephysics/mathematics/Transform.cpp', - 'ephysics/mathematics/Matrix2x2.cpp', 'ephysics/mathematics/mathematics_functions.cpp', - 'ephysics/mathematics/Matrix3x3.cpp', - 'ephysics/mathematics/Quaternion.cpp', 'ephysics/engine/CollisionWorld.cpp', 'ephysics/engine/OverlappingPair.cpp', 'ephysics/engine/Material.cpp', @@ -138,13 +132,7 @@ def configure(target, my_module): 'ephysics/body/CollisionBody.h', 'ephysics/mathematics/mathematics.h', 'ephysics/mathematics/Ray.h', - 'ephysics/mathematics/Matrix2x2.h', - 'ephysics/mathematics/Vector2.h', 'ephysics/mathematics/mathematics_functions.h', - 'ephysics/mathematics/Vector3.h', - 'ephysics/mathematics/Quaternion.h', - 'ephysics/mathematics/Transform.h', - 'ephysics/mathematics/Matrix3x3.h', 'ephysics/engine/CollisionWorld.h', 'ephysics/engine/DynamicsWorld.h', 'ephysics/engine/ConstraintSolver.h', diff --git a/test/main.cpp b/test/main.cpp index 5cfea4f..f970874 100644 --- a/test/main.cpp +++ b/test/main.cpp @@ -25,12 +25,12 @@ // Libraries #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -46,12 +46,12 @@ int32_t main() { // ---------- Mathematics tests ---------- // - testSuite.addTest(new TestVector2("Vector2")); - testSuite.addTest(new TestVector3("Vector3")); - testSuite.addTest(new TestTransform("Transform")); - testSuite.addTest(new TestQuaternion("Quaternion")); - testSuite.addTest(new TestMatrix3x3("Matrix3x3")); - testSuite.addTest(new TestMatrix2x2("Matrix2x2")); + testSuite.addTest(new Testvec2("vec2")); + testSuite.addTest(new Testvec3("vec3")); + testSuite.addTest(new Testetk::Transform3D("Transform")); + testSuite.addTest(new Testetk::Quaternion("Quaternion")); + testSuite.addTest(new Testetk::Matrix3x3("Matrix3x3")); + testSuite.addTest(new Testetk::Matrix2x2("Matrix2x2")); testSuite.addTest(new TestMathematicsFunctions("Maths Functions")); // ---------- Collision Detection tests ---------- // diff --git a/test/tests/collision/TestAABB.h b/test/tests/collision/TestAABB.h index 736d6bd..ab719d7 100644 --- a/test/tests/collision/TestAABB.h +++ b/test/tests/collision/TestAABB.h @@ -43,10 +43,10 @@ class TestAABB : public Test { // ---------- Atributes ---------- // - AABB mAABB1; - AABB mAABB2; - AABB mAABB3; - AABB mAABB4; + AABB m_AABB1; + AABB m_AABB2; + AABB m_AABB3; + AABB m_AABB4; public : @@ -55,20 +55,20 @@ class TestAABB : public Test { /// Constructor TestAABB(const std::string& name) : Test(name) { - mAABB1.setMin(Vector3(-10, -10, -10)); - mAABB1.setMax(Vector3(10, 10, 10)); + m_AABB1.setMin(vec3(-10, -10, -10)); + m_AABB1.setMax(vec3(10, 10, 10)); // AABB2 int32_tersect with AABB1 - mAABB2.setMin(Vector3(-5, 4, -30)); - mAABB2.setMax(Vector3(-2, 20, 30)); + m_AABB2.setMin(vec3(-5, 4, -30)); + m_AABB2.setMax(vec3(-2, 20, 30)); // AABB3 contains AABB1 - mAABB3.setMin(Vector3(-25, -25, -25)); - mAABB3.setMax(Vector3(25, 25, 25)); + m_AABB3.setMin(vec3(-25, -25, -25)); + m_AABB3.setMax(vec3(25, 25, 25)); // AABB4 does not collide with AABB1 - mAABB4.setMin(Vector3(-40, -40, -40)); - mAABB4.setMax(Vector3(-15, -25, -12)); + m_AABB4.setMin(vec3(-40, -40, -40)); + m_AABB4.setMax(vec3(-15, -25, -12)); } /// Destructor @@ -87,201 +87,201 @@ class TestAABB : public Test { // -------- Test constructors -------- // AABB aabb1; - AABB aabb2(Vector3(-3, -5, -8), Vector3(65, -1, 56)); - Vector3 trianglePoints[] = { - Vector3(-5, 7, 23), Vector3(45, -34, -73), Vector3(-12, 98, 76) + AABB aabb2(vec3(-3, -5, -8), vec3(65, -1, 56)); + vec3 trianglePoints[] = { + vec3(-5, 7, 23), vec3(45, -34, -73), vec3(-12, 98, 76) }; AABB aabb3 = AABB::createAABBForTriangle(trianglePoints); - test(aabb1.getMin().x == 0); - test(aabb1.getMin().y == 0); - test(aabb1.getMin().z == 0); - test(aabb1.getMax().x == 0); - test(aabb1.getMax().y == 0); - test(aabb1.getMax().z == 0); + test(aabb1.getMin().x() == 0); + test(aabb1.getMin().y() == 0); + test(aabb1.getMin().z() == 0); + test(aabb1.getMax().x() == 0); + test(aabb1.getMax().y() == 0); + test(aabb1.getMax().z() == 0); - test(aabb2.getMin().x == -3); - test(aabb2.getMin().y == -5); - test(aabb2.getMin().z == -8); - test(aabb2.getMax().x == 65); - test(aabb2.getMax().y == -1); - test(aabb2.getMax().z == 56); + test(aabb2.getMin().x() == -3); + test(aabb2.getMin().y() == -5); + test(aabb2.getMin().z() == -8); + test(aabb2.getMax().x() == 65); + test(aabb2.getMax().y() == -1); + test(aabb2.getMax().z() == 56); - test(aabb3.getMin().x == -12); - test(aabb3.getMin().y == -34); - test(aabb3.getMin().z == -73); - test(aabb3.getMax().x == 45); - test(aabb3.getMax().y == 98); - test(aabb3.getMax().z == 76); + test(aabb3.getMin().x() == -12); + test(aabb3.getMin().y() == -34); + test(aabb3.getMin().z() == -73); + test(aabb3.getMax().x() == 45); + test(aabb3.getMax().y() == 98); + test(aabb3.getMax().z() == 76); // -------- Test inflate() -------- // - AABB aabbInflate(Vector3(-3, 4, 8), Vector3(-1, 6, 32)); + AABB aabbInflate(vec3(-3, 4, 8), vec3(-1, 6, 32)); aabbInflate.inflate(1, 2, 3); - test(approxEqual(aabbInflate.getMin().x, -4, 0.00001)); - test(approxEqual(aabbInflate.getMin().y, 2, 0.00001)); - test(approxEqual(aabbInflate.getMin().z, 5, 0.00001)); - test(approxEqual(aabbInflate.getMax().x, 0, 0.00001)); - test(approxEqual(aabbInflate.getMax().y, 8, 0.00001)); - test(approxEqual(aabbInflate.getMax().z, 35, 0.00001)); + test(approxEqual(aabbInflate.getMin().x(), -4, 0.00001)); + test(approxEqual(aabbInflate.getMin().y(), 2, 0.00001)); + test(approxEqual(aabbInflate.getMin().z(), 5, 0.00001)); + test(approxEqual(aabbInflate.getMax().x(), 0, 0.00001)); + test(approxEqual(aabbInflate.getMax().y(), 8, 0.00001)); + test(approxEqual(aabbInflate.getMax().z(), 35, 0.00001)); // -------- Test getExtent() --------- // - test(approxEqual(mAABB1.getExtent().x, 20)); - test(approxEqual(mAABB1.getExtent().y, 20)); - test(approxEqual(mAABB1.getExtent().z, 20)); + test(approxEqual(m_AABB1.getExtent().x(), 20)); + test(approxEqual(m_AABB1.getExtent().y(), 20)); + test(approxEqual(m_AABB1.getExtent().z(), 20)); - test(approxEqual(mAABB2.getExtent().x, 3)); - test(approxEqual(mAABB2.getExtent().y, 16)); - test(approxEqual(mAABB2.getExtent().z, 60)); + test(approxEqual(m_AABB2.getExtent().x(), 3)); + test(approxEqual(m_AABB2.getExtent().y(), 16)); + test(approxEqual(m_AABB2.getExtent().z(), 60)); - test(approxEqual(mAABB3.getExtent().x, 50)); - test(approxEqual(mAABB3.getExtent().y, 50)); - test(approxEqual(mAABB3.getExtent().z, 50)); + test(approxEqual(m_AABB3.getExtent().x(), 50)); + test(approxEqual(m_AABB3.getExtent().y(), 50)); + test(approxEqual(m_AABB3.getExtent().z(), 50)); // -------- Test getCenter() -------- // - test(mAABB1.getCenter().x == 0); - test(mAABB1.getCenter().y == 0); - test(mAABB1.getCenter().z == 0); + test(m_AABB1.getCenter().x() == 0); + test(m_AABB1.getCenter().y() == 0); + test(m_AABB1.getCenter().z() == 0); - test(approxEqual(mAABB2.getCenter().x, -3.5)); - test(approxEqual(mAABB2.getCenter().y, 12)); - test(approxEqual(mAABB2.getCenter().z, 0)); + test(approxEqual(m_AABB2.getCenter().x(), -3.5)); + test(approxEqual(m_AABB2.getCenter().y(), 12)); + test(approxEqual(m_AABB2.getCenter().z(), 0)); // -------- Test setMin(), setMax(), getMin(), getMax() -------- // AABB aabb5; - aabb5.setMin(Vector3(-12, 34, 6)); - aabb5.setMax(Vector3(-3, 56, 20)); + aabb5.setMin(vec3(-12, 34, 6)); + aabb5.setMax(vec3(-3, 56, 20)); - test(aabb5.getMin().x == -12); - test(aabb5.getMin().y == 34); - test(aabb5.getMin().z == 6); - test(aabb5.getMax().x == -3); - test(aabb5.getMax().y == 56); - test(aabb5.getMax().z == 20); + test(aabb5.getMin().x() == -12); + test(aabb5.getMin().y() == 34); + test(aabb5.getMin().z() == 6); + test(aabb5.getMax().x() == -3); + test(aabb5.getMax().y() == 56); + test(aabb5.getMax().z() == 20); // -------- Test assignment operator -------- // AABB aabb6; aabb6 = aabb2; - test(aabb6.getMin().x == -3); - test(aabb6.getMin().y == -5); - test(aabb6.getMin().z == -8); - test(aabb6.getMax().x == 65); - test(aabb6.getMax().y == -1); - test(aabb6.getMax().z == 56); + test(aabb6.getMin().x() == -3); + test(aabb6.getMin().y() == -5); + test(aabb6.getMin().z() == -8); + test(aabb6.getMax().x() == 65); + test(aabb6.getMax().y() == -1); + test(aabb6.getMax().z() == 56); // -------- Test getVolume() -------- // - test(approxEqual(mAABB1.getVolume(), 8000)); - test(approxEqual(mAABB2.getVolume(), 2880)); + test(approxEqual(m_AABB1.getVolume(), 8000)); + test(approxEqual(m_AABB2.getVolume(), 2880)); } void testMergeMethods() { - AABB aabb1(Vector3(-45, 7, -2), Vector3(23, 8, 1)); - AABB aabb2(Vector3(-15, 6, 23), Vector3(-5, 9, 45)); + AABB aabb1(vec3(-45, 7, -2), vec3(23, 8, 1)); + AABB aabb2(vec3(-15, 6, 23), vec3(-5, 9, 45)); // -------- Test mergeTwoAABBs() -------- // AABB aabb3; - aabb3.mergeTwoAABBs(aabb1, mAABB1); + aabb3.mergeTwoAABBs(aabb1, m_AABB1); - test(aabb3.getMin().x == -45); - test(aabb3.getMin().y == -10); - test(aabb3.getMin().z == -10); - test(aabb3.getMax().x == 23); - test(aabb3.getMax().y == 10); - test(aabb3.getMax().z == 10); + test(aabb3.getMin().x() == -45); + test(aabb3.getMin().y() == -10); + test(aabb3.getMin().z() == -10); + test(aabb3.getMax().x() == 23); + test(aabb3.getMax().y() == 10); + test(aabb3.getMax().z() == 10); AABB aabb4; aabb4.mergeTwoAABBs(aabb1, aabb2); - test(aabb4.getMin().x == -45); - test(aabb4.getMin().y == 6); - test(aabb4.getMin().z == -2); - test(aabb4.getMax().x == 23); - test(aabb4.getMax().y == 9); - test(aabb4.getMax().z == 45); + test(aabb4.getMin().x() == -45); + test(aabb4.getMin().y() == 6); + test(aabb4.getMin().z() == -2); + test(aabb4.getMax().x() == 23); + test(aabb4.getMax().y() == 9); + test(aabb4.getMax().z() == 45); // -------- Test mergeWithAABB() -------- // - aabb1.mergeWithAABB(mAABB1); + aabb1.mergeWithAABB(m_AABB1); - test(aabb1.getMin().x == -45); - test(aabb1.getMin().y == -10); - test(aabb1.getMin().z == -10); - test(aabb1.getMax().x == 23); - test(aabb1.getMax().y == 10); - test(aabb1.getMax().z == 10); + test(aabb1.getMin().x() == -45); + test(aabb1.getMin().y() == -10); + test(aabb1.getMin().z() == -10); + test(aabb1.getMax().x() == 23); + test(aabb1.getMax().y() == 10); + test(aabb1.getMax().z() == 10); - aabb2.mergeWithAABB(mAABB1); + aabb2.mergeWithAABB(m_AABB1); - test(aabb2.getMin().x == -15); - test(aabb2.getMin().y == -10); - test(aabb2.getMin().z == -10); - test(aabb2.getMax().x == 10); - test(aabb2.getMax().y == 10); - test(aabb2.getMax().z == 45); + test(aabb2.getMin().x() == -15); + test(aabb2.getMin().y() == -10); + test(aabb2.getMin().z() == -10); + test(aabb2.getMax().x() == 10); + test(aabb2.getMax().y() == 10); + test(aabb2.getMax().z() == 45); } void testIntersection() { // -------- Test contains(AABB) -------- // - test(!mAABB1.contains(mAABB2)); - test(mAABB3.contains(mAABB1)); - test(!mAABB1.contains(mAABB3)); - test(!mAABB1.contains(mAABB4)); - test(!mAABB4.contains(mAABB1)); + test(!m_AABB1.contains(m_AABB2)); + test(m_AABB3.contains(m_AABB1)); + test(!m_AABB1.contains(m_AABB3)); + test(!m_AABB1.contains(m_AABB4)); + test(!m_AABB4.contains(m_AABB1)); - // -------- Test contains(Vector3) -------- // - test(mAABB1.contains(Vector3(0, 0, 0))); - test(mAABB1.contains(Vector3(-5, 6, 9))); - test(mAABB1.contains(Vector3(-9, -4, -9))); - test(mAABB1.contains(Vector3(9, 4, 7))); - test(!mAABB1.contains(Vector3(-11, -4, -9))); - test(!mAABB1.contains(Vector3(1, 12, -9))); - test(!mAABB1.contains(Vector3(1, 8, -13))); - test(!mAABB1.contains(Vector3(-14, 82, -13))); + // -------- Test contains(vec3) -------- // + test(m_AABB1.contains(vec3(0, 0, 0))); + test(m_AABB1.contains(vec3(-5, 6, 9))); + test(m_AABB1.contains(vec3(-9, -4, -9))); + test(m_AABB1.contains(vec3(9, 4, 7))); + test(!m_AABB1.contains(vec3(-11, -4, -9))); + test(!m_AABB1.contains(vec3(1, 12, -9))); + test(!m_AABB1.contains(vec3(1, 8, -13))); + test(!m_AABB1.contains(vec3(-14, 82, -13))); // -------- Test testCollision() -------- // - test(mAABB1.testCollision(mAABB2)); - test(mAABB2.testCollision(mAABB1)); - test(mAABB1.testCollision(mAABB3)); - test(mAABB3.testCollision(mAABB1)); - test(!mAABB1.testCollision(mAABB4)); - test(!mAABB4.testCollision(mAABB1)); + test(m_AABB1.testCollision(m_AABB2)); + test(m_AABB2.testCollision(m_AABB1)); + test(m_AABB1.testCollision(m_AABB3)); + test(m_AABB3.testCollision(m_AABB1)); + test(!m_AABB1.testCollision(m_AABB4)); + test(!m_AABB4.testCollision(m_AABB1)); // -------- Test testCollisionTriangleAABB() -------- // - AABB aabb(Vector3(100, 100, 100), Vector3(200, 200, 200)); - Vector3 trianglePoints[] = { - Vector3(-2, 4, 6), Vector3(20, -34, -73), Vector3(-12, 98, 76) + AABB aabb(vec3(100, 100, 100), vec3(200, 200, 200)); + vec3 trianglePoints[] = { + vec3(-2, 4, 6), vec3(20, -34, -73), vec3(-12, 98, 76) }; - test(mAABB1.testCollisionTriangleAABB(trianglePoints)); + test(m_AABB1.testCollisionTriangleAABB(trianglePoints)); test(!aabb.testCollisionTriangleAABB(trianglePoints)); // -------- Test testRayIntersect() -------- // - Ray ray1(Vector3(-20, 4, -7), Vector3(20, 4, -7)); - Ray ray2(Vector3(-20, 11, -7), Vector3(20, 11, -7)); - Ray ray3(Vector3(0, 15, 0), Vector3(0, -15, 0)); - Ray ray4(Vector3(0, -15, 0), Vector3(0, 15, 0)); - Ray ray5(Vector3(-3, 4, 8), Vector3(-7, 9, 4)); - Ray ray6(Vector3(-4, 6, -100), Vector3(-4, 6, -9)); - Ray ray7(Vector3(-4, 6, -100), Vector3(-4, 6, -11), 0.6); - Ray ray8(Vector3(-403, -432, -100), Vector3(134, 643, 23)); + Ray ray1(vec3(-20, 4, -7), vec3(20, 4, -7)); + Ray ray2(vec3(-20, 11, -7), vec3(20, 11, -7)); + Ray ray3(vec3(0, 15, 0), vec3(0, -15, 0)); + Ray ray4(vec3(0, -15, 0), vec3(0, 15, 0)); + Ray ray5(vec3(-3, 4, 8), vec3(-7, 9, 4)); + Ray ray6(vec3(-4, 6, -100), vec3(-4, 6, -9)); + Ray ray7(vec3(-4, 6, -100), vec3(-4, 6, -11), 0.6); + Ray ray8(vec3(-403, -432, -100), vec3(134, 643, 23)); - test(mAABB1.testRayIntersect(ray1)); - test(!mAABB1.testRayIntersect(ray2)); - test(mAABB1.testRayIntersect(ray3)); - test(mAABB1.testRayIntersect(ray4)); - test(mAABB1.testRayIntersect(ray5)); - test(mAABB1.testRayIntersect(ray6)); - test(!mAABB1.testRayIntersect(ray7)); - test(!mAABB1.testRayIntersect(ray8)); + test(m_AABB1.testRayIntersect(ray1)); + test(!m_AABB1.testRayIntersect(ray2)); + test(m_AABB1.testRayIntersect(ray3)); + test(m_AABB1.testRayIntersect(ray4)); + test(m_AABB1.testRayIntersect(ray5)); + test(m_AABB1.testRayIntersect(ray6)); + test(!m_AABB1.testRayIntersect(ray7)); + test(!m_AABB1.testRayIntersect(ray8)); } }; diff --git a/test/tests/collision/TestCollisionWorld.h b/test/tests/collision/TestCollisionWorld.h index 6b5b644..e4d5761 100644 --- a/test/tests/collision/TestCollisionWorld.h +++ b/test/tests/collision/TestCollisionWorld.h @@ -139,24 +139,24 @@ class TestCollisionWorld : public Test { m_world = new CollisionWorld(); // Create the bodies - Transform boxTransform(Vector3(10, 0, 0), Quaternion::identity()); - mBoxBody = m_world->createCollisionBody(boxTransform); - mBoxShape = new BoxShape(Vector3(3, 3, 3)); - mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, Transform::identity()); + etk::Transform3D boxTransform(vec3(10, 0, 0), etk::Quaternion::identity()); + mBoxBody = m_world->createCollisionBody(boxetk::Transform3D); + mBoxShape = new BoxShape(vec3(3, 3, 3)); + mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, etk::Transform3D::identity()); mSphereShape = new SphereShape(3.0); - Transform sphere1Transform(Vector3(10,5, 0), Quaternion::identity()); - mSphere1Body = m_world->createCollisionBody(sphere1Transform); - mSphere1ProxyShape = mSphere1Body->addCollisionShape(mSphereShape, Transform::identity()); + etk::Transform3D sphere1Transform(vec3(10,5, 0), etk::Quaternion::identity()); + mSphere1Body = m_world->createCollisionBody(sphere1etk::Transform3D); + mSphere1ProxyShape = mSphere1Body->addCollisionShape(mSphereShape, etk::Transform3D::identity()); - Transform sphere2Transform(Vector3(30, 10, 10), Quaternion::identity()); - mSphere2Body = m_world->createCollisionBody(sphere2Transform); - mSphere2ProxyShape = mSphere2Body->addCollisionShape(mSphereShape, Transform::identity()); + etk::Transform3D sphere2Transform(vec3(30, 10, 10), etk::Quaternion::identity()); + mSphere2Body = m_world->createCollisionBody(sphere2etk::Transform3D); + mSphere2ProxyShape = mSphere2Body->addCollisionShape(mSphereShape, etk::Transform3D::identity()); - Transform cylinderTransform(Vector3(10, -5, 0), Quaternion::identity()); - mCylinderBody = m_world->createCollisionBody(cylinderTransform); + etk::Transform3D cylinderTransform(vec3(10, -5, 0), etk::Quaternion::identity()); + mCylinderBody = m_world->createCollisionBody(cylinderetk::Transform3D); mCylinderShape = new CylinderShape(2, 5); - mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, Transform::identity()); + mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, etk::Transform3D::identity()); // Assign collision categories to proxy shapes mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1); @@ -238,7 +238,7 @@ class TestCollisionWorld : public Test { test(!m_collisionCallback.sphere1CollideWithSphere2); // Move sphere 1 to collide with sphere 2 - mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); + mSphere1Body->setTransform(Transform(vec3(30, 15, 10), etk::Quaternion::identity())); m_collisionCallback.reset(); m_world->testCollision(&m_collisionCallback); @@ -262,7 +262,7 @@ class TestCollisionWorld : public Test { test(!m_collisionCallback.sphere1CollideWithSphere2); // Move sphere 1 to collide with box - mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); + mSphere1Body->setTransform(Transform(vec3(10, 5, 0), etk::Quaternion::identity())); // --------- Test collision with inactive bodies --------- // @@ -307,7 +307,7 @@ class TestCollisionWorld : public Test { test(!m_collisionCallback.sphere1CollideWithSphere2); // Move sphere 1 to collide with sphere 2 - mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); + mSphere1Body->setTransform(Transform(vec3(30, 15, 10), etk::Quaternion::identity())); m_collisionCallback.reset(); m_world->testCollision(&m_collisionCallback); @@ -329,7 +329,7 @@ class TestCollisionWorld : public Test { test(!m_collisionCallback.sphere1CollideWithSphere2); // Move sphere 1 to collide with box - mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); + mSphere1Body->setTransform(Transform(vec3(10, 5, 0), etk::Quaternion::identity())); mBoxProxyShape->setCollideWithMaskBits(0xFFFF); mSphere1ProxyShape->setCollideWithMaskBits(0xFFFF); diff --git a/test/tests/collision/TestDynamicAABBTree.h b/test/tests/collision/TestDynamicAABBTree.h index 21ce604..57b82b5 100644 --- a/test/tests/collision/TestDynamicAABBTree.h +++ b/test/tests/collision/TestDynamicAABBTree.h @@ -123,31 +123,31 @@ class TestDynamicAABBTree : public Test { int32_t object4Data = 7; // First object - AABB aabb1 = AABB(Vector3(-6, 4, -3), Vector3(4, 8, 3)); + AABB aabb1 = AABB(vec3(-6, 4, -3), vec3(4, 8, 3)); int32_t object1Id = tree.addObject(aabb1, &object1Data); // Second object - AABB aabb2 = AABB(Vector3(5, 2, -3), Vector3(10, 7, 3)); + AABB aabb2 = AABB(vec3(5, 2, -3), vec3(10, 7, 3)); int32_t object2Id = tree.addObject(aabb2, &object2Data); // Third object - AABB aabb3 = AABB(Vector3(-5, 1, -3), Vector3(-2, 3, 3)); + AABB aabb3 = AABB(vec3(-5, 1, -3), vec3(-2, 3, 3)); int32_t object3Id = tree.addObject(aabb3, &object3Data); // Fourth object - AABB aabb4 = AABB(Vector3(0, -4, -3), Vector3(3, -2, 3)); + AABB aabb4 = AABB(vec3(0, -4, -3), vec3(3, -2, 3)); int32_t object4Id = tree.addObject(aabb4, &object4Data); // ----------- Tests ----------- // // Test root AABB AABB rootAABB = tree.getRootAABB(); - test(rootAABB.getMin().x == -6); - test(rootAABB.getMin().y == -4); - test(rootAABB.getMin().z == -3); - test(rootAABB.getMax().x == 10); - test(rootAABB.getMax().y == 8); - test(rootAABB.getMax().z == 3); + test(rootAABB.getMin().x() == -6); + test(rootAABB.getMin().y() == -4); + test(rootAABB.getMin().z() == -3); + test(rootAABB.getMax().x() == 10); + test(rootAABB.getMax().y() == 8); + test(rootAABB.getMax().z() == 3); // Test data stored at the nodes of the tree test(*(int32_t*)(tree.getNodeDataPointer(object1Id)) == object1Data); @@ -169,26 +169,26 @@ class TestDynamicAABBTree : public Test { int32_t object4Data = 7; // First object - AABB aabb1 = AABB(Vector3(-6, 4, -3), Vector3(4, 8, 3)); + AABB aabb1 = AABB(vec3(-6, 4, -3), vec3(4, 8, 3)); int32_t object1Id = tree.addObject(aabb1, &object1Data); // Second object - AABB aabb2 = AABB(Vector3(5, 2, -3), Vector3(10, 7, 3)); + AABB aabb2 = AABB(vec3(5, 2, -3), vec3(10, 7, 3)); int32_t object2Id = tree.addObject(aabb2, &object2Data); // Third object - AABB aabb3 = AABB(Vector3(-5, 1, -3), Vector3(-2, 3, 3)); + AABB aabb3 = AABB(vec3(-5, 1, -3), vec3(-2, 3, 3)); int32_t object3Id = tree.addObject(aabb3, &object3Data); // Fourth object - AABB aabb4 = AABB(Vector3(0, -4, -3), Vector3(3, -2, 3)); + AABB aabb4 = AABB(vec3(0, -4, -3), vec3(3, -2, 3)); int32_t object4Id = tree.addObject(aabb4, &object4Data); // ---------- Tests ---------- // // AABB overlapping nothing mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-10, 12, -4), Vector3(10, 50, 4)), mOverlapCallback); + tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-10, 12, -4), vec3(10, 50, 4)), mOverlapCallback); test(!mOverlapCallback.isOverlapping(object1Id)); test(!mOverlapCallback.isOverlapping(object2Id)); test(!mOverlapCallback.isOverlapping(object3Id)); @@ -196,7 +196,7 @@ class TestDynamicAABBTree : public Test { // AABB overlapping everything mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-15, -15, -4), Vector3(15, 15, 4)), mOverlapCallback); + tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-15, -15, -4), vec3(15, 15, 4)), mOverlapCallback); test(mOverlapCallback.isOverlapping(object1Id)); test(mOverlapCallback.isOverlapping(object2Id)); test(mOverlapCallback.isOverlapping(object3Id)); @@ -204,7 +204,7 @@ class TestDynamicAABBTree : public Test { // AABB overlapping object 1 and 3 mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-4, 2, -4), Vector3(-1, 7, 4)), mOverlapCallback); + tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-4, 2, -4), vec3(-1, 7, 4)), mOverlapCallback); test(mOverlapCallback.isOverlapping(object1Id)); test(!mOverlapCallback.isOverlapping(object2Id)); test(mOverlapCallback.isOverlapping(object3Id)); @@ -212,7 +212,7 @@ class TestDynamicAABBTree : public Test { // AABB overlapping object 3 and 4 mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-6, -5, -2), Vector3(2, 2, 0)), mOverlapCallback); + tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-6, -5, -2), vec3(2, 2, 0)), mOverlapCallback); test(!mOverlapCallback.isOverlapping(object1Id)); test(!mOverlapCallback.isOverlapping(object2Id)); test(mOverlapCallback.isOverlapping(object3Id)); @@ -220,7 +220,7 @@ class TestDynamicAABBTree : public Test { // AABB overlapping object 2 mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(5, -10, -2), Vector3(7, 10, 9)), mOverlapCallback); + tree.reportAllShapesOverlappingWithAABB(AABB(vec3(5, -10, -2), vec3(7, 10, 9)), mOverlapCallback); test(!mOverlapCallback.isOverlapping(object1Id)); test(mOverlapCallback.isOverlapping(object2Id)); test(!mOverlapCallback.isOverlapping(object3Id)); @@ -228,14 +228,14 @@ class TestDynamicAABBTree : public Test { // ---- Update the object AABBs with the initial AABBs (no reinsertion) ----- // - tree.updateObject(object1Id, aabb1, Vector3::zero(), false); - tree.updateObject(object2Id, aabb2, Vector3::zero(), false); - tree.updateObject(object3Id, aabb3, Vector3::zero(), false); - tree.updateObject(object4Id, aabb4, Vector3::zero(), false); + tree.updateObject(object1Id, aabb1, vec3::zero(), false); + tree.updateObject(object2Id, aabb2, vec3::zero(), false); + tree.updateObject(object3Id, aabb3, vec3::zero(), false); + tree.updateObject(object4Id, aabb4, vec3::zero(), false); // AABB overlapping nothing mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-10, 12, -4), Vector3(10, 50, 4)), mOverlapCallback); + tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-10, 12, -4), vec3(10, 50, 4)), mOverlapCallback); test(!mOverlapCallback.isOverlapping(object1Id)); test(!mOverlapCallback.isOverlapping(object2Id)); test(!mOverlapCallback.isOverlapping(object3Id)); @@ -243,7 +243,7 @@ class TestDynamicAABBTree : public Test { // AABB overlapping everything mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-15, -15, -4), Vector3(15, 15, 4)), mOverlapCallback); + tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-15, -15, -4), vec3(15, 15, 4)), mOverlapCallback); test(mOverlapCallback.isOverlapping(object1Id)); test(mOverlapCallback.isOverlapping(object2Id)); test(mOverlapCallback.isOverlapping(object3Id)); @@ -251,7 +251,7 @@ class TestDynamicAABBTree : public Test { // AABB overlapping object 1 and 3 mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-4, 2, -4), Vector3(-1, 7, 4)), mOverlapCallback); + tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-4, 2, -4), vec3(-1, 7, 4)), mOverlapCallback); test(mOverlapCallback.isOverlapping(object1Id)); test(!mOverlapCallback.isOverlapping(object2Id)); test(mOverlapCallback.isOverlapping(object3Id)); @@ -259,7 +259,7 @@ class TestDynamicAABBTree : public Test { // AABB overlapping object 3 and 4 mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-6, -5, -2), Vector3(2, 2, 0)), mOverlapCallback); + tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-6, -5, -2), vec3(2, 2, 0)), mOverlapCallback); test(!mOverlapCallback.isOverlapping(object1Id)); test(!mOverlapCallback.isOverlapping(object2Id)); test(mOverlapCallback.isOverlapping(object3Id)); @@ -267,7 +267,7 @@ class TestDynamicAABBTree : public Test { // AABB overlapping object 2 mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(5, -10, -2), Vector3(7, 10, 9)), mOverlapCallback); + tree.reportAllShapesOverlappingWithAABB(AABB(vec3(5, -10, -2), vec3(7, 10, 9)), mOverlapCallback); test(!mOverlapCallback.isOverlapping(object1Id)); test(mOverlapCallback.isOverlapping(object2Id)); test(!mOverlapCallback.isOverlapping(object3Id)); @@ -275,14 +275,14 @@ class TestDynamicAABBTree : public Test { // ---- Update the object AABBs with the initial AABBs (with reinsertion) ----- // - tree.updateObject(object1Id, aabb1, Vector3::zero(), true); - tree.updateObject(object2Id, aabb2, Vector3::zero(), true); - tree.updateObject(object3Id, aabb3, Vector3::zero(), true); - tree.updateObject(object4Id, aabb4, Vector3::zero(), true); + tree.updateObject(object1Id, aabb1, vec3::zero(), true); + tree.updateObject(object2Id, aabb2, vec3::zero(), true); + tree.updateObject(object3Id, aabb3, vec3::zero(), true); + tree.updateObject(object4Id, aabb4, vec3::zero(), true); // AABB overlapping nothing mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-10, 12, -4), Vector3(10, 50, 4)), mOverlapCallback); + tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-10, 12, -4), vec3(10, 50, 4)), mOverlapCallback); test(!mOverlapCallback.isOverlapping(object1Id)); test(!mOverlapCallback.isOverlapping(object2Id)); test(!mOverlapCallback.isOverlapping(object3Id)); @@ -290,7 +290,7 @@ class TestDynamicAABBTree : public Test { // AABB overlapping everything mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-15, -15, -4), Vector3(15, 15, 4)), mOverlapCallback); + tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-15, -15, -4), vec3(15, 15, 4)), mOverlapCallback); test(mOverlapCallback.isOverlapping(object1Id)); test(mOverlapCallback.isOverlapping(object2Id)); test(mOverlapCallback.isOverlapping(object3Id)); @@ -298,7 +298,7 @@ class TestDynamicAABBTree : public Test { // AABB overlapping object 1 and 3 mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-4, 2, -4), Vector3(-1, 7, 4)), mOverlapCallback); + tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-4, 2, -4), vec3(-1, 7, 4)), mOverlapCallback); test(mOverlapCallback.isOverlapping(object1Id)); test(!mOverlapCallback.isOverlapping(object2Id)); test(mOverlapCallback.isOverlapping(object3Id)); @@ -306,7 +306,7 @@ class TestDynamicAABBTree : public Test { // AABB overlapping object 3 and 4 mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-6, -5, -2), Vector3(2, 2, 0)), mOverlapCallback); + tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-6, -5, -2), vec3(2, 2, 0)), mOverlapCallback); test(!mOverlapCallback.isOverlapping(object1Id)); test(!mOverlapCallback.isOverlapping(object2Id)); test(mOverlapCallback.isOverlapping(object3Id)); @@ -314,7 +314,7 @@ class TestDynamicAABBTree : public Test { // AABB overlapping object 2 mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(5, -10, -2), Vector3(7, 10, 9)), mOverlapCallback); + tree.reportAllShapesOverlappingWithAABB(AABB(vec3(5, -10, -2), vec3(7, 10, 9)), mOverlapCallback); test(!mOverlapCallback.isOverlapping(object1Id)); test(mOverlapCallback.isOverlapping(object2Id)); test(!mOverlapCallback.isOverlapping(object3Id)); @@ -322,15 +322,15 @@ class TestDynamicAABBTree : public Test { // ---- Move objects 2 and 3 ----- // - AABB newAABB2(Vector3(-7, 10, -3), Vector3(1, 13, 3)); - tree.updateObject(object2Id, newAABB2, Vector3::zero()); + AABB newAABB2(vec3(-7, 10, -3), vec3(1, 13, 3)); + tree.updateObject(object2Id, newAABB2, vec3::zero()); - AABB newAABB3(Vector3(7, -6, -3), Vector3(9, 1, 3)); - tree.updateObject(object3Id, newAABB3, Vector3::zero()); + AABB newAABB3(vec3(7, -6, -3), vec3(9, 1, 3)); + tree.updateObject(object3Id, newAABB3, vec3::zero()); // AABB overlapping object 3 mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(6, -10, -2), Vector3(8, 5, 3)), mOverlapCallback); + tree.reportAllShapesOverlappingWithAABB(AABB(vec3(6, -10, -2), vec3(8, 5, 3)), mOverlapCallback); test(!mOverlapCallback.isOverlapping(object1Id)); test(!mOverlapCallback.isOverlapping(object2Id)); test(mOverlapCallback.isOverlapping(object3Id)); @@ -338,7 +338,7 @@ class TestDynamicAABBTree : public Test { // AABB overlapping objects 1, 2 mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-8, 5, -3), Vector3(-2, 11, 3)), mOverlapCallback); + tree.reportAllShapesOverlappingWithAABB(AABB(vec3(-8, 5, -3), vec3(-2, 11, 3)), mOverlapCallback); test(mOverlapCallback.isOverlapping(object1Id)); test(mOverlapCallback.isOverlapping(object2Id)); test(!mOverlapCallback.isOverlapping(object3Id)); @@ -359,26 +359,26 @@ class TestDynamicAABBTree : public Test { int32_t object4Data = 7; // First object - AABB aabb1 = AABB(Vector3(-6, 4, -3), Vector3(4, 8, 3)); + AABB aabb1 = AABB(vec3(-6, 4, -3), vec3(4, 8, 3)); int32_t object1Id = tree.addObject(aabb1, &object1Data); // Second object - AABB aabb2 = AABB(Vector3(5, 2, -3), Vector3(10, 7, 3)); + AABB aabb2 = AABB(vec3(5, 2, -3), vec3(10, 7, 3)); int32_t object2Id = tree.addObject(aabb2, &object2Data); // Third object - AABB aabb3 = AABB(Vector3(-5, 1, -3), Vector3(-2, 3, 3)); + AABB aabb3 = AABB(vec3(-5, 1, -3), vec3(-2, 3, 3)); int32_t object3Id = tree.addObject(aabb3, &object3Data); // Fourth object - AABB aabb4 = AABB(Vector3(0, -4, -3), Vector3(3, -2, 3)); + AABB aabb4 = AABB(vec3(0, -4, -3), vec3(3, -2, 3)); int32_t object4Id = tree.addObject(aabb4, &object4Data); // ---------- Tests ---------- // // Ray with no hits m_raycastCallback.reset(); - Ray ray1(Vector3(4.5, -10, -5), Vector3(4.5, 10, -5)); + Ray ray1(vec3(4.5, -10, -5), vec3(4.5, 10, -5)); tree.raycast(ray1, m_raycastCallback); test(!m_raycastCallback.isHit(object1Id)); test(!m_raycastCallback.isHit(object2Id)); @@ -387,7 +387,7 @@ class TestDynamicAABBTree : public Test { // Ray that hits object 1 m_raycastCallback.reset(); - Ray ray2(Vector3(-1, -20, -2), Vector3(-1, 20, -2)); + Ray ray2(vec3(-1, -20, -2), vec3(-1, 20, -2)); tree.raycast(ray2, m_raycastCallback); test(m_raycastCallback.isHit(object1Id)); test(!m_raycastCallback.isHit(object2Id)); @@ -396,7 +396,7 @@ class TestDynamicAABBTree : public Test { // Ray that hits object 1 and 2 m_raycastCallback.reset(); - Ray ray3(Vector3(-7, 6, -2), Vector3(8, 6, -2)); + Ray ray3(vec3(-7, 6, -2), vec3(8, 6, -2)); tree.raycast(ray3, m_raycastCallback); test(m_raycastCallback.isHit(object1Id)); test(m_raycastCallback.isHit(object2Id)); @@ -405,7 +405,7 @@ class TestDynamicAABBTree : public Test { // Ray that hits object 3 m_raycastCallback.reset(); - Ray ray4(Vector3(-7, 2, 0), Vector3(-1, 2, 0)); + Ray ray4(vec3(-7, 2, 0), vec3(-1, 2, 0)); tree.raycast(ray4, m_raycastCallback); test(!m_raycastCallback.isHit(object1Id)); test(!m_raycastCallback.isHit(object2Id)); @@ -414,10 +414,10 @@ class TestDynamicAABBTree : public Test { // ---- Update the object AABBs with the initial AABBs (no reinsertion) ----- // - tree.updateObject(object1Id, aabb1, Vector3::zero(), false); - tree.updateObject(object2Id, aabb2, Vector3::zero(), false); - tree.updateObject(object3Id, aabb3, Vector3::zero(), false); - tree.updateObject(object4Id, aabb4, Vector3::zero(), false); + tree.updateObject(object1Id, aabb1, vec3::zero(), false); + tree.updateObject(object2Id, aabb2, vec3::zero(), false); + tree.updateObject(object3Id, aabb3, vec3::zero(), false); + tree.updateObject(object4Id, aabb4, vec3::zero(), false); // Ray with no hits m_raycastCallback.reset(); @@ -453,10 +453,10 @@ class TestDynamicAABBTree : public Test { // ---- Update the object AABBs with the initial AABBs (with reinsertion) ----- // - tree.updateObject(object1Id, aabb1, Vector3::zero(), true); - tree.updateObject(object2Id, aabb2, Vector3::zero(), true); - tree.updateObject(object3Id, aabb3, Vector3::zero(), true); - tree.updateObject(object4Id, aabb4, Vector3::zero(), true); + tree.updateObject(object1Id, aabb1, vec3::zero(), true); + tree.updateObject(object2Id, aabb2, vec3::zero(), true); + tree.updateObject(object3Id, aabb3, vec3::zero(), true); + tree.updateObject(object4Id, aabb4, vec3::zero(), true); // Ray with no hits m_raycastCallback.reset(); @@ -492,14 +492,14 @@ class TestDynamicAABBTree : public Test { // ---- Move objects 2 and 3 ----- // - AABB newAABB2(Vector3(-7, 10, -3), Vector3(1, 13, 3)); - tree.updateObject(object2Id, newAABB2, Vector3::zero()); + AABB newAABB2(vec3(-7, 10, -3), vec3(1, 13, 3)); + tree.updateObject(object2Id, newAABB2, vec3::zero()); - AABB newAABB3(Vector3(7, -6, -3), Vector3(9, 1, 3)); - tree.updateObject(object3Id, newAABB3, Vector3::zero()); + AABB newAABB3(vec3(7, -6, -3), vec3(9, 1, 3)); + tree.updateObject(object3Id, newAABB3, vec3::zero()); // Ray that hits object 1, 2 - Ray ray5(Vector3(-4, -5, 0), Vector3(-4, 12, 0)); + Ray ray5(vec3(-4, -5, 0), vec3(-4, 12, 0)); m_raycastCallback.reset(); tree.raycast(ray5, m_raycastCallback); test(m_raycastCallback.isHit(object1Id)); @@ -508,7 +508,7 @@ class TestDynamicAABBTree : public Test { test(!m_raycastCallback.isHit(object4Id)); // Ray that hits object 3 and 4 - Ray ray6(Vector3(11, -3, 1), Vector3(-2, -3, 1)); + Ray ray6(vec3(11, -3, 1), vec3(-2, -3, 1)); m_raycastCallback.reset(); tree.raycast(ray6, m_raycastCallback); test(!m_raycastCallback.isHit(object1Id)); diff --git a/test/tests/collision/TestPointInside.h b/test/tests/collision/TestPointInside.h index 7f832e2..b5b5b53 100644 --- a/test/tests/collision/TestPointInside.h +++ b/test/tests/collision/TestPointInside.h @@ -70,11 +70,11 @@ class TestPointInside : public Test { ConvexMeshShape* mConvexMeshShapeBodyEdgesInfo; CylinderShape* mCylinderShape; - // Transform - Transform m_bodyTransform; - Transform m_shapeTransform; - Transform mLocalShapeToWorld; - Transform mLocalShape2ToWorld; + // etk::Transform3D + etk::Transform3D m_bodyTransform; + etk::Transform3D m_shapeTransform; + etk::Transform3D mLocalShapeToWorld; + etk::Transform3D mLocalShape2ToWorld; // Proxy Shapes ProxyShape* mBoxProxyShape; @@ -96,30 +96,30 @@ class TestPointInside : public Test { m_world = new CollisionWorld(); // Body transform - Vector3 position(-3, 2, 7); - Quaternion orientation(PI / 5, PI / 6, PI / 7); - m_bodyTransform = Transform(position, orientation); + vec3 position(-3, 2, 7); + etk::Quaternion orientation(PI / 5, PI / 6, PI / 7); + m_bodyetk::Transform3D = Transform(position, orientation); // Create the bodies - mBoxBody = m_world->createCollisionBody(m_bodyTransform); - mSphereBody = m_world->createCollisionBody(m_bodyTransform); - mCapsuleBody = m_world->createCollisionBody(m_bodyTransform); - mConeBody = m_world->createCollisionBody(m_bodyTransform); - mConvexMeshBody = m_world->createCollisionBody(m_bodyTransform); - mConvexMeshBodyEdgesInfo = m_world->createCollisionBody(m_bodyTransform); - mCylinderBody = m_world->createCollisionBody(m_bodyTransform); - mCompoundBody = m_world->createCollisionBody(m_bodyTransform); + mBoxBody = m_world->createCollisionBody(m_bodyetk::Transform3D); + mSphereBody = m_world->createCollisionBody(m_bodyetk::Transform3D); + mCapsuleBody = m_world->createCollisionBody(m_bodyetk::Transform3D); + mConeBody = m_world->createCollisionBody(m_bodyetk::Transform3D); + mConvexMeshBody = m_world->createCollisionBody(m_bodyetk::Transform3D); + mConvexMeshBodyEdgesInfo = m_world->createCollisionBody(m_bodyetk::Transform3D); + mCylinderBody = m_world->createCollisionBody(m_bodyetk::Transform3D); + mCompoundBody = m_world->createCollisionBody(m_bodyetk::Transform3D); // Collision shape transform - Vector3 shapePosition(1, -4, -3); - Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3); + vec3 shapePosition(1, -4, -3); + etk::Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3); m_shapeTransform = Transform(shapePosition, shapeOrientation); // Compute the the transform from a local shape point to world-space - mLocalShapeToWorld = m_bodyTransform * m_shapeTransform; + mLocalShapeToWorld = m_bodyetk::Transform3D * m_shapeTransform; // Create collision shapes - mBoxShape = new BoxShape(Vector3(2, 3, 4), 0); + mBoxShape = new BoxShape(vec3(2, 3, 4), 0); mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, m_shapeTransform); mSphereShape = new SphereShape(3); @@ -132,25 +132,25 @@ class TestPointInside : public Test { mConeProxyShape = mConeBody->addCollisionShape(mConeShape, m_shapeTransform); mConvexMeshShape = new ConvexMeshShape(0.0); // Box of dimension (2, 3, 4) - mConvexMeshShape->addVertex(Vector3(-2, -3, -4)); - mConvexMeshShape->addVertex(Vector3(2, -3, -4)); - mConvexMeshShape->addVertex(Vector3(2, -3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, -3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, 3, -4)); - mConvexMeshShape->addVertex(Vector3(2, 3, -4)); - mConvexMeshShape->addVertex(Vector3(2, 3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, 3, 4)); + mConvexMeshShape->addVertex(vec3(-2, -3, -4)); + mConvexMeshShape->addVertex(vec3(2, -3, -4)); + mConvexMeshShape->addVertex(vec3(2, -3, 4)); + mConvexMeshShape->addVertex(vec3(-2, -3, 4)); + mConvexMeshShape->addVertex(vec3(-2, 3, -4)); + mConvexMeshShape->addVertex(vec3(2, 3, -4)); + mConvexMeshShape->addVertex(vec3(2, 3, 4)); + mConvexMeshShape->addVertex(vec3(-2, 3, 4)); mConvexMeshProxyShape = mConvexMeshBody->addCollisionShape(mConvexMeshShape, m_shapeTransform); mConvexMeshShapeBodyEdgesInfo = new ConvexMeshShape(0.0); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, -3, -4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, -3, -4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, -3, 4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, -3, 4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, 3, -4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, 3, -4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, 3, 4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, 3, 4)); + mConvexMeshShapeBodyEdgesInfo->addVertex(vec3(-2, -3, -4)); + mConvexMeshShapeBodyEdgesInfo->addVertex(vec3(2, -3, -4)); + mConvexMeshShapeBodyEdgesInfo->addVertex(vec3(2, -3, 4)); + mConvexMeshShapeBodyEdgesInfo->addVertex(vec3(-2, -3, 4)); + mConvexMeshShapeBodyEdgesInfo->addVertex(vec3(-2, 3, -4)); + mConvexMeshShapeBodyEdgesInfo->addVertex(vec3(2, 3, -4)); + mConvexMeshShapeBodyEdgesInfo->addVertex(vec3(2, 3, 4)); + mConvexMeshShapeBodyEdgesInfo->addVertex(vec3(-2, 3, 4)); mConvexMeshShapeBodyEdgesInfo->addEdge(0, 1); mConvexMeshShapeBodyEdgesInfo->addEdge(1, 2); mConvexMeshShapeBodyEdgesInfo->addEdge(2, 3); @@ -172,10 +172,10 @@ class TestPointInside : public Test { mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, m_shapeTransform); // Compound shape is a cylinder and a sphere - Vector3 positionShape2(Vector3(4, 2, -3)); - Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); - Transform shapeTransform2(positionShape2, orientationShape2); - mLocalShape2ToWorld = m_bodyTransform * shapeTransform2; + vec3 positionShape2(vec3(4, 2, -3)); + etk::Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); + etk::Transform3D shapeTransform2(positionShape2, orientationShape2); + mLocalShape2ToWorld = m_bodyetk::Transform3D * shapeTransform2; mCompoundBody->addCollisionShape(mCylinderShape, m_shapeTransform); mCompoundBody->addCollisionShape(mSphereShape, shapeTransform2); } @@ -207,56 +207,56 @@ class TestPointInside : public Test { void testBox() { // Tests with CollisionBody - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 0))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * vec3(-1.9, 0, 0))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * vec3(1.9, 0, 0))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * vec3(0, -2.9, 0))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * vec3(0, 2.9, 0))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, -3.9))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 3.9))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * vec3(-1.9, -2.9, -3.9))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * vec3(1.9, 2.9, 3.9))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * vec3(-1, -2, -1.5))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * vec3(-1, 2, -2.5))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * vec3(1, -2, 3.5))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * vec3(-2.1, 0, 0))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * vec3(2.1, 0, 0))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * vec3(0, -3.1, 0))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * vec3(0, 3.1, 0))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, -4.1))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 4.1))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * vec3(-2.1, -3.1, -4.1))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * vec3(2.1, 3.1, 4.1))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * vec3(-10, -2, -1.5))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * vec3(-1, 4, -2.5))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * vec3(1, -2, 4.5))); // Tests with ProxyBoxShape - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, 0))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.9, 0, 0))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.9, 0, 0))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -2.9, 0))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 2.9, 0))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, -3.9))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, 3.9))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.9, -2.9, -3.9))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.9, 2.9, 3.9))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1, -2, -1.5))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1, 2, -2.5))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(1, -2, 3.5))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(-2.1, 0, 0))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(2.1, 0, 0))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -3.1, 0))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 3.1, 0))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, -4.1))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, 4.1))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(-2.1, -3.1, -4.1))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(2.1, 3.1, 4.1))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(-10, -2, -1.5))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1, 4, -2.5))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * vec3(1, -2, 4.5))); } /// Test the ProxySphereShape::testPointInside() and @@ -264,48 +264,48 @@ class TestPointInside : public Test { void testSphere() { // Tests with CollisionBody - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0))); - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0))); - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -1.5))); - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 1.5))); + test(mSphereBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 0))); + test(mSphereBody->testPointInside(mLocalShapeToWorld * vec3(2.9, 0, 0))); + test(mSphereBody->testPointInside(mLocalShapeToWorld * vec3(-2.9, 0, 0))); + test(mSphereBody->testPointInside(mLocalShapeToWorld * vec3(0, 2.9, 0))); + test(mSphereBody->testPointInside(mLocalShapeToWorld * vec3(0, -2.9, 0))); + test(mSphereBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 2.9))); + test(mSphereBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 2.9))); + test(mSphereBody->testPointInside(mLocalShapeToWorld * vec3(-1, -2, -1.5))); + test(mSphereBody->testPointInside(mLocalShapeToWorld * vec3(-1, 2, -1.5))); + test(mSphereBody->testPointInside(mLocalShapeToWorld * vec3(1, -2, 1.5))); - test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); - test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); - test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); - test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); - test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(-2, -2, -2))); - test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(-2, 2, -1.5))); - test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2, 2.5))); + test(!mSphereBody->testPointInside(mLocalShapeToWorld * vec3(3.1, 0, 0))); + test(!mSphereBody->testPointInside(mLocalShapeToWorld * vec3(-3.1, 0, 0))); + test(!mSphereBody->testPointInside(mLocalShapeToWorld * vec3(0, 3.1, 0))); + test(!mSphereBody->testPointInside(mLocalShapeToWorld * vec3(0, -3.1, 0))); + test(!mSphereBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 3.1))); + test(!mSphereBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, -3.1))); + test(!mSphereBody->testPointInside(mLocalShapeToWorld * vec3(-2, -2, -2))); + test(!mSphereBody->testPointInside(mLocalShapeToWorld * vec3(-2, 2, -1.5))); + test(!mSphereBody->testPointInside(mLocalShapeToWorld * vec3(1.5, -2, 2.5))); // Tests with ProxySphereShape - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0))); - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0))); - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -1.5))); - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 1.5))); + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, 0))); + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * vec3(2.9, 0, 0))); + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * vec3(-2.9, 0, 0))); + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 2.9, 0))); + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -2.9, 0))); + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, 2.9))); + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, 2.9))); + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1, -2, -1.5))); + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1, 2, -1.5))); + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * vec3(1, -2, 1.5))); - test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); - test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); - test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); - test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); - test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2, -2, -2))); - test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2, 2, -1.5))); - test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2, 2.5))); + test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * vec3(3.1, 0, 0))); + test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * vec3(-3.1, 0, 0))); + test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 3.1, 0))); + test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -3.1, 0))); + test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, 3.1))); + test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, -3.1))); + test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * vec3(-2, -2, -2))); + test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * vec3(-2, 2, -1.5))); + test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.5, -2, 2.5))); } /// Test the ProxyCapsuleShape::testPointInside() and @@ -313,98 +313,98 @@ class TestPointInside : public Test { void testCapsule() { // Tests with CollisionBody - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -6.9, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 6.9, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, -0.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 1.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -1.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, 5, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 5, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, 5, 0.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, 5, -0.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 1.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -1.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, -5, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -5, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, -5, 0.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, -5, -0.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -4, -0.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, 0.4))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.3, 1, 1.5))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0, 5, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0, -5, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0, -6.9, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0, 6.9, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 1.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, -1.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(1.9, 0, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(-1.9, 0, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0.9, 0, 0.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0.9, 0, -0.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0, 5, 1.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0, 5, -1.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(1.9, 5, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(-1.9, 5, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0.9, 5, 0.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0.9, 5, -0.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0, -5, 1.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0, -5, -1.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(1.9, -5, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(-1.9, -5, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0.9, -5, 0.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0.9, -5, -0.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(-1.7, -4, -0.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(-1, 2, 0.4))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(1.3, 1, 1.5))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -7.1, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 7.1, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 5, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 5, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, 1.6))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, -1.7))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, -5, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -5, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, 1.6))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, -1.7))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0, -7.1, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0, 7.1, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 2.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, -2.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(2.1, 0, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(-2.1, 0, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0, 5, 2.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0, 5, -2.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(2.1, 5, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(-2.1, 5, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(1.5, 5, 1.6))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(1.5, 5, -1.7))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0, -5, 2.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(0, -5, -2.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(2.1, -5, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(-2.1, -5, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(1.5, -5, 1.6))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * vec3(1.5, -5, -1.7))); // Tests with ProxyCapsuleShape - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -6.9, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 6.9, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, -0.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 1.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -1.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 5, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 5, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 5, 0.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 5, -0.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 1.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -1.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, -5, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -5, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, -5, 0.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, -5, -0.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -4, -0.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, 0.4))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.3, 1, 1.5))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 5, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -5, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -6.9, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 6.9, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, 1.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, -1.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.9, 0, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.9, 0, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0.9, 0, 0.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0.9, 0, -0.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 5, 1.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 5, -1.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.9, 5, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.9, 5, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0.9, 5, 0.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0.9, 5, -0.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -5, 1.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -5, -1.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.9, -5, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.9, -5, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0.9, -5, 0.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0.9, -5, -0.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.7, -4, -0.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1, 2, 0.4))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.3, 1, 1.5))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -7.1, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 7.1, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 5, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 5, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, 1.6))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, -1.7))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, -5, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -5, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, 1.6))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, -1.7))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -7.1, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 7.1, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, 2.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, -2.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(2.1, 0, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(-2.1, 0, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 5, 2.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 5, -2.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(2.1, 5, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(-2.1, 5, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.5, 5, 1.6))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.5, 5, -1.7))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -5, 2.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -5, -2.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(2.1, -5, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(-2.1, -5, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.5, -5, 1.6))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.5, -5, -1.7))); } /// Test the ProxyConeShape::testPointInside() and @@ -412,76 +412,76 @@ class TestPointInside : public Test { void testCone() { // Tests with CollisionBody - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.9, 0, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0.9))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -0.9))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, -0.7))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, 0.7))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, -0.7))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, 0.7))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.96, -2.9, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.96, -2.9, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.96))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.96))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.3, -2.9, -1.4))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.3, -2.9, 1.4))); + test(mConeBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 0))); + test(mConeBody->testPointInside(mLocalShapeToWorld * vec3(0.9, 0, 0))); + test(mConeBody->testPointInside(mLocalShapeToWorld * vec3(-0.9, 0, 0))); + test(mConeBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 0.9))); + test(mConeBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, -0.9))); + test(mConeBody->testPointInside(mLocalShapeToWorld * vec3(0.6, 0, -0.7))); + test(mConeBody->testPointInside(mLocalShapeToWorld * vec3(0.6, 0, 0.7))); + test(mConeBody->testPointInside(mLocalShapeToWorld * vec3(-0.6, 0, -0.7))); + test(mConeBody->testPointInside(mLocalShapeToWorld * vec3(-0.6, 0, 0.7))); + test(mConeBody->testPointInside(mLocalShapeToWorld * vec3(0, 2.9, 0))); + test(mConeBody->testPointInside(mLocalShapeToWorld * vec3(0, -2.9, 0))); + test(mConeBody->testPointInside(mLocalShapeToWorld * vec3(1.96, -2.9, 0))); + test(mConeBody->testPointInside(mLocalShapeToWorld * vec3(-1.96, -2.9, 0))); + test(mConeBody->testPointInside(mLocalShapeToWorld * vec3(0, -2.9, 1.96))); + test(mConeBody->testPointInside(mLocalShapeToWorld * vec3(0, -2.9, -1.96))); + test(mConeBody->testPointInside(mLocalShapeToWorld * vec3(1.3, -2.9, -1.4))); + test(mConeBody->testPointInside(mLocalShapeToWorld * vec3(-1.3, -2.9, 1.4))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.1, 0, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.1, 0, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.1))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.1))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, -0.8))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, 0.8))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, -0.8))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, 0.8))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.97, -2.9, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.97, -2.9, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.97))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.97))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2.9, -1.5))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.5, -2.9, 1.5))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * vec3(1.1, 0, 0))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * vec3(-1.1, 0, 0))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 1.1))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, -1.1))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * vec3(0.8, 0, -0.8))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * vec3(0.8, 0, 0.8))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * vec3(-0.8, 0, -0.8))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * vec3(-0.8, 0, 0.8))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * vec3(0, 3.1, 0))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * vec3(0, -3.1, 0))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * vec3(1.97, -2.9, 0))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * vec3(-1.97, -2.9, 0))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * vec3(0, -2.9, 1.97))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * vec3(0, -2.9, -1.97))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * vec3(1.5, -2.9, -1.5))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * vec3(-1.5, -2.9, 1.5))); // Tests with ProxyConeShape - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.9, 0, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0.9))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -0.9))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, -0.7))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, 0.7))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, -0.7))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, 0.7))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.96, -2.9, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.96, -2.9, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.96))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.96))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.3, -2.9, -1.4))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.3, -2.9, 1.4))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, 0))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(0.9, 0, 0))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(-0.9, 0, 0))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, 0.9))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, -0.9))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(0.6, 0, -0.7))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(0.6, 0, 0.7))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(-0.6, 0, -0.7))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(-0.6, 0, 0.7))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 2.9, 0))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -2.9, 0))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.96, -2.9, 0))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.96, -2.9, 0))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -2.9, 1.96))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -2.9, -1.96))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.3, -2.9, -1.4))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.3, -2.9, 1.4))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.1, 0, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.1, 0, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.1))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.1))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, -0.8))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, 0.8))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, -0.8))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, 0.8))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.97, -2.9, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.97, -2.9, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.97))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.97))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2.9, -1.5))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.5, -2.9, 1.5))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.1, 0, 0))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.1, 0, 0))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, 1.1))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, -1.1))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(0.8, 0, -0.8))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(0.8, 0, 0.8))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(-0.8, 0, -0.8))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(-0.8, 0, 0.8))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 3.1, 0))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -3.1, 0))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.97, -2.9, 0))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.97, -2.9, 0))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -2.9, 1.97))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -2.9, -1.97))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.5, -2.9, -1.5))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.5, -2.9, 1.5))); } /// Test the ProxyConvexMeshShape::testPointInside() and @@ -491,110 +491,110 @@ class TestPointInside : public Test { // ----- Tests without using edges information ----- // // Tests with CollisionBody - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 0))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(-1.9, 0, 0))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(1.9, 0, 0))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(0, -2.9, 0))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(0, 2.9, 0))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, -3.9))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 3.9))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(-1.9, -2.9, -3.9))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(1.9, 2.9, 3.9))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(-1, -2, -1.5))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(-1, 2, -2.5))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(1, -2, 3.5))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(-2.1, 0, 0))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(2.1, 0, 0))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(0, -3.1, 0))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(0, 3.1, 0))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, -4.1))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 4.1))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(-2.1, -3.1, -4.1))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(2.1, 3.1, 4.1))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(-10, -2, -1.5))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(-1, 4, -2.5))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * vec3(1, -2, 4.5))); // Tests with ProxyConvexMeshShape - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, 0))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.9, 0, 0))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.9, 0, 0))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -2.9, 0))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 2.9, 0))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, -3.9))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, 3.9))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.9, -2.9, -3.9))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.9, 2.9, 3.9))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1, -2, -1.5))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1, 2, -2.5))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(1, -2, 3.5))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(-2.1, 0, 0))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(2.1, 0, 0))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -3.1, 0))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 3.1, 0))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, -4.1))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, 4.1))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(-2.1, -3.1, -4.1))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(2.1, 3.1, 4.1))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(-10, -2, -1.5))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1, 4, -2.5))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * vec3(1, -2, 4.5))); // ----- Tests using edges information ----- // // Tests with CollisionBody - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(0, 0, 0))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(-1.9, 0, 0))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(1.9, 0, 0))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(0, -2.9, 0))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(0, 2.9, 0))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(0, 0, -3.9))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(0, 0, 3.9))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(-1.9, -2.9, -3.9))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(1.9, 2.9, 3.9))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(-1, -2, -1.5))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(-1, 2, -2.5))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(1, -2, 3.5))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(-2.1, 0, 0))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(2.1, 0, 0))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(0, -3.1, 0))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(0, 3.1, 0))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(0, 0, -4.1))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(0, 0, 4.1))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(-2.1, -3.1, -4.1))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(2.1, 3.1, 4.1))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(-10, -2, -1.5))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(-1, 4, -2.5))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(1, -2, 4.5))); // Tests with ProxyConvexMeshShape - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(0, 0, 0))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(-1.9, 0, 0))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(1.9, 0, 0))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(0, -2.9, 0))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(0, 2.9, 0))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(0, 0, -3.9))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(0, 0, 3.9))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(-1.9, -2.9, -3.9))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(1.9, 2.9, 3.9))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(-1, -2, -1.5))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(-1, 2, -2.5))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(1, -2, 3.5))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(-2.1, 0, 0))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(2.1, 0, 0))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(0, -3.1, 0))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(0, 3.1, 0))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(0, 0, -4.1))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(0, 0, 4.1))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(-2.1, -3.1, -4.1))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(2.1, 3.1, 4.1))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(-10, -2, -1.5))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(-1, 4, -2.5))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * vec3(1, -2, 4.5))); } /// Test the ProxyCylinderShape::testPointInside() and @@ -602,161 +602,161 @@ class TestPointInside : public Test { void testCylinder() { // Tests with CollisionBody - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, -3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, -3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, 1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 0))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(0, 3.9, 0))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(0, -3.9, 0))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(2.9, 0, 0))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(-2.9, 0, 0))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 2.9))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, -2.9))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(1.7, 0, 1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(1.7, 0, -1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(-1.7, 0, -1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(-1.7, 0, 1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(2.9, 3.9, 0))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(-2.9, 3.9, 0))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(0, 3.9, 2.9))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(0, 3.9, -2.9))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(1.7, 3.9, 1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(1.7, 3.9, -1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(-1.7, 3.9, -1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(-1.7, 3.9, 1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(2.9, -3.9, 0))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(-2.9, -3.9, 0))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(0, -3.9, 2.9))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(0, -3.9, -2.9))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(1.7, -3.9, 1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(1.7, -3.9, -1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(-1.7, -3.9, -1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(-1.7, -3.9, 1.7))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 4.1, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -4.1, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, 2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 0, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.3, 0, 2.8))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 3.9, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 3.9, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, 2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, 2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, -3.9, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -3.9, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, 2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, 2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(0, 4.1, 0))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(0, -4.1, 0))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(3.1, 0, 0))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(-3.1, 0, 0))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 3.1))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, -3.1))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(2.2, 0, 2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(2.2, 0, -2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(-2.2, 0, -2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(-1.3, 0, 2.8))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(3.1, 3.9, 0))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(-3.1, 3.9, 0))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(0, 3.9, 3.1))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(0, 3.9, -3.1))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(2.2, 3.9, 2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(2.2, 3.9, -2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(-2.2, 3.9, -2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(-2.2, 3.9, 2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(3.1, -3.9, 0))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(-3.1, -3.9, 0))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(0, -3.9, 3.1))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(0, -3.9, -3.1))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(2.2, -3.9, 2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(2.2, -3.9, -2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(-2.2, -3.9, -2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * vec3(-2.2, -3.9, 2.2))); // Tests with ProxyCylinderShape - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, -3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, -3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, 1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, 0))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 3.9, 0))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -3.9, 0))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(2.9, 0, 0))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(-2.9, 0, 0))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, 2.9))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, -2.9))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.7, 0, 1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.7, 0, -1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.7, 0, -1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.7, 0, 1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(2.9, 3.9, 0))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(-2.9, 3.9, 0))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 3.9, 2.9))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 3.9, -2.9))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.7, 3.9, 1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.7, 3.9, -1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.7, 3.9, -1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.7, 3.9, 1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(2.9, -3.9, 0))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(-2.9, -3.9, 0))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -3.9, 2.9))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -3.9, -2.9))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.7, -3.9, 1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(1.7, -3.9, -1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.7, -3.9, -1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.7, -3.9, 1.7))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 4.1, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -4.1, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, 2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 0, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.3, 0, 2.8))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 3.9, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 3.9, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, 2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, 2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, -3.9, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -3.9, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, 2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, 2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 4.1, 0))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -4.1, 0))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(3.1, 0, 0))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(-3.1, 0, 0))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, 3.1))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 0, -3.1))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(2.2, 0, 2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(2.2, 0, -2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(-2.2, 0, -2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(-1.3, 0, 2.8))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(3.1, 3.9, 0))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(-3.1, 3.9, 0))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 3.9, 3.1))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, 3.9, -3.1))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(2.2, 3.9, 2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(2.2, 3.9, -2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(-2.2, 3.9, -2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(-2.2, 3.9, 2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(3.1, -3.9, 0))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(-3.1, -3.9, 0))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -3.9, 3.1))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(0, -3.9, -3.1))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(2.2, -3.9, 2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(2.2, -3.9, -2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(-2.2, -3.9, -2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * vec3(-2.2, -3.9, 2.2))); } /// Test the CollisionBody::testPointInside() method void testCompound() { // Points on the cylinder - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.9))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, 1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, -1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, -1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, 1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 3.9, 0))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 3.9, 0))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 2.9))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -2.9))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, 1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, -1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, -1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, 1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, -3.9, 0))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, -3.9, 0))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 2.9))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -2.9))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, 1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, -1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, -1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, 1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 0))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(0, 3.9, 0))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(0, -3.9, 0))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(2.9, 0, 0))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(-2.9, 0, 0))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, 2.9))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(0, 0, -2.9))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(1.7, 0, 1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(1.7, 0, -1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(-1.7, 0, -1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(-1.7, 0, 1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(2.9, 3.9, 0))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(-2.9, 3.9, 0))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(0, 3.9, 2.9))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(0, 3.9, -2.9))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(1.7, 3.9, 1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(1.7, 3.9, -1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(-1.7, 3.9, -1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(-1.7, 3.9, 1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(2.9, -3.9, 0))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(-2.9, -3.9, 0))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(0, -3.9, 2.9))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(0, -3.9, -2.9))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(1.7, -3.9, 1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(1.7, -3.9, -1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(-1.7, -3.9, -1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * vec3(-1.7, -3.9, 1.7))); // Points on the sphere - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(0, 0, 0))); - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(2.9, 0, 0))); - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(-2.9, 0, 0))); - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(0, 2.9, 0))); - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(0, -2.9, 0))); - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(0, 0, 2.9))); - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(0, 0, 2.9))); - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(-1, -2, -1.5))); - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(-1, 2, -1.5))); - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(1, -2, 1.5))); + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * vec3(0, 0, 0))); + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * vec3(2.9, 0, 0))); + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * vec3(-2.9, 0, 0))); + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * vec3(0, 2.9, 0))); + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * vec3(0, -2.9, 0))); + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * vec3(0, 0, 2.9))); + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * vec3(0, 0, 2.9))); + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * vec3(-1, -2, -1.5))); + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * vec3(-1, 2, -1.5))); + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * vec3(1, -2, 1.5))); } }; diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index 53aed0f..9a697d9 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -75,15 +75,15 @@ class WorldRaycastCallback : public RaycastCallback { } // Return a fraction of 1.0 because we need to gather all hits - return float(1.0); + return 1.0f; } void reset() { raycastInfo.body = NULL; - raycastInfo.hitFraction = float(0.0); + raycastInfo.hitFraction = 0.0f; raycastInfo.proxyShape = NULL; - raycastInfo.worldNormal.setToZero(); - raycastInfo.worldPoint.setToZero(); + raycastInfo.worldNormal.setZero(); + raycastInfo.worldPoint.setZero(); isHit = false; } }; @@ -120,11 +120,11 @@ class TestRaycast : public Test { CollisionBody* mConcaveMeshBody; CollisionBody* mHeightFieldBody; - // Transform - Transform m_bodyTransform; - Transform m_shapeTransform; - Transform mLocalShapeToWorld; - Transform mLocalShape2ToWorld; + // etk::Transform3D + etk::Transform3D m_bodyTransform; + etk::Transform3D m_shapeTransform; + etk::Transform3D mLocalShapeToWorld; + etk::Transform3D mLocalShape2ToWorld; // Collision shapes BoxShape* mBoxShape; @@ -155,10 +155,10 @@ class TestRaycast : public Test { // Triangle meshes TriangleMesh mConcaveTriangleMesh; - std::vector mConcaveMeshVertices; + std::vector mConcaveMeshVertices; std::vector mConcaveMeshIndices; TriangleVertexArray* mConcaveMeshVertexArray; - float mHeightFieldData[100]; + float m_heightFieldData[100]; public : @@ -173,41 +173,41 @@ class TestRaycast : public Test { m_world = new CollisionWorld(); // Body transform - Vector3 position(-3, 2, 7); - Quaternion orientation(PI / 5, PI / 6, PI / 7); - m_bodyTransform = Transform(position, orientation); + vec3 position(-3, 2, 7); + etk::Quaternion orientation(PI / 5, PI / 6, PI / 7); + m_bodyetk::Transform3D = Transform(position, orientation); // Create the bodies - mBoxBody = m_world->createCollisionBody(m_bodyTransform); - mSphereBody = m_world->createCollisionBody(m_bodyTransform); - mCapsuleBody = m_world->createCollisionBody(m_bodyTransform); - mConeBody = m_world->createCollisionBody(m_bodyTransform); - mConvexMeshBody = m_world->createCollisionBody(m_bodyTransform); - mConvexMeshBodyEdgesInfo = m_world->createCollisionBody(m_bodyTransform); - mCylinderBody = m_world->createCollisionBody(m_bodyTransform); - mCompoundBody = m_world->createCollisionBody(m_bodyTransform); - mTriangleBody = m_world->createCollisionBody(m_bodyTransform); - mConcaveMeshBody = m_world->createCollisionBody(m_bodyTransform); - mHeightFieldBody = m_world->createCollisionBody(m_bodyTransform); + mBoxBody = m_world->createCollisionBody(m_bodyetk::Transform3D); + mSphereBody = m_world->createCollisionBody(m_bodyetk::Transform3D); + mCapsuleBody = m_world->createCollisionBody(m_bodyetk::Transform3D); + mConeBody = m_world->createCollisionBody(m_bodyetk::Transform3D); + mConvexMeshBody = m_world->createCollisionBody(m_bodyetk::Transform3D); + mConvexMeshBodyEdgesInfo = m_world->createCollisionBody(m_bodyetk::Transform3D); + mCylinderBody = m_world->createCollisionBody(m_bodyetk::Transform3D); + mCompoundBody = m_world->createCollisionBody(m_bodyetk::Transform3D); + mTriangleBody = m_world->createCollisionBody(m_bodyetk::Transform3D); + mConcaveMeshBody = m_world->createCollisionBody(m_bodyetk::Transform3D); + mHeightFieldBody = m_world->createCollisionBody(m_bodyetk::Transform3D); // Collision shape transform - Vector3 shapePosition(1, -4, -3); - Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3); + vec3 shapePosition(1, -4, -3); + etk::Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3); m_shapeTransform = Transform(shapePosition, shapeOrientation); // Compute the the transform from a local shape point to world-space - mLocalShapeToWorld = m_bodyTransform * m_shapeTransform; + mLocalShapeToWorld = m_bodyetk::Transform3D * m_shapeTransform; // Create collision shapes - mBoxShape = new BoxShape(Vector3(2, 3, 4), 0); + mBoxShape = new BoxShape(vec3(2, 3, 4), 0); mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, m_shapeTransform); mSphereShape = new SphereShape(3); mSphereProxyShape = mSphereBody->addCollisionShape(mSphereShape, m_shapeTransform); - const Vector3 triangleVertex1(100, 100, 0); - const Vector3 triangleVertex2(105, 100, 0); - const Vector3 triangleVertex3(100, 103, 0); + const vec3 triangleVertex1(100, 100, 0); + const vec3 triangleVertex2(105, 100, 0); + const vec3 triangleVertex3(100, 103, 0); mTriangleShape = new TriangleShape(triangleVertex1, triangleVertex2, triangleVertex3); mTriangleProxyShape = mTriangleBody->addCollisionShape(mTriangleShape, m_shapeTransform); @@ -219,25 +219,25 @@ class TestRaycast : public Test { // Box of dimension (2, 3, 4) mConvexMeshShape = new ConvexMeshShape(0.0); - mConvexMeshShape->addVertex(Vector3(-2, -3, -4)); - mConvexMeshShape->addVertex(Vector3(2, -3, -4)); - mConvexMeshShape->addVertex(Vector3(2, -3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, -3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, 3, -4)); - mConvexMeshShape->addVertex(Vector3(2, 3, -4)); - mConvexMeshShape->addVertex(Vector3(2, 3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, 3, 4)); + mConvexMeshShape->addVertex(vec3(-2, -3, -4)); + mConvexMeshShape->addVertex(vec3(2, -3, -4)); + mConvexMeshShape->addVertex(vec3(2, -3, 4)); + mConvexMeshShape->addVertex(vec3(-2, -3, 4)); + mConvexMeshShape->addVertex(vec3(-2, 3, -4)); + mConvexMeshShape->addVertex(vec3(2, 3, -4)); + mConvexMeshShape->addVertex(vec3(2, 3, 4)); + mConvexMeshShape->addVertex(vec3(-2, 3, 4)); mConvexMeshProxyShape = mConvexMeshBody->addCollisionShape(mConvexMeshShape, m_shapeTransform); mConvexMeshShapeEdgesInfo = new ConvexMeshShape(0.0); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, -3, -4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, -3, -4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, -3, 4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, -3, 4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, 3, -4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, 3, -4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, 3, 4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, 3, 4)); + mConvexMeshShapeEdgesInfo->addVertex(vec3(-2, -3, -4)); + mConvexMeshShapeEdgesInfo->addVertex(vec3(2, -3, -4)); + mConvexMeshShapeEdgesInfo->addVertex(vec3(2, -3, 4)); + mConvexMeshShapeEdgesInfo->addVertex(vec3(-2, -3, 4)); + mConvexMeshShapeEdgesInfo->addVertex(vec3(-2, 3, -4)); + mConvexMeshShapeEdgesInfo->addVertex(vec3(2, 3, -4)); + mConvexMeshShapeEdgesInfo->addVertex(vec3(2, 3, 4)); + mConvexMeshShapeEdgesInfo->addVertex(vec3(-2, 3, 4)); mConvexMeshShapeEdgesInfo->addEdge(0, 1); mConvexMeshShapeEdgesInfo->addEdge(1, 2); mConvexMeshShapeEdgesInfo->addEdge(2, 3); @@ -259,22 +259,22 @@ class TestRaycast : public Test { mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, m_shapeTransform); // Compound shape is a cylinder and a sphere - Vector3 positionShape2(Vector3(4, 2, -3)); - Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); - Transform shapeTransform2(positionShape2, orientationShape2); - mLocalShape2ToWorld = m_bodyTransform * shapeTransform2; + vec3 positionShape2(vec3(4, 2, -3)); + etk::Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); + etk::Transform3D shapeTransform2(positionShape2, orientationShape2); + mLocalShape2ToWorld = m_bodyetk::Transform3D * shapeTransform2; mCompoundCylinderProxyShape = mCompoundBody->addCollisionShape(mCylinderShape, m_shapeTransform); mCompoundSphereProxyShape = mCompoundBody->addCollisionShape(mSphereShape, shapeTransform2); // Concave Mesh shape - mConcaveMeshVertices.push_back(Vector3(-2, -3, -4)); - mConcaveMeshVertices.push_back(Vector3(2, -3, -4)); - mConcaveMeshVertices.push_back(Vector3(2, -3, 4)); - mConcaveMeshVertices.push_back(Vector3(-2, -3, 4)); - mConcaveMeshVertices.push_back(Vector3(-2, 3, -4)); - mConcaveMeshVertices.push_back(Vector3(2, 3, -4)); - mConcaveMeshVertices.push_back(Vector3(2, 3, 4)); - mConcaveMeshVertices.push_back(Vector3(-2, 3, 4)); + mConcaveMeshVertices.push_back(vec3(-2, -3, -4)); + mConcaveMeshVertices.push_back(vec3(2, -3, -4)); + mConcaveMeshVertices.push_back(vec3(2, -3, 4)); + mConcaveMeshVertices.push_back(vec3(-2, -3, 4)); + mConcaveMeshVertices.push_back(vec3(-2, 3, -4)); + mConcaveMeshVertices.push_back(vec3(2, 3, -4)); + mConcaveMeshVertices.push_back(vec3(2, 3, 4)); + mConcaveMeshVertices.push_back(vec3(-2, 3, 4)); mConcaveMeshIndices.push_back(0); mConcaveMeshIndices.push_back(1); mConcaveMeshIndices.push_back(2); mConcaveMeshIndices.push_back(0); mConcaveMeshIndices.push_back(2); mConcaveMeshIndices.push_back(3); @@ -291,7 +291,7 @@ class TestRaycast : public Test { TriangleVertexArray::VertexDataType vertexType = sizeof(float) == 4 ? TriangleVertexArray::VERTEX_FLOAT_TYPE : TriangleVertexArray::VERTEX_DOUBLE_TYPE; mConcaveMeshVertexArray = - new TriangleVertexArray(8, &(mConcaveMeshVertices[0]), sizeof(Vector3), + new TriangleVertexArray(8, &(mConcaveMeshVertices[0]), sizeof(vec3), 12, &(mConcaveMeshIndices[0]), sizeof(uint32_t), vertexType, TriangleVertexArray::INDEX_INTEGER_TYPE); @@ -304,8 +304,8 @@ class TestRaycast : public Test { // Heightfield shape (plane height field at height=4) - for (int32_t i=0; i<100; i++) mHeightFieldData[i] = 4; - mHeightFieldShape = new HeightFieldShape(10, 10, 0, 4, mHeightFieldData, HeightFieldShape::HEIGHT_FLOAT_TYPE); + for (int32_t i=0; i<100; i++) m_heightFieldData[i] = 4; + mHeightFieldShape = new HeightFieldShape(10, 10, 0, 4, m_heightFieldData, HeightFieldShape::HEIGHT_FLOAT_TYPE); mHeightFieldProxyShape = mHeightFieldBody->addCollisionShape(mHeightFieldShape, m_shapeTransform); // Assign proxy shapes to the different categories @@ -358,10 +358,10 @@ class TestRaycast : public Test { void testBox() { // ----- Test feedback data ----- // - Vector3 point1 = mLocalShapeToWorld * Vector3(1 , 2, 10); - Vector3 point2 = mLocalShapeToWorld * Vector3(1, 2, -20); + vec3 point1 = mLocalShapeToWorld * vec3(1 , 2, 10); + vec3 point2 = mLocalShapeToWorld * vec3(1, 2, -20); Ray ray(point1, point2); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(1, 2, 4); + vec3 hitPoint = mLocalShapeToWorld * vec3(1, 2, 4); mCallback.shapeToTest = mBoxProxyShape; @@ -372,9 +372,9 @@ class TestRaycast : public Test { test(mCallback.raycastInfo.body == mBoxBody); test(mCallback.raycastInfo.proxyShape == mBoxProxyShape); test(approxEqual(mCallback.raycastInfo.hitFraction, float(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z(), hitPoint.z, epsilon)); // Correct category filter mask mCallback.reset(); @@ -392,9 +392,9 @@ class TestRaycast : public Test { test(raycastInfo2.body == mBoxBody); test(raycastInfo2.proxyShape == mBoxProxyShape); test(approxEqual(raycastInfo2.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z(), hitPoint.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo3; @@ -402,26 +402,26 @@ class TestRaycast : public Test { test(raycastInfo3.body == mBoxBody); test(raycastInfo3.proxyShape == mBoxProxyShape); test(approxEqual(raycastInfo3.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z(), hitPoint.z, epsilon)); - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); - Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 29, 28)); - Ray ray3(mLocalShapeToWorld * Vector3(1, 2, 3), mLocalShapeToWorld * Vector3(-11, 2, 24)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31)); - Ray ray5(mLocalShapeToWorld * Vector3(3, 1, -5), mLocalShapeToWorld * Vector3(-30, 1, -5)); - Ray ray6(mLocalShapeToWorld * Vector3(4, 4, 1), mLocalShapeToWorld * Vector3(4, -20, 1)); - Ray ray7(mLocalShapeToWorld * Vector3(1, -4, 5), mLocalShapeToWorld * Vector3(1, -4, -20)); - Ray ray8(mLocalShapeToWorld * Vector3(-4, 4, 0), mLocalShapeToWorld * Vector3(20, 4, 0)); - Ray ray9(mLocalShapeToWorld * Vector3(0, -4, -7), mLocalShapeToWorld * Vector3(0, 50, -7)); - Ray ray10(mLocalShapeToWorld * Vector3(-3, 0, -6), mLocalShapeToWorld * Vector3(-3, 0, 20)); - Ray ray11(mLocalShapeToWorld * Vector3(3, 1, 2), mLocalShapeToWorld * Vector3(-20, 1, 2)); - Ray ray12(mLocalShapeToWorld * Vector3(1, 4, -1), mLocalShapeToWorld * Vector3(1, -20, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 5), mLocalShapeToWorld * Vector3(-1, 2, -20)); - Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -2), mLocalShapeToWorld * Vector3(20, 2, -2)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -4, 1), mLocalShapeToWorld * Vector3(0, 20, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -5), mLocalShapeToWorld * Vector3(-1, 2, 20)); + Ray ray1(mLocalShapeToWorld * vec3(0, 0, 0), mLocalShapeToWorld * vec3(5, 7, -1)); + Ray ray2(mLocalShapeToWorld * vec3(5, 11, 7), mLocalShapeToWorld * vec3(17, 29, 28)); + Ray ray3(mLocalShapeToWorld * vec3(1, 2, 3), mLocalShapeToWorld * vec3(-11, 2, 24)); + Ray ray4(mLocalShapeToWorld * vec3(10, 10, 10), mLocalShapeToWorld * vec3(22, 28, 31)); + Ray ray5(mLocalShapeToWorld * vec3(3, 1, -5), mLocalShapeToWorld * vec3(-30, 1, -5)); + Ray ray6(mLocalShapeToWorld * vec3(4, 4, 1), mLocalShapeToWorld * vec3(4, -20, 1)); + Ray ray7(mLocalShapeToWorld * vec3(1, -4, 5), mLocalShapeToWorld * vec3(1, -4, -20)); + Ray ray8(mLocalShapeToWorld * vec3(-4, 4, 0), mLocalShapeToWorld * vec3(20, 4, 0)); + Ray ray9(mLocalShapeToWorld * vec3(0, -4, -7), mLocalShapeToWorld * vec3(0, 50, -7)); + Ray ray10(mLocalShapeToWorld * vec3(-3, 0, -6), mLocalShapeToWorld * vec3(-3, 0, 20)); + Ray ray11(mLocalShapeToWorld * vec3(3, 1, 2), mLocalShapeToWorld * vec3(-20, 1, 2)); + Ray ray12(mLocalShapeToWorld * vec3(1, 4, -1), mLocalShapeToWorld * vec3(1, -20, -1)); + Ray ray13(mLocalShapeToWorld * vec3(-1, 2, 5), mLocalShapeToWorld * vec3(-1, 2, -20)); + Ray ray14(mLocalShapeToWorld * vec3(-3, 2, -2), mLocalShapeToWorld * vec3(20, 2, -2)); + Ray ray15(mLocalShapeToWorld * vec3(0, -4, 1), mLocalShapeToWorld * vec3(0, 20, 1)); + Ray ray16(mLocalShapeToWorld * vec3(-1, 2, -5), mLocalShapeToWorld * vec3(-1, 2, 20)); // ----- Test raycast miss ----- // test(!mBoxBody->raycast(ray1, raycastInfo3)); @@ -570,10 +570,10 @@ class TestRaycast : public Test { void testSphere() { // ----- Test feedback data ----- // - Vector3 point1 = mLocalShapeToWorld * Vector3(-5 , 0, 0); - Vector3 point2 = mLocalShapeToWorld * Vector3(5, 0, 0); + vec3 point1 = mLocalShapeToWorld * vec3(-5 , 0, 0); + vec3 point2 = mLocalShapeToWorld * vec3(5, 0, 0); Ray ray(point1, point2); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(-3, 0, 0); + vec3 hitPoint = mLocalShapeToWorld * vec3(-3, 0, 0); mCallback.shapeToTest = mSphereProxyShape; @@ -584,9 +584,9 @@ class TestRaycast : public Test { test(mCallback.raycastInfo.body == mSphereBody); test(mCallback.raycastInfo.proxyShape == mSphereProxyShape); test(approxEqual(mCallback.raycastInfo.hitFraction, 0.2, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z(), hitPoint.z, epsilon)); // Correct category filter mask mCallback.reset(); @@ -604,9 +604,9 @@ class TestRaycast : public Test { test(raycastInfo2.body == mSphereBody); test(raycastInfo2.proxyShape == mSphereProxyShape); test(approxEqual(raycastInfo2.hitFraction, 0.2, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z(), hitPoint.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo3; @@ -614,26 +614,26 @@ class TestRaycast : public Test { test(raycastInfo3.body == mSphereBody); test(raycastInfo3.proxyShape == mSphereProxyShape); test(approxEqual(raycastInfo3.hitFraction, 0.2, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z(), hitPoint.z, epsilon)); - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); - Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(4, 6, 7)); - Ray ray3(mLocalShapeToWorld * Vector3(1, 2, 2), mLocalShapeToWorld * Vector3(-4, 0, 7)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(4, 6, 7)); - Ray ray5(mLocalShapeToWorld * Vector3(4, 1, -5), mLocalShapeToWorld * Vector3(-30, 1, -5)); - Ray ray6(mLocalShapeToWorld * Vector3(4, 4, 1), mLocalShapeToWorld * Vector3(4, -30, 1)); - Ray ray7(mLocalShapeToWorld * Vector3(1, -4, 5), mLocalShapeToWorld * Vector3(1, -4, -30)); - Ray ray8(mLocalShapeToWorld * Vector3(-4, 4, 0), mLocalShapeToWorld * Vector3(30, 4, 0)); - Ray ray9(mLocalShapeToWorld * Vector3(0, -4, -4), mLocalShapeToWorld * Vector3(0, 30, -4)); - Ray ray10(mLocalShapeToWorld * Vector3(-4, 0, -6), mLocalShapeToWorld * Vector3(-4, 0, 30)); - Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 2), mLocalShapeToWorld * Vector3(-30, 1, 2)); - Ray ray12(mLocalShapeToWorld * Vector3(1, 4, -1), mLocalShapeToWorld * Vector3(1, -30, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 5), mLocalShapeToWorld * Vector3(-1, 2, -30)); - Ray ray14(mLocalShapeToWorld * Vector3(-5, 2, -2), mLocalShapeToWorld * Vector3(30, 2, -2)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -4, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -11), mLocalShapeToWorld * Vector3(-1, 2, 30)); + Ray ray1(mLocalShapeToWorld * vec3(0, 0, 0), mLocalShapeToWorld * vec3(5, 7, -1)); + Ray ray2(mLocalShapeToWorld * vec3(5, 11, 7), mLocalShapeToWorld * vec3(4, 6, 7)); + Ray ray3(mLocalShapeToWorld * vec3(1, 2, 2), mLocalShapeToWorld * vec3(-4, 0, 7)); + Ray ray4(mLocalShapeToWorld * vec3(10, 10, 10), mLocalShapeToWorld * vec3(4, 6, 7)); + Ray ray5(mLocalShapeToWorld * vec3(4, 1, -5), mLocalShapeToWorld * vec3(-30, 1, -5)); + Ray ray6(mLocalShapeToWorld * vec3(4, 4, 1), mLocalShapeToWorld * vec3(4, -30, 1)); + Ray ray7(mLocalShapeToWorld * vec3(1, -4, 5), mLocalShapeToWorld * vec3(1, -4, -30)); + Ray ray8(mLocalShapeToWorld * vec3(-4, 4, 0), mLocalShapeToWorld * vec3(30, 4, 0)); + Ray ray9(mLocalShapeToWorld * vec3(0, -4, -4), mLocalShapeToWorld * vec3(0, 30, -4)); + Ray ray10(mLocalShapeToWorld * vec3(-4, 0, -6), mLocalShapeToWorld * vec3(-4, 0, 30)); + Ray ray11(mLocalShapeToWorld * vec3(4, 1, 2), mLocalShapeToWorld * vec3(-30, 1, 2)); + Ray ray12(mLocalShapeToWorld * vec3(1, 4, -1), mLocalShapeToWorld * vec3(1, -30, -1)); + Ray ray13(mLocalShapeToWorld * vec3(-1, 2, 5), mLocalShapeToWorld * vec3(-1, 2, -30)); + Ray ray14(mLocalShapeToWorld * vec3(-5, 2, -2), mLocalShapeToWorld * vec3(30, 2, -2)); + Ray ray15(mLocalShapeToWorld * vec3(0, -4, 1), mLocalShapeToWorld * vec3(0, 30, 1)); + Ray ray16(mLocalShapeToWorld * vec3(-1, 2, -11), mLocalShapeToWorld * vec3(-1, 2, 30)); // ----- Test raycast miss ----- // test(!mSphereBody->raycast(ray1, raycastInfo3)); @@ -780,20 +780,20 @@ class TestRaycast : public Test { void testCapsule() { // ----- Test feedback data ----- // - Vector3 point1A = mLocalShapeToWorld * Vector3(4 , 1, 0); - Vector3 point1B = mLocalShapeToWorld * Vector3(-6, 1, 0); + vec3 point1A = mLocalShapeToWorld * vec3(4 , 1, 0); + vec3 point1B = mLocalShapeToWorld * vec3(-6, 1, 0); Ray ray(point1A, point1B); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(2, 1, 0); + vec3 hitPoint = mLocalShapeToWorld * vec3(2, 1, 0); - Vector3 point2A = mLocalShapeToWorld * Vector3(0 , 6.5, 0); - Vector3 point2B = mLocalShapeToWorld * Vector3(0, -3.5, 0); + vec3 point2A = mLocalShapeToWorld * vec3(0 , 6.5, 0); + vec3 point2B = mLocalShapeToWorld * vec3(0, -3.5, 0); Ray rayTop(point2A, point2B); - Vector3 hitPointTop = mLocalShapeToWorld * Vector3(0, float(4.5), 0); + vec3 hitPointTop = mLocalShapeToWorld * vec3(0, float(4.5), 0); - Vector3 point3A = mLocalShapeToWorld * Vector3(0 , -6.5, 0); - Vector3 point3B = mLocalShapeToWorld * Vector3(0, 3.5, 0); + vec3 point3A = mLocalShapeToWorld * vec3(0 , -6.5, 0); + vec3 point3B = mLocalShapeToWorld * vec3(0, 3.5, 0); Ray rayBottom(point3A, point3B); - Vector3 hitPointBottom = mLocalShapeToWorld * Vector3(0, float(-4.5), 0); + vec3 hitPointBottom = mLocalShapeToWorld * vec3(0, float(-4.5), 0); mCallback.shapeToTest = mCapsuleProxyShape; @@ -804,9 +804,9 @@ class TestRaycast : public Test { test(mCallback.raycastInfo.body == mCapsuleBody); test(mCallback.raycastInfo.proxyShape == mCapsuleProxyShape); test(approxEqual(mCallback.raycastInfo.hitFraction, float(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z(), hitPoint.z, epsilon)); // Correct category filter mask mCallback.reset(); @@ -824,9 +824,9 @@ class TestRaycast : public Test { test(raycastInfo2.body == mCapsuleBody); test(raycastInfo2.proxyShape == mCapsuleProxyShape); test(approxEqual(raycastInfo2.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z(), hitPoint.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo3; @@ -834,18 +834,18 @@ class TestRaycast : public Test { test(raycastInfo3.body == mCapsuleBody); test(raycastInfo3.proxyShape == mCapsuleProxyShape); test(approxEqual(raycastInfo3.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z(), hitPoint.z, epsilon)); RaycastInfo raycastInfo4; test(mCapsuleProxyShape->raycast(rayTop, raycastInfo4)); test(raycastInfo4.body == mCapsuleBody); test(raycastInfo4.proxyShape == mCapsuleProxyShape); test(approxEqual(raycastInfo4.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo4.worldPoint.x, hitPointTop.x, epsilon)); - test(approxEqual(raycastInfo4.worldPoint.y, hitPointTop.y, epsilon)); - test(approxEqual(raycastInfo4.worldPoint.z, hitPointTop.z, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.x(), hitPointTop.x, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.y(), hitPointTop.y, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.z(), hitPointTop.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo5; @@ -853,26 +853,26 @@ class TestRaycast : public Test { test(raycastInfo5.body == mCapsuleBody); test(raycastInfo5.proxyShape == mCapsuleProxyShape); test(approxEqual(raycastInfo5.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo5.worldPoint.x, hitPointBottom.x, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.y, hitPointBottom.y, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.z, hitPointBottom.z, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.x(), hitPointBottom.x, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.y(), hitPointBottom.y, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.z(), hitPointBottom.z, epsilon)); - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); - Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(9, 17, 14)); - Ray ray3(mLocalShapeToWorld * Vector3(1, 3, -1), mLocalShapeToWorld * Vector3(-3, 3, 6)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(14, 16, 17)); - Ray ray5(mLocalShapeToWorld * Vector3(4, 1, -5), mLocalShapeToWorld * Vector3(1, 1, -5)); - Ray ray6(mLocalShapeToWorld * Vector3(4, 9, 1), mLocalShapeToWorld * Vector3(4, 7, 1)); - Ray ray7(mLocalShapeToWorld * Vector3(1, -9, 5), mLocalShapeToWorld * Vector3(1, -9, 3)); - Ray ray8(mLocalShapeToWorld * Vector3(-4, 9, 0), mLocalShapeToWorld * Vector3(-3, 9, 0)); - Ray ray9(mLocalShapeToWorld * Vector3(0, -9, -4), mLocalShapeToWorld * Vector3(0, -4, -4)); - Ray ray10(mLocalShapeToWorld * Vector3(-4, 0, -6), mLocalShapeToWorld * Vector3(-4, 0, 2)); - Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 1.5), mLocalShapeToWorld * Vector3(-30, 1, 1.5)); - Ray ray12(mLocalShapeToWorld * Vector3(1, 9, -1), mLocalShapeToWorld * Vector3(1, -30, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 3), mLocalShapeToWorld * Vector3(-1, 2, -30)); - Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -1.7), mLocalShapeToWorld * Vector3(30, 2, -1.7)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -9, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); + Ray ray1(mLocalShapeToWorld * vec3(0, 0, 0), mLocalShapeToWorld * vec3(5, 7, -1)); + Ray ray2(mLocalShapeToWorld * vec3(5, 11, 7), mLocalShapeToWorld * vec3(9, 17, 14)); + Ray ray3(mLocalShapeToWorld * vec3(1, 3, -1), mLocalShapeToWorld * vec3(-3, 3, 6)); + Ray ray4(mLocalShapeToWorld * vec3(10, 10, 10), mLocalShapeToWorld * vec3(14, 16, 17)); + Ray ray5(mLocalShapeToWorld * vec3(4, 1, -5), mLocalShapeToWorld * vec3(1, 1, -5)); + Ray ray6(mLocalShapeToWorld * vec3(4, 9, 1), mLocalShapeToWorld * vec3(4, 7, 1)); + Ray ray7(mLocalShapeToWorld * vec3(1, -9, 5), mLocalShapeToWorld * vec3(1, -9, 3)); + Ray ray8(mLocalShapeToWorld * vec3(-4, 9, 0), mLocalShapeToWorld * vec3(-3, 9, 0)); + Ray ray9(mLocalShapeToWorld * vec3(0, -9, -4), mLocalShapeToWorld * vec3(0, -4, -4)); + Ray ray10(mLocalShapeToWorld * vec3(-4, 0, -6), mLocalShapeToWorld * vec3(-4, 0, 2)); + Ray ray11(mLocalShapeToWorld * vec3(4, 1, 1.5), mLocalShapeToWorld * vec3(-30, 1, 1.5)); + Ray ray12(mLocalShapeToWorld * vec3(1, 9, -1), mLocalShapeToWorld * vec3(1, -30, -1)); + Ray ray13(mLocalShapeToWorld * vec3(-1, 2, 3), mLocalShapeToWorld * vec3(-1, 2, -30)); + Ray ray14(mLocalShapeToWorld * vec3(-3, 2, -1.7), mLocalShapeToWorld * vec3(30, 2, -1.7)); + Ray ray15(mLocalShapeToWorld * vec3(0, -9, 1), mLocalShapeToWorld * vec3(0, 30, 1)); + Ray ray16(mLocalShapeToWorld * vec3(-1, 2, -7), mLocalShapeToWorld * vec3(-1, 2, 30)); // ----- Test raycast miss ----- // test(!mCapsuleBody->raycast(ray1, raycastInfo3)); @@ -1020,13 +1020,13 @@ class TestRaycast : public Test { void testTriangle() { // ----- Test feedback data ----- // - Vector3 point1 = mLocalShapeToWorld * Vector3(101, 101, 400); - Vector3 point2 = mLocalShapeToWorld * Vector3(101, 101, -200); + vec3 point1 = mLocalShapeToWorld * vec3(101, 101, 400); + vec3 point2 = mLocalShapeToWorld * vec3(101, 101, -200); Ray ray(point1, point2); Ray rayBackward(point2, point1); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(101, 101, 0); - Vector3 hitNormal = mLocalShapeToWorld.getOrientation() * Vector3(0, 0, 1); + vec3 hitPoint = mLocalShapeToWorld * vec3(101, 101, 0); + vec3 hitNormal = mLocalShapeToWorld.getOrientation() * vec3(0, 0, 1); hitNormal.normalize(); mCallback.shapeToTest = mTriangleProxyShape; @@ -1038,12 +1038,12 @@ class TestRaycast : public Test { test(mCallback.raycastInfo.body == mTriangleBody); test(mCallback.raycastInfo.proxyShape == mTriangleProxyShape); test(approxEqual(mCallback.raycastInfo.hitFraction, 0.6666, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.x, hitNormal.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.y, hitNormal.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.z, hitNormal.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z(), hitPoint.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.x(), hitNormal.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.y(), hitNormal.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.z(), hitNormal.z, epsilon)); mCallback.reset(); mTriangleShape->setRaycastTestType(BACK); @@ -1052,12 +1052,12 @@ class TestRaycast : public Test { test(mCallback.raycastInfo.body == mTriangleBody); test(mCallback.raycastInfo.proxyShape == mTriangleProxyShape); test(approxEqual(mCallback.raycastInfo.hitFraction, 0.3333, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.x, -hitNormal.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.y, -hitNormal.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.z, -hitNormal.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z(), hitPoint.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.x(), -hitNormal.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.y(), -hitNormal.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.z(), -hitNormal.z, epsilon)); mCallback.reset(); mTriangleShape->setRaycastTestType(FRONT_AND_BACK); @@ -1066,12 +1066,12 @@ class TestRaycast : public Test { test(mCallback.raycastInfo.body == mTriangleBody); test(mCallback.raycastInfo.proxyShape == mTriangleProxyShape); test(approxEqual(mCallback.raycastInfo.hitFraction, 0.6666, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.x, hitNormal.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.y, hitNormal.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.z, hitNormal.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z(), hitPoint.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.x(), hitNormal.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.y(), hitNormal.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.z(), hitNormal.z, epsilon)); mCallback.reset(); mTriangleShape->setRaycastTestType(FRONT_AND_BACK); @@ -1080,12 +1080,12 @@ class TestRaycast : public Test { test(mCallback.raycastInfo.body == mTriangleBody); test(mCallback.raycastInfo.proxyShape == mTriangleProxyShape); test(approxEqual(mCallback.raycastInfo.hitFraction, 0.3333, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.x, -hitNormal.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.y, -hitNormal.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.z, -hitNormal.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z(), hitPoint.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.x(), -hitNormal.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.y(), -hitNormal.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.z(), -hitNormal.z, epsilon)); mTriangleShape->setRaycastTestType(FRONT); @@ -1105,9 +1105,9 @@ class TestRaycast : public Test { test(raycastInfo2.body == mTriangleBody); test(raycastInfo2.proxyShape == mTriangleProxyShape); test(approxEqual(raycastInfo2.hitFraction, 0.6666, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z(), hitPoint.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo3; @@ -1115,21 +1115,21 @@ class TestRaycast : public Test { test(raycastInfo3.body == mTriangleBody); test(raycastInfo3.proxyShape == mTriangleProxyShape); test(approxEqual(raycastInfo3.hitFraction, 0.6666, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z(), hitPoint.z, epsilon)); - Ray ray1(mLocalShapeToWorld * Vector3(-10, 10, 4), mLocalShapeToWorld * Vector3(15, 6, -4)); - Ray ray2(mLocalShapeToWorld * Vector3(102, 107, 5), mLocalShapeToWorld * Vector3(102, 107, -5)); - Ray ray3(mLocalShapeToWorld * Vector3(106, 102, 6), mLocalShapeToWorld * Vector3(106, 102, -8)); + Ray ray1(mLocalShapeToWorld * vec3(-10, 10, 4), mLocalShapeToWorld * vec3(15, 6, -4)); + Ray ray2(mLocalShapeToWorld * vec3(102, 107, 5), mLocalShapeToWorld * vec3(102, 107, -5)); + Ray ray3(mLocalShapeToWorld * vec3(106, 102, 6), mLocalShapeToWorld * vec3(106, 102, -8)); - Ray ray4(mLocalShapeToWorld * Vector3(100.2, 101, 5), mLocalShapeToWorld * Vector3(100.2, 101, -5)); - Ray ray5(mLocalShapeToWorld * Vector3(100.5, 101.5, 4), mLocalShapeToWorld * Vector3(100.5, 101.5, -54)); - Ray ray6(mLocalShapeToWorld * Vector3(102, 101, 1), mLocalShapeToWorld * Vector3(102, 102, -1)); + Ray ray4(mLocalShapeToWorld * vec3(100.2, 101, 5), mLocalShapeToWorld * vec3(100.2, 101, -5)); + Ray ray5(mLocalShapeToWorld * vec3(100.5, 101.5, 4), mLocalShapeToWorld * vec3(100.5, 101.5, -54)); + Ray ray6(mLocalShapeToWorld * vec3(102, 101, 1), mLocalShapeToWorld * vec3(102, 102, -1)); - Ray ray4Back(mLocalShapeToWorld * Vector3(100.2, 101, -5), mLocalShapeToWorld * Vector3(100.2, 101, 5)); - Ray ray5Back(mLocalShapeToWorld * Vector3(100.5, 101.5, -54), mLocalShapeToWorld * Vector3(100.5, 101.5, 4)); - Ray ray6Back(mLocalShapeToWorld * Vector3(102, 102, -1), mLocalShapeToWorld * Vector3(102, 101, 1)); + Ray ray4Back(mLocalShapeToWorld * vec3(100.2, 101, -5), mLocalShapeToWorld * vec3(100.2, 101, 5)); + Ray ray5Back(mLocalShapeToWorld * vec3(100.5, 101.5, -54), mLocalShapeToWorld * vec3(100.5, 101.5, 4)); + Ray ray6Back(mLocalShapeToWorld * vec3(102, 102, -1), mLocalShapeToWorld * vec3(102, 101, 1)); // ----- Test raycast miss ----- // test(!mTriangleBody->raycast(ray1, raycastInfo3)); @@ -1246,7 +1246,7 @@ class TestRaycast : public Test { m_world->raycast(ray5Back, &mCallback); test(mCallback.isHit); mCallback.reset(); - m_world->raycast(Ray(ray5Back.point1, ray5Back.point2, float(1.0)), &mCallback); + m_world->raycast(Ray(ray5Back.point1, ray5Back.point2, 1.0f), &mCallback); test(mCallback.isHit); test(mTriangleBody->raycast(ray6Back, raycastInfo3)); @@ -1302,7 +1302,7 @@ class TestRaycast : public Test { m_world->raycast(ray5Back, &mCallback); test(mCallback.isHit); mCallback.reset(); - m_world->raycast(Ray(ray5Back.point1, ray5Back.point2, float(1.0)), &mCallback); + m_world->raycast(Ray(ray5Back.point1, ray5Back.point2, 1.0f), &mCallback); test(mCallback.isHit); test(mTriangleBody->raycast(ray6Back, raycastInfo3)); @@ -1318,15 +1318,15 @@ class TestRaycast : public Test { void testCone() { // ----- Test feedback data ----- // - Vector3 point1A = mLocalShapeToWorld * Vector3(0 , 0, 3); - Vector3 point1B = mLocalShapeToWorld * Vector3(0, 0, -7); + vec3 point1A = mLocalShapeToWorld * vec3(0 , 0, 3); + vec3 point1B = mLocalShapeToWorld * vec3(0, 0, -7); Ray ray(point1A, point1B); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(0, 0, 1); + vec3 hitPoint = mLocalShapeToWorld * vec3(0, 0, 1); - Vector3 point2A = mLocalShapeToWorld * Vector3(1 , -5, 0); - Vector3 point2B = mLocalShapeToWorld * Vector3(1, 5, 0); + vec3 point2A = mLocalShapeToWorld * vec3(1 , -5, 0); + vec3 point2B = mLocalShapeToWorld * vec3(1, 5, 0); Ray rayBottom(point2A, point2B); - Vector3 hitPoint2 = mLocalShapeToWorld * Vector3(1, -3, 0); + vec3 hitPoint2 = mLocalShapeToWorld * vec3(1, -3, 0); mCallback.shapeToTest = mConeProxyShape; @@ -1337,9 +1337,9 @@ class TestRaycast : public Test { test(mCallback.raycastInfo.body == mConeBody); test(mCallback.raycastInfo.proxyShape == mConeProxyShape); test(approxEqual(mCallback.raycastInfo.hitFraction, float(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z(), hitPoint.z, epsilon)); // Correct category filter mask mCallback.reset(); @@ -1357,9 +1357,9 @@ class TestRaycast : public Test { test(raycastInfo2.body == mConeBody); test(raycastInfo2.proxyShape == mConeProxyShape); test(approxEqual(raycastInfo2.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z(), hitPoint.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo3; @@ -1367,9 +1367,9 @@ class TestRaycast : public Test { test(raycastInfo3.body == mConeBody); test(raycastInfo3.proxyShape == mConeProxyShape); test(approxEqual(raycastInfo3.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z(), hitPoint.z, epsilon)); mCallback.reset(); m_world->raycast(rayBottom, &mCallback); @@ -1377,9 +1377,9 @@ class TestRaycast : public Test { test(mCallback.raycastInfo.body == mConeBody); test(mCallback.raycastInfo.proxyShape == mConeProxyShape); test(approxEqual(mCallback.raycastInfo.hitFraction, float(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint2.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint2.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint2.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x(), hitPoint2.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y(), hitPoint2.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z(), hitPoint2.z, epsilon)); // CollisionBody::raycast() RaycastInfo raycastInfo5; @@ -1387,9 +1387,9 @@ class TestRaycast : public Test { test(raycastInfo5.body == mConeBody); test(raycastInfo5.proxyShape == mConeProxyShape); test(approxEqual(raycastInfo5.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo5.worldPoint.x, hitPoint2.x, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.y, hitPoint2.y, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.z, hitPoint2.z, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.x(), hitPoint2.x, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.y(), hitPoint2.y, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.z(), hitPoint2.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo6; @@ -1397,26 +1397,26 @@ class TestRaycast : public Test { test(raycastInfo6.body == mConeBody); test(raycastInfo6.proxyShape == mConeProxyShape); test(approxEqual(raycastInfo6.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo6.worldPoint.x, hitPoint2.x, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.y, hitPoint2.y, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.z, hitPoint2.z, epsilon)); + test(approxEqual(raycastInfo6.worldPoint.x(), hitPoint2.x, epsilon)); + test(approxEqual(raycastInfo6.worldPoint.y(), hitPoint2.y, epsilon)); + test(approxEqual(raycastInfo6.worldPoint.z(), hitPoint2.z, epsilon)); - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); - Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 29, 28)); - Ray ray3(mLocalShapeToWorld * Vector3(-1, -2, 1), mLocalShapeToWorld * Vector3(-13, -2, 22)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31)); - Ray ray5(mLocalShapeToWorld * Vector3(4, 1, -1), mLocalShapeToWorld * Vector3(-26, 1, -1)); - Ray ray6(mLocalShapeToWorld * Vector3(3, 4, 1), mLocalShapeToWorld * Vector3(3, -16, 1)); - Ray ray7(mLocalShapeToWorld * Vector3(1, -4, 3), mLocalShapeToWorld * Vector3(1, -4, -17)); - Ray ray8(mLocalShapeToWorld * Vector3(-4, 4, 0), mLocalShapeToWorld * Vector3(26, 4, 0)); - Ray ray9(mLocalShapeToWorld * Vector3(0, -4, -7), mLocalShapeToWorld * Vector3(0, 46, -7)); - Ray ray10(mLocalShapeToWorld * Vector3(-3, -2, -6), mLocalShapeToWorld * Vector3(-3, -2, 74)); - Ray ray11(mLocalShapeToWorld * Vector3(3, -1, 0.5), mLocalShapeToWorld * Vector3(-27, -1, 0.5)); - Ray ray12(mLocalShapeToWorld * Vector3(1, 4, -1), mLocalShapeToWorld * Vector3(1, -26, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, -2, 3), mLocalShapeToWorld * Vector3(-1, -2, -27)); - Ray ray14(mLocalShapeToWorld * Vector3(-2, 0, 0.8), mLocalShapeToWorld * Vector3(30, 0, 0.8)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -4, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-0.9, 0, -4), mLocalShapeToWorld * Vector3(-0.9, 0, 30)); + Ray ray1(mLocalShapeToWorld * vec3(0, 0, 0), mLocalShapeToWorld * vec3(5, 7, -1)); + Ray ray2(mLocalShapeToWorld * vec3(5, 11, 7), mLocalShapeToWorld * vec3(17, 29, 28)); + Ray ray3(mLocalShapeToWorld * vec3(-1, -2, 1), mLocalShapeToWorld * vec3(-13, -2, 22)); + Ray ray4(mLocalShapeToWorld * vec3(10, 10, 10), mLocalShapeToWorld * vec3(22, 28, 31)); + Ray ray5(mLocalShapeToWorld * vec3(4, 1, -1), mLocalShapeToWorld * vec3(-26, 1, -1)); + Ray ray6(mLocalShapeToWorld * vec3(3, 4, 1), mLocalShapeToWorld * vec3(3, -16, 1)); + Ray ray7(mLocalShapeToWorld * vec3(1, -4, 3), mLocalShapeToWorld * vec3(1, -4, -17)); + Ray ray8(mLocalShapeToWorld * vec3(-4, 4, 0), mLocalShapeToWorld * vec3(26, 4, 0)); + Ray ray9(mLocalShapeToWorld * vec3(0, -4, -7), mLocalShapeToWorld * vec3(0, 46, -7)); + Ray ray10(mLocalShapeToWorld * vec3(-3, -2, -6), mLocalShapeToWorld * vec3(-3, -2, 74)); + Ray ray11(mLocalShapeToWorld * vec3(3, -1, 0.5), mLocalShapeToWorld * vec3(-27, -1, 0.5)); + Ray ray12(mLocalShapeToWorld * vec3(1, 4, -1), mLocalShapeToWorld * vec3(1, -26, -1)); + Ray ray13(mLocalShapeToWorld * vec3(-1, -2, 3), mLocalShapeToWorld * vec3(-1, -2, -27)); + Ray ray14(mLocalShapeToWorld * vec3(-2, 0, 0.8), mLocalShapeToWorld * vec3(30, 0, 0.8)); + Ray ray15(mLocalShapeToWorld * vec3(0, -4, 1), mLocalShapeToWorld * vec3(0, 30, 1)); + Ray ray16(mLocalShapeToWorld * vec3(-0.9, 0, -4), mLocalShapeToWorld * vec3(-0.9, 0, 30)); // ----- Test raycast miss ----- // test(!mConeBody->raycast(ray1, raycastInfo3)); @@ -1565,10 +1565,10 @@ class TestRaycast : public Test { void testConvexMesh() { // ----- Test feedback data ----- // - Vector3 point1 = mLocalShapeToWorld * Vector3(1 , 2, 6); - Vector3 point2 = mLocalShapeToWorld * Vector3(1, 2, -4); + vec3 point1 = mLocalShapeToWorld * vec3(1 , 2, 6); + vec3 point2 = mLocalShapeToWorld * vec3(1, 2, -4); Ray ray(point1, point2); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(1, 2, 4); + vec3 hitPoint = mLocalShapeToWorld * vec3(1, 2, 4); mCallback.shapeToTest = mConvexMeshProxyShape; @@ -1579,9 +1579,9 @@ class TestRaycast : public Test { test(mCallback.raycastInfo.body == mConvexMeshBody); test(mCallback.raycastInfo.proxyShape == mConvexMeshProxyShape); test(approxEqual(mCallback.raycastInfo.hitFraction, float(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z(), hitPoint.z, epsilon)); // Correct category filter mask mCallback.reset(); @@ -1599,9 +1599,9 @@ class TestRaycast : public Test { test(raycastInfo2.body == mConvexMeshBody); test(raycastInfo2.proxyShape == mConvexMeshProxyShape); test(approxEqual(raycastInfo2.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z(), hitPoint.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo3; @@ -1609,9 +1609,9 @@ class TestRaycast : public Test { test(raycastInfo3.body == mConvexMeshBodyEdgesInfo); test(raycastInfo3.proxyShape == mConvexMeshProxyShapeEdgesInfo); test(approxEqual(raycastInfo3.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z(), hitPoint.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo4; @@ -1619,9 +1619,9 @@ class TestRaycast : public Test { test(raycastInfo4.body == mConvexMeshBody); test(raycastInfo4.proxyShape == mConvexMeshProxyShape); test(approxEqual(raycastInfo4.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo4.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo4.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo4.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.z(), hitPoint.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo5; @@ -1629,26 +1629,26 @@ class TestRaycast : public Test { test(raycastInfo5.body == mConvexMeshBodyEdgesInfo); test(raycastInfo5.proxyShape == mConvexMeshProxyShapeEdgesInfo); test(approxEqual(raycastInfo5.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo5.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.z(), hitPoint.z, epsilon)); - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); - Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 29, 28)); - Ray ray3(mLocalShapeToWorld * Vector3(1, 2, 3), mLocalShapeToWorld * Vector3(-11, 2, 24)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31)); - Ray ray5(mLocalShapeToWorld * Vector3(3, 1, -5), mLocalShapeToWorld * Vector3(-30, 1, -5)); - Ray ray6(mLocalShapeToWorld * Vector3(4, 4, 1), mLocalShapeToWorld * Vector3(4, -30, 1)); - Ray ray7(mLocalShapeToWorld * Vector3(1, -4, 5), mLocalShapeToWorld * Vector3(1, -4, -30)); - Ray ray8(mLocalShapeToWorld * Vector3(-4, 4, 0), mLocalShapeToWorld * Vector3(30, 4, 0)); - Ray ray9(mLocalShapeToWorld * Vector3(0, -4, -7), mLocalShapeToWorld * Vector3(0, 30, -7)); - Ray ray10(mLocalShapeToWorld * Vector3(-3, 0, -6), mLocalShapeToWorld * Vector3(-3, 0, 30)); - Ray ray11(mLocalShapeToWorld * Vector3(3, 1, 2), mLocalShapeToWorld * Vector3(-30, 0, -6)); - Ray ray12(mLocalShapeToWorld * Vector3(1, 4, -1), mLocalShapeToWorld * Vector3(1, -30, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 5), mLocalShapeToWorld * Vector3(-1, 2, -30)); - Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -2), mLocalShapeToWorld * Vector3(30, 2, -2)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -4, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); + Ray ray1(mLocalShapeToWorld * vec3(0, 0, 0), mLocalShapeToWorld * vec3(5, 7, -1)); + Ray ray2(mLocalShapeToWorld * vec3(5, 11, 7), mLocalShapeToWorld * vec3(17, 29, 28)); + Ray ray3(mLocalShapeToWorld * vec3(1, 2, 3), mLocalShapeToWorld * vec3(-11, 2, 24)); + Ray ray4(mLocalShapeToWorld * vec3(10, 10, 10), mLocalShapeToWorld * vec3(22, 28, 31)); + Ray ray5(mLocalShapeToWorld * vec3(3, 1, -5), mLocalShapeToWorld * vec3(-30, 1, -5)); + Ray ray6(mLocalShapeToWorld * vec3(4, 4, 1), mLocalShapeToWorld * vec3(4, -30, 1)); + Ray ray7(mLocalShapeToWorld * vec3(1, -4, 5), mLocalShapeToWorld * vec3(1, -4, -30)); + Ray ray8(mLocalShapeToWorld * vec3(-4, 4, 0), mLocalShapeToWorld * vec3(30, 4, 0)); + Ray ray9(mLocalShapeToWorld * vec3(0, -4, -7), mLocalShapeToWorld * vec3(0, 30, -7)); + Ray ray10(mLocalShapeToWorld * vec3(-3, 0, -6), mLocalShapeToWorld * vec3(-3, 0, 30)); + Ray ray11(mLocalShapeToWorld * vec3(3, 1, 2), mLocalShapeToWorld * vec3(-30, 0, -6)); + Ray ray12(mLocalShapeToWorld * vec3(1, 4, -1), mLocalShapeToWorld * vec3(1, -30, -1)); + Ray ray13(mLocalShapeToWorld * vec3(-1, 2, 5), mLocalShapeToWorld * vec3(-1, 2, -30)); + Ray ray14(mLocalShapeToWorld * vec3(-3, 2, -2), mLocalShapeToWorld * vec3(30, 2, -2)); + Ray ray15(mLocalShapeToWorld * vec3(0, -4, 1), mLocalShapeToWorld * vec3(0, 30, 1)); + Ray ray16(mLocalShapeToWorld * vec3(-1, 2, -7), mLocalShapeToWorld * vec3(-1, 2, 30)); // ----- Test raycast miss ----- // test(!mConvexMeshBody->raycast(ray1, raycastInfo3)); @@ -1829,20 +1829,20 @@ class TestRaycast : public Test { void testCylinder() { // ----- Test feedback data ----- // - Vector3 point1A = mLocalShapeToWorld * Vector3(4 , 1, 0); - Vector3 point1B = mLocalShapeToWorld * Vector3(-6, 1, 0); + vec3 point1A = mLocalShapeToWorld * vec3(4 , 1, 0); + vec3 point1B = mLocalShapeToWorld * vec3(-6, 1, 0); Ray ray(point1A, point1B); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(2, 1, 0); + vec3 hitPoint = mLocalShapeToWorld * vec3(2, 1, 0); - Vector3 point2A = mLocalShapeToWorld * Vector3(0 , 4.5, 0); - Vector3 point2B = mLocalShapeToWorld * Vector3(0, -5.5, 0); + vec3 point2A = mLocalShapeToWorld * vec3(0 , 4.5, 0); + vec3 point2B = mLocalShapeToWorld * vec3(0, -5.5, 0); Ray rayTop(point2A, point2B); - Vector3 hitPointTop = mLocalShapeToWorld * Vector3(0, float(2.5), 0); + vec3 hitPointTop = mLocalShapeToWorld * vec3(0, float(2.5), 0); - Vector3 point3A = mLocalShapeToWorld * Vector3(0 , -4.5, 0); - Vector3 point3B = mLocalShapeToWorld * Vector3(0, 5.5, 0); + vec3 point3A = mLocalShapeToWorld * vec3(0 , -4.5, 0); + vec3 point3B = mLocalShapeToWorld * vec3(0, 5.5, 0); Ray rayBottom(point3A, point3B); - Vector3 hitPointBottom = mLocalShapeToWorld * Vector3(0, float(-2.5), 0); + vec3 hitPointBottom = mLocalShapeToWorld * vec3(0, float(-2.5), 0); mCallback.shapeToTest = mCylinderProxyShape; @@ -1853,9 +1853,9 @@ class TestRaycast : public Test { test(mCallback.raycastInfo.body == mCylinderBody); test(mCallback.raycastInfo.proxyShape == mCylinderProxyShape); test(approxEqual(mCallback.raycastInfo.hitFraction, float(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z(), hitPoint.z, epsilon)); // Correct category filter mask mCallback.reset(); @@ -1873,9 +1873,9 @@ class TestRaycast : public Test { test(raycastInfo2.body == mCylinderBody); test(raycastInfo2.proxyShape == mCylinderProxyShape); test(approxEqual(raycastInfo2.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z(), hitPoint.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo3; @@ -1883,9 +1883,9 @@ class TestRaycast : public Test { test(raycastInfo3.body == mCylinderBody); test(raycastInfo3.proxyShape == mCylinderProxyShape); test(approxEqual(raycastInfo3.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z(), hitPoint.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo5; @@ -1893,9 +1893,9 @@ class TestRaycast : public Test { test(raycastInfo5.body == mCylinderBody); test(raycastInfo5.proxyShape == mCylinderProxyShape); test(approxEqual(raycastInfo5.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo5.worldPoint.x, hitPointTop.x, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.y, hitPointTop.y, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.z, hitPointTop.z, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.x(), hitPointTop.x, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.y(), hitPointTop.y, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.z(), hitPointTop.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo6; @@ -1903,26 +1903,26 @@ class TestRaycast : public Test { test(raycastInfo6.body == mCylinderBody); test(raycastInfo6.proxyShape == mCylinderProxyShape); test(approxEqual(raycastInfo6.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo6.worldPoint.x, hitPointBottom.x, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.y, hitPointBottom.y, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.z, hitPointBottom.z, epsilon)); + test(approxEqual(raycastInfo6.worldPoint.x(), hitPointBottom.x, epsilon)); + test(approxEqual(raycastInfo6.worldPoint.y(), hitPointBottom.y, epsilon)); + test(approxEqual(raycastInfo6.worldPoint.z(), hitPointBottom.z, epsilon)); - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); - Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 20, 28)); - Ray ray3(mLocalShapeToWorld * Vector3(1, 3, -1), mLocalShapeToWorld * Vector3(-11,3, 20)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31)); - Ray ray5(mLocalShapeToWorld * Vector3(4, 1, -5), mLocalShapeToWorld * Vector3(-30, 1, -5)); - Ray ray6(mLocalShapeToWorld * Vector3(4, 9, 1), mLocalShapeToWorld * Vector3(4, -30, 1)); - Ray ray7(mLocalShapeToWorld * Vector3(1, -9, 5), mLocalShapeToWorld * Vector3(1, -9, -30)); - Ray ray8(mLocalShapeToWorld * Vector3(-4, 9, 0), mLocalShapeToWorld * Vector3(30, 9, 0)); - Ray ray9(mLocalShapeToWorld * Vector3(0, -9, -4), mLocalShapeToWorld * Vector3(0, 30, -4)); - Ray ray10(mLocalShapeToWorld * Vector3(-4, 0, -6), mLocalShapeToWorld * Vector3(-4, 0, 30)); - Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 1.5), mLocalShapeToWorld * Vector3(-30, 1, 1.5)); - Ray ray12(mLocalShapeToWorld * Vector3(1, 9, -1), mLocalShapeToWorld * Vector3(1, -30, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 3), mLocalShapeToWorld * Vector3(-1, 2, -30)); - Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -1.7), mLocalShapeToWorld * Vector3(30, 2, -1.7)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -9, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); + Ray ray1(mLocalShapeToWorld * vec3(0, 0, 0), mLocalShapeToWorld * vec3(5, 7, -1)); + Ray ray2(mLocalShapeToWorld * vec3(5, 11, 7), mLocalShapeToWorld * vec3(17, 20, 28)); + Ray ray3(mLocalShapeToWorld * vec3(1, 3, -1), mLocalShapeToWorld * vec3(-11,3, 20)); + Ray ray4(mLocalShapeToWorld * vec3(10, 10, 10), mLocalShapeToWorld * vec3(22, 28, 31)); + Ray ray5(mLocalShapeToWorld * vec3(4, 1, -5), mLocalShapeToWorld * vec3(-30, 1, -5)); + Ray ray6(mLocalShapeToWorld * vec3(4, 9, 1), mLocalShapeToWorld * vec3(4, -30, 1)); + Ray ray7(mLocalShapeToWorld * vec3(1, -9, 5), mLocalShapeToWorld * vec3(1, -9, -30)); + Ray ray8(mLocalShapeToWorld * vec3(-4, 9, 0), mLocalShapeToWorld * vec3(30, 9, 0)); + Ray ray9(mLocalShapeToWorld * vec3(0, -9, -4), mLocalShapeToWorld * vec3(0, 30, -4)); + Ray ray10(mLocalShapeToWorld * vec3(-4, 0, -6), mLocalShapeToWorld * vec3(-4, 0, 30)); + Ray ray11(mLocalShapeToWorld * vec3(4, 1, 1.5), mLocalShapeToWorld * vec3(-30, 1, 1.5)); + Ray ray12(mLocalShapeToWorld * vec3(1, 9, -1), mLocalShapeToWorld * vec3(1, -30, -1)); + Ray ray13(mLocalShapeToWorld * vec3(-1, 2, 3), mLocalShapeToWorld * vec3(-1, 2, -30)); + Ray ray14(mLocalShapeToWorld * vec3(-3, 2, -1.7), mLocalShapeToWorld * vec3(30, 2, -1.7)); + Ray ray15(mLocalShapeToWorld * vec3(0, -9, 1), mLocalShapeToWorld * vec3(0, 30, 1)); + Ray ray16(mLocalShapeToWorld * vec3(-1, 2, -7), mLocalShapeToWorld * vec3(-1, 2, 30)); // ----- Test raycast miss ----- // test(!mCylinderBody->raycast(ray1, raycastInfo3)); @@ -2073,12 +2073,12 @@ class TestRaycast : public Test { // ----- Test feedback data ----- // // Raycast hit against the sphere shape - Ray ray1(mLocalShape2ToWorld * Vector3(4, 1, 2), mLocalShape2ToWorld * Vector3(-30, 1, 2)); - Ray ray2(mLocalShape2ToWorld * Vector3(1, 4, -1), mLocalShape2ToWorld * Vector3(1, -30, -1)); - Ray ray3(mLocalShape2ToWorld * Vector3(-1, 2, 5), mLocalShape2ToWorld * Vector3(-1, 2, -30)); - Ray ray4(mLocalShape2ToWorld * Vector3(-5, 2, -2), mLocalShape2ToWorld * Vector3(30, 2, -2)); - Ray ray5(mLocalShape2ToWorld * Vector3(0, -4, 1), mLocalShape2ToWorld * Vector3(0, 30, 1)); - Ray ray6(mLocalShape2ToWorld * Vector3(-1, 2, -11), mLocalShape2ToWorld * Vector3(-1, 2, 30)); + Ray ray1(mLocalShape2ToWorld * vec3(4, 1, 2), mLocalShape2ToWorld * vec3(-30, 1, 2)); + Ray ray2(mLocalShape2ToWorld * vec3(1, 4, -1), mLocalShape2ToWorld * vec3(1, -30, -1)); + Ray ray3(mLocalShape2ToWorld * vec3(-1, 2, 5), mLocalShape2ToWorld * vec3(-1, 2, -30)); + Ray ray4(mLocalShape2ToWorld * vec3(-5, 2, -2), mLocalShape2ToWorld * vec3(30, 2, -2)); + Ray ray5(mLocalShape2ToWorld * vec3(0, -4, 1), mLocalShape2ToWorld * vec3(0, 30, 1)); + Ray ray6(mLocalShape2ToWorld * vec3(-1, 2, -11), mLocalShape2ToWorld * vec3(-1, 2, 30)); mCallback.shapeToTest = mCompoundSphereProxyShape; @@ -2142,12 +2142,12 @@ class TestRaycast : public Test { test(mCallback.isHit); // Raycast hit agains the cylinder shape - Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 1.5), mLocalShapeToWorld * Vector3(-30, 1.5, 2)); - Ray ray12(mLocalShapeToWorld * Vector3(1.5, 9, -1), mLocalShapeToWorld * Vector3(1.5, -30, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 3), mLocalShapeToWorld * Vector3(-1, 2, -30)); - Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -1.5), mLocalShapeToWorld * Vector3(30, 1, -1.5)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -9, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); + Ray ray11(mLocalShapeToWorld * vec3(4, 1, 1.5), mLocalShapeToWorld * vec3(-30, 1.5, 2)); + Ray ray12(mLocalShapeToWorld * vec3(1.5, 9, -1), mLocalShapeToWorld * vec3(1.5, -30, -1)); + Ray ray13(mLocalShapeToWorld * vec3(-1, 2, 3), mLocalShapeToWorld * vec3(-1, 2, -30)); + Ray ray14(mLocalShapeToWorld * vec3(-3, 2, -1.5), mLocalShapeToWorld * vec3(30, 1, -1.5)); + Ray ray15(mLocalShapeToWorld * vec3(0, -9, 1), mLocalShapeToWorld * vec3(0, 30, 1)); + Ray ray16(mLocalShapeToWorld * vec3(-1, 2, -7), mLocalShapeToWorld * vec3(-1, 2, 30)); mCallback.shapeToTest = mCompoundCylinderProxyShape; @@ -2204,10 +2204,10 @@ class TestRaycast : public Test { void testConcaveMesh() { // ----- Test feedback data ----- // - Vector3 point1 = mLocalShapeToWorld * Vector3(1 , 2, 6); - Vector3 point2 = mLocalShapeToWorld * Vector3(1, 2, -4); + vec3 point1 = mLocalShapeToWorld * vec3(1 , 2, 6); + vec3 point2 = mLocalShapeToWorld * vec3(1, 2, -4); Ray ray(point1, point2); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(1, 2, 4); + vec3 hitPoint = mLocalShapeToWorld * vec3(1, 2, 4); mCallback.shapeToTest = mConcaveMeshProxyShape; @@ -2218,9 +2218,9 @@ class TestRaycast : public Test { test(mCallback.raycastInfo.body == mConcaveMeshBody); test(mCallback.raycastInfo.proxyShape == mConcaveMeshProxyShape); test(approxEqual(mCallback.raycastInfo.hitFraction, float(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z(), hitPoint.z, epsilon)); // Correct category filter mask mCallback.reset(); @@ -2238,9 +2238,9 @@ class TestRaycast : public Test { test(raycastInfo2.body == mConcaveMeshBody); test(raycastInfo2.proxyShape == mConcaveMeshProxyShape); test(approxEqual(raycastInfo2.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z(), hitPoint.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo3; @@ -2248,9 +2248,9 @@ class TestRaycast : public Test { test(raycastInfo3.body == mConcaveMeshBody); test(raycastInfo3.proxyShape == mConcaveMeshProxyShape); test(approxEqual(raycastInfo3.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z(), hitPoint.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo4; @@ -2258,9 +2258,9 @@ class TestRaycast : public Test { test(raycastInfo4.body == mConcaveMeshBody); test(raycastInfo4.proxyShape == mConcaveMeshProxyShape); test(approxEqual(raycastInfo4.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo4.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo4.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo4.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.z(), hitPoint.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo5; @@ -2268,26 +2268,26 @@ class TestRaycast : public Test { test(raycastInfo5.body == mConcaveMeshBody); test(raycastInfo5.proxyShape == mConcaveMeshProxyShape); test(approxEqual(raycastInfo5.hitFraction, float(0.2), epsilon)); - test(approxEqual(raycastInfo5.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.z(), hitPoint.z, epsilon)); - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); - Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 29, 28)); - Ray ray3(mLocalShapeToWorld * Vector3(1, 2, 3), mLocalShapeToWorld * Vector3(-11, 2, 24)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31)); - Ray ray5(mLocalShapeToWorld * Vector3(3, 1, -5), mLocalShapeToWorld * Vector3(-30, 1, -5)); - Ray ray6(mLocalShapeToWorld * Vector3(4, 4, 1), mLocalShapeToWorld * Vector3(4, -30, 1)); - Ray ray7(mLocalShapeToWorld * Vector3(1, -4, 5), mLocalShapeToWorld * Vector3(1, -4, -30)); - Ray ray8(mLocalShapeToWorld * Vector3(-4, 4, 0), mLocalShapeToWorld * Vector3(30, 4, 0)); - Ray ray9(mLocalShapeToWorld * Vector3(0, -4, -7), mLocalShapeToWorld * Vector3(0, 30, -7)); - Ray ray10(mLocalShapeToWorld * Vector3(-3, 0, -6), mLocalShapeToWorld * Vector3(-3, 0, 30)); - Ray ray11(mLocalShapeToWorld * Vector3(3, 1, 2), mLocalShapeToWorld * Vector3(-30, 0, -6)); - Ray ray12(mLocalShapeToWorld * Vector3(1, 4, -1), mLocalShapeToWorld * Vector3(1, -30, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 5), mLocalShapeToWorld * Vector3(-1, 2, -30)); - Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -2), mLocalShapeToWorld * Vector3(30, 2, -2)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -4, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); + Ray ray1(mLocalShapeToWorld * vec3(0, 0, 0), mLocalShapeToWorld * vec3(5, 7, -1)); + Ray ray2(mLocalShapeToWorld * vec3(5, 11, 7), mLocalShapeToWorld * vec3(17, 29, 28)); + Ray ray3(mLocalShapeToWorld * vec3(1, 2, 3), mLocalShapeToWorld * vec3(-11, 2, 24)); + Ray ray4(mLocalShapeToWorld * vec3(10, 10, 10), mLocalShapeToWorld * vec3(22, 28, 31)); + Ray ray5(mLocalShapeToWorld * vec3(3, 1, -5), mLocalShapeToWorld * vec3(-30, 1, -5)); + Ray ray6(mLocalShapeToWorld * vec3(4, 4, 1), mLocalShapeToWorld * vec3(4, -30, 1)); + Ray ray7(mLocalShapeToWorld * vec3(1, -4, 5), mLocalShapeToWorld * vec3(1, -4, -30)); + Ray ray8(mLocalShapeToWorld * vec3(-4, 4, 0), mLocalShapeToWorld * vec3(30, 4, 0)); + Ray ray9(mLocalShapeToWorld * vec3(0, -4, -7), mLocalShapeToWorld * vec3(0, 30, -7)); + Ray ray10(mLocalShapeToWorld * vec3(-3, 0, -6), mLocalShapeToWorld * vec3(-3, 0, 30)); + Ray ray11(mLocalShapeToWorld * vec3(3, 1, 2), mLocalShapeToWorld * vec3(-30, 0, -6)); + Ray ray12(mLocalShapeToWorld * vec3(1, 4, -1), mLocalShapeToWorld * vec3(1, -30, -1)); + Ray ray13(mLocalShapeToWorld * vec3(-1, 2, 5), mLocalShapeToWorld * vec3(-1, 2, -30)); + Ray ray14(mLocalShapeToWorld * vec3(-3, 2, -2), mLocalShapeToWorld * vec3(30, 2, -2)); + Ray ray15(mLocalShapeToWorld * vec3(0, -4, 1), mLocalShapeToWorld * vec3(0, 30, 1)); + Ray ray16(mLocalShapeToWorld * vec3(-1, 2, -7), mLocalShapeToWorld * vec3(-1, 2, 30)); // ----- Test raycast miss ----- // test(!mConcaveMeshBody->raycast(ray1, raycastInfo3)); @@ -2434,15 +2434,15 @@ class TestRaycast : public Test { void testHeightField() { // ----- Test feedback data ----- // - Vector3 point1A = mLocalShapeToWorld * Vector3(0 , 10, 2); - Vector3 point1B = mLocalShapeToWorld * Vector3(0, -10, 2); + vec3 point1A = mLocalShapeToWorld * vec3(0 , 10, 2); + vec3 point1B = mLocalShapeToWorld * vec3(0, -10, 2); Ray ray(point1A, point1B); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(0, 2, 2); + vec3 hitPoint = mLocalShapeToWorld * vec3(0, 2, 2); - Vector3 point2A = mLocalShapeToWorld * Vector3(1 , 8, -4); - Vector3 point2B = mLocalShapeToWorld * Vector3(1, -8, -4); + vec3 point2A = mLocalShapeToWorld * vec3(1 , 8, -4); + vec3 point2B = mLocalShapeToWorld * vec3(1, -8, -4); Ray rayBottom(point2A, point2B); - Vector3 hitPoint2 = mLocalShapeToWorld * Vector3(1, 2, -4); + vec3 hitPoint2 = mLocalShapeToWorld * vec3(1, 2, -4); mCallback.shapeToTest = mHeightFieldProxyShape; @@ -2453,9 +2453,9 @@ class TestRaycast : public Test { test(mCallback.raycastInfo.body == mHeightFieldBody); test(mCallback.raycastInfo.proxyShape == mHeightFieldProxyShape); test(approxEqual(mCallback.raycastInfo.hitFraction, float(0.4), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z(), hitPoint.z, epsilon)); // Correct category filter mask mCallback.reset(); @@ -2473,9 +2473,9 @@ class TestRaycast : public Test { test(raycastInfo2.body == mHeightFieldBody); test(raycastInfo2.proxyShape == mHeightFieldProxyShape); test(approxEqual(raycastInfo2.hitFraction, float(0.4), epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z(), hitPoint.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo3; @@ -2483,9 +2483,9 @@ class TestRaycast : public Test { test(raycastInfo3.body == mHeightFieldBody); test(raycastInfo3.proxyShape == mHeightFieldProxyShape); test(approxEqual(raycastInfo3.hitFraction, float(0.4), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x(), hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y(), hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z(), hitPoint.z, epsilon)); mCallback.reset(); m_world->raycast(rayBottom, &mCallback); @@ -2493,9 +2493,9 @@ class TestRaycast : public Test { test(mCallback.raycastInfo.body == mHeightFieldBody); test(mCallback.raycastInfo.proxyShape == mHeightFieldProxyShape); test(approxEqual(mCallback.raycastInfo.hitFraction, float(0.375), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint2.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint2.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint2.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x(), hitPoint2.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y(), hitPoint2.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z(), hitPoint2.z, epsilon)); // CollisionBody::raycast() RaycastInfo raycastInfo5; @@ -2503,9 +2503,9 @@ class TestRaycast : public Test { test(raycastInfo5.body == mHeightFieldBody); test(raycastInfo5.proxyShape == mHeightFieldProxyShape); test(approxEqual(raycastInfo5.hitFraction, float(0.375), epsilon)); - test(approxEqual(raycastInfo5.worldPoint.x, hitPoint2.x, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.y, hitPoint2.y, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.z, hitPoint2.z, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.x(), hitPoint2.x, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.y(), hitPoint2.y, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.z(), hitPoint2.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo6; @@ -2513,20 +2513,20 @@ class TestRaycast : public Test { test(raycastInfo6.body == mHeightFieldBody); test(raycastInfo6.proxyShape == mHeightFieldProxyShape); test(approxEqual(raycastInfo6.hitFraction, float(0.375), epsilon)); - test(approxEqual(raycastInfo6.worldPoint.x, hitPoint2.x, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.y, hitPoint2.y, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.z, hitPoint2.z, epsilon)); + test(approxEqual(raycastInfo6.worldPoint.x(), hitPoint2.x, epsilon)); + test(approxEqual(raycastInfo6.worldPoint.y(), hitPoint2.y, epsilon)); + test(approxEqual(raycastInfo6.worldPoint.z(), hitPoint2.z, epsilon)); - Ray ray1(mLocalShapeToWorld * Vector3(0, 5, 0), mLocalShapeToWorld * Vector3(5, 7, 5)); - Ray ray2(mLocalShapeToWorld * Vector3(-4, -4, 7), mLocalShapeToWorld * Vector3(-4, 15, 7)); - Ray ray3(mLocalShapeToWorld * Vector3(23, 7, 2), mLocalShapeToWorld * Vector3(23, 1, 2)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 3, 10), mLocalShapeToWorld * Vector3(22, 3, 31)); - Ray ray5(mLocalShapeToWorld * Vector3(4, 10, -1), mLocalShapeToWorld * Vector3(4, 3, -1)); + Ray ray1(mLocalShapeToWorld * vec3(0, 5, 0), mLocalShapeToWorld * vec3(5, 7, 5)); + Ray ray2(mLocalShapeToWorld * vec3(-4, -4, 7), mLocalShapeToWorld * vec3(-4, 15, 7)); + Ray ray3(mLocalShapeToWorld * vec3(23, 7, 2), mLocalShapeToWorld * vec3(23, 1, 2)); + Ray ray4(mLocalShapeToWorld * vec3(10, 3, 10), mLocalShapeToWorld * vec3(22, 3, 31)); + Ray ray5(mLocalShapeToWorld * vec3(4, 10, -1), mLocalShapeToWorld * vec3(4, 3, -1)); - Ray ray11(mLocalShapeToWorld * Vector3(3, 15, 0.5), mLocalShapeToWorld * Vector3(3, 1, 0.5)); - Ray ray12(mLocalShapeToWorld * Vector3(0, 45, 0), mLocalShapeToWorld * Vector3(0, -5, 0)); - Ray ray13(mLocalShapeToWorld * Vector3(1, 23, 2), mLocalShapeToWorld * Vector3(1, -23, 2)); - Ray ray14(mLocalShapeToWorld * Vector3(3, 8, 3), mLocalShapeToWorld * Vector3(3, 0, 3)); + Ray ray11(mLocalShapeToWorld * vec3(3, 15, 0.5), mLocalShapeToWorld * vec3(3, 1, 0.5)); + Ray ray12(mLocalShapeToWorld * vec3(0, 45, 0), mLocalShapeToWorld * vec3(0, -5, 0)); + Ray ray13(mLocalShapeToWorld * vec3(1, 23, 2), mLocalShapeToWorld * vec3(1, -23, 2)); + Ray ray14(mLocalShapeToWorld * vec3(3, 8, 3), mLocalShapeToWorld * vec3(3, 0, 3)); // ----- Test raycast miss ----- // test(!mHeightFieldBody->raycast(ray1, raycastInfo3)); diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h index 1016b63..bb64ad1 100644 --- a/test/tests/mathematics/TestMathematicsFunctions.h +++ b/test/tests/mathematics/TestMathematicsFunctions.h @@ -104,10 +104,10 @@ class TestMathematicsFunctions : public Test { test(!sameSign(-4, 53)); // Test computeBarycentricCoordinatesInTriangle() - Vector3 a(0, 0, 0); - Vector3 b(5, 0, 0); - Vector3 c(0, 0, 5); - Vector3 testPoint(4, 0, 1); + vec3 a(0, 0, 0); + vec3 b(5, 0, 0); + vec3 c(0, 0, 5); + vec3 testPoint(4, 0, 1); float u,v,w; computeBarycentricCoordinatesInTriangle(a, b, c, a, u, v, w); test(approxEqual(u, 1.0, 0.000001)); diff --git a/test/tests/mathematics/TestMatrix2x2.h b/test/tests/mathematics/TestMatrix2x2.h index a61f766..d523a1e 100644 --- a/test/tests/mathematics/TestMatrix2x2.h +++ b/test/tests/mathematics/TestMatrix2x2.h @@ -28,34 +28,34 @@ // Libraries #include -#include +#include /// Reactphysics3D namespace namespace reactphysics3d { -// Class TestMatrix2x2 +// Class Testetk::Matrix2x2 /** - * Unit test for the Matrix2x2 class + * Unit test for the etk::Matrix2x2 class */ -class TestMatrix2x2 : public Test { +class Testetk::Matrix2x2 : public Test { private : // ---------- Atributes ---------- // /// Identity transform - Matrix2x2 mIdentity; + etk::Matrix2x2 mIdentity; /// First example matrix - Matrix2x2 mMatrix1; + etk::Matrix2x2 mMatrix1; public : // ---------- Methods ---------- // /// Constructor - TestMatrix2x2(const std::string& name) - : Test(name), mIdentity(Matrix2x2::identity()), mMatrix1(2, 24, -4, 5) { + Testetk::Matrix2x2(const std::string& name) + : Test(name), mIdentity(etk::Matrix2x2::identity()), mMatrix1(2, 24, -4, 5) { } @@ -72,9 +72,9 @@ class TestMatrix2x2 : public Test { /// Test the constructors void testConstructors() { - Matrix2x2 test1(5.0); - Matrix2x2 test2(2, 3, 4, 5); - Matrix2x2 test3(mMatrix1); + etk::Matrix2x2 test1(5.0); + etk::Matrix2x2 test2(2, 3, 4, 5); + etk::Matrix2x2 test3(mMatrix1); test(test1[0][0] == 5); test(test1[0][1] == 5); @@ -93,32 +93,32 @@ class TestMatrix2x2 : public Test { void testGetSet() { // Test method to set all the values - Matrix2x2 test2; - test2.setAllValues(2, 24, -4, 5); + etk::Matrix2x2 test2; + test2.setValue(2, 24, -4, 5); test(test2 == mMatrix1); // Test method to set to zero - test2.setToZero(); - test(test2 == Matrix2x2(0, 0, 0, 0)); + test2.setZero(); + test(test2 == etk::Matrix2x2(0, 0, 0, 0)); // Test method that returns a column - Vector2 column1 = mMatrix1.getColumn(0); - Vector2 column2 = mMatrix1.getColumn(1); - test(column1 == Vector2(2, -4)); - test(column2 == Vector2(24, 5)); + vec2 column1 = mMatrix1.getColumn(0); + vec2 column2 = mMatrix1.getColumn(1); + test(column1 == vec2(2, -4)); + test(column2 == vec2(24, 5)); // Test method that returns a row - Vector2 row1 = mMatrix1.getRow(0); - Vector2 row2 = mMatrix1.getRow(1); - test(row1 == Vector2(2, 24)); - test(row2 == Vector2(-4, 5)); + vec2 row1 = mMatrix1.getRow(0); + vec2 row2 = mMatrix1.getRow(1); + test(row1 == vec2(2, 24)); + test(row2 == vec2(-4, 5)); } /// Test the identity methods void testIdentity() { - Matrix2x2 identity = Matrix2x2::identity(); - Matrix2x2 test1; + etk::Matrix2x2 identity = Matrix2x2::identity(); + etk::Matrix2x2 test1; test1.setToIdentity(); test(identity[0][0] == 1); @@ -126,13 +126,13 @@ class TestMatrix2x2 : public Test { test(identity[1][0] == 0); test(identity[1][1] == 1); - test(test1 == Matrix2x2::identity()); + test(test1 == etk::Matrix2x2::identity()); } /// Test the zero method void testZero() { - Matrix2x2 zero = Matrix2x2::zero(); + etk::Matrix2x2 zero = Matrix2x2::zero(); test(zero[0][0] == 0); test(zero[0][1] == 0); @@ -144,90 +144,90 @@ class TestMatrix2x2 : public Test { void testOthersMethods() { // Test transpose - Matrix2x2 transpose = mMatrix1.getTranspose(); - test(transpose == Matrix2x2(2, -4, 24, 5)); + etk::Matrix2x2 transpose = mMatrix1.getTranspose(); + test(transpose == etk::Matrix2x2(2, -4, 24, 5)); // Test trace test(mMatrix1.getTrace() ==7); - test(Matrix2x2::identity().getTrace() == 2); + test(etk::Matrix2x2::identity().getTrace() == 2); // Test determinant - Matrix2x2 matrix(-24, 64, 253, -35); + etk::Matrix2x2 matrix(-24, 64, 253, -35); test(mMatrix1.getDeterminant() == 106); test(matrix.getDeterminant() == -15352); test(mIdentity.getDeterminant() == 1); // Test inverse - Matrix2x2 matrix2(1, 2, 3, 4); - Matrix2x2 inverseMatrix = matrix2.getInverse(); + etk::Matrix2x2 matrix2(1, 2, 3, 4); + etk::Matrix2x2 inverseMatrix = matrix2.getInverse(); test(approxEqual(inverseMatrix[0][0], float(-2), float(10e-6))); test(approxEqual(inverseMatrix[0][1], float(1), float(10e-6))); test(approxEqual(inverseMatrix[1][0], float(1.5), float(10e-6))); test(approxEqual(inverseMatrix[1][1], float(-0.5), float(10e-6))); - Matrix2x2 inverseMatrix1 = mMatrix1.getInverse(); + etk::Matrix2x2 inverseMatrix1 = mMatrix1.getInverse(); test(approxEqual(inverseMatrix1[0][0], float(0.047169811), float(10e-6))); test(approxEqual(inverseMatrix1[0][1], float(-0.226415094), float(10e-6))); test(approxEqual(inverseMatrix1[1][0], float(0.037735849), float(10e-6))); test(approxEqual(inverseMatrix1[1][1], float(0.018867925), float(10e-6))); // Test absolute matrix - Matrix2x2 matrix3(-2, -3, -4, -5); - test(matrix.getAbsoluteMatrix() == Matrix2x2(24, 64, 253, 35)); - Matrix2x2 absoluteMatrix = matrix3.getAbsoluteMatrix(); - test(absoluteMatrix == Matrix2x2(2, 3, 4, 5)); + etk::Matrix2x2 matrix3(-2, -3, -4, -5); + test(matrix.getAbsolute() == etk::Matrix2x2(24, 64, 253, 35)); + etk::Matrix2x2 absoluteMatrix = matrix3.getAbsolute(); + test(absoluteMatrix == etk::Matrix2x2(2, 3, 4, 5)); } /// Test the operators void testOperators() { // Test addition - Matrix2x2 matrix1(2, 3, 4, 5); - Matrix2x2 matrix2(-2, 3, -5, 10); - Matrix2x2 addition1 = matrix1 + matrix2; - Matrix2x2 addition2(matrix1); + etk::Matrix2x2 matrix1(2, 3, 4, 5); + etk::Matrix2x2 matrix2(-2, 3, -5, 10); + etk::Matrix2x2 addition1 = matrix1 + matrix2; + etk::Matrix2x2 addition2(matrix1); addition2 += matrix2; - test(addition1 == Matrix2x2(0, 6, -1, 15)); - test(addition2 == Matrix2x2(0, 6, -1, 15)); + test(addition1 == etk::Matrix2x2(0, 6, -1, 15)); + test(addition2 == etk::Matrix2x2(0, 6, -1, 15)); // Test substraction - Matrix2x2 substraction1 = matrix1 - matrix2; - Matrix2x2 substraction2(matrix1); + etk::Matrix2x2 substraction1 = matrix1 - matrix2; + etk::Matrix2x2 substraction2(matrix1); substraction2 -= matrix2; - test(substraction1 == Matrix2x2(4, 0, 9, -5)); - test(substraction2 == Matrix2x2(4, 0, 9, -5)); + test(substraction1 == etk::Matrix2x2(4, 0, 9, -5)); + test(substraction2 == etk::Matrix2x2(4, 0, 9, -5)); // Test negative operator - Matrix2x2 negative = -matrix1; - test(negative == Matrix2x2(-2, -3, -4, -5)); + etk::Matrix2x2 negative = -matrix1; + test(negative == etk::Matrix2x2(-2, -3, -4, -5)); // Test multiplication with a number - Matrix2x2 multiplication1 = 3 * matrix1; - Matrix2x2 multiplication2 = matrix1 * 3; - Matrix2x2 multiplication3(matrix1); + etk::Matrix2x2 multiplication1 = 3 * matrix1; + etk::Matrix2x2 multiplication2 = matrix1 * 3; + etk::Matrix2x2 multiplication3(matrix1); multiplication3 *= 3; - test(multiplication1 == Matrix2x2(6, 9, 12, 15)); - test(multiplication2 == Matrix2x2(6, 9, 12, 15)); - test(multiplication3 == Matrix2x2(6, 9, 12, 15)); + test(multiplication1 == etk::Matrix2x2(6, 9, 12, 15)); + test(multiplication2 == etk::Matrix2x2(6, 9, 12, 15)); + test(multiplication3 == etk::Matrix2x2(6, 9, 12, 15)); // Test multiplication with a matrix - Matrix2x2 multiplication4 = matrix1 * matrix2; - Matrix2x2 multiplication5 = matrix2 * matrix1; - test(multiplication4 == Matrix2x2(-19, 36, -33, 62)); - test(multiplication5 == Matrix2x2(8, 9, 30, 35)); + etk::Matrix2x2 multiplication4 = matrix1 * matrix2; + etk::Matrix2x2 multiplication5 = matrix2 * matrix1; + test(multiplication4 == etk::Matrix2x2(-19, 36, -33, 62)); + test(multiplication5 == etk::Matrix2x2(8, 9, 30, 35)); // Test multiplication with a vector - Vector2 vector1(3, -32); - Vector2 vector2(-31, -422); - Vector2 test1 = matrix1 * vector1; - Vector2 test2 = matrix2 * vector2; - test(test1 == Vector2(-90, -148)); - test(test2 == Vector2(-1204, -4065)); + vec2 vector1(3, -32); + vec2 vector2(-31, -422); + vec2 test1 = matrix1 * vector1; + vec2 test2 = matrix2 * vector2; + test(test1 == vec2(-90, -148)); + test(test2 == vec2(-1204, -4065)); // Test equality operators - test(Matrix2x2(34, 38, 43, 64) == - Matrix2x2(34, 38, 43, 64)); - test(Matrix2x2(34, 64, 43, 7) != - Matrix2x2(34, 38, 43, 64)); + test(etk::Matrix2x2(34, 38, 43, 64) == + etk::Matrix2x2(34, 38, 43, 64)); + test(etk::Matrix2x2(34, 64, 43, 7) != + etk::Matrix2x2(34, 38, 43, 64)); // Test operator to read a value test(mMatrix1[0][0] == 2); @@ -236,7 +236,7 @@ class TestMatrix2x2 : public Test { test(mMatrix1[1][1] == 5); // Test operator to set a value - Matrix2x2 test3; + etk::Matrix2x2 test3; test3[0][0] = 2; test3[0][1] = 24; test3[1][0] = -4; diff --git a/test/tests/mathematics/TestMatrix3x3.h b/test/tests/mathematics/TestMatrix3x3.h index 5d0a002..2df0623 100644 --- a/test/tests/mathematics/TestMatrix3x3.h +++ b/test/tests/mathematics/TestMatrix3x3.h @@ -28,34 +28,34 @@ // Libraries #include -#include +#include /// Reactphysics3D namespace namespace reactphysics3d { -// Class TestMatrix3x3 +// Class Testetk::Matrix3x3 /** - * Unit test for the Matrix3x3 class + * Unit test for the etk::Matrix3x3 class */ -class TestMatrix3x3 : public Test { +class Testetk::Matrix3x3 : public Test { private : // ---------- Atributes ---------- // /// Identity transform - Matrix3x3 mIdentity; + etk::Matrix3x3 mIdentity; /// First example matrix - Matrix3x3 mMatrix1; + etk::Matrix3x3 mMatrix1; public : // ---------- Methods ---------- // /// Constructor - TestMatrix3x3(const std::string& name) - : Test(name), mIdentity(Matrix3x3::identity()), + Testetk::Matrix3x3(const std::string& name) + : Test(name), mIdentity(etk::Matrix3x3::identity()), mMatrix1(2, 24, 4, 5, -6, 234, -15, 11, 66) { @@ -74,9 +74,9 @@ class TestMatrix3x3 : public Test { /// Test the constructors void testConstructors() { - Matrix3x3 test1(5.0); - Matrix3x3 test2(2, 3, 4, 5, 6, 7, 8, 9, 10); - Matrix3x3 test3(mMatrix1); + etk::Matrix3x3 test1(5.0); + etk::Matrix3x3 test2(2, 3, 4, 5, 6, 7, 8, 9, 10); + etk::Matrix3x3 test3(mMatrix1); test(test1[0][0] == 5); test(test1[0][1] == 5); test(test1[0][2] == 5); @@ -104,36 +104,36 @@ class TestMatrix3x3 : public Test { void testGetSet() { // Test method to set all the values - Matrix3x3 test2; - test2.setAllValues(2, 24, 4, 5, -6, 234, -15, 11, 66); + etk::Matrix3x3 test2; + test2.setValue(2, 24, 4, 5, -6, 234, -15, 11, 66); test(test2 == mMatrix1); // Test method to set to zero - test2.setToZero(); - test(test2 == Matrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0)); + test2.setZero(); + test(test2 == etk::Matrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0)); // Test method that returns a column - Vector3 column1 = mMatrix1.getColumn(0); - Vector3 column2 = mMatrix1.getColumn(1); - Vector3 column3 = mMatrix1.getColumn(2); - test(column1 == Vector3(2, 5, -15)); - test(column2 == Vector3(24, -6, 11)); - test(column3 == Vector3(4, 234, 66)); + vec3 column1 = mMatrix1.getColumn(0); + vec3 column2 = mMatrix1.getColumn(1); + vec3 column3 = mMatrix1.getColumn(2); + test(column1 == vec3(2, 5, -15)); + test(column2 == vec3(24, -6, 11)); + test(column3 == vec3(4, 234, 66)); // Test method that returns a row - Vector3 row1 = mMatrix1.getRow(0); - Vector3 row2 = mMatrix1.getRow(1); - Vector3 row3 = mMatrix1.getRow(2); - test(row1 == Vector3(2, 24, 4)); - test(row2 == Vector3(5, -6, 234)); - test(row3 == Vector3(-15, 11, 66)); + vec3 row1 = mMatrix1.getRow(0); + vec3 row2 = mMatrix1.getRow(1); + vec3 row3 = mMatrix1.getRow(2); + test(row1 == vec3(2, 24, 4)); + test(row2 == vec3(5, -6, 234)); + test(row3 == vec3(-15, 11, 66)); } /// Test the identity methods void testIdentity() { - Matrix3x3 identity = Matrix3x3::identity(); - Matrix3x3 test1; + etk::Matrix3x3 identity = Matrix3x3::identity(); + etk::Matrix3x3 test1; test1.setToIdentity(); test(identity[0][0] == 1); @@ -146,13 +146,13 @@ class TestMatrix3x3 : public Test { test(identity[2][1] == 0); test(identity[2][2] == 1); - test(test1 == Matrix3x3::identity()); + test(test1 == etk::Matrix3x3::identity()); } /// Test the zero method void testZero() { - Matrix3x3 zero = Matrix3x3::zero(); + etk::Matrix3x3 zero = Matrix3x3::zero(); test(zero[0][0] == 0); test(zero[0][1] == 0); @@ -169,21 +169,21 @@ class TestMatrix3x3 : public Test { void testOthersMethods() { // Test transpose - Matrix3x3 transpose = mMatrix1.getTranspose(); - test(transpose == Matrix3x3(2, 5, -15, 24, -6, 11, 4, 234, 66)); + etk::Matrix3x3 transpose = mMatrix1.getTranspose(); + test(transpose == etk::Matrix3x3(2, 5, -15, 24, -6, 11, 4, 234, 66)); // Test trace test(mMatrix1.getTrace() == 62); - test(Matrix3x3::identity().getTrace() == 3); + test(etk::Matrix3x3::identity().getTrace() == 3); // Test determinant - Matrix3x3 matrix(-24, 64, 253, -35, 52, 72, 21, -35, -363); + etk::Matrix3x3 matrix(-24, 64, 253, -35, 52, 72, 21, -35, -363); test(mMatrix1.getDeterminant() == -98240); test(matrix.getDeterminant() == -290159); test(mIdentity.getDeterminant() == 1); // Test inverse - Matrix3x3 inverseMatrix = matrix.getInverse(); + etk::Matrix3x3 inverseMatrix = matrix.getInverse(); test(approxEqual(inverseMatrix[0][0], float(0.056369), float(10e-6))); test(approxEqual(inverseMatrix[0][1], float(-0.049549), float(10e-6))); test(approxEqual(inverseMatrix[0][2], float(0.029460), float(10e-6))); @@ -193,7 +193,7 @@ class TestMatrix3x3 : public Test { test(approxEqual(inverseMatrix[2][0], float(-0.000458), float(10e-6))); test(approxEqual(inverseMatrix[2][1], float(-0.001737), float(10e-6))); test(approxEqual(inverseMatrix[2][2], float(-0.003419), float(10e-6))); - Matrix3x3 inverseMatrix1 = mMatrix1.getInverse(); + etk::Matrix3x3 inverseMatrix1 = mMatrix1.getInverse(); test(approxEqual(inverseMatrix1[0][0], float(0.030232), float(10e-6))); test(approxEqual(inverseMatrix1[0][1], float(0.015676), float(10e-6))); test(approxEqual(inverseMatrix1[0][2], float(-0.057410), float(10e-6))); @@ -205,18 +205,18 @@ class TestMatrix3x3 : public Test { test(approxEqual(inverseMatrix1[2][2], float(0.001344), float(10e-6))); // Test absolute matrix - Matrix3x3 matrix2(-2, -3, -4, -5, -6, -7, -8, -9, -10); - test(matrix.getAbsoluteMatrix() == Matrix3x3(24, 64, 253, 35, 52, 72, 21, 35, 363)); - Matrix3x3 absoluteMatrix = matrix2.getAbsoluteMatrix(); - test(absoluteMatrix == Matrix3x3(2, 3, 4, 5, 6, 7, 8, 9, 10)); + etk::Matrix3x3 matrix2(-2, -3, -4, -5, -6, -7, -8, -9, -10); + test(matrix.getAbsolute() == etk::Matrix3x3(24, 64, 253, 35, 52, 72, 21, 35, 363)); + etk::Matrix3x3 absoluteMatrix = matrix2.getAbsolute(); + test(absoluteMatrix == etk::Matrix3x3(2, 3, 4, 5, 6, 7, 8, 9, 10)); // Test method that computes skew-symmetric matrix for cross product - Vector3 vector1(3, -5, 6); - Vector3 vector2(73, 42, 26); - Matrix3x3 skewMatrix = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(vector1); - test(skewMatrix == Matrix3x3(0, -6, -5, 6, 0, -3, 5, 3, 0)); - Vector3 crossProduct1 = vector1.cross(vector2); - Vector3 crossProduct2 = skewMatrix * vector2; + vec3 vector1(3, -5, 6); + vec3 vector2(73, 42, 26); + etk::Matrix3x3 skewMatrix = etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(vector1); + test(skewMatrix == etk::Matrix3x3(0, -6, -5, 6, 0, -3, 5, 3, 0)); + vec3 crossProduct1 = vector1.cross(vector2); + vec3 crossProduct2 = skewMatrix * vector2; test(crossProduct1 == crossProduct2); } @@ -224,53 +224,53 @@ class TestMatrix3x3 : public Test { void testOperators() { // Test addition - Matrix3x3 matrix1(2, 3, 4, 5, 6, 7, 8, 9, 10); - Matrix3x3 matrix2(-2, 3, -5, 10, 4, 7, 2, 5, 8); - Matrix3x3 addition1 = matrix1 + matrix2; - Matrix3x3 addition2(matrix1); + etk::Matrix3x3 matrix1(2, 3, 4, 5, 6, 7, 8, 9, 10); + etk::Matrix3x3 matrix2(-2, 3, -5, 10, 4, 7, 2, 5, 8); + etk::Matrix3x3 addition1 = matrix1 + matrix2; + etk::Matrix3x3 addition2(matrix1); addition2 += matrix2; - test(addition1 == Matrix3x3(0, 6, -1, 15, 10, 14, 10, 14, 18)); - test(addition2 == Matrix3x3(0, 6, -1, 15, 10, 14, 10, 14, 18)); + test(addition1 == etk::Matrix3x3(0, 6, -1, 15, 10, 14, 10, 14, 18)); + test(addition2 == etk::Matrix3x3(0, 6, -1, 15, 10, 14, 10, 14, 18)); // Test substraction - Matrix3x3 substraction1 = matrix1 - matrix2; - Matrix3x3 substraction2(matrix1); + etk::Matrix3x3 substraction1 = matrix1 - matrix2; + etk::Matrix3x3 substraction2(matrix1); substraction2 -= matrix2; - test(substraction1 == Matrix3x3(4, 0, 9, -5, 2, 0, 6, 4, 2)); - test(substraction2 == Matrix3x3(4, 0, 9, -5, 2, 0, 6, 4, 2)); + test(substraction1 == etk::Matrix3x3(4, 0, 9, -5, 2, 0, 6, 4, 2)); + test(substraction2 == etk::Matrix3x3(4, 0, 9, -5, 2, 0, 6, 4, 2)); // Test negative operator - Matrix3x3 negative = -matrix1; - test(negative == Matrix3x3(-2, -3, -4, -5, -6, -7, -8, -9, -10)); + etk::Matrix3x3 negative = -matrix1; + test(negative == etk::Matrix3x3(-2, -3, -4, -5, -6, -7, -8, -9, -10)); // Test multiplication with a number - Matrix3x3 multiplication1 = 3 * matrix1; - Matrix3x3 multiplication2 = matrix1 * 3; - Matrix3x3 multiplication3(matrix1); + etk::Matrix3x3 multiplication1 = 3 * matrix1; + etk::Matrix3x3 multiplication2 = matrix1 * 3; + etk::Matrix3x3 multiplication3(matrix1); multiplication3 *= 3; - test(multiplication1 == Matrix3x3(6, 9, 12, 15, 18, 21, 24, 27, 30)); - test(multiplication2 == Matrix3x3(6, 9, 12, 15, 18, 21, 24, 27, 30)); - test(multiplication3 == Matrix3x3(6, 9, 12, 15, 18, 21, 24, 27, 30)); + test(multiplication1 == etk::Matrix3x3(6, 9, 12, 15, 18, 21, 24, 27, 30)); + test(multiplication2 == etk::Matrix3x3(6, 9, 12, 15, 18, 21, 24, 27, 30)); + test(multiplication3 == etk::Matrix3x3(6, 9, 12, 15, 18, 21, 24, 27, 30)); // Test multiplication with a matrix - Matrix3x3 multiplication4 = matrix1 * matrix2; - Matrix3x3 multiplication5 = matrix2 * matrix1; - test(multiplication4 == Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103)); - test(multiplication5 == Matrix3x3(-29, -33, -37, 96, 117, 138, 93, 108, 123)); + etk::Matrix3x3 multiplication4 = matrix1 * matrix2; + etk::Matrix3x3 multiplication5 = matrix2 * matrix1; + test(multiplication4 == etk::Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103)); + test(multiplication5 == etk::Matrix3x3(-29, -33, -37, 96, 117, 138, 93, 108, 123)); // Test multiplication with a vector - Vector3 vector1(3, -32, 59); - Vector3 vector2(-31, -422, 34); - Vector3 test1 = matrix1 * vector1; - Vector3 test2 = matrix2 * vector2; - test(test1 == Vector3(146, 236, 326)); - test(test2 == Vector3(-1374, -1760, -1900)); + vec3 vector1(3, -32, 59); + vec3 vector2(-31, -422, 34); + vec3 test1 = matrix1 * vector1; + vec3 test2 = matrix2 * vector2; + test(test1 == vec3(146, 236, 326)); + test(test2 == vec3(-1374, -1760, -1900)); // Test equality operators - test(Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103) == - Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103)); - test(Matrix3x3(34, 64, 43, 7, -1, 73, 94, 110, 103) != - Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103)); + test(etk::Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103) == + etk::Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103)); + test(etk::Matrix3x3(34, 64, 43, 7, -1, 73, 94, 110, 103) != + etk::Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103)); // Test operator to read a value test(mMatrix1[0][0] == 2); @@ -284,7 +284,7 @@ class TestMatrix3x3 : public Test { test(mMatrix1[2][2] == 66); // Test operator to set a value - Matrix3x3 test3; + etk::Matrix3x3 test3; test3[0][0] = 2; test3[0][1] = 24; test3[0][2] = 4; diff --git a/test/tests/mathematics/TestQuaternion.h b/test/tests/mathematics/TestQuaternion.h index 033cf0a..9c01bcd 100644 --- a/test/tests/mathematics/TestQuaternion.h +++ b/test/tests/mathematics/TestQuaternion.h @@ -28,40 +28,40 @@ // Libraries #include -#include +#include /// Reactphysics3D namespace namespace reactphysics3d { -// Class TestQuaternion +// Class Testetk::Quaternion /** - * Unit test for the Quaternion class + * Unit test for the etk::Quaternion class */ -class TestQuaternion : public Test { +class Testetk::Quaternion : public Test { private : // ---------- Atributes ---------- // - /// Identity Quaternion - Quaternion mIdentity; + /// Identity etk::Quaternion + etk::Quaternion mIdentity; /// First test quaternion - Quaternion mQuaternion1; + etk::Quaternion mQuaternion1; public : // ---------- Methods ---------- // /// Constructor - TestQuaternion(const std::string& name) : Test(name), mIdentity(Quaternion::identity()) { + Testetk::Quaternion(const std::string& name) : Test(name), mIdentity(Quaternion::identity()) { float sinA = sin(float(PI/8.0)); float cosA = cos(float(PI/8.0)); - Vector3 vector(2, 3, 4); + vec3 vector(2, 3, 4); vector.normalize(); - mQuaternion1 = Quaternion(vector.x * sinA, vector.y * sinA, vector.z * sinA, cosA); - mQuaternion1.normalize(); + metk::Quaternion1 = etk::Quaternion(vector.x() * sinA, vector.y() * sinA, vector.z() * sinA, cosA); + metk::Quaternion1.normalize(); } /// Run the tests @@ -75,47 +75,47 @@ class TestQuaternion : public Test { /// Test the constructors void testConstructors() { - Quaternion quaternion1(mQuaternion1); - test(mQuaternion1 == quaternion1); + etk::Quaternion quaternion1(mQuaternion1); + test(metk::Quaternion1 == quaternion1); - Quaternion quaternion2(4, 5, 6, 7); - test(quaternion2 == Quaternion(4, 5, 6, 7)); + etk::Quaternion quaternion2(4, 5, 6, 7); + test(quaternion2 == etk::Quaternion(4, 5, 6, 7)); - Quaternion quaternion3(8, Vector3(3, 5, 2)); - test(quaternion3 == Quaternion(3, 5, 2, 8)); + etk::Quaternion quaternion3(8, vec3(3, 5, 2)); + test(quaternion3 == etk::Quaternion(3, 5, 2, 8)); - Quaternion quaternion4(mQuaternion1.getMatrix()); - test(approxEqual(quaternion4.x, mQuaternion1.x)); - test(approxEqual(quaternion4.y, mQuaternion1.y)); - test(approxEqual(quaternion4.z, mQuaternion1.z)); - test(approxEqual(quaternion4.w, mQuaternion1.w)); + etk::Quaternion quaternion4(mQuaternion1.getMatrix()); + test(approxEqual(quaternion4.x(), metk::Quaternion1.x)); + test(approxEqual(quaternion4.y(), metk::Quaternion1.y)); + test(approxEqual(quaternion4.z(), metk::Quaternion1.z)); + test(approxEqual(quaternion4.w, metk::Quaternion1.w)); // Test conversion from Euler angles to quaternion - const float PI_OVER_2 = PI * float(0.5); - const float PI_OVER_4 = PI_OVER_2 * float(0.5); - Quaternion quaternion5(PI_OVER_2, 0, 0); - Quaternion quaternionTest5(std::sin(PI_OVER_4), 0, 0, std::cos(PI_OVER_4)); + const float PI_OVER_2 = PI * 0.5f; + const float PI_OVER_4 = PI_OVER_2 * 0.5f; + etk::Quaternion quaternion5(PI_OVER_2, 0, 0); + etk::Quaternion quaternionTest5(std::sin(PI_OVER_4), 0, 0, std::cos(PI_OVER_4)); quaternionTest5.normalize(); - test(approxEqual(quaternion5.x, quaternionTest5.x)); - test(approxEqual(quaternion5.y, quaternionTest5.y)); - test(approxEqual(quaternion5.z, quaternionTest5.z)); + test(approxEqual(quaternion5.x(), quaternionTest5.x)); + test(approxEqual(quaternion5.y(), quaternionTest5.y)); + test(approxEqual(quaternion5.z(), quaternionTest5.z)); test(approxEqual(quaternion5.w, quaternionTest5.w)); - Quaternion quaternion6(0, PI_OVER_2, 0); - Quaternion quaternionTest6(0, std::sin(PI_OVER_4), 0, std::cos(PI_OVER_4)); + etk::Quaternion quaternion6(0, PI_OVER_2, 0); + etk::Quaternion quaternionTest6(0, std::sin(PI_OVER_4), 0, std::cos(PI_OVER_4)); quaternionTest6.normalize(); - test(approxEqual(quaternion6.x, quaternionTest6.x)); - test(approxEqual(quaternion6.y, quaternionTest6.y)); - test(approxEqual(quaternion6.z, quaternionTest6.z)); + test(approxEqual(quaternion6.x(), quaternionTest6.x)); + test(approxEqual(quaternion6.y(), quaternionTest6.y)); + test(approxEqual(quaternion6.z(), quaternionTest6.z)); test(approxEqual(quaternion6.w, quaternionTest6.w)); - Quaternion quaternion7(Vector3(0, 0, PI_OVER_2)); - Quaternion quaternionTest7(0, 0, std::sin(PI_OVER_4), std::cos(PI_OVER_4)); + etk::Quaternion quaternion7(vec3(0, 0, PI_OVER_2)); + etk::Quaternion quaternionTest7(0, 0, std::sin(PI_OVER_4), std::cos(PI_OVER_4)); quaternionTest7.normalize(); - test(approxEqual(quaternion7.x, quaternionTest7.x)); - test(approxEqual(quaternion7.y, quaternionTest7.y)); - test(approxEqual(quaternion7.z, quaternionTest7.z)); + test(approxEqual(quaternion7.x(), quaternionTest7.x)); + test(approxEqual(quaternion7.y(), quaternionTest7.y)); + test(approxEqual(quaternion7.z(), quaternionTest7.z)); test(approxEqual(quaternion7.w, quaternionTest7.w)); } @@ -123,14 +123,14 @@ class TestQuaternion : public Test { void testUnitLengthNormalize() { // Test method that returns the length - Quaternion quaternion(2, 3, -4, 5); + etk::Quaternion quaternion(2, 3, -4, 5); test(approxEqual(quaternion.length(), sqrt(float(54.0)))); // Test method that returns a unit quaternion - test(approxEqual(quaternion.getUnit().length(), 1.0)); + test(approxEqual(quaternion.safeNormalized().length(), 1.0)); // Test the normalization method - Quaternion quaternion2(4, 5, 6, 7); + etk::Quaternion quaternion2(4, 5, 6, 7); quaternion2.normalize(); test(approxEqual(quaternion2.length(), 1.0)); } @@ -139,81 +139,81 @@ class TestQuaternion : public Test { void testOthersMethods() { // Test the method to set the values - Quaternion quaternion; - quaternion.setAllValues(1, 2, 3, 4); - test(quaternion == Quaternion(1, 2, 3, 4)); + etk::Quaternion quaternion; + quaternion.setValue(1, 2, 3, 4); + test(quaternion == etk::Quaternion(1, 2, 3, 4)); // Test the method to set the quaternion to zero - quaternion.setToZero(); - test(quaternion == Quaternion(0, 0, 0, 0)); + quaternion.setZero(); + test(quaternion == etk::Quaternion(0, 0, 0, 0)); // Tes the methods to get or set to identity - Quaternion identity1(1, 2, 3, 4); + etk::Quaternion identity1(1, 2, 3, 4); identity1.setToIdentity(); - test(identity1 == Quaternion(0, 0, 0, 1)); - test(Quaternion::identity() == Quaternion(0, 0, 0, 1)); + test(identity1 == etk::Quaternion(0, 0, 0, 1)); + test(etk::Quaternion::identity() == etk::Quaternion(0, 0, 0, 1)); // Test the method to get the vector (x, y, z) - Vector3 v = mQuaternion1.getVectorV(); - test(v.x == mQuaternion1.x); - test(v.y == mQuaternion1.y); - test(v.z == mQuaternion1.z); + vec3 v = metk::Quaternion1.getVectorV(); + test(v.x() == metk::Quaternion1.x); + test(v.y() == metk::Quaternion1.y); + test(v.z() == metk::Quaternion1.z); // Test the conjugate method - Quaternion conjugate = mQuaternion1.getConjugate(); - test(conjugate.x == -mQuaternion1.x); - test(conjugate.y == -mQuaternion1.y); - test(conjugate.z == -mQuaternion1.z); - test(conjugate.w == mQuaternion1.w); + etk::Quaternion conjugate = mQuaternion1.getConjugate(); + test(conjugate.x() == -metk::Quaternion1.x); + test(conjugate.y() == -metk::Quaternion1.y); + test(conjugate.z() == -metk::Quaternion1.z); + test(conjugate.w == metk::Quaternion1.w); // Test the inverse methods - Quaternion inverse1 = mQuaternion1.getInverse(); - Quaternion inverse2(mQuaternion1); + etk::Quaternion inverse1 = mQuaternion1.getInverse(); + etk::Quaternion inverse2(mQuaternion1); inverse2.inverse(); test(inverse2 == inverse1); - Quaternion product = mQuaternion1 * inverse1; - test(approxEqual(product.x, mIdentity.x, float(10e-6))); - test(approxEqual(product.y, mIdentity.y, float(10e-6))); - test(approxEqual(product.z, mIdentity.z, float(10e-6))); + etk::Quaternion product = mQuaternion1 * inverse1; + test(approxEqual(product.x(), mIdentity.x, float(10e-6))); + test(approxEqual(product.y(), mIdentity.y, float(10e-6))); + test(approxEqual(product.z(), mIdentity.z, float(10e-6))); test(approxEqual(product.w, mIdentity.w, float(10e-6))); // Test the dot product - Quaternion quaternion1(2, 3, 4, 5); - Quaternion quaternion2(6, 7, 8, 9); + etk::Quaternion quaternion1(2, 3, 4, 5); + etk::Quaternion quaternion2(6, 7, 8, 9); float dotProduct = quaternion1.dot(quaternion2); test(dotProduct == 110); // Test the method that returns the rotation angle and axis - Vector3 axis; + vec3 axis; float angle; - Vector3 originalAxis = Vector3(2, 3, 4).getUnit(); - mQuaternion1.getRotationAngleAxis(angle, axis); - test(approxEqual(axis.x, originalAxis.x)); + vec3 originalAxis = vec3(2, 3, 4).safeNormalized(); + metk::Quaternion1.getRotationAngleAxis(angle, axis); + test(approxEqual(axis.x(), originalAxis.x)); test(approxEqual(angle, float(PI/4.0), float(10e-6))); // Test the method that returns the corresponding matrix - Matrix3x3 matrix = mQuaternion1.getMatrix(); - Vector3 vector(56, -2, 82); - Vector3 vector1 = matrix * vector; - Vector3 vector2 = mQuaternion1 * vector; - test(approxEqual(vector1.x, vector2.x, float(10e-6))); - test(approxEqual(vector1.y, vector2.y, float(10e-6))); - test(approxEqual(vector1.z, vector2.z, float(10e-6))); + etk::Matrix3x3 matrix = metk::Quaternion1.getMatrix(); + vec3 vector(56, -2, 82); + vec3 vector1 = matrix * vector; + vec3 vector2 = metk::Quaternion1 * vector; + test(approxEqual(vector1.x(), vector2.x, float(10e-6))); + test(approxEqual(vector1.y(), vector2.y, float(10e-6))); + test(approxEqual(vector1.z(), vector2.z, float(10e-6))); // Test slerp method - Quaternion quatStart = quaternion1.getUnit(); - Quaternion quatEnd = quaternion2.getUnit(); - Quaternion test1 = Quaternion::slerp(quatStart, quatEnd, 0.0); - Quaternion test2 = Quaternion::slerp(quatStart, quatEnd, 1.0); + etk::Quaternion quatStart = quaternion1.safeNormalized(); + etk::Quaternion quatEnd = quaternion2.safeNormalized(); + etk::Quaternion test1 = etk::Quaternion::slerp(quatStart, quatEnd, 0.0); + etk::Quaternion test2 = etk::Quaternion::slerp(quatStart, quatEnd, 1.0); test(test1 == quatStart); test(test2 == quatEnd); float sinA = sin(float(PI/4.0)); float cosA = cos(float(PI/4.0)); - Quaternion quat(sinA, 0, 0, cosA); - Quaternion test3 = Quaternion::slerp(mIdentity, quat, float(0.5)); - test(approxEqual(test3.x, sin(float(PI/8.0)))); - test(approxEqual(test3.y, 0.0)); - test(approxEqual(test3.z, 0.0)); + etk::Quaternion quat(sinA, 0, 0, cosA); + etk::Quaternion test3 = etk::Quaternion::slerp(mIdentity, quat, 0.5f); + test(approxEqual(test3.x(), sin(float(PI/8.0)))); + test(approxEqual(test3.y(), 0.0)); + test(approxEqual(test3.z(), 0.0)); test(approxEqual(test3.w, cos(float(PI/8.0)), float(10e-6))); } @@ -221,48 +221,48 @@ class TestQuaternion : public Test { void testOperators() { // Test addition - Quaternion quat1(4, 5, 2, 10); - Quaternion quat2(-2, 7, 8, 3); - Quaternion test1 = quat1 + quat2; - Quaternion test11(-6, 52, 2, 8); + etk::Quaternion quat1(4, 5, 2, 10); + etk::Quaternion quat2(-2, 7, 8, 3); + etk::Quaternion test1 = quat1 + quat2; + etk::Quaternion test11(-6, 52, 2, 8); test11 += quat1; - test(test1 == Quaternion(2, 12, 10, 13)); - test(test11 == Quaternion(-2, 57, 4, 18)); + test(test1 == etk::Quaternion(2, 12, 10, 13)); + test(test11 == etk::Quaternion(-2, 57, 4, 18)); // Test substraction - Quaternion test2 = quat1 - quat2; - Quaternion test22(-73, 62, 25, 9); + etk::Quaternion test2 = quat1 - quat2; + etk::Quaternion test22(-73, 62, 25, 9); test22 -= quat1; - test(test2 == Quaternion(6, -2, -6, 7)); - test(test22 == Quaternion(-77, 57, 23, -1)); + test(test2 == etk::Quaternion(6, -2, -6, 7)); + test(test22 == etk::Quaternion(-77, 57, 23, -1)); // Test multiplication with a number - Quaternion test3 = quat1 * 3.0; - test(test3 == Quaternion(12, 15, 6, 30)); + etk::Quaternion test3 = quat1 * 3.0; + test(test3 == etk::Quaternion(12, 15, 6, 30)); // Test multiplication between two quaternions - Quaternion test4 = quat1 * quat2; - Quaternion test5 = mQuaternion1 * mIdentity; - test(test4 == Quaternion(18, 49, 124, -13)); - test(test5 == mQuaternion1); + etk::Quaternion test4 = quat1 * quat2; + etk::Quaternion test5 = mQuaternion1 * mIdentity; + test(test4 == etk::Quaternion(18, 49, 124, -13)); + test(test5 == metk::Quaternion1); // Test multiplication between a quaternion and a point - Vector3 point(5, -24, 563); - Vector3 vector1 = mIdentity * point; - Vector3 vector2 = mQuaternion1 * point; - Vector3 testVector2 = mQuaternion1.getMatrix() * point; + vec3 point(5, -24, 563); + vec3 vector1 = mIdentity * point; + vec3 vector2 = metk::Quaternion1 * point; + vec3 testvec2 = metk::Quaternion1.getMatrix() * point; test(vector1 == point); - test(approxEqual(vector2.x, testVector2.x, float(10e-5))); - test(approxEqual(vector2.y, testVector2.y, float(10e-5))); - test(approxEqual(vector2.z, testVector2.z, float(10e-5))); + test(approxEqual(vector2.x(), testvec2.x, float(10e-5))); + test(approxEqual(vector2.y(), testvec2.y, float(10e-5))); + test(approxEqual(vector2.z(), testvec2.z, float(10e-5))); // Test assignment operator - Quaternion quaternion; - quaternion = mQuaternion1; - test(quaternion == mQuaternion1); + etk::Quaternion quaternion; + quaternion = metk::Quaternion1; + test(quaternion == metk::Quaternion1); // Test equality operator - test(mQuaternion1 == mQuaternion1); + test(metk::Quaternion1 == mQuaternion1); } }; diff --git a/test/tests/mathematics/TestTransform.h b/test/tests/mathematics/TestTransform.h index ee977a2..75bc860 100644 --- a/test/tests/mathematics/TestTransform.h +++ b/test/tests/mathematics/TestTransform.h @@ -28,46 +28,46 @@ // Libraries #include -#include +#include /// Reactphysics3D namespace namespace reactphysics3d { -// Class TestTransform +// Class Testetk::Transform3D /** - * Unit test for the Transform class + * Unit test for the etk::Transform3D class */ -class TestTransform : public Test { +class Testetk::Transform3D : public Test { private : // ---------- Atributes ---------- // /// Identity transform - Transform mIdentityTransform; + etk::Transform3D mIdentityTransform; /// First example transform - Transform m_transform1; + etk::Transform3D m_transform1; /// Second example transform - Transform m_transform2; + etk::Transform3D m_transform2; public : // ---------- Methods ---------- // /// Constructor - TestTransform(const std::string& name) : Test(name) { + Testetk::Transform3D(const std::string& name) : Test(name) { - mIdentityTransform.setToIdentity(); + mIdentityetk::Transform3D.setToIdentity(); float sinA = sin(PI/8.0f); float cosA = cos(PI/8.0f); - m_transform1 = Transform(Vector3(4, 5, 6), Quaternion(sinA, sinA, sinA, cosA)); + m_transform1 = etk::Transform3D(vec3(4, 5, 6), etk::Quaternion(sinA, sinA, sinA, cosA)); float sinB = sin(PI/3.0f); float cosB = cos(PI/3.0f); - m_transform2 = Transform(Vector3(8, 45, -6), Quaternion(sinB, sinB, sinB, cosB)); + m_transform2 = etk::Transform3D(vec3(8, 45, -6), etk::Quaternion(sinB, sinB, sinB, cosB)); } /// Run the tests @@ -76,60 +76,60 @@ class TestTransform : public Test { testGetSet(); testInverse(); testGetSetOpenGLMatrix(); - testInterpolateTransform(); + testInterpolateetk::Transform3D(); testIdentity(); testOperators(); } /// Test the constructors void testConstructors() { - Transform transform1(Vector3(1, 2, 3), Quaternion(6, 7, 8, 9)); - Transform transform2(Vector3(4, 5, 6), Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1)); - Transform transform3(transform1); - test(transform1.getPosition() == Vector3(1, 2, 3)); - test(transform1.getOrientation() == Quaternion(6, 7, 8, 9)); - test(transform2.getPosition() == Vector3(4, 5, 6)); - test(transform2.getOrientation() == Quaternion::identity()); + etk::Transform3D transform1(vec3(1, 2, 3), etk::Quaternion(6, 7, 8, 9)); + etk::Transform3D transform2(vec3(4, 5, 6), etk::Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1)); + etk::Transform3D transform3(transform1); + test(transform1.getPosition() == vec3(1, 2, 3)); + test(transform1.getOrientation() == etk::Quaternion(6, 7, 8, 9)); + test(transform2.getPosition() == vec3(4, 5, 6)); + test(transform2.getOrientation() == etk::Quaternion::identity()); test(transform3 == transform1); } /// Test getter and setter void testGetSet() { - test(mIdentityTransform.getPosition() == Vector3(0, 0, 0)); - test(mIdentityTransform.getOrientation() == Quaternion::identity()); - Transform transform; - transform.setPosition(Vector3(5, 7, 8)); - transform.setOrientation(Quaternion(1, 2, 3, 1)); - test(transform.getPosition() == Vector3(5, 7, 8)); - test(transform.getOrientation() == Quaternion(1, 2, 3, 1)); + test(mIdentityetk::Transform3D.getPosition() == vec3(0, 0, 0)); + test(mIdentityetk::Transform3D.getOrientation() == etk::Quaternion::identity()); + etk::Transform3D transform; + transform.setPosition(vec3(5, 7, 8)); + transform.setOrientation(etk::Quaternion(1, 2, 3, 1)); + test(transform.getPosition() == vec3(5, 7, 8)); + test(transform.getOrientation() == etk::Quaternion(1, 2, 3, 1)); transform.setToIdentity(); - test(transform.getPosition() == Vector3(0, 0, 0)); - test(transform.getOrientation() == Quaternion::identity()); + test(transform.getPosition() == vec3(0, 0, 0)); + test(transform.getOrientation() == etk::Quaternion::identity()); } /// Test the inverse void testInverse() { - Transform inverseTransform = m_transform1.getInverse(); - Vector3 vector(2, 3, 4); - Vector3 tempVector = m_transform1 * vector; - Vector3 tempVector2 = inverseTransform * tempVector; - test(approxEqual(tempVector2.x, vector.x, float(10e-6))); - test(approxEqual(tempVector2.y, vector.y, float(10e-6))); - test(approxEqual(tempVector2.z, vector.z, float(10e-6))); + etk::Transform3D inverseTransform = m_transform1.getInverse(); + vec3 vector(2, 3, 4); + vec3 tempVector = m_transform1 * vector; + vec3 tempvec2 = inverseetk::Transform3D * tempVector; + test(approxEqual(tempvec2.x(), vector.x, float(10e-6))); + test(approxEqual(tempvec2.y(), vector.y, float(10e-6))); + test(approxEqual(tempvec2.z(), vector.z, float(10e-6))); } /// Test methods to set and get transform matrix from and to OpenGL void testGetSetOpenGLMatrix() { - Transform transform; - Vector3 position = m_transform1.getPosition(); - Matrix3x3 orientation = m_transform1.getOrientation().getMatrix(); + etk::Transform3D transform; + vec3 position = m_transform1.getPosition(); + etk::Matrix3x3 orientation = m_transform1.getOrientation().getMatrix(); float openglMatrix[16] = {orientation[0][0], orientation[1][0], orientation[2][0], 0, orientation[0][1], orientation[1][1], orientation[2][1], 0, orientation[0][2], orientation[1][2], orientation[2][2], 0, - position.x, position.y, position.z, 1}; + position.x(), position.y(), position.z(), 1}; transform.setFromOpenGL(openglMatrix); float openglMatrix2[16]; transform.getOpenGLMatrix(openglMatrix2); @@ -145,16 +145,16 @@ class TestTransform : public Test { test(approxEqual(openglMatrix2[9], orientation[1][2])); test(approxEqual(openglMatrix2[10], orientation[2][2])); test(approxEqual(openglMatrix2[11], 0)); - test(approxEqual(openglMatrix2[12], position.x)); - test(approxEqual(openglMatrix2[13], position.y)); - test(approxEqual(openglMatrix2[14], position.z)); + test(approxEqual(openglMatrix2[12], position.x())); + test(approxEqual(openglMatrix2[13], position.y())); + test(approxEqual(openglMatrix2[14], position.z())); test(approxEqual(openglMatrix2[15], 1)); } /// Test the method to int32_terpolate transforms - void testInterpolateTransform() { - Transform transformStart = Transform::int32_terpolateTransforms(m_transform1, m_transform2,0); - Transform transformEnd = Transform::int32_terpolateTransforms(m_transform1, m_transform2,1); + void testInterpolateetk::Transform3D() { + etk::Transform3D transformStart = Transform::int32_terpolateTransforms(m_transform1, m_transform2,0); + etk::Transform3D transformEnd = Transform::int32_terpolateTransforms(m_transform1, m_transform2,1); test(transformStart == m_transform1); test(transformEnd == m_transform2); @@ -162,30 +162,30 @@ class TestTransform : public Test { float cosA = cos(PI/3.0f); float sinB = sin(PI/6.0f); float cosB = cos(PI/6.0f); - Transform transform1(Vector3(4, 5, 6), Quaternion::identity()); - Transform transform2(Vector3(8, 11, 16), Quaternion(sinA, sinA, sinA, cosA)); - Transform transform = Transform::int32_terpolateTransforms(transform1, transform2, 0.5); - Vector3 position = transform.getPosition(); - Quaternion orientation = transform.getOrientation(); - test(approxEqual(position.x, 6)); - test(approxEqual(position.y, 8)); - test(approxEqual(position.z, 11)); - test(approxEqual(orientation.x, sinB)); - test(approxEqual(orientation.y, sinB)); - test(approxEqual(orientation.z, sinB)); + etk::Transform3D transform1(vec3(4, 5, 6), etk::Quaternion::identity()); + etk::Transform3D transform2(vec3(8, 11, 16), etk::Quaternion(sinA, sinA, sinA, cosA)); + etk::Transform3D transform = Transform::int32_terpolateTransforms(transform1, transform2, 0.5); + vec3 position = transform.getPosition(); + etk::Quaternion orientation = transform.getOrientation(); + test(approxEqual(position.x(), 6)); + test(approxEqual(position.y(), 8)); + test(approxEqual(position.z(), 11)); + test(approxEqual(orientation.x(), sinB)); + test(approxEqual(orientation.y(), sinB)); + test(approxEqual(orientation.z(), sinB)); test(approxEqual(orientation.w, cosB)); } /// Test the identity methods void testIdentity() { - Transform transform = Transform::identity(); - test(transform.getPosition() == Vector3(0, 0, 0)); - test(transform.getOrientation() == Quaternion::identity()); + etk::Transform3D transform = Transform::identity(); + test(transform.getPosition() == vec3(0, 0, 0)); + test(transform.getOrientation() == etk::Quaternion::identity()); - Transform transform2(Vector3(5, 6, 2), Quaternion(3, 5, 1, 6)); + etk::Transform3D transform2(vec3(5, 6, 2), etk::Quaternion(3, 5, 1, 6)); transform2.setToIdentity(); - test(transform2.getPosition() == Vector3(0, 0, 0)); - test(transform2.getOrientation() == Quaternion::identity()); + test(transform2.getPosition() == vec3(0, 0, 0)); + test(transform2.getOrientation() == etk::Quaternion::identity()); } /// Test the overloaded operators @@ -196,17 +196,17 @@ class TestTransform : public Test { test(m_transform1 != m_transform2); // Assignment operator - Transform transform; + etk::Transform3D transform; transform = m_transform1; test(transform == m_transform1); // Multiplication - Vector3 vector(7, 53, 5); - Vector3 vector2 = m_transform2 * (m_transform1 * vector); - Vector3 vector3 = (m_transform2 * m_transform1) * vector; - test(approxEqual(vector2.x, vector3.x, float(10e-6))); - test(approxEqual(vector2.y, vector3.y, float(10e-6))); - test(approxEqual(vector2.z, vector3.z, float(10e-6))); + vec3 vector(7, 53, 5); + vec3 vector2 = m_transform2 * (m_transform1 * vector); + vec3 vector3 = (m_transform2 * m_transform1) * vector; + test(approxEqual(vector2.x(), vector3.x, float(10e-6))); + test(approxEqual(vector2.y(), vector3.y, float(10e-6))); + test(approxEqual(vector2.z(), vector3.z, float(10e-6))); } }; diff --git a/test/tests/mathematics/TestVector2.h b/test/tests/mathematics/TestVector2.h index 84e6807..608d384 100644 --- a/test/tests/mathematics/TestVector2.h +++ b/test/tests/mathematics/TestVector2.h @@ -28,33 +28,33 @@ // Libraries #include -#include +#include /// Reactphysics3D namespace namespace reactphysics3d { -// Class TestVector2 +// Class Testvec2 /** - * Unit test for the Vector2 class + * Unit test for the vec2 class */ -class TestVector2 : public Test { +class Testvec2 : public Test { private : // ---------- Atributes ---------- // /// Zero vector - Vector2 mVectorZero; + vec2 mVectorZero; // Vector (3, 4) - Vector2 mVector34; + vec2 mvec34; public : // ---------- Methods ---------- // /// Constructor - TestVector2(const std::string& name) : Test(name), mVectorZero(0, 0), mVector34(3, 4) {} + Testvec2(const std::string& name) : Test(name), mVectorZero(0, 0), mvec34(3, 4) {} /// Run the tests void run() { @@ -69,25 +69,25 @@ class TestVector2 : public Test { void testConstructors() { // Test constructor - test(mVectorZero.x == 0.0); - test(mVectorZero.y == 0.0); - test(mVector34.x == 3.0); - test(mVector34.y == 4.0); + test(mVectorZero.x() == 0.0); + test(mVectorZero.y() == 0.0); + test(mvec34.x() == 3.0); + test(mvec34.y() == 4.0); // Test copy-constructor - Vector2 newVector(mVector34); - test(newVector.x == 3.0); - test(newVector.y == 4.0); + vec2 newVector(mvec34); + test(newVector.x() == 3.0); + test(newVector.y() == 4.0); // Test method to set values - Vector2 newVector2; - newVector2.setAllValues(float(6.1), float(7.2)); - test(approxEqual(newVector2.x, float(6.1))); - test(approxEqual(newVector2.y, float(7.2))); + vec2 newvec2; + newvec2.setValue(float(6.1), float(7.2)); + test(approxEqual(newvec2.x(), float(6.1))); + test(approxEqual(newvec2.y(), float(7.2))); // Test method to set to zero - newVector2.setToZero(); - test(newVector2 == Vector2(0, 0)); + newvec2.setZero(); + test(newvec2 == vec2(0, 0)); } /// Test the length, unit vector and normalize methods @@ -95,124 +95,124 @@ class TestVector2 : public Test { // Test length methods test(mVectorZero.length() == 0.0); - test(mVectorZero.lengthSquare() == 0.0); - test(Vector2(1, 0).length() == 1.0); - test(Vector2(0, 1).length() == 1.0); - test(mVector34.lengthSquare() == 25.0); + test(mVectorZero.length2() == 0.0); + test(vec2(1, 0).length() == 1.0); + test(vec2(0, 1).length() == 1.0); + test(mvec34.length2() == 25.0); // Test unit vector methods - test(Vector2(1, 0).isUnit()); - test(Vector2(0, 1).isUnit()); - test(!mVector34.isUnit()); - test(Vector2(5, 0).getUnit() == Vector2(1, 0)); - test(Vector2(0, 5).getUnit() == Vector2(0, 1)); + test(vec2(1, 0).isUnit()); + test(vec2(0, 1).isUnit()); + test(!mvec34.isUnit()); + test(vec2(5, 0).safeNormalized() == vec2(1, 0)); + test(vec2(0, 5).safeNormalized() == vec2(0, 1)); - test(!mVector34.isZero()); + test(!mvec34.isZero()); test(mVectorZero.isZero()); // Test normalization method - Vector2 mVector10(1, 0); - Vector2 mVector01(0, 1); - Vector2 mVector50(5, 0); - Vector2 mVector05(0, 5); + vec2 mVector10(1, 0); + vec2 mVector01(0, 1); + vec2 mVector50(5, 0); + vec2 mVector05(0, 5); mVector10.normalize(); mVector01.normalize(); mVector50.normalize(); mVector05.normalize(); - test(mVector10 == Vector2(1, 0)); - test(mVector01 == Vector2(0, 1)); - test(mVector50 == Vector2(1, 0)); - test(mVector05 == Vector2(0, 1)); + test(mVector10 == vec2(1, 0)); + test(mVector01 == vec2(0, 1)); + test(mVector50 == vec2(1, 0)); + test(mVector05 == vec2(0, 1)); } /// Test the dot product void testDotProduct() { // Test the dot product - test(Vector2(5, 0).dot(Vector2(0, 8)) == 0); - test(Vector2(5, 8).dot(Vector2(0, 0)) == 0); - test(Vector2(12, 45).dot(Vector2(0, 0)) == 0); - test(Vector2(5, 7).dot(Vector2(5, 7)) == 74); - test(Vector2(3, 6).dot(Vector2(-3, -6)) == -45); - test(Vector2(2, 3).dot(Vector2(-7, 4)) == -2); - test(Vector2(4, 3).dot(Vector2(8, 9)) == 59); + test(vec2(5, 0).dot(vec2(0, 8)) == 0); + test(vec2(5, 8).dot(vec2(0, 0)) == 0); + test(vec2(12, 45).dot(vec2(0, 0)) == 0); + test(vec2(5, 7).dot(vec2(5, 7)) == 74); + test(vec2(3, 6).dot(vec2(-3, -6)) == -45); + test(vec2(2, 3).dot(vec2(-7, 4)) == -2); + test(vec2(4, 3).dot(vec2(8, 9)) == 59); } /// Test others methods void testOthersMethods() { // Test the method that returns the absolute vector - test(Vector2(4, 5).getAbsoluteVector() == Vector2(4, 5)); - test(Vector2(-7, -24).getAbsoluteVector() == Vector2(7, 24)); + test(vec2(4, 5).absolute() == vec2(4, 5)); + test(vec2(-7, -24).absolute() == vec2(7, 24)); // Test the method that returns the minimal element - test(Vector2(6, 35).getMinAxis() == 0); - test(Vector2(564, 45).getMinAxis() == 1); - test(Vector2(98, 23).getMinAxis() == 1); - test(Vector2(-53, -25).getMinAxis() == 0); + test(vec2(6, 35).getMinAxis() == 0); + test(vec2(564, 45).getMinAxis() == 1); + test(vec2(98, 23).getMinAxis() == 1); + test(vec2(-53, -25).getMinAxis() == 0); // Test the method that returns the maximal element - test(Vector2(6, 35).getMaxAxis() == 1); - test(Vector2(7, 537).getMaxAxis() == 1); - test(Vector2(98, 23).getMaxAxis() == 0); - test(Vector2(-53, -25).getMaxAxis() == 1); + test(vec2(6, 35).getMaxAxis() == 1); + test(vec2(7, 537).getMaxAxis() == 1); + test(vec2(98, 23).getMaxAxis() == 0); + test(vec2(-53, -25).getMaxAxis() == 1); // Test the methot that return a max/min vector - Vector2 vec1(-5, 4); - Vector2 vec2(-8, 6); - test(Vector2::min(vec1, vec2) == Vector2(-8, 4)); - test(Vector2::max(vec1, vec2) == Vector2(-5, 6)); + vec2 vec1(-5, 4); + vec2 vec2(-8, 6); + test(vec2::min(vec1, vec2) == vec2(-8, 4)); + test(vec2::max(vec1, vec2) == vec2(-5, 6)); } /// Test the operators void testOperators() { // Test the [] operator - test(mVector34[0] == 3); - test(mVector34[1] == 4); + test(mvec34[0] == 3); + test(mvec34[1] == 4); // Assignment operator - Vector2 newVector(6, 4); - newVector = Vector2(7, 8); - test(newVector == Vector2(7, 8)); + vec2 newVector(6, 4); + newVector = vec2(7, 8); + test(newVector == vec2(7, 8)); // Equality, inequality operators - test(Vector2(5, 7) == Vector2(5, 7)); - test(Vector2(63, 64) != Vector2(63, 84)); - test(Vector2(63, 64) != Vector2(12, 64)); + test(vec2(5, 7) == vec2(5, 7)); + test(vec2(63, 64) != vec2(63, 84)); + test(vec2(63, 64) != vec2(12, 64)); // Addition, substraction - Vector2 vector1(6, 33); - Vector2 vector2(7, 68); - test(Vector2(63, 24) + Vector2(3, 4) == Vector2(66, 28)); - test(Vector2(63, 24) - Vector2(3, 4) == Vector2(60, 20)); - vector1 += Vector2(5, 10); - vector2 -= Vector2(10, 21); - test(vector1 == Vector2(11, 43)); - test(vector2 == Vector2(-3, 47)); + vec2 vector1(6, 33); + vec2 vector2(7, 68); + test(vec2(63, 24) + vec2(3, 4) == vec2(66, 28)); + test(vec2(63, 24) - vec2(3, 4) == vec2(60, 20)); + vector1 += vec2(5, 10); + vector2 -= vec2(10, 21); + test(vector1 == vec2(11, 43)); + test(vector2 == vec2(-3, 47)); // Multiplication, division - Vector2 vector3(6, 33); - Vector2 vector4(15, 60); - test(Vector2(63, 24) * 3 == Vector2(189, 72)); - test(3 * Vector2(63, 24) == Vector2(189, 72)); - test(Vector2(14, 8) / 2 == Vector2(7, 4)); + vec2 vector3(6, 33); + vec2 vector4(15, 60); + test(vec2(63, 24) * 3 == vec2(189, 72)); + test(3 * vec2(63, 24) == vec2(189, 72)); + test(vec2(14, 8) / 2 == vec2(7, 4)); vector3 *= 10; vector4 /= 3; - test(vector3 == Vector2(60, 330)); - test(vector4 == Vector2(5, 20)); - Vector2 vector5(21, 80); - Vector2 vector6(7, 10); - Vector2 vector7 = vector5 * vector6; - test(vector7 == Vector2(147, 800)); - Vector2 vector8 = vector5 / vector6; - test(approxEqual(vector8.x, 3)); - test(approxEqual(vector8.y, 8)); + test(vector3 == vec2(60, 330)); + test(vector4 == vec2(5, 20)); + vec2 vector5(21, 80); + vec2 vector6(7, 10); + vec2 vector7 = vector5 * vector6; + test(vector7 == vec2(147, 800)); + vec2 vector8 = vector5 / vector6; + test(approxEqual(vector8.x(), 3)); + test(approxEqual(vector8.y(), 8)); // Negative operator - Vector2 vector9(-34, 5); - Vector2 negative = -vector9; - test(negative == Vector2(34, -5)); + vec2 vector9(-34, 5); + vec2 negative = -vector9; + test(negative == vec2(34, -5)); } }; diff --git a/test/tests/mathematics/TestVector3.h b/test/tests/mathematics/TestVector3.h index 0ee0db1..cfe66c3 100644 --- a/test/tests/mathematics/TestVector3.h +++ b/test/tests/mathematics/TestVector3.h @@ -28,33 +28,33 @@ // Libraries #include -#include +#include /// Reactphysics3D namespace namespace reactphysics3d { -// Class TestVector3 +// Class Testvec3 /** - * Unit test for the Vector3 class + * Unit test for the vec3 class */ -class TestVector3 : public Test { +class Testvec3 : public Test { private : // ---------- Atributes ---------- // /// Zero vector - Vector3 mVectorZero; + vec3 mVectorZero; // Vector (3, 4, 5) - Vector3 mVector345; + vec3 mvec345; public : // ---------- Methods ---------- // /// Constructor - TestVector3(const std::string& name): Test(name),mVectorZero(0, 0, 0),mVector345(3, 4, 5) {} + Testvec3(const std::string& name): Test(name),mVectorZero(0, 0, 0),mvec345(3, 4, 5) {} /// Run the tests void run() { @@ -69,29 +69,29 @@ class TestVector3 : public Test { void testConstructors() { // Test constructor - test(mVectorZero.x == 0.0); - test(mVectorZero.y == 0.0); - test(mVectorZero.z == 0.0); - test(mVector345.x == 3.0); - test(mVector345.y == 4.0); - test(mVector345.z == 5.0); + test(mVectorZero.x() == 0.0); + test(mVectorZero.y() == 0.0); + test(mVectorZero.z() == 0.0); + test(mvec345.x() == 3.0); + test(mvec345.y() == 4.0); + test(mvec345.z() == 5.0); // Test copy-constructor - Vector3 newVector(mVector345); - test(newVector.x == 3.0); - test(newVector.y == 4.0); - test(newVector.z == 5.0); + vec3 newVector(mvec345); + test(newVector.x() == 3.0); + test(newVector.y() == 4.0); + test(newVector.z() == 5.0); // Test method to set values - Vector3 newVector2; - newVector2.setAllValues(float(6.1), float(7.2), float(8.6)); - test(approxEqual(newVector2.x, float(6.1))); - test(approxEqual(newVector2.y, float(7.2))); - test(approxEqual(newVector2.z, float(8.6))); + vec3 newvec2; + newvec2.setValue(float(6.1), float(7.2), float(8.6)); + test(approxEqual(newvec2.x(), float(6.1))); + test(approxEqual(newvec2.y(), float(7.2))); + test(approxEqual(newvec2.z(), float(8.6))); // Test method to set to zero - newVector2.setToZero(); - test(newVector2 == Vector3(0, 0, 0)); + newvec2.setZero(); + test(newvec2 == vec3(0, 0, 0)); } /// Test the length, unit vector and normalize methods @@ -99,145 +99,145 @@ class TestVector3 : public Test { // Test length methods test(mVectorZero.length() == 0.0); - test(mVectorZero.lengthSquare() == 0.0); - test(Vector3(1, 0, 0).length() == 1.0); - test(Vector3(0, 1, 0).length() == 1.0); - test(Vector3(0, 0, 1).length() == 1.0); - test(mVector345.lengthSquare() == 50.0); + test(mVectorZero.length2() == 0.0); + test(vec3(1, 0, 0).length() == 1.0); + test(vec3(0, 1, 0).length() == 1.0); + test(vec3(0, 0, 1).length() == 1.0); + test(mvec345.length2() == 50.0); // Test unit vector methods - test(Vector3(1, 0, 0).isUnit()); - test(Vector3(0, 1, 0).isUnit()); - test(Vector3(0, 0, 1).isUnit()); - test(!mVector345.isUnit()); - test(Vector3(5, 0, 0).getUnit() == Vector3(1, 0, 0)); - test(Vector3(0, 5, 0).getUnit() == Vector3(0, 1, 0)); - test(Vector3(0, 0, 5).getUnit() == Vector3(0, 0, 1)); + test(vec3(1, 0, 0).isUnit()); + test(vec3(0, 1, 0).isUnit()); + test(vec3(0, 0, 1).isUnit()); + test(!mvec345.isUnit()); + test(vec3(5, 0, 0).safeNormalized() == vec3(1, 0, 0)); + test(vec3(0, 5, 0).safeNormalized() == vec3(0, 1, 0)); + test(vec3(0, 0, 5).safeNormalized() == vec3(0, 0, 1)); - test(!mVector345.isZero()); + test(!mvec345.isZero()); test(mVectorZero.isZero()); // Test normalization method - Vector3 mVector100(1, 0, 0); - Vector3 mVector010(0, 1, 0); - Vector3 mVector001(0, 0, 1); - Vector3 mVector500(5, 0, 0); - Vector3 mVector050(0, 5, 0); - Vector3 mVector005(0, 0, 5); + vec3 mVector100(1, 0, 0); + vec3 mVector010(0, 1, 0); + vec3 mVector001(0, 0, 1); + vec3 mVector500(5, 0, 0); + vec3 mVector050(0, 5, 0); + vec3 mVector005(0, 0, 5); mVector100.normalize(); mVector010.normalize(); mVector001.normalize(); mVector500.normalize(); mVector050.normalize(); mVector005.normalize(); - test(mVector100 == Vector3(1, 0, 0)); - test(mVector010 == Vector3(0, 1, 0)); - test(mVector001 == Vector3(0, 0, 1)); - test(mVector500 == Vector3(1, 0, 0)); - test(mVector050 == Vector3(0, 1, 0)); - test(mVector005 == Vector3(0, 0, 1)); + test(mVector100 == vec3(1, 0, 0)); + test(mVector010 == vec3(0, 1, 0)); + test(mVector001 == vec3(0, 0, 1)); + test(mVector500 == vec3(1, 0, 0)); + test(mVector050 == vec3(0, 1, 0)); + test(mVector005 == vec3(0, 0, 1)); } /// Test the dot and cross products void testDotCrossProducts() { // Test the dot product - test(Vector3(5, 0, 0).dot(Vector3(0, 8, 0)) == 0); - test(Vector3(5, 8, 0).dot(Vector3(0, 0, 6)) == 0); - test(Vector3(12, 45, 83).dot(Vector3(0, 0, 0)) == 0); - test(Vector3(5, 7, 8).dot(Vector3(5, 7, 8)) == 138); - test(Vector3(3, 6, 78).dot(Vector3(-3, -6, -78)) == -6129); - test(Vector3(2, 3, 5).dot(Vector3(2, 3, 5)) == 38); - test(Vector3(4, 3, 2).dot(Vector3(8, 9, 10)) == 79); + test(vec3(5, 0, 0).dot(vec3(0, 8, 0)) == 0); + test(vec3(5, 8, 0).dot(vec3(0, 0, 6)) == 0); + test(vec3(12, 45, 83).dot(vec3(0, 0, 0)) == 0); + test(vec3(5, 7, 8).dot(vec3(5, 7, 8)) == 138); + test(vec3(3, 6, 78).dot(vec3(-3, -6, -78)) == -6129); + test(vec3(2, 3, 5).dot(vec3(2, 3, 5)) == 38); + test(vec3(4, 3, 2).dot(vec3(8, 9, 10)) == 79); // Test the cross product - test(Vector3(0, 0, 0).cross(Vector3(0, 0, 0)) == Vector3(0, 0, 0)); - test(Vector3(6, 7, 2).cross(Vector3(6, 7, 2)) == Vector3(0, 0, 0)); - test(Vector3(1, 0, 0).cross(Vector3(0, 1, 0)) == Vector3(0, 0, 1)); - test(Vector3(0, 1, 0).cross(Vector3(0, 0, 1)) == Vector3(1, 0, 0)); - test(Vector3(0, 0, 1).cross(Vector3(0, 1, 0)) == Vector3(-1, 0, 0)); - test(Vector3(4, 7, 24).cross(Vector3(8, 13, 11)) == Vector3(-235, 148, -4)); - test(Vector3(-4, 42, -2).cross(Vector3(35, 7, -21)) == Vector3(-868, -154, -1498)); + test(vec3(0, 0, 0).cross(vec3(0, 0, 0)) == vec3(0, 0, 0)); + test(vec3(6, 7, 2).cross(vec3(6, 7, 2)) == vec3(0, 0, 0)); + test(vec3(1, 0, 0).cross(vec3(0, 1, 0)) == vec3(0, 0, 1)); + test(vec3(0, 1, 0).cross(vec3(0, 0, 1)) == vec3(1, 0, 0)); + test(vec3(0, 0, 1).cross(vec3(0, 1, 0)) == vec3(-1, 0, 0)); + test(vec3(4, 7, 24).cross(vec3(8, 13, 11)) == vec3(-235, 148, -4)); + test(vec3(-4, 42, -2).cross(vec3(35, 7, -21)) == vec3(-868, -154, -1498)); } /// Test others methods void testOthersMethods() { // Test the method that returns the absolute vector - test(Vector3(4, 5, 6).getAbsoluteVector() == Vector3(4, 5, 6)); - test(Vector3(-7, -24, -12).getAbsoluteVector() == Vector3(7, 24, 12)); + test(vec3(4, 5, 6).absolute() == vec3(4, 5, 6)); + test(vec3(-7, -24, -12).absolute() == vec3(7, 24, 12)); // Test the method that returns the minimal element - test(Vector3(6, 35, 82).getMinAxis() == 0); - test(Vector3(564, 45, 532).getMinAxis() == 1); - test(Vector3(98, 23, 3).getMinAxis() == 2); - test(Vector3(-53, -25, -63).getMinAxis() == 2); + test(vec3(6, 35, 82).getMinAxis() == 0); + test(vec3(564, 45, 532).getMinAxis() == 1); + test(vec3(98, 23, 3).getMinAxis() == 2); + test(vec3(-53, -25, -63).getMinAxis() == 2); // Test the method that returns the maximal element - test(Vector3(6, 35, 82).getMaxAxis() == 2); - test(Vector3(7, 533, 36).getMaxAxis() == 1); - test(Vector3(98, 23, 3).getMaxAxis() == 0); - test(Vector3(-53, -25, -63).getMaxAxis() == 1); + test(vec3(6, 35, 82).getMaxAxis() == 2); + test(vec3(7, 533, 36).getMaxAxis() == 1); + test(vec3(98, 23, 3).getMaxAxis() == 0); + test(vec3(-53, -25, -63).getMaxAxis() == 1); // Test the methot that return a max/min vector - Vector3 vec1(-5, 4, 2); - Vector3 vec2(-8, 6, -1); - test(Vector3::min(vec1, vec2) == Vector3(-8, 4, -1)); - test(Vector3::max(vec1, vec2) == Vector3(-5, 6, 2)); + vec3 vec1(-5, 4, 2); + vec3 vec2(-8, 6, -1); + test(vec3::min(vec1, vec2) == vec3(-8, 4, -1)); + test(vec3::max(vec1, vec2) == vec3(-5, 6, 2)); } /// Test the operators void testOperators() { // Test the [] operator - test(mVector345[0] == 3); - test(mVector345[1] == 4); - test(mVector345[2] == 5); + test(mvec345[0] == 3); + test(mvec345[1] == 4); + test(mvec345[2] == 5); // Assignment operator - Vector3 newVector(6, 4, 2); - newVector = Vector3(7, 8, 9); - test(newVector == Vector3(7, 8, 9)); + vec3 newVector(6, 4, 2); + newVector = vec3(7, 8, 9); + test(newVector == vec3(7, 8, 9)); // Equality, inequality operators - test(Vector3(5, 7, 3) == Vector3(5, 7, 3)); - test(Vector3(63, 64, 24) != Vector3(63, 64, 5)); - test(Vector3(63, 64, 24) != Vector3(12, 64, 24)); - test(Vector3(63, 64, 24) != Vector3(63, 8, 24)); + test(vec3(5, 7, 3) == vec3(5, 7, 3)); + test(vec3(63, 64, 24) != vec3(63, 64, 5)); + test(vec3(63, 64, 24) != vec3(12, 64, 24)); + test(vec3(63, 64, 24) != vec3(63, 8, 24)); // Addition, substraction - Vector3 vector1(6, 33, 62); - Vector3 vector2(7, 68, 35); - test(Vector3(63, 24, 5) + Vector3(3, 4, 2) == Vector3(66, 28, 7)); - test(Vector3(63, 24, 5) - Vector3(3, 4, 2) == Vector3(60, 20, 3)); - vector1 += Vector3(5, 10, 12); - vector2 -= Vector3(10, 21, 5); - test(vector1 == Vector3(11, 43, 74)); - test(vector2 == Vector3(-3, 47, 30)); + vec3 vector1(6, 33, 62); + vec3 vector2(7, 68, 35); + test(vec3(63, 24, 5) + vec3(3, 4, 2) == vec3(66, 28, 7)); + test(vec3(63, 24, 5) - vec3(3, 4, 2) == vec3(60, 20, 3)); + vector1 += vec3(5, 10, 12); + vector2 -= vec3(10, 21, 5); + test(vector1 == vec3(11, 43, 74)); + test(vector2 == vec3(-3, 47, 30)); // Multiplication, division - Vector3 vector3(6, 33, 62); - Vector3 vector4(15, 60, 33); - test(Vector3(63, 24, 5) * 3 == Vector3(189, 72, 15)); - test(3 * Vector3(63, 24, 5) == Vector3(189, 72, 15)); - test(Vector3(14, 8, 50) / 2 == Vector3(7, 4, 25)); + vec3 vector3(6, 33, 62); + vec3 vector4(15, 60, 33); + test(vec3(63, 24, 5) * 3 == vec3(189, 72, 15)); + test(3 * vec3(63, 24, 5) == vec3(189, 72, 15)); + test(vec3(14, 8, 50) / 2 == vec3(7, 4, 25)); vector3 *= 10; vector4 /= 3; - test(vector3 == Vector3(60, 330, 620)); - test(vector4 == Vector3(5, 20, 11)); - Vector3 vector5(21, 80, 45); - Vector3 vector6(7, 10, 3); - Vector3 vector7 = vector5 * vector6; - test(vector7 == Vector3(147, 800, 135)); - Vector3 vector8 = vector5 / vector6; - test(approxEqual(vector8.x, 3)); - test(approxEqual(vector8.y, 8)); - test(approxEqual(vector8.z, 15)); + test(vector3 == vec3(60, 330, 620)); + test(vector4 == vec3(5, 20, 11)); + vec3 vector5(21, 80, 45); + vec3 vector6(7, 10, 3); + vec3 vector7 = vector5 * vector6; + test(vector7 == vec3(147, 800, 135)); + vec3 vector8 = vector5 / vector6; + test(approxEqual(vector8.x(), 3)); + test(approxEqual(vector8.y(), 8)); + test(approxEqual(vector8.z(), 15)); // Negative operator - Vector3 vector9(-34, 5, 422); - Vector3 negative = -vector9; - test(negative == Vector3(34, -5, -422)); + vec3 vector9(-34, 5, 422); + vec3 negative = -vector9; + test(negative == vec3(34, -5, -422)); } }; diff --git a/tools/testbed/common/Box.cpp b/tools/testbed/common/Box.cpp index 5350027..0bdac01 100644 --- a/tools/testbed/common/Box.cpp +++ b/tools/testbed/common/Box.cpp @@ -111,17 +111,17 @@ GLfloat Box::mCubeNormals[108] = { 0.0f, 0.0f, 1.0f// }; // Constructor -Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &position, +Box::Box(const openglframework::vec3& size, const openglframework::vec3 &position, reactphysics3d::CollisionWorld* world) : openglframework::Object3D() { // Initialize the size of the box - mSize[0] = size.x * 0.5f; - mSize[1] = size.y * 0.5f; - mSize[2] = size.z * 0.5f; + mSize[0] = size.x() * 0.5f; + mSize[1] = size.y() * 0.5f; + mSize[2] = size.z() * 0.5f; // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mSize[0], 0, 0, 0, + m_scalingMatrix = openglframework::Matrix4(mSize[0], 0, 0, 0, 0, mSize[1], 0, 0, 0, 0, mSize[2], 0, 0, 0, 0, 1); @@ -132,20 +132,20 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p // Create the collision shape for the rigid body (box shape) // ReactPhysics3D will clone this object to create an int32_ternal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() - mBoxShape = new rp3d::BoxShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2])); + mBoxShape = new rp3d::BoxShape(rp3d::vec3(mSize[0], mSize[1], mSize[2])); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); - mPreviousTransform = transform; + mPreviousetk::Transform3D = transform; // Create a rigid body in the dynamics world m_body = world->createCollisionBody(transform); // Add the collision shape to the body - m_proxyShape = m_body->addCollisionShape(mBoxShape, rp3d::Transform::identity()); + m_proxyShape = m_body->addCollisionShape(mBoxShape, rp3d::etk::Transform3D::identity()); // If the Vertex Buffer object has not been created yet if (totalNbBoxes == 0) { @@ -156,21 +156,21 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p totalNbBoxes++; - m_transformMatrix = m_transformMatrix * mScalingMatrix; + m_transformMatrix = m_transformMatrix * m_scalingMatrix; } // Constructor -Box::Box(const openglframework::Vector3& size, const openglframework::Vector3& position, +Box::Box(const openglframework::vec3& size, const openglframework::vec3& position, float mass, reactphysics3d::DynamicsWorld* world) : openglframework::Object3D() { // Initialize the size of the box - mSize[0] = size.x * 0.5f; - mSize[1] = size.y * 0.5f; - mSize[2] = size.z * 0.5f; + mSize[0] = size.x() * 0.5f; + mSize[1] = size.y() * 0.5f; + mSize[2] = size.z() * 0.5f; // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mSize[0], 0, 0, 0, + m_scalingMatrix = openglframework::Matrix4(mSize[0], 0, 0, 0, 0, mSize[1], 0, 0, 0, 0, mSize[2], 0, 0, 0, 0, 1); @@ -181,20 +181,20 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3& p // Create the collision shape for the rigid body (box shape) // ReactPhysics3D will clone this object to create an int32_ternal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() - mBoxShape = new rp3d::BoxShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2])); + mBoxShape = new rp3d::BoxShape(rp3d::vec3(mSize[0], mSize[1], mSize[2])); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); - mPreviousTransform = transform; + mPreviousetk::Transform3D = transform; // Create a rigid body in the dynamics world rp3d::RigidBody* body = world->createRigidBody(transform); // Add the collision shape to the body - m_proxyShape = body->addCollisionShape(mBoxShape, rp3d::Transform::identity(), mass); + m_proxyShape = body->addCollisionShape(mBoxShape, rp3d::etk::Transform3D::identity(), mass); m_body = body; @@ -207,7 +207,7 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3& p totalNbBoxes++; - m_transformMatrix = m_transformMatrix * mScalingMatrix; + m_transformMatrix = m_transformMatrix * m_scalingMatrix; } // Destructor @@ -245,7 +245,7 @@ void Box::render(openglframework::Shader& shader, const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * m_transformMatrix; const openglframework::Matrix3 normalMatrix = localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + shader.setetk::Matrix3x3Uniform("normalMatrix", normalMatrix, false); // Set the vertex color openglframework::Color currentColor = m_body->isSleeping() ? mSleepingColor : mColor; @@ -322,22 +322,22 @@ void Box::resetTransform(const rp3d::Transform& transform) { // Reset the velocity of the rigid body rp3d::RigidBody* rigidBody = dynamic_cast(m_body); if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setLinearVelocity(rp3d::vec3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::vec3(0, 0, 0)); } - updateTransform(1.0f); + updateetk::Transform3D(1.0f); } // Set the scaling of the object -void Box::setScaling(const openglframework::Vector3& scaling) { +void Box::setScaling(const openglframework::vec3& scaling) { // Scale the collision shape - m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); + m_proxyShape->setLocalScaling(rp3d::vec3(scaling.x(), scaling.y(), scaling.z())); // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(mSize[0] * scaling.x, 0, 0, 0, - 0, mSize[1] * scaling.y, 0, 0, - 0, 0, mSize[2] * scaling.z, 0, + m_scalingMatrix = openglframework::Matrix4(mSize[0] * scaling.x(), 0, 0, 0, + 0, mSize[1] * scaling.y(), 0, 0, + 0, 0, mSize[2] * scaling.z(), 0, 0, 0, 0, 1); } diff --git a/tools/testbed/common/Box.h b/tools/testbed/common/Box.h index 33e4ffe..460cff0 100644 --- a/tools/testbed/common/Box.h +++ b/tools/testbed/common/Box.h @@ -45,7 +45,7 @@ class Box : public openglframework::Object3D, public PhysicsObject { rp3d::ProxyShape* m_proxyShape; /// Scaling matrix (applied to a cube to obtain the correct box dimensions) - openglframework::Matrix4 mScalingMatrix; + openglframework::Matrix4 m_scalingMatrix; /// Vertex Buffer Object for the vertices data used to render the box with OpenGL static openglframework::VertexBufferObject mVBOVertices; @@ -75,11 +75,11 @@ class Box : public openglframework::Object3D, public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - Box(const openglframework::Vector3& size, const openglframework::Vector3& position, + Box(const openglframework::vec3& size, const openglframework::vec3& position, reactphysics3d::CollisionWorld* world); /// Constructor - Box(const openglframework::Vector3& size, const openglframework::Vector3& position, + Box(const openglframework::vec3& size, const openglframework::vec3& position, float mass, reactphysics3d::DynamicsWorld *world); /// Destructor @@ -92,15 +92,15 @@ class Box : public openglframework::Object3D, public PhysicsObject { void resetTransform(const rp3d::Transform& transform); /// Update the transform matrix of the object - virtual void updateTransform(float int32_terpolationFactor); + virtual void updateetk::Transform3D(float int32_terpolationFactor); /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::vec3& scaling); }; // Update the transform matrix of the object -inline void Box::updateTransform(float int32_terpolationFactor) { - m_transformMatrix = computeTransform(int32_terpolationFactor, mScalingMatrix); +inline void Box::updateetk::Transform3D(float int32_terpolationFactor) { + m_transformMatrix = computeetk::Transform3D(int32_terpolationFactor, m_scalingMatrix); } diff --git a/tools/testbed/common/Capsule.cpp b/tools/testbed/common/Capsule.cpp index a530d43..6473484 100644 --- a/tools/testbed/common/Capsule.cpp +++ b/tools/testbed/common/Capsule.cpp @@ -34,7 +34,7 @@ openglframework::VertexArrayObject Capsule::mVAO; int32_t Capsule::totalNbCapsules = 0; // Constructor -Capsule::Capsule(float radius, float height, const openglframework::Vector3& position, +Capsule::Capsule(float radius, float height, const openglframework::vec3& position, reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) : openglframework::Mesh(), mRadius(radius), mHeight(height) { @@ -46,7 +46,7 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos calculateNormals(); // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, + m_scalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, 0, (mHeight + 2.0f * mRadius) / 3, 0,0, 0, 0, mRadius, 0, 0, 0, 0, 1.0f); @@ -60,19 +60,19 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos mCapsuleShape = new rp3d::CapsuleShape(mRadius, mHeight); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); - mPreviousTransform = transform; + mPreviousetk::Transform3D = transform; // Create a rigid body corresponding in the dynamics world m_body = world->createCollisionBody(transform); // Add a collision shape to the body and specify the mass of the shape - m_proxyShape = m_body->addCollisionShape(mCapsuleShape, rp3d::Transform::identity()); + m_proxyShape = m_body->addCollisionShape(mCapsuleShape, rp3d::etk::Transform3D::identity()); - m_transformMatrix = m_transformMatrix * mScalingMatrix; + m_transformMatrix = m_transformMatrix * m_scalingMatrix; // Create the VBOs and VAO if (totalNbCapsules == 0) { @@ -83,7 +83,7 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos } // Constructor -Capsule::Capsule(float radius, float height, const openglframework::Vector3& position, +Capsule::Capsule(float radius, float height, const openglframework::vec3& position, float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) : openglframework::Mesh(), mRadius(radius), mHeight(height) { @@ -95,7 +95,7 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos calculateNormals(); // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, + m_scalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, 0, (mHeight + 2.0f * mRadius) / 3.0f, 0,0, 0, 0, mRadius, 0, 0, 0, 0, 1.0f); @@ -109,19 +109,19 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos mCapsuleShape = new rp3d::CapsuleShape(mRadius, mHeight); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); // Create a rigid body corresponding in the dynamics world rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); // Add a collision shape to the body and specify the mass of the shape - m_proxyShape = body->addCollisionShape(mCapsuleShape, rp3d::Transform::identity(), mass); + m_proxyShape = body->addCollisionShape(mCapsuleShape, rp3d::etk::Transform3D::identity(), mass); m_body = body; - m_transformMatrix = m_transformMatrix * mScalingMatrix; + m_transformMatrix = m_transformMatrix * m_scalingMatrix; // Create the VBOs and VAO if (totalNbCapsules == 0) { @@ -166,7 +166,7 @@ void Capsule::render(openglframework::Shader& shader, const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * m_transformMatrix; const openglframework::Matrix3 normalMatrix = localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + shader.setetk::Matrix3x3Uniform("normalMatrix", normalMatrix, false); // Set the vertex color openglframework::Color currentColor = m_body->isSleeping() ? mSleepingColor : mColor; @@ -215,14 +215,14 @@ void Capsule::createVBOAndVAO() { // Create the VBO for the vertices data mVBOVertices.create(); mVBOVertices.bind(); - size_t sizeVertices = m_vertices.size() * sizeof(openglframework::Vector3); + size_t sizeVertices = m_vertices.size() * sizeof(openglframework::vec3); mVBOVertices.copyDataIntoVBO(sizeVertices, getVerticesPointer(), GL_STATIC_DRAW); mVBOVertices.unbind(); // Create the VBO for the normals data mVBONormals.create(); mVBONormals.bind(); - size_t sizeNormals = mNormals.size() * sizeof(openglframework::Vector3); + size_t sizeNormals = mNormals.size() * sizeof(openglframework::vec3); mVBONormals.copyDataIntoVBO(sizeNormals, getNormalsPointer(), GL_STATIC_DRAW); mVBONormals.unbind(); @@ -230,7 +230,7 @@ void Capsule::createVBOAndVAO() { // Create the VBO for the texture co data mVBOTextureCoords.create(); mVBOTextureCoords.bind(); - size_t sizeTextureCoords = mUVs.size() * sizeof(openglframework::Vector2); + size_t sizeTextureCoords = mUVs.size() * sizeof(openglframework::vec2); mVBOTextureCoords.copyDataIntoVBO(sizeTextureCoords, getUVTextureCoordinatesPointer(), GL_STATIC_DRAW); mVBOTextureCoords.unbind(); } @@ -275,22 +275,22 @@ void Capsule::resetTransform(const rp3d::Transform& transform) { // Reset the velocity of the rigid body rp3d::RigidBody* rigidBody = dynamic_cast(m_body); if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setLinearVelocity(rp3d::vec3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::vec3(0, 0, 0)); } - updateTransform(1.0f); + updateetk::Transform3D(1.0f); } // Set the scaling of the object -void Capsule::setScaling(const openglframework::Vector3& scaling) { +void Capsule::setScaling(const openglframework::vec3& scaling) { // Scale the collision shape - m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); + m_proxyShape->setLocalScaling(rp3d::vec3(scaling.x(), scaling.y(), scaling.z())); // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0, - 0, (mHeight * scaling.y + 2.0f * mRadius * scaling.x) / 3, 0,0, - 0, 0, mRadius * scaling.x, 0, + m_scalingMatrix = openglframework::Matrix4(mRadius * scaling.x(), 0, 0, 0, + 0, (mHeight * scaling.y() + 2.0f * mRadius * scaling.x()) / 3, 0,0, + 0, 0, mRadius * scaling.x(), 0, 0, 0, 0, 1.0f); } diff --git a/tools/testbed/common/Capsule.h b/tools/testbed/common/Capsule.h index 18c039b..7124239 100644 --- a/tools/testbed/common/Capsule.h +++ b/tools/testbed/common/Capsule.h @@ -45,14 +45,14 @@ class Capsule : public openglframework::Mesh, public PhysicsObject { float mHeight; /// Scaling matrix (applied to a sphere to obtain the correct sphere dimensions) - openglframework::Matrix4 mScalingMatrix; + openglframework::Matrix4 m_scalingMatrix; /// Collision shape rp3d::CapsuleShape* mCapsuleShape; rp3d::ProxyShape* m_proxyShape; /// Previous transform (for int32_terpolation) - rp3d::Transform mPreviousTransform; + rp3d::etk::Transform3D mPreviousTransform; /// Vertex Buffer Object for the vertices data static openglframework::VertexBufferObject mVBOVertices; @@ -82,11 +82,11 @@ class Capsule : public openglframework::Mesh, public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - Capsule(float radius, float height, const openglframework::Vector3& position, + Capsule(float radius, float height, const openglframework::vec3& position, reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath); /// Constructor - Capsule(float radius, float height, const openglframework::Vector3& position, + Capsule(float radius, float height, const openglframework::vec3& position, float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath); @@ -101,15 +101,15 @@ class Capsule : public openglframework::Mesh, public PhysicsObject { void resetTransform(const rp3d::Transform& transform); /// Update the transform matrix of the object - virtual void updateTransform(float int32_terpolationFactor); + virtual void updateetk::Transform3D(float int32_terpolationFactor); /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::vec3& scaling); }; // Update the transform matrix of the object -inline void Capsule::updateTransform(float int32_terpolationFactor) { - m_transformMatrix = computeTransform(int32_terpolationFactor, mScalingMatrix); +inline void Capsule::updateetk::Transform3D(float int32_terpolationFactor) { + m_transformMatrix = computeetk::Transform3D(int32_terpolationFactor, m_scalingMatrix); } #endif diff --git a/tools/testbed/common/ConcaveMesh.cpp b/tools/testbed/common/ConcaveMesh.cpp index a0ace53..a147608 100644 --- a/tools/testbed/common/ConcaveMesh.cpp +++ b/tools/testbed/common/ConcaveMesh.cpp @@ -27,7 +27,7 @@ #include // Constructor -ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, +ConcaveMesh::ConcaveMesh(const openglframework::vec3 &position, reactphysics3d::CollisionWorld* world, const std::string& meshPath) : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), @@ -44,14 +44,14 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, translateWorld(position); // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4::identity(); + m_scalingMatrix = openglframework::Matrix4::identity(); // For each subpart of the mesh for (uint32_t i=0; icreateCollisionBody(transform); // Add a collision shape to the body and specify the mass of the collision shape - m_proxyShape = m_body->addCollisionShape(mConcaveShape, rp3d::Transform::identity()); + m_proxyShape = m_body->addCollisionShape(mConcaveShape, rp3d::etk::Transform3D::identity()); // Create the VBOs and VAO createVBOAndVAO(); - m_transformMatrix = m_transformMatrix * mScalingMatrix; + m_transformMatrix = m_transformMatrix * m_scalingMatrix; } // Constructor -ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, float mass, +ConcaveMesh::ConcaveMesh(const openglframework::vec3 &position, float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath) : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), @@ -101,14 +101,14 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, float mass, translateWorld(position); // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4::identity(); + m_scalingMatrix = openglframework::Matrix4::identity(); // For each subpart of the mesh for (uint32_t i=0; isetIsSmoothMeshCollisionEnabled(false); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); // Create a rigid body corresponding to the sphere in the dynamics world rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); // Add a collision shape to the body and specify the mass of the collision shape - m_proxyShape = body->addCollisionShape(mConcaveShape, rp3d::Transform::identity(), mass); + m_proxyShape = body->addCollisionShape(mConcaveShape, rp3d::etk::Transform3D::identity(), mass); m_body = body; // Create the VBOs and VAO createVBOAndVAO(); - m_transformMatrix = m_transformMatrix * mScalingMatrix; + m_transformMatrix = m_transformMatrix * m_scalingMatrix; } // Destructor @@ -179,7 +179,7 @@ void ConcaveMesh::render(openglframework::Shader& shader, const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * m_transformMatrix; const openglframework::Matrix3 normalMatrix = localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + shader.setetk::Matrix3x3Uniform("normalMatrix", normalMatrix, false); // Set the vertex color openglframework::Color currentColor = m_body->isSleeping() ? mSleepingColor : mColor; @@ -228,14 +228,14 @@ void ConcaveMesh::createVBOAndVAO() { // Create the VBO for the vertices data mVBOVertices.create(); mVBOVertices.bind(); - size_t sizeVertices = m_vertices.size() * sizeof(openglframework::Vector3); + size_t sizeVertices = m_vertices.size() * sizeof(openglframework::vec3); mVBOVertices.copyDataIntoVBO(sizeVertices, getVerticesPointer(), GL_STATIC_DRAW); mVBOVertices.unbind(); // Create the VBO for the normals data mVBONormals.create(); mVBONormals.bind(); - size_t sizeNormals = mNormals.size() * sizeof(openglframework::Vector3); + size_t sizeNormals = mNormals.size() * sizeof(openglframework::vec3); mVBONormals.copyDataIntoVBO(sizeNormals, getNormalsPointer(), GL_STATIC_DRAW); mVBONormals.unbind(); @@ -243,7 +243,7 @@ void ConcaveMesh::createVBOAndVAO() { // Create the VBO for the texture co data mVBOTextureCoords.create(); mVBOTextureCoords.bind(); - size_t sizeTextureCoords = mUVs.size() * sizeof(openglframework::Vector2); + size_t sizeTextureCoords = mUVs.size() * sizeof(openglframework::vec2); mVBOTextureCoords.copyDataIntoVBO(sizeTextureCoords, getUVTextureCoordinatesPointer(), GL_STATIC_DRAW); mVBOTextureCoords.unbind(); } @@ -288,23 +288,23 @@ void ConcaveMesh::resetTransform(const rp3d::Transform& transform) { // Reset the velocity of the rigid body rp3d::RigidBody* rigidBody = dynamic_cast(m_body); if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setLinearVelocity(rp3d::vec3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::vec3(0, 0, 0)); } - updateTransform(1.0f); + updateetk::Transform3D(1.0f); } // Set the scaling of the object -void ConcaveMesh::setScaling(const openglframework::Vector3& scaling) { +void ConcaveMesh::setScaling(const openglframework::vec3& scaling) { // Scale the collision shape - m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); + m_proxyShape->setLocalScaling(rp3d::vec3(scaling.x(), scaling.y(), scaling.z())); // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(scaling.x, 0, 0, 0, - 0, scaling.y, 0,0, - 0, 0, scaling.z, 0, + m_scalingMatrix = openglframework::Matrix4(scaling.x(), 0, 0, 0, + 0, scaling.y(), 0,0, + 0, 0, scaling.z(), 0, 0, 0, 0, 1.0f); } diff --git a/tools/testbed/common/ConcaveMesh.h b/tools/testbed/common/ConcaveMesh.h index 1b55fb3..1fd5bc4 100644 --- a/tools/testbed/common/ConcaveMesh.h +++ b/tools/testbed/common/ConcaveMesh.h @@ -39,14 +39,14 @@ class ConcaveMesh : public openglframework::Mesh, public PhysicsObject { // -------------------- Attributes -------------------- // /// Previous transform (for int32_terpolation) - rp3d::Transform mPreviousTransform; + rp3d::etk::Transform3D mPreviousTransform; /// Collision shape rp3d::ConcaveMeshShape* mConcaveShape; rp3d::ProxyShape* m_proxyShape; /// Scaling matrix - openglframework::Matrix4 mScalingMatrix; + openglframework::Matrix4 m_scalingMatrix; /// Vertex Buffer Object for the vertices data openglframework::VertexBufferObject mVBOVertices; @@ -76,11 +76,11 @@ class ConcaveMesh : public openglframework::Mesh, public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - ConcaveMesh(const openglframework::Vector3& position, + ConcaveMesh(const openglframework::vec3& position, rp3d::CollisionWorld* world, const std::string& meshPath); /// Constructor - ConcaveMesh(const openglframework::Vector3& position, float mass, + ConcaveMesh(const openglframework::vec3& position, float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath); /// Destructor @@ -94,15 +94,15 @@ class ConcaveMesh : public openglframework::Mesh, public PhysicsObject { void resetTransform(const rp3d::Transform& transform); /// Update the transform matrix of the object - virtual void updateTransform(float int32_terpolationFactor); + virtual void updateetk::Transform3D(float int32_terpolationFactor); /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::vec3& scaling); }; // Update the transform matrix of the object -inline void ConcaveMesh::updateTransform(float int32_terpolationFactor) { - m_transformMatrix = computeTransform(int32_terpolationFactor, mScalingMatrix); +inline void ConcaveMesh::updateetk::Transform3D(float int32_terpolationFactor) { + m_transformMatrix = computeetk::Transform3D(int32_terpolationFactor, m_scalingMatrix); } #endif diff --git a/tools/testbed/common/Cone.cpp b/tools/testbed/common/Cone.cpp index b0b51a2..34c091e 100644 --- a/tools/testbed/common/Cone.cpp +++ b/tools/testbed/common/Cone.cpp @@ -34,7 +34,7 @@ openglframework::VertexArrayObject Cone::mVAO; int32_t Cone::totalNbCones = 0; // Constructor -Cone::Cone(float radius, float height, const openglframework::Vector3 &position, +Cone::Cone(float radius, float height, const openglframework::vec3 &position, reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) : openglframework::Mesh(), mRadius(radius), mHeight(height) { @@ -46,7 +46,7 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position, calculateNormals(); // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, + m_scalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, 0, mHeight, 0, 0, 0, 0, mRadius, 0, 0, 0, 0, 1); @@ -59,19 +59,19 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position, mConeShape = new rp3d::ConeShape(mRadius, mHeight); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); - mPreviousTransform = transform; + mPreviousetk::Transform3D = transform; // Create a rigid body corresponding to the cone in the dynamics world m_body = world->createCollisionBody(transform); // Add a collision shape to the body and specify the mass of the shape - m_proxyShape = m_body->addCollisionShape(mConeShape, rp3d::Transform::identity()); + m_proxyShape = m_body->addCollisionShape(mConeShape, rp3d::etk::Transform3D::identity()); - m_transformMatrix = m_transformMatrix * mScalingMatrix; + m_transformMatrix = m_transformMatrix * m_scalingMatrix; // Create the VBOs and VAO if (totalNbCones == 0) { @@ -82,7 +82,7 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position, } // Constructor -Cone::Cone(float radius, float height, const openglframework::Vector3 &position, +Cone::Cone(float radius, float height, const openglframework::vec3 &position, float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) : openglframework::Mesh(), mRadius(radius), mHeight(height) { @@ -94,7 +94,7 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position, calculateNormals(); // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, + m_scalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, 0, mHeight, 0, 0, 0, 0, mRadius, 0, 0, 0, 0, 1); @@ -107,19 +107,19 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position, mConeShape = new rp3d::ConeShape(mRadius, mHeight); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); // Create a rigid body corresponding to the cone in the dynamics world rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); // Add a collision shape to the body and specify the mass of the shape - m_proxyShape = body->addCollisionShape(mConeShape, rp3d::Transform::identity(), mass); + m_proxyShape = body->addCollisionShape(mConeShape, rp3d::etk::Transform3D::identity(), mass); m_body = body; - m_transformMatrix = m_transformMatrix * mScalingMatrix; + m_transformMatrix = m_transformMatrix * m_scalingMatrix; // Create the VBOs and VAO if (totalNbCones == 0) { @@ -163,7 +163,7 @@ void Cone::render(openglframework::Shader& shader, const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * m_transformMatrix; const openglframework::Matrix3 normalMatrix = localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + shader.setetk::Matrix3x3Uniform("normalMatrix", normalMatrix, false); // Set the vertex color openglframework::Color currentColor = m_body->isSleeping() ? mSleepingColor : mColor; @@ -212,14 +212,14 @@ void Cone::createVBOAndVAO() { // Create the VBO for the vertices data mVBOVertices.create(); mVBOVertices.bind(); - size_t sizeVertices = m_vertices.size() * sizeof(openglframework::Vector3); + size_t sizeVertices = m_vertices.size() * sizeof(openglframework::vec3); mVBOVertices.copyDataIntoVBO(sizeVertices, getVerticesPointer(), GL_STATIC_DRAW); mVBOVertices.unbind(); // Create the VBO for the normals data mVBONormals.create(); mVBONormals.bind(); - size_t sizeNormals = mNormals.size() * sizeof(openglframework::Vector3); + size_t sizeNormals = mNormals.size() * sizeof(openglframework::vec3); mVBONormals.copyDataIntoVBO(sizeNormals, getNormalsPointer(), GL_STATIC_DRAW); mVBONormals.unbind(); @@ -227,7 +227,7 @@ void Cone::createVBOAndVAO() { // Create the VBO for the texture co data mVBOTextureCoords.create(); mVBOTextureCoords.bind(); - size_t sizeTextureCoords = mUVs.size() * sizeof(openglframework::Vector2); + size_t sizeTextureCoords = mUVs.size() * sizeof(openglframework::vec2); mVBOTextureCoords.copyDataIntoVBO(sizeTextureCoords, getUVTextureCoordinatesPointer(), GL_STATIC_DRAW); mVBOTextureCoords.unbind(); } @@ -272,23 +272,23 @@ void Cone::resetTransform(const rp3d::Transform& transform) { // Reset the velocity of the rigid body rp3d::RigidBody* rigidBody = dynamic_cast(m_body); if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setLinearVelocity(rp3d::vec3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::vec3(0, 0, 0)); } - updateTransform(1.0f); + updateetk::Transform3D(1.0f); } // Set the scaling of the object -void Cone::setScaling(const openglframework::Vector3& scaling) { +void Cone::setScaling(const openglframework::vec3& scaling) { // Scale the collision shape - m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); + m_proxyShape->setLocalScaling(rp3d::vec3(scaling.x(), scaling.y(), scaling.z())); // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0, - 0, mHeight * scaling.y, 0, 0, - 0, 0, mRadius * scaling.z, 0, + m_scalingMatrix = openglframework::Matrix4(mRadius * scaling.x(), 0, 0, 0, + 0, mHeight * scaling.y(), 0, 0, + 0, 0, mRadius * scaling.z(), 0, 0, 0, 0, 1); } diff --git a/tools/testbed/common/Cone.h b/tools/testbed/common/Cone.h index 0e5a4e9..837822d 100644 --- a/tools/testbed/common/Cone.h +++ b/tools/testbed/common/Cone.h @@ -49,10 +49,10 @@ class Cone : public openglframework::Mesh, public PhysicsObject { rp3d::ProxyShape* m_proxyShape; /// Scaling matrix (applied to a sphere to obtain the correct cone dimensions) - openglframework::Matrix4 mScalingMatrix; + openglframework::Matrix4 m_scalingMatrix; /// Previous transform (for int32_terpolation) - rp3d::Transform mPreviousTransform; + rp3d::etk::Transform3D mPreviousTransform; /// Vertex Buffer Object for the vertices data static openglframework::VertexBufferObject mVBOVertices; @@ -82,11 +82,11 @@ class Cone : public openglframework::Mesh, public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - Cone(float radius, float height, const openglframework::Vector3& position, + Cone(float radius, float height, const openglframework::vec3& position, rp3d::CollisionWorld* world, const std::string& meshFolderPath); /// Constructor - Cone(float radius, float height, const openglframework::Vector3& position, + Cone(float radius, float height, const openglframework::vec3& position, float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath); /// Destructor @@ -100,14 +100,14 @@ class Cone : public openglframework::Mesh, public PhysicsObject { void resetTransform(const rp3d::Transform& transform); /// Update the transform matrix of the object - virtual void updateTransform(float int32_terpolationFactor); + virtual void updateetk::Transform3D(float int32_terpolationFactor); /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::vec3& scaling); }; -inline void Cone::updateTransform(float int32_terpolationFactor) { - m_transformMatrix = computeTransform(int32_terpolationFactor, mScalingMatrix); +inline void Cone::updateetk::Transform3D(float int32_terpolationFactor) { + m_transformMatrix = computeetk::Transform3D(int32_terpolationFactor, m_scalingMatrix); } #endif diff --git a/tools/testbed/common/ConvexMesh.cpp b/tools/testbed/common/ConvexMesh.cpp index c5185e1..5195049 100644 --- a/tools/testbed/common/ConvexMesh.cpp +++ b/tools/testbed/common/ConvexMesh.cpp @@ -27,7 +27,7 @@ #include // Constructor -ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, +ConvexMesh::ConvexMesh(const openglframework::vec3 &position, reactphysics3d::CollisionWorld* world, const std::string& meshPath) : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), @@ -44,12 +44,12 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, translateWorld(position); // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4::identity(); + m_scalingMatrix = openglframework::Matrix4::identity(); // Vertex and Indices array for the triangle mesh (data in shared and not copied) mPhysicsTriangleVertexArray = - new rp3d::TriangleVertexArray(getNbVertices(), &(m_vertices[0]), sizeof(openglframework::Vector3), + new rp3d::TriangleVertexArray(getNbVertices(), &(m_vertices[0]), sizeof(openglframework::vec3), getNbFaces(0), &(mIndices[0][0]), sizeof(int32_t), rp3d::TriangleVertexArray::VERTEX_FLOAT_TYPE, rp3d::TriangleVertexArray::INDEX_INTEGER_TYPE); @@ -59,26 +59,26 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, mConvexShape = new rp3d::ConvexMeshShape(mPhysicsTriangleVertexArray); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); - mPreviousTransform = transform; + mPreviousetk::Transform3D = transform; // Create a rigid body corresponding to the sphere in the dynamics world m_body = world->createCollisionBody(transform); // Add a collision shape to the body and specify the mass of the collision shape - m_proxyShape = m_body->addCollisionShape(mConvexShape, rp3d::Transform::identity()); + m_proxyShape = m_body->addCollisionShape(mConvexShape, rp3d::etk::Transform3D::identity()); // Create the VBOs and VAO createVBOAndVAO(); - m_transformMatrix = m_transformMatrix * mScalingMatrix; + m_transformMatrix = m_transformMatrix * m_scalingMatrix; } // Constructor -ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass, +ConvexMesh::ConvexMesh(const openglframework::vec3 &position, float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath) : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), @@ -95,11 +95,11 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass, translateWorld(position); // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4::identity(); + m_scalingMatrix = openglframework::Matrix4::identity(); // Vertex and Indices array for the triangle mesh (data in shared and not copied) mPhysicsTriangleVertexArray = - new rp3d::TriangleVertexArray(getNbVertices(), &(m_vertices[0]), sizeof(openglframework::Vector3), + new rp3d::TriangleVertexArray(getNbVertices(), &(m_vertices[0]), sizeof(openglframework::vec3), getNbFaces(0), &(mIndices[0][0]), sizeof(int32_t), rp3d::TriangleVertexArray::VERTEX_FLOAT_TYPE, rp3d::TriangleVertexArray::INDEX_INTEGER_TYPE); @@ -109,22 +109,22 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass, mConvexShape = new rp3d::ConvexMeshShape(mPhysicsTriangleVertexArray); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); // Create a rigid body corresponding to the sphere in the dynamics world rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); // Add a collision shape to the body and specify the mass of the collision shape - m_proxyShape = body->addCollisionShape(mConvexShape, rp3d::Transform::identity(), mass); + m_proxyShape = body->addCollisionShape(mConvexShape, rp3d::etk::Transform3D::identity(), mass); m_body = body; // Create the VBOs and VAO createVBOAndVAO(); - m_transformMatrix = m_transformMatrix * mScalingMatrix; + m_transformMatrix = m_transformMatrix * m_scalingMatrix; } // Destructor @@ -160,7 +160,7 @@ void ConvexMesh::render(openglframework::Shader& shader, const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * m_transformMatrix; const openglframework::Matrix3 normalMatrix = localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + shader.setetk::Matrix3x3Uniform("normalMatrix", normalMatrix, false); // Set the vertex color openglframework::Color currentColor = m_body->isSleeping() ? mSleepingColor : mColor; @@ -209,14 +209,14 @@ void ConvexMesh::createVBOAndVAO() { // Create the VBO for the vertices data mVBOVertices.create(); mVBOVertices.bind(); - size_t sizeVertices = m_vertices.size() * sizeof(openglframework::Vector3); + size_t sizeVertices = m_vertices.size() * sizeof(openglframework::vec3); mVBOVertices.copyDataIntoVBO(sizeVertices, getVerticesPointer(), GL_STATIC_DRAW); mVBOVertices.unbind(); // Create the VBO for the normals data mVBONormals.create(); mVBONormals.bind(); - size_t sizeNormals = mNormals.size() * sizeof(openglframework::Vector3); + size_t sizeNormals = mNormals.size() * sizeof(openglframework::vec3); mVBONormals.copyDataIntoVBO(sizeNormals, getNormalsPointer(), GL_STATIC_DRAW); mVBONormals.unbind(); @@ -224,7 +224,7 @@ void ConvexMesh::createVBOAndVAO() { // Create the VBO for the texture co data mVBOTextureCoords.create(); mVBOTextureCoords.bind(); - size_t sizeTextureCoords = mUVs.size() * sizeof(openglframework::Vector2); + size_t sizeTextureCoords = mUVs.size() * sizeof(openglframework::vec2); mVBOTextureCoords.copyDataIntoVBO(sizeTextureCoords, getUVTextureCoordinatesPointer(), GL_STATIC_DRAW); mVBOTextureCoords.unbind(); } @@ -269,22 +269,22 @@ void ConvexMesh::resetTransform(const rp3d::Transform& transform) { // Reset the velocity of the rigid body rp3d::RigidBody* rigidBody = dynamic_cast(m_body); if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setLinearVelocity(rp3d::vec3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::vec3(0, 0, 0)); } - updateTransform(1.0f); + updateetk::Transform3D(1.0f); } // Set the scaling of the object -void ConvexMesh::setScaling(const openglframework::Vector3& scaling) { +void ConvexMesh::setScaling(const openglframework::vec3& scaling) { // Scale the collision shape - m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); + m_proxyShape->setLocalScaling(rp3d::vec3(scaling.x(), scaling.y(), scaling.z())); // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(scaling.x, 0, 0, 0, - 0, scaling.y, 0, 0, - 0, 0, scaling.z, 0, + m_scalingMatrix = openglframework::Matrix4(scaling.x(), 0, 0, 0, + 0, scaling.y(), 0, 0, + 0, 0, scaling.z(), 0, 0, 0, 0, 1); } diff --git a/tools/testbed/common/ConvexMesh.h b/tools/testbed/common/ConvexMesh.h index 220bfc5..2c9e3e1 100644 --- a/tools/testbed/common/ConvexMesh.h +++ b/tools/testbed/common/ConvexMesh.h @@ -39,7 +39,7 @@ class ConvexMesh : public openglframework::Mesh, public PhysicsObject { // -------------------- Attributes -------------------- // /// Previous transform (for int32_terpolation) - rp3d::Transform mPreviousTransform; + rp3d::etk::Transform3D mPreviousTransform; rp3d::TriangleVertexArray* mPhysicsTriangleVertexArray; @@ -48,7 +48,7 @@ class ConvexMesh : public openglframework::Mesh, public PhysicsObject { rp3d::ProxyShape* m_proxyShape; /// Scaling matrix - openglframework::Matrix4 mScalingMatrix; + openglframework::Matrix4 m_scalingMatrix; /// Vertex Buffer Object for the vertices data openglframework::VertexBufferObject mVBOVertices; @@ -75,11 +75,11 @@ class ConvexMesh : public openglframework::Mesh, public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - ConvexMesh(const openglframework::Vector3& position, + ConvexMesh(const openglframework::vec3& position, rp3d::CollisionWorld* world, const std::string& meshPath); /// Constructor - ConvexMesh(const openglframework::Vector3& position, float mass, + ConvexMesh(const openglframework::vec3& position, float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath); /// Destructor @@ -93,15 +93,15 @@ class ConvexMesh : public openglframework::Mesh, public PhysicsObject { void resetTransform(const rp3d::Transform& transform); /// Update the transform matrix of the object - virtual void updateTransform(float int32_terpolationFactor); + virtual void updateetk::Transform3D(float int32_terpolationFactor); /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::vec3& scaling); }; // Update the transform matrix of the object -inline void ConvexMesh::updateTransform(float int32_terpolationFactor) { - m_transformMatrix = computeTransform(int32_terpolationFactor, mScalingMatrix); +inline void ConvexMesh::updateetk::Transform3D(float int32_terpolationFactor) { + m_transformMatrix = computeetk::Transform3D(int32_terpolationFactor, m_scalingMatrix); } #endif diff --git a/tools/testbed/common/Cylinder.cpp b/tools/testbed/common/Cylinder.cpp index cfd62f6..26e97f9 100644 --- a/tools/testbed/common/Cylinder.cpp +++ b/tools/testbed/common/Cylinder.cpp @@ -34,7 +34,7 @@ openglframework::VertexArrayObject Cylinder::mVAO; int32_t Cylinder::totalNbCylinders = 0; // Constructor -Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position, +Cylinder::Cylinder(float radius, float height, const openglframework::vec3& position, reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) : openglframework::Mesh(), mRadius(radius), mHeight(height) { @@ -46,7 +46,7 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& p calculateNormals(); // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, + m_scalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, 0, mHeight, 0, 0, 0, 0, mRadius, 0, 0, 0, 0, 1); @@ -59,19 +59,19 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& p mCylinderShape = new rp3d::CylinderShape(mRadius, mHeight); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); - mPreviousTransform = transform; + mPreviousetk::Transform3D = transform; // Create a rigid body corresponding to the cylinder in the dynamics world m_body = world->createCollisionBody(transform); // Add a collision shape to the body and specify the mass of the shape - m_proxyShape = m_body->addCollisionShape(mCylinderShape, rp3d::Transform::identity()); + m_proxyShape = m_body->addCollisionShape(mCylinderShape, rp3d::etk::Transform3D::identity()); - m_transformMatrix = m_transformMatrix * mScalingMatrix; + m_transformMatrix = m_transformMatrix * m_scalingMatrix; // Create the VBOs and VAO if (totalNbCylinders == 0) { @@ -82,7 +82,7 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& p } // Constructor -Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position, +Cylinder::Cylinder(float radius, float height, const openglframework::vec3& position, float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) : openglframework::Mesh(), mRadius(radius), mHeight(height) { @@ -94,7 +94,7 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& p calculateNormals(); // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, + m_scalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, 0, mHeight, 0, 0, 0, 0, mRadius, 0, 0, 0, 0, 1); @@ -107,17 +107,17 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& p mCylinderShape = new rp3d::CylinderShape(mRadius, mHeight); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); // Create a rigid body corresponding to the cylinder in the dynamics world rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); // Add a collision shape to the body and specify the mass of the shape - m_proxyShape = body->addCollisionShape(mCylinderShape, rp3d::Transform::identity(), mass); + m_proxyShape = body->addCollisionShape(mCylinderShape, rp3d::etk::Transform3D::identity(), mass); - m_transformMatrix = m_transformMatrix * mScalingMatrix; + m_transformMatrix = m_transformMatrix * m_scalingMatrix; m_body = body; @@ -164,7 +164,7 @@ void Cylinder::render(openglframework::Shader& shader, const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * m_transformMatrix; const openglframework::Matrix3 normalMatrix = localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + shader.setetk::Matrix3x3Uniform("normalMatrix", normalMatrix, false); // Set the vertex color openglframework::Color currentColor = m_body->isSleeping() ? mSleepingColor : mColor; @@ -213,14 +213,14 @@ void Cylinder::createVBOAndVAO() { // Create the VBO for the vertices data mVBOVertices.create(); mVBOVertices.bind(); - size_t sizeVertices = m_vertices.size() * sizeof(openglframework::Vector3); + size_t sizeVertices = m_vertices.size() * sizeof(openglframework::vec3); mVBOVertices.copyDataIntoVBO(sizeVertices, getVerticesPointer(), GL_STATIC_DRAW); mVBOVertices.unbind(); // Create the VBO for the normals data mVBONormals.create(); mVBONormals.bind(); - size_t sizeNormals = mNormals.size() * sizeof(openglframework::Vector3); + size_t sizeNormals = mNormals.size() * sizeof(openglframework::vec3); mVBONormals.copyDataIntoVBO(sizeNormals, getNormalsPointer(), GL_STATIC_DRAW); mVBONormals.unbind(); @@ -228,7 +228,7 @@ void Cylinder::createVBOAndVAO() { // Create the VBO for the texture co data mVBOTextureCoords.create(); mVBOTextureCoords.bind(); - size_t sizeTextureCoords = mUVs.size() * sizeof(openglframework::Vector2); + size_t sizeTextureCoords = mUVs.size() * sizeof(openglframework::vec2); mVBOTextureCoords.copyDataIntoVBO(sizeTextureCoords, getUVTextureCoordinatesPointer(), GL_STATIC_DRAW); mVBOTextureCoords.unbind(); } @@ -273,22 +273,22 @@ void Cylinder::resetTransform(const rp3d::Transform& transform) { // Reset the velocity of the rigid body rp3d::RigidBody* rigidBody = dynamic_cast(m_body); if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setLinearVelocity(rp3d::vec3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::vec3(0, 0, 0)); } - updateTransform(1.0f); + updateetk::Transform3D(1.0f); } // Set the scaling of the object -void Cylinder::setScaling(const openglframework::Vector3& scaling) { +void Cylinder::setScaling(const openglframework::vec3& scaling) { // Scale the collision shape - m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); + m_proxyShape->setLocalScaling(rp3d::vec3(scaling.x(), scaling.y(), scaling.z())); // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0, - 0, mHeight * scaling.y, 0, 0, - 0, 0, mRadius * scaling.z, 0, + m_scalingMatrix = openglframework::Matrix4(mRadius * scaling.x(), 0, 0, 0, + 0, mHeight * scaling.y(), 0, 0, + 0, 0, mRadius * scaling.z(), 0, 0, 0, 0, 1); } diff --git a/tools/testbed/common/Cylinder.h b/tools/testbed/common/Cylinder.h index 1aa75a1..ca2e527 100644 --- a/tools/testbed/common/Cylinder.h +++ b/tools/testbed/common/Cylinder.h @@ -45,10 +45,10 @@ class Cylinder : public openglframework::Mesh, public PhysicsObject { float mHeight; /// Scaling matrix (applied to a sphere to obtain the correct cylinder dimensions) - openglframework::Matrix4 mScalingMatrix; + openglframework::Matrix4 m_scalingMatrix; /// Previous transform (for int32_terpolation) - rp3d::Transform mPreviousTransform; + rp3d::etk::Transform3D mPreviousTransform; /// Collision shape rp3d::CylinderShape* mCylinderShape; @@ -82,11 +82,11 @@ class Cylinder : public openglframework::Mesh, public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - Cylinder(float radius, float height, const openglframework::Vector3& position, + Cylinder(float radius, float height, const openglframework::vec3& position, rp3d::CollisionWorld* world, const std::string &meshFolderPath); /// Constructor - Cylinder(float radius, float height, const openglframework::Vector3& position, + Cylinder(float radius, float height, const openglframework::vec3& position, float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string &meshFolderPath); /// Destructor @@ -100,15 +100,15 @@ class Cylinder : public openglframework::Mesh, public PhysicsObject { void resetTransform(const rp3d::Transform& transform); /// Update the transform matrix of the object - virtual void updateTransform(float int32_terpolationFactor); + virtual void updateetk::Transform3D(float int32_terpolationFactor); /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::vec3& scaling); }; // Update the transform matrix of the object -inline void Cylinder::updateTransform(float int32_terpolationFactor) { - m_transformMatrix = computeTransform(int32_terpolationFactor, mScalingMatrix); +inline void Cylinder::updateetk::Transform3D(float int32_terpolationFactor) { + m_transformMatrix = computeetk::Transform3D(int32_terpolationFactor, m_scalingMatrix); } #endif diff --git a/tools/testbed/common/Dumbbell.cpp b/tools/testbed/common/Dumbbell.cpp index d275da9..c30b9d8 100644 --- a/tools/testbed/common/Dumbbell.cpp +++ b/tools/testbed/common/Dumbbell.cpp @@ -34,7 +34,7 @@ openglframework::VertexArrayObject Dumbbell::mVAO; int32_t Dumbbell::totalNbDumbbells = 0; // Constructor -Dumbbell::Dumbbell(const openglframework::Vector3 &position, +Dumbbell::Dumbbell(const openglframework::vec3 &position, reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) : openglframework::Mesh() { @@ -45,7 +45,7 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, calculateNormals(); // Identity scaling matrix - mScalingMatrix.setToIdentity(); + m_scalingMatrix.setToIdentity(); mDistanceBetweenSphere = 8.0f; @@ -62,27 +62,27 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, // Create a cylinder collision shape for the middle of the dumbbell // ReactPhysics3D will clone this object to create an int32_ternal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() - const rp3d::float radiusCylinder = rp3d::float(0.5); + const rp3d::float radiusCylinder = rp3d::0.5f; const rp3d::float heightCylinder = rp3d::float(8.0); - const rp3d::float massCylinder = rp3d::float(1.0); + const rp3d::float massCylinder = rp3d::1.0f; mCylinderShape = new rp3d::CylinderShape(radiusCylinder, heightCylinder); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); rp3d::float angleAroundX = 0;//rp3d::PI / 2; - rp3d::Quaternion initOrientation(angleAroundX, 0, 0); - rp3d::Transform transform_body(initPosition, initOrientation); + rp3d::etk::Quaternion initOrientation(angleAroundX, 0, 0); + rp3d::etk::Transform3D transform_body(initPosition, initOrientation); - mPreviousTransform = transform_body; + mPreviousetk::Transform3D = transform_body; // Initial transform of the first sphere collision shape of the dumbbell (in local-space) - rp3d::Transform transformSphereShape1(rp3d::Vector3(0, mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); + rp3d::etk::Transform3D transformSphereShape1(rp3d::vec3(0, mDistanceBetweenSphere / 2.0f, 0), rp3d::etk::Quaternion::identity()); // Initial transform of the second sphere collision shape of the dumbell (in local-space) - rp3d::Transform transformSphereShape2(rp3d::Vector3(0, -mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); + rp3d::etk::Transform3D transformSphereShape2(rp3d::vec3(0, -mDistanceBetweenSphere / 2.0f, 0), rp3d::etk::Quaternion::identity()); // Initial transform of the cylinder collision shape of the dumbell (in local-space) - rp3d::Transform transformCylinderShape(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()); + rp3d::etk::Transform3D transformCylinderShape(rp3d::vec3(0, 0, 0), rp3d::etk::Quaternion::identity()); // Create a rigid body corresponding to the dumbbell in the dynamics world rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform_body); @@ -94,7 +94,7 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, m_body = body; - m_transformMatrix = m_transformMatrix * mScalingMatrix; + m_transformMatrix = m_transformMatrix * m_scalingMatrix; // Create the VBOs and VAO if (totalNbDumbbells == 0) { @@ -105,7 +105,7 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, } // Constructor -Dumbbell::Dumbbell(const openglframework::Vector3 &position, +Dumbbell::Dumbbell(const openglframework::vec3 &position, reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) : openglframework::Mesh() { @@ -116,7 +116,7 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, calculateNormals(); // Identity scaling matrix - mScalingMatrix.setToIdentity(); + m_scalingMatrix.setToIdentity(); mDistanceBetweenSphere = 8.0f; @@ -132,24 +132,24 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, // Create a cylinder collision shape for the middle of the dumbbell // ReactPhysics3D will clone this object to create an int32_ternal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() - const rp3d::float radiusCylinder = rp3d::float(0.5); + const rp3d::float radiusCylinder = rp3d::0.5f; const rp3d::float heightCylinder = rp3d::float(8.0); mCylinderShape = new rp3d::CylinderShape(radiusCylinder, heightCylinder); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); rp3d::float angleAroundX = 0;//rp3d::PI / 2; - rp3d::Quaternion initOrientation(angleAroundX, 0, 0); - rp3d::Transform transform_body(initPosition, initOrientation); + rp3d::etk::Quaternion initOrientation(angleAroundX, 0, 0); + rp3d::etk::Transform3D transform_body(initPosition, initOrientation); // Initial transform of the first sphere collision shape of the dumbbell (in local-space) - rp3d::Transform transformSphereShape1(rp3d::Vector3(0, mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); + rp3d::etk::Transform3D transformSphereShape1(rp3d::vec3(0, mDistanceBetweenSphere / 2.0f, 0), rp3d::etk::Quaternion::identity()); // Initial transform of the second sphere collision shape of the dumbell (in local-space) - rp3d::Transform transformSphereShape2(rp3d::Vector3(0, -mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); + rp3d::etk::Transform3D transformSphereShape2(rp3d::vec3(0, -mDistanceBetweenSphere / 2.0f, 0), rp3d::etk::Quaternion::identity()); // Initial transform of the cylinder collision shape of the dumbell (in local-space) - rp3d::Transform transformCylinderShape(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()); + rp3d::etk::Transform3D transformCylinderShape(rp3d::vec3(0, 0, 0), rp3d::etk::Quaternion::identity()); // Create a rigid body corresponding to the dumbbell in the dynamics world m_body = world->createCollisionBody(transform_body); @@ -159,7 +159,7 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, m_proxyShapeSphere2 = m_body->addCollisionShape(mSphereShape, transformSphereShape2); m_proxyShapeCylinder = m_body->addCollisionShape(mCylinderShape, transformCylinderShape); - m_transformMatrix = m_transformMatrix * mScalingMatrix; + m_transformMatrix = m_transformMatrix * m_scalingMatrix; // Create the VBOs and VAO if (totalNbDumbbells == 0) { @@ -205,7 +205,7 @@ void Dumbbell::render(openglframework::Shader& shader, const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * m_transformMatrix; const openglframework::Matrix3 normalMatrix = localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + shader.setetk::Matrix3x3Uniform("normalMatrix", normalMatrix, false); // Set the vertex color openglframework::Color currentColor = m_body->isSleeping() ? mSleepingColor : mColor; @@ -254,14 +254,14 @@ void Dumbbell::createVBOAndVAO() { // Create the VBO for the vertices data mVBOVertices.create(); mVBOVertices.bind(); - size_t sizeVertices = m_vertices.size() * sizeof(openglframework::Vector3); + size_t sizeVertices = m_vertices.size() * sizeof(openglframework::vec3); mVBOVertices.copyDataIntoVBO(sizeVertices, getVerticesPointer(), GL_STATIC_DRAW); mVBOVertices.unbind(); // Create the VBO for the normals data mVBONormals.create(); mVBONormals.bind(); - size_t sizeNormals = mNormals.size() * sizeof(openglframework::Vector3); + size_t sizeNormals = mNormals.size() * sizeof(openglframework::vec3); mVBONormals.copyDataIntoVBO(sizeNormals, getNormalsPointer(), GL_STATIC_DRAW); mVBONormals.unbind(); @@ -269,7 +269,7 @@ void Dumbbell::createVBOAndVAO() { // Create the VBO for the texture co data mVBOTextureCoords.create(); mVBOTextureCoords.bind(); - size_t sizeTextureCoords = mUVs.size() * sizeof(openglframework::Vector2); + size_t sizeTextureCoords = mUVs.size() * sizeof(openglframework::vec2); mVBOTextureCoords.copyDataIntoVBO(sizeTextureCoords, getUVTextureCoordinatesPointer(), GL_STATIC_DRAW); mVBOTextureCoords.unbind(); } @@ -314,36 +314,36 @@ void Dumbbell::resetTransform(const rp3d::Transform& transform) { // Reset the velocity of the rigid body rp3d::RigidBody* rigidBody = dynamic_cast(m_body); if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setLinearVelocity(rp3d::vec3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::vec3(0, 0, 0)); } - updateTransform(1.0f); + updateetk::Transform3D(1.0f); } // Set the scaling of the object -void Dumbbell::setScaling(const openglframework::Vector3& scaling) { +void Dumbbell::setScaling(const openglframework::vec3& scaling) { // Scale the collision shape - rp3d::Vector3 newScaling(scaling.x, scaling.y, scaling.z); + rp3d::vec3 newScaling(scaling.x(), scaling.y(), scaling.z()); m_proxyShapeCylinder->setLocalScaling(newScaling); m_proxyShapeSphere1->setLocalScaling(newScaling); m_proxyShapeSphere2->setLocalScaling(newScaling); - mDistanceBetweenSphere = (mDistanceBetweenSphere / mScalingMatrix.getValue(1, 1)) * scaling.y; + mDistanceBetweenSphere = (mDistanceBetweenSphere / m_scalingMatrix.getValue(1, 1)) * scaling.y(); // Initial transform of the first sphere collision shape of the dumbbell (in local-space) - rp3d::Transform transformSphereShape1(rp3d::Vector3(0, mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); + rp3d::etk::Transform3D transformSphereShape1(rp3d::vec3(0, mDistanceBetweenSphere / 2.0f, 0), rp3d::etk::Quaternion::identity()); // Initial transform of the second sphere collision shape of the dumbell (in local-space) - rp3d::Transform transformSphereShape2(rp3d::Vector3(0, -mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); + rp3d::etk::Transform3D transformSphereShape2(rp3d::vec3(0, -mDistanceBetweenSphere / 2.0f, 0), rp3d::etk::Quaternion::identity()); m_proxyShapeSphere1->setLocalToBodyTransform(transformSphereShape1); m_proxyShapeSphere2->setLocalToBodyTransform(transformSphereShape2); // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(scaling.x, 0, 0, 0, - 0, scaling.y, 0, 0, - 0, 0, scaling.z, 0, + m_scalingMatrix = openglframework::Matrix4(scaling.x(), 0, 0, 0, + 0, scaling.y(), 0, 0, + 0, 0, scaling.z(), 0, 0, 0, 0, 1); } diff --git a/tools/testbed/common/Dumbbell.h b/tools/testbed/common/Dumbbell.h index caf539b..172ce96 100644 --- a/tools/testbed/common/Dumbbell.h +++ b/tools/testbed/common/Dumbbell.h @@ -49,10 +49,10 @@ class Dumbbell : public openglframework::Mesh, public PhysicsObject { rp3d::ProxyShape* m_proxyShapeSphere2; /// Scaling matrix (applied to a sphere to obtain the correct sphere dimensions) - openglframework::Matrix4 mScalingMatrix; + openglframework::Matrix4 m_scalingMatrix; /// Previous transform (for int32_terpolation) - rp3d::Transform mPreviousTransform; + rp3d::etk::Transform3D mPreviousTransform; /// Vertex Buffer Object for the vertices data static openglframework::VertexBufferObject mVBOVertices; @@ -82,11 +82,11 @@ class Dumbbell : public openglframework::Mesh, public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - Dumbbell(const openglframework::Vector3& position, rp3d::DynamicsWorld* dynamicsWorld, + Dumbbell(const openglframework::vec3& position, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath); /// Constructor - Dumbbell(const openglframework::Vector3& position, rp3d::CollisionWorld* world, + Dumbbell(const openglframework::vec3& position, rp3d::CollisionWorld* world, const std::string& meshFolderPath); @@ -101,15 +101,15 @@ class Dumbbell : public openglframework::Mesh, public PhysicsObject { void resetTransform(const rp3d::Transform& transform); /// Update the transform matrix of the object - virtual void updateTransform(float int32_terpolationFactor); + virtual void updateetk::Transform3D(float int32_terpolationFactor); /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::vec3& scaling); }; // Update the transform matrix of the object -inline void Dumbbell::updateTransform(float int32_terpolationFactor) { - m_transformMatrix = computeTransform(int32_terpolationFactor, mScalingMatrix); +inline void Dumbbell::updateetk::Transform3D(float int32_terpolationFactor) { + m_transformMatrix = computeetk::Transform3D(int32_terpolationFactor, m_scalingMatrix); } #endif diff --git a/tools/testbed/common/HeightField.cpp b/tools/testbed/common/HeightField.cpp index bdc930b..3c3dd9e 100644 --- a/tools/testbed/common/HeightField.cpp +++ b/tools/testbed/common/HeightField.cpp @@ -28,7 +28,7 @@ #include // Constructor -HeightField::HeightField(const openglframework::Vector3 &position, +HeightField::HeightField(const openglframework::vec3 &position, reactphysics3d::CollisionWorld* world) : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), @@ -38,7 +38,7 @@ HeightField::HeightField(const openglframework::Vector3 &position, translateWorld(position); // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4::identity(); + m_scalingMatrix = openglframework::Matrix4::identity(); // Generate the height field generateHeightField(); @@ -48,30 +48,30 @@ HeightField::HeightField(const openglframework::Vector3 &position, // Create the collision shape for the rigid body (convex mesh shape) and // do not forget to delete it at the end - mHeightFieldShape = new rp3d::HeightFieldShape(NB_POINTS_WIDTH, NB_POINTS_LENGTH, mMinHeight, mMaxHeight, + mHeightFieldShape = new rp3d::HeightFieldShape(NB_POINTS_WIDTH, NB_POINTS_LENGTH, m_minHeight, m_maxHeight, mHeightData, rp3d::HeightFieldShape::HEIGHT_FLOAT_TYPE); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); - mPreviousTransform = transform; + mPreviousetk::Transform3D = transform; // Create a rigid body corresponding to the sphere in the dynamics world m_body = world->createCollisionBody(transform); // Add a collision shape to the body and specify the mass of the collision shape - m_proxyShape = m_body->addCollisionShape(mHeightFieldShape, rp3d::Transform::identity()); + m_proxyShape = m_body->addCollisionShape(mHeightFieldShape, rp3d::etk::Transform3D::identity()); // Create the VBOs and VAO createVBOAndVAO(); - m_transformMatrix = m_transformMatrix * mScalingMatrix; + m_transformMatrix = m_transformMatrix * m_scalingMatrix; } // Constructor -HeightField::HeightField(const openglframework::Vector3 &position, float mass, +HeightField::HeightField(const openglframework::vec3 &position, float mass, reactphysics3d::DynamicsWorld* dynamicsWorld) : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), @@ -81,7 +81,7 @@ HeightField::HeightField(const openglframework::Vector3 &position, float mass, translateWorld(position); // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4::identity(); + m_scalingMatrix = openglframework::Matrix4::identity(); // Generate the height field generateHeightField(); @@ -91,26 +91,26 @@ HeightField::HeightField(const openglframework::Vector3 &position, float mass, // Create the collision shape for the rigid body (convex mesh shape) and // do not forget to delete it at the end - mHeightFieldShape = new rp3d::HeightFieldShape(NB_POINTS_WIDTH, NB_POINTS_LENGTH, mMinHeight, mMaxHeight, + mHeightFieldShape = new rp3d::HeightFieldShape(NB_POINTS_WIDTH, NB_POINTS_LENGTH, m_minHeight, m_maxHeight, mHeightData, rp3d::HeightFieldShape::HEIGHT_FLOAT_TYPE); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); // Create a rigid body corresponding to the sphere in the dynamics world rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); // Add a collision shape to the body and specify the mass of the collision shape - m_proxyShape = body->addCollisionShape(mHeightFieldShape, rp3d::Transform::identity(), mass); + m_proxyShape = body->addCollisionShape(mHeightFieldShape, rp3d::etk::Transform3D::identity(), mass); m_body = body; // Create the VBOs and VAO createVBOAndVAO(); - m_transformMatrix = m_transformMatrix * mScalingMatrix; + m_transformMatrix = m_transformMatrix * m_scalingMatrix; } // Destructor @@ -145,7 +145,7 @@ void HeightField::render(openglframework::Shader& shader, const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * m_transformMatrix; const openglframework::Matrix3 normalMatrix = localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + shader.setetk::Matrix3x3Uniform("normalMatrix", normalMatrix, false); // Set the vertex color openglframework::Color currentColor = m_body->isSleeping() ? mSleepingColor : mColor; @@ -197,8 +197,8 @@ void HeightField::generateHeightField() { int32_t randomseed = 23; PerlinNoise perlinNoise(persistence, frequency, amplitude, octaves, randomseed); - mMinHeight = 0; - mMaxHeight = 0; + m_minHeight = 0; + m_maxHeight = 0; float width = (NB_POINTS_WIDTH - 1); float length = (NB_POINTS_LENGTH - 1); @@ -211,12 +211,12 @@ void HeightField::generateHeightField() { mHeightData[arrayIndex] = (float)(perlinNoise.GetHeight(-width * 0.5 + i, -length * 0.5 + j)); if (i==0 && j==0) { - mMinHeight = mHeightData[arrayIndex] ; - mMaxHeight = mHeightData[arrayIndex] ; + m_minHeight = mHeightData[arrayIndex] ; + m_maxHeight = mHeightData[arrayIndex] ; } - if (mHeightData[arrayIndex] > mMaxHeight) mMaxHeight = mHeightData[arrayIndex] ; - if (mHeightData[arrayIndex] < mMinHeight) mMinHeight = mHeightData[arrayIndex] ; + if (mHeightData[arrayIndex] > m_maxHeight) m_maxHeight = mHeightData[arrayIndex] ; + if (mHeightData[arrayIndex] < m_minHeight) m_minHeight = mHeightData[arrayIndex] ; } } } @@ -230,9 +230,9 @@ void HeightField::generateGraphicsMesh() { for (int32_t i=0; i(m_body); if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setLinearVelocity(rp3d::vec3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::vec3(0, 0, 0)); } - updateTransform(1.0f); + updateetk::Transform3D(1.0f); } // Set the scaling of the object -void HeightField::setScaling(const openglframework::Vector3& scaling) { +void HeightField::setScaling(const openglframework::vec3& scaling) { // Scale the collision shape - m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); + m_proxyShape->setLocalScaling(rp3d::vec3(scaling.x(), scaling.y(), scaling.z())); // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(scaling.x, 0, 0, 0, - 0, scaling.y, 0,0, - 0, 0, scaling.z, 0, + m_scalingMatrix = openglframework::Matrix4(scaling.x(), 0, 0, 0, + 0, scaling.y(), 0,0, + 0, 0, scaling.z(), 0, 0, 0, 0, 1.0f); } diff --git a/tools/testbed/common/HeightField.h b/tools/testbed/common/HeightField.h index aad9751..eaed251 100644 --- a/tools/testbed/common/HeightField.h +++ b/tools/testbed/common/HeightField.h @@ -46,14 +46,14 @@ class HeightField : public openglframework::Mesh, public PhysicsObject { float mHeightData[NB_POINTS_WIDTH * NB_POINTS_LENGTH]; /// Previous transform (for int32_terpolation) - rp3d::Transform mPreviousTransform; + rp3d::etk::Transform3D mPreviousTransform; /// Collision shape rp3d::HeightFieldShape* mHeightFieldShape; rp3d::ProxyShape* m_proxyShape; /// Scaling matrix - openglframework::Matrix4 mScalingMatrix; + openglframework::Matrix4 m_scalingMatrix; /// Vertex Buffer Object for the vertices data openglframework::VertexBufferObject mVBOVertices; @@ -71,7 +71,7 @@ class HeightField : public openglframework::Mesh, public PhysicsObject { openglframework::VertexArrayObject mVAO; /// Min/Max height of the height field values - float mMinHeight, mMaxHeight; + float m_minHeight, m_maxHeight; // -------------------- Methods -------------------- // @@ -89,11 +89,11 @@ class HeightField : public openglframework::Mesh, public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - HeightField(const openglframework::Vector3& position, + HeightField(const openglframework::vec3& position, rp3d::CollisionWorld* world); /// Constructor - HeightField(const openglframework::Vector3& position, float mass, + HeightField(const openglframework::vec3& position, float mass, rp3d::DynamicsWorld* dynamicsWorld); /// Destructor @@ -107,15 +107,15 @@ class HeightField : public openglframework::Mesh, public PhysicsObject { void resetTransform(const rp3d::Transform& transform); /// Update the transform matrix of the object - virtual void updateTransform(float int32_terpolationFactor); + virtual void updateetk::Transform3D(float int32_terpolationFactor); /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::vec3& scaling); }; // Update the transform matrix of the object -inline void HeightField::updateTransform(float int32_terpolationFactor) { - m_transformMatrix = computeTransform(int32_terpolationFactor, mScalingMatrix); +inline void HeightField::updateetk::Transform3D(float int32_terpolationFactor) { + m_transformMatrix = computeetk::Transform3D(int32_terpolationFactor, m_scalingMatrix); } #endif diff --git a/tools/testbed/common/Line.cpp b/tools/testbed/common/Line.cpp index 7ba9585..be0a3bc 100644 --- a/tools/testbed/common/Line.cpp +++ b/tools/testbed/common/Line.cpp @@ -27,8 +27,8 @@ #include // Constructor -Line::Line(const openglframework::Vector3& worldPoint1, - const openglframework::Vector3& worldPoint2) +Line::Line(const openglframework::vec3& worldPoint1, + const openglframework::vec3& worldPoint2) : m_worldPoint1(worldPoint1), m_worldPoint2(worldPoint2) { } @@ -56,8 +56,8 @@ void Line::render(openglframework::Shader& shader, /* glBegin(GL_LINES); - glVertex3f(m_worldPoint1.x, m_worldPoint1.y, m_worldPoint1.z); - glVertex3f(m_worldPoint2.x, m_worldPoint2.y, m_worldPoint2.z); + glVertex3f(m_worldPoint1.x(), m_worldPoint1.y(), m_worldPoint1.z()); + glVertex3f(m_worldPoint2.x(), m_worldPoint2.y(), m_worldPoint2.z()); glEnd(); */ diff --git a/tools/testbed/common/Line.h b/tools/testbed/common/Line.h index f455437..ccfc213 100644 --- a/tools/testbed/common/Line.h +++ b/tools/testbed/common/Line.h @@ -37,7 +37,7 @@ class Line : public openglframework::Object3D { // -------------------- Attributes -------------------- // - openglframework::Vector3 m_worldPoint1, m_worldPoint2; + openglframework::vec3 m_worldPoint1, m_worldPoint2; // -------------------- Methods -------------------- // @@ -46,17 +46,17 @@ class Line : public openglframework::Object3D { // -------------------- Methods -------------------- // /// Constructor - Line(const openglframework::Vector3& worldPoint1, - const openglframework::Vector3& worldPoint2); + Line(const openglframework::vec3& worldPoint1, + const openglframework::vec3& worldPoint2); /// Destructor ~Line(); /// Return the first point of the line - openglframework::Vector3 getPoint1() const; + openglframework::vec3 getPoint1() const; /// Return the second point of the line - openglframework::Vector3 getPoint2() const; + openglframework::vec3 getPoint2() const; /// Render the line at the correct position and with the correct orientation void render(openglframework::Shader& shader, @@ -64,12 +64,12 @@ class Line : public openglframework::Object3D { }; // Return the first point of the line -inline openglframework::Vector3 Line::getPoint1() const { +inline openglframework::vec3 Line::getPoint1() const { return m_worldPoint1; } // Return the second point of the line -inline openglframework::Vector3 Line::getPoint2() const { +inline openglframework::vec3 Line::getPoint2() const { return m_worldPoint2; } diff --git a/tools/testbed/common/PhysicsObject.cpp b/tools/testbed/common/PhysicsObject.cpp index 8765977..fe47351 100644 --- a/tools/testbed/common/PhysicsObject.cpp +++ b/tools/testbed/common/PhysicsObject.cpp @@ -34,21 +34,21 @@ PhysicsObject::PhysicsObject() { } // Compute the new transform matrix -openglframework::Matrix4 PhysicsObject::computeTransform(float int32_terpolationFactor, +openglframework::Matrix4 PhysicsObject::computeetk::Transform3D(float int32_terpolationFactor, const openglframework::Matrix4& scalingMatrix) { // Get the transform of the rigid body - rp3d::Transform transform = m_body->getTransform(); + rp3d::etk::Transform3D transform = m_body->getTransform(); // Interpolate the transform between the previous one and the new one - rp3d::Transform int32_terpolatedTransform = rp3d::Transform::int32_terpolateTransforms(mPreviousTransform, + rp3d::etk::Transform3D int32_terpolatedTransform = rp3d::Transform::int32_terpolateTransforms(mPreviousTransform, transform, int32_terpolationFactor); - mPreviousTransform = transform; + mPreviousetk::Transform3D = transform; // Compute the transform used for rendering the box rp3d::float matrix[16]; - int32_terpolatedTransform.getOpenGLMatrix(matrix); + int32_terpolatedetk::Transform3D.getOpenGLMatrix(matrix); openglframework::Matrix4 newMatrix(matrix[0], matrix[4], matrix[8], matrix[12], matrix[1], matrix[5], matrix[9], matrix[13], matrix[2], matrix[6], matrix[10], matrix[14], diff --git a/tools/testbed/common/PhysicsObject.h b/tools/testbed/common/PhysicsObject.h index f73accd..3cac18e 100644 --- a/tools/testbed/common/PhysicsObject.h +++ b/tools/testbed/common/PhysicsObject.h @@ -39,7 +39,7 @@ class PhysicsObject { rp3d::CollisionBody* m_body; /// Previous transform of the body (for int32_terpolation) - rp3d::Transform mPreviousTransform; + rp3d::etk::Transform3D mPreviousTransform; /// Main color of the box openglframework::Color mColor; @@ -48,7 +48,7 @@ class PhysicsObject { openglframework::Color mSleepingColor; // Compute the new transform matrix - openglframework::Matrix4 computeTransform(float int32_terpolationFactor, + openglframework::Matrix4 computeetk::Transform3D(float int32_terpolationFactor, const openglframework::Matrix4 &scalingMatrix); public: @@ -57,7 +57,7 @@ class PhysicsObject { PhysicsObject(); /// Update the transform matrix of the object - virtual void updateTransform(float int32_terpolationFactor)=0; + virtual void updateetk::Transform3D(float int32_terpolationFactor)=0; /// Set the color of the box void setColor(const openglframework::Color& color); @@ -72,7 +72,7 @@ class PhysicsObject { reactphysics3d::RigidBody* getRigidBody(); /// Set the scaling of the object - virtual void setScaling(const openglframework::Vector3& scaling)=0; + virtual void setScaling(const openglframework::vec3& scaling)=0; }; // Set the color of the box diff --git a/tools/testbed/common/Sphere.cpp b/tools/testbed/common/Sphere.cpp index 68247e0..818b2b2 100644 --- a/tools/testbed/common/Sphere.cpp +++ b/tools/testbed/common/Sphere.cpp @@ -34,7 +34,7 @@ openglframework::VertexArrayObject Sphere::mVAO; int32_t Sphere::totalNbSpheres = 0; // Constructor -Sphere::Sphere(float radius, const openglframework::Vector3 &position, +Sphere::Sphere(float radius, const openglframework::vec3 &position, reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) : openglframework::Mesh(), mRadius(radius) { @@ -46,7 +46,7 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position, calculateNormals(); // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, + m_scalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, 0, mRadius, 0, 0, 0, 0, mRadius, 0, 0, 0, 0, 1); @@ -60,19 +60,19 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position, m_collisionShape = new rp3d::SphereShape(mRadius); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); - mPreviousTransform = transform; + mPreviousetk::Transform3D = transform; // Create a rigid body corresponding to the sphere in the dynamics world m_body = world->createCollisionBody(transform); // Add a collision shape to the body and specify the mass of the shape - m_proxyShape = m_body->addCollisionShape(m_collisionShape, rp3d::Transform::identity()); + m_proxyShape = m_body->addCollisionShape(m_collisionShape, rp3d::etk::Transform3D::identity()); - m_transformMatrix = m_transformMatrix * mScalingMatrix; + m_transformMatrix = m_transformMatrix * m_scalingMatrix; // Create the VBOs and VAO if (totalNbSpheres == 0) { @@ -83,7 +83,7 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position, } // Constructor -Sphere::Sphere(float radius, const openglframework::Vector3 &position, +Sphere::Sphere(float radius, const openglframework::vec3 &position, float mass, reactphysics3d::DynamicsWorld* world, const std::string& meshFolderPath) : openglframework::Mesh(), mRadius(radius) { @@ -95,7 +95,7 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position, calculateNormals(); // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, + m_scalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, 0, mRadius, 0, 0, 0, 0, mRadius, 0, 0, 0, 0, 1); @@ -109,19 +109,19 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position, m_collisionShape = new rp3d::SphereShape(mRadius); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); // Create a rigid body corresponding to the sphere in the dynamics world rp3d::RigidBody* body = world->createRigidBody(transform); // Add a collision shape to the body and specify the mass of the shape - m_proxyShape = body->addCollisionShape(m_collisionShape, rp3d::Transform::identity(), mass); + m_proxyShape = body->addCollisionShape(m_collisionShape, rp3d::etk::Transform3D::identity(), mass); m_body = body; - m_transformMatrix = m_transformMatrix * mScalingMatrix; + m_transformMatrix = m_transformMatrix * m_scalingMatrix; // Create the VBOs and VAO if (totalNbSpheres == 0) { @@ -165,7 +165,7 @@ void Sphere::render(openglframework::Shader& shader, const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * m_transformMatrix; const openglframework::Matrix3 normalMatrix = localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + shader.setetk::Matrix3x3Uniform("normalMatrix", normalMatrix, false); // Set the vertex color openglframework::Color currentColor = m_body->isSleeping() ? mSleepingColor : mColor; @@ -214,14 +214,14 @@ void Sphere::createVBOAndVAO() { // Create the VBO for the vertices data mVBOVertices.create(); mVBOVertices.bind(); - size_t sizeVertices = m_vertices.size() * sizeof(openglframework::Vector3); + size_t sizeVertices = m_vertices.size() * sizeof(openglframework::vec3); mVBOVertices.copyDataIntoVBO(sizeVertices, getVerticesPointer(), GL_STATIC_DRAW); mVBOVertices.unbind(); // Create the VBO for the normals data mVBONormals.create(); mVBONormals.bind(); - size_t sizeNormals = mNormals.size() * sizeof(openglframework::Vector3); + size_t sizeNormals = mNormals.size() * sizeof(openglframework::vec3); mVBONormals.copyDataIntoVBO(sizeNormals, getNormalsPointer(), GL_STATIC_DRAW); mVBONormals.unbind(); @@ -229,7 +229,7 @@ void Sphere::createVBOAndVAO() { // Create the VBO for the texture co data mVBOTextureCoords.create(); mVBOTextureCoords.bind(); - size_t sizeTextureCoords = mUVs.size() * sizeof(openglframework::Vector2); + size_t sizeTextureCoords = mUVs.size() * sizeof(openglframework::vec2); mVBOTextureCoords.copyDataIntoVBO(sizeTextureCoords, getUVTextureCoordinatesPointer(), GL_STATIC_DRAW); mVBOTextureCoords.unbind(); } @@ -274,22 +274,22 @@ void Sphere::resetTransform(const rp3d::Transform& transform) { // Reset the velocity of the rigid body rp3d::RigidBody* rigidBody = dynamic_cast(m_body); if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setLinearVelocity(rp3d::vec3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::vec3(0, 0, 0)); } - updateTransform(1.0f); + updateetk::Transform3D(1.0f); } // Set the scaling of the object -void Sphere::setScaling(const openglframework::Vector3& scaling) { +void Sphere::setScaling(const openglframework::vec3& scaling) { // Scale the collision shape - m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); + m_proxyShape->setLocalScaling(rp3d::vec3(scaling.x(), scaling.y(), scaling.z())); // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0, - 0, mRadius * scaling.y, 0, 0, - 0, 0, mRadius * scaling.z, 0, + m_scalingMatrix = openglframework::Matrix4(mRadius * scaling.x(), 0, 0, 0, + 0, mRadius * scaling.y(), 0, 0, + 0, 0, mRadius * scaling.z(), 0, 0, 0, 0, 1); } diff --git a/tools/testbed/common/Sphere.h b/tools/testbed/common/Sphere.h index c6a8db4..f2a0c96 100644 --- a/tools/testbed/common/Sphere.h +++ b/tools/testbed/common/Sphere.h @@ -46,10 +46,10 @@ class Sphere : public openglframework::Mesh, public PhysicsObject { rp3d::ProxyShape* m_proxyShape; /// Scaling matrix (applied to a sphere to obtain the correct sphere dimensions) - openglframework::Matrix4 mScalingMatrix; + openglframework::Matrix4 m_scalingMatrix; /// Previous transform (for int32_terpolation) - rp3d::Transform mPreviousTransform; + rp3d::etk::Transform3D mPreviousTransform; /// Vertex Buffer Object for the vertices data static openglframework::VertexBufferObject mVBOVertices; @@ -79,11 +79,11 @@ class Sphere : public openglframework::Mesh, public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - Sphere(float radius, const openglframework::Vector3& position, + Sphere(float radius, const openglframework::vec3& position, rp3d::CollisionWorld* world, const std::string& meshFolderPath); /// Constructor - Sphere(float radius, const openglframework::Vector3& position, + Sphere(float radius, const openglframework::vec3& position, float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath); /// Destructor @@ -97,15 +97,15 @@ class Sphere : public openglframework::Mesh, public PhysicsObject { void resetTransform(const rp3d::Transform& transform); /// Update the transform matrix of the object - virtual void updateTransform(float int32_terpolationFactor); + virtual void updateetk::Transform3D(float int32_terpolationFactor); /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::vec3& scaling); }; // Update the transform matrix of the object -inline void Sphere::updateTransform(float int32_terpolationFactor) { - m_transformMatrix = computeTransform(int32_terpolationFactor, mScalingMatrix); +inline void Sphere::updateetk::Transform3D(float int32_terpolationFactor) { + m_transformMatrix = computeetk::Transform3D(int32_terpolationFactor, m_scalingMatrix); } #endif diff --git a/tools/testbed/common/VisualContactPoint.cpp b/tools/testbed/common/VisualContactPoint.cpp index 718c947..deef888 100644 --- a/tools/testbed/common/VisualContactPoint.cpp +++ b/tools/testbed/common/VisualContactPoint.cpp @@ -36,7 +36,7 @@ openglframework::Mesh VisualContactPoint::mMesh; bool VisualContactPoint::mStaticDataCreated = false; // Constructor -VisualContactPoint::VisualContactPoint(const openglframework::Vector3& position, +VisualContactPoint::VisualContactPoint(const openglframework::vec3& position, const std::string& meshFolderPath) : mColor(1.0f, 0.0f, 0.0f, 1.0f) { @@ -104,7 +104,7 @@ void VisualContactPoint::render(openglframework::Shader& shader, const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * m_transformMatrix; const openglframework::Matrix3 normalMatrix = localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + shader.setetk::Matrix3x3Uniform("normalMatrix", normalMatrix, false); // Set the vertex color openglframework::Vector4 color(mColor.r, mColor.g, mColor.b, mColor.a); @@ -147,14 +147,14 @@ void VisualContactPoint::createVBOAndVAO() { // Create the VBO for the vertices data mVBOVertices.create(); mVBOVertices.bind(); - size_t sizeVertices = mMesh.getVertices().size() * sizeof(openglframework::Vector3); + size_t sizeVertices = mMesh.getVertices().size() * sizeof(openglframework::vec3); mVBOVertices.copyDataIntoVBO(sizeVertices, mMesh.getVerticesPointer(), GL_STATIC_DRAW); mVBOVertices.unbind(); // Create the VBO for the normals data mVBONormals.create(); mVBONormals.bind(); - size_t sizeNormals = mMesh.getNormals().size() * sizeof(openglframework::Vector3); + size_t sizeNormals = mMesh.getNormals().size() * sizeof(openglframework::vec3); mVBONormals.copyDataIntoVBO(sizeNormals, mMesh.getNormalsPointer(), GL_STATIC_DRAW); mVBONormals.unbind(); diff --git a/tools/testbed/common/VisualContactPoint.h b/tools/testbed/common/VisualContactPoint.h index 95a71fb..9e6fc2c 100644 --- a/tools/testbed/common/VisualContactPoint.h +++ b/tools/testbed/common/VisualContactPoint.h @@ -72,7 +72,7 @@ class VisualContactPoint : public openglframework::Object3D { // -------------------- Methods -------------------- // /// Constructor - VisualContactPoint(const openglframework::Vector3& position, + VisualContactPoint(const openglframework::vec3& position, const std::string &meshFolderPath); /// Destructor diff --git a/tools/testbed/opengl-framework/src/Camera.cpp b/tools/testbed/opengl-framework/src/Camera.cpp index 35b6277..ef062c4 100644 --- a/tools/testbed/opengl-framework/src/Camera.cpp +++ b/tools/testbed/opengl-framework/src/Camera.cpp @@ -40,7 +40,7 @@ Camera::Camera() : Object3D() { mSceneRadius = 1.0f; mNearPlane = 0.1f; mFarPlane = 10.0f; - mWidth = 1; + m_width = 1; mHeight = 1; // Update the projection matrix @@ -56,7 +56,7 @@ Camera::~Camera() { void Camera::updateProjectionMatrix() { // Compute the aspect ratio - float aspect = float(mWidth) / float(mHeight); + float aspect = float(m_width) / float(mHeight); float top = mNearPlane * tan((mFieldOfView / 2.0f) * (float(PI) / 180.0f)); float bottom = -top; @@ -76,22 +76,22 @@ void Camera::updateProjectionMatrix() { } // Translate the camera go a given point using the dx, dy fraction -void Camera::translateCamera(float dx, float dy, const Vector3& worldPoint) { +void Camera::translateCamera(float dx, float dy, const vec3& worldPoint) { - // Transform the world point int32_to camera coordinates - Vector3 pointCamera = m_transformMatrix.getInverse() * worldPoint; + // etk::Transform3D the world point int32_to camera coordinates + vec3 pointCamera = m_transformMatrix.getInverse() * worldPoint; // Get the depth - float z = -pointCamera.z; + float z = -pointCamera.z(); // Find the scaling of dx and dy from windows coordinates to near plane coordinates // and from there to camera coordinates at the object's depth - float aspect = float(mWidth) / float(mHeight); + float aspect = float(m_width) / float(mHeight); float top = mNearPlane * tan(mFieldOfView * PI / 360.0f); float right = top * aspect; // Translate the camera - translateLocal(Vector3(2.0f * dx * right / mNearPlane * z, + translateLocal(vec3(2.0f * dx * right / mNearPlane * z, -2.0f * dy * top / mNearPlane * z, 0.0f)); } diff --git a/tools/testbed/opengl-framework/src/Camera.h b/tools/testbed/opengl-framework/src/Camera.h index 5b69a46..6482254 100644 --- a/tools/testbed/opengl-framework/src/Camera.h +++ b/tools/testbed/opengl-framework/src/Camera.h @@ -52,7 +52,7 @@ class Camera : public Object3D { float mFarPlane; // Width of the camera - uint32_t mWidth; + uint32_t m_width; // Height of the camera uint32_t mHeight; @@ -98,7 +98,7 @@ class Camera : public Object3D { void setZoom(float fraction); // Translate the camera go a given point using the dx, dy fraction - void translateCamera(float dx, float dy, const Vector3& worldPoint); + void translateCamera(float dx, float dy, const vec3& worldPoint); // Get the near clipping plane float getNearClippingPlane() const; @@ -120,7 +120,7 @@ inline const Matrix4& Camera::getProjectionMatrix() const { // Set the dimensions of the camera inline void Camera::setDimensions(uint32_t width, uint32_t height) { - mWidth = width; + m_width = width; mHeight = height; updateProjectionMatrix(); } @@ -152,7 +152,7 @@ inline void Camera::setFieldOfView(float fov) { // Set the zoom of the camera (a fraction between 0 and 1) inline void Camera::setZoom(float fraction) { - Vector3 zoomVector(0, 0, mSceneRadius * fraction); + vec3 zoomVector(0, 0, mSceneRadius * fraction); translateLocal(zoomVector); } @@ -168,7 +168,7 @@ inline float Camera::getFarClippingPlane() const { // Get the width inline uint32_t Camera::getWidth() const { - return mWidth; + return m_width; } // Get the height diff --git a/tools/testbed/opengl-framework/src/Light.h b/tools/testbed/opengl-framework/src/Light.h index 3082397..b701caa 100644 --- a/tools/testbed/opengl-framework/src/Light.h +++ b/tools/testbed/opengl-framework/src/Light.h @@ -28,7 +28,7 @@ // Libraries #include -#include +#include #include #include #include diff --git a/tools/testbed/opengl-framework/src/Mesh.cpp b/tools/testbed/opengl-framework/src/Mesh.cpp index abd7754..47c3c11 100644 --- a/tools/testbed/opengl-framework/src/Mesh.cpp +++ b/tools/testbed/opengl-framework/src/Mesh.cpp @@ -55,7 +55,7 @@ void Mesh::destroy() { // Compute the normals of the mesh void Mesh::calculateNormals() { - mNormals = vector(getNbVertices(), Vector3(0, 0, 0)); + mNormals = vector(getNbVertices(), vec3(0, 0, 0)); // For each triangular face for (uint32_t i=0; i(getNbVertices(), Vector3(0, 0, 0)); + mTangents = std::vector(getNbVertices(), vec3(0, 0, 0)); // For each face for (uint32_t i=0; i::const_iterator it(m_vertices.begin()); + std::vector::const_iterator it(m_vertices.begin()); // For each vertex of the mesh for (; it != m_vertices.end(); ++it) { - if( (*it).x < min.x ) min.x = (*it).x; - else if ( (*it).x > max.x ) max.x = (*it).x; + if( (*it).x() < min.x ) min.x = (*it).x; + else if ( (*it).x() > max.x ) max.x = (*it).x; - if( (*it).y < min.y ) min.y = (*it).y; - else if ( (*it).y > max.y ) max.y = (*it).y; + if( (*it).y() < min.y ) min.y = (*it).y; + else if ( (*it).y() > max.y ) max.y = (*it).y; - if( (*it).z < min.z ) min.z = (*it).z; - else if ( (*it).z > max.z ) max.z = (*it).z; + if( (*it).z() < min.z ) min.z = (*it).z; + else if ( (*it).z() > max.z ) max.z = (*it).z; } } else { diff --git a/tools/testbed/opengl-framework/src/Mesh.h b/tools/testbed/opengl-framework/src/Mesh.h index c8750ba..c6abfef 100644 --- a/tools/testbed/opengl-framework/src/Mesh.h +++ b/tools/testbed/opengl-framework/src/Mesh.h @@ -31,8 +31,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include @@ -52,19 +52,19 @@ class Mesh : public Object3D { std::vector > mIndices; // Vertices coordinates (local space) - std::vector m_vertices; + std::vector m_vertices; // Normals coordinates - std::vector mNormals; + std::vector mNormals; // Tangents coordinates - std::vector mTangents; + std::vector mTangents; // Color for each vertex std::vector mColors; // UV texture coordinates - std::vector mUVs; + std::vector mUVs; // Textures of the mesh (one for each part of the mesh) std::map mTextures; @@ -89,7 +89,7 @@ class Mesh : public Object3D { void calculateTangents(); // Calculate the bounding box of the mesh - void calculateBoundingBox(Vector3& min, Vector3& max) const; + void calculateBoundingBox(vec3& min, vec3& max) const; // Scale of vertices of the mesh using a given factor void scaleVertices(float factor); @@ -104,22 +104,22 @@ class Mesh : public Object3D { uint32_t getNbParts() const; // Return a reference to the vertices - const std::vector& getVertices() const; + const std::vector& getVertices() const; // Set the vertices of the mesh - void setVertices(std::vector& vertices); + void setVertices(std::vector& vertices); // Return a reference to the normals - const std::vector& getNormals() const; + const std::vector& getNormals() const; // set the normals of the mesh - void setNormals(std::vector& normals); + void setNormals(std::vector& normals); // Return a reference to the UVs - const std::vector& getUVs() const; + const std::vector& getUVs() const; // Set the UV texture coordinates of the mesh - void setUVs(std::vector& uvs); + void setUVs(std::vector& uvs); // Return a reference to the vertex indices const std::vector& getIndices(uint32_t part = 0) const; @@ -128,16 +128,16 @@ class Mesh : public Object3D { void setIndices(std::vector >& indices); // Return the coordinates of a given vertex - const Vector3& getVertex(uint32_t i) const; + const vec3& getVertex(uint32_t i) const; // Set the coordinates of a given vertex - void setVertex(uint32_t i, const Vector3& vertex); + void setVertex(uint32_t i, const vec3& vertex); // Return the coordinates of a given normal - const Vector3& getNormal(uint32_t i) const; + const vec3& getNormal(uint32_t i) const; // Set the coordinates of a given normal - void setNormal(uint32_t i, const Vector3& normal); + void setNormal(uint32_t i, const vec3& normal); // Return the color of a given vertex const Color& getVertexColor(uint32_t i) const; @@ -149,10 +149,10 @@ class Mesh : public Object3D { void setColorToAllVertices(const Color& color); // Return the UV of a given vertex - const Vector2& getUV(uint32_t i) const; + const vec2& getUV(uint32_t i) const; // Set the UV of a given vertex - void setUV(uint32_t i, const Vector2& uv); + void setUV(uint32_t i, const vec2& uv); // Return the vertex index of the ith (i=0,1,2) vertex of a given face uint32_t getVertexIndexInFace(uint32_t faceIndex, uint32_t i, uint32_t part = 0) const; @@ -218,32 +218,32 @@ inline uint32_t Mesh::getNbParts() const { } // Return a reference to the vertices -inline const std::vector& Mesh::getVertices() const { +inline const std::vector& Mesh::getVertices() const { return m_vertices; } // Set the vertices of the mesh -inline void Mesh::setVertices(std::vector& vertices) { +inline void Mesh::setVertices(std::vector& vertices) { m_vertices = vertices; } // Return a reference to the normals -inline const std::vector& Mesh::getNormals() const { +inline const std::vector& Mesh::getNormals() const { return mNormals; } // set the normals of the mesh -inline void Mesh::setNormals(std::vector& normals) { +inline void Mesh::setNormals(std::vector& normals) { mNormals = normals; } // Return a reference to the UVs -inline const std::vector& Mesh::getUVs() const { +inline const std::vector& Mesh::getUVs() const { return mUVs; } // Set the UV texture coordinates of the mesh -inline void Mesh::setUVs(std::vector& uvs) { +inline void Mesh::setUVs(std::vector& uvs) { mUVs = uvs; } @@ -258,25 +258,25 @@ inline void Mesh::setIndices(std::vector >& indices) { } // Return the coordinates of a given vertex -inline const Vector3& Mesh::getVertex(uint32_t i) const { +inline const vec3& Mesh::getVertex(uint32_t i) const { assert(i < getNbVertices()); return m_vertices[i]; } // Set the coordinates of a given vertex -inline void Mesh::setVertex(uint32_t i, const Vector3& vertex) { +inline void Mesh::setVertex(uint32_t i, const vec3& vertex) { assert(i < getNbVertices()); m_vertices[i] = vertex; } // Return the coordinates of a given normal -inline const Vector3& Mesh::getNormal(uint32_t i) const { +inline const vec3& Mesh::getNormal(uint32_t i) const { assert(i < getNbVertices()); return mNormals[i]; } // Set the coordinates of a given normal -inline void Mesh::setNormal(uint32_t i, const Vector3& normal) { +inline void Mesh::setNormal(uint32_t i, const vec3& normal) { assert(i < getNbVertices()); mNormals[i] = normal; } @@ -318,13 +318,13 @@ inline void Mesh::setColorToAllVertices(const Color& color) { } // Return the UV of a given vertex -inline const Vector2& Mesh::getUV(uint32_t i) const { +inline const vec2& Mesh::getUV(uint32_t i) const { assert(i < getNbVertices()); return mUVs[i]; } // Set the UV of a given vertex -inline void Mesh::setUV(uint32_t i, const Vector2& uv) { +inline void Mesh::setUV(uint32_t i, const vec2& uv) { assert(i < getNbVertices()); mUVs[i] = uv; } diff --git a/tools/testbed/opengl-framework/src/MeshReaderWriter.cpp b/tools/testbed/opengl-framework/src/MeshReaderWriter.cpp index c47503a..f8cdf47 100644 --- a/tools/testbed/opengl-framework/src/MeshReaderWriter.cpp +++ b/tools/testbed/opengl-framework/src/MeshReaderWriter.cpp @@ -105,9 +105,9 @@ void MeshReaderWriter::loadOBJFile(const string &filename, Mesh& meshToCreate) { float v1, v2, v3; size_t found1, found2; std::vector isQuad; - std::vector vertices; - std::vector normals; - std::vector uvs; + std::vector vertices; + std::vector normals; + std::vector uvs; std::vector verticesIndices; std::vector normalsIndices; std::vector uvsIndices; @@ -128,15 +128,15 @@ void MeshReaderWriter::loadOBJFile(const string &filename, Mesh& meshToCreate) { } else if(word == "v") { // Vertex position sscanf(buffer.c_str(), "%*s %f %f %f", &v1, &v2, &v3); - vertices.push_back(Vector3(v1, v2, v3)); + vertices.push_back(vec3(v1, v2, v3)); } else if(word == "vt") { // Vertex texture coordinate sscanf(buffer.c_str(), "%*s %f %f", &v1, &v2); - uvs.push_back(Vector2(v1,v2)); + uvs.push_back(vec2(v1,v2)); } else if(word == "vn") { // Vertex normal sscanf(buffer.c_str(), "%*s %f %f %f", &v1, &v2, &v3); - normals.push_back(Vector3(v1 ,v2, v3)); + normals.push_back(vec3(v1 ,v2, v3)); } else if (word == "f") { // Face line = buffer; @@ -215,10 +215,10 @@ void MeshReaderWriter::loadOBJFile(const string &filename, Mesh& meshToCreate) { // Mesh data vector > meshIndices; - vector meshNormals; - if (!normals.empty()) meshNormals = vector(vertices.size(), Vector3(0, 0, 0)); - vector meshUVs; - if (!uvs.empty()) meshUVs = vector(vertices.size(), Vector2(0, 0)); + vector meshNormals; + if (!normals.empty()) meshNormals = vector(vertices.size(), vec3(0, 0, 0)); + vector meshUVs; + if (!uvs.empty()) meshUVs = vector(vertices.size(), vec2(0, 0)); // We cannot load mesh with several parts for the moment uint32_t meshPart = 0; @@ -259,15 +259,15 @@ void MeshReaderWriter::loadOBJFile(const string &filename, Mesh& meshToCreate) { } else { // If the current vertex is in a quad - Vector3 v1 = vertices[i1]; - Vector3 v2 = vertices[i2]; - Vector3 v3 = vertices[i3]; + vec3 v1 = vertices[i1]; + vec3 v2 = vertices[i2]; + vec3 v3 = vertices[i3]; uint32_t i4 = verticesIndices[i+3]; - Vector3 v4 = vertices[i4]; + vec3 v4 = vertices[i4]; - Vector3 v13 = v3-v1; - Vector3 v12 = v2-v1; - Vector3 v14 = v4-v1; + vec3 v13 = v3-v1; + vec3 v12 = v2-v1; + vec3 v14 = v4-v1; float a1 = v13.dot(v12); float a2 = v13.dot(v14); @@ -317,9 +317,9 @@ void MeshReaderWriter::writeOBJFile(const std::string& filename, const Mesh& mes std::ofstream file(filename.c_str()); // Geth the mesh data - const std::vector& vertices = meshToWrite.getVertices(); - const std::vector& normals = meshToWrite.getNormals(); - const std::vector& uvs = meshToWrite.getUVs(); + const std::vector& vertices = meshToWrite.getVertices(); + const std::vector& normals = meshToWrite.getNormals(); + const std::vector& uvs = meshToWrite.getUVs(); // If we can open the file if (file.is_open()) { @@ -329,7 +329,7 @@ void MeshReaderWriter::writeOBJFile(const std::string& filename, const Mesh& mes // Write the vertices for (uint32_t v=0; v +#include #include namespace openglframework { @@ -40,7 +40,7 @@ class Object3D { // -------------------- Attributes -------------------- // - // Transformation matrix that convert local-space + // etk::Transform3Dation matrix that convert local-space // coordinates to world-space coordinates Matrix4 m_transformMatrix; @@ -64,25 +64,25 @@ class Object3D { void setToIdentity(); // Return the origin of object in world-space - Vector3 getOrigin() const; + vec3 getOrigin() const; // Translate the object in world-space - void translateWorld(const Vector3& v); + void translateWorld(const vec3& v); // Translate the object in local-space - void translateLocal(const Vector3& v); + void translateLocal(const vec3& v); // Rotate the object in world-space - void rotateWorld(const Vector3& axis, float angle); + void rotateWorld(const vec3& axis, float angle); // Rotate the object in local-space - void rotateLocal(const Vector3& axis, float angle); + void rotateLocal(const vec3& axis, float angle); // Rotate around a world-space point - void rotateAroundWorldPoint(const Vector3& axis, float angle, const Vector3& point); + void rotateAroundWorldPoint(const vec3& axis, float angle, const vec3& point); // Rotate around a local-space point - void rotateAroundLocalPoint(const Vector3& axis, float angle, const Vector3& worldPoint); + void rotateAroundLocalPoint(const vec3& axis, float angle, const vec3& worldPoint); }; // Return the transform matrix @@ -101,43 +101,43 @@ inline void Object3D::setToIdentity() { } // Return the origin of object in world-space -inline Vector3 Object3D::getOrigin() const { - return m_transformMatrix * Vector3(0.0, 0.0, 0.0); +inline vec3 Object3D::getOrigin() const { + return m_transformMatrix * vec3(0.0, 0.0, 0.0); } // Translate the object in world-space -inline void Object3D::translateWorld(const Vector3& v) { +inline void Object3D::translateWorld(const vec3& v) { m_transformMatrix = Matrix4::translationMatrix(v) * m_transformMatrix; } // Translate the object in local-space -inline void Object3D::translateLocal(const Vector3& v) { +inline void Object3D::translateLocal(const vec3& v) { m_transformMatrix = m_transformMatrix * Matrix4::translationMatrix(v); } // Rotate the object in world-space -inline void Object3D::rotateWorld(const Vector3& axis, float angle) { +inline void Object3D::rotateWorld(const vec3& axis, float angle) { m_transformMatrix = Matrix4::rotationMatrix(axis, angle) * m_transformMatrix; } // Rotate the object in local-space -inline void Object3D::rotateLocal(const Vector3& axis, float angle) { +inline void Object3D::rotateLocal(const vec3& axis, float angle) { m_transformMatrix = m_transformMatrix * Matrix4::rotationMatrix(axis, angle); } // Rotate the object around a world-space point -inline void Object3D::rotateAroundWorldPoint(const Vector3& axis, float angle, - const Vector3& worldPoint) { +inline void Object3D::rotateAroundWorldPoint(const vec3& axis, float angle, + const vec3& worldPoint) { m_transformMatrix = Matrix4::translationMatrix(worldPoint) * Matrix4::rotationMatrix(axis, angle) * Matrix4::translationMatrix(-worldPoint) * m_transformMatrix; } // Rotate the object around a local-space point -inline void Object3D::rotateAroundLocalPoint(const Vector3& axis, float angle, - const Vector3& worldPoint) { +inline void Object3D::rotateAroundLocalPoint(const vec3& axis, float angle, + const vec3& worldPoint) { // Convert the world point int32_to the local coordinate system - Vector3 localPoint = m_transformMatrix.getInverse() * worldPoint; + vec3 localPoint = m_transformMatrix.getInverse() * worldPoint; m_transformMatrix = m_transformMatrix * Matrix4::translationMatrix(localPoint) * Matrix4::rotationMatrix(axis, angle) diff --git a/tools/testbed/opengl-framework/src/Shader.h b/tools/testbed/opengl-framework/src/Shader.h index a330c92..a452cc4 100644 --- a/tools/testbed/opengl-framework/src/Shader.h +++ b/tools/testbed/opengl-framework/src/Shader.h @@ -29,8 +29,8 @@ // Libraries #include #include -#include -#include +#include +#include #include #include #include @@ -98,12 +98,12 @@ class Shader { // Set a vector 2 uniform value to this shader (be careful if the uniform is not // used in the shader, the compiler will remove it, then when you will try // to set it, an assert will occur) - void setVector2Uniform(const std::string& variableName, const Vector2& v, bool errorIfMissing = true) const; + void setvec2Uniform(const std::string& variableName, const vec2& v, bool errorIfMissing = true) const; // Set a vector 3 uniform value to this shader (be careful if the uniform is not // used in the shader, the compiler will remove it, then when you will try // to set it, an assert will occur) - void setVector3Uniform(const std::string& variableName, const Vector3& v, bool errorIfMissing = true) const; + void setvec3Uniform(const std::string& variableName, const vec3& v, bool errorIfMissing = true) const; // Set a vector 4 uniform value to this shader (be careful if the uniform is not // used in the shader, the compiler will remove it, then when you will try @@ -113,13 +113,13 @@ class Shader { // Set a 3x3 matrix uniform value to this shader (be careful if the uniform is not // used in the shader, the compiler will remove it, then when you will try // to set it, an assert will occur) - void setMatrix3x3Uniform(const std::string& variableName, const float* matrix, + void setetk::Matrix3x3Uniform(const std::string& variableName, const float* matrix, bool transpose = false, bool errorIfMissing = true) const; // Set a 3x3 matrix uniform value to this shader (be careful if the uniform is not // used in the shader, the compiler will remove it, then when you will try // to set it, an assert will occur) - void setMatrix3x3Uniform(const std::string& variableName, const Matrix3& matrix, bool errorIfMissing = true) const; + void setetk::Matrix3x3Uniform(const std::string& variableName, const Matrix3& matrix, bool errorIfMissing = true) const; // Set a 4x4 matrix uniform value to this shader (be careful if the uniform is not // used in the shader, the compiler will remove it, then when you will try @@ -212,22 +212,22 @@ inline void Shader::setIntUniform(const std::string& variableName, int32_t value // Set a vector 2 uniform value to this shader (be careful if the uniform is not // used in the shader, the compiler will remove it, then when you will try // to set it, an assert will occur) -inline void Shader::setVector2Uniform(const std::string& variableName, const Vector2& v, bool errorIfMissing) const { +inline void Shader::setvec2Uniform(const std::string& variableName, const vec2& v, bool errorIfMissing) const { assert(mProgramObjectID != 0); GLint32_t location = getUniformLocation(variableName, errorIfMissing); if (location != -1) { - glUniform2f(location, v.x, v.y); + glUniform2f(location, v.x(), v.y()); } } // Set a vector 3 uniform value to this shader (be careful if the uniform is not // used in the shader, the compiler will remove it, then when you will try // to set it, an assert will occur) -inline void Shader::setVector3Uniform(const std::string& variableName, const Vector3 &v, bool errorIfMissing) const { +inline void Shader::setvec3Uniform(const std::string& variableName, const vec3 &v, bool errorIfMissing) const { assert(mProgramObjectID != 0); GLint32_t location = getUniformLocation(variableName, errorIfMissing); if (location != -1) { - glUniform3f(location, v.x, v.y, v.z); + glUniform3f(location, v.x(), v.y(), v.z()); } } @@ -238,14 +238,14 @@ inline void Shader::setVector4Uniform(const std::string& variableName, const Vec assert(mProgramObjectID != 0); GLint32_t location = getUniformLocation(variableName, errorIfMissing); if (location != -1) { - glUniform4f(location, v.x, v.y, v.z, v.w); + glUniform4f(location, v.x(), v.y(), v.z(), v.w); } } // Set a 4x4 matrix uniform value to this shader (be careful if the uniform is not // used in the shader, the compiler will remove it, then when you will try // to set it, an assert will occur) -inline void Shader::setMatrix3x3Uniform(const std::string& variableName, const float* matrix, +inline void Shader::setetk::Matrix3x3Uniform(const std::string& variableName, const float* matrix, bool transpose, bool errorIfMissing) const { assert(mProgramObjectID != 0); GLint32_t location = getUniformLocation(variableName, errorIfMissing); @@ -257,7 +257,7 @@ inline void Shader::setMatrix3x3Uniform(const std::string& variableName, const f // Set a 3x3 matrix uniform value to this shader (be careful if the uniform is not // used in the shader, the compiler will remove it, then when you will try // to set it, an assert will occur) -inline void Shader::setMatrix3x3Uniform(const std::string& variableName, const Matrix3& matrix, bool errorIfMissing) const { +inline void Shader::setetk::Matrix3x3Uniform(const std::string& variableName, const Matrix3& matrix, bool errorIfMissing) const { assert(mProgramObjectID != 0); GLfloat mat[9]; for (int32_t i=0; i<3; i++) { diff --git a/tools/testbed/opengl-framework/src/Texture2D.cpp b/tools/testbed/opengl-framework/src/Texture2D.cpp index 19449d7..11ddc5c 100644 --- a/tools/testbed/opengl-framework/src/Texture2D.cpp +++ b/tools/testbed/opengl-framework/src/Texture2D.cpp @@ -33,13 +33,13 @@ using namespace openglframework; // Constructor -Texture2D::Texture2D() : m_id(0), mUnit(0), mWidth(0), mHeight(0) { +Texture2D::Texture2D() : m_id(0), mUnit(0), m_width(0), mHeight(0) { } // Constructor Texture2D::Texture2D(uint32_t width, uint32_t height, uint32_t int32_ternalFormat, uint32_t format, uint32_t type) - : m_id(0), mUnit(0), mWidth(0), mHeight(0) { + : m_id(0), mUnit(0), m_width(0), mHeight(0) { // Create the texture create(width, height, int32_ternalFormat, format, type); @@ -57,7 +57,7 @@ void Texture2D::create(uint32_t width, uint32_t height, uint32_t int32_ternalFor // Destroy the current texture destroy(); - mWidth = width; + m_width = width; mHeight = height; // Create the OpenGL texture @@ -68,7 +68,7 @@ void Texture2D::create(uint32_t width, uint32_t height, uint32_t int32_ternalFor glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); - glTexImage2D(GL_TEXTURE_2D, 0, int32_ternalFormat, mWidth, mHeight, 0, format, type, data); + glTexImage2D(GL_TEXTURE_2D, 0, int32_ternalFormat, m_width, mHeight, 0, format, type, data); glBindTexture(GL_TEXTURE_2D, 0); } @@ -79,7 +79,7 @@ void Texture2D::create(uint32_t width, uint32_t height, uint32_t int32_ternalFor // Destroy the current texture destroy(); - mWidth = width; + m_width = width; mHeight = height; // Create the OpenGL texture @@ -90,7 +90,7 @@ void Texture2D::create(uint32_t width, uint32_t height, uint32_t int32_ternalFor glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, wrapT); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, maxFilter); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, minFilter); - glTexImage2D(GL_TEXTURE_2D, 0, int32_ternalFormat, mWidth, mHeight, 0, format, type, data); + glTexImage2D(GL_TEXTURE_2D, 0, int32_ternalFormat, m_width, mHeight, 0, format, type, data); glBindTexture(GL_TEXTURE_2D, 0); } @@ -100,7 +100,7 @@ void Texture2D::destroy() { glDeleteTextures(1, &m_id); m_id = 0; mUnit = 0; - mWidth = 0; + m_width = 0; mHeight = 0; } } diff --git a/tools/testbed/opengl-framework/src/Texture2D.h b/tools/testbed/opengl-framework/src/Texture2D.h index 42defbf..3aabd34 100644 --- a/tools/testbed/opengl-framework/src/Texture2D.h +++ b/tools/testbed/opengl-framework/src/Texture2D.h @@ -49,7 +49,7 @@ class Texture2D { GLuint32_t mUnit; // Width - uint32_t mWidth; + uint32_t m_width; // Height uint32_t mHeight; @@ -88,7 +88,7 @@ class Texture2D { uint32_t getID() const; // Get the unit of the texture - uint32_t getUnit() const; + uint32_t safeNormalized() const; // Set the unit of the texture void setUnit(uint32_t unit); @@ -120,7 +120,7 @@ inline uint32_t Texture2D::getID() const { } // Get the unit of the texture -inline uint32_t Texture2D::getUnit() const { +inline uint32_t Texture2D::safeNormalized() const { return mUnit; } @@ -131,7 +131,7 @@ inline void Texture2D::setUnit(uint32_t unit) { // Get the width inline uint32_t Texture2D::getWidth() const { - return mWidth; + return m_width; } // Get the height diff --git a/tools/testbed/opengl-framework/src/TextureReaderWriter.cpp b/tools/testbed/opengl-framework/src/TextureReaderWriter.cpp index 6dec50c..289ce62 100644 --- a/tools/testbed/opengl-framework/src/TextureReaderWriter.cpp +++ b/tools/testbed/opengl-framework/src/TextureReaderWriter.cpp @@ -191,8 +191,8 @@ void TextureReaderWriter::writeTGAPicture(const std::string& filename, const Tex header.colourmapstart = 0; // first colour map entry in palette header.colourmaplength = 0; // number of colours in palette header.colourmapbits=0; // number of bits per palette entry 15,16,24,32 - header.xstart = 0; // image x origin - header.ystart = 0; // image y origin + header.x()start = 0; // image x origin + header.y()start = 0; // image y origin header.width = (short)texture.getWidth(); // image width in pixels header.height = (short)texture.getHeight(); // image height in pixels header.bits = 24; // image bits per pixel 8,16,24,32 diff --git a/tools/testbed/opengl-framework/src/maths/Matrix3.h b/tools/testbed/opengl-framework/src/maths/Matrix3.h index 1d14f99..71e4173 100644 --- a/tools/testbed/opengl-framework/src/maths/Matrix3.h +++ b/tools/testbed/opengl-framework/src/maths/Matrix3.h @@ -29,7 +29,7 @@ // Libraries #include #include -#include +#include namespace openglframework { @@ -55,7 +55,7 @@ class Matrix3 { Matrix3(float a1, float a2, float a3, float b1, float b2, float b3, float c1, float c2, float c3) { - setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3); + setValue(a1, a2, a3, b1, b2, b3, c1, c2, c3); } // Constructor @@ -66,15 +66,15 @@ class Matrix3 { } // Constructor - Matrix3(const Vector3& a1, const Vector3& a2, const Vector3& a3) { - m[0][0] = a1.x; m[0][1] = a2.x; m[0][2] = a3.x; - m[1][0] = a1.y; m[1][1] = a2.y; m[1][2] = a3.y; - m[2][0] = a1.z; m[2][1] = a2.z; m[2][2] = a3.z; + Matrix3(const vec3& a1, const vec3& a2, const vec3& a3) { + m[0][0] = a1.x(); m[0][1] = a2.x; m[0][2] = a3.x; + m[1][0] = a1.y(); m[1][1] = a2.y; m[1][2] = a3.y; + m[2][0] = a1.z(); m[2][1] = a2.z; m[2][2] = a3.z; } // Constructor Matrix3(const Matrix3& matrix) { - setAllValues(matrix.m[0][0], matrix.m[0][1], matrix.m[0][2], + setValue(matrix.m[0][0], matrix.m[0][1], matrix.m[0][2], matrix.m[1][0], matrix.m[1][1], matrix.m[1][2], matrix.m[2][0], matrix.m[2][1], matrix.m[2][2]); } @@ -92,7 +92,7 @@ class Matrix3 { } // Method to set all the values in the matrix - void setAllValues(float a1, float a2, float a3, float b1, float b2, float b3, + void setValue(float a1, float a2, float a3, float b1, float b2, float b3, float c1, float c2, float c3) { m[0][0] = a1; m[0][1] = a2; m[0][2] = a3; m[1][0] = b1; m[1][1] = b2; m[1][2] = b3; @@ -100,9 +100,9 @@ class Matrix3 { } // Return a column - Vector3 getColumn(int32_t i) const { + vec3 getColumn(int32_t i) const { assert(i>= 0 && i<3); - return Vector3(m[0][i], m[1][i], m[2][i]); + return vec3(m[0][i], m[1][i], m[2][i]); } // Return the transpose matrix @@ -181,7 +181,7 @@ class Matrix3 { // Overloaded operator = Matrix3& operator=(const Matrix3& matrix) { if (&matrix != this) { - setAllValues(matrix.m[0][0], matrix.m[0][1], matrix.m[0][2], + setValue(matrix.m[0][0], matrix.m[0][1], matrix.m[0][2], matrix.m[1][0], matrix.m[1][1], matrix.m[1][2], matrix.m[2][0], matrix.m[2][1], matrix.m[2][2]); } @@ -231,10 +231,10 @@ class Matrix3 { } // Overloaded operator for multiplication with a vector - Vector3 operator*(const Vector3& vector) { - return Vector3(m[0][0]*vector.x + m[0][1]*vector.y + m[0][2]*vector.z, - m[1][0]*vector.x + m[1][1]*vector.y + m[1][2]*vector.z, - m[2][0]*vector.x + m[2][1]*vector.y + m[2][2]*vector.z); + vec3 operator*(const vec3& vector) { + return vec3(m[0][0]*vector.x() + m[0][1]*vector.y() + m[0][2]*vector.z(), + m[1][0]*vector.x() + m[1][1]*vector.y() + m[1][2]*vector.z(), + m[2][0]*vector.x() + m[2][1]*vector.y() + m[2][2]*vector.z()); } // Overloaded operator for equality condition diff --git a/tools/testbed/opengl-framework/src/maths/Matrix4.h b/tools/testbed/opengl-framework/src/maths/Matrix4.h index fb65afa..7e20e43 100644 --- a/tools/testbed/opengl-framework/src/maths/Matrix4.h +++ b/tools/testbed/opengl-framework/src/maths/Matrix4.h @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include #include @@ -70,25 +70,25 @@ class Matrix4 { } // Constructor - Matrix4(const Vector3& a1, const Vector3& a2, const Vector3& a3) { - m[0][0] = a1.x; m[0][1] = a2.x; m[0][2] = a3.x; m[0][3] = 0.f; - m[1][0] = a1.y; m[1][1] = a2.y; m[1][2] = a3.y; m[1][3] = 0.f; - m[2][0] = a1.z; m[2][1] = a2.z; m[2][2] = a3.z; m[2][3] = 0.f; + Matrix4(const vec3& a1, const vec3& a2, const vec3& a3) { + m[0][0] = a1.x(); m[0][1] = a2.x; m[0][2] = a3.x; m[0][3] = 0.f; + m[1][0] = a1.y(); m[1][1] = a2.y; m[1][2] = a3.y; m[1][3] = 0.f; + m[2][0] = a1.z(); m[2][1] = a2.z; m[2][2] = a3.z; m[2][3] = 0.f; m[3][0] = 0.f; m[3][1] = 0.f; m[3][2] = 0.f; m[3][3] = 1.f; } // Constructor Matrix4(const Vector4& a1, const Vector4& a2, const Vector4& a3) { - m[0][0] = a1.x; m[0][1] = a2.x; m[0][2] = a3.x; m[0][3] = 0.f; - m[1][0] = a1.y; m[1][1] = a2.y; m[1][2] = a3.y; m[1][3] = 0.f; - m[2][0] = a1.z; m[2][1] = a2.z; m[2][2] = a3.z; m[2][3] = 0.f; + m[0][0] = a1.x(); m[0][1] = a2.x; m[0][2] = a3.x; m[0][3] = 0.f; + m[1][0] = a1.y(); m[1][1] = a2.y; m[1][2] = a3.y; m[1][3] = 0.f; + m[2][0] = a1.z(); m[2][1] = a2.z; m[2][2] = a3.z; m[2][3] = 0.f; m[3][0] = a1.w; m[3][1] = a2.w; m[3][2] = a3.w; m[3][3] = 1.f; } // Constructor Matrix4(const Matrix4& matrix) { - setAllValues(matrix.m[0][0], matrix.m[0][1], matrix.m[0][2], matrix.m[0][3], + setValue(matrix.m[0][0], matrix.m[0][1], matrix.m[0][2], matrix.m[0][3], matrix.m[1][0], matrix.m[1][1], matrix.m[1][2], matrix.m[1][3], matrix.m[2][0], matrix.m[2][1], matrix.m[2][2], matrix.m[2][3], matrix.m[3][0], matrix.m[3][1], matrix.m[3][2], matrix.m[3][3]); @@ -131,7 +131,7 @@ class Matrix4 { // = operator Matrix4& operator=(const Matrix4& matrix) { if (&matrix != this) { - setAllValues(matrix.m[0][0], matrix.m[0][1], matrix.m[0][2], matrix.m[0][3], + setValue(matrix.m[0][0], matrix.m[0][1], matrix.m[0][2], matrix.m[0][3], matrix.m[1][0], matrix.m[1][1], matrix.m[1][2], matrix.m[1][3], matrix.m[2][0], matrix.m[2][1], matrix.m[2][2], matrix.m[2][3], matrix.m[3][0], matrix.m[3][1], matrix.m[3][2], matrix.m[3][3]); @@ -163,20 +163,20 @@ class Matrix4 { } // * operator - Vector3 operator*(const Vector3 &v) const { - Vector3 u =Vector3(m[0][0]*v.x + m[0][1]*v.y + m[0][2]*v.z + m[0][3], - m[1][0]*v.x + m[1][1]*v.y + m[1][2]*v.z + m[1][3], - m[2][0]*v.x + m[2][1]*v.y + m[2][2]*v.z + m[2][3]); - float w = m[3][0]*v.x + m[3][1]*v.y + m[3][2]*v.z + m[3][3]; + vec3 operator*(const vec3 &v) const { + vec3 u =vec3(m[0][0]*v.x() + m[0][1]*v.y() + m[0][2]*v.z() + m[0][3], + m[1][0]*v.x() + m[1][1]*v.y() + m[1][2]*v.z() + m[1][3], + m[2][0]*v.x() + m[2][1]*v.y() + m[2][2]*v.z() + m[2][3]); + float w = m[3][0]*v.x() + m[3][1]*v.y() + m[3][2]*v.z() + m[3][3]; return u/w; } // * operator Vector4 operator*(const Vector4 &v) const { - Vector4 u = Vector4(m[0][0]*v.x + m[0][1]*v.y + m[0][2]*v.z + v.w*m[0][3], - m[1][0]*v.x + m[1][1]*v.y + m[1][2]*v.z + v.w*m[1][3], - m[2][0]*v.x + m[2][1]*v.y + m[2][2]*v.z + v.w*m[2][3], - m[3][0]*v.x + m[3][1]*v.y + m[3][2]*v.z + v.w*m[3][3]); + Vector4 u = Vector4(m[0][0]*v.x() + m[0][1]*v.y() + m[0][2]*v.z() + v.w*m[0][3], + m[1][0]*v.x() + m[1][1]*v.y() + m[1][2]*v.z() + v.w*m[1][3], + m[2][0]*v.x() + m[2][1]*v.y() + m[2][2]*v.z() + v.w*m[2][3], + m[3][0]*v.x() + m[3][1]*v.y() + m[3][2]*v.z() + v.w*m[3][3]); if(u.w != 0) return u/u.w; else @@ -321,7 +321,7 @@ class Matrix4 { } // Method to set all the values in the matrix - void setAllValues(float a1, float a2, float a3, float a4, + void setValue(float a1, float a2, float a3, float a4, float b1, float b2, float b3, float b4, float c1, float c2, float c3, float c4, float d1, float d2, float d3, float d4) { @@ -373,10 +373,10 @@ class Matrix4 { } // Return a 4x4 translation matrix - static Matrix4 translationMatrix(const Vector3& v); + static Matrix4 translationMatrix(const vec3& v); // Return a 4x4 rotation matrix - static Matrix4 rotationMatrix(const Vector3& axis, float angle); + static Matrix4 rotationMatrix(const vec3& axis, float angle); // Return a 4x4 perspective projection matrix static Matrix4 perspectiveProjectionMatrix(float near, float far, @@ -397,34 +397,34 @@ inline Matrix4 operator*(float f, const Matrix4 & m) { } // Return a 4x4 translation matrix -inline Matrix4 Matrix4::translationMatrix(const Vector3& v) { - return Matrix4(1, 0, 0, v.x, - 0, 1, 0, v.y, - 0, 0, 1, v.z, +inline Matrix4 Matrix4::translationMatrix(const vec3& v) { + return Matrix4(1, 0, 0, v.x(), + 0, 1, 0, v.y(), + 0, 0, 1, v.z(), 0, 0, 0, 1); } // Return a 4x4 rotation matrix -inline Matrix4 Matrix4::rotationMatrix(const Vector3& axis, float angle) { +inline Matrix4 Matrix4::rotationMatrix(const vec3& axis, float angle) { float cosA = cos(angle); float sinA = sin(angle); Matrix4 rotationMatrix; rotationMatrix.setToIdentity(); - rotationMatrix.m[0][0] = cosA + (1-cosA) * axis.x * axis.x; - rotationMatrix.m[0][1] = (1-cosA) * axis.x * axis.y - axis.z * sinA; - rotationMatrix.m[0][2] = (1-cosA) * axis.x * axis.z + axis.y * sinA; + rotationMatrix.m[0][0] = cosA + (1-cosA) * axis.x() * axis.x; + rotationMatrix.m[0][1] = (1-cosA) * axis.x() * axis.y() - axis.z() * sinA; + rotationMatrix.m[0][2] = (1-cosA) * axis.x() * axis.z() + axis.y() * sinA; rotationMatrix.m[0][3] = 0.f; - rotationMatrix.m[1][0] = (1-cosA) * axis.x * axis.y + axis.z * sinA; - rotationMatrix.m[1][1] = cosA + (1-cosA) * axis.y * axis.y; - rotationMatrix.m[1][2] = (1-cosA) * axis.y * axis.z - axis.x * sinA; + rotationMatrix.m[1][0] = (1-cosA) * axis.x() * axis.y() + axis.z() * sinA; + rotationMatrix.m[1][1] = cosA + (1-cosA) * axis.y() * axis.y; + rotationMatrix.m[1][2] = (1-cosA) * axis.y() * axis.z() - axis.x() * sinA; rotationMatrix.m[1][3] = 0.f; - rotationMatrix.m[2][0] = (1-cosA) * axis.x * axis.z - axis.y * sinA; - rotationMatrix.m[2][1] = (1-cosA) * axis.y * axis.z + axis.x * sinA; - rotationMatrix.m[2][2] = cosA + (1-cosA) * axis.z * axis.z; + rotationMatrix.m[2][0] = (1-cosA) * axis.x() * axis.z() - axis.y() * sinA; + rotationMatrix.m[2][1] = (1-cosA) * axis.y() * axis.z() + axis.x() * sinA; + rotationMatrix.m[2][2] = cosA + (1-cosA) * axis.z() * axis.z; rotationMatrix.m[2][3] = 0.f; rotationMatrix.m[3][0] = 0.f; diff --git a/tools/testbed/opengl-framework/src/maths/Vector2.h b/tools/testbed/opengl-framework/src/maths/Vector2.h index d86fbad..a01e738 100644 --- a/tools/testbed/opengl-framework/src/maths/Vector2.h +++ b/tools/testbed/opengl-framework/src/maths/Vector2.h @@ -32,9 +32,9 @@ namespace openglframework { -// Class Vector2 +// Class vec2 // This class represents a 2D vector. -class Vector2 { +class vec2 { public: @@ -47,67 +47,67 @@ class Vector2 { // -------------------- Methods -------------------- // // Constructor - Vector2(float x=0, float y=0) : x(x), y(y) {} + vec2(float x=0, float y=0) : x(x), y(y) {} // Constructor - Vector2(const Vector2& vector) : x(vector.x), y(vector.y) {} + vec2(const vec2& vector) : x(vector.x()), y(vector.y()) {} // + operator - Vector2 operator+(const Vector2 &v) const { - return Vector2(x + v.x, y + v.y); + vec2 operator+(const vec2 &v) const { + return vec2(x + v.x(), y + v.y()); } // += operator - Vector2& operator+=(const Vector2 &v) { - x += v.x; y += v.y; + vec2& operator+=(const vec2 &v) { + x += v.x(); y += v.y(); return *this; } // - operator - Vector2 operator-(const Vector2 &v) const { - return Vector2(x - v.x, y - v.y); + vec2 operator-(const vec2 &v) const { + return vec2(x - v.x(), y - v.y()); } // -= operator - Vector2& operator-=(const Vector2 &v) { - x -= v.x; y -= v.y; + vec2& operator-=(const vec2 &v) { + x -= v.x(); y -= v.y(); return *this; } // = operator - Vector2& operator=(const Vector2& vector) { + vec2& operator=(const vec2& vector) { if (&vector != this) { - x = vector.x; - y = vector.y; + x = vector.x(); + y = vector.y(); } return *this; } // == operator - bool operator==(const Vector2 &v) const { - return x == v.x && y == v.y; + bool operator==(const vec2 &v) const { + return x == v.x() && y == v.y(); } // * operator - Vector2 operator*(float f) const { - return Vector2(f*x, f*y); + vec2 operator*(float f) const { + return vec2(f*x, f*y); } // *= operator - Vector2 &operator*=(float f) { + vec2 &operator*=(float f) { x *= f; y *= f; return *this; } // / operator - Vector2 operator/(float f) const { + vec2 operator/(float f) const { assert(f!=0); float inv = 1.f / f; - return Vector2(x * inv, y * inv); + return vec2(x * inv, y * inv); } // /= operator - Vector2 &operator/=(float f) { + vec2 &operator/=(float f) { assert(f!=0); float inv = 1.f / f; x *= inv; y *= inv; @@ -115,8 +115,8 @@ class Vector2 { } // - operator - Vector2 operator-() const { - return Vector2(-x, -y); + vec2 operator-() const { + return vec2(-x, -y); } // [] operator @@ -130,7 +130,7 @@ class Vector2 { } // Normalize the vector and return it - Vector2 normalize() { + vec2 normalize() { float l = length(); assert(l > 0); x /= l; @@ -139,7 +139,7 @@ class Vector2 { } // Clamp the vector values between 0 and 1 - Vector2 clamp01() { + vec2 clamp01() { if (x>1.f) x=1.f; else if (x<0.f) x=0.f; if (y>1.f) y=1.f; @@ -148,10 +148,10 @@ class Vector2 { } // Return the squared length of the vector - float lengthSquared() const { return x*x + y*y; } + float length2d() const { return x*x + y*y; } // Return the length of the vector - float length() const { return sqrt(lengthSquared()); } + float length() const { return sqrt(length2d()); } }; } diff --git a/tools/testbed/opengl-framework/src/maths/Vector3.h b/tools/testbed/opengl-framework/src/maths/Vector3.h index a96db54..2a4d3cc 100644 --- a/tools/testbed/opengl-framework/src/maths/Vector3.h +++ b/tools/testbed/opengl-framework/src/maths/Vector3.h @@ -33,9 +33,9 @@ namespace openglframework { -// Class Vector3 +// Class vec3 // This class represents a 3D vector. -class Vector3 { +class vec3 { public: @@ -47,76 +47,76 @@ class Vector3 { // -------------------- Methods -------------------- // // Constructor - Vector3(float x=0, float y=0, float z=0) : x(x), y(y), z(z) {} + vec3(float x=0, float y=0, float z=0) : x(x), y(y), z(z) {} // Constructor - Vector3(const Vector3& vector) : x(vector.x), y(vector.y), z(vector.z) {} + vec3(const vec3& vector) : x(vector.x()), y(vector.y()), z(vector.z()) {} // Constructor - ~Vector3() {} + ~vec3() {} // = operator - Vector3& operator=(const Vector3& vector) { + vec3& operator=(const vec3& vector) { if (&vector != this) { - x = vector.x; - y = vector.y; - z = vector.z; + x = vector.x(); + y = vector.y(); + z = vector.z(); } return *this; } // + operator - Vector3 operator+(const Vector3 &v) const { - return Vector3(x + v.x, y + v.y, z + v.z); + vec3 operator+(const vec3 &v) const { + return vec3(x + v.x(), y + v.y(), z + v.z()); } // += operator - Vector3& operator+=(const Vector3 &v) { - x += v.x; y += v.y; z += v.z; + vec3& operator+=(const vec3 &v) { + x += v.x(); y += v.y(); z += v.z(); return *this; } // - operator - Vector3 operator-(const Vector3 &v) const { - return Vector3(x - v.x, y - v.y, z - v.z); + vec3 operator-(const vec3 &v) const { + return vec3(x - v.x(), y - v.y(), z - v.z()); } // -= operator - Vector3& operator-=(const Vector3 &v) { - x -= v.x; y -= v.y; z -= v.z; + vec3& operator-=(const vec3 &v) { + x -= v.x(); y -= v.y(); z -= v.z(); return *this; } // == operator - bool operator==(const Vector3 &v) const { - return x == v.x && y == v.y && z == v.z; + bool operator==(const vec3 &v) const { + return x == v.x() && y == v.y() && z == v.z(); } // != operator - bool operator!=(const Vector3 &v) const { + bool operator!=(const vec3 &v) const { return !( *this == v ); } // * operator - Vector3 operator*(float f) const { - return Vector3(f*x, f*y, f*z); + vec3 operator*(float f) const { + return vec3(f*x, f*y, f*z); } // *= operator - Vector3 &operator*=(float f) { + vec3 &operator*=(float f) { x *= f; y *= f; z *= f; return *this; } // / operator - Vector3 operator/(float f) const { + vec3 operator/(float f) const { assert(f > std::numeric_limits::epsilon() ); float inv = 1.f / f; - return Vector3(x * inv, y * inv, z * inv); + return vec3(x * inv, y * inv, z * inv); } // /= operator - Vector3 &operator/=(float f) { + vec3 &operator/=(float f) { assert(f > std::numeric_limits::epsilon()); float inv = 1.f / f; x *= inv; y *= inv; z *= inv; @@ -124,8 +124,8 @@ class Vector3 { } // - operator - Vector3 operator-() const { - return Vector3(-x, -y, -z); + vec3 operator-() const { + return vec3(-x, -y, -z); } // [] operator @@ -151,17 +151,17 @@ class Vector3 { } // Cross product operator - Vector3 cross(const Vector3 &v) const{ - return Vector3(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x); + vec3 cross(const vec3 &v) const{ + return vec3(y * v.z() - z * v.y(), z * v.x() - x * v.z, x * v.y - y * v.x); } // Dot product operator - float dot(const Vector3 &v) const{ - return x * v.x + y * v.y + z * v.z; + float dot(const vec3 &v) const{ + return x * v.x() + y * v.y() + z * v.z(); } // Normalize the vector and return it - Vector3 normalize() { + vec3 normalize() { float l = length(); if(l < std::numeric_limits::epsilon() ) { return *this; @@ -177,7 +177,7 @@ class Vector3 { } // Clamp the values between 0 and 1 - Vector3 clamp01() { + vec3 clamp01() { if (x>1.f) x=1.f; else if (x<0.f) x=0.f; if (y>1.f) y=1.f; @@ -188,13 +188,13 @@ class Vector3 { } // Return the squared length of the vector - float lengthSquared() const { return x*x + y*y + z*z; } + float length2d() const { return x*x + y*y + z*z; } // Return the length of the vector - float length() const { return sqrt(lengthSquared()); } + float length() const { return sqrt(length2d()); } }; -inline Vector3 operator*(float f, const Vector3 & o) { +inline vec3 operator*(float f, const vec3 & o) { return o*f; } diff --git a/tools/testbed/opengl-framework/src/maths/Vector4.h b/tools/testbed/opengl-framework/src/maths/Vector4.h index fe58033..9480182 100644 --- a/tools/testbed/opengl-framework/src/maths/Vector4.h +++ b/tools/testbed/opengl-framework/src/maths/Vector4.h @@ -49,36 +49,36 @@ class Vector4 { Vector4(float x=0, float y=0, float z=0, float w=0) : x(x), y(y), z(z), w(w) {} // Constructor - Vector4(const Vector4& vector) : x(vector.x), y(vector.y), z(vector.z), w(vector.w) {} + Vector4(const Vector4& vector) : x(vector.x()), y(vector.y()), z(vector.z()), w(vector.w) {} // + operator Vector4 operator+(const Vector4 &v) const { - return Vector4(x + v.x, y + v.y, z + v.z, w + v.w); + return Vector4(x + v.x(), y + v.y(), z + v.z(), w + v.w); } // += operator Vector4& operator+=(const Vector4 &v) { - x += v.x; y += v.y; z += v.z; w += v.w; + x += v.x(); y += v.y(); z += v.z(); w += v.w; return *this; } // - operator Vector4 operator-(const Vector4 &v) const { - return Vector4(x - v.x, y - v.y, z - v.z, w - v.w); + return Vector4(x - v.x(), y - v.y(), z - v.z(), w - v.w); } // -= operator Vector4& operator-=(const Vector4 &v) { - x -= v.x; y -= v.y; z -= v.z, w -=v.w; + x -= v.x(); y -= v.y(); z -= v.z(), w -=v.w; return *this; } // = operator Vector4& operator=(const Vector4& vector) { if (&vector != this) { - x = vector.x; - y = vector.y; - z = vector.z; + x = vector.x(); + y = vector.y(); + z = vector.z(); w = vector.w; } return *this; @@ -86,7 +86,7 @@ class Vector4 { // == operator bool operator==(const Vector4 &v) const { - return x == v.x && y == v.y && z == v.z && w == v.w; + return x == v.x() && y == v.y() && z == v.z() && w == v.w; } // * operator @@ -134,12 +134,12 @@ class Vector4 { // Dot product operator float dot(const Vector4 &v) const { - return x * v.x + y * v.y + z * v.z + w * v.w; + return x * v.x() + y * v.y() + z * v.z() + w * v.w; } // Multiply two vectors by their components Vector4 componentMul(const Vector4 &v) const { - return Vector4(x * v.x, y * v.y, z * v.z, w * v.w); + return Vector4(x * v.x(), y * v.y(), z * v.z(), w * v.w); } // Clamp the values between 0 and 1 @@ -156,10 +156,10 @@ class Vector4 { } // Return the squared length of the vector - float lengthSquared() const { return x * x + y * y + z * z + w * w; } + float length2d() const { return x * x + y * y + z * z + w * w; } // Return the length of the vector - float length() const { return sqrt(lengthSquared()); } + float length() const { return sqrt(length2d()); } }; } diff --git a/tools/testbed/opengl-framework/src/openglframework.h b/tools/testbed/opengl-framework/src/openglframework.h index 640b85a..aeae72a 100644 --- a/tools/testbed/opengl-framework/src/openglframework.h +++ b/tools/testbed/opengl-framework/src/openglframework.h @@ -39,8 +39,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include diff --git a/tools/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/tools/testbed/scenes/collisionshapes/CollisionShapesScene.cpp index fa674a6..1c52e90 100644 --- a/tools/testbed/scenes/collisionshapes/CollisionShapesScene.cpp +++ b/tools/testbed/scenes/collisionshapes/CollisionShapesScene.cpp @@ -37,13 +37,13 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) std::string meshFolderPath("meshes/"); // Compute the radius and the center of the scene - openglframework::Vector3 center(0, 5, 0); + openglframework::vec3 center(0, 5, 0); // Set the center of the scene setScenePosition(center, SCENE_RADIUS); // Gravity vector in the dynamics world - rp3d::Vector3 gravity(0, -9.81, 0); + rp3d::vec3 gravity(0, -9.81, 0); // Create the dynamics world for the physics simulation mDynamicsWorld = new rp3d::DynamicsWorld(gravity); @@ -57,7 +57,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Position float angle = i * 30.0f; - openglframework::Vector3 position(radius * cos(angle), + openglframework::vec3 position(radius * cos(angle), 100 + i * (DUMBBELL_HEIGHT + 0.3f), radius * sin(angle)); @@ -81,8 +81,8 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Position float angle = i * 30.0f; - openglframework::Vector3 position(radius * cos(angle), - 60 + i * (BOX_SIZE.y + 0.8f), + openglframework::vec3 position(radius * cos(angle), + 60 + i * (BOX_SIZE.y() + 0.8f), radius * sin(angle)); // Create a sphere and a corresponding rigid in the dynamics world @@ -105,7 +105,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Position float angle = i * 35.0f; - openglframework::Vector3 position(radius * cos(angle), + openglframework::vec3 position(radius * cos(angle), 50 + i * (SPHERE_RADIUS + 0.8f), radius * sin(angle)); @@ -133,7 +133,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Position float angle = i * 50.0f; - openglframework::Vector3 position(radius * cos(angle), + openglframework::vec3 position(radius * cos(angle), 35 + i * (CONE_HEIGHT + 0.3f), radius * sin(angle)); @@ -161,7 +161,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Position float angle = i * 35.0f; - openglframework::Vector3 position(radius * cos(angle), + openglframework::vec3 position(radius * cos(angle), 25 + i * (CYLINDER_HEIGHT + 0.3f), radius * sin(angle)); @@ -189,7 +189,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Position float angle = i * 45.0f; - openglframework::Vector3 position(radius * cos(angle), + openglframework::vec3 position(radius * cos(angle), 15 + i * (CAPSULE_HEIGHT + 0.3f), radius * sin(angle)); @@ -216,7 +216,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Position float angle = i * 30.0f; - openglframework::Vector3 position(radius * cos(angle), + openglframework::vec3 position(radius * cos(angle), 5 + i * (CAPSULE_HEIGHT + 0.3f), radius * sin(angle)); @@ -237,7 +237,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // ---------- Create the floor --------- - openglframework::Vector3 floorPosition(0, 0, 0); + openglframework::vec3 floorPosition(0, 0, 0); mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); // Set the box color @@ -255,7 +255,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) /* // Position - openglframework::Vector3 position(0, 0, 0); + openglframework::vec3 position(0, 0, 0); rp3d::float mass = 1.0; // Create a convex mesh and a corresponding rigid in the dynamics world @@ -276,8 +276,8 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Get the physics engine parameters mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); - mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); + rp3d::vec3 gravityVector = mDynamicsWorld->getGravity(); + mEngineSettings.gravity = openglframework::vec3(gravityVector.x(), gravityVector.y(), gravityVector.z()); mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); @@ -380,8 +380,8 @@ void CollisionShapesScene::updatePhysics() { // Update the physics engine parameters mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); + rp3d::vec3 gravity(mEngineSettings.gravity.x(), mEngineSettings.gravity.y(), + mEngineSettings.gravity.z()); mDynamicsWorld->setGravity(gravity); mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); @@ -403,35 +403,35 @@ void CollisionShapesScene::update() { for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); + (*it)->updateetk::Transform3D(mInterpolationFactor); } // Update the position and orientation of the sphere for (std::vector::iterator it = mSpheres.begin(); it != mSpheres.end(); ++it) { // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); + (*it)->updateetk::Transform3D(mInterpolationFactor); } // Update the position and orientation of the cones for (std::vector::iterator it = mCones.begin(); it != mCones.end(); ++it) { // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); + (*it)->updateetk::Transform3D(mInterpolationFactor); } // Update the position and orientation of the cylinders for (std::vector::iterator it = mCylinders.begin(); it != mCylinders.end(); ++it) { // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); + (*it)->updateetk::Transform3D(mInterpolationFactor); } // Update the position and orientation of the capsules for (std::vector::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) { // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); + (*it)->updateetk::Transform3D(mInterpolationFactor); } // Update the position and orientation of the convex meshes @@ -439,7 +439,7 @@ void CollisionShapesScene::update() { it != mConvexMeshes.end(); ++it) { // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); + (*it)->updateetk::Transform3D(mInterpolationFactor); } // Update the position and orientation of the dumbbells @@ -447,12 +447,12 @@ void CollisionShapesScene::update() { it != mDumbbells.end(); ++it) { // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); + (*it)->updateetk::Transform3D(mInterpolationFactor); } - //mConcaveMesh->updateTransform(mInterpolationFactor); + //mConcaveMesh->updateetk::Transform3D(mInterpolationFactor); - mFloor->updateTransform(mInterpolationFactor); + mFloor->updateetk::Transform3D(mInterpolationFactor); } // Render the scene @@ -517,14 +517,14 @@ void CollisionShapesScene::reset() { // Position float angle = i * 30.0f; - openglframework::Vector3 position(radius * cos(angle), + openglframework::vec3 position(radius * cos(angle), 100 + i * (DUMBBELL_HEIGHT + 0.3f), radius * sin(angle)); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); // Reset the transform mDumbbells[i]->resetTransform(transform); @@ -535,14 +535,14 @@ void CollisionShapesScene::reset() { // Position float angle = i * 30.0f; - openglframework::Vector3 position(radius * cos(angle), - 60 + i * (BOX_SIZE.y + 0.8f), + openglframework::vec3 position(radius * cos(angle), + 60 + i * (BOX_SIZE.y() + 0.8f), radius * sin(angle)); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); // Reset the transform mBoxes[i]->resetTransform(transform); @@ -553,14 +553,14 @@ void CollisionShapesScene::reset() { // Position float angle = i * 35.0f; - openglframework::Vector3 position(radius * cos(angle), + openglframework::vec3 position(radius * cos(angle), 50 + i * (SPHERE_RADIUS + 0.8f), radius * sin(angle)); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); // Reset the transform mSpheres[i]->resetTransform(transform); @@ -571,14 +571,14 @@ void CollisionShapesScene::reset() { // Position float angle = i * 50.0f; - openglframework::Vector3 position(radius * cos(angle), + openglframework::vec3 position(radius * cos(angle), 35 + i * (CONE_HEIGHT + 0.3f), radius * sin(angle)); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); // Reset the transform mCones[i]->resetTransform(transform); @@ -589,14 +589,14 @@ void CollisionShapesScene::reset() { // Position float angle = i * 35.0f; - openglframework::Vector3 position(radius * cos(angle), + openglframework::vec3 position(radius * cos(angle), 25 + i * (CYLINDER_HEIGHT + 0.3f), radius * sin(angle)); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); // Reset the transform mCylinders[i]->resetTransform(transform); @@ -607,14 +607,14 @@ void CollisionShapesScene::reset() { // Position float angle = i * 45.0f; - openglframework::Vector3 position(radius * cos(angle), + openglframework::vec3 position(radius * cos(angle), 15 + i * (CAPSULE_HEIGHT + 0.3f), radius * sin(angle)); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); // Reset the transform mCapsules[i]->resetTransform(transform); @@ -625,14 +625,14 @@ void CollisionShapesScene::reset() { // Position float angle = i * 30.0f; - openglframework::Vector3 position(radius * cos(angle), + openglframework::vec3 position(radius * cos(angle), 5 + i * (CAPSULE_HEIGHT + 0.3f), radius * sin(angle)); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); // Reset the transform mConvexMeshes[i]->resetTransform(transform); diff --git a/tools/testbed/scenes/collisionshapes/CollisionShapesScene.h b/tools/testbed/scenes/collisionshapes/CollisionShapesScene.h index 4e47e53..c289e1f 100644 --- a/tools/testbed/scenes/collisionshapes/CollisionShapesScene.h +++ b/tools/testbed/scenes/collisionshapes/CollisionShapesScene.h @@ -51,7 +51,7 @@ const int32_t NB_CYLINDERS = 5; const int32_t NB_CAPSULES = 5; const int32_t NB_MESHES = 3; const int32_t NB_COMPOUND_SHAPES = 3; -const openglframework::Vector3 BOX_SIZE(2, 2, 2); +const openglframework::vec3 BOX_SIZE(2, 2, 2); const float SPHERE_RADIUS = 1.5f; const float CONE_RADIUS = 2.0f; const float CONE_HEIGHT = 3.0f; @@ -60,7 +60,7 @@ const float CYLINDER_HEIGHT = 5.0f; const float CAPSULE_RADIUS = 1.0f; const float CAPSULE_HEIGHT = 1.0f; const float DUMBBELL_HEIGHT = 1.0f; -const openglframework::Vector3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters +const openglframework::vec3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters const float BOX_MASS = 1.0f; const float CONE_MASS = 1.0f; const float CYLINDER_MASS = 1.0f; diff --git a/tools/testbed/scenes/concavemesh/ConcaveMeshScene.cpp b/tools/testbed/scenes/concavemesh/ConcaveMeshScene.cpp index 67e9d28..8903c7f 100644 --- a/tools/testbed/scenes/concavemesh/ConcaveMeshScene.cpp +++ b/tools/testbed/scenes/concavemesh/ConcaveMeshScene.cpp @@ -37,13 +37,13 @@ ConcaveMeshScene::ConcaveMeshScene(const std::string& name) std::string meshFolderPath("meshes/"); // Compute the radius and the center of the scene - openglframework::Vector3 center(0, 5, 0); + openglframework::vec3 center(0, 5, 0); // Set the center of the scene setScenePosition(center, SCENE_RADIUS); // Gravity vector in the dynamics world - rp3d::Vector3 gravity(0, rp3d::float(-9.81), 0); + rp3d::vec3 gravity(0, rp3d::float(-9.81), 0); // Create the dynamics world for the physics simulation mDynamicsWorld = new rp3d::DynamicsWorld(gravity); @@ -58,10 +58,10 @@ ConcaveMeshScene::ConcaveMeshScene(const std::string& name) for (int32_t j=0; jsetColor(mDemoColors[0]); @@ -76,7 +76,7 @@ ConcaveMeshScene::ConcaveMeshScene(const std::string& name) // ---------- Create the triangular mesh ---------- // // Position - openglframework::Vector3 position(0, 0, 0); + openglframework::vec3 position(0, 0, 0); rp3d::float mass = 1.0; // Create a convex mesh and a corresponding rigid in the dynamics world @@ -96,8 +96,8 @@ ConcaveMeshScene::ConcaveMeshScene(const std::string& name) // Get the physics engine parameters mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); - mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); + rp3d::vec3 gravityVector = mDynamicsWorld->getGravity(); + mEngineSettings.gravity = openglframework::vec3(gravityVector.x(), gravityVector.y(), gravityVector.z()); mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); @@ -130,8 +130,8 @@ void ConcaveMeshScene::updatePhysics() { // Update the physics engine parameters mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); + rp3d::vec3 gravity(mEngineSettings.gravity.x(), mEngineSettings.gravity.y(), + mEngineSettings.gravity.z()); mDynamicsWorld->setGravity(gravity); mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); @@ -150,10 +150,10 @@ void ConcaveMeshScene::update() { SceneDemo::update(); // Update the transform used for the rendering - mConcaveMesh->updateTransform(mInterpolationFactor); + mConcaveMesh->updateetk::Transform3D(mInterpolationFactor); for (int32_t i=0; iupdateTransform(mInterpolationFactor); + mBoxes[i]->updateetk::Transform3D(mInterpolationFactor); } } @@ -177,16 +177,16 @@ void ConcaveMeshScene::renderSinglePass(Shader& shader, const openglframework::M void ConcaveMeshScene::reset() { // Reset the transform - rp3d::Transform transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity()); + rp3d::etk::Transform3D transform(rp3d::vec3::zero(), rp3d::etk::Quaternion::identity()); mConcaveMesh->resetTransform(transform); for (int32_t i=0; iresetTransform(boxTransform); } } diff --git a/tools/testbed/scenes/cubes/CubesScene.cpp b/tools/testbed/scenes/cubes/CubesScene.cpp index 1a4e6a5..0cf1e4f 100644 --- a/tools/testbed/scenes/cubes/CubesScene.cpp +++ b/tools/testbed/scenes/cubes/CubesScene.cpp @@ -35,13 +35,13 @@ CubesScene::CubesScene(const std::string& name) : SceneDemo(name, SCENE_RADIUS) { // Compute the radius and the center of the scene - openglframework::Vector3 center(0, 5, 0); + openglframework::vec3 center(0, 5, 0); // Set the center of the scene setScenePosition(center, SCENE_RADIUS); // Gravity vector in the dynamics world - rp3d::Vector3 gravity(0, rp3d::float(-9.81), 0); + rp3d::vec3 gravity(0, rp3d::float(-9.81), 0); // Create the dynamics world for the physics simulation m_dynamicsWorld = new rp3d::DynamicsWorld(gravity); @@ -56,8 +56,8 @@ CubesScene::CubesScene(const std::string& name) // Position of the cubes float angle = i * 30.0f; - openglframework::Vector3 position(radius * cos(angle), - 30 + i * (BOX_SIZE.y + 0.3f), + openglframework::vec3 position(radius * cos(angle), + 30 + i * (BOX_SIZE.y() + 0.3f), 0); // Create a cube and a corresponding rigid in the dynamics world @@ -76,7 +76,7 @@ CubesScene::CubesScene(const std::string& name) } // Create the floor - openglframework::Vector3 floorPosition(0, 0, 0); + openglframework::vec3 floorPosition(0, 0, 0); mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, m_dynamicsWorld); mFloor->setColor(mGreyColorDemo); mFloor->setSleepingColor(mGreyColorDemo); @@ -90,8 +90,8 @@ CubesScene::CubesScene(const std::string& name) // Get the physics engine parameters mEngineSettings.isGravityEnabled = m_dynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = m_dynamicsWorld->getGravity(); - mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); + rp3d::vec3 gravityVector = m_dynamicsWorld->getGravity(); + mEngineSettings.gravity = openglframework::vec3(gravityVector.x(), gravityVector.y(), gravityVector.z()); mEngineSettings.isSleepingEnabled = m_dynamicsWorld->isSleepingEnabled(); mEngineSettings.sleepLinearVelocity = m_dynamicsWorld->getSleepLinearVelocity(); mEngineSettings.sleepAngularVelocity = m_dynamicsWorld->getSleepAngularVelocity(); @@ -128,8 +128,8 @@ void CubesScene::updatePhysics() { // Update the physics engine parameters m_dynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); + rp3d::vec3 gravity(mEngineSettings.gravity.x(), mEngineSettings.gravity.y(), + mEngineSettings.gravity.z()); m_dynamicsWorld->setGravity(gravity); m_dynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); m_dynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); @@ -151,10 +151,10 @@ void CubesScene::update() { for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); + (*it)->updateetk::Transform3D(mInterpolationFactor); } - mFloor->updateTransform(mInterpolationFactor); + mFloor->updateetk::Transform3D(mInterpolationFactor); } // Render the scene in a single pass @@ -184,14 +184,14 @@ void CubesScene::reset() { // Position of the cubes float angle = i * 30.0f; - openglframework::Vector3 position(radius * cos(angle), - 10 + i * (BOX_SIZE.y + 0.3f), + openglframework::vec3 position(radius * cos(angle), + 10 + i * (BOX_SIZE.y() + 0.3f), 0); // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + rp3d::vec3 initPosition(position.x(), position.y(), position.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transform(initPosition, initOrientation); mBoxes[i]->resetTransform(transform); } diff --git a/tools/testbed/scenes/cubes/CubesScene.h b/tools/testbed/scenes/cubes/CubesScene.h index 28bbd50..dd7443a 100644 --- a/tools/testbed/scenes/cubes/CubesScene.h +++ b/tools/testbed/scenes/cubes/CubesScene.h @@ -36,8 +36,8 @@ namespace cubesscene { // Constants const float SCENE_RADIUS = 30.0f; // Radius of the scene in meters const int32_t NB_CUBES = 30; // Number of boxes in the scene - const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters - const openglframework::Vector3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters + const openglframework::vec3 BOX_SIZE(2, 2, 2); // Box dimensions in meters + const openglframework::vec3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters const float BOX_MASS = 1.0f; // Box mass in kilograms const float FLOOR_MASS = 100.0f; // Floor mass in kilograms // Class CubesScene diff --git a/tools/testbed/scenes/heightfield/HeightFieldScene.cpp b/tools/testbed/scenes/heightfield/HeightFieldScene.cpp index 713a1d1..fb1e4af 100644 --- a/tools/testbed/scenes/heightfield/HeightFieldScene.cpp +++ b/tools/testbed/scenes/heightfield/HeightFieldScene.cpp @@ -34,13 +34,13 @@ using namespace heightfieldscene; HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SCENE_RADIUS) { // Compute the radius and the center of the scene - openglframework::Vector3 center(0, 5, 0); + openglframework::vec3 center(0, 5, 0); // Set the center of the scene setScenePosition(center, SCENE_RADIUS); // Gravity vector in the dynamics world - rp3d::Vector3 gravity(0, rp3d::float(-9.81), 0); + rp3d::vec3 gravity(0, rp3d::float(-9.81), 0); // Create the dynamics world for the physics simulation mDynamicsWorld = new rp3d::DynamicsWorld(gravity); @@ -54,10 +54,10 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC for (int32_t i=0; isetColor(mDemoColors[2]); @@ -71,7 +71,7 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC // ---------- Create the height field ---------- // // Position - openglframework::Vector3 position(0, 0, 0); + openglframework::vec3 position(0, 0, 0); rp3d::float mass = 1.0; // Create a convex mesh and a corresponding rigid in the dynamics world @@ -91,8 +91,8 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC // Get the physics engine parameters mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); - mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); + rp3d::vec3 gravityVector = mDynamicsWorld->getGravity(); + mEngineSettings.gravity = openglframework::vec3(gravityVector.x(), gravityVector.y(), gravityVector.z()); mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); @@ -126,8 +126,8 @@ void HeightFieldScene::updatePhysics() { // Update the physics engine parameters mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); + rp3d::vec3 gravity(mEngineSettings.gravity.x(), mEngineSettings.gravity.y(), + mEngineSettings.gravity.z()); mDynamicsWorld->setGravity(gravity); mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); @@ -146,10 +146,10 @@ void HeightFieldScene::update() { SceneDemo::update(); // Update the transform used for the rendering - mHeightField->updateTransform(mInterpolationFactor); + mHeightField->updateetk::Transform3D(mInterpolationFactor); for (int32_t i=0; iupdateTransform(mInterpolationFactor); + mBoxes[i]->updateetk::Transform3D(mInterpolationFactor); } } @@ -173,14 +173,14 @@ void HeightFieldScene::renderSinglePass(Shader& shader, const openglframework::M void HeightFieldScene::reset() { // Reset the transform - rp3d::Transform transform(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()); + rp3d::etk::Transform3D transform(rp3d::vec3(0, 0, 0), rp3d::etk::Quaternion::identity()); mHeightField->resetTransform(transform); float heightFieldWidth = 10.0f; float stepDist = heightFieldWidth / (NB_BOXES + 1); for (int32_t i=0; iresetTransform(boxTransform); } } diff --git a/tools/testbed/scenes/joints/JointsScene.cpp b/tools/testbed/scenes/joints/JointsScene.cpp index 94f2c42..f47da76 100644 --- a/tools/testbed/scenes/joints/JointsScene.cpp +++ b/tools/testbed/scenes/joints/JointsScene.cpp @@ -36,13 +36,13 @@ JointsScene::JointsScene(const std::string& name) : SceneDemo(name, SCENE_RADIUS) { // Compute the radius and the center of the scene - openglframework::Vector3 center(0, 5, 0); + openglframework::vec3 center(0, 5, 0); // Set the center of the scene setScenePosition(center, SCENE_RADIUS); // Gravity vector in the dynamics world - rp3d::Vector3 gravity(0, rp3d::float(-9.81), 0); + rp3d::vec3 gravity(0, rp3d::float(-9.81), 0); // Create the dynamics world for the physics simulation mDynamicsWorld = new rp3d::DynamicsWorld(gravity); @@ -67,8 +67,8 @@ JointsScene::JointsScene(const std::string& name) // Get the physics engine parameters mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); - mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); + rp3d::vec3 gravityVector = mDynamicsWorld->getGravity(); + mEngineSettings.gravity = openglframework::vec3(gravityVector.x(), gravityVector.y(), gravityVector.z()); mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); @@ -121,8 +121,8 @@ void JointsScene::updatePhysics() { // Update the physics engine parameters mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); + rp3d::vec3 gravity(mEngineSettings.gravity.x(), mEngineSettings.gravity.y(), + mEngineSettings.gravity.z()); mDynamicsWorld->setGravity(gravity); mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); @@ -145,17 +145,17 @@ void JointsScene::update() { SceneDemo::update(); // Update the position and orientation of the boxes - mSliderJointBottomBox->updateTransform(mInterpolationFactor); - mSliderJointTopBox->updateTransform(mInterpolationFactor); - mPropellerBox->updateTransform(mInterpolationFactor); - mFixedJointBox1->updateTransform(mInterpolationFactor); - mFixedJointBox2->updateTransform(mInterpolationFactor); + mSliderJointBottomBox->updateetk::Transform3D(mInterpolationFactor); + mSliderJointTopBox->updateetk::Transform3D(mInterpolationFactor); + mPropellerBox->updateetk::Transform3D(mInterpolationFactor); + mFixedJointBox1->updateetk::Transform3D(mInterpolationFactor); + mFixedJointBox2->updateetk::Transform3D(mInterpolationFactor); for (int32_t i=0; iupdateTransform(mInterpolationFactor); + mBallAndSocketJointChainBoxes[i]->updateetk::Transform3D(mInterpolationFactor); } // Update the position and orientation of the floor - mFloor->updateTransform(mInterpolationFactor); + mFloor->updateetk::Transform3D(mInterpolationFactor); } // Render the scene @@ -185,38 +185,38 @@ void JointsScene::renderSinglePass(openglframework::Shader& shader, // Reset the scene void JointsScene::reset() { - openglframework::Vector3 positionBox(0, 15, 5); - openglframework::Vector3 boxDimension(1, 1, 1); + openglframework::vec3 positionBox(0, 15, 5); + openglframework::vec3 boxDimension(1, 1, 1); for (int32_t i=0; iresetTransform(transform); - positionBox.y -= boxDimension.y + 0.5f; + positionBox.y() -= boxDimension.y + 0.5f; } // --------------- Slider Joint --------------- // // Position of the box - openglframework::Vector3 positionBox1(0, 2.1f, 0); - rp3d::Vector3 initPosition(positionBox1.x, positionBox1.y, positionBox1.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transformBottomBox(initPosition, initOrientation); + openglframework::vec3 positionBox1(0, 2.1f, 0); + rp3d::vec3 initPosition(positionBox1.x(), positionBox1.y(), positionBox1.z()); + rp3d::etk::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::etk::Transform3D transformBottomBox(initPosition, initOrientation); // Create a box and a corresponding rigid in the dynamics world mSliderJointBottomBox->resetTransform(transformBottomBox); // Position of the box - openglframework::Vector3 positionBox2(0, 4.2f, 0); - initPosition = rp3d::Vector3(positionBox2.x, positionBox2.y, positionBox2.z); - initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transformTopBox(initPosition, initOrientation); + openglframework::vec3 positionBox2(0, 4.2f, 0); + initPosition = rp3d::vec3(positionBox2.x(), positionBox2.y(), positionBox2.z()); + initOrientation = rp3d::etk::Quaternion::identity(); + rp3d::etk::Transform3D transformTopBox(initPosition, initOrientation); // Create a box and a corresponding rigid in the dynamics world mSliderJointTopBox->resetTransform(transformTopBox); @@ -224,10 +224,10 @@ void JointsScene::reset() { // --------------- Propeller Hinge joint --------------- // // Position of the box - positionBox1 = openglframework::Vector3(0, 7, 0); - initPosition = rp3d::Vector3(positionBox1.x, positionBox1.y, positionBox1.z); - initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transformHingeBox(initPosition, initOrientation); + positionBox1 = openglframework::vec3(0, 7, 0); + initPosition = rp3d::vec3(positionBox1.x(), positionBox1.y(), positionBox1.z()); + initOrientation = rp3d::etk::Quaternion::identity(); + rp3d::etk::Transform3D transformHingeBox(initPosition, initOrientation); // Create a box and a corresponding rigid in the dynamics world mPropellerBox->resetTransform(transformHingeBox); @@ -235,19 +235,19 @@ void JointsScene::reset() { // --------------- Fixed joint --------------- // // Position of the box - positionBox1 = openglframework::Vector3(5, 7, 0); - initPosition = rp3d::Vector3(positionBox1.x, positionBox1.y, positionBox1.z); - initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transformFixedBox1(initPosition, initOrientation); + positionBox1 = openglframework::vec3(5, 7, 0); + initPosition = rp3d::vec3(positionBox1.x(), positionBox1.y(), positionBox1.z()); + initOrientation = rp3d::etk::Quaternion::identity(); + rp3d::etk::Transform3D transformFixedBox1(initPosition, initOrientation); // Create a box and a corresponding rigid in the dynamics world mFixedJointBox1->resetTransform(transformFixedBox1); // Position of the box - positionBox2 = openglframework::Vector3(-5, 7, 0); - initPosition = rp3d::Vector3(positionBox2.x, positionBox2.y, positionBox2.z); - initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transformFixedBox2(initPosition, initOrientation); + positionBox2 = openglframework::vec3(-5, 7, 0); + initPosition = rp3d::vec3(positionBox2.x(), positionBox2.y(), positionBox2.z()); + initOrientation = rp3d::etk::Quaternion::identity(); + rp3d::etk::Transform3D transformFixedBox2(initPosition, initOrientation); // Create a box and a corresponding rigid in the dynamics world mFixedJointBox2->resetTransform(transformFixedBox2); @@ -258,8 +258,8 @@ void JointsScene::createBallAndSocketJoints() { // --------------- Create the boxes --------------- // - openglframework::Vector3 positionBox(0, 15, 5); - openglframework::Vector3 boxDimension(1, 1, 1); + openglframework::vec3 positionBox(0, 15, 5); + openglframework::vec3 boxDimension(1, 1, 1); const float boxMass = 0.5f; for (int32_t i=0; igetRigidBody()->getMaterial(); material.setBounciness(rp3d::float(0.4)); - positionBox.y -= boxDimension.y + 0.5f; + positionBox.y() -= boxDimension.y + 0.5f; } // --------------- Create the joints --------------- // @@ -294,9 +294,9 @@ void JointsScene::createBallAndSocketJoints() { // Create the joint info object rp3d::RigidBody* body1 = mBallAndSocketJointChainBoxes[i]->getRigidBody(); rp3d::RigidBody* body2 = mBallAndSocketJointChainBoxes[i+1]->getRigidBody(); - rp3d::Vector3 body1Position = body1->getTransform().getPosition(); - rp3d::Vector3 body2Position = body2->getTransform().getPosition(); - const rp3d::Vector3 m_anchorPointWorldSpace = 0.5 * (body1Position + body2Position); + rp3d::vec3 body1Position = body1->getTransform().getPosition(); + rp3d::vec3 body2Position = body2->getTransform().getPosition(); + const rp3d::vec3 m_anchorPointWorldSpace = 0.5 * (body1Position + body2Position); rp3d::BallAndSocketJointInfo jointInfo(body1, body2, m_anchorPointWorldSpace); // Create the joint in the dynamics world @@ -311,10 +311,10 @@ void JointsScene::createSliderJoint() { // --------------- Create the first box --------------- // // Position of the box - openglframework::Vector3 positionBox1(0, 2.1f, 0); + openglframework::vec3 positionBox1(0, 2.1f, 0); // Create a box and a corresponding rigid in the dynamics world - openglframework::Vector3 box1Dimension(2, 4, 2); + openglframework::vec3 box1Dimension(2, 4, 2); mSliderJointBottomBox = new Box(box1Dimension, positionBox1 , BOX_MASS, mDynamicsWorld); // Set the box color @@ -331,10 +331,10 @@ void JointsScene::createSliderJoint() { // --------------- Create the second box --------------- // // Position of the box - openglframework::Vector3 positionBox2(0, 4.2f, 0); + openglframework::vec3 positionBox2(0, 4.2f, 0); // Create a box and a corresponding rigid in the dynamics world - openglframework::Vector3 box2Dimension(1.5f, 4, 1.5f); + openglframework::vec3 box2Dimension(1.5f, 4, 1.5f); mSliderJointTopBox = new Box(box2Dimension, positionBox2, BOX_MASS, mDynamicsWorld); // Set the box color @@ -350,10 +350,10 @@ void JointsScene::createSliderJoint() { // Create the joint info object rp3d::RigidBody* body1 = mSliderJointBottomBox->getRigidBody(); rp3d::RigidBody* body2 = mSliderJointTopBox->getRigidBody(); - const rp3d::Vector3& body1Position = body1->getTransform().getPosition(); - const rp3d::Vector3& body2Position = body2->getTransform().getPosition(); - const rp3d::Vector3 m_anchorPointWorldSpace = rp3d::float(0.5) * (body2Position + body1Position); - const rp3d::Vector3 sliderAxisWorldSpace = (body2Position - body1Position); + const rp3d::vec3& body1Position = body1->getTransform().getPosition(); + const rp3d::vec3& body2Position = body2->getTransform().getPosition(); + const rp3d::vec3 m_anchorPointWorldSpace = rp3d::0.5f * (body2Position + body1Position); + const rp3d::vec3 sliderAxisWorldSpace = (body2Position - body1Position); rp3d::SliderJointInfo jointInfo(body1, body2, m_anchorPointWorldSpace, sliderAxisWorldSpace, rp3d::float(-1.7), rp3d::float(1.7)); jointInfo.isMotorEnabled = true; @@ -371,10 +371,10 @@ void JointsScene::createPropellerHingeJoint() { // --------------- Create the propeller box --------------- // // Position of the box - openglframework::Vector3 positionBox1(0, 7, 0); + openglframework::vec3 positionBox1(0, 7, 0); // Create a box and a corresponding rigid in the dynamics world - openglframework::Vector3 boxDimension(10, 1, 1); + openglframework::vec3 boxDimension(10, 1, 1); mPropellerBox = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld); // Set the box color @@ -390,13 +390,13 @@ void JointsScene::createPropellerHingeJoint() { // Create the joint info object rp3d::RigidBody* body1 = mPropellerBox->getRigidBody(); rp3d::RigidBody* body2 = mSliderJointTopBox->getRigidBody(); - const rp3d::Vector3& body1Position = body1->getTransform().getPosition(); - const rp3d::Vector3& body2Position = body2->getTransform().getPosition(); - const rp3d::Vector3 m_anchorPointWorldSpace = 0.5 * (body2Position + body1Position); - const rp3d::Vector3 hingeAxisWorldSpace(0, 1, 0); + const rp3d::vec3& body1Position = body1->getTransform().getPosition(); + const rp3d::vec3& body2Position = body2->getTransform().getPosition(); + const rp3d::vec3 m_anchorPointWorldSpace = 0.5 * (body2Position + body1Position); + const rp3d::vec3 hingeAxisWorldSpace(0, 1, 0); rp3d::HingeJointInfo jointInfo(body1, body2, m_anchorPointWorldSpace, hingeAxisWorldSpace); jointInfo.isMotorEnabled = true; - jointInfo.motorSpeed = - rp3d::float(0.5) * PI; + jointInfo.motorSpeed = - rp3d::0.5f * PI; jointInfo.maxMotorTorque = rp3d::float(60.0); jointInfo.isCollisionEnabled = false; @@ -410,10 +410,10 @@ void JointsScene::createFixedJoints() { // --------------- Create the first box --------------- // // Position of the box - openglframework::Vector3 positionBox1(5, 7, 0); + openglframework::vec3 positionBox1(5, 7, 0); // Create a box and a corresponding rigid in the dynamics world - openglframework::Vector3 boxDimension(1.5, 1.5, 1.5); + openglframework::vec3 boxDimension(1.5, 1.5, 1.5); mFixedJointBox1 = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld); // Set the box color @@ -427,7 +427,7 @@ void JointsScene::createFixedJoints() { // --------------- Create the second box --------------- // // Position of the box - openglframework::Vector3 positionBox2(-5, 7, 0); + openglframework::vec3 positionBox2(-5, 7, 0); // Create a box and a corresponding rigid in the dynamics world mFixedJointBox2 = new Box(boxDimension, positionBox2 , BOX_MASS, mDynamicsWorld); @@ -445,7 +445,7 @@ void JointsScene::createFixedJoints() { // Create the joint info object rp3d::RigidBody* body1 = mFixedJointBox1->getRigidBody(); rp3d::RigidBody* propellerBody = mPropellerBox->getRigidBody(); - const rp3d::Vector3 m_anchorPointWorldSpace1(5, 7, 0); + const rp3d::vec3 m_anchorPointWorldSpace1(5, 7, 0); rp3d::FixedJointInfo jointInfo1(body1, propellerBody, m_anchorPointWorldSpace1); jointInfo1.isCollisionEnabled = false; @@ -456,7 +456,7 @@ void JointsScene::createFixedJoints() { // Create the joint info object rp3d::RigidBody* body2 = mFixedJointBox2->getRigidBody(); - const rp3d::Vector3 m_anchorPointWorldSpace2(-5, 7, 0); + const rp3d::vec3 m_anchorPointWorldSpace2(-5, 7, 0); rp3d::FixedJointInfo jointInfo2(body2, propellerBody, m_anchorPointWorldSpace2); jointInfo2.isCollisionEnabled = false; @@ -468,7 +468,7 @@ void JointsScene::createFixedJoints() { void JointsScene::createFloor() { // Create the floor - openglframework::Vector3 floorPosition(0, 0, 0); + openglframework::vec3 floorPosition(0, 0, 0); mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); // Set the box color diff --git a/tools/testbed/scenes/joints/JointsScene.h b/tools/testbed/scenes/joints/JointsScene.h index 7287535..d1706fe 100644 --- a/tools/testbed/scenes/joints/JointsScene.h +++ b/tools/testbed/scenes/joints/JointsScene.h @@ -36,8 +36,8 @@ namespace jointsscene { // Constants const float SCENE_RADIUS = 30.0f; -const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters -const openglframework::Vector3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters +const openglframework::vec3 BOX_SIZE(2, 2, 2); // Box dimensions in meters +const openglframework::vec3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters const float BOX_MASS = 1.0f; // Box mass in kilograms const float FLOOR_MASS = 100.0f; // Floor mass in kilograms const int32_t NB_BALLSOCKETJOINT_BOXES = 7; // Number of Ball-And-Socket chain boxes diff --git a/tools/testbed/scenes/raycast/RaycastScene.cpp b/tools/testbed/scenes/raycast/RaycastScene.cpp index b89778a..dd0b031 100644 --- a/tools/testbed/scenes/raycast/RaycastScene.cpp +++ b/tools/testbed/scenes/raycast/RaycastScene.cpp @@ -39,7 +39,7 @@ RaycastScene::RaycastScene(const std::string& name) mIsContactPointsDisplayed = true; // Compute the radius and the center of the scene - openglframework::Vector3 center(0, 0, 0); + openglframework::vec3 center(0, 0, 0); // Set the center of the scene setScenePosition(center, SCENE_RADIUS); @@ -48,7 +48,7 @@ RaycastScene::RaycastScene(const std::string& name) mCollisionWorld = new rp3d::CollisionWorld(); // ---------- Dumbbell ---------- // - openglframework::Vector3 position1(0, 0, 0); + openglframework::vec3 position1(0, 0, 0); // Create a convex mesh and a corresponding collision body in the dynamics world mDumbbell = new Dumbbell(position1, mCollisionWorld, mMeshFolderPath); @@ -58,7 +58,7 @@ RaycastScene::RaycastScene(const std::string& name) mDumbbell->setSleepingColor(mRedColorDemo); // ---------- Box ---------- // - openglframework::Vector3 position2(0, 0, 0); + openglframework::vec3 position2(0, 0, 0); // Create a box and a corresponding collision body in the dynamics world mBox = new Box(BOX_SIZE, position2, mCollisionWorld); @@ -69,7 +69,7 @@ RaycastScene::RaycastScene(const std::string& name) mBox->setSleepingColor(mRedColorDemo); // ---------- Sphere ---------- // - openglframework::Vector3 position3(0, 0, 0); + openglframework::vec3 position3(0, 0, 0); // Create a sphere and a corresponding collision body in the dynamics world mSphere = new Sphere(SPHERE_RADIUS, position3, mCollisionWorld, @@ -80,7 +80,7 @@ RaycastScene::RaycastScene(const std::string& name) mSphere->setSleepingColor(mRedColorDemo); // ---------- Cone ---------- // - openglframework::Vector3 position4(0, 0, 0); + openglframework::vec3 position4(0, 0, 0); // Create a cone and a corresponding collision body in the dynamics world mCone = new Cone(CONE_RADIUS, CONE_HEIGHT, position4, mCollisionWorld, @@ -91,7 +91,7 @@ RaycastScene::RaycastScene(const std::string& name) mCone->setSleepingColor(mRedColorDemo); // ---------- Cylinder ---------- // - openglframework::Vector3 position5(0, 0, 0); + openglframework::vec3 position5(0, 0, 0); // Create a cylinder and a corresponding collision body in the dynamics world mCylinder = new Cylinder(CYLINDER_RADIUS, CYLINDER_HEIGHT, position5, @@ -102,7 +102,7 @@ RaycastScene::RaycastScene(const std::string& name) mCylinder->setSleepingColor(mRedColorDemo); // ---------- Capsule ---------- // - openglframework::Vector3 position6(0, 0, 0); + openglframework::vec3 position6(0, 0, 0); // Create a cylinder and a corresponding collision body in the dynamics world mCapsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position6 , @@ -113,7 +113,7 @@ RaycastScene::RaycastScene(const std::string& name) mCapsule->setSleepingColor(mRedColorDemo); // ---------- Convex Mesh ---------- // - openglframework::Vector3 position7(0, 0, 0); + openglframework::vec3 position7(0, 0, 0); // Create a convex mesh and a corresponding collision body in the dynamics world mConvexMesh = new ConvexMesh(position7, mCollisionWorld, mMeshFolderPath + "convexmesh.obj"); @@ -123,7 +123,7 @@ RaycastScene::RaycastScene(const std::string& name) mConvexMesh->setSleepingColor(mRedColorDemo); // ---------- Concave Mesh ---------- // - openglframework::Vector3 position8(0, 0, 0); + openglframework::vec3 position8(0, 0, 0); // Create a convex mesh and a corresponding collision body in the dynamics world mConcaveMesh = new ConcaveMesh(position8, mCollisionWorld, mMeshFolderPath + "city.obj"); @@ -133,7 +133,7 @@ RaycastScene::RaycastScene(const std::string& name) mConcaveMesh->setSleepingColor(mRedColorDemo); // ---------- Heightfield ---------- // - openglframework::Vector3 position9(0, 0, 0); + openglframework::vec3 position9(0, 0, 0); // Create a convex mesh and a corresponding collision body in the dynamics world mHeightField = new HeightField(position9, mCollisionWorld); @@ -169,8 +169,8 @@ void RaycastScene::createLines() { // Create a line from the point on the sphere to the center of // the scene - openglframework::Vector3 point1(x, y, z); - openglframework::Vector3 point2(0.0f, 0.0f, 0.0f); + openglframework::vec3 point1(x, y, z); + openglframework::vec3 point2(0.0f, 0.0f, 0.0f); Line* line = new Line(point1, point2); mLines.push_back(line); @@ -315,11 +315,11 @@ void RaycastScene::update() { Line* line = *it; // Create a ray corresponding to the line - openglframework::Vector3 p1 = line->getPoint1(); - openglframework::Vector3 p2 = line->getPoint2(); + openglframework::vec3 p1 = line->getPoint1(); + openglframework::vec3 p2 = line->getPoint2(); - rp3d::Vector3 point1(p1.x, p1.y, p1.z); - rp3d::Vector3 point2(p2.x, p2.y, p2.z); + rp3d::vec3 point1(p1.x(), p1.y(), p1.z()); + rp3d::vec3 point2(p2.x(), p2.y(), p2.z()); rp3d::Ray ray(point1, point2); // Perform a raycast query on the physics world by passing a raycast @@ -351,7 +351,7 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader, // model-view matrix) const openglframework::Matrix3 normalMatrix = localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + shader.setetk::Matrix3x3Uniform("normalMatrix", normalMatrix, false); // Set the vertex color openglframework::Vector4 color(1, 0, 0, 1); @@ -399,7 +399,7 @@ void RaycastScene::createVBOAndVAO(openglframework::Shader& shader) { // Create the VBO for the vertices data mVBOVertices.create(); mVBOVertices.bind(); - size_t sizeVertices = mLinePoints.size() * sizeof(openglframework::Vector3); + size_t sizeVertices = mLinePoints.size() * sizeof(openglframework::vec3); mVBOVertices.copyDataIntoVBO(sizeVertices, &mLinePoints[0], GL_STATIC_DRAW); mVBOVertices.unbind(); diff --git a/tools/testbed/scenes/raycast/RaycastScene.h b/tools/testbed/scenes/raycast/RaycastScene.h index 932f775..99f94af 100644 --- a/tools/testbed/scenes/raycast/RaycastScene.h +++ b/tools/testbed/scenes/raycast/RaycastScene.h @@ -48,7 +48,7 @@ namespace raycastscene { // Constants const float SCENE_RADIUS = 30.0f; -const openglframework::Vector3 BOX_SIZE(4, 2, 1); +const openglframework::vec3 BOX_SIZE(4, 2, 1); const float SPHERE_RADIUS = 3.0f; const float CONE_RADIUS = 3.0f; const float CONE_HEIGHT = 5.0f; @@ -84,13 +84,13 @@ class RaycastManager : public rp3d::RaycastCallback { } virtual rp3d::float notifyRaycastHit(const rp3d::RaycastInfo& raycastInfo) { - rp3d::Vector3 hitPos = raycastInfo.worldPoint; - openglframework::Vector3 position(hitPos.x, hitPos.y, hitPos.z); + rp3d::vec3 hitPos = raycastInfo.worldPoint; + openglframework::vec3 position(hitPos.x(), hitPos.y(), hitPos.z()); mHitPoints.push_back(ContactPoint(position)); // Create a line to display the normal at hit point - rp3d::Vector3 n = raycastInfo.worldNormal; - openglframework::Vector3 normal(n.x, n.y, n.z); + rp3d::vec3 n = raycastInfo.worldNormal; + openglframework::vec3 normal(n.x(), n.y(), n.z()); Line* normalLine = new Line(position, position + normal); mNormals.push_back(normalLine); @@ -153,7 +153,7 @@ class RaycastScene : public SceneDemo { rp3d::CollisionWorld* mCollisionWorld; /// All the points to render the lines - std::vector mLinePoints; + std::vector mLinePoints; /// Vertex Buffer Object for the vertices data openglframework::VertexBufferObject mVBOVertices; diff --git a/tools/testbed/src/Gui.cpp b/tools/testbed/src/Gui.cpp index 1b2df88..16e12cf 100644 --- a/tools/testbed/src/Gui.cpp +++ b/tools/testbed/src/Gui.cpp @@ -91,7 +91,7 @@ void Gui::update() { void Gui::createSimulationPanel() { mSimulationPanel = new Window(mApp, "Simulation"); - mSimulationPanel->setPosition(Vector2i(15, 15)); + mSimulationPanel->setPosition(vec2i(15, 15)); mSimulationPanel->setLayout(new GroupLayout(10, 5, 10 , 20)); mSimulationPanel->setId("SimulationPanel"); mSimulationPanel->setFixedWidth(220); @@ -137,7 +137,7 @@ void Gui::createSimulationPanel() { void Gui::createSettingsPanel() { mSettingsPanel = new Window(mApp, "Settings"); - mSettingsPanel->setPosition(Vector2i(15, 180)); + mSettingsPanel->setPosition(vec2i(15, 180)); mSettingsPanel->setLayout(new BoxLayout(Orientation::Vertical, Alignment::Middle, 10, 5)); mSettingsPanel->setId("SettingsPanel"); mSettingsPanel->setFixedWidth(220); @@ -185,7 +185,7 @@ void Gui::createSettingsPanel() { Label* labelTimeStep = new Label(panelTimeStep, "Time step","sans-bold"); labelTimeStep->setFixedWidth(120); TextBox* textboxTimeStep = new TextBox(panelTimeStep); - textboxTimeStep->setFixedSize(Vector2i(70, 25)); + textboxTimeStep->setFixedSize(vec2i(70, 25)); textboxTimeStep->setEditable(true); std::ostringstream out; out << std::setprecision(1) << std::fixed << (mApp->mEngineSettings.timeStep * 1000); @@ -219,7 +219,7 @@ void Gui::createSettingsPanel() { Label* labelVelocityIterations = new Label(panelVelocityIterations, "Velocity solver","sans-bold"); labelVelocityIterations->setFixedWidth(120); TextBox* textboxVelocityIterations = new TextBox(panelVelocityIterations); - textboxVelocityIterations->setFixedSize(Vector2i(70, 25)); + textboxVelocityIterations->setFixedSize(vec2i(70, 25)); textboxVelocityIterations->setEditable(true); textboxVelocityIterations->setValue(std::to_string(mApp->mEngineSettings.nbVelocitySolverIterations)); textboxVelocityIterations->setUnits("iter"); @@ -250,7 +250,7 @@ void Gui::createSettingsPanel() { Label* labelPositionIterations = new Label(panelPositionIterations, "Position solver","sans-bold"); labelPositionIterations->setFixedWidth(120); TextBox* textboxPositionIterations = new TextBox(panelPositionIterations); - textboxPositionIterations->setFixedSize(Vector2i(70, 25)); + textboxPositionIterations->setFixedSize(vec2i(70, 25)); textboxPositionIterations->setEditable(true); textboxPositionIterations->setValue(std::to_string(mApp->mEngineSettings.nbPositionSolverIterations)); textboxPositionIterations->setUnits("iter"); @@ -283,7 +283,7 @@ void Gui::createSettingsPanel() { out.str(""); out << std::setprecision(0) << std::fixed << (mApp->mEngineSettings.timeBeforeSleep * 1000); TextBox* textboxTimeSleep = new TextBox(panelTimeSleep); - textboxTimeSleep->setFixedSize(Vector2i(70, 25)); + textboxTimeSleep->setFixedSize(vec2i(70, 25)); textboxTimeSleep->setEditable(true); textboxTimeSleep->setValue(out.str()); textboxTimeSleep->setUnits("ms"); @@ -317,7 +317,7 @@ void Gui::createSettingsPanel() { out.str(""); out << std::setprecision(2) << std::fixed << (mApp->mEngineSettings.sleepLinearVelocity); TextBox* textboxSleepLinearVel = new TextBox(panelSleepLinearVel); - textboxSleepLinearVel->setFixedSize(Vector2i(70, 25)); + textboxSleepLinearVel->setFixedSize(vec2i(70, 25)); textboxSleepLinearVel->setEditable(true); textboxSleepLinearVel->setValue(out.str()); textboxSleepLinearVel->setUnits("m/s"); @@ -351,7 +351,7 @@ void Gui::createSettingsPanel() { out.str(""); out << std::setprecision(2) << std::fixed << (mApp->mEngineSettings.sleepAngularVelocity); TextBox* textboxSleepAngularVel = new TextBox(panelSleepAngularVel); - textboxSleepAngularVel->setFixedSize(Vector2i(70, 25)); + textboxSleepAngularVel->setFixedSize(vec2i(70, 25)); textboxSleepAngularVel->setEditable(true); textboxSleepAngularVel->setValue(out.str()); textboxSleepAngularVel->setUnits("rad/s"); @@ -409,7 +409,7 @@ void Gui::createSettingsPanel() { void Gui::createProfilingPanel() { Widget* profilingPanel = new Window(mApp, "Profiling"); - profilingPanel->setPosition(Vector2i(15, 525)); + profilingPanel->setPosition(vec2i(15, 525)); profilingPanel->setLayout(new BoxLayout(Orientation::Vertical, Alignment::Fill, 10, 5)); profilingPanel->setId("SettingsPanel"); profilingPanel->setFixedWidth(220); diff --git a/tools/testbed/src/Scene.cpp b/tools/testbed/src/Scene.cpp index bfe4ae3..747933b 100644 --- a/tools/testbed/src/Scene.cpp +++ b/tools/testbed/src/Scene.cpp @@ -43,7 +43,7 @@ Scene::~Scene() { } // Set the scene position (where the camera needs to look at) -void Scene::setScenePosition(const openglframework::Vector3& position, float sceneRadius) { +void Scene::setScenePosition(const openglframework::vec3& position, float sceneRadius) { // Set the position and radius of the scene mCenterScene = position; @@ -69,7 +69,7 @@ void Scene::resetCameraToViewAll() { // Map the mouse x,y coordinates to a point on a sphere bool Scene::mapMouseCoordinatesToSphere(double xMouse, double yMouse, - Vector3& spherePoint) const { + vec3& spherePoint) const { if ((xMouse >= 0) && (xMouse <= mWindowWidth) && (yMouse >= 0) && (yMouse <= mWindowHeight)) { float x = float(xMouse - 0.5f * mWindowWidth) / float(mWindowWidth); @@ -79,9 +79,9 @@ bool Scene::mapMouseCoordinatesToSphere(double xMouse, double yMouse, float sinx2siny2 = sinx * sinx + siny * siny; // Compute the point on the sphere - spherePoint.x = sinx; - spherePoint.y = siny; - spherePoint.z = (sinx2siny2 < 1.0) ? sqrt(1.0f - sinx2siny2) : 0.0f; + spherePoint.x() = sinx; + spherePoint.y() = siny; + spherePoint.z() = (sinx2siny2 < 1.0) ? sqrt(1.0f - sinx2siny2) : 0.0f; return true; } @@ -167,11 +167,11 @@ void Scene::rotate(int32_t xMouse, int32_t yMouse) { if (mIsLastPointOnSphereValid) { - Vector3 newPoint3D; + vec3 newPoint3D; bool isNewPointOK = mapMouseCoordinatesToSphere(xMouse, yMouse, newPoint3D); if (isNewPointOK) { - Vector3 axis = mLastPointOnSphere.cross(newPoint3D); + vec3 axis = mLastPointOnSphere.cross(newPoint3D); float cosAngle = mLastPointOnSphere.dot(newPoint3D); float epsilon = std::numeric_limits::epsilon(); diff --git a/tools/testbed/src/Scene.h b/tools/testbed/src/Scene.h index 8ff9cdf..6805e5b 100644 --- a/tools/testbed/src/Scene.h +++ b/tools/testbed/src/Scene.h @@ -33,10 +33,10 @@ struct ContactPoint { public: - openglframework::Vector3 point; + openglframework::vec3 point; /// Constructor - ContactPoint(const openglframework::Vector3& pointWorld) : point(pointWorld) { + ContactPoint(const openglframework::vec3& pointWorld) : point(pointWorld) { } }; @@ -56,7 +56,7 @@ struct EngineSettings { float sleepLinearVelocity; // Sleep linear velocity float sleepAngularVelocity; // Sleep angular velocity bool isGravityEnabled; // True if gravity is enabled - openglframework::Vector3 gravity; // Gravity vector + openglframework::vec3 gravity; // Gravity vector /// Constructor EngineSettings() : elapsedTime(0.0f), timeStep(0.0f) { @@ -82,7 +82,7 @@ class Scene { openglframework::Camera mCamera; /// Center of the scene - openglframework::Vector3 mCenterScene; + openglframework::vec3 mCenterScene; /// Last mouse coordinates on the windows double mLastMouseX, mLastMouseY; @@ -91,7 +91,7 @@ class Scene { int32_t mWindowWidth, mWindowHeight; /// Last point computed on a sphere (for camera rotation) - openglframework::Vector3 mLastPointOnSphere; + openglframework::vec3 mLastPointOnSphere; /// True if the last point computed on a sphere (for camera rotation) is valid bool mIsLastPointOnSphereValid; @@ -111,14 +111,14 @@ class Scene { // -------------------- Methods -------------------- // /// Set the scene position (where the camera needs to look at) - void setScenePosition(const openglframework::Vector3& position, float sceneRadius); + void setScenePosition(const openglframework::vec3& position, float sceneRadius); /// Set the camera so that we can view the whole scene void resetCameraToViewAll(); /// Map mouse coordinates to coordinates on the sphere bool mapMouseCoordinatesToSphere(double xMouse, double yMouse, - openglframework::Vector3& spherePoint) const; + openglframework::vec3& spherePoint) const; /// Zoom the camera void zoom(float zoomDiff); diff --git a/tools/testbed/src/SceneDemo.cpp b/tools/testbed/src/SceneDemo.cpp index c95c17e..136432f 100644 --- a/tools/testbed/src/SceneDemo.cpp +++ b/tools/testbed/src/SceneDemo.cpp @@ -51,18 +51,18 @@ SceneDemo::SceneDemo(const std::string& name, float sceneRadius, bool isShadowMa shadowMapTextureLevel++; // Move the light0 - mLight0.translateWorld(Vector3(-2, 35, 40)); + mLight0.translateWorld(vec3(-2, 35, 40)); // Camera at light0 postion for the shadow map mShadowMapLightCamera.translateWorld(mLight0.getOrigin()); - mShadowMapLightCamera.rotateLocal(Vector3(1, 0, 0), -PI / 4.0f); - mShadowMapLightCamera.rotateWorld(Vector3(0, 1, 0), PI / 8.0f); + mShadowMapLightCamera.rotateLocal(vec3(1, 0, 0), -PI / 4.0f); + mShadowMapLightCamera.rotateWorld(vec3(0, 1, 0), PI / 8.0f); mShadowMapLightCamera.setDimensions(SHADOWMAP_WIDTH, SHADOWMAP_HEIGHT); mShadowMapLightCamera.setFieldOfView(80.0f); mShadowMapLightCamera.setSceneRadius(100); - mShadowMapBiasMatrix.setAllValues(0.5, 0.0, 0.0, 0.5, + mShadowMapBiasMatrix.setValue(0.5, 0.0, 0.0, 0.5, 0.0, 0.5, 0.0, 0.5, 0.0, 0.0, 0.5, 0.5, 0.0, 0.0, 0.0, 1.0); @@ -160,12 +160,12 @@ void SceneDemo::render() { mPhongShader.setMatrix4x4Uniform("projectionMatrix", mCamera.getProjectionMatrix()); mPhongShader.setMatrix4x4Uniform("shadowMapProjectionMatrix", mShadowMapBiasMatrix * shadowMapProjMatrix); mPhongShader.setMatrix4x4Uniform("worldToLight0CameraMatrix", worldToLightCameraMatrix); - mPhongShader.setVector3Uniform("light0PosCameraSpace", worldToCameraMatrix * mLight0.getOrigin()); - mPhongShader.setVector3Uniform("lightAmbientColor", Vector3(0.4f, 0.4f, 0.4f)); - mPhongShader.setVector3Uniform("light0DiffuseColor", Vector3(diffCol.r, diffCol.g, diffCol.b)); + mPhongShader.setvec3Uniform("light0PosCameraSpace", worldToCameraMatrix * mLight0.getOrigin()); + mPhongShader.setvec3Uniform("lightAmbientColor", vec3(0.4f, 0.4f, 0.4f)); + mPhongShader.setvec3Uniform("light0DiffuseColor", vec3(diffCol.r, diffCol.g, diffCol.b)); mPhongShader.setIntUniform("shadowMapSampler", textureUnit); mPhongShader.setIntUniform("isShadowEnabled", mIsShadowMappingEnabled); - mPhongShader.setVector2Uniform("shadowMapDimension", Vector2(SHADOWMAP_WIDTH, SHADOWMAP_HEIGHT)); + mPhongShader.setvec2Uniform("shadowMapDimension", vec2(SHADOWMAP_WIDTH, SHADOWMAP_HEIGHT)); // Set the viewport to render the scene glViewport(mViewportX, mViewportY, mViewportWidth, mViewportHeight); @@ -334,8 +334,8 @@ std::vector SceneDemo::computeContactPointsOfWorld(const rp3d::Dyn for (uint32_t i=0; igetNbContactPoints(); i++) { rp3d::ContactPoint* contactPoint = manifold->getContactPoint(i); - rp3d::Vector3 point = contactPoint->getWorldPointOnBody1(); - ContactPoint contact(openglframework::Vector3(point.x, point.y, point.z)); + rp3d::vec3 point = contactPoint->getWorldPointOnBody1(); + ContactPoint contact(openglframework::vec3(point.x(), point.y(), point.z())); contactPoints.push_back(contact); } diff --git a/tools/testbed/src/TestbedApplication.cpp b/tools/testbed/src/TestbedApplication.cpp index f3e290e..b951e15 100644 --- a/tools/testbed/src/TestbedApplication.cpp +++ b/tools/testbed/src/TestbedApplication.cpp @@ -49,24 +49,24 @@ const float TestbedApplication::SCROLL_SENSITIVITY = 0.08f; // Constructor TestbedApplication::TestbedApplication(bool isFullscreen) - : Screen(Vector2i(1280, 800), "Testbed ReactPhysics3D", true, isFullscreen), + : Screen(vec2i(1280, 800), "Testbed ReactPhysics3D", true, isFullscreen), mIsInitialized(false), mGui(this), mFPS(0), mNbFrames(0), mPreviousTime(0), mLastTimeComputedFPS(0), mFrameTime(0), mPhysicsTime(0) { mCurrentScene = NULL; mIsMultisamplingActive = true; - mWidth = 1280; + m_width = 1280; mHeight = 720; mSinglePhysicsStepEnabled = false; mSinglePhysicsStepDone = false; - mWindowToFramebufferRatio = Vector2(1, 1); + mWindowToFramebufferRatio = vec2(1, 1); mIsShadowMappingEnabled = true; mIsVSyncEnabled = false; mIsContactPointsDisplayed = false; init(); - resizeEvent(Vector2i(0, 0)); + resizeEvent(vec2i(0, 0)); } // Destructor @@ -223,7 +223,7 @@ void TestbedApplication::drawContents() { } /// Window resize event handler -bool TestbedApplication::resizeEvent(const Vector2i& size) { +bool TestbedApplication::resizeEvent(const vec2i& size) { if (!mIsInitialized) return false; @@ -257,7 +257,7 @@ void TestbedApplication::switchScene(Scene* newScene) { // Reset the scene mCurrentScene->reset(); - resizeEvent(Vector2i(0, 0)); + resizeEvent(vec2i(0, 0)); } // Check the OpenGL errors @@ -339,7 +339,7 @@ bool TestbedApplication::keyboardEvent(int32_t key, int32_t scancode, int32_t ac } // Handle a mouse button event (default implementation: propagate to children) -bool TestbedApplication::mouseButtonEvent(const Vector2i &p, int32_t button, bool down, int32_t modifiers) { +bool TestbedApplication::mouseButtonEvent(const vec2i &p, int32_t button, bool down, int32_t modifiers) { if (Screen::mouseButtonEvent(p, button, down, modifiers)) { return true; @@ -353,7 +353,7 @@ bool TestbedApplication::mouseButtonEvent(const Vector2i &p, int32_t button, boo } // Handle a mouse motion event (default implementation: propagate to children) -bool TestbedApplication::mouseMotionEvent(const Vector2i &p, const Vector2i &rel, int32_t button, int32_t modifiers) { +bool TestbedApplication::mouseMotionEvent(const vec2i &p, const vec2i &rel, int32_t button, int32_t modifiers) { if (Screen::mouseMotionEvent(p, rel, button, modifiers)) { return true; @@ -369,7 +369,7 @@ bool TestbedApplication::mouseMotionEvent(const Vector2i &p, const Vector2i &rel } // Handle a mouse scroll event (default implementation: propagate to children) -bool TestbedApplication::scrollEvent(const Vector2i &p, const Vector2f &rel) { +bool TestbedApplication::scrollEvent(const vec2i &p, const vec2f &rel) { if (Screen::scrollEvent(p, rel)) { return true; diff --git a/tools/testbed/src/TestbedApplication.h b/tools/testbed/src/TestbedApplication.h index 343371f..f2840a2 100644 --- a/tools/testbed/src/TestbedApplication.h +++ b/tools/testbed/src/TestbedApplication.h @@ -93,7 +93,7 @@ class TestbedApplication : public Screen { bool mIsMultisamplingActive; /// Width and height of the window - int32_t mWidth, mHeight; + int32_t m_width, mHeight; /// True if the next simulation update is a single physics step bool mSinglePhysicsStepEnabled; @@ -101,7 +101,7 @@ class TestbedApplication : public Screen { /// True if the single physics step has been taken already bool mSinglePhysicsStepDone; - openglframework::Vector2 mWindowToFramebufferRatio; + openglframework::vec2 mWindowToFramebufferRatio; /// True if shadow mapping is enabled bool mIsShadowMappingEnabled; @@ -179,19 +179,19 @@ class TestbedApplication : public Screen { virtual void drawContents(); /// Window resize event handler - virtual bool resizeEvent(const Vector2i& size); + virtual bool resizeEvent(const vec2i& size); /// Default keyboard event handler virtual bool keyboardEvent(int32_t key, int32_t scancode, int32_t action, int32_t modifiers); /// Handle a mouse button event (default implementation: propagate to children) - virtual bool mouseButtonEvent(const Vector2i &p, int32_t button, bool down, int32_t modifiers); + virtual bool mouseButtonEvent(const vec2i &p, int32_t button, bool down, int32_t modifiers); /// Handle a mouse motion event (default implementation: propagate to children) - virtual bool mouseMotionEvent(const Vector2i &p, const Vector2i &rel, int32_t button, int32_t modifiers); + virtual bool mouseMotionEvent(const vec2i &p, const vec2i &rel, int32_t button, int32_t modifiers); /// Handle a mouse scroll event (default implementation: propagate to children) - virtual bool scrollEvent(const Vector2i &p, const Vector2f &rel); + virtual bool scrollEvent(const vec2i &p, const vec2f &rel); /// Initialize the application void init();