From 764eb2a2e77bdf699f99e5e25ad0d9026efc0927 Mon Sep 17 00:00:00 2001
From: andy <andy>
Date: Wed, 20 Feb 2002 07:12:27 +0000
Subject: [PATCH] Automatically generate "contact" points for collision
 detection.  Implemented as extra (and invisible) gear objects.

---
 src/FDM/YASim/Airplane.cpp | 61 ++++++++++++++++++++++++++++++++++++++
 src/FDM/YASim/Airplane.hpp |  3 ++
 src/FDM/YASim/Wing.cpp     | 15 ++++++++++
 src/FDM/YASim/Wing.hpp     |  4 +++
 4 files changed, 83 insertions(+)

diff --git a/src/FDM/YASim/Airplane.cpp b/src/FDM/YASim/Airplane.cpp
index da81beba2..7515dc639 100644
--- a/src/FDM/YASim/Airplane.cpp
+++ b/src/FDM/YASim/Airplane.cpp
@@ -46,6 +46,8 @@ Airplane::~Airplane()
 	delete (GearRec*)_gears.get(i);
     for(i=0; i<_surfs.size(); i++)
 	delete (Surface*)_surfs.get(i);    
+    for(i=0; i<_contacts.size(); i++)
+        delete[] (float*)_contacts.get(i);
 }
 
 void Airplane::iterate(float dt)
@@ -350,8 +352,26 @@ void Airplane::setupState(float aoa, float speed, State* s)
     s->pos[2] = 1;
 }
 
+void Airplane::addContactPoint(float* pos)
+{
+    float* cp = new float[3];
+    cp[0] = pos[0];
+    cp[1] = pos[1];
+    cp[2] = pos[2];
+    _contacts.add(cp);
+}
+
 float Airplane::compileWing(Wing* w)
 {
+    // The tip of the wing is a contact point
+    float tip[3];
+    w->getTip(tip);
+    addContactPoint(tip);
+    if(w->isMirrored()) {
+        tip[1] *= -1;
+        addContactPoint(tip);
+    }
+
     // Make sure it's initialized.  The surfaces will pop out with
     // total drag coefficients equal to their areas, which is what we
     // want.
@@ -379,6 +399,10 @@ float Airplane::compileWing(Wing* w)
 
 float Airplane::compileFuselage(Fuselage* f)
 {
+    // The front and back are contact points
+    addContactPoint(f->front);
+    addContactPoint(f->back);
+
     float wgt = 0;
     float fwd[3];
     Math::sub3(f->front, f->back, fwd);
@@ -456,6 +480,39 @@ void Airplane::compileGear(GearRec* gr)
     _surfs.add(s);
 }
 
+void Airplane::compileContactPoints()
+{
+    // Figure it will compress by 20cm
+    float comp[3];
+    float DIST = 0.2;
+    comp[0] = 0; comp[1] = 0; comp[2] = DIST;
+
+    // Give it a spring constant such that at full compression it will
+    // hold up 10 times the planes mass.  That's about right.  Yeah.
+    float mass = _model.getBody()->getTotalMass();
+    float spring = (1/DIST) * 9.8 * 10 * mass;
+    float damp = 2 * Math::sqrt(spring * mass);
+
+    int i;
+    for(i=0; i<_contacts.size(); i++) {
+        float *cp = (float*)_contacts.get(i);
+
+        Gear* g = new Gear();
+        g->setPosition(cp);
+        
+        g->setCompression(comp);
+        g->setSpring(spring);
+        g->setDamping(damp);
+        g->setBrake(1);
+
+        // I made these up
+        g->setStaticFriction(0.6);
+        g->setDynamicFriction(0.5);
+
+        _model.addGear(g);
+    }
+}
+
 void Airplane::compile()
 {
     double ground[3];
@@ -529,6 +586,10 @@ void Airplane::compile()
     solveGear();
     solve();
 
+    // Do this after solveGear, because it creates "gear" objects that
+    // we don't want to affect.
+    compileContactPoints();
+
     // Drop the gear (use a really big dt)
     setGearState(true, 1000000);
 }
diff --git a/src/FDM/YASim/Airplane.hpp b/src/FDM/YASim/Airplane.hpp
index de0038b63..e1901af76 100644
--- a/src/FDM/YASim/Airplane.hpp
+++ b/src/FDM/YASim/Airplane.hpp
@@ -90,6 +90,8 @@ private:
     void applyDragFactor(float factor);
     void applyLiftRatio(float factor);
     float clamp(float val, float min, float max);
+    void addContactPoint(float* pos);
+    void compileContactPoints();
     float normFactor(float f);
 
     Model _model;
@@ -108,6 +110,7 @@ private:
     float _ballast;
 
     Vector _gears;
+    Vector _contacts; // non-gear ground contact points
     Vector _weights;
     Vector _surfs; // NON-wing Surfaces
 
diff --git a/src/FDM/YASim/Wing.cpp b/src/FDM/YASim/Wing.cpp
index 22cc84a66..bd8ecb979 100644
--- a/src/FDM/YASim/Wing.cpp
+++ b/src/FDM/YASim/Wing.cpp
@@ -208,6 +208,21 @@ float Wing::getGroundEffect(float* posOut)
     return span;
 }
 
+void Wing::getTip(float* tip)
+{
+    tip[0] = -Math::tan(_sweep);
+    tip[1] = Math::cos(_dihedral);
+    tip[2] = Math::sin(_dihedral);
+    Math::unit3(tip, tip);
+    Math::mul3(_length, tip, tip);
+    Math::add3(_base, tip, tip);
+}
+
+bool Wing::isMirrored()
+{
+    return _mirror;
+}
+
 void Wing::compile()
 {
     // Have we already been compiled?
diff --git a/src/FDM/YASim/Wing.hpp b/src/FDM/YASim/Wing.hpp
index 861436e8a..f6353ddc9 100644
--- a/src/FDM/YASim/Wing.hpp
+++ b/src/FDM/YASim/Wing.hpp
@@ -44,6 +44,10 @@ public:
     // Compile the thing into a bunch of Surface objects
     void compile();
 
+    void getTip(float* tip);
+
+    bool isMirrored();
+
     // Ground effect information
     float getGroundEffect(float* posOut);
     
-- 
2.39.5