#include <float.h>
-#include <plib/sg.h>
#include <osg/CullFace>
#include <osg/Drawable>
#include <osg/Geode>
#include "flight.hxx"
#include "groundcache.hxx"
+/// Ok, variant that uses a infinite line istead of the ray.
+/// also not that this only works if the ray direction is normalized.
static inline bool
-fgdPointInTriangle( const SGVec3d& point, const SGVec3d tri[3] )
+intersectsInf(const SGRayd& ray, const SGSphered& sphere)
{
- SGVec3d dif;
-
- // Some tolerance in meters we accept a point to be outside of the triangle
- // and still return that it is inside.
- SGDfloat eps = 1e-2;
- SGDfloat min, max;
- // punt if outside bouding cube
- SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
- if( (point[0] < min - eps) || (point[0] > max + eps) )
- return false;
- dif[0] = max - min;
-
- SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
- if( (point[1] < min - eps) || (point[1] > max + eps) )
- return false;
- dif[1] = max - min;
-
- SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
- if( (point[2] < min - eps) || (point[2] > max + eps) )
- return false;
- dif[2] = max - min;
-
- // drop the smallest dimension so we only have to work in 2d.
- SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
- SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
- if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
- // x is the smallest dimension
- x1 = point[1];
- y1 = point[2];
- x2 = tri[0][1];
- y2 = tri[0][2];
- x3 = tri[1][1];
- y3 = tri[1][2];
- rx = tri[2][1];
- ry = tri[2][2];
- } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
- // y is the smallest dimension
- x1 = point[0];
- y1 = point[2];
- x2 = tri[0][0];
- y2 = tri[0][2];
- x3 = tri[1][0];
- y3 = tri[1][2];
- rx = tri[2][0];
- ry = tri[2][2];
- } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
- // z is the smallest dimension
- x1 = point[0];
- y1 = point[1];
- x2 = tri[0][0];
- y2 = tri[0][1];
- x3 = tri[1][0];
- y3 = tri[1][1];
- rx = tri[2][0];
- ry = tri[2][1];
- } else {
- // all dimensions are really small so lets call it close
- // enough and return a successful match
- return true;
- }
-
- // check if intersection point is on the same side of p1 <-> p2 as p3
- SGDfloat tmp = (y2 - y3);
- SGDfloat tmpn = (x2 - x3);
- int side1 = SG_SIGN (tmp * (rx - x3) + (y3 - ry) * tmpn);
- int side2 = SG_SIGN (tmp * (x1 - x3) + (y3 - y1) * tmpn
- + side1 * eps * fabs(tmpn));
- if ( side1 != side2 ) {
- // printf("failed side 1 check\n");
- return false;
- }
-
- // check if intersection point is on correct side of p2 <-> p3 as p1
- tmp = (y3 - ry);
- tmpn = (x3 - rx);
- side1 = SG_SIGN (tmp * (x2 - rx) + (ry - y2) * tmpn);
- side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
- + side1 * eps * fabs(tmpn));
- if ( side1 != side2 ) {
- // printf("failed side 2 check\n");
- return false;
- }
-
- // check if intersection point is on correct side of p1 <-> p3 as p2
- tmp = (y2 - ry);
- tmpn = (x2 - rx);
- side1 = SG_SIGN (tmp * (x3 - rx) + (ry - y3) * tmpn);
- side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
- + side1 * eps * fabs(tmpn));
- if ( side1 != side2 ) {
- // printf("failed side 3 check\n");
- return false;
- }
-
- return true;
-}
-
-// Test if the line given by the point on the line pt_on_line and the
-// line direction dir intersects the sphere sp.
-// Adapted from plib.
-static inline bool
-fgdIsectSphereInfLine(const SGVec3d& sphereCenter, double radius,
- const SGVec3d& pt_on_line, const SGVec3d& dir)
-{
- SGVec3d r = sphereCenter - pt_on_line;
- double projectedDistance = dot(r, dir);
+ SGVec3d r = sphere.getCenter() - ray.getOrigin();
+ double projectedDistance = dot(r, ray.getDirection());
double dist = dot(r, r) - projectedDistance * projectedDistance;
- return dist < radius*radius;
+ return dist < sphere.getRadius2();
}
template<typename T>
{
setTraversalMask(SG_NODEMASK_TERRAIN_BIT);
mDown = down;
+ mLocalDown = down;
sphIsec = true;
mBackfaceCulling = false;
mCacheReference = cacheReference;
+ mLocalCacheReference = cacheReference;
mCacheRadius = cacheRadius;
mWireCacheRadius = wireCacheRadius;
// cats or wires
double rw = bs.radius() + mWireCacheRadius;
if (rw*rw < centerDist2 &&
- !fgdIsectSphereInfLine(cntr, bs.radius(), mCacheReference, mDown))
+ !intersectsInf(SGRayd(mCacheReference, mDown),
+ SGSphered(cntr, bs.radius())))
return false;
sphIsec = false;
}
FGGroundCache::GroundProperty& gp = mGroundProperty;
// get some material information for use in the gear model
- gp.material = globals->get_matlib()->findMaterial(&node);
- if (gp.material) {
- gp.type = gp.material->get_solid() ? FGInterface::Solid : FGInterface::Water;
- return true;
- }
+ gp.type = FGInterface::Unknown;
osg::Referenced* base = node.getUserData();
if (!base)
return true;
break;
}
// Copy the velocity from the carrier class.
- ud->carrier->getVelocityWrtEarth( gp.vel, gp.rot, gp.pivot );
+ ud->carrier->getVelocityWrtEarth(gp.vel, gp.rot, gp.pivot);
return true;
}
bool oldBackfaceCulling = mBackfaceCulling;
updateCullMode(drawable->getStateSet());
+ FGGroundCache::GroundProperty& gp = mGroundProperty;
+ // get some material information for use in the gear model
+ gp.material = globals->get_matlib()->findMaterial(drawable->getStateSet());
+ if (gp.material)
+ gp.type = gp.material->get_solid() ? FGInterface::Solid : FGInterface::Water;
+
drawable->accept(mTriangleFunctor);
mBackfaceCulling = oldBackfaceCulling;
virtual void apply(osg::Transform& transform)
{
+ if (!enterNode(transform))
+ return;
+ bool oldBackfaceCulling = mBackfaceCulling;
+ bool oldSphIsec = sphIsec;
+ FGGroundCache::GroundProperty oldGp = mGroundProperty;
/// transform the caches center to local coords
osg::Matrix oldLocalToGlobal = mLocalToGlobal;
+ osg::Matrix oldGlobalToLocal = mGlobalToLocal;
transform.computeLocalToWorldMatrix(mLocalToGlobal, this);
+ transform.computeWorldToLocalMatrix(mGlobalToLocal, this);
+
+ SGVec3d oldLocalCacheReference = mLocalCacheReference;
+ mLocalCacheReference.osg() = mCacheReference.osg()*mGlobalToLocal;
+ SGVec3d oldLocalDown = mLocalDown;
+ mLocalDown.osg() = osg::Matrixd::transform3x3(mDown.osg(), mGlobalToLocal);
// walk the children
- apply((osg::Group&)transform);
+ traverse(transform);
// Restore that one
+ mLocalDown = oldLocalDown;
+ mLocalCacheReference = oldLocalCacheReference;
mLocalToGlobal = oldLocalToGlobal;
+ mGlobalToLocal = oldGlobalToLocal;
+ sphIsec = oldSphIsec;
+ mBackfaceCulling = oldBackfaceCulling;
+ mGroundProperty = oldGp;
}
-
+
void addTriangle(const osg::Vec3& v1, const osg::Vec3& v2,
const osg::Vec3& v3)
{
- FGGroundCache::Triangle t;
- osg::Vec3d gv1 = osg::Vec3d(v1)*mLocalToGlobal;
- osg::Vec3d gv2 = osg::Vec3d(v2)*mLocalToGlobal;
- osg::Vec3d gv3 = osg::Vec3d(v3)*mLocalToGlobal;
- for (unsigned i = 0; i < 3; ++i) {
- t.vertices[0][i] = gv1[i];
- t.vertices[1][i] = gv2[i];
- t.vertices[2][i] = gv3[i];
- }
- // FIXME: can do better ...
- t.boundCenter = (1.0/3)*(t.vertices[0] + t.vertices[1] + t.vertices[2]);
- t.boundRadius = std::max(length(t.vertices[0] - t.boundCenter),
- length(t.vertices[1] - t.boundCenter));
- t.boundRadius = std::max(t.boundRadius,
- length(t.vertices[2] - t.boundCenter));
-
- sgdMakePlane(t.plane.sg(), t.vertices[0].sg(), t.vertices[1].sg(),
- t.vertices[2].sg());
- double d = sgdScalarProductVec3(mDown.sg(), t.plane.sg());
- if (d > 0) {
+ SGVec3d v[3] = {
+ SGVec3d(v1),
+ SGVec3d(v2),
+ SGVec3d(v3)
+ };
+
+ // a bounding sphere in the node local system
+ SGVec3d boundCenter = (1.0/3)*(v[0] + v[1] + v[2]);
+ double boundRadius = std::max(distSqr(v[0], boundCenter),
+ distSqr(v[1], boundCenter));
+ boundRadius = std::max(boundRadius, distSqr(v[2], boundCenter));
+ boundRadius = sqrt(boundRadius);
+
+ SGRayd ray(mLocalCacheReference, mLocalDown);
+
+ // if we are not in the downward cylinder bail out
+ if (!intersectsInf(ray, SGSphered(boundCenter, boundRadius + mCacheRadius)))
+ return;
+
+ SGTriangled triangle(v);
+
+ // The normal and plane in the node local coordinate system
+ SGVec3d n = cross(triangle.getEdge(0), triangle.getEdge(1));
+ if (0 < dot(mLocalDown, n)) {
if (mBackfaceCulling) {
// Surface points downwards, ignore for altitude computations.
return;
- } else
- t.plane = -t.plane;
+ } else {
+ triangle.flip();
+ }
}
-
- // Check if the sphere around the vehicle intersects the sphere
- // around that triangle. If so, put that triangle into the cache.
- if (sphIsec &&
- distSqr(t.boundCenter, mCacheReference)
- < (t.boundRadius + mCacheRadius)*(t.boundRadius + mCacheRadius) ) {
- t.velocity = mGroundProperty.vel;
- t.rotation = mGroundProperty.rot;
- t.rotation_pivot = mGroundProperty.pivot - mGroundCache->cache_center;
- t.type = mGroundProperty.type;
- mGroundCache->triangles.push_back(t);
+
+ // Only check if the triangle is in the cache sphere if the plane
+ // containing the triangle is near enough
+ if (sphIsec) {
+ double d = dot(n, v[0] - mLocalCacheReference);
+ if (d*d < mCacheRadius*dot(n, n)) {
+ // Check if the sphere around the vehicle intersects the sphere
+ // around that triangle. If so, put that triangle into the cache.
+ double r2 = boundRadius + mCacheRadius;
+ if (distSqr(boundCenter, mLocalCacheReference) < r2*r2) {
+ FGGroundCache::Triangle t;
+ t.triangle.setBaseVertex(SGVec3d(v[0].osg()*mLocalToGlobal));
+ t.triangle.setEdge(0, SGVec3d(osg::Matrixd::transform3x3(triangle.getEdge(0).osg(), mLocalToGlobal)));
+ t.triangle.setEdge(1, SGVec3d(osg::Matrixd::transform3x3(triangle.getEdge(1).osg(), mLocalToGlobal)));
+
+ t.sphere.setCenter(SGVec3d(boundCenter.osg()*mLocalToGlobal));
+ t.sphere.setRadius(boundRadius);
+
+ t.velocity = mGroundProperty.vel;
+ t.rotation = mGroundProperty.rot;
+ t.rotation_pivot = mGroundProperty.pivot;
+ t.type = mGroundProperty.type;
+ t.material = mGroundProperty.material;
+ mGroundCache->triangles.push_back(t);
+ }
+ }
}
// In case the cache is empty, we still provide agl computations.
// But then we use the old way of having a fixed elevation value for
// the whole lifetime of this cache.
- if ( fgdIsectSphereInfLine(t.boundCenter, t.boundRadius,
- mCacheReference, mDown) ) {
- SGVec3d isectpoint;
- if ( sgdIsectInfLinePlane( isectpoint.sg(), mCacheReference.sg(),
- mDown.sg(), t.plane.sg() ) &&
- fgdPointInTriangle( isectpoint, t.vertices ) ) {
- // Only accept the altitude if the intersection point is below the
- // ground cache midpoint
- if (0 < dot(isectpoint - mCacheReference, mDown)) {
- mGroundCache->found_ground = true;
- isectpoint += mGroundCache->cache_center;
- double this_radius = length(isectpoint);
- if (mGroundCache->ground_radius < this_radius)
- mGroundCache->ground_radius = this_radius;
- }
+ SGVec3d isectpoint;
+ if (intersects(isectpoint, triangle, ray, 1e-4)) {
+ mGroundCache->found_ground = true;
+ isectpoint.osg() = isectpoint.osg()*mLocalToGlobal;
+ double this_radius = length(isectpoint);
+ if (mGroundCache->ground_radius < this_radius) {
+ mGroundCache->ground_radius = this_radius;
+ mGroundCache->_type = mGroundProperty.type;
+ mGroundCache->_material = mGroundProperty.material;
}
}
}
-
+
void addLine(const osg::Vec3& v1, const osg::Vec3& v2)
{
- SGVec3d gv1 = SGVec3d(osg::Vec3d(v1)*mLocalToGlobal);
- SGVec3d gv2 = SGVec3d(osg::Vec3d(v2)*mLocalToGlobal);
+ SGVec3d gv1(osg::Vec3d(v1)*mLocalToGlobal);
+ SGVec3d gv2(osg::Vec3d(v2)*mLocalToGlobal);
SGVec3d boundCenter = 0.5*(gv1 + gv2);
double boundRadius = length(gv1 - boundCenter);
wire.ends[1] = gv2;
wire.velocity = mGroundProperty.vel;
wire.rotation = mGroundProperty.rot;
- wire.rotation_pivot = mGroundProperty.pivot - mGroundCache->cache_center;
+ wire.rotation_pivot = mGroundProperty.pivot;
wire.wire_id = mGroundProperty.wire_id;
mGroundCache->wires.push_back(wire);
// Trick to get the ends in the right order.
// Use the x axis in the original coordinate system. Choose the
// most negative x-axis as the one pointing forward
- if (v1[0] < v2[0]) {
+ if (v1[0] > v2[0]) {
cat.start = gv1;
cat.end = gv2;
} else {
}
cat.velocity = mGroundProperty.vel;
cat.rotation = mGroundProperty.rot;
- cat.rotation_pivot = mGroundProperty.pivot - mGroundCache->cache_center;
+ cat.rotation_pivot = mGroundProperty.pivot;
mGroundCache->catapults.push_back(cat);
}
double mCacheRadius;
double mWireCacheRadius;
osg::Matrix mLocalToGlobal;
+ osg::Matrix mGlobalToLocal;
SGVec3d mDown;
+ SGVec3d mLocalDown;
+ SGVec3d mLocalCacheReference;
bool sphIsec;
bool mBackfaceCulling;
FGGroundCache::GroundProperty mGroundProperty;
FGGroundCache::FGGroundCache()
{
- cache_center = SGVec3d(0, 0, 0);
ground_radius = 0.0;
cache_ref_time = 0.0;
wire_id = 0;
inline void
FGGroundCache::velocityTransformTriangle(double dt,
- FGGroundCache::Triangle& dst,
+ SGTriangled& dst, SGSphered& sdst,
const FGGroundCache::Triangle& src)
{
- dst = src;
+ dst = src.triangle;
+ sdst = src.sphere;
- if (fabs(dt*dot(src.velocity, src.velocity)) < SGLimitsd::epsilon())
+ if (dt*dt*dot(src.velocity, src.velocity) < SGLimitsd::epsilon())
return;
- for (int i = 0; i < 3; ++i) {
- SGVec3d pivotoff = src.vertices[i] - src.rotation_pivot;
- dst.vertices[i] += dt*(src.velocity + cross(src.rotation, pivotoff));
- }
-
- // Transform the plane equation
- SGVec3d pivotoff, vel;
- sgdSubVec3(pivotoff.sg(), dst.plane.sg(), src.rotation_pivot.sg());
- vel = src.velocity + cross(src.rotation, pivotoff);
- dst.plane[3] += dt*sgdScalarProductVec3(dst.plane.sg(), vel.sg());
-
- dst.boundCenter += dt*src.velocity;
+ SGVec3d baseVert = dst.getBaseVertex();
+ SGVec3d pivotoff = baseVert - src.rotation_pivot;
+ baseVert += dt*(src.velocity + cross(src.rotation, pivotoff));
+ dst.setBaseVertex(baseVert);
+ dst.setEdge(0, dst.getEdge(0) + dt*cross(src.rotation, dst.getEdge(0)));
+ dst.setEdge(1, dst.getEdge(1) + dt*cross(src.rotation, dst.getEdge(1)));
}
+
bool
FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt,
double rad)
SGQuatd hlToEc = SGQuatd::fromLonLat(SGGeod::fromCart(pt));
down = hlToEc.rotate(SGVec3d(0, 0, 1));
- // Decide where we put the scenery center.
- SGVec3d old_cntr = globals->get_scenery()->get_center();
- SGVec3d cntr(pt);
- // Only move the cache center if it is unacceptable far away.
- if (40*40 < distSqr(old_cntr, cntr))
- globals->get_scenery()->set_center(cntr);
- else
- cntr = old_cntr;
-
- // The center of the cache.
- cache_center = cntr;
-
// Prepare sphere around the aircraft.
- SGVec3d ptoff = pt - cache_center;
double cacheRadius = rad;
// Prepare bigger sphere around the aircraft.
double wireCacheRadius = max_wire_dist < rad ? rad : max_wire_dist;
// Walk the scene graph and extract solid ground triangles and carrier data.
- GroundCacheFillVisitor gcfv(this, down, ptoff, cacheRadius, wireCacheRadius);
+ GroundCacheFillVisitor gcfv(this, down, pt, cacheRadius, wireCacheRadius);
globals->get_scenery()->get_scene_graph()->accept(gcfv);
// some stats
SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache "
"without any scenery below the aircraft" );
- if (cntr != old_cntr)
- globals->get_scenery()->set_center(old_cntr);
-
return found_ground;
}
rvel[1] = catapults[i].velocity + cross(catapults[i].rotation, pivotoff);
SGVec3d thisEnd[2];
- thisEnd[0] = cache_center + catapults[i].start + t*rvel[0];
- thisEnd[1] = cache_center + catapults[i].end + t*rvel[1];
-
- sgdLineSegment3 ls;
- sgdCopyVec3(ls.a, thisEnd[0].sg());
- sgdCopyVec3(ls.b, thisEnd[1].sg());
- double this_dist = sgdDistSquaredToLineSegmentVec3( ls, dpt.sg() );
+ thisEnd[0] = catapults[i].start + t*rvel[0];
+ thisEnd[1] = catapults[i].end + t*rvel[1];
+ double this_dist = distSqr(SGLineSegmentd(thisEnd[0], thisEnd[1]), dpt);
if (this_dist < dist) {
SG_LOG(SG_FLIGHT,SG_INFO, "Found catapult "
<< this_dist << " meters away");
t -= cache_ref_time;
// The double valued point we start to search for intersection.
- SGVec3d pt = dpt - cache_center;
+ SGVec3d pt = dpt;
+ // shift the start of our ray by maxaltoff upwards
+ SGRayd ray(pt - max_altoff*down, down);
// Initialize to something sensible
double current_radius = 0.0;
size_t sz = triangles.size();
for (size_t i = 0; i < sz; ++i) {
- Triangle triangle;
- velocityTransformTriangle(t, triangle, triangles[i]);
- if (!fgdIsectSphereInfLine(triangle.boundCenter, triangle.boundRadius, pt, down))
+ SGSphered sphere;
+ SGTriangled triangle;
+ velocityTransformTriangle(t, triangle, sphere, triangles[i]);
+ if (!intersectsInf(ray, sphere))
continue;
// Check for intersection.
SGVec3d isecpoint;
- if ( sgdIsectInfLinePlane( isecpoint.sg(), pt.sg(), down.sg(), triangle.plane.sg() ) &&
- fgdPointInTriangle( isecpoint, triangle.vertices ) ) {
+ if (intersects(isecpoint, triangle, ray, 1e-4)) {
// Compute the vector from pt to the intersection point ...
SGVec3d off = isecpoint - pt;
// ... and check if it is too high or not
- if (-max_altoff < dot(off, down)) {
- // Transform to the wgs system
- isecpoint += cache_center;
- // compute the radius, good enough approximation to take the geocentric radius
- double radius = dot(isecpoint, isecpoint);
- if (current_radius < radius) {
- current_radius = radius;
- ret = true;
- // Save the new potential intersection point.
- contact = isecpoint;
- // The first three values in the vector are the plane normal.
- sgdCopyVec3( normal.sg(), triangle.plane.sg() );
- // The velocity wrt earth.
- SGVec3d pivotoff = pt - triangle.rotation_pivot;
- vel = triangle.velocity + cross(triangle.rotation, pivotoff);
- // Save the ground type.
- *type = triangle.type;
- *agl = dot(down, contact - dpt);
- if (material)
- *material = triangle.material;
- }
+
+ // compute the radius, good enough approximation to take the geocentric radius
+ double radius = dot(isecpoint, isecpoint);
+ if (current_radius < radius) {
+ current_radius = radius;
+ ret = true;
+ // Save the new potential intersection point.
+ contact = isecpoint;
+ // The first three values in the vector are the plane normal.
+ normal = triangle.getNormal();
+ // The velocity wrt earth.
+ SGVec3d pivotoff = pt - triangles[i].rotation_pivot;
+ vel = triangles[i].velocity + cross(triangles[i].rotation, pivotoff);
+ // Save the ground type.
+ *type = triangles[i].type;
+ *agl = dot(down, contact - dpt);
+ if (material)
+ *material = triangles[i].material;
}
}
}
// The altitude is the distance of the requested point from the
// contact point.
*agl = dot(down, contact - dpt);
- *type = FGInterface::Unknown;
+ *type = _type;
+ if (material)
+ *material = _material;
return ret;
}
// Build the two triangles spanning the area where the hook has moved
// during the past step.
- SGVec4d plane[2];
- SGVec3d tri[2][3];
- sgdMakePlane( plane[0].sg(), pt[0].sg(), pt[1].sg(), pt[2].sg() );
- tri[0][0] = pt[0];
- tri[0][1] = pt[1];
- tri[0][2] = pt[2];
- sgdMakePlane( plane[1].sg(), pt[0].sg(), pt[2].sg(), pt[3].sg() );
- tri[1][0] = pt[0];
- tri[1][1] = pt[2];
- tri[1][2] = pt[3];
+ SGTriangled triangle[2];
+ triangle[0].set(pt[0], pt[1], pt[2]);
+ triangle[1].set(pt[0], pt[2], pt[3]);
// Intersect the wire lines with each of these triangles.
// You have caught a wire if they intersect.
le[k] = wires[i].ends[k];
SGVec3d pivotoff = le[k] - wires[i].rotation_pivot;
SGVec3d vel = wires[i].velocity + cross(wires[i].rotation, pivotoff);
- le[k] += t*vel + cache_center;
+ le[k] += t*vel;
}
+ SGLineSegmentd lineSegment(le[0], le[1]);
for (int k=0; k<2; ++k) {
- SGVec3d isecpoint;
- double isecval = sgdIsectLinesegPlane(isecpoint.sg(), le[0].sg(),
- le[1].sg(), plane[k].sg());
- if ( 0.0 <= isecval && isecval <= 1.0 &&
- fgdPointInTriangle( isecpoint, tri[k] ) ) {
+ if (intersects(triangle[k], lineSegment)) {
SG_LOG(SG_FLIGHT,SG_INFO, "Caught wire");
// Store the wire id.
wire_id = wires[i].wire_id;
for (size_t i = 0; i < sz; ++i) {
if (wires[i].wire_id == wire_id) {
for (size_t k = 0; k < 2; ++k) {
- SGVec3d pivotoff = end[k] - wires[i].rotation_pivot;
+ SGVec3d pivotoff = wires[i].ends[k] - wires[i].rotation_pivot;
vel[k] = wires[i].velocity + cross(wires[i].rotation, pivotoff);
- end[k] = cache_center + wires[i].ends[k] + t*vel[k];
+ end[k] = wires[i].ends[k] + t*vel[k];
}
return true;
}
projects / flightgear.git / blobdiff