C++ vector3df::getLength方法代码示例

//██████████████████████████████████████████████████████████████████████████████████
// reflection with normal  
core::vector3df getReflected( core::vector3df vector, core::vector3df normal ) { 
	f32 length = (f32)vector.getLength();
	vector.normalize();
	normal.normalize(); 
	
	return (vector - normal * 2.0f * (vector.dotProduct( normal))) * length; 
}

/*------------------------------------------------------------------------------
|
|                           PROCEDURE LAMBERTUNIV
|
|  This PROCEDURE solves the Lambert problem for orbit determination and returns
|    the velocity vectors at each of two given position vectors.  The solution
|    uses Universal Variables for calculation and a bissection technique for
|    updating psi.
|
|  Algorithm     : Setting the initial bounds:
|                  Using -8Pi and 4Pi2 will allow single rev solutions
|                  Using -4Pi2 and 8Pi2 will allow multi-rev solutions
|                  The farther apart the initial guess, the more iterations
|                    because of the iteration
|                  Inner loop is for special cases. Must be sure to exit both!
|
|  Author        : David Vallado                  303-344-6037    1 Mar 2001
|
|  Inputs          Description                    Range / Units
|    R1          - IJK Position vector 1          ER
|    R2          - IJK Position vector 2          ER
|    DM          - direction of motion            'L','S'
|    DtTU        - Time between R1 and R2         TU
|
|  OutPuts       :
|    V1          - IJK Velocity vector            ER / TU
|    V2          - IJK Velocity vector            ER / TU
|    Error       - Error flag                     'ok', ...
|
|  Locals        :
|    VarA        - Variable of the iteration,
|                  NOT the semi or axis!
|    Y           - Area between position vectors
|    Upper       - Upper bound for Z
|    Lower       - Lower bound for Z
|    CosDeltaNu  - Cosine of true anomaly change  rad
|    F           - f expression
|    G           - g expression
|    GDot        - g DOT expression
|    XOld        - Old Universal Variable X
|    XOldCubed   - XOld cubed
|    ZOld        - Old value of z
|    ZNew        - New value of z
|    C2New       - C2(z) FUNCTION
|    C3New       - C3(z) FUNCTION
|    TimeNew     - New time                       TU
|    Small       - Tolerance for roundoff errors
|    i, j        - index
|
|  Coupling
|    MAG         - Magnitude of a vector
|    DOT         - DOT product of two vectors
|    FINDC2C3    - Find C2 and C3 functions
|
|  References    :
|    Vallado       2001, 459-464, Alg 55, Ex 7-5
|
-----------------------------------------------------------------------------*/
void LambertUniv
(
 core::vector3df Ro, core::vector3df R, char Dm, char OverRev, f64 DtTU,
 core::vector3df& Vo, core::vector3df& V, char* Error)
{
	const f64 TwoPi   = 2.0 * core::PI64;
	const f64 Small   = 0.0000001;
	const u32 NumIter = 40;

	u32 Loops, i, YNegKtr;
	f64 VarA, Y, Upper, Lower, CosDeltaNu, F, G, GDot, XOld, XOldCubed, FDot,
		PsiOld, PsiNew, C2New, C3New, dtNew;

	/* --------------------  Initialize values   -------------------- */
	strcpy(Error, "ok");
	PsiNew = 0.0;
	Vo = core::vector3df(0,0,0);
	V = core::vector3df(0,0,0);

	CosDeltaNu = Ro.dotProduct(R) / (Ro.getLength() * R.getLength());
	if (Dm == 'L')
		VarA = -sqrt(Ro.getLength() * R.getLength() * (1.0 + CosDeltaNu));
	else
		VarA =  sqrt(Ro.getLength() * R.getLength() * (1.0 + CosDeltaNu));

	/* ----------------  Form Initial guesses   --------------------- */
	PsiOld = 0.0;
	PsiNew = 0.0;
	XOld   = 0.0;
	C2New  = 0.5;
	C3New  = 1.0 / 6.0;

	/* -------- Set up initial bounds for the bissection ------------ */
	if (OverRev == 'N')
	{
		Upper = TwoPi * TwoPi;
		Lower = -4.0 * TwoPi;
	}
	else
	{
		Upper = -0.001 + 4.0 * TwoPi * TwoPi; // at 4, not alw work, 2.0, makes
		Lower =  0.001+TwoPi*TwoPi;           // orbit bigger, how about 2 revs??xx
//.........这里部分代码省略.........

/*------------------------------------------------------------------------------
|
|                           PROCEDURE LAMBERBATTIN
|
|  This PROCEDURE solves Lambert's problem using Battins method. The method is
|    developed in Battin (1987).
|
|  Author        : David Vallado                  303-344-6037    1 Mar 2001
|
|  Inputs          Description                    Range / Units
|    Ro          - IJK Position vector 1          ER
|    R           - IJK Position vector 2          ER
|    DM          - direction of motion            'L','S'
|    DtTU        - Time between R1 and R2         TU
|
|  OutPuts       :
|    Vo          - IJK Velocity vector            ER / TU
|    V           - IJK Velocity vector            ER / TU
|    Error       - Error flag                     'ok',...
|
|  Locals        :
|    i           - Index
|    Loops       -
|    u           -
|    b           -
|    Sinv        -
|    Cosv        -
|    rp          -
|    x           -
|    xn          -
|    y           -
|    l           -
|    m           -
|    CosDeltaNu  -
|    SinDeltaNu  -
|    DNu         -
|    a           -
|    Tan2w       -
|    RoR         -
|    h1          -
|    h2          -
|    Tempx       -
|    eps         -
|    denom       -
|    chord       -
|    k2          -
|    s           -
|    f           -
|    g           -
|    fDot        -
|    am          -
|    ae          -
|    be          -
|    tm          -
|    gDot        -
|    arg1        -
|    arg2        -
|    tc          -
|    AlpE        -
|    BetE        -
|    AlpH        -
|    BetH        -
|    DE          -
|    DH          -
|
|  Coupling      :
|    ARCSIN      - Arc sine FUNCTION
|    ARCCOS      - Arc cosine FUNCTION
|    MAG         - Magnitude of a vector
|    ARCSINH     - Inverse hyperbolic sine
|    ARCCOSH     - Inverse hyperbolic cosine
|    SINH        - Hyperbolic sine
|    POWER       - Raise a base to a POWER
|    ATAN2       - Arc tangent FUNCTION that resolves quadrants
|
|  References    :
|    Vallado       2001, 464-467, Ex 7-5
|
-----------------------------------------------------------------------------*/
void LambertBattin
(
 core::vector3df Ro, core::vector3df R, char dm, char OverRev, f64 DtTU,
 core::vector3df* Vo, core::vector3df* V, char* Error
 )
{
	const f64 Small = 0.000001;

	core::vector3df RCrossR;
	s32   i, Loops;
	f64   u, b, Sinv,Cosv, rp, x, xn, y, L, m, CosDeltaNu, SinDeltaNu,DNu, a,
		tan2w, RoR, h1, h2, Tempx, eps, Denom, chord, k2, s, f, g, FDot, am,
		ae, be, tm, GDot, arg1, arg2, tc, AlpE, BetE, AlpH, BetH, DE, DH;

	strcpy(Error, "ok");
	CosDeltaNu = Ro.dotProduct(R) / (Ro.getLength() * R.getLength());
	RCrossR    = Ro.crossProduct(R);
	SinDeltaNu = RCrossR.getLength() / (Ro.getLength() * R.getLength());
	DNu        = atan2(SinDeltaNu, CosDeltaNu);

	RoR   = R.getLength() / Ro.getLength();
//.........这里部分代码省略.........

/** Computes the delta-v's required to go from r0,v0 to rf,vf.
* @return Total delta-v (magnitude) required.
* @param dt Time of flight
* @param r0 Initial position vector.
* @param v0 Initial velocity vector.
* @param rf Desired final position vector.
* @param vf Desired final velocity vector.
* //pointers to return results
* @param deltaV0 computed Initial delta-V.
* @param deltaV1 computed Final delta-V.
* @param totalV0 computed Initial total-V.
* @param totalV0 computed Final total-V.
*/
void LambertCompute(f64 GM, 
                    core::vector3df r0,
                    core::vector3df v0,
                    core::vector3df rf,
                    core::vector3df vf, 
                    f64 dt,
                    core::vector3df* deltaV0,
                    core::vector3df* deltaV1,
                    core::vector3df* totalV0,
                    core::vector3df* totalV1)
{
    s = 0.0;
    c = 0.0;
    aflag = false;
    bflag = false;
    debug_print=true;

    f64 tp = 0.0;
    f64 magr0 = r0.getLength();
    f64 magrf = rf.getLength();
    //GM is expected in km^3/s^2
    mu = GM / 1000000000.0;
    //time of flight expected in seconds
    dt *= 86400;
    tof = dt;

    core::vector3df dr = r0 - (rf);
    c = dr.getLength();
    s = (magr0 + magrf + c) / 2.0;
    f64 amin = s / 2.0;
    
    if(debug_print)
        printf("amin = %.9E\n", amin);
    
    f64 dtheta = acos(r0.dotProduct(rf) / (magr0 * magrf));

    //dtheta = 2.0 * core::PI64 - dtheta;

    if(debug_print)
        printf("dtheta = %.9E\n", dtheta);
    
    if (dtheta < core::PI64)
    {
        tp = sqrt(2.0 / (mu)) * (pow(s, 1.5) - pow(s - c, 1.5)) / 3.0;
    }
    if (dtheta > core::PI64)
    {
        tp = sqrt(2.0 / (mu)) * (pow(s, 1.5) + pow(s - c, 1.5)) / 3.0;
        bflag = true;
    }

    if(debug_print)
        printf("tp = %.9f\n", tp);

    f64 betam = getbeta(amin);
    f64 tm = getdt(amin, core::PI64, betam);

    if(debug_print)
        printf("tm = %.9E\n", tm);

    if (dtheta == core::PI64)
    {
        printf(" dtheta = 180.0. Do a Hohmann\n");
        return;
    }

    f64 ahigh = 1000.0 * amin;
    f64 npts = 3000.0;
    
    if(debug_print)
        printf("dt = %.9E seconds\n", dt);

    if(debug_print)
        printf("************************************************\n");

    if (dt < tp)
    {
        printf(" No elliptical path possible \n");
        return;
    }

    if (dt > tm)
    {
        aflag = true;
    }

    f64 fm = evaluate(amin);
//.........这里部分代码省略.........

core::vector3df collideWithWorld(s32 recursionDepth, SCollisionData &colData, core::vector3df pos, core::vector3df vel)
{
	f32 veryCloseDistance = colData.slidingSpeed;
	
	if (recursionDepth > 5)
		return pos;

	colData.velocity = vel;
	colData.normalizedVelocity = vel;
	colData.normalizedVelocity.normalize();
	colData.basePoint = pos;
	colData.foundCollision = false;
	colData.nearestDistance = FLT_MAX;

	double uhel_cos = 90 - acos(colData.normalizedVelocity.dotProduct(vector3df(0, -1, 0).normalize())) * 180.0 / PI;

	if (recursionDepth > 0 && vel.getLength() > 0 && vel.Y < 0)
	{
		if (abs(uhel_cos) < 50)
			return pos;
	}


	//------------------ collide with world

	// get all triangles with which we might collide
	core::aabbox3d<f32> box(colData.R3Position);
	box.addInternalPoint(colData.R3Position + colData.R3Velocity);
	box.MinEdge -= colData.eRadius;
	box.MaxEdge += colData.eRadius;

	s32 totalTriangleCnt = colData.selector->getTriangleCount();
	Triangles.set_used(totalTriangleCnt);

	core::matrix4 scaleMatrix;
	scaleMatrix.setScale(
			core::vector3df(1.0f / colData.eRadius.X,
					1.0f / colData.eRadius.Y,
					1.0f / colData.eRadius.Z));

	s32 triangleCnt = 0;


	colData.selector->getTriangles(Triangles.pointer(), totalTriangleCnt, triangleCnt, box, &scaleMatrix);


	for (s32 i=0; i<triangleCnt; ++i)
		if(testTriangleIntersection(&colData, Triangles[i]))
			colData.triangleIndex = i;


	//---------------- end collide with world

	if (!colData.foundCollision)
		return pos + vel;

	// original destination point
	const core::vector3df destinationPoint = pos + vel;
	core::vector3df newBasePoint = pos;

	if (colData.nearestDistance >= veryCloseDistance)
	{
		core::vector3df v = vel;
		v.setLength( colData.nearestDistance - veryCloseDistance );
		newBasePoint = colData.basePoint + v;

		v.normalize();
		colData.intersectionPoint -= (v * veryCloseDistance);
	}

	// calculate sliding plane

	const core::vector3df slidePlaneOrigin = colData.intersectionPoint;
	const core::vector3df slidePlaneNormal = (newBasePoint - colData.intersectionPoint).normalize();
	core::plane3d<f32> slidingPlane(slidePlaneOrigin, slidePlaneNormal);

	core::vector3df newDestinationPoint =
		destinationPoint -
		(slidePlaneNormal * slidingPlane.getDistanceTo(destinationPoint));

	// generate slide vector

	const core::vector3df newVelocityVector = newDestinationPoint -
		colData.intersectionPoint;

	if (newVelocityVector.getLength() < veryCloseDistance)
		return newBasePoint;

	//printf("Puvodni delka: %f | nova delka: %f\n", colData.velocity.getLength(), newVelocityVector.getLength());

	return collideWithWorld(recursionDepth+1, colData,
		newBasePoint, newVelocityVector);
}