From c4ad354ac045cd241c816c1a9911ccb6f94a0cc7 Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Mon, 13 Jul 2009 21:48:19 +0000 Subject: [PATCH] More GJK degeneracy fixes, thanks Jacob Langford for the feedback: http://code.google.com/p/bullet/issues/detail?id=250 Added missing files for Maya Dynamica plugin Thanks Herbert Law for the patch, and damrit and others for the report http://code.google.com/p/bullet/issues/detail?id=231 Fix btQuaternion shortestArcQuat, thanks Stan Melax for original fix and shogun for reminder http://bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1989 Implemented btDiscreteDynamicsWorld::removeCollisionObject (and btSoftBodyDynamicsWorld) to avoid crashes. Thanks Jacob Langford for bringing that up. Minor sphere-debug drawing issue (spheres were drawn inside-out (wrong face winding) --- Demos/OpenGL/DemoApplication.cpp | 64 +++++----- Demos/OpenGL/GL_ShapeDrawer.cpp | 5 +- Extras/MayaPlugin/constraint/bt_constraint.h | 54 ++++++++ .../MayaPlugin/constraint/hinge_constraint.h | 118 ++++++++++++++++++ .../constraint/slider_constraint_impl.h | 50 ++++++++ .../btGjkPairDetector.cpp | 10 +- .../Dynamics/btDiscreteDynamicsWorld.cpp | 10 +- .../Dynamics/btDiscreteDynamicsWorld.h | 3 + .../Dynamics/btSimpleDynamicsWorld.cpp | 12 +- .../Dynamics/btSimpleDynamicsWorld.h | 3 + .../btSoftRigidDynamicsWorld.cpp | 9 ++ src/BulletSoftBody/btSoftRigidDynamicsWorld.h | 3 + src/LinearMath/btQuaternion.h | 6 +- src/LinearMath/btScalar.h | 4 + src/LinearMath/btTransformUtil.h | 23 +--- src/LinearMath/btVector3.h | 20 +++ 16 files changed, 329 insertions(+), 65 deletions(-) create mode 100644 Extras/MayaPlugin/constraint/bt_constraint.h create mode 100644 Extras/MayaPlugin/constraint/hinge_constraint.h create mode 100644 Extras/MayaPlugin/constraint/slider_constraint_impl.h diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp index 1f4fd9770..157da252a 100644 --- a/Demos/OpenGL/DemoApplication.cpp +++ b/Demos/OpenGL/DemoApplication.cpp @@ -1354,47 +1354,47 @@ void DemoApplication::clientResetScene() if (m_dynamicsWorld) { numObjects = m_dynamicsWorld->getNumCollisionObjects(); - } - - ///create a copy of the array, not a reference! - btCollisionObjectArray copyArray = m_dynamicsWorld->getCollisionObjectArray(); - + ///create a copy of the array, not a reference! + btCollisionObjectArray copyArray = m_dynamicsWorld->getCollisionObjectArray(); + + - for (i=0;igetMotionState()) - { - btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState(); - myMotionState->m_graphicsWorldTrans = myMotionState->m_startWorldTrans; - body->setCenterOfMassTransform( myMotionState->m_graphicsWorldTrans ); - colObj->setInterpolationWorldTransform( myMotionState->m_startWorldTrans ); - colObj->forceActivationState(ACTIVE_TAG); - colObj->activate(); - colObj->setDeactivationTime(0); - //colObj->setActivationState(WANTS_DEACTIVATION); - } - //removed cached contact points (this is not necessary if all objects have been removed from the dynamics world) - //m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(colObj->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher()); - + btCollisionObject* colObj = copyArray[i]; btRigidBody* body = btRigidBody::upcast(colObj); - if (body && !body->isStaticObject()) + if (body) { - btRigidBody::upcast(colObj)->setLinearVelocity(btVector3(0,0,0)); - btRigidBody::upcast(colObj)->setAngularVelocity(btVector3(0,0,0)); + if (body->getMotionState()) + { + btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState(); + myMotionState->m_graphicsWorldTrans = myMotionState->m_startWorldTrans; + body->setCenterOfMassTransform( myMotionState->m_graphicsWorldTrans ); + colObj->setInterpolationWorldTransform( myMotionState->m_startWorldTrans ); + colObj->forceActivationState(ACTIVE_TAG); + colObj->activate(); + colObj->setDeactivationTime(0); + //colObj->setActivationState(WANTS_DEACTIVATION); + } + //removed cached contact points (this is not necessary if all objects have been removed from the dynamics world) + //m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(colObj->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher()); + + btRigidBody* body = btRigidBody::upcast(colObj); + if (body && !body->isStaticObject()) + { + btRigidBody::upcast(colObj)->setLinearVelocity(btVector3(0,0,0)); + btRigidBody::upcast(colObj)->setAngularVelocity(btVector3(0,0,0)); + } } + } + ///reset some internal cached data in the broadphase + m_dynamicsWorld->getBroadphase()->resetPool(getDynamicsWorld()->getDispatcher()); + m_dynamicsWorld->getConstraintSolver()->reset(); + } - ///reset some internal cached data in the broadphase - m_dynamicsWorld->getBroadphase()->resetPool(getDynamicsWorld()->getDispatcher()); - m_dynamicsWorld->getConstraintSolver()->reset(); - - } diff --git a/Demos/OpenGL/GL_ShapeDrawer.cpp b/Demos/OpenGL/GL_ShapeDrawer.cpp index e882cc498..4da0c527f 100644 --- a/Demos/OpenGL/GL_ShapeDrawer.cpp +++ b/Demos/OpenGL/GL_ShapeDrawer.cpp @@ -290,11 +290,10 @@ void GL_ShapeDrawer::drawSphere(btScalar radius, int lats, int longs) btScalar lng = 2 * SIMD_PI * (btScalar) (j - 1) / longs; btScalar x = cos(lng); btScalar y = sin(lng); - - glNormal3f(x * zr0, y * zr0, z0); - glVertex3f(x * zr0, y * zr0, z0); glNormal3f(x * zr1, y * zr1, z1); glVertex3f(x * zr1, y * zr1, z1); + glNormal3f(x * zr0, y * zr0, z0); + glVertex3f(x * zr0, y * zr0, z0); } glEnd(); } diff --git a/Extras/MayaPlugin/constraint/bt_constraint.h b/Extras/MayaPlugin/constraint/bt_constraint.h new file mode 100644 index 000000000..ab4fa149d --- /dev/null +++ b/Extras/MayaPlugin/constraint/bt_constraint.h @@ -0,0 +1,54 @@ +/* +Bullet Continuous Collision Detection and Physics Library Maya Plugin +Copyright (c) 2008 Walt Disney Studios + +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. + +Written by: Nicola Candussi +*/ + +//bt_constraint.h + +#ifndef DYN_BT_CONSTRAINT_H +#define DYN_BT_CONSTRAINT_H + +#include "btBulletCollisionCommon.h" +#include "btBulletDynamicsCommon.h" +#include "shared_ptr.h" + +class bt_constraint_t +{ +public: + +protected: + friend class bt_solver_t; + + bt_constraint_t() { } + + btTypedConstraint* constraint() { return m_constraint.get(); } + void set_constraint(btTypedConstraint *constraint) { return m_constraint.reset(constraint); } + virtual void update_constraint() = 0; + +public: + friend class bt_rigid_body_t; + + virtual ~bt_constraint_t() { } + +protected: + shared_ptr m_constraint; +}; + +#endif diff --git a/Extras/MayaPlugin/constraint/hinge_constraint.h b/Extras/MayaPlugin/constraint/hinge_constraint.h new file mode 100644 index 000000000..b3f625050 --- /dev/null +++ b/Extras/MayaPlugin/constraint/hinge_constraint.h @@ -0,0 +1,118 @@ +/* +Bullet Continuous Collision Detection and Physics Library Maya Plugin +Copyright (c) 2008 Herbert Law + +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. + +Written by: Herbert Law +*/ + +//hinge_constraint.h + +#ifndef DYN_HINGE_CONSTRAINT_H +#define DYN_HINGE_CONSTRAINT_H + +#include "shared_ptr.h" +#include "rigid_body.h" +#include "mathUtils.h" + +#include "constraint.h" +#include "hinge_constraint_impl.h" + +class hinge_constraint_t: public constraint_t +{ +public: + //typedefs + typedef shared_ptr pointer; + + // + rigid_body_t::pointer rigid_body() { return m_rigid_body; } + + // + void set_pivot(vec3f const& p) + { + hinge_constraint_impl_t* hinge_impl = dynamic_cast(impl()); + hinge_impl->set_pivot(p); + } + + //local space pivot + void get_pivot(vec3f& p) const { + hinge_constraint_impl_t const* hinge_impl = dynamic_cast(impl()); + hinge_impl->get_pivot(p); + } + + // + void get_world_pivot(vec3f& p) const { + hinge_constraint_impl_t const* hinge_impl = dynamic_cast(impl()); + hinge_impl->get_world_pivot(p); + } + + // + void set_damping(float d) { + hinge_constraint_impl_t* hinge_impl = dynamic_cast(impl()); + hinge_impl->set_damping(d); + } + + void set_limit(float lower, float upper, float softness, float bias_factor, float relaxation_factor) { + hinge_constraint_impl_t* hinge_impl = dynamic_cast(impl()); + hinge_impl->set_limit(lower, upper, softness, bias_factor, relaxation_factor); + } + + void set_axis(vec3f const& p) { + hinge_constraint_impl_t* hinge_impl = dynamic_cast(impl()); + hinge_impl->set_axis(p); + } + + float damping() const { + hinge_constraint_impl_t const* hinge_impl = dynamic_cast(impl()); + return hinge_impl->damping(); + } + + void set_world(vec3f const& p) + { + hinge_constraint_impl_t* hinge_impl = dynamic_cast(impl()); + hinge_impl->set_world(p); + } + + //local space pivot + void get_world(vec3f& p) const { + hinge_constraint_impl_t const* hinge_impl = dynamic_cast(impl()); + hinge_impl->get_world(p); + } + + void enable_motor(bool enable, float velocity, float impulse) { + hinge_constraint_impl_t* hinge_impl = dynamic_cast(impl()); + hinge_impl->enable_motor(enable, velocity, impulse); + } + +public: + virtual ~hinge_constraint_t() {}; + +protected: + friend class solver_t; + hinge_constraint_t(hinge_constraint_impl_t* impl, rigid_body_t::pointer& rigid_body): + constraint_t(impl), + m_rigid_body(rigid_body) + { + } + +private: + rigid_body_t::pointer m_rigid_body; +}; + + + +#endif //DYN_HINGE_CONSTRAINT_H diff --git a/Extras/MayaPlugin/constraint/slider_constraint_impl.h b/Extras/MayaPlugin/constraint/slider_constraint_impl.h new file mode 100644 index 000000000..f6d6a476d --- /dev/null +++ b/Extras/MayaPlugin/constraint/slider_constraint_impl.h @@ -0,0 +1,50 @@ +/* +Bullet Continuous Collision Detection and Physics Library Maya Plugin +Copyright (c) 2008 Herbert Law + +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. + +Written by: Herbert Law +*/ + +//slider_constraint_impl.h + +#ifndef DYN_SLIDER_CONSTRAINT_IMPL_H +#define DYN_SLIDER_CONSTRAINT_IMPL_H + +#include "constraint_impl.h" + +class slider_constraint_impl_t: public constraint_impl_t +{ +public: + // + virtual void set_pivot(vec3f const& p) = 0; + virtual void get_pivot(vec3f& p) const = 0; + virtual void get_world_pivot(vec3f& p) const = 0; + virtual void set_world(vec3f const& p) = 0; + virtual void get_world(vec3f& p) const = 0; + + // + virtual void set_damping(float d) = 0; + virtual float damping() const = 0; + virtual void set_LinLimit(float lower, float upper) = 0; + virtual void set_AngLimit(float lower, float upper) = 0; + +public: + virtual ~slider_constraint_impl_t() {}; +}; + +#endif //DYN_SLIDER_CONSTRAINT_IMPL_H diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 1545c75c7..2b17d8126 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -208,7 +208,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& { m_degenerateSimplex = 7; squaredDistance = previousSquaredDistance; - checkSimplex = true; + checkSimplex = false; break; } @@ -315,13 +315,11 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) { tmpNormalInB /= btSqrt(lenSqr); - btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length()-margin; + btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length(); //only replace valid penetrations when the result is deeper (check) if (!isValid || (distance2 < distance)) { distance = distance2; - pointOnA -= m_cachedSeparatingAxis * (marginA / distance); - pointOnB += m_cachedSeparatingAxis * (marginB / distance); pointOnA = tmpPointOnA; pointOnB = tmpPointOnB; normalInB = tmpNormalInB; @@ -349,13 +347,15 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) { tmpNormalInB /= btSqrt(lenSqr); - btScalar distance2 = (tmpPointOnA-tmpPointOnB).length(); + btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin; //only replace valid distances when the distance is less if (!isValid || (distance2 < distance)) { distance = distance2; pointOnA = tmpPointOnA; pointOnB = tmpPointOnB; + pointOnA -= tmpNormalInB * marginA ; + pointOnB += tmpNormalInB * marginB ; normalInB = tmpNormalInB; isValid = true; m_lastUsedMethod = 6; diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index f1a808dcb..3e4016dae 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -416,11 +416,19 @@ btVector3 btDiscreteDynamicsWorld::getGravity () const return m_gravity; } +void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) +{ + btRigidBody* body = btRigidBody::upcast(collisionObject); + if (body) + removeRigidBody(body); + else + btCollisionWorld::removeCollisionObject(collisionObject); +} void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) { m_nonStaticRigidBodies.remove(body); - removeCollisionObject(body); + btCollisionWorld::removeCollisionObject(body); } void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index 58508a2ca..caa3e73c3 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -130,6 +130,9 @@ public: virtual void removeRigidBody(btRigidBody* body); + ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject + virtual void removeCollisionObject(btCollisionObject* collisionObject); + void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); void debugDrawConstraint(btTypedConstraint* constraint); diff --git a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index 3f6141463..ae449f292 100644 --- a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -132,9 +132,19 @@ btVector3 btSimpleDynamicsWorld::getGravity () const void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body) { - removeCollisionObject(body); + btCollisionWorld::removeCollisionObject(body); } +void btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) +{ + btRigidBody* body = btRigidBody::upcast(collisionObject); + if (body) + removeRigidBody(body); + else + btCollisionWorld::removeCollisionObject(collisionObject); +} + + void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) { body->setGravity(m_gravity); diff --git a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h index ea1a2a1cc..ad1f54134 100644 --- a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h @@ -57,6 +57,9 @@ public: virtual void addRigidBody(btRigidBody* body); virtual void removeRigidBody(btRigidBody* body); + + ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject + virtual void removeCollisionObject(btCollisionObject* collisionObject); virtual void updateAabbs(); diff --git a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp index a0069b951..47f072e6d 100644 --- a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp @@ -119,6 +119,15 @@ void btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body) btCollisionWorld::removeCollisionObject(body); } +void btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) +{ + btSoftBody* body = btSoftBody::upcast(collisionObject); + if (body) + removeSoftBody(body); + else + btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); +} + void btSoftRigidDynamicsWorld::debugDrawWorld() { btDiscreteDynamicsWorld::debugDrawWorld(); diff --git a/src/BulletSoftBody/btSoftRigidDynamicsWorld.h b/src/BulletSoftBody/btSoftRigidDynamicsWorld.h index edb848e24..2cb9d712e 100644 --- a/src/BulletSoftBody/btSoftRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btSoftRigidDynamicsWorld.h @@ -54,6 +54,9 @@ public: void removeSoftBody(btSoftBody* body); + ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btDiscreteDynamicsWorld::removeCollisionObject + virtual void removeCollisionObject(btCollisionObject* collisionObject); + int getDrawFlags() const { return(m_drawFlags); } void setDrawFlags(int f) { m_drawFlags=f; } diff --git a/src/LinearMath/btQuaternion.h b/src/LinearMath/btQuaternion.h index 77cfb8c38..efcac3a5f 100644 --- a/src/LinearMath/btQuaternion.h +++ b/src/LinearMath/btQuaternion.h @@ -387,7 +387,11 @@ shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Ge btScalar d = v0.dot(v1); if (d < -1.0 + SIMD_EPSILON) - return btQuaternion(0.0f,1.0f,0.0f,0.0f); // just pick any vector + { + btVector3 n,unused; + btPlaneSpace1(v0,n,unused); + return btQuaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0 + } btScalar s = btSqrt((1.0f + d) * 2.0f); btScalar rs = 1.0f / s; diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index 544b28feb..a6d66d61e 100644 --- a/src/LinearMath/btScalar.h +++ b/src/LinearMath/btScalar.h @@ -242,6 +242,10 @@ SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); } #define SIMD_HALF_PI (SIMD_2_PI * btScalar(0.25)) #define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0)) #define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI) +#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490) + +#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x)))) /* reciprocal square root */ + #ifdef BT_USE_DOUBLE_PRECISION #define SIMD_EPSILON DBL_EPSILON diff --git a/src/LinearMath/btTransformUtil.h b/src/LinearMath/btTransformUtil.h index e8328da4c..7a090df62 100644 --- a/src/LinearMath/btTransformUtil.h +++ b/src/LinearMath/btTransformUtil.h @@ -21,9 +21,6 @@ subject to the following restrictions: -#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490) - -#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x)))) /* reciprocal square root */ SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir) { @@ -33,25 +30,7 @@ SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btV } -SIMD_FORCE_INLINE void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q) -{ - if (btFabs(n.z()) > SIMDSQRT12) { - // choose p in y-z plane - btScalar a = n[1]*n[1] + n[2]*n[2]; - btScalar k = btRecipSqrt (a); - p.setValue(0,-n[2]*k,n[1]*k); - // set q = n x p - q.setValue(a*k,-n[0]*p[2],n[0]*p[1]); - } - else { - // choose p in x-y plane - btScalar a = n.x()*n.x() + n.y()*n.y(); - btScalar k = btRecipSqrt (a); - p.setValue(-n.y()*k,n.x()*k,0); - // set q = n x p - q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k); - } -} + diff --git a/src/LinearMath/btVector3.h b/src/LinearMath/btVector3.h index 2988c2399..ada94d88b 100644 --- a/src/LinearMath/btVector3.h +++ b/src/LinearMath/btVector3.h @@ -635,4 +635,24 @@ SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3& vector) vector = swappedVec; } +SIMD_FORCE_INLINE void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q) +{ + if (btFabs(n.z()) > SIMDSQRT12) { + // choose p in y-z plane + btScalar a = n[1]*n[1] + n[2]*n[2]; + btScalar k = btRecipSqrt (a); + p.setValue(0,-n[2]*k,n[1]*k); + // set q = n x p + q.setValue(a*k,-n[0]*p[2],n[0]*p[1]); + } + else { + // choose p in x-y plane + btScalar a = n.x()*n.x() + n.y()*n.y(); + btScalar k = btRecipSqrt (a); + p.setValue(-n.y()*k,n.x()*k,0); + // set q = n x p + q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k); + } +} + #endif //SIMD__VECTOR3_H