C++ Vect3类代码示例

	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();
}