void BodyRigid::forceAndTorqueAccum(Vect3 const &f, bool forceInLocal, Vect3 const &forceAppLocal, Vect3 const &t, bool torqueInLocal)
	{
		Vect3 force; Vect3 torque;
		if(forceInLocal)
		{
			force = m_q*f;
			torque = forceAppLocal.cross(f);
		}
		else
		{
			force = f;
			torque = forceAppLocal.cross(m_q.inverse()*f);
		}

		if(torqueInLocal)
		{
			torque += t;
		}
		else
		{
			torque += m_q.inverse()*t;
		}

		m_appliedForce += force;
		m_appliedTorque += torque;

	}

// Assumes 0 <= B < T
LossData WCV_tvar::WCV_interval(const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi, double B, double T) const {
  double time_in = T;
  double time_out = B;

  Vect2 so2 = so.vect2();
  Vect2 si2 = si.vect2();
  Vect2 s2 = so2.Sub(si2);
  Vect2 vo2 = vo.vect2();
  Vect2 vi2 = vi.vect2();
  Vect2 v2 = vo2.Sub(vi2);
  double sz = so.z-si.z;
  double vz = vo.z-vi.z;

  Interval ii = wcv_vertical->vertical_WCV_interval(table.getZTHR(),table.getTCOA(),B,T,sz,vz);

  if (ii.low > ii.up) {
    return LossData(time_in,time_out);
  }
  Vect2 step = v2.ScalAdd(ii.low,s2);
  if (Util::almost_equals(ii.low,ii.up)) { // [CAM] Changed from == to almost_equals to mitigate numerical problems
    if (horizontal_WCV(step,v2)) {
      time_in = ii.low;
      time_out = ii.up;
    }
    return LossData(time_in,time_out);
  }
  LossData ld = horizontal_WCV_interval(ii.up-ii.low,step,v2);
  time_in = ld.getTimeIn() + ii.low;
  time_out = ld.getTimeOut() + ii.low;
  return LossData(time_in,time_out);
}

void CollisionSphere::genContactsS(const CollisionSphere& s, std::vector<std::shared_ptr<IContact>>& o) const
{
	// Get distance between two objects, with this one at the origin...
	Vect3 distance = s.getPos() - this->getPos();

	// Spheres are touching if the magnitude of the distance is less than
	//  the sum of the radii
	// Early out if this is not the case.
	if (distance.squareMagnitude() > ((this->getRadius() + s.getRadius()) * (this->getRadius() + s.getRadius())))
	{
		return;
	}
	else
	{
		// Generate contacts!
		// Contact is midpoint in world space of intersection,
		// normal is direction of intersection.
		Vect3Normal directionNormal = (s.getPos() - this->getPos());
		float contactMagnitude = this->getRadius() + s.getRadius() - distance.magnitude();

		// From the other object, go to the edge of the sphere.
		//  Subtract that from the edge of this sphere.
		//  now multiply the whole thing by 1/2, and add the edge of this sphere.
		// That's the midpoint of the intersected space of the two spheres.
		Vect3 contactPoint = ((s.getPos() - directionNormal * s.getRadius())
			- (this->getPos() + directionNormal * this->getRadius()))
			* 0.5f
			+ (this->getPos() + directionNormal * this->getRadius());

		o.push_back(summonDemons(contactPoint, directionNormal * -1.f, contactMagnitude, s.getAttachedObjectPtr(), _attachedObject));
		o.push_back(summonDemons(contactPoint, directionNormal, contactMagnitude, this->getAttachedObjectPtr(), s.getAttachedObjectPtr()));
	}
}

	void Force2BodySpringDamp::apply(double t)
	{
		// pt1 on body body 1 & pt2 on body 2
		Vect3 pt1Pos = m_body1->getPosition(m_body1Offset);
		Vect3 pt1Vel = m_body1->getVelocityGlobal(m_body1Offset);
		Vect3 pt2Pos = m_body2->getPosition(m_body2Offset);
		Vect3 pt2Vel = m_body2->getVelocityGlobal(m_body2Offset);

		// distance between 2 points in global
		Vect3 dist = pt2Pos-pt1Pos;
		// Relative Velocity
		Vect3 revVel = pt2Vel - pt1Vel;

		// just to have meaningfull names
		Vect3 normalForceVect = dist;
		//normailze normalForceVector in place and get magnitude
		double dist_mag = magnitude(normalForceVect);
		normalForceVect.normalize();

		double springForceMag = (dist_mag-m_fl)*m_k;

		double velAlongSpring = normalForceVect.dot(revVel);
		double dampForceMag = velAlongSpring * m_c;

		double totalForceMag = springForceMag + dampForceMag;

		Vect3 totalForceVect = normalForceVect * totalForceMag;

		m_body1->forceAccum(totalForceVect,false,m_body1Offset);
		m_body2->forceAccum(-totalForceVect,false,m_body2Offset);

	}

