← Back to branch summary

~ubuntu-branches/ubuntu/vivid/emscripten/vivid

« back to all changes in this revision

Viewing changes to tests/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp

  • Committer: Package Import Robot
  • Author(s): Sylvestre Ledru
  • Date: 2013-05-02 13:11:51 UTC
  • Revision ID: package-import@ubuntu.com-20130502131151-q8dvteqr1ef2x7xz
Tags: upstream-1.4.1~20130504~adb56cb
Import upstream version 1.4.1~20130504~adb56cb

Show diffs side-by-side

added added

removed removed

Lines of Context:
tests/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp
 
1
/*
 
2
Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
 
3
Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. 
 
4
 
 
5
This software is provided 'as-is', without any express or implied warranty.
 
6
In no event will the authors be held liable for any damages arising from the use of this software.
 
7
Permission is granted to anyone to use this software for any purpose, 
 
8
including commercial applications, and to alter it and redistribute it freely, 
 
9
subject to the following restrictions:
 
10
 
 
11
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.
 
12
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
 
13
3. 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
 
 
21
btGeneric6DofSpringConstraint::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
 
 
36
void 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
 
 
52
void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness)
 
53
{
 
54
        btAssert((index >= 0) && (index < 6));
 
55
        m_springStiffness[index] = stiffness;
 
56
}
 
57
 
 
58
 
 
59
void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping)
 
60
{
 
61
        btAssert((index >= 0) && (index < 6));
 
62
        m_springDamping[index] = damping;
 
63
}
 
64
 
 
65
 
 
66
void 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
 
 
83
void 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
 
 
97
void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index, btScalar val)
 
98
{
 
99
        btAssert((index >= 0) && (index < 6));
 
100
        m_equilibriumPoint[index] = val;
 
101
}
 
102
 
 
103
 
 
104
void 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
 
 
142
void 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
 
 
152
void 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