2 #include "BodyEnvironment.hpp"
4 #include "RigidBody.hpp"
9 static const float YASIM_PI2 = 3.14159265358979323846/2;
15 _pos[i] = _force[i] = 0;
17 _global_ground[i] = 0;
18 _global_ground[2] = 1;
19 _global_ground[3] = -1e5;
28 void Hook::setPosition(float* position)
31 for(i=0; i<3; i++) _pos[i] = position[i];
34 void Hook::setLength(float length)
39 void Hook::setDownAngle(float ang)
44 void Hook::setUpAngle(float ang)
49 void Hook::setExtension(float extension)
51 _extension = extension;
54 void Hook::setGlobalGround(double *global_ground)
57 for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
60 void Hook::getPosition(float* out)
63 for(i=0; i<3; i++) out[i] = _pos[i];
66 float Hook::getHookPos(int i)
71 float Hook::getLength(void)
76 float Hook::getDownAngle(void)
81 float Hook::getUpAngle(void)
86 float Hook::getAngle(void)
91 float Hook::getExtension(void)
96 void Hook::getForce(float* force, float* off)
98 Math::set3(_force, force);
99 Math::set3(_pos, off);
102 float Hook::getCompressFraction()
107 void Hook::getTipPosition(float* out)
109 // The hook tip in local coordinates.
110 _ang = _frac*(_down_ang - _up_ang) + _up_ang;
111 float pos_tip[3] = { _length*Math::cos(_ang), 0, _length*Math::sin(_ang) };
112 Math::sub3(_pos, pos_tip, out);
115 void Hook::getTipGlobalPosition(State* s, double* out)
117 // The hook tip in local coordinates.
119 getTipPosition(pos_tip);
120 // The hook tip in global coordinates.
121 s->posLocalToGlobal(pos_tip, out);
124 void Hook::calcForce(Ground* g_cb, RigidBody* body, State* s, float* lv, float* lrot)
126 // Init the return values
128 for(i=0; i<3; i++) _force[i] = 0;
130 // Don't bother if it's fully retracted
134 // For the first guess, the position fraction is equal to the
138 // The ground plane transformed to the local frame.
140 s->planeGlobalToLocal(_global_ground, ground);
142 // The hook tip in local coordinates.
144 getTipPosition(ltip);
147 // Correct the extension value for no intersection.
149 // Check if the tip will intersect the ground or not. That is, compute
150 // the distance of the tip to the ground plane.
151 float tipdist = ground[3] - Math::dot3(ltip, ground);
155 // Compute the distance of the hooks mount point from the ground plane.
156 float mountdist = ground[3] - Math::dot3(_pos, ground);
158 // Compute the distance of the hooks mount point from the ground plane
159 // in the x-z plane. It holds:
160 // mountdist = mountdist_xz*cos(angle(normal_yz, e_z))
162 float mountdist_xz = _length;
163 if (ground[2] != 0) {
164 float nrm_yz = Math::sqrt(ground[1]*ground[1]+ground[2]*ground[2]);
165 mountdist_xz = -mountdist*nrm_yz/ground[2];
168 if (mountdist_xz < _length) {
169 float ang = Math::asin(mountdist_xz/_length)
170 + Math::atan2(ground[2], ground[0]) + YASIM_PI2;
171 _frac = (ang - _up_ang)/(_down_ang - _up_ang);
177 double hook_area[4][3];
178 // The hook mount in global coordinates.
179 s->posLocalToGlobal(_pos, hook_area[1]);
181 // Recompute the hook tip in global coordinates.
182 getTipGlobalPosition(s, hook_area[0]);
184 // The old positions.
185 hook_area[2][0] = _old_mount[0];
186 hook_area[2][1] = _old_mount[1];
187 hook_area[2][2] = _old_mount[2];
188 hook_area[3][0] = _old_tip[0];
189 hook_area[3][1] = _old_tip[1];
190 hook_area[3][2] = _old_tip[2];
193 // Check if we caught a wire.
194 // Returns true if we caught one.
195 if (!_has_wire && g_cb->caughtWire(hook_area))
199 // save actual position as old position ...
200 _old_mount[0] = hook_area[1][0];
201 _old_mount[1] = hook_area[1][1];
202 _old_mount[2] = hook_area[1][2];
203 _old_tip[0] = hook_area[0][0];
204 _old_tip[1] = hook_area[0][1];
205 _old_tip[2] = hook_area[0][2];
210 // Get the wire endpoints and their velocities wrt the earth.
212 float wire_vel[2][3];
213 g_cb->getWire(dpos, wire_vel);
215 // Transform those to the local coordinate system
216 float wire_lpos[2][3];
217 s->posGlobalToLocal(dpos[0], wire_lpos[0]);
218 s->posGlobalToLocal(dpos[1], wire_lpos[1]);
219 s->velGlobalToLocal(wire_vel[0], wire_vel[0]);
220 s->velGlobalToLocal(wire_vel[1], wire_vel[1]);
222 // Compute the velocity of the hooks mount point in the local frame.
224 body->pointVelocity(_pos, lrot, mount_vel);
225 Math::add3(lv, mount_vel, mount_vel);
227 // The velocity of the hook mount point wrt the earth in
229 float v_wrt_we[2][3];
230 Math::sub3(mount_vel, wire_vel[0], v_wrt_we[0]);
231 Math::sub3(mount_vel, wire_vel[1], v_wrt_we[1]);
234 // The vector from the wire ends to the hook mount point.
235 Math::sub3(_pos, wire_lpos[0], f[0]);
236 Math::sub3(_pos, wire_lpos[1], f[1]);
238 // We only need the direction.
239 float mf0 = Math::mag3(f[0]);
240 float mf1 = Math::mag3(f[1]);
241 Math::mul3(1.0/mf0, f[0], f[0]);
242 Math::mul3(1.0/mf1, f[1], f[1]);
244 // The velocity of the wire wrt the wire ends at the wire
246 float v0 = Math::dot3(v_wrt_we[0], f[0]);
247 float v1 = Math::dot3(v_wrt_we[1], f[1]);
249 // We assume that the wire slips through the hook. So the velocity
250 // will be equal at both sides. So take the mean of both.
251 float v = 0.5*(v0+v1);
253 // Release wire when we reach zero velocity.
260 // The trick is to multiply with the current mass of the aircraft.
261 // That way we control the acceleration and not the force. This is
262 // the implicit calibration of the wires for aircrafts of
264 float mass = body->getTotalMass();
266 // The local force is the vector sum of the force on each wire.
267 // The force is computed with some constant tension on the wires
268 // (80000N) plus a velocity dependent component.
269 Math::add3(f[0], f[1], _force);
270 Math::mul3(-mass*( 1.0 + ((mf0+mf1)/70) + 0.2*v ), _force, _force);
272 // Now in the body coordinate system, eliminate the Y coord part
273 // of the hook force. Physically this means that the wire will just
274 // slip through the hook instead of applying a side force.
278 }; // namespace yasim