void TIME_STEPPER::time_loop() {

    if (first_step) {
        REAL OmRMax = 0.0, MaxRadius = 0.0;
        if (globalSystem->useBodies) {
            for (int i = 0; i < BODY::AllBodyFaces.size(); ++i) {
                Vect3 Pos = BODY::AllBodyFaces[i]->CollocationPoint - BODY::AllBodyFaces[i]->Owner->CG;
                // 	Get point kinematic velocity - rotational part first
                Vect3 Vrot = BODY::AllBodyFaces[i]->Owner->BodyRates.Cross(Pos);
                // 	Add to translational velocity....
                Vect3 Vkin = BODY::AllBodyFaces[i]->Owner->Velocity + Vrot;
                OmRMax = max(Vkin.Mag(), OmRMax);
                MaxRadius = max(MaxRadius, Pos.Mag());
            }
        }

        
        dt =  globalSystem->dtInit;// = cfl_lim/OmRMax;
        dt_prev = cfl_lim/OmRMax;
        if (globalSystem->useBodies) {
            BODY::BodySubStep(globalSystem->dtInit, globalSystem->NumSubSteps);
            globalSystem->PutWakesInTree();
            
        }
        
        globalOctree->Reset();
        globalOctree->GetVels();
        
//        if (globalSystem->useFMM) {
//#pragma omp parallel for
//            for (int i = 0; i < FVMCell::AllCells.size(); ++i)
//                for (int j = 0; j < FVMCell::AllCells.size(); ++j)
//                    FVMCell::AllCells[i]->Velocity += UTIL::globalDirectVel(FVMCell::AllCells[j]->Position - FVMCell::AllCells[i]->Position, FVMCell::AllCells[j]->Omega);
//        }
        first_step = false;
    } else {
#ifdef TIME_STEPS
        long unsigned int t1 = ticks();
#endif
       

        TimeAdvance();
        //      Produce Output
        if (globalTimeStepper->dump_next) {
            globalSystem->WriteData();
        }


#ifdef TIME_STEPS
        long unsigned int t13 = ticks();
        stringstream tmp;
        tmp << "Total....................: " << double(t13 - t1) / 1000.0 << endl;
        globalIO->step_data += tmp.str();
#endif
        //      Display Status
        globalIO->stat_step();
    }
}

	void BodyRigid::forceAccumGlobal(Vect3 const &globalForce, Vect3 const &globalPnt)
	{
	    Vect3 fLocal = m_q.inverse() * globalForce;
	    Vect3 pntLocal = m_q.inverse() * (globalPnt-m_pos);
	    Vect3 tLocal = pntLocal.cross(fLocal);
		//std::cout << "dfadsfads" << std::endl;
	    m_appliedForce +=  globalForce;
		m_appliedTorque += tLocal;
	}

// time of closest approach
double VectFuns::tau(const Vect3& s, const Vect3& vo, const Vect3& vi) {
	double rtn;
	Vect3 v = vo.Sub(vi);
	double nv = v.norm();
	if (Util::almost_equals(nv,0.0)) {
		rtn = std::numeric_limits<double>::max();        // pseudo infinity
	} else
		rtn = -s.dot(v)/(nv*nv);
	return rtn;
}// tau

bool WCV_tvar::violation(const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi) const {
  Vect2 so2 = so.vect2();
  Vect2 si2 = si.vect2();
  Vect2 s2 = so2.Sub(si2);
  Vect2 vo2 = vo.vect2();
  Vect2 vi2 = vi.vect2();
  Vect2 v2 = vo2.Sub(vi2);
  return horizontal_WCV(s2,v2) &&
      wcv_vertical->vertical_WCV(table.getZTHR(),table.getTCOA(),so.z-si.z,vo.z-vi.z);
}

 void BodyRigid::setMassInertia(Vect3 const &row0,Vect3 const &row1,Vect3 const &row2)
 {
     m_inertia(0,0)=row0.x(); m_inertia(0,1)=row0.y(); m_inertia(0,2)=row0.z();
     m_inertia(1,0)=row1.x(); m_inertia(1,1)=row1.y(); m_inertia(1,2)=row1.z();
     m_inertia(2,0)=row2.x(); m_inertia(2,1)=row2.y(); m_inertia(2,2)=row2.z();
     cout << m_inertia << std::endl;
 }

