1 // groundcache.cxx -- carries a small subset of the scenegraph near the vehicle
3 // Written by Mathias Froehlich, started Nov 2004.
5 // Copyright (C) 2004 Mathias Froehlich - Mathias.Froehlich@web.de
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Public License for more details.
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
31 #include <simgear/sg_inlines.h>
32 #include <simgear/constants.h>
33 #include <simgear/debug/logstream.hxx>
34 #include <simgear/math/sg_geodesy.hxx>
35 #include <simgear/scene/material/mat.hxx>
36 #include <simgear/scene/material/matlib.hxx>
38 #include <Main/globals.hxx>
39 #include <Scenery/scenery.hxx>
40 #include <Scenery/tilemgr.hxx>
41 #include <AIModel/AICarrier.hxx>
44 #include "groundcache.hxx"
46 // Specialized version of sgMultMat4 needed because of mixed matrix
48 static inline void fgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2) {
49 for ( int j = 0 ; j < 4 ; j++ ) {
50 dst[0][j] = m2[0][0] * m1[0][j] +
55 dst[1][j] = m2[1][0] * m1[0][j] +
60 dst[2][j] = m2[2][0] * m1[0][j] +
65 dst[3][j] = m2[3][0] * m1[0][j] +
72 static inline bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
76 // Some tolerance in meters we accept a point to be outside of the triangle
77 // and still return that it is inside.
80 // punt if outside bouding cube
81 SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
82 if( (point[0] < min - eps) || (point[0] > max + eps) )
86 SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
87 if( (point[1] < min - eps) || (point[1] > max + eps) )
91 SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
92 if( (point[2] < min - eps) || (point[2] > max + eps) )
96 // drop the smallest dimension so we only have to work in 2d.
97 SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
98 SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
99 if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
100 // x is the smallest dimension
109 } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
110 // y is the smallest dimension
119 } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
120 // z is the smallest dimension
130 // all dimensions are really small so lets call it close
131 // enough and return a successful match
135 // check if intersection point is on the same side of p1 <-> p2 as p3
136 SGDfloat tmp = (y2 - y3);
137 SGDfloat tmpn = (x2 - x3);
138 int side1 = SG_SIGN (tmp * (rx - x3) + (y3 - ry) * tmpn);
139 int side2 = SG_SIGN (tmp * (x1 - x3) + (y3 - y1) * tmpn
140 + side1 * eps * fabs(tmpn));
141 if ( side1 != side2 ) {
142 // printf("failed side 1 check\n");
146 // check if intersection point is on correct side of p2 <-> p3 as p1
149 side1 = SG_SIGN (tmp * (x2 - rx) + (ry - y2) * tmpn);
150 side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
151 + side1 * eps * fabs(tmpn));
152 if ( side1 != side2 ) {
153 // printf("failed side 2 check\n");
157 // check if intersection point is on correct side of p1 <-> p3 as p2
160 side1 = SG_SIGN (tmp * (x3 - rx) + (ry - y3) * tmpn);
161 side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
162 + side1 * eps * fabs(tmpn));
163 if ( side1 != side2 ) {
164 // printf("failed side 3 check\n");
171 // Test if the line given by the point on the line pt_on_line and the
172 // line direction dir intersects the sphere sp.
173 // Adapted from plib.
175 fgdIsectSphereInfLine(const sgdSphere& sp,
176 const sgdVec3 pt_on_line, const sgdVec3 dir)
179 sgdSubVec3( r, sp.getCenter(), pt_on_line ) ;
181 SGDfloat projectedDistance = sgdScalarProductVec3(r, dir);
183 SGDfloat dist = sgdScalarProductVec3 ( r, r ) -
184 projectedDistance * projectedDistance;
186 SGDfloat radius = sp.getRadius();
187 return dist < radius*radius;
190 FGGroundCache::FGGroundCache()
192 sgdSetVec3(cache_center, 0.0, 0.0, 0.0);
194 cache_ref_time = 0.0;
196 sgdSetVec3(reference_wgs84_point, 0.0, 0.0, 0.0);
197 reference_vehicle_radius = 0.0;
198 found_ground = false;
201 FGGroundCache::~FGGroundCache()
205 FGGroundCache::GroundProperty
206 FGGroundCache::extractGroundProperty( ssgLeaf* l )
208 // FIXME: Do more ...
209 // Idea: have a get_globals() function which knows about that stuff.
210 // Or most probably read that from a configuration file,
211 // from property tree or whatever ...
213 // Get ground dependent data.
217 FGAICarrierHardware *ud =
218 dynamic_cast<FGAICarrierHardware*>(l->getUserData());
221 case FGAICarrierHardware::Wire:
222 gp.type = FGInterface::Wire;
225 case FGAICarrierHardware::Catapult:
226 gp.type = FGInterface::Catapult;
229 gp.type = FGInterface::Solid;
233 // Copy the velocity from the carrier class.
234 ud->carrier->getVelocityWrtEarth( gp.vel, gp.rot, gp.pivot );
238 // Initialize velocity field.
239 sgdSetVec3( gp.vel, 0.0, 0.0, 0.0 );
240 sgdSetVec3( gp.rot, 0.0, 0.0, 0.0 );
241 sgdSetVec3( gp.pivot, 0.0, 0.0, 0.0 );
243 // get some material information for use in the gear model
244 gp.material = globals->get_matlib()->findMaterial(l);
246 gp.type = gp.material->get_solid() ? FGInterface::Solid : FGInterface::Water;
253 FGGroundCache::putLineLeafIntoCache(const sgdSphere *wsp, const sgdMat4 xform,
256 GroundProperty gp = extractGroundProperty(l);
258 // Lines must have special meanings.
259 // Wires and catapults are done with lines.
260 int nl = l->getNumLines();
261 for (int i = 0; i < nl; ++i) {
266 l->getLine(i, v, v+1 );
267 for (int k=0; k<2; ++k) {
268 sgdSetVec3(ends[k], l->getVertex(v[k]));
269 sgdXformPnt3(ends[k], xform);
270 sphere.extend(ends[k]);
273 if (wsp->intersects( &sphere )) {
274 if (gp.type == FGInterface::Wire) {
276 sgdCopyVec3(wire.ends[0], ends[0]);
277 sgdCopyVec3(wire.ends[1], ends[1]);
278 sgdCopyVec3(wire.velocity, gp.vel);
279 sgdCopyVec3(wire.rotation, gp.rot);
280 sgdSubVec3(wire.rotation_pivot, gp.pivot, cache_center);
281 wire.wire_id = gp.wire_id;
283 wires.push_back(wire);
285 if (gp.type == FGInterface::Catapult) {
287 sgdCopyVec3(cat.start, ends[0]);
288 sgdCopyVec3(cat.end, ends[1]);
289 sgdCopyVec3(cat.velocity, gp.vel);
290 sgdCopyVec3(cat.rotation, gp.rot);
291 sgdSubVec3(cat.rotation_pivot, gp.pivot, cache_center);
293 catapults.push_back(cat);
300 FGGroundCache::putSurfaceLeafIntoCache(const sgdSphere *sp,
301 const sgdMat4 xform, bool sphIsec,
302 sgdVec3 down, ssgLeaf *l)
304 GroundProperty gp = extractGroundProperty(l);
306 int nt = l->getNumTriangles();
307 for (int i = 0; i < nt; ++i) {
310 t.material = gp.material;
312 l->getTriangle(i, &v[0], &v[1], &v[2]);
313 for (int k = 0; k < 3; ++k) {
314 sgdSetVec3(t.vertices[k], l->getVertex(v[k]));
315 sgdXformPnt3(t.vertices[k], xform);
316 t.sphere.extend(t.vertices[k]);
319 sgdMakePlane(t.plane, t.vertices[0], t.vertices[1], t.vertices[2]);
320 SGDfloat dot = sgdScalarProductVec3(down, t.plane);
322 if (!l->getCullFace()) {
323 // Surface points downwards, ignore for altitude computations.
326 sgdScaleVec4( t.plane, -1 );
329 // Check if the sphere around the vehicle intersects the sphere
330 // around that triangle. If so, put that triangle into the cache.
331 if (sphIsec && sp->intersects(&t.sphere)) {
332 sgdCopyVec3(t.velocity, gp.vel);
333 sgdCopyVec3(t.rotation, gp.rot);
334 sgdSubVec3(t.rotation_pivot, gp.pivot, cache_center);
336 triangles.push_back(t);
339 // In case the cache is empty, we still provide agl computations.
340 // But then we use the old way of having a fixed elevation value for
341 // the whole lifetime of this cache.
342 if ( fgdIsectSphereInfLine(t.sphere, sp->getCenter(), down) ) {
344 sgdSetVec3(tmp, sp->center[0], sp->center[1], sp->center[2]);
346 if ( sgdIsectInfLinePlane( isectpoint, tmp, down, t.plane ) &&
347 fgdPointInTriangle( isectpoint, t.vertices ) ) {
348 // Compute the offset to the ground cache midpoint
350 sgdSubVec3(off, isectpoint, tmp);
351 // Only accept the altitude if the intersection point is below the
352 // ground cache midpoint
353 if (0 < sgdScalarProductVec3( off, down )) {
355 sgdAddVec3(isectpoint, cache_center);
356 double this_radius = sgdLengthVec3(isectpoint);
357 if (ground_radius < this_radius)
358 ground_radius = this_radius;
366 FGGroundCache::velocityTransformTriangle(double dt,
367 FGGroundCache::Triangle& dst,
368 const FGGroundCache::Triangle& src)
370 sgdCopyVec3(dst.vertices[0], src.vertices[0]);
371 sgdCopyVec3(dst.vertices[1], src.vertices[1]);
372 sgdCopyVec3(dst.vertices[2], src.vertices[2]);
374 sgdCopyVec4(dst.plane, src.plane);
376 sgdCopyVec3(dst.sphere.center, src.sphere.center);
377 dst.sphere.radius = src.sphere.radius;
379 sgdCopyVec3(dst.velocity, src.velocity);
380 sgdCopyVec3(dst.rotation, src.rotation);
381 sgdCopyVec3(dst.rotation_pivot, src.rotation_pivot);
384 dst.material = src.material;
386 if (dt*sgdLengthSquaredVec3(src.velocity) != 0) {
387 sgdVec3 pivotoff, vel;
388 for (int i = 0; i < 3; ++i) {
389 sgdSubVec3(pivotoff, src.vertices[i], src.rotation_pivot);
390 sgdVectorProductVec3(vel, src.rotation, pivotoff);
391 sgdAddVec3(vel, src.velocity);
392 sgdAddScaledVec3(dst.vertices[i], vel, dt);
395 // Transform the plane equation
396 sgdSubVec3(pivotoff, dst.plane, src.rotation_pivot);
397 sgdVectorProductVec3(vel, src.rotation, pivotoff);
398 sgdAddVec3(vel, src.velocity);
399 dst.plane[3] += dt*sgdScalarProductVec3(dst.plane, vel);
401 sgdAddScaledVec3(dst.sphere.center, src.velocity, dt);
406 FGGroundCache::cache_fill(ssgBranch *branch, sgdMat4 xform,
407 sgdSphere* sp, sgdVec3 down, sgdSphere* wsp)
409 // Travel through all kids.
411 for ( e = branch->getKid(0); e != NULL ; e = branch->getNextKid() ) {
412 if ( !(e->getTraversalMask() & SSGTRAV_HOT) )
414 if ( e->getBSphere()->isEmpty() )
417 // We need to check further if either the sphere around the branch
418 // intersects the sphere around the aircraft or the line downwards from
419 // the aircraft intersects the branchs sphere.
421 sgdSetVec3(esphere.center, e->getBSphere()->center);
422 esphere.radius = e->getBSphere()->radius;
423 esphere.orthoXform(xform);
424 bool wspIsec = wsp->intersects(&esphere);
425 bool downIsec = fgdIsectSphereInfLine(esphere, sp->getCenter(), down);
426 if (!wspIsec && !downIsec)
429 // For branches collect up the transforms to reach that branch and
430 // call cache_fill recursively.
431 if ( e->isAKindOf( ssgTypeBranch() ) ) {
432 ssgBranch *b = (ssgBranch *)e;
433 if ( b->isAKindOf( ssgTypeTransform() ) ) {
434 // Collect up the transforms required to reach that part of
437 sgMakeIdentMat4( xform2 );
438 ssgTransform *t = (ssgTransform*)b;
439 t->getTransform( xform2 );
441 fgMultMat4(xform3, xform, xform2);
442 cache_fill( b, xform3, sp, down, wsp );
444 cache_fill( b, xform, sp, down, wsp );
447 // For leafs, check each triangle for intersection.
448 // This will minimize the number of vertices/triangles in the cache.
449 else if (e->isAKindOf(ssgTypeLeaf())) {
450 // Since we reach that leaf if we have an intersection with the
451 // most probably bigger wire/catapult cache sphere, we need to check
452 // that here, if the smaller cache for the surface has a chance for hits.
453 // Also, if the spheres do not intersect compute a coarse agl value
454 // by following the line downwards originating at the aircraft.
455 bool spIsec = sp->intersects(&esphere);
456 putSurfaceLeafIntoCache(sp, xform, spIsec, down, (ssgLeaf *)e);
458 // If we are here, we need to put all special hardware here into
461 putLineLeafIntoCache(wsp, xform, (ssgLeaf *)e);
467 FGGroundCache::prepare_ground_cache(double ref_time, const double pt[3],
472 found_ground = false;
477 // Store the parameters we used to build up that cache.
478 sgdCopyVec3(reference_wgs84_point, pt);
479 reference_vehicle_radius = rad;
480 // Store the time reference used to compute movements of moving triangles.
481 cache_ref_time = ref_time;
483 // Decide where we put the scenery center.
484 Point3D old_cntr = globals->get_scenery()->get_center();
485 Point3D cntr(pt[0], pt[1], pt[2]);
486 // Only move the cache center if it is unacceptable far away.
487 if (40*40 < old_cntr.distance3Dsquared(cntr))
488 globals->get_scenery()->set_center(cntr);
492 // The center of the cache.
493 sgdSetVec3(cache_center, cntr[0], cntr[1], cntr[2]);
496 sgdSubVec3(ptoff, pt, cache_center);
497 // Prepare sphere around the aircraft.
499 acSphere.setRadius(rad);
500 acSphere.setCenter(ptoff);
502 // Prepare bigger sphere around the aircraft.
503 // This one is required for reliably finding wires we have caught but
504 // have already left the hopefully smaller sphere for the ground reactions.
505 const double max_wire_dist = 300.0;
506 sgdSphere wireSphere;
507 wireSphere.setRadius(max_wire_dist < rad ? rad : max_wire_dist);
508 wireSphere.setCenter(ptoff);
510 // Down vector. Is used for croase agl computations when we are far enough
511 // from ground that we have an empty cache.
513 sgdSetVec3(down, -pt[0], -pt[1], -pt[2]);
514 sgdNormalizeVec3(down);
516 // We collapse all transforms we need to reach a particular leaf.
517 // The leafs itself will be then transformed later.
518 // So our cache is just flat.
519 // For leafs which are moving (carriers surface, etc ...)
520 // we will later store a speed in the GroundType class. We can then apply
521 // some translations to that nodes according to the time which has passed
522 // compared to that snapshot.
524 sgdMakeIdentMat4( xform );
527 // Walk the scene graph and extract solid ground triangles and carrier data.
528 ssgBranch *terrain = globals->get_scenery()->get_scene_graph();
529 cache_fill(terrain, xform, &acSphere, down, &wireSphere);
532 SG_LOG(SG_FLIGHT,SG_DEBUG, "prepare_ground_cache(): ac radius = " << rad
533 << ", # triangles = " << triangles.size()
534 << ", # wires = " << wires.size()
535 << ", # catapults = " << catapults.size()
536 << ", ground_radius = " << ground_radius );
538 // If the ground radius is still below 5e6 meters, then we do not yet have
540 found_ground = found_ground && 5e6 < ground_radius;
542 SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache "
543 "without any scenery below the aircraft" );
545 if (cntr != old_cntr)
546 globals->get_scenery()->set_center(old_cntr);
552 FGGroundCache::is_valid(double *ref_time, double pt[3], double *rad)
554 sgdCopyVec3(pt, reference_wgs84_point);
555 *rad = reference_vehicle_radius;
556 *ref_time = cache_ref_time;
561 FGGroundCache::get_cat(double t, const double dpt[3],
562 double end[2][3], double vel[2][3])
564 // start with a distance of 1e10 meters...
567 // Time difference to the reference time.
570 size_t sz = catapults.size();
571 for (size_t i = 0; i < sz; ++i) {
572 sgdVec3 pivotoff, rvel[2];
574 sgdCopyVec3(ls.a, catapults[i].start);
575 sgdCopyVec3(ls.b, catapults[i].end);
577 sgdSubVec3(pivotoff, ls.a, catapults[i].rotation_pivot);
578 sgdVectorProductVec3(rvel[0], catapults[i].rotation, pivotoff);
579 sgdAddVec3(rvel[0], catapults[i].velocity);
580 sgdSubVec3(pivotoff, ls.b, catapults[i].rotation_pivot);
581 sgdVectorProductVec3(rvel[1], catapults[i].rotation, pivotoff);
582 sgdAddVec3(rvel[1], catapults[i].velocity);
584 sgdAddVec3(ls.a, cache_center);
585 sgdAddVec3(ls.b, cache_center);
587 sgdAddScaledVec3(ls.a, rvel[0], t);
588 sgdAddScaledVec3(ls.b, rvel[1], t);
590 double this_dist = sgdDistSquaredToLineSegmentVec3( ls, dpt );
591 if (this_dist < dist) {
592 SG_LOG(SG_FLIGHT,SG_INFO, "Found catapult "
593 << this_dist << " meters away");
596 // The carrier code takes care of that ordering.
597 sgdCopyVec3( end[0], ls.a );
598 sgdCopyVec3( end[1], ls.b );
599 sgdCopyVec3( vel[0], rvel[0] );
600 sgdCopyVec3( vel[1], rvel[1] );
604 // At the end take the root, we only computed squared distances ...
609 FGGroundCache::get_agl(double t, const double dpt[3], double max_altoff,
610 double contact[3], double normal[3], double vel[3],
611 int *type, const SGMaterial** material, double *agl)
615 *type = FGInterface::Unknown;
619 sgdSetVec3( vel, 0.0, 0.0, 0.0 );
620 sgdSetVec3( contact, 0.0, 0.0, 0.0 );
621 sgdSetVec3( normal, 0.0, 0.0, 0.0 );
623 // Time difference to th reference time.
626 // The double valued point we start to search for intersection.
628 sgdSubVec3( pt, dpt, cache_center );
630 // The search direction
632 sgdSetVec3( dir, -dpt[0], -dpt[1], -dpt[2] );
633 sgdNormaliseVec3( dir );
635 // Initialize to something sensible
636 double current_radius = 0.0;
638 size_t sz = triangles.size();
639 for (size_t i = 0; i < sz; ++i) {
641 velocityTransformTriangle(t, triangle, triangles[i]);
642 if (!fgdIsectSphereInfLine(triangle.sphere, pt, dir))
645 // Check for intersection.
647 if ( sgdIsectInfLinePlane( isecpoint, pt, dir, triangle.plane ) &&
648 sgdPointInTriangle( isecpoint, triangle.vertices ) ) {
649 // Compute the vector from pt to the intersection point ...
651 sgdSubVec3(off, isecpoint, pt);
652 // ... and check if it is too high or not
653 if (-max_altoff < sgdScalarProductVec3( off, dir )) {
654 // Transform to the wgs system
655 sgdAddVec3( isecpoint, cache_center );
656 // compute the radius, good enough approximation to take the geocentric radius
657 SGDfloat radius = sgdLengthSquaredVec3(isecpoint);
658 if (current_radius < radius) {
659 current_radius = radius;
661 // Save the new potential intersection point.
662 sgdCopyVec3( contact, isecpoint );
663 // The first three values in the vector are the plane normal.
664 sgdCopyVec3( normal, triangle.plane );
665 // The velocity wrt earth.
667 sgdSubVec3(pivotoff, pt, triangle.rotation_pivot);
668 sgdVectorProductVec3(vel, triangle.rotation, pivotoff);
669 sgdAddVec3(vel, triangle.velocity);
670 // Save the ground type.
671 *type = triangle.type;
672 sgdVec3 dstToContact;
673 sgdSubVec3(dstToContact, contact, dpt);
674 *agl = sgdScalarProductVec3(dir, dstToContact);
676 *material = triangle.material;
685 // Whenever we did not have a ground triangle for the requested point,
686 // take the ground level we found during the current cache build.
687 // This is as good as what we had before for agl.
688 double r = sgdLengthVec3( dpt );
689 sgdCopyVec3( contact, dpt );
690 sgdScaleVec3( contact, ground_radius/r );
691 sgdCopyVec3( normal, dpt );
692 sgdNormaliseVec3( normal );
693 sgdSetVec3( vel, 0.0, 0.0, 0.0 );
695 // The altitude is the distance of the requested point from the
697 sgdVec3 dstToContact;
698 sgdSubVec3(dstToContact, contact, dpt);
699 *agl = sgdScalarProductVec3(dir, dstToContact);
700 *type = FGInterface::Unknown;
705 bool FGGroundCache::caught_wire(double t, const double pt[4][3])
707 size_t sz = wires.size();
711 // Time difference to the reference time.
714 // Build the two triangles spanning the area where the hook has moved
715 // during the past step.
718 sgdMakePlane( plane[0], pt[0], pt[1], pt[2] );
719 sgdCopyVec3( tri[0][0], pt[0] );
720 sgdCopyVec3( tri[0][1], pt[1] );
721 sgdCopyVec3( tri[0][2], pt[2] );
722 sgdMakePlane( plane[1], pt[0], pt[2], pt[3] );
723 sgdCopyVec3( tri[1][0], pt[0] );
724 sgdCopyVec3( tri[1][1], pt[2] );
725 sgdCopyVec3( tri[1][2], pt[3] );
727 // Intersect the wire lines with each of these triangles.
728 // You have caught a wire if they intersect.
729 for (size_t i = 0; i < sz; ++i) {
731 for (int k = 0; k < 2; ++k) {
732 sgdVec3 pivotoff, vel;
733 sgdCopyVec3(le[k], wires[i].ends[k]);
734 sgdSubVec3(pivotoff, le[k], wires[i].rotation_pivot);
735 sgdVectorProductVec3(vel, wires[i].rotation, pivotoff);
736 sgdAddVec3(vel, wires[i].velocity);
737 sgdAddScaledVec3(le[k], vel, t);
738 sgdAddVec3(le[k], cache_center);
741 for (int k=0; k<2; ++k) {
743 double isecval = sgdIsectLinesegPlane(isecpoint, le[0], le[1], plane[k]);
744 if ( 0.0 <= isecval && isecval <= 1.0 &&
745 sgdPointInTriangle( isecpoint, tri[k] ) ) {
746 SG_LOG(SG_FLIGHT,SG_INFO, "Caught wire");
747 // Store the wire id.
748 wire_id = wires[i].wire_id;
757 bool FGGroundCache::get_wire_ends(double t, double end[2][3], double vel[2][3])
759 // Fast return if we do not have an active wire.
763 // Time difference to the reference time.
766 // Search for the wire with the matching wire id.
767 size_t sz = wires.size();
768 for (size_t i = 0; i < sz; ++i) {
769 if (wires[i].wire_id == wire_id) {
770 for (size_t k = 0; k < 2; ++k) {
772 sgdCopyVec3(end[k], wires[i].ends[k]);
773 sgdSubVec3(pivotoff, end[k], wires[i].rotation_pivot);
774 sgdVectorProductVec3(vel[k], wires[i].rotation, pivotoff);
775 sgdAddVec3(vel[k], wires[i].velocity);
776 sgdAddScaledVec3(end[k], vel[k], t);
777 sgdAddVec3(end[k], cache_center);
786 void FGGroundCache::release_wire(void)