Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/SuperOrxoBros_HS18/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp @ 12177

Last change on this file since 12177 was 12177, checked in by siramesh, 5 years ago

Super Orxo Bros Final (Sidharth Ramesh, Nisa Balta, Jeff Ren)

File size: 5.2 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
3Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
16#include "btGeneric6DofSpringConstraint.h"
17#include "BulletDynamics/Dynamics/btRigidBody.h"
18#include "LinearMath/btTransformUtil.h"
19
20
21btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA)
22        : btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA)
23{
24        m_objectType = D6_SPRING_CONSTRAINT_TYPE;
25
26        for(int i = 0; i < 6; i++)
27        {
28                m_springEnabled[i] = false;
29                m_equilibriumPoint[i] = btScalar(0.f);
30                m_springStiffness[i] = btScalar(0.f);
31                m_springDamping[i] = btScalar(1.f);
32        }
33}
34
35
36void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff)
37{
38        btAssert((index >= 0) && (index < 6));
39        m_springEnabled[index] = onOff;
40        if(index < 3)
41        {
42                m_linearLimits.m_enableMotor[index] = onOff;
43        }
44        else
45        {
46                m_angularLimits[index - 3].m_enableMotor = onOff;
47        }
48}
49
50
51
52void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness)
53{
54        btAssert((index >= 0) && (index < 6));
55        m_springStiffness[index] = stiffness;
56}
57
58
59void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping)
60{
61        btAssert((index >= 0) && (index < 6));
62        m_springDamping[index] = damping;
63}
64
65
66void btGeneric6DofSpringConstraint::setEquilibriumPoint()
67{
68        calculateTransforms();
69        int i;
70
71        for( i = 0; i < 3; i++)
72        {
73                m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
74        }
75        for(i = 0; i < 3; i++)
76        {
77                m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i];
78        }
79}
80
81
82
83void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index)
84{
85        btAssert((index >= 0) && (index < 6));
86        calculateTransforms();
87        if(index < 3)
88        {
89                m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
90        }
91        else
92        {
93                m_equilibriumPoint[index] = m_calculatedAxisAngleDiff[index - 3];
94        }
95}
96
97void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index, btScalar val)
98{
99        btAssert((index >= 0) && (index < 6));
100        m_equilibriumPoint[index] = val;
101}
102
103
104void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info)
105{
106        // it is assumed that calculateTransforms() have been called before this call
107        int i;
108        btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
109        for(i = 0; i < 3; i++)
110        {
111                if(m_springEnabled[i])
112                {
113                        // get current position of constraint
114                        btScalar currPos = m_calculatedLinearDiff[i];
115                        // calculate difference
116                        btScalar delta = currPos - m_equilibriumPoint[i];
117                        // spring force is (delta * m_stiffness) according to Hooke's Law
118                        btScalar force = delta * m_springStiffness[i];
119                        btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations);
120                        m_linearLimits.m_targetVelocity[i] =  velFactor * force;
121                        m_linearLimits.m_maxMotorForce[i] =  btFabs(force) / info->fps;
122                }
123        }
124        for(i = 0; i < 3; i++)
125        {
126                if(m_springEnabled[i + 3])
127                {
128                        // get current position of constraint
129                        btScalar currPos = m_calculatedAxisAngleDiff[i];
130                        // calculate difference
131                        btScalar delta = currPos - m_equilibriumPoint[i+3];
132                        // spring force is (-delta * m_stiffness) according to Hooke's Law
133                        btScalar force = -delta * m_springStiffness[i+3];
134                        btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations);
135                        m_angularLimits[i].m_targetVelocity = velFactor * force;
136                        m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
137                }
138        }
139}
140
141
142void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
143{
144        // this will be called by constraint solver at the constraint setup stage
145        // set current motor parameters
146        internalUpdateSprings(info);
147        // do the rest of job for constraint setup
148        btGeneric6DofConstraint::getInfo2(info);
149}
150
151
152void btGeneric6DofSpringConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
153{
154        btVector3 zAxis = axis1.normalized();
155        btVector3 yAxis = axis2.normalized();
156        btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
157
158        btTransform frameInW;
159        frameInW.setIdentity();
160        frameInW.getBasis().setValue(   xAxis[0], yAxis[0], zAxis[0],   
161                                xAxis[1], yAxis[1], zAxis[1],
162                                xAxis[2], yAxis[2], zAxis[2]);
163
164        // now get constraint frame in local coordinate systems
165        m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
166        m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
167
168  calculateTransforms();
169}
170
171
172
Note: See TracBrowser for help on using the repository browser.