void Quat::invXform(Vect3 &v) const
{
  Vect3 *u, uv, uuv;
  
  u = (Vect3 *) &x_;
  uv.cross(*u, v);
  uuv.cross(*u, uv);
  uv.scale(2.0 * -s_);
  uuv.scale(2.0);
  v.add(uv);
  v.add(uuv);
}

bool Target_Icosahedron::Check(real x, real y, real z)
{
	const int indd[20] = { 0, 0, 0, 0, 0, 5, 6, 7, 8, 8, 9, 9, 9, 10, 10, 11, 11, 11, 11, 11 };

	Vect3<real> tmp;
	bool res = true;
	for(int i=0; i<numNormals; ++i)
	{
		tmp.Set(x, y, z); 
		res = res && ((normals[i].Scalar(tmp - vertices[indd[i]])) <= (real)0.);
	}
	return res;
}

/**
 * Returns values indicating whether the ownship state will pass in front of or behind the intruder (from a horizontal perspective)
 * @param so ownship position
 * @param vo ownship velocity
 * @param si intruder position
 * @param vi intruder velocity
 * @return 1 if ownship will pass in front (or collide, from a horizontal sense), -1 if ownship will pass behind, 0 if divergent or parallel
 */
int VectFuns::passingDirection(const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi) {
	double toi = timeOfIntersection(so,vo,si,vi);
	double tii = timeOfIntersection(si,vi,so,vo); // these values may have opposite sign!
//fpln("toi="+toi);
//fpln("int = "+	intersection(so,vo,si,vi));
	if (ISNAN(toi) || toi < 0 || tii < 0) return 0;
	Vect3 so3 = so.linear(vo, toi);
	Vect3 si3 = si.linear(vi, toi);
//fpln("so3="+so3);
//fpln("si3="+si3);
	if (behind(so3.vect2(), si3.vect2(), vi.vect2())) return -1;
	return 1;
}

Vect3 VectFuns::closestPointOnSegment(const Vect3& a, const Vect3& b, const Vect3& so) {
	Vect3 i = closestPoint(a,b,so);
	double d1 = a.distanceH(b);
	double d2 = a.distanceH(i);
	double d3 = b.distanceH(i);
	if (d2 <= d1 && d3 <= d1) {
		return i;
	} else if (d2 < d3) {
		return a;
	} else {
		return b;
	}
}

void Quat::xform(const Vect3 &v, Vect3 &xv) const
{
  Vect3 *u, uv, uuv;


  u = (Vect3 *) &x_;
  uv.cross(*u, v);
  uuv.cross(*u, uv);
  uv.scale(2.0 * s_);
  uuv.scale(2.0);
  xv.add(v, uv);
  xv.add(uuv);
}

