small cleanup in the DemoApplication, removed references to obsolete PHY_ShapeProps/PHY_MaterialProps
removed deletion of motionstate in CcdPhysics/CcdPhysicsController destructor disabled m_type optimization in SimdTransform (making the memory size 64 byte, potentially more cache friendly) fixed a bug in island generation, causing the activation not propagating in one frame, but one 'layer' of rigidbodies at a time
This commit is contained in:
@@ -612,43 +612,19 @@ CcdPhysicsController* DemoApplication::LocalCreatePhysicsObject(bool isDynamic,
|
||||
|
||||
startTransforms[numObjects] = startTransform;
|
||||
|
||||
PHY_ShapeProps shapeProps;
|
||||
|
||||
shapeProps.m_do_anisotropic = false;
|
||||
shapeProps.m_do_fh = false;
|
||||
shapeProps.m_do_rot_fh = false;
|
||||
shapeProps.m_friction_scaling[0] = 1.;
|
||||
shapeProps.m_friction_scaling[1] = 1.;
|
||||
shapeProps.m_friction_scaling[2] = 1.;
|
||||
|
||||
shapeProps.m_inertia = 1.f;
|
||||
shapeProps.m_lin_drag = 0.2f;
|
||||
shapeProps.m_ang_drag = 0.1f;
|
||||
shapeProps.m_mass = 10.0f;
|
||||
|
||||
PHY_MaterialProps materialProps;
|
||||
materialProps.m_friction = 10.5f;
|
||||
materialProps.m_restitution = 0.0f;
|
||||
|
||||
CcdConstructionInfo ccdObjectCi;
|
||||
ccdObjectCi.m_friction = 0.5f;
|
||||
|
||||
ccdObjectCi.m_linearDamping = shapeProps.m_lin_drag;
|
||||
ccdObjectCi.m_angularDamping = shapeProps.m_ang_drag;
|
||||
|
||||
SimdTransform tr;
|
||||
tr.setIdentity();
|
||||
|
||||
int i = numObjects;
|
||||
{
|
||||
gShapePtr[i] = shape;
|
||||
|
||||
shapeProps.m_shape = gShapePtr[i];
|
||||
shapeProps.m_shape->SetMargin(0.05f);
|
||||
gShapePtr[i]->SetMargin(0.05f);
|
||||
|
||||
SimdQuaternion orn = startTransform.getRotation();
|
||||
|
||||
|
||||
ms[i].setWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
|
||||
ms[i].setWorldPosition(startTransform.getOrigin().getX(),startTransform.getOrigin().getY(),startTransform.getOrigin().getZ());
|
||||
|
||||
@@ -657,25 +633,22 @@ CcdPhysicsController* DemoApplication::LocalCreatePhysicsObject(bool isDynamic,
|
||||
ccdObjectCi.m_localInertiaTensor =SimdVector3(0,0,0);
|
||||
if (!isDynamic)
|
||||
{
|
||||
shapeProps.m_mass = 0.f;
|
||||
ccdObjectCi.m_mass = shapeProps.m_mass;
|
||||
ccdObjectCi.m_mass = 0.f;
|
||||
ccdObjectCi.m_collisionFlags = CollisionObject::isStatic;
|
||||
ccdObjectCi.m_collisionFilterGroup = CcdConstructionInfo::StaticFilter;
|
||||
ccdObjectCi.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::StaticFilter;
|
||||
}
|
||||
else
|
||||
{
|
||||
shapeProps.m_mass = mass;
|
||||
ccdObjectCi.m_mass = shapeProps.m_mass;
|
||||
ccdObjectCi.m_mass = mass;
|
||||
ccdObjectCi.m_collisionFlags = 0;
|
||||
}
|
||||
|
||||
|
||||
SimdVector3 localInertia(0.f,0.f,0.f);
|
||||
|
||||
if (isDynamic)
|
||||
{
|
||||
gShapePtr[i]->CalculateLocalInertia(shapeProps.m_mass,localInertia);
|
||||
gShapePtr[i]->CalculateLocalInertia(ccdObjectCi.m_mass,localInertia);
|
||||
}
|
||||
|
||||
ccdObjectCi.m_localInertiaTensor = localInertia;
|
||||
@@ -727,11 +700,9 @@ void DemoApplication::renderme()
|
||||
|
||||
|
||||
bool isSatEnabled = (getDebugMode() & IDebugDraw::DBG_EnableSatComparison);
|
||||
|
||||
m_physicsEnvironmentPtr->EnableSatCollisionDetection(isSatEnabled);
|
||||
|
||||
|
||||
|
||||
int numPhysicsObjects = m_physicsEnvironmentPtr->GetNumControllers();
|
||||
|
||||
int i;
|
||||
@@ -772,7 +743,7 @@ void DemoApplication::renderme()
|
||||
}
|
||||
|
||||
char extraDebug[125];
|
||||
sprintf(extraDebug,"islId, Body=%i , %i",ctrl->GetRigidBody()->m_islandTag1,ctrl->GetRigidBody()->m_debugBodyId);
|
||||
sprintf(extraDebug,"Island:%i, Body:%i",ctrl->GetRigidBody()->m_islandTag1,ctrl->GetRigidBody()->m_debugBodyId);
|
||||
ctrl->GetRigidBody()->GetCollisionShape()->SetExtraDebugInfo(extraDebug);
|
||||
|
||||
float vec[16];
|
||||
|
||||
@@ -330,18 +330,19 @@ void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const Sim
|
||||
|
||||
}
|
||||
|
||||
/*glDisable(GL_DEPTH_BUFFER_BIT);
|
||||
if (debugMode==IDebugDraw::DBG_DrawText)
|
||||
glDisable(GL_DEPTH_BUFFER_BIT);
|
||||
glRasterPos3f(0,0,0);//mvtx.x(), vtx.y(), vtx.z());
|
||||
if (debugMode&IDebugDraw::DBG_DrawText)
|
||||
{
|
||||
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),shape->GetName());
|
||||
}
|
||||
|
||||
if (debugMode==IDebugDraw::DBG_DrawFeaturesText)
|
||||
if (debugMode&IDebugDraw::DBG_DrawFeaturesText)
|
||||
{
|
||||
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),shape->GetExtraDebugInfo());
|
||||
}
|
||||
glEnable(GL_DEPTH_BUFFER_BIT);
|
||||
*/
|
||||
|
||||
|
||||
// glPopMatrix();
|
||||
}
|
||||
|
||||
@@ -107,7 +107,7 @@ CcdPhysicsController::~CcdPhysicsController()
|
||||
if (m_cci.m_physicsEnv)
|
||||
m_cci.m_physicsEnv->removeCcdPhysicsController(this);
|
||||
|
||||
delete m_MotionState;
|
||||
///this should not be deleted, it has been allocated by the user//delete m_MotionState;
|
||||
delete m_body;
|
||||
}
|
||||
|
||||
|
||||
@@ -52,5 +52,6 @@ void CollisionObject::activate()
|
||||
|
||||
bool CollisionObject::mergesSimulationIslands() const
|
||||
{
|
||||
return ( !(m_collisionFlags & isStatic));
|
||||
//static objects, and object without contact response don't merge islands
|
||||
return ( !(m_collisionFlags & (isStatic |noContactResponse )));
|
||||
}
|
||||
|
||||
@@ -34,6 +34,8 @@ class CollisionShape;
|
||||
struct CollisionObject
|
||||
{
|
||||
SimdTransform m_worldTransform;
|
||||
BroadphaseProxy* m_broadphaseHandle;
|
||||
CollisionShape* m_collisionShape;
|
||||
|
||||
//m_interpolationWorldTransform is used for CCD and interpolation
|
||||
//it can be either previous or future (predicted) transform
|
||||
@@ -57,8 +59,7 @@ struct CollisionObject
|
||||
SimdScalar m_friction;
|
||||
SimdScalar m_restitution;
|
||||
|
||||
BroadphaseProxy* m_broadphaseHandle;
|
||||
CollisionShape* m_collisionShape;
|
||||
|
||||
|
||||
//users can point to their objects, m_userPointer is not used by Bullet
|
||||
void* m_userObjectPointer;
|
||||
|
||||
@@ -36,8 +36,6 @@ void SimulationIslandManager::FindUnions(Dispatcher* dispatcher)
|
||||
const CollisionObject* colObj0 = static_cast<const CollisionObject*>(manifold->GetBody0());
|
||||
const CollisionObject* colObj1 = static_cast<const CollisionObject*>(manifold->GetBody1());
|
||||
|
||||
if (colObj0 && colObj1 && dispatcher->NeedsResponse(*colObj0,*colObj1))
|
||||
{
|
||||
if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
|
||||
((colObj1) && ((colObj1)->mergesSimulationIslands())))
|
||||
{
|
||||
@@ -45,7 +43,6 @@ void SimulationIslandManager::FindUnions(Dispatcher* dispatcher)
|
||||
m_unionFind.unite((colObj0)->m_islandTag1,
|
||||
(colObj1)->m_islandTag1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -37,6 +37,24 @@ extern bool gUseEpa;
|
||||
///
|
||||
class RigidBody : public CollisionObject
|
||||
{
|
||||
|
||||
SimdMatrix3x3 m_invInertiaTensorWorld;
|
||||
SimdVector3 m_linearVelocity;
|
||||
SimdVector3 m_angularVelocity;
|
||||
SimdScalar m_inverseMass;
|
||||
|
||||
SimdVector3 m_gravity;
|
||||
SimdVector3 m_invInertiaLocal;
|
||||
SimdVector3 m_totalForce;
|
||||
SimdVector3 m_totalTorque;
|
||||
|
||||
SimdScalar m_linearDamping;
|
||||
SimdScalar m_angularDamping;
|
||||
|
||||
SimdScalar m_kinematicTimeStep;
|
||||
|
||||
BroadphaseProxy* m_broadphaseProxy;
|
||||
|
||||
public:
|
||||
|
||||
RigidBody(const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping,SimdScalar friction,SimdScalar restitution);
|
||||
@@ -197,29 +215,6 @@ public:
|
||||
|
||||
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_kinematicTimeStep;
|
||||
|
||||
BroadphaseProxy* m_broadphaseProxy;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
public:
|
||||
@@ -240,7 +235,8 @@ public:
|
||||
int m_contactSolverType;
|
||||
int m_frictionSolverType;
|
||||
|
||||
|
||||
/*
|
||||
//unused
|
||||
/// for ode solver-binding
|
||||
dMatrix3 m_R;//temp
|
||||
dMatrix3 m_I;
|
||||
@@ -250,7 +246,7 @@ public:
|
||||
|
||||
SimdVector3 m_tacc;//temp
|
||||
SimdVector3 m_facc;
|
||||
|
||||
*/
|
||||
|
||||
|
||||
int m_debugBodyId;
|
||||
|
||||
@@ -21,6 +21,7 @@ subject to the following restrictions:
|
||||
#include "LinearMath/SimdMatrix3x3.h"
|
||||
|
||||
|
||||
#define IGNORE_TYPE 1
|
||||
|
||||
class SimdTransform {
|
||||
|
||||
@@ -41,37 +42,49 @@ public:
|
||||
explicit SIMD_FORCE_INLINE SimdTransform(const SimdQuaternion& q,
|
||||
const SimdVector3& c = SimdVector3(SimdScalar(0), SimdScalar(0), SimdScalar(0)))
|
||||
: m_basis(q),
|
||||
m_origin(c),
|
||||
m_type(RIGID)
|
||||
m_origin(c)
|
||||
#ifndef IGNORE_TYPE
|
||||
, m_type(RIGID)
|
||||
#endif //IGNORE_TYPE
|
||||
{}
|
||||
|
||||
explicit SIMD_FORCE_INLINE SimdTransform(const SimdMatrix3x3& b,
|
||||
const SimdVector3& c = SimdVector3(SimdScalar(0), SimdScalar(0), SimdScalar(0)),
|
||||
unsigned int type = AFFINE)
|
||||
: m_basis(b),
|
||||
m_origin(c),
|
||||
m_type(type)
|
||||
m_origin(c)
|
||||
#ifndef IGNORE_TYPE
|
||||
, m_type(type)
|
||||
#endif //IGNORE_TYPE
|
||||
{}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE void mult(const SimdTransform& t1, const SimdTransform& t2) {
|
||||
m_basis = t1.m_basis * t2.m_basis;
|
||||
m_origin = t1(t2.m_origin);
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type = t1.m_type | t2.m_type;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
void multInverseLeft(const SimdTransform& t1, const SimdTransform& t2) {
|
||||
SimdVector3 v = t2.m_origin - t1.m_origin;
|
||||
#ifndef IGNORE_TYPE
|
||||
if (t1.m_type & SCALING) {
|
||||
SimdMatrix3x3 inv = t1.m_basis.inverse();
|
||||
m_basis = inv * t2.m_basis;
|
||||
m_origin = inv * v;
|
||||
}
|
||||
else {
|
||||
else
|
||||
#else
|
||||
{
|
||||
m_basis = SimdMultTransposeLeft(t1.m_basis, t2.m_basis);
|
||||
m_origin = v * t1.m_basis;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type = t1.m_type | t2.m_type;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3 operator()(const SimdVector3& x) const
|
||||
@@ -102,7 +115,9 @@ public:
|
||||
{
|
||||
m_basis.setValue(m);
|
||||
m_origin.setValue(&m[12]);
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type = AFFINE;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
|
||||
@@ -126,7 +141,9 @@ public:
|
||||
SIMD_FORCE_INLINE void setOrigin(const SimdVector3& origin)
|
||||
{
|
||||
m_origin = origin;
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type |= TRANSLATION;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3 invXform(const SimdVector3& inVec) const;
|
||||
@@ -136,40 +153,60 @@ public:
|
||||
SIMD_FORCE_INLINE void setBasis(const SimdMatrix3x3& basis)
|
||||
{
|
||||
m_basis = basis;
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type |= LINEAR;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void setRotation(const SimdQuaternion& q)
|
||||
{
|
||||
m_basis.setRotation(q);
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type = (m_type & ~LINEAR) | ROTATION;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void scale(const SimdVector3& scaling)
|
||||
{
|
||||
m_basis = m_basis.scaled(scaling);
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type |= SCALING;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
void setIdentity()
|
||||
{
|
||||
m_basis.setIdentity();
|
||||
m_origin.setValue(SimdScalar(0.0), SimdScalar(0.0), SimdScalar(0.0));
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type = 0x0;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE bool isIdentity() const { return m_type == 0x0; }
|
||||
SIMD_FORCE_INLINE bool isIdentity() const {
|
||||
#ifdef IGNORE_TYPE
|
||||
return false;
|
||||
#else
|
||||
return m_type == 0x0;
|
||||
#endif
|
||||
}
|
||||
|
||||
SimdTransform& operator*=(const SimdTransform& t)
|
||||
{
|
||||
m_origin += m_basis * t.m_origin;
|
||||
m_basis *= t.m_basis;
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type |= t.m_type;
|
||||
#endif //IGNORE_TYPE
|
||||
return *this;
|
||||
}
|
||||
|
||||
SimdTransform inverse() const
|
||||
{
|
||||
#ifdef IGNORE_TYPE
|
||||
SimdMatrix3x3 inv = m_basis.transpose();
|
||||
return SimdTransform(inv, inv * -m_origin, 0);
|
||||
#else
|
||||
if (m_type)
|
||||
{
|
||||
SimdMatrix3x3 inv = (m_type & SCALING) ?
|
||||
@@ -180,6 +217,7 @@ public:
|
||||
}
|
||||
|
||||
return *this;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
SimdTransform inverseTimes(const SimdTransform& t) const;
|
||||
@@ -190,7 +228,9 @@ private:
|
||||
|
||||
SimdMatrix3x3 m_basis;
|
||||
SimdVector3 m_origin;
|
||||
#ifndef IGNORE_TYPE
|
||||
unsigned int m_type;
|
||||
#endif //IGNORE_TYPE
|
||||
};
|
||||
|
||||
|
||||
@@ -205,6 +245,7 @@ SIMD_FORCE_INLINE SimdTransform
|
||||
SimdTransform::inverseTimes(const SimdTransform& t) const
|
||||
{
|
||||
SimdVector3 v = t.getOrigin() - m_origin;
|
||||
#ifndef IGNORE_TYPE
|
||||
if (m_type & SCALING)
|
||||
{
|
||||
SimdMatrix3x3 inv = m_basis.inverse();
|
||||
@@ -212,10 +253,12 @@ SimdTransform::inverseTimes(const SimdTransform& t) const
|
||||
m_type | t.m_type);
|
||||
}
|
||||
else
|
||||
#else
|
||||
{
|
||||
return SimdTransform(m_basis.transposeTimes(t.m_basis),
|
||||
v * m_basis, m_type | t.m_type);
|
||||
v * m_basis, 0);
|
||||
}
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdTransform
|
||||
@@ -223,7 +266,11 @@ SimdTransform::operator*(const SimdTransform& t) const
|
||||
{
|
||||
return SimdTransform(m_basis * t.m_basis,
|
||||
(*this)(t.m_origin),
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type | t.m_type);
|
||||
#else
|
||||
0);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user