← Back to branch summary

~ubuntu-branches/ubuntu/quantal/flightgear/quantal

« back to all changes in this revision

Viewing changes to src/FDM/YASim/Gear.cpp

  • Committer: Bazaar Package Importer
  • Author(s): Ove Kaaven
  • Date: 2002-03-27 21:50:15 UTC
  • Revision ID: james.westby@ubuntu.com-20020327215015-0rvi3o8iml0a8s93
Tags: upstream-0.7.9
Import upstream version 0.7.9

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/FDM/YASim/Gear.cpp
 
1
#include "Math.hpp"
 
2
#include "RigidBody.hpp"
 
3
 
 
4
#include "Gear.hpp"
 
5
namespace yasim {
 
6
 
 
7
Gear::Gear()
 
8
{
 
9
    int i;
 
10
    for(i=0; i<3; i++)
 
11
        _pos[i] = _cmpr[i] = 0;
 
12
    _spring = 1;
 
13
    _damp = 0;
 
14
    _sfric = 0.8;
 
15
    _dfric = 0.7;
 
16
    _brake = 0;
 
17
    _rot = 0;
 
18
    _extension = 1;
 
19
    _castering = false;
 
20
}
 
21
 
 
22
void Gear::setPosition(float* position)
 
23
{
 
24
    int i;
 
25
    for(i=0; i<3; i++) _pos[i] = position[i];
 
26
}
 
27
 
 
28
void Gear::setCompression(float* compression)
 
29
{
 
30
    int i;
 
31
    for(i=0; i<3; i++) _cmpr[i] = compression[i];
 
32
}
 
33
 
 
34
void Gear::setSpring(float spring)
 
35
{
 
36
    _spring = spring;
 
37
}
 
38
 
 
39
void Gear::setDamping(float damping)
 
40
{
 
41
    _damp = damping;
 
42
}
 
43
 
 
44
void Gear::setStaticFriction(float sfric)
 
45
{
 
46
    _sfric = sfric;
 
47
}
 
48
 
 
49
void Gear::setDynamicFriction(float dfric)
 
50
{
 
51
    _dfric = dfric;
 
52
}
 
53
 
 
54
void Gear::setBrake(float brake)
 
55
{
 
56
    _brake = Math::clamp(brake, 0, 1);
 
57
}
 
58
 
 
59
void Gear::setRotation(float rotation)
 
60
{
 
61
    _rot = rotation;
 
62
}
 
63
 
 
64
void Gear::setExtension(float extension)
 
65
{
 
66
    _extension = Math::clamp(extension, 0, 1);
 
67
}
 
68
 
 
69
void Gear::setCastering(bool c)
 
70
{
 
71
    _castering = c;
 
72
}
 
73
 
 
74
void Gear::getPosition(float* out)
 
75
{
 
76
    int i;
 
77
    for(i=0; i<3; i++) out[i] = _pos[i];
 
78
}
 
79
 
 
80
void Gear::getCompression(float* out)
 
81
{
 
82
    int i;
 
83
    for(i=0; i<3; i++) out[i] = _cmpr[i];    
 
84
}
 
85
 
 
86
float Gear::getSpring()
 
87
{
 
88
    return _spring;
 
89
}
 
90
 
 
91
float Gear::getDamping()
 
92
{
 
93
    return _damp;
 
94
}
 
95
 
 
96
float Gear::getStaticFriction()
 
97
{
 
98
    return _sfric;
 
99
}
 
100
 
 
101
float Gear::getDynamicFriction()
 
102
{
 
103
    return _dfric;
 
104
}
 
105
 
 
106
float Gear::getBrake()
 
107
{
 
108
    return _brake;
 
109
}
 
110
 
 
111
float Gear::getRotation()
 
112
{
 
113
    return _rot;
 
114
}
 
115
 
 
116
float Gear::getExtension()
 
117
{
 
118
    return _extension;
 
119
}
 
120
 
 
121
void Gear::getForce(float* force, float* contact)
 
122
{
 
123
    Math::set3(_force, force);
 
124
    Math::set3(_contact, contact);
 
125
}
 
126
 
 
127
float Gear::getWoW()
 
128
{
 
129
    return _wow;
 
130
}
 
131
 
 
132
float Gear::getCompressFraction()
 
133
{
 
134
    return _frac;
 
135
}
 
136
 
 
137
bool Gear::getCastering()
 
138
{
 
139
    return _castering;
 
140
}
 
141
 
 
142
void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
 
143
{
 
144
    // Init the return values
 
145
    int i;
 
146
    for(i=0; i<3; i++) _force[i] = _contact[i] = 0;
 
147
 
 
148
    // Don't bother if it's not down
 
149
    if(_extension < 1)
 
150
        return;
 
151
 
 
152
    float tmp[3];
 
153
 
 
154
    // First off, make sure that the gear "tip" is below the ground.
 
155
    // If it's not, there's no force.
 
156
    float a = ground[3] - Math::dot3(_pos, ground);
 
157
    if(a > 0) {
 
158
        _wow = 0;
 
159
        _frac = 0;
 
160
        return;
 
161
    }
 
162
 
 
163
    // Now a is the distance from the tip to ground, so make b the
 
164
    // distance from the base to ground.  We can get the fraction
 
165
    // (0-1) of compression from a/(a-b). Note the minus sign -- stuff
 
166
    // above ground is negative.
 
167
    Math::add3(_cmpr, _pos, tmp);
 
168
    float b = ground[3] - Math::dot3(tmp, ground);
 
169
 
 
170
    // Calculate the point of ground _contact.
 
171
    _frac = a/(a-b);
 
172
    if(b < 0) _frac = 1;
 
173
    for(i=0; i<3; i++)
 
174
        _contact[i] = _pos[i] + _frac*_cmpr[i];
 
175
 
 
176
    // Turn _cmpr into a unit vector and a magnitude
 
177
    float cmpr[3];
 
178
    float clen = Math::mag3(_cmpr);
 
179
    Math::mul3(1/clen, _cmpr, cmpr);
 
180
 
 
181
    // Now get the velocity of the point of contact
 
182
    float cv[3];
 
183
    body->pointVelocity(_contact, rot, cv);
 
184
    Math::add3(cv, v, cv);
 
185
 
 
186
    // Finally, we can start adding up the forces.  First the spring
 
187
    // compression.   (note the clamping of _frac to 1):
 
188
    _frac = (_frac > 1) ? 1 : _frac;
 
189
    float fmag = _frac*clen*_spring;
 
190
 
 
191
    // Then the damping.  Use the only the velocity into the ground
 
192
    // (projection along "ground") projected along the compression
 
193
    // axis.  So Vdamp = ground*(ground dot cv) dot cmpr
 
194
    Math::mul3(Math::dot3(ground, cv), ground, tmp);
 
195
    float dv = Math::dot3(cmpr, tmp);
 
196
    float damp = _damp * dv;
 
197
    if(damp > fmag) damp = fmag; // can't pull the plane down!
 
198
    if(damp < -fmag) damp = -fmag; // sanity
 
199
 
 
200
    // The actual force applied is only the component perpendicular to
 
201
    // the ground.  Side forces come from velocity only.
 
202
    _wow = (fmag - damp) * -Math::dot3(cmpr, ground);
 
203
    Math::mul3(-_wow, ground, _force);
 
204
 
 
205
    // Castering gear feel no force in the ground plane
 
206
    if(_castering)
 
207
        return;
 
208
 
 
209
    // Wheels are funky.  Split the velocity along the ground plane
 
210
    // into rolling and skidding components.  Assuming small angles,
 
211
    // we generate "forward" and "left" unit vectors (the compression
 
212
    // goes "up") for the gear, make a "steer" direction from these,
 
213
    // and then project it onto the ground plane.  Project the
 
214
    // velocity onto the ground plane too, and extract the "steer"
 
215
    // component.  The remainder is the skid velocity.
 
216
 
 
217
    float gup[3]; // "up" unit vector from the ground
 
218
    Math::set3(ground, gup);
 
219
    Math::mul3(-1, gup, gup);
 
220
 
 
221
    float xhat[] = {1,0,0};
 
222
    float steer[3], skid[3];
 
223
    Math::cross3(gup, xhat, skid);  // up cross xhat =~ skid
 
224
    Math::unit3(skid, skid);        //               == skid
 
225
 
 
226
    Math::cross3(skid, gup, steer); // skid cross up == steer
 
227
 
 
228
    if(_rot != 0) {
 
229
        // Correct for a (small) rotation
 
230
        Math::mul3(_rot, steer, tmp);
 
231
        Math::add3(tmp, skid, skid);
 
232
        Math::unit3(skid, skid);
 
233
        Math::cross3(skid, gup, steer);
 
234
    }
 
235
 
 
236
    float vsteer = Math::dot3(cv, steer);
 
237
    float vskid  = Math::dot3(cv, skid);
 
238
    float wgt = Math::dot3(_force, gup); // force into the ground
 
239
 
 
240
    float fsteer = _brake * calcFriction(wgt, vsteer);
 
241
    float fskid  = calcFriction(wgt, vskid);
 
242
    if(vsteer > 0) fsteer = -fsteer;
 
243
    if(vskid > 0) fskid = -fskid;
 
244
 
 
245
    // Phoo!  All done.  Add it up and get out of here.
 
246
    Math::mul3(fsteer, steer, tmp);
 
247
    Math::add3(tmp, _force, _force);
 
248
 
 
249
    Math::mul3(fskid, skid, tmp);
 
250
    Math::add3(tmp, _force, _force);
 
251
}
 
252
 
 
253
float Gear::calcFriction(float wgt, float v)
 
254
{
 
255
    // How slow is stopped?  10 cm/second?
 
256
    const float STOP = 0.1;
 
257
    const float iSTOP = 1/STOP;
 
258
    v = Math::abs(v);
 
259
    if(v < STOP) return v*iSTOP * wgt * _sfric;
 
260
    else         return wgt * _dfric;
 
261
}
 
262
 
 
263
}; // namespace yasim
 
264