static double dpc(const Vect3& m, const Vect3 *pts, const Triangle& cell, Vect3& alphas, int nb, int* idx, bool& inside)
{
    if(nb == 1) {
        alphas(idx[0]) = 1.0;
        return (m - pts[cell[idx[0]]]).norm();
    }
    // Solves H=sum(alpha_i A_i), sum(alpha_i)=1, et HM.(A_i-A_0)=0
    Vect3 A0Ai[3]; // A_i-A_0
    for(int i=1; i<nb; i++) {
        A0Ai[i] = pts[cell[idx[i]]] - pts[cell[idx[0]]];
    }
    Vect3 A0M = m - pts[cell[idx[0]]]; // M-A_0
    if(nb == 2) {
        alphas(idx[1]) = (A0M * A0Ai[1]) / (A0Ai[1] * A0Ai[1]);
        alphas(idx[0]) = 1.0 - alphas(idx[1]);
    } else if (nb==3) {
        // direct inversion (2x2 linear system)
        double a00 = A0Ai[1] * A0Ai[1];
        double a10 = A0Ai[1] * A0Ai[2];
        double a11 = A0Ai[2] * A0Ai[2];
        double b0 = A0M * A0Ai[1];
        double b1 = A0M * A0Ai[2];
        double d = a00 * a11 - a10 * a10;
        assert(d != 0);
        alphas(idx[1]) = (b0 * a11 - b1 * a10) / d;
        alphas(idx[2]) = (a00 * b1 - a10 * b0) / d;
        alphas(idx[0]) = 1.0 - alphas(idx[1]) - alphas(idx[2]);
    } else {
        // 3 unknowns or more -> solve system
        //  Ax=b with: A(i, j)=A0Ai.AjA0, x=(alpha_1, alpha_2, ...), b=A0M.A0Ai
        cerr << "Error : dim>=4 in danielsson !" << endl;
        exit(0);
    }
    // If alpha_i<0 -> brought to 0 and recursion
    // NB: also takes care of alpha > 1 because if alpha_i>1 then alpha_j<0 for at least one j
    for (int i=0; i<nb; i++) {
        if (alphas(idx[i])<0) {
            inside = false;
            alphas(idx[i]) = 0;
            swap(idx[i], idx[nb-1]);
            return dpc(m, pts, cell, alphas, nb-1, idx, inside);
        }
    }
    // Sinon: distance HM
    Vect3 MH = -A0M;
    for (int i=1; i<nb; i++) {
        MH = MH + alphas(idx[i]) * A0Ai[i];
    }
    return MH.norm();
}

  /* Solve the following equation on vz:
   *   sz+t*vz = eps*H,
   *
   * where t = Theta_D(s,v,epsp).
   * eps determines the bottom, i.e.,-1, or top, i.e., 1, circle.
   */
  Vertical Vertical::vs_circle(const Vect3& s, const Vect3& vo, const Vect3& vi,
		     const int eps, const double D, const double H) {
    
    Vect2 s2 = s.vect2();
    Vect2 vo2 = vo.vect2();
    Vect2 vi2 = vi.vect2();
    Vect2 v2 = vo2-vi2;
    
    if (vo2.almostEquals(vi2) && eps == sign(s.z))
      return Vertical(vi.z);
    else if (Horizontal::Delta(s2,v2,D) > 0)
      return vs_only(s,v2,Horizontal::Theta_D(s2,v2,larcfm::Exit,D),eps,D,H).add_this(vi.z);
    return NoVerticalSolution;
  }

// TODO: Test!
// REALLY TODO: TEST. There definitely is a bug in here.
void CollisionSphere::genContactsB(const CollisionBox& b, std::vector<std::shared_ptr<IContact>>& o) const
{
	// Get the sphere position in local coordinates of the box.

	Vect3 transformedSpherePosition = b.getTransformMatrix().getInverse() * this->getPos();

	// If the sphere is further away than the magnitude of the furthest distance
	//  a corner can be, early out:
	if (transformedSpherePosition.squareMagnitude() > b.getSize().squareMagnitude())
		return;

	// Otherwise, find the closest point on the cube.
	//  For any axis, it will be the cube half length on that side if
	//  the sphere's position is further than the half length. If not, it'll
	//  just be the component on that side.
	Vect3 closestBoxPoint = transformedSpherePosition;
	closestBoxPoint._x = CLAMP(closestBoxPoint._x, -b.getSize()._x, b.getSize()._x);
	closestBoxPoint._y = CLAMP(closestBoxPoint._y, -b.getSize()._y, b.getSize()._y);
	closestBoxPoint._z = CLAMP(closestBoxPoint._z, -b.getSize()._z, b.getSize()._z);

	// Now we have the closest point on the box.
	//  If it's closer than the radius, we have a contact
	if ((closestBoxPoint - transformedSpherePosition).squareMagnitude() < this->getRadius() * this->getRadius()
		&& ((closestBoxPoint - transformedSpherePosition).squareMagnitude() > 0.f))
	{
		// Box contact data: Contact is under the surface of the sphere, pointing directly out.
		Vect3Normal d = (closestBoxPoint - transformedSpherePosition);
		Vect3 collisionPoint_l = transformedSpherePosition + d * _radius;
		Vect3 penetration_l = closestBoxPoint - collisionPoint_l;
		Vect3 collisionPoint_w = b.getTransformMatrix() * collisionPoint_l;
		Vect3 penetration_w = b.getTransformMatrix().transformDirn(penetration_l);
		/*Vect3 collisionPoint_l = (Vect3Normal(closestBoxPoint - this->GetPos()) * this->GetRadius()) + this->GetPos();
		Vect3 collisionPoint_w = b.GetTransformMatrix() * (closestBoxPoint - transformedSpherePosition);
		Vect3 penetration_w = (Vect3Normal(collisionPoint_w - this->GetPos()) * this->GetRadius()) - collisionPoint_w;*/
		o.push_back(this->summonDemons(
			collisionPoint_w,
			penetration_w,
			penetration_w.magnitude(),
			b.getAttachedObjectPtr(), _attachedObject));

		// Sphere contact data: Exact opposite of the box contact.
		penetration_w *= -1.f;
		o.push_back(this->summonDemons(
			collisionPoint_w,
			penetration_w,
			penetration_w.magnitude(),
			this->getAttachedObjectPtr(), b.getAttachedObjectPtr()));
	}
}

