btUniversalConstraint.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
00003 Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. 
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 
00017 
00018 #include "btUniversalConstraint.h"
00019 #include "BulletDynamics/Dynamics/btRigidBody.h"
00020 #include "LinearMath/btTransformUtil.h"
00021 
00022 
00023 
00024 #define UNIV_EPS btScalar(0.01f)
00025 
00026 
00027 // constructor
00028 // anchor, axis1 and axis2 are in world coordinate system
00029 // axis1 must be orthogonal to axis2
00030 btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2)
00031 : btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
00032  m_anchor(anchor),
00033  m_axis1(axis1),
00034  m_axis2(axis2)
00035 {
00036         // build frame basis
00037         // 6DOF constraint uses Euler angles and to define limits
00038         // it is assumed that rotational order is :
00039         // Z - first, allowed limits are (-PI,PI);
00040         // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number 
00041         // used to prevent constraint from instability on poles;
00042         // new position of X, allowed limits are (-PI,PI);
00043         // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
00044         // Build the frame in world coordinate system first
00045         btVector3 zAxis = m_axis1.normalize();
00046         btVector3 yAxis = m_axis2.normalize();
00047         btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
00048         btTransform frameInW;
00049         frameInW.setIdentity();
00050         frameInW.getBasis().setValue(   xAxis[0], yAxis[0], zAxis[0],   
00051                                                                         xAxis[1], yAxis[1], zAxis[1],
00052                                                                         xAxis[2], yAxis[2], zAxis[2]);
00053         frameInW.setOrigin(anchor);
00054         // now get constraint frame in local coordinate systems
00055         m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
00056         m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW;
00057         // sei limits
00058         setLinearLowerLimit(btVector3(0., 0., 0.));
00059         setLinearUpperLimit(btVector3(0., 0., 0.));
00060         setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI + UNIV_EPS, -SIMD_PI + UNIV_EPS));
00061         setAngularUpperLimit(btVector3(0.f,  SIMD_HALF_PI - UNIV_EPS,  SIMD_PI - UNIV_EPS));
00062 }
00063 
00064 void btUniversalConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
00065 {
00066   m_axis1 = axis1;
00067   m_axis2 = axis2;
00068 
00069         btVector3 zAxis = axis1.normalized();
00070         btVector3 yAxis = axis2.normalized();
00071         btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
00072 
00073         btTransform frameInW;
00074         frameInW.setIdentity();
00075         frameInW.getBasis().setValue(   xAxis[0], yAxis[0], zAxis[0],   
00076                                 xAxis[1], yAxis[1], zAxis[1],
00077                                 xAxis[2], yAxis[2], zAxis[2]);
00078         frameInW.setOrigin(m_anchor);
00079 
00080         // now get constraint frame in local coordinate systems
00081         m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
00082         m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
00083 
00084   calculateTransforms();
00085 }
00086 
00087