2 #include "BodyEnvironment.hpp"
4 #include "RigidBody.hpp"
6 #include "Launchbar.hpp"
12 Launchbar::Launchbar()
16 _launchbar_mount[i] = _holdback_mount[i] = _force[i] = 0;
18 _global_ground[i] = 0;
19 _global_ground[2] = 1;
20 _global_ground[3] = -1e5;
22 _holdback_length = 2.0;
32 void Launchbar::setLaunchbarMount(float* position)
35 for(i=0; i<3; i++) _launchbar_mount[i] = position[i];
38 void Launchbar::setHoldbackMount(float* position)
41 for(i=0; i<3; i++) _holdback_mount[i] = position[i];
44 void Launchbar::setLength(float length)
49 void Launchbar::setDownAngle(float ang)
54 void Launchbar::setUpAngle(float ang)
59 void Launchbar::setExtension(float extension)
61 _extension = extension;
64 void Launchbar::setLaunchCmd(bool cmd)
69 void Launchbar::setGlobalGround(double *global_ground)
72 for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
75 void Launchbar::getLaunchbarMount(float* out)
78 for(i=0; i<3; i++) out[i] = _launchbar_mount[i];
81 void Launchbar::getHoldbackMount(float* out)
84 for(i=0; i<3; i++) out[i] = _holdback_mount[i];
87 float Launchbar::getLength(void)
92 float Launchbar::getDownAngle(void)
97 float Launchbar::getUpAngle(void)
102 float Launchbar::getExtension(void)
107 void Launchbar::getForce(float* force, float* off)
109 Math::set3(_force, force);
110 Math::set3(_launchbar_mount, off);
113 float Launchbar::getCompressFraction()
118 void Launchbar::getTipPosition(float* out)
120 // The launchbar tip in local coordinates.
121 float ang = _frac*(_down_ang - _up_ang) + _up_ang;
122 float pos_tip[3] = { _length*Math::cos(ang), 0.0,-_length*Math::sin(ang) };
123 Math::add3(_launchbar_mount, pos_tip, out);
126 void Launchbar::getTipGlobalPosition(State* s, double* out)
128 // The launchbar tip in local coordinates.
130 getTipPosition(pos_tip);
131 // The launchbar tip in global coordinates.
132 s->posLocalToGlobal(pos_tip, out);
135 float Launchbar::getPercentPosOnCat(float* lpos, float off, float lends[2][3])
137 // Compute the forward direction of the cat.
139 Math::sub3(lends[1], lends[0], lforward);
141 Math::sub3(lpos, lends[0], ltopos);
142 float fwlen = Math::mag3(lforward);
144 return (Math::dot3(ltopos, lforward)/fwlen + off)/fwlen;
147 void Launchbar::getPosOnCat(float perc, float* lpos, float* lvel,
148 float lends[2][3], float lendvels[2][3])
155 // Compute the forward direction of the cat.
157 Math::sub3(lends[1], lends[0], lforward);
158 Math::mul3(perc, lforward, lpos);
159 Math::add3(lends[0], lpos, lpos);
162 Math::mul3(perc, lendvels[0], lvel);
163 Math::mul3(1.0-perc, lendvels[1], tmp);
164 Math::add3(tmp, lvel, lvel);
167 void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, float* lrot)
169 // Init the return values
171 for(i=0; i<3; i++) _force[i] = 0;
173 if (_state != Unmounted)
176 // Don't bother if it's fully retracted
180 if (_extension < _frac)
183 // The launchbar tip in global coordinates.
184 double launchbar_pos[3];
185 getTipGlobalPosition(s, launchbar_pos);
187 // If the launchbars tip is less extended than it could be.
188 if(_frac < _extension) {
189 // Correct the extension value for no intersection.
190 // Compute the distance of the mount point from the ground plane.
191 double a = - _global_ground[3] + launchbar_pos[0]*_global_ground[0]
192 + launchbar_pos[1]*_global_ground[1]
193 + launchbar_pos[2]*_global_ground[2];
195 float ang = Math::asin(a/_length);
196 _frac = (ang - _up_ang)/(_down_ang - _up_ang);
198 // FIXME: this will jump
202 // Recompute the launchbar tip.
204 getTipPosition(llb_mount);
205 // The launchbar tip in global coordinates.
206 s->posLocalToGlobal(llb_mount, launchbar_pos);
208 double end[2][3]; float vel[2][3];
209 float dist = g_cb->getCatapult(launchbar_pos, end, vel);
210 // Work around a problem of flightgear returning totally screwed up
211 // scenery when switching views.
215 // Compute the positions of the catapult start and endpoints in the
216 // local coordiante system
218 s->posGlobalToLocal(end[0], lend[0]);
219 s->posGlobalToLocal(end[1], lend[1]);
221 // Transform the velocities of the endpoints to the
222 // local coordinate sytem.
224 s->velGlobalToLocal(vel[0], lvel[0]);
225 s->velGlobalToLocal(vel[1], lvel[1]);
227 // Compute the position of the launchbar tip relative to the cat.
228 float tip_pos_on_cat = getPercentPosOnCat(llb_mount, 0.0, lend);
229 float llbtip[3], lvlbtip[3];
230 getPosOnCat(tip_pos_on_cat, llbtip, lvlbtip, lend, lvel);
232 // Compute the direction from the launchbar mount at the gear
233 // to the lauchbar mount on the cat.
235 Math::sub3(llbtip, _launchbar_mount, llbdir);
236 float lblen = Math::mag3(llbdir);
237 Math::mul3(1.0/lblen, llbdir, llbdir);
239 // Check if we are near enough to the cat.
240 if (_state == Unmounted && dist < 0.5) {
241 // croase approximation for the velocity of the launchbar.
242 // Might be sufficient because arresting at the cat makes only
243 // sense when the aircraft does not rotate much.
246 float lrot[3], lv[3];
247 Math::vmul33(s->orient, s->rot, lrot);
248 Math::vmul33(s->orient, s->v, lv);
249 body->pointVelocity(llb_mount, lrot, tmp);
250 Math::sub3(tmp, lvlbtip, lv_mount);
251 Math::add3(lv, lv_mount, lv_mount);
253 // We cannot arrest at the cat if we move too fast wrt the cat.
254 if (0.2 < Math::mag3(lv_mount))
257 // Compute the position of the holdback mount relative to the cat.
258 double dd[2][3]; float fd[2][3]; double ghldbkpos[3];
259 s->posLocalToGlobal(_holdback_mount, ghldbkpos);
260 float hbdist = g_cb->getCatapult(ghldbkpos, dd, fd);
261 float offset = -Math::sqrt(_holdback_length*_holdback_length - hbdist*hbdist);
262 _pos_on_cat = getPercentPosOnCat(_holdback_mount, offset, lend);
265 // We cannot arrest if we are not at the start of the cat.
266 if (_pos_on_cat < 0.0 || 0.2 < _pos_on_cat)
269 // Now we are arrested at the cat.
270 // The force is applied at the next step.
275 // Get the actual distance from the holdback to its mountpoint
276 // on the cat. If it is longer than the holdback apply a force.
277 float lhldbk_cmount[3]; float lvhldbk_cmount[3];
278 getPosOnCat(_pos_on_cat, lhldbk_cmount, lvhldbk_cmount, lend, lvel);
279 // Compute the direction of holdback.
281 Math::sub3(lhldbk_cmount, _holdback_mount, lhldbkdir);
282 float hldbklen = Math::mag3(lhldbkdir);
283 Math::mul3(1/hldbklen, lhldbkdir, lhldbkdir);
285 if (_state == Arrested) {
286 // Now apply a constant tension from the catapult over the launchbar.
287 Math::mul3(2.0, llbdir, _force);
289 // If the distance from the holdback mount at the aircraft to the
290 // holdback mount on the cat is larger than the holdback length itself,
291 // the holdback applies a force to the gear.
292 if (_holdback_length < hldbklen) {
293 // croase approximation for the velocity of the holdback mount
295 // Might be sufficient because arresting at the cat makes only
296 // sense when the aircraft does not rotate much.
297 float lvhldbk_gmount[3];
298 float lrot[3], lv[3];
299 Math::vmul33(s->orient, s->rot, lrot);
300 Math::vmul33(s->orient, s->v, lv);
301 body->pointVelocity(_holdback_mount, lrot, lvhldbk_gmount);
302 Math::add3(lv, lvhldbk_gmount, lvhldbk_gmount);
304 // The velocity of the holdback mount at the gear wrt the
305 // holdback mount at the cat.
307 Math::sub3(lvhldbk_gmount, lvhldbk_cmount, lvhldbk);
309 // The spring force the holdback will apply to the gear
311 Math::mul3(1e1*(hldbklen - _holdback_length), lhldbkdir, tmp);
312 Math::add3(tmp, _force, _force);
314 // The damping force here ...
315 Math::mul3(2e0, lvhldbk, tmp);
316 Math::sub3(_force, tmp, _force);
323 if (_state == Launch) {
324 // Now apply a constant tension from the catapult over the launchbar.
325 Math::mul3(25.0, llbdir, _force);
331 // Scale by the mass. That keeps the stiffness in reasonable bounds.
332 float mass = body->getTotalMass();
333 Math::mul3(mass, _force, _force);
336 }; // namespace yasim