std::pair<Vect3,double> VectFuns::intersectionAvgZ(const Vect3& so1, const Vect3& so2, double dto, const Vect3& si1, const Vect3& si2) {
	Velocity vo3 = Velocity::genVel(so1, so2, dto);
	Velocity vi3 = Velocity::genVel(si1, si2, dto);      // its ok to use any time here,  all times are relative to so
	std::pair<Vect3,double> iP = intersection(so1,vo3,si1,vi3);
	Vect3 interSec = iP.first;
			double do1 = distanceH(so1,interSec);
			double do2 = distanceH(so2,interSec);
			double alt_o = so1.z;
			if (do2 < do1) alt_o = so2.z;
			double di1 = distanceH(si1,interSec);
			double di2 = distanceH(si2,interSec);
			double alt_i = si1.z;
			if (di2 < di1) alt_i = si2.z;
			double nZ = (alt_o + alt_i)/2.0;
	        return std::pair<Vect3,double>(interSec.mkZ(nZ),iP.second);
}

void HOATDispatcher::sendExtendedResponse( 
	CigiOutgoingMsg *outgoing, 
	HOTRequest *request, 
	HOTResponse *response, int numResponses )
{
	CigiHatHotXRespV3_2 packet;
	
	packet.SetHatHotID( request->id );
	packet.SetValid( true );
	packet.SetHostFrame( request->hostFrameNumber & 0x0f );
	
	Vect3 delta = response->hitLocation - request->locationMSL;
	//FIXME - negative heights must be handled!!!
	double hot = delta.mag();
	packet.SetHot( hot );
	
	delta = request->location - response->hitLocation;
	//FIXME - negative heights must be handled!!!
	double hat = delta.mag();
	packet.SetHat( hat );
	
	if( terrainMaterialOverride )
		packet.SetMaterial( terrainMaterialOverrideCode );
	else
		packet.SetMaterial( response->materialCode );
	
	// fixme - does this work w/ geocentric?
	double heading = 180.0/M_PI * normalize_angle( atan2(response->normal[1], response->normal[0]) );
	double pitch   = 180.0/M_PI * normalize_angle( atan2(response->normal[1], response->normal[2]) );

	// Convert database heading/pitch into geodetic
	CoordinateSet lru_pos;
	lru_pos.LatX  = response->hitLocation[0];
	lru_pos.LonY  = response->hitLocation[1];
	lru_pos.AltZ  = response->hitLocation[2];
	lru_pos.Yaw   = heading;
	lru_pos.Pitch = pitch;
	lru_pos.Roll  = 0.0;

	CoordinateSet gd_pos;
	coordinateConverter->performReverseConversion( lru_pos, gd_pos );

	packet.SetNormAz( normalize_angle( gd_pos.Yaw ) );
	packet.SetNormEl( clamp_pitch( gd_pos.Pitch ) );
	
	*outgoing << packet;
}

 static Vect3 equator_map_inv(const Vect3& ref, const Vect3& p) {
   Vect3 xmult = ref.Hat();
   Vect3 ymult = vect3_orthog_toy(ref).Hat();
   Vect3 zmult = ref.cross(vect3_orthog_toy(ref)).Hat();
   Vect3 xmultInv = Vect3(xmult.x, ymult.x, zmult.x);
   Vect3 ymultInv = Vect3(xmult.y, ymult.y, zmult.y);
   Vect3 zmultInv = Vect3(xmult.z, ymult.z, zmult.z);
   return  Vect3(xmultInv.dot(p), ymultInv.dot(p), zmultInv.dot(p));
 }