moved files around
This commit is contained in:
ejcoumans
25
BulletDynamics/Dynamics/BU_Joint.cpp
Normal file
25
BulletDynamics/Dynamics/BU_Joint.cpp
Normal file
@@ -0,0 +1,25 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#include "BU_Joint.h"
|
||||
|
||||
BU_Joint::BU_Joint()
|
||||
{
|
||||
|
||||
}
|
||||
BU_Joint::~BU_Joint()
|
||||
{
|
||||
|
||||
}
|
||||
93
BulletDynamics/Dynamics/BU_Joint.h
Normal file
93
BulletDynamics/Dynamics/BU_Joint.h
Normal file
@@ -0,0 +1,93 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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 BU_Joint_H
|
||||
#define BU_Joint_H
|
||||
|
||||
class RigidBody;
|
||||
class BU_Joint;
|
||||
#include "SimdScalar.h"
|
||||
|
||||
struct BU_ContactJointNode {
|
||||
BU_Joint *joint; // pointer to enclosing BU_Joint object
|
||||
RigidBody*body; // *other* body this joint is connected to
|
||||
};
|
||||
typedef SimdScalar dVector3[4];
|
||||
|
||||
|
||||
class BU_Joint {
|
||||
|
||||
public:
|
||||
// naming convention: the "first" body this is connected to is node[0].body,
|
||||
// and the "second" body is node[1].body. if this joint is only connected
|
||||
// to one body then the second body is 0.
|
||||
|
||||
// info returned by getInfo1 function. the constraint dimension is m (<=6).
|
||||
// i.e. that is the total number of rows in the jacobian. `nub' is the
|
||||
// number of unbounded variables (which have lo,hi = -/+ infinity).
|
||||
|
||||
BU_Joint();
|
||||
virtual ~BU_Joint();
|
||||
|
||||
|
||||
struct Info1 {
|
||||
int m,nub;
|
||||
};
|
||||
|
||||
// info returned by getInfo2 function
|
||||
|
||||
struct Info2 {
|
||||
// integrator parameters: frames per second (1/stepsize), default error
|
||||
// reduction parameter (0..1).
|
||||
SimdScalar fps,erp;
|
||||
|
||||
// for the first and second body, pointers to two (linear and angular)
|
||||
// n*3 jacobian sub matrices, stored by rows. these matrices will have
|
||||
// been initialized to 0 on entry. if the second body is zero then the
|
||||
// J2xx pointers may be 0.
|
||||
SimdScalar *J1l,*J1a,*J2l,*J2a;
|
||||
|
||||
// elements to jump from one row to the next in J's
|
||||
int rowskip;
|
||||
|
||||
// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
|
||||
// "constraint force mixing" vector. c is set to zero on entry, cfm is
|
||||
// set to a constant value (typically very small or zero) value on entry.
|
||||
SimdScalar *c,*cfm;
|
||||
|
||||
// lo and hi limits for variables (set to -/+ infinity on entry).
|
||||
SimdScalar *lo,*hi;
|
||||
|
||||
// findex vector for variables. see the LCP solver interface for a
|
||||
// description of what this does. this is set to -1 on entry.
|
||||
// note that the returned indexes are relative to the first index of
|
||||
// the constraint.
|
||||
int *findex;
|
||||
};
|
||||
|
||||
// virtual function table: size of the joint structure, function pointers.
|
||||
// we do it this way instead of using C++ virtual functions because
|
||||
// sometimes we need to allocate joints ourself within a memory pool.
|
||||
|
||||
virtual void GetInfo1 (Info1 *info)=0;
|
||||
virtual void GetInfo2 (Info2 *info)=0;
|
||||
|
||||
int flags; // dJOINT_xxx flags
|
||||
BU_ContactJointNode node[2]; // connections to bodies. node[1].body can be 0
|
||||
SimdScalar lambda[6]; // lambda generated by last step
|
||||
};
|
||||
|
||||
|
||||
#endif //BU_Joint_H
|
||||
270
BulletDynamics/Dynamics/ContactJoint.cpp
Normal file
270
BulletDynamics/Dynamics/ContactJoint.cpp
Normal file
@@ -0,0 +1,270 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
*/
|
||||
#include "ContactJoint.h"
|
||||
#include "RigidBody.h"
|
||||
#include "NarrowPhaseCollision/PersistentManifold.h"
|
||||
|
||||
|
||||
//this constant needs to be set up so different solvers give 'similar' results
|
||||
#define FRICTION_CONSTANT 120.f
|
||||
|
||||
|
||||
ContactJoint::ContactJoint(PersistentManifold* manifold,int index,bool swap,RigidBody* body0,RigidBody* body1)
|
||||
:m_manifold(manifold),
|
||||
m_index(index),
|
||||
m_swapBodies(swap),
|
||||
m_body0(body0),
|
||||
m_body1(body1)
|
||||
{
|
||||
}
|
||||
|
||||
int m_numRows = 3;
|
||||
|
||||
|
||||
void ContactJoint::GetInfo1(Info1 *info)
|
||||
{
|
||||
info->m = m_numRows;
|
||||
//friction adds another 2...
|
||||
|
||||
info->nub = 0;
|
||||
}
|
||||
|
||||
#define dCROSS(a,op,b,c) \
|
||||
(a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \
|
||||
(a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \
|
||||
(a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]);
|
||||
|
||||
#define M_SQRT12 SimdScalar(0.7071067811865475244008443621048490)
|
||||
|
||||
#define dRecipSqrt(x) ((float)(1.0f/SimdSqrt(float(x)))) /* reciprocal square root */
|
||||
|
||||
|
||||
|
||||
void dPlaneSpace1 (const dVector3 n, dVector3 p, dVector3 q)
|
||||
{
|
||||
if (SimdFabs(n[2]) > M_SQRT12) {
|
||||
// choose p in y-z plane
|
||||
SimdScalar a = n[1]*n[1] + n[2]*n[2];
|
||||
SimdScalar k = dRecipSqrt (a);
|
||||
p[0] = 0;
|
||||
p[1] = -n[2]*k;
|
||||
p[2] = n[1]*k;
|
||||
// set q = n x p
|
||||
q[0] = a*k;
|
||||
q[1] = -n[0]*p[2];
|
||||
q[2] = n[0]*p[1];
|
||||
}
|
||||
else {
|
||||
// choose p in x-y plane
|
||||
SimdScalar a = n[0]*n[0] + n[1]*n[1];
|
||||
SimdScalar k = dRecipSqrt (a);
|
||||
p[0] = -n[1]*k;
|
||||
p[1] = n[0]*k;
|
||||
p[2] = 0;
|
||||
// set q = n x p
|
||||
q[0] = -n[2]*p[1];
|
||||
q[1] = n[2]*p[0];
|
||||
q[2] = a*k;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void ContactJoint::GetInfo2(Info2 *info)
|
||||
{
|
||||
|
||||
int s = info->rowskip;
|
||||
int s2 = 2*s;
|
||||
|
||||
float swapFactor = m_swapBodies ? -1.f : 1.f;
|
||||
|
||||
// get normal, with sign adjusted for body1/body2 polarity
|
||||
dVector3 normal;
|
||||
|
||||
|
||||
ManifoldPoint& point = m_manifold->GetContactPoint(m_index);
|
||||
|
||||
normal[0] = swapFactor*point.m_normalWorldOnB[0];
|
||||
normal[1] = swapFactor*point.m_normalWorldOnB[1];
|
||||
normal[2] = swapFactor*point.m_normalWorldOnB[2];
|
||||
normal[3] = 0; // @@@ hmmm
|
||||
|
||||
// if (GetBody0())
|
||||
SimdVector3 relativePositionA;
|
||||
{
|
||||
relativePositionA = point.GetPositionWorldOnA() - m_body0->getCenterOfMassPosition();
|
||||
dVector3 c1;
|
||||
c1[0] = relativePositionA[0];
|
||||
c1[1] = relativePositionA[1];
|
||||
c1[2] = relativePositionA[2];
|
||||
|
||||
// set jacobian for normal
|
||||
info->J1l[0] = normal[0];
|
||||
info->J1l[1] = normal[1];
|
||||
info->J1l[2] = normal[2];
|
||||
dCROSS (info->J1a,=,c1,normal);
|
||||
|
||||
}
|
||||
// if (GetBody1())
|
||||
SimdVector3 relativePositionB;
|
||||
{
|
||||
dVector3 c2;
|
||||
relativePositionB = point.GetPositionWorldOnB() - m_body1->getCenterOfMassPosition();
|
||||
|
||||
// for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] -
|
||||
// j->node[1].body->pos[i];
|
||||
c2[0] = relativePositionB[0];
|
||||
c2[1] = relativePositionB[1];
|
||||
c2[2] = relativePositionB[2];
|
||||
|
||||
info->J2l[0] = -normal[0];
|
||||
info->J2l[1] = -normal[1];
|
||||
info->J2l[2] = -normal[2];
|
||||
dCROSS (info->J2a,= -,c2,normal);
|
||||
}
|
||||
|
||||
SimdScalar k = info->fps * info->erp;
|
||||
|
||||
float depth = -point.GetDistance();
|
||||
// if (depth < 0.f)
|
||||
// depth = 0.f;
|
||||
|
||||
info->c[0] = k * depth;
|
||||
//float maxvel = .2f;
|
||||
|
||||
// if (info->c[0] > maxvel)
|
||||
// info->c[0] = maxvel;
|
||||
|
||||
|
||||
//can override it, not necessary
|
||||
// info->cfm[0] = 0.f;
|
||||
// info->cfm[1] = 0.f;
|
||||
// info->cfm[2] = 0.f;
|
||||
|
||||
|
||||
|
||||
// set LCP limits for normal
|
||||
info->lo[0] = 0;
|
||||
info->hi[0] = 1e30f;//dInfinity;
|
||||
info->lo[1] = 0;
|
||||
info->hi[1] = 0.f;
|
||||
info->lo[2] = 0.f;
|
||||
info->hi[2] = 0.f;
|
||||
|
||||
#define DO_THE_FRICTION_2
|
||||
#ifdef DO_THE_FRICTION_2
|
||||
// now do jacobian for tangential forces
|
||||
dVector3 t1,t2; // two vectors tangential to normal
|
||||
|
||||
dVector3 c1;
|
||||
c1[0] = relativePositionA[0];
|
||||
c1[1] = relativePositionA[1];
|
||||
c1[2] = relativePositionA[2];
|
||||
|
||||
dVector3 c2;
|
||||
c2[0] = relativePositionB[0];
|
||||
c2[1] = relativePositionB[1];
|
||||
c2[2] = relativePositionB[2];
|
||||
|
||||
|
||||
float friction = FRICTION_CONSTANT*m_body0->getFriction() * m_body1->getFriction();
|
||||
|
||||
// first friction direction
|
||||
if (m_numRows >= 2)
|
||||
{
|
||||
|
||||
|
||||
|
||||
dPlaneSpace1 (normal,t1,t2);
|
||||
|
||||
info->J1l[s+0] = t1[0];
|
||||
info->J1l[s+1] = t1[1];
|
||||
info->J1l[s+2] = t1[2];
|
||||
dCROSS (info->J1a+s,=,c1,t1);
|
||||
if (1) { //j->node[1].body) {
|
||||
info->J2l[s+0] = -t1[0];
|
||||
info->J2l[s+1] = -t1[1];
|
||||
info->J2l[s+2] = -t1[2];
|
||||
dCROSS (info->J2a+s,= -,c2,t1);
|
||||
}
|
||||
// set right hand side
|
||||
if (0) {//j->contact.surface.mode & dContactMotion1) {
|
||||
//info->c[1] = j->contact.surface.motion1;
|
||||
}
|
||||
// set LCP bounds and friction index. this depends on the approximation
|
||||
// mode
|
||||
//1e30f
|
||||
|
||||
|
||||
info->lo[1] = -friction;//-j->contact.surface.mu;
|
||||
info->hi[1] = friction;//j->contact.surface.mu;
|
||||
if (1)//j->contact.surface.mode & dContactApprox1_1)
|
||||
info->findex[1] = 0;
|
||||
|
||||
// set slip (constraint force mixing)
|
||||
if (0)//j->contact.surface.mode & dContactSlip1)
|
||||
{
|
||||
// info->cfm[1] = j->contact.surface.slip1;
|
||||
} else
|
||||
{
|
||||
//info->cfm[1] = 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
// second friction direction
|
||||
if (m_numRows >= 3) {
|
||||
info->J1l[s2+0] = t2[0];
|
||||
info->J1l[s2+1] = t2[1];
|
||||
info->J1l[s2+2] = t2[2];
|
||||
dCROSS (info->J1a+s2,=,c1,t2);
|
||||
if (1) { //j->node[1].body) {
|
||||
info->J2l[s2+0] = -t2[0];
|
||||
info->J2l[s2+1] = -t2[1];
|
||||
info->J2l[s2+2] = -t2[2];
|
||||
dCROSS (info->J2a+s2,= -,c2,t2);
|
||||
}
|
||||
|
||||
// set right hand side
|
||||
if (0){//j->contact.surface.mode & dContactMotion2) {
|
||||
//info->c[2] = j->contact.surface.motion2;
|
||||
}
|
||||
// set LCP bounds and friction index. this depends on the approximation
|
||||
// mode
|
||||
if (0){//j->contact.surface.mode & dContactMu2) {
|
||||
//info->lo[2] = -j->contact.surface.mu2;
|
||||
//info->hi[2] = j->contact.surface.mu2;
|
||||
}
|
||||
else {
|
||||
info->lo[2] = -friction;
|
||||
info->hi[2] = friction;
|
||||
}
|
||||
if (0)//j->contact.surface.mode & dContactApprox1_2)
|
||||
|
||||
{
|
||||
info->findex[2] = 0;
|
||||
}
|
||||
// set slip (constraint force mixing)
|
||||
if (0) //j->contact.surface.mode & dContactSlip2)
|
||||
|
||||
{
|
||||
//info->cfm[2] = j->contact.surface.slip2;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif //DO_THE_FRICTION_2
|
||||
|
||||
}
|
||||
|
||||
50
BulletDynamics/Dynamics/ContactJoint.h
Normal file
50
BulletDynamics/Dynamics/ContactJoint.h
Normal file
@@ -0,0 +1,50 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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 CONTACT_JOINT_H
|
||||
#define CONTACT_JOINT_H
|
||||
|
||||
#include "BU_Joint.h"
|
||||
class RigidBody;
|
||||
class PersistentManifold;
|
||||
|
||||
class ContactJoint : public BU_Joint
|
||||
{
|
||||
PersistentManifold* m_manifold;
|
||||
int m_index;
|
||||
bool m_swapBodies;
|
||||
RigidBody* m_body0;
|
||||
RigidBody* m_body1;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
ContactJoint() {};
|
||||
|
||||
ContactJoint(PersistentManifold* manifold,int index,bool swap,RigidBody* body0,RigidBody* body1);
|
||||
|
||||
//BU_Joint interface for solver
|
||||
|
||||
virtual void GetInfo1(Info1 *info);
|
||||
|
||||
virtual void GetInfo2(Info2 *info);
|
||||
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //CONTACT_JOINT_H
|
||||
|
||||
33
BulletDynamics/Dynamics/MassProps.h
Normal file
33
BulletDynamics/Dynamics/MassProps.h
Normal file
@@ -0,0 +1,33 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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 MASS_PROPS_H
|
||||
#define MASS_PROPS_H
|
||||
|
||||
#include <SimdVector3.h>
|
||||
|
||||
struct MassProps {
|
||||
MassProps(float mass,const SimdVector3& inertiaLocal):
|
||||
m_mass(mass),
|
||||
m_inertiaLocal(inertiaLocal)
|
||||
{
|
||||
}
|
||||
float m_mass;
|
||||
SimdVector3 m_inertiaLocal;
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
220
BulletDynamics/Dynamics/RigidBody.cpp
Normal file
220
BulletDynamics/Dynamics/RigidBody.cpp
Normal file
@@ -0,0 +1,220 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#include "RigidBody.h"
|
||||
#include "MassProps.h"
|
||||
#include "CollisionShapes/ConvexShape.h"
|
||||
#include "GEN_MinMax.h"
|
||||
#include <SimdTransformUtil.h>
|
||||
|
||||
float gLinearAirDamping = 1.f;
|
||||
|
||||
static int uniqueId = 0;
|
||||
|
||||
RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping,SimdScalar friction,SimdScalar restitution)
|
||||
:
|
||||
m_gravity(0.0f, 0.0f, 0.0f),
|
||||
m_totalForce(0.0f, 0.0f, 0.0f),
|
||||
m_totalTorque(0.0f, 0.0f, 0.0f),
|
||||
m_linearVelocity(0.0f, 0.0f, 0.0f),
|
||||
m_angularVelocity(0.f,0.f,0.f),
|
||||
m_linearDamping(0.f),
|
||||
m_angularDamping(0.5f),
|
||||
m_friction(friction),
|
||||
m_restitution(restitution),
|
||||
m_kinematicTimeStep(0.f)
|
||||
{
|
||||
m_debugBodyId = uniqueId++;
|
||||
|
||||
setMassProps(massProps.m_mass, massProps.m_inertiaLocal);
|
||||
setDamping(linearDamping, angularDamping);
|
||||
m_worldTransform.setIdentity();
|
||||
updateInertiaTensor();
|
||||
|
||||
}
|
||||
|
||||
|
||||
void RigidBody::setLinearVelocity(const SimdVector3& lin_vel)
|
||||
{
|
||||
|
||||
m_linearVelocity = lin_vel;
|
||||
}
|
||||
|
||||
|
||||
void RigidBody::predictIntegratedTransform(SimdScalar timeStep,SimdTransform& predictedTransform) const
|
||||
{
|
||||
SimdTransformUtil::IntegrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
|
||||
}
|
||||
|
||||
void RigidBody::saveKinematicState(SimdScalar timeStep)
|
||||
{
|
||||
|
||||
if (m_kinematicTimeStep)
|
||||
{
|
||||
SimdVector3 linVel,angVel;
|
||||
SimdTransformUtil::CalculateVelocity(m_interpolationWorldTransform,m_worldTransform,m_kinematicTimeStep,m_linearVelocity,m_angularVelocity);
|
||||
//printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
|
||||
}
|
||||
|
||||
|
||||
m_interpolationWorldTransform = m_worldTransform;
|
||||
|
||||
m_kinematicTimeStep = timeStep;
|
||||
}
|
||||
|
||||
void RigidBody::getAabb(SimdVector3& aabbMin,SimdVector3& aabbMax) const
|
||||
{
|
||||
GetCollisionShape()->GetAabb(m_worldTransform,aabbMin,aabbMax);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void RigidBody::setGravity(const SimdVector3& acceleration)
|
||||
{
|
||||
if (m_inverseMass != 0.0f)
|
||||
{
|
||||
m_gravity = acceleration * (1.0f / m_inverseMass);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void RigidBody::setDamping(SimdScalar lin_damping, SimdScalar ang_damping)
|
||||
{
|
||||
m_linearDamping = GEN_clamped(lin_damping, 0.0f, 1.0f);
|
||||
m_angularDamping = GEN_clamped(ang_damping, 0.0f, 1.0f);
|
||||
}
|
||||
|
||||
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
void RigidBody::applyForces(SimdScalar step)
|
||||
{
|
||||
if (IsStatic())
|
||||
return;
|
||||
|
||||
|
||||
applyCentralForce(m_gravity);
|
||||
|
||||
m_linearVelocity *= GEN_clamped((1.f - step * gLinearAirDamping * m_linearDamping), 0.0f, 1.0f);
|
||||
m_angularVelocity *= GEN_clamped((1.f - step * m_angularDamping), 0.0f, 1.0f);
|
||||
|
||||
#define FORCE_VELOCITY_DAMPING 1
|
||||
#ifdef FORCE_VELOCITY_DAMPING
|
||||
float speed = m_linearVelocity.length();
|
||||
if (speed < m_linearDamping)
|
||||
{
|
||||
float dampVel = 0.005f;
|
||||
if (speed > dampVel)
|
||||
{
|
||||
SimdVector3 dir = m_linearVelocity.normalized();
|
||||
m_linearVelocity -= dir * dampVel;
|
||||
} else
|
||||
{
|
||||
m_linearVelocity.setValue(0.f,0.f,0.f);
|
||||
}
|
||||
}
|
||||
|
||||
float angSpeed = m_angularVelocity.length();
|
||||
if (angSpeed < m_angularDamping)
|
||||
{
|
||||
float angDampVel = 0.005f;
|
||||
if (angSpeed > angDampVel)
|
||||
{
|
||||
SimdVector3 dir = m_angularVelocity.normalized();
|
||||
m_angularVelocity -= dir * angDampVel;
|
||||
} else
|
||||
{
|
||||
m_angularVelocity.setValue(0.f,0.f,0.f);
|
||||
}
|
||||
}
|
||||
#endif //FORCE_VELOCITY_DAMPING
|
||||
|
||||
}
|
||||
|
||||
void RigidBody::proceedToTransform(const SimdTransform& newTrans)
|
||||
{
|
||||
setCenterOfMassTransform( newTrans );
|
||||
}
|
||||
|
||||
|
||||
void RigidBody::setMassProps(SimdScalar mass, const SimdVector3& inertia)
|
||||
{
|
||||
if (mass == 0.f)
|
||||
{
|
||||
m_collisionFlags = CollisionObject::isStatic;
|
||||
m_inverseMass = 0.f;
|
||||
} else
|
||||
{
|
||||
m_collisionFlags = 0;
|
||||
m_inverseMass = 1.0f / mass;
|
||||
}
|
||||
|
||||
|
||||
m_invInertiaLocal.setValue(inertia[0] != 0.0f ? 1.0f / inertia[0]: 0.0f,
|
||||
inertia[1] != 0.0f ? 1.0f / inertia[1]: 0.0f,
|
||||
inertia[2] != 0.0f ? 1.0f / inertia[2]: 0.0f);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void RigidBody::updateInertiaTensor()
|
||||
{
|
||||
m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
|
||||
}
|
||||
|
||||
|
||||
void RigidBody::integrateVelocities(SimdScalar step)
|
||||
{
|
||||
if (IsStatic())
|
||||
return;
|
||||
|
||||
m_linearVelocity += m_totalForce * (m_inverseMass * step);
|
||||
m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
|
||||
|
||||
#define MAX_ANGVEL SIMD_HALF_PI
|
||||
/// clamp angular velocity. collision calculations will fail on higher angular velocities
|
||||
float angvel = m_angularVelocity.length();
|
||||
if (angvel*step > MAX_ANGVEL)
|
||||
{
|
||||
m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
|
||||
}
|
||||
|
||||
clearForces();
|
||||
}
|
||||
|
||||
SimdQuaternion RigidBody::getOrientation() const
|
||||
{
|
||||
SimdQuaternion orn;
|
||||
m_worldTransform.getBasis().getRotation(orn);
|
||||
return orn;
|
||||
}
|
||||
|
||||
|
||||
void RigidBody::setCenterOfMassTransform(const SimdTransform& xform)
|
||||
{
|
||||
m_worldTransform = xform;
|
||||
updateInertiaTensor();
|
||||
}
|
||||
|
||||
|
||||
|
||||
274
BulletDynamics/Dynamics/RigidBody.h
Normal file
274
BulletDynamics/Dynamics/RigidBody.h
Normal file
@@ -0,0 +1,274 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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 RIGIDBODY_H
|
||||
#define RIGIDBODY_H
|
||||
|
||||
#include <vector>
|
||||
#include <SimdPoint3.h>
|
||||
#include <SimdTransform.h>
|
||||
#include "BroadphaseCollision/BroadphaseProxy.h"
|
||||
|
||||
|
||||
#include "CollisionDispatch/CollisionObject.h"
|
||||
|
||||
class CollisionShape;
|
||||
struct MassProps;
|
||||
typedef SimdScalar dMatrix3[4*3];
|
||||
|
||||
extern float gLinearAirDamping;
|
||||
extern bool gUseEpa;
|
||||
|
||||
#define MAX_RIGIDBODIES 8192
|
||||
|
||||
|
||||
/// RigidBody class for RigidBody Dynamics
|
||||
///
|
||||
class RigidBody : public CollisionObject
|
||||
{
|
||||
public:
|
||||
|
||||
RigidBody(const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping,SimdScalar friction,SimdScalar restitution);
|
||||
|
||||
void proceedToTransform(const SimdTransform& newTrans);
|
||||
|
||||
|
||||
/// continuous collision detection needs prediction
|
||||
void predictIntegratedTransform(SimdScalar step, SimdTransform& predictedTransform) const;
|
||||
|
||||
void saveKinematicState(SimdScalar step);
|
||||
|
||||
|
||||
void applyForces(SimdScalar step);
|
||||
|
||||
void setGravity(const SimdVector3& acceleration);
|
||||
|
||||
void setDamping(SimdScalar lin_damping, SimdScalar ang_damping);
|
||||
|
||||
inline const CollisionShape* GetCollisionShape() const {
|
||||
return m_collisionShape;
|
||||
}
|
||||
|
||||
inline CollisionShape* GetCollisionShape() {
|
||||
return m_collisionShape;
|
||||
}
|
||||
|
||||
void setMassProps(SimdScalar mass, const SimdVector3& inertia);
|
||||
|
||||
SimdScalar getInvMass() const { return m_inverseMass; }
|
||||
const SimdMatrix3x3& getInvInertiaTensorWorld() const {
|
||||
return m_invInertiaTensorWorld;
|
||||
}
|
||||
|
||||
void integrateVelocities(SimdScalar step);
|
||||
|
||||
void setCenterOfMassTransform(const SimdTransform& xform);
|
||||
|
||||
void applyCentralForce(const SimdVector3& force)
|
||||
{
|
||||
m_totalForce += force;
|
||||
}
|
||||
|
||||
const SimdVector3& getInvInertiaDiagLocal()
|
||||
{
|
||||
return m_invInertiaLocal;
|
||||
};
|
||||
|
||||
void setInvInertiaDiagLocal(const SimdVector3& diagInvInertia)
|
||||
{
|
||||
m_invInertiaLocal = diagInvInertia;
|
||||
}
|
||||
|
||||
void applyTorque(const SimdVector3& torque)
|
||||
{
|
||||
m_totalTorque += torque;
|
||||
}
|
||||
|
||||
void applyForce(const SimdVector3& force, const SimdVector3& rel_pos)
|
||||
{
|
||||
applyCentralForce(force);
|
||||
applyTorque(rel_pos.cross(force));
|
||||
}
|
||||
|
||||
void applyCentralImpulse(const SimdVector3& impulse)
|
||||
{
|
||||
m_linearVelocity += impulse * m_inverseMass;
|
||||
}
|
||||
|
||||
void applyTorqueImpulse(const SimdVector3& torque)
|
||||
{
|
||||
if (!IsStatic())
|
||||
m_angularVelocity += m_invInertiaTensorWorld * torque;
|
||||
|
||||
}
|
||||
|
||||
void applyImpulse(const SimdVector3& impulse, const SimdVector3& rel_pos)
|
||||
{
|
||||
if (m_inverseMass != 0.f)
|
||||
{
|
||||
applyCentralImpulse(impulse);
|
||||
applyTorqueImpulse(rel_pos.cross(impulse));
|
||||
}
|
||||
}
|
||||
|
||||
void clearForces()
|
||||
{
|
||||
m_totalForce.setValue(0.0f, 0.0f, 0.0f);
|
||||
m_totalTorque.setValue(0.0f, 0.0f, 0.0f);
|
||||
}
|
||||
|
||||
void updateInertiaTensor();
|
||||
|
||||
const SimdPoint3& getCenterOfMassPosition() const {
|
||||
return m_worldTransform.getOrigin();
|
||||
}
|
||||
SimdQuaternion getOrientation() const;
|
||||
|
||||
const SimdTransform& getCenterOfMassTransform() const {
|
||||
return m_worldTransform;
|
||||
}
|
||||
const SimdVector3& getLinearVelocity() const {
|
||||
return m_linearVelocity;
|
||||
}
|
||||
const SimdVector3& getAngularVelocity() const {
|
||||
return m_angularVelocity;
|
||||
}
|
||||
|
||||
|
||||
void setLinearVelocity(const SimdVector3& lin_vel);
|
||||
void setAngularVelocity(const SimdVector3& ang_vel) {
|
||||
if (!IsStatic())
|
||||
{
|
||||
m_angularVelocity = ang_vel;
|
||||
}
|
||||
}
|
||||
|
||||
SimdVector3 getVelocityInLocalPoint(const SimdVector3& rel_pos) const
|
||||
{
|
||||
//we also calculate lin/ang velocity for kinematic objects
|
||||
return m_linearVelocity + m_angularVelocity.cross(rel_pos);
|
||||
|
||||
//for kinematic objects, we could also use use:
|
||||
// return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
|
||||
}
|
||||
|
||||
void translate(const SimdVector3& v)
|
||||
{
|
||||
m_worldTransform.getOrigin() += v;
|
||||
}
|
||||
|
||||
|
||||
void getAabb(SimdVector3& aabbMin,SimdVector3& aabbMax) const;
|
||||
|
||||
|
||||
void setRestitution(float rest)
|
||||
{
|
||||
m_restitution = rest;
|
||||
}
|
||||
float getRestitution() const
|
||||
{
|
||||
return m_restitution;
|
||||
}
|
||||
void setFriction(float frict)
|
||||
{
|
||||
m_friction = frict;
|
||||
}
|
||||
float getFriction() const
|
||||
{
|
||||
return m_friction;
|
||||
}
|
||||
|
||||
|
||||
inline float ComputeImpulseDenominator(const SimdPoint3& pos, const SimdVector3& normal) const
|
||||
{
|
||||
SimdVector3 r0 = pos - getCenterOfMassPosition();
|
||||
|
||||
SimdVector3 c0 = (r0).cross(normal);
|
||||
|
||||
SimdVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0);
|
||||
|
||||
return m_inverseMass + normal.dot(vec);
|
||||
|
||||
}
|
||||
|
||||
inline float ComputeAngularImpulseDenominator(const SimdVector3& axis) const
|
||||
{
|
||||
SimdVector3 vec = axis * getInvInertiaTensorWorld();
|
||||
return axis.dot(vec);
|
||||
}
|
||||
|
||||
|
||||
|
||||
private:
|
||||
|
||||
SimdMatrix3x3 m_invInertiaTensorWorld;
|
||||
SimdVector3 m_gravity;
|
||||
SimdVector3 m_invInertiaLocal;
|
||||
SimdVector3 m_totalForce;
|
||||
SimdVector3 m_totalTorque;
|
||||
// SimdQuaternion m_orn1;
|
||||
|
||||
SimdVector3 m_linearVelocity;
|
||||
|
||||
SimdVector3 m_angularVelocity;
|
||||
|
||||
SimdScalar m_linearDamping;
|
||||
SimdScalar m_angularDamping;
|
||||
SimdScalar m_inverseMass;
|
||||
|
||||
SimdScalar m_friction;
|
||||
SimdScalar m_restitution;
|
||||
|
||||
SimdScalar m_kinematicTimeStep;
|
||||
|
||||
BroadphaseProxy* m_broadphaseProxy;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
public:
|
||||
const BroadphaseProxy* GetBroadphaseProxy() const
|
||||
{
|
||||
return m_broadphaseProxy;
|
||||
}
|
||||
BroadphaseProxy* GetBroadphaseProxy()
|
||||
{
|
||||
return m_broadphaseProxy;
|
||||
}
|
||||
void SetBroadphaseProxy(BroadphaseProxy* broadphaseProxy)
|
||||
{
|
||||
m_broadphaseProxy = broadphaseProxy;
|
||||
}
|
||||
|
||||
|
||||
/// for ode solver-binding
|
||||
dMatrix3 m_R;//temp
|
||||
dMatrix3 m_I;
|
||||
dMatrix3 m_invI;
|
||||
|
||||
int m_odeTag;
|
||||
float m_padding[1024];
|
||||
SimdVector3 m_tacc;//temp
|
||||
SimdVector3 m_facc;
|
||||
|
||||
|
||||
int m_debugBodyId;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user