From ddb7d8309ad7c01bcb529deab80047ffa28e543e Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Fri, 13 Feb 2009 11:55:18 +0000 Subject: [PATCH] git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@102 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- sources/reactphysics3d/body/BodyState.cpp | 101 +++++++++++++++ sources/reactphysics3d/body/BodyState.h | 149 ++++++++++++++++++++++ sources/reactphysics3d/body/RigidBody.cpp | 68 ++++++++++ sources/reactphysics3d/body/RigidBody.h | 108 ++++++++++++++++ 4 files changed, 426 insertions(+) create mode 100644 sources/reactphysics3d/body/BodyState.cpp create mode 100644 sources/reactphysics3d/body/BodyState.h create mode 100644 sources/reactphysics3d/body/RigidBody.cpp create mode 100644 sources/reactphysics3d/body/RigidBody.h diff --git a/sources/reactphysics3d/body/BodyState.cpp b/sources/reactphysics3d/body/BodyState.cpp new file mode 100644 index 00000000..070af1da --- /dev/null +++ b/sources/reactphysics3d/body/BodyState.cpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + + // Libraries + #include "BodyState.h" + #include + #include + + // We want to use the ReactPhysics3D namespace + using namespace reactphysics3d; + + // Constructor + BodyState::BodyState(const Vector3D& position, const Matrix3x3& inertiaTensorInverse, const Kilogram& massInverse) + : position(position), linearMomentum(Vector3D()), orientation(Quaternion(1,0,0, 0)), angularMomentum(Vector3D()), + linearVelocity(Vector3D()), angularVelocity(Vector3D()), spin(Quaternion()), inertiaTensorInverse(inertiaTensorInverse), + massInverse(massInverse) { + // TODO : orientation will be initialized in another way + // TODO : linearMomentum will be initialized in another way + // TODO : angularMomentum will be initialize in another way + // TODO : linearVelocity will be initialize in another way + // TODO : angularVelocity will be initialize in another way + // TODO : spin will be initialize in another way + } + +// Copy-constructor +BodyState::BodyState(const BodyState& bodyState) + : position(bodyState.position), linearMomentum(bodyState.linearMomentum), orientation(bodyState.orientation), + angularMomentum(bodyState.angularMomentum), linearVelocity(bodyState.linearVelocity), + angularVelocity(bodyState.angularVelocity), spin(bodyState.spin), inertiaTensorInverse(bodyState.inertiaTensorInverse), + massInverse(bodyState.massInverse) { +} + +// Destructor +BodyState::~BodyState() { + +} + +// Recalculate the secondary values of the BodyState when the primary values have changed +void BodyState::recalculate() { + // Compute the linear velocity + linearVelocity = linearMomentum * massInverse.getValue(); + + // Compute the angular velocity + angularVelocity = inertiaTensorInverse * angularMomentum; + + // Normalize the orientation quaternion + orientation = orientation.getUnit(); + + // Compute the spin quaternion + spin = Quaternion(0, angularVelocity.getX(), angularVelocity.getY(), angularVelocity.getZ()) * orientation * 0.5; +} + +// Compute the body state at time t + dt +void BodyState::computeAtTime(const Time& timeStep, const DerivativeBodyState& lastDerivativeBodyState) { + + double dt = timeStep.getValue(); // Timestep + + // Compute the position at time t + dt + position = position + lastDerivativeBodyState.getLinearVelocity() * dt; + + // Compute the linear momentum at time t + dt + linearMomentum = linearMomentum + lastDerivativeBodyState.getForce() * dt; + + // Compute the orientation at time t + dt + orientation = orientation + lastDerivativeBodyState.getSpin() * dt; + + // Compute the angular momentum at time t + dt + angularMomentum = angularMomentum + lastDerivativeBodyState.getTorque() * dt; + + // Recalculate the secondary values of the body state + recalculate(); +} + +// Return the force on the body at time t +Vector3D BodyState::computeForce(Time time) const { + // TODO : Implement this method + return Vector3D(sin(time.getValue()*0.9 + 0.5), sin(time.getValue()*0.5 + 0.4), + sin(time.getValue()*0.7f + 0.9f)); +} + +// Return the torque on the body at time +Vector3D BodyState::computeTorque(Time time) const { + // TODO : Implement this method + return Vector3D(-1.0, 0.0 ,0); +} diff --git a/sources/reactphysics3d/body/BodyState.h b/sources/reactphysics3d/body/BodyState.h new file mode 100644 index 00000000..8ce52f84 --- /dev/null +++ b/sources/reactphysics3d/body/BodyState.h @@ -0,0 +1,149 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +#ifndef BODYSTATE_H +#define BODYSTATE_H + +// Libraries +#include "../mathematics/mathematics.h" +#include "../physics/physics.h" +#include "DerivativeBodyState.h" + +// Namespace reactphysics3d +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class BodyState : + A BodyState contains all the dynamics values of a body. + ------------------------------------------------------------------- +*/ +class BodyState { + private : + // Primary values + Vector3D position; // Position of the body + Vector3D linearMomentum; // Linear momentum of the body + Quaternion orientation; // Orientation quaternion of the body + Vector3D angularMomentum; // Angular momentum of the body + + // Secondary values + Vector3D linearVelocity; // Linear velocity of the body + Vector3D angularVelocity; // Angular velocity of the body + Quaternion spin; // Spin is the derivative of orientation quaternion over time. + + // Constants + Matrix3x3 inertiaTensorInverse; // Inverse of the inertia tensor of the body + Kilogram massInverse; // Inverse of the mass of the body + + public : + BodyState(const Vector3D& position, const Matrix3x3& inertiaTensorInverse, const Kilogram& massInverse); // Constructor + BodyState(const BodyState& bodyState); // Copy-constructor + virtual ~BodyState(); // Destructor + + Vector3D getPosition() const; // Return the position of the body + void setPosition(const Vector3D& position); // Set the position of the body + Vector3D getLinearMomentum() const; // Return the linear momemtum + void setLinearMomentum(const Vector3D& linearMomentum); // Set the linear momentum + Quaternion getOrientation() const; // Return the orientation quaternion + void setOrientation(const Quaternion& orientation); // Set the orientation quaternion + Vector3D getAngularMomentum() const; // Return the angular momentum + void setAngularMomentum(const Vector3D& angularMomentum); // Set the angular momentum + Vector3D getLinearVelocity() const; // Return the linear velocity + Vector3D getAngularVelocity() const; // Return the angular velocity + Quaternion getSpin() const; // Return the spin of the body + void setMassInverse(Kilogram massInverse); // Set the inverse of the mass + void setInertiaTensorInverse(const Matrix3x3& inertiaTensorInverse); // Set the inverse of the inertia tensor + + void recalculate(); // Recalculate the secondary values + // of the BodyState from the primary ones + void computeAtTime(const Time& timeStep, + const DerivativeBodyState& lastDerivativeBodyState); // Compute the body state at time t + dt + Vector3D computeForce(Time time) const; // Return the force on the body at time t + Vector3D computeTorque(Time time) const; // Return the torque on the body at time t +}; + +// --- Inlines functions --- // + +// Return the position of the body +inline Vector3D BodyState::getPosition() const { + return position; +} + +// Set the position of the body +inline void BodyState::setPosition(const Vector3D& position) { + this->position = position; +} + +// Return the linear momentum of the body +inline Vector3D BodyState::getLinearMomentum() const { + return linearMomentum; +} + +// Set the linear momentum of the body +inline void BodyState::setLinearMomentum(const Vector3D& linearMomentum) { + this->linearMomentum = linearMomentum; +} + +// Return the orientation quaternion of the body +inline Quaternion BodyState::getOrientation() const { + return orientation; +} + +// Set the orientation quaternion +inline void BodyState::setOrientation(const Quaternion& orientation) { + this->orientation = orientation; +} + +// Return the angular momentum of the body +inline Vector3D BodyState::getAngularMomentum() const { + return angularMomentum; +} + +// Set the angular momentum of the body +inline void BodyState::setAngularMomentum(const Vector3D& angularMomentum) { + this->angularMomentum = angularMomentum; +} + +// Return the linear velocity +inline Vector3D BodyState::getLinearVelocity() const { + return linearVelocity; +} + +// Return the angular velocity of the body +inline Vector3D BodyState::getAngularVelocity() const { + return angularVelocity; +} + +// Return the spin of the body +inline Quaternion BodyState::getSpin() const { + return spin; +} + +// Set the inverse of the mass +inline void BodyState::setMassInverse(Kilogram massInverse) { + this->massInverse = massInverse; +} + +// Set the inverse of the inertia tensor +inline void BodyState::setInertiaTensorInverse(const Matrix3x3& inertiaTensorInverse) { + this->inertiaTensorInverse = inertiaTensorInverse; +} + +} + +#endif diff --git a/sources/reactphysics3d/body/RigidBody.cpp b/sources/reactphysics3d/body/RigidBody.cpp new file mode 100644 index 00000000..4e4d0b78 --- /dev/null +++ b/sources/reactphysics3d/body/RigidBody.cpp @@ -0,0 +1,68 @@ +/**************************************************************************** +* Copyright (C) 2009 Daniel Chappuis * +**************************************************************************** +* This file is part of ReactPhysics3D. * +* * +* ReactPhysics3D is free software: you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation, either version 3 of the License, or * +* (at your option) any later version. * +* * +* ReactPhysics3D is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License for more details. * +* * +* You should have received a copy of the GNU General Public License * +* along with ReactPhysics3D. If not, see . * +***************************************************************************/ + +// Libraries +#include "RigidBody.h" + + // We want to use the ReactPhysics3D namespace + using namespace reactphysics3d; + + // Constructor + RigidBody::RigidBody(const Vector3D& position, const Kilogram& mass, const Matrix3x3& inertiaTensor) + : Body(mass), inertiaTensor(inertiaTensor), + currentBodyState(position, inertiaTensor.getInverse(),Kilogram(1.0/mass.getValue())), + previousBodyState(position, inertiaTensor.getInverse(), Kilogram(1.0/mass.getValue())) { + isMotionEnabled = true; + isCollisionEnabled = true; + interpolationFactor = 0.0; + } + + // Copy-constructor + RigidBody::RigidBody(const RigidBody& rigidBody) : Body(rigidBody), inertiaTensor(rigidBody.inertiaTensor), + currentBodyState(rigidBody.currentBodyState), previousBodyState(rigidBody.previousBodyState) { + this->isMotionEnabled = rigidBody.isMotionEnabled; + this->isCollisionEnabled = rigidBody.isCollisionEnabled; + interpolationFactor = rigidBody.interpolationFactor; + } + + // Destructor + RigidBody::~RigidBody() { + + }; + + // Compute the linear interpolation state between the previous body state and the current body state +// This is used to avoid visual stuttering when the display and physics framerates are out of synchronization +BodyState RigidBody::getInterpolatedState() const { + + // Get the interpolation factor + double alpha = interpolationFactor; + + // Compute the linear interpolation state + BodyState interpolatedState = currentBodyState; // Used to take massInverse, inertiaTensorInverse + interpolatedState.setPosition(previousBodyState.getPosition() * (1-alpha) + currentBodyState.getPosition() * alpha); + interpolatedState.setLinearMomentum(previousBodyState.getLinearMomentum() * (1-alpha) + currentBodyState.getLinearMomentum() * alpha); + interpolatedState.setOrientation(Quaternion::slerp(previousBodyState.getOrientation(), currentBodyState.getOrientation(), alpha)); + interpolatedState.setAngularMomentum(previousBodyState.getAngularMomentum() * (1-alpha) + currentBodyState.getAngularMomentum() * alpha); + + // Recalculate the secondary state values + interpolatedState.recalculate(); + + // Return the interpolated state + return interpolatedState; +} diff --git a/sources/reactphysics3d/body/RigidBody.h b/sources/reactphysics3d/body/RigidBody.h new file mode 100644 index 00000000..ac1dfedb --- /dev/null +++ b/sources/reactphysics3d/body/RigidBody.h @@ -0,0 +1,108 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + + #ifndef RIGIDBODY_H + #define RIGIDBODY_H + + // Libraries + #include + #include "Body.h" + #include "BodyState.h" + #include "../mathematics/mathematics.h" + #include "../physics/physics.h" + +// Namespace reactphysics3d +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class RigidBody : + This class represents a rigid body of the physics + engine. A rigid body is a non-deformable body that + has a constant mass. + ------------------------------------------------------------------- +*/ +class RigidBody : public Body { + private : + Matrix3x3 inertiaTensor; // Inertia tensor of the body + BodyState currentBodyState; // Current body state + BodyState previousBodyState; // Previous body state + bool isMotionEnabled; // True if the body can move + bool isCollisionEnabled; // True if the body can collide with others bodies + double interpolationFactor; // Interpolation factor used for the state interpolation + + public : + RigidBody(const Vector3D& position, const Kilogram& mass, const Matrix3x3& inertiaTensor); // Constructor + RigidBody(const RigidBody& rigidBody); // Copy-constructor + virtual ~RigidBody(); // Destructor + + Matrix3x3 getInertiaTensor() const; // Return the inertia tensor of the body + void setInertiaTensor(const Matrix3x3& inertiaTensor); // Set the inertia tensor of the body + BodyState& getCurrentBodyState(); // Return a reference to the current state of the body + void setInterpolationFactor(double factor); // Set the interpolation factor of the body + BodyState getInterpolatedState() const; // Compute and return the interpolated state + bool getIsMotionEnabled() const; // Return if the rigid body can move + void setIsMotionEnabled(bool isMotionEnabled); // Set the value to true if the body can move + void updatePreviousBodyState(); // Update the previous body state of the body +}; + +// --- Inline functions --- // + +// Return the inertia tensor of the body +inline Matrix3x3 RigidBody::getInertiaTensor() const { + return inertiaTensor; +} + +// Set the inertia tensor of the body +inline void RigidBody::setInertiaTensor(const Matrix3x3& inertiaTensor) { + this->inertiaTensor = inertiaTensor; +} + +// Return a reference to the current state of the body +// This way the currentBodyState could be modify outside the rigid body +inline BodyState& RigidBody::getCurrentBodyState() { + return currentBodyState; +} + +// Set the interpolation factor of the body +inline void RigidBody::setInterpolationFactor(double factor) { + assert(factor >= 0 && factor <= 1); + + // Set the factor + interpolationFactor = factor; +} + +// Return if the rigid body can move +inline bool RigidBody::getIsMotionEnabled() const { + return isMotionEnabled; +} + +// Set the value to true if the body can move +inline void RigidBody::setIsMotionEnabled(bool isMotionEnabled) { + this->isMotionEnabled = isMotionEnabled; +} + +// Update the previous body state of the body +inline void RigidBody::updatePreviousBodyState() { + // The current body state becomes the previous body state + previousBodyState = currentBodyState; +} + +} // End of the ReactPhyscis3D namespace + + #endif