btHinge2Constraint.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 "btHinge2Constraint.h"
00019 #include "BulletDynamics/Dynamics/btRigidBody.h"
00020 #include "LinearMath/btTransformUtil.h"
00021 
00022 
00023 
00024 // constructor
00025 // anchor, axis1 and axis2 are in world coordinate system
00026 // axis1 must be orthogonal to axis2
00027 btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2)
00028 : btGeneric6DofSpringConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
00029  m_anchor(anchor),
00030  m_axis1(axis1),
00031  m_axis2(axis2)
00032 {
00033         // build frame basis
00034         // 6DOF constraint uses Euler angles and to define limits
00035         // it is assumed that rotational order is :
00036         // Z - first, allowed limits are (-PI,PI);
00037         // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number 
00038         // used to prevent constraint from instability on poles;
00039         // new position of X, allowed limits are (-PI,PI);
00040         // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
00041         // Build the frame in world coordinate system first
00042         btVector3 zAxis = axis1.normalize();
00043         btVector3 xAxis = axis2.normalize();
00044         btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system
00045         btTransform frameInW;
00046         frameInW.setIdentity();
00047         frameInW.getBasis().setValue(   xAxis[0], yAxis[0], zAxis[0],   
00048                                                                         xAxis[1], yAxis[1], zAxis[1],
00049                                                                         xAxis[2], yAxis[2], zAxis[2]);
00050         frameInW.setOrigin(anchor);
00051         // now get constraint frame in local coordinate systems
00052         m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
00053         m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW;
00054         // sei limits
00055         setLinearLowerLimit(btVector3(0.f, 0.f, -1.f));
00056         setLinearUpperLimit(btVector3(0.f, 0.f,  1.f));
00057         // like front wheels of a car
00058         setAngularLowerLimit(btVector3(1.f,  0.f, -SIMD_HALF_PI * 0.5f)); 
00059         setAngularUpperLimit(btVector3(-1.f, 0.f,  SIMD_HALF_PI * 0.5f));
00060         // enable suspension
00061         enableSpring(2, true);
00062         setStiffness(2, SIMD_PI * SIMD_PI * 4.f); // period 1 sec for 1 kilogramm weel :-)
00063         setDamping(2, 0.01f);
00064         setEquilibriumPoint();
00065 }
00066