//██████████████████████████████████████████████████████████████████████████████████
// 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;
}
void CBillboardGroupSceneNode::applyVertexShadows( const core::vector3df& lightDir, f32 intensity, f32 ambient )
{
for ( s32 i=0; i<Billboards.size(); i++ )
{
core::vector3df normal = Billboards[i].Position;
normal.normalize();
f32 light = -lightDir.dotProduct(normal)*intensity + ambient;
if ( light < 0 )
light = 0;
if ( light > 1 )
light = 1;
video::SColor color;
color.setRed( (u8)(Billboards[i].Color.getRed() * light) );
color.setGreen( (u8)(Billboards[i].Color.getGreen() * light) );
color.setBlue( (u8)(Billboards[i].Color.getBlue() * light) );
color.setAlpha( Billboards[i].Color.getAlpha() );
for ( s32 j=0; j<4; j++ )
{
MeshBuffer.Vertices[i*4+j].Color = color;
}
}
}
/*------------------------------------------------------------------------------
|
| 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
}
/* -------- Determine IF the orbit is possible at all ---------- */
if (fabs(VarA) > Small)
{
Loops = 0;
YNegKtr = 1; // y neg ktr
while (1 == 1)
{
if (fabs(C2New) > Small)
Y = Ro.getLength() + R.getLength() - (VarA * (1.0 - PsiOld * C3New) / sqrt(C2New));
else
Y = Ro.getLength() + R.getLength();
/* ------- Check for negative values of y ------- */
if ((VarA > 0.0) && (Y < 0.0))
{
YNegKtr = 1;
while (1 == 1)
{
PsiNew = 0.8 * (1.0 / C3New) *
(1.0 - (Ro.getLength() + R.getLength()) * sqrt(C2New) / VarA);
/* ------ Find C2 and C3 functions ------ */
FindC2C3(PsiNew, C2New, C3New);
PsiOld = PsiNew;
Lower = PsiOld;
if (fabs(C2New) > Small)
Y = Ro.getLength() + R.getLength() -
(VarA * (1.0 - PsiOld * C3New) / sqrt(C2New));
else
Y = Ro.getLength() + R.getLength();
/*
if (Show == 'Y')
if (FileOut != NULL)
fprintf(FileOut, "%3d %10.5f %10.5f %10.5f %7.3f %9.5f %9.5f\n",
Loops, PsiOld, Y, XOld, dtNew, VarA, Upper, Lower);
*/
YNegKtr++;
if ((Y >= 0.0) || (YNegKtr >= 10))
break;
}
}
if (YNegKtr < 10)
{
if (fabs(C2New) > Small)
XOld = sqrt(Y / C2New);
else
XOld = 0.0;
XOldCubed = XOld * XOld * XOld;
dtNew = XOldCubed * C3New + VarA * sqrt(Y);
/* ---- Readjust upper and lower bounds ---- */
if (dtNew < DtTU)
Lower = PsiOld;
if (dtNew > DtTU)
Upper = PsiOld;
PsiNew = (Upper + Lower) * 0.5;
/*
if (Show == 'Y')
if (FileOut != NULL)
fprintf(FileOut, "%3d %10.5f %10.5f %10.5f %7.3f %9.5f %9.5f\n",
Loops, PsiOld, Y, XOld, dtNew, VarA, Upper, Lower);
*/
/* -------------- Find c2 and c3 functions ---------- */
FindC2C3(PsiNew, C2New, C3New);
PsiOld = PsiNew;
Loops++;
/* ---- Make sure the first guess isn't too close --- */
if ((fabs(dtNew - DtTU) < Small) && (Loops == 1))
dtNew = DtTU - 1.0;
}
if ((fabs(dtNew - DtTU) < Small) || (Loops > NumIter) || (YNegKtr > 10))
break;
}
if ((Loops >= NumIter) || (YNegKtr >= 10))
{
strcpy(Error, "GNotConv");
if (YNegKtr >= 10)
strcpy(Error, "Y negative");
}
else
{
/* ---- Use F and G series to find Velocity Vectors ----- */
F = 1.0 - Y / Ro.getLength();
GDot = 1.0 - Y / R.getLength();
G = 1.0 / (VarA * sqrt(Y)); // 1 over G
FDot = sqrt(Y) * (-R.getLength() - Ro.getLength() + Y) / (R.getLength() * Ro.getLength() * VarA);
/*
for (u32 i = 0; i <= 2; i++)
{
Vo[i] = (R[i] - F * Ro[i]) * G;
V[i] = (GDot * R[i] - Ro[i]) * G;
}
*/
Vo.X = (R.X - F * Ro.X) * G;
Vo.Y = (R.Y - F * Ro.Y) * G;
Vo.Z = (R.Z - F * Ro.Z) * G;
V.X = (GDot * R.X - Ro.X) * G;
V.Y = (GDot * R.Y - Ro.Y) * G;
V.Z = (GDot * R.Z - Ro.Z) * G;
}
}
else
strcpy(Error, "impos 180ø");
/*
---- For Fig 6-14 dev with testgau.pas ----
IF Error = 'ok' THEN Write( FileOut,PsiNew:14:7,DtTU*13.44685:14:7 )
ELSE Write( FileOut,' 9999.0 ':14,DtTU*13.44685:14:7 );
*/
}
/*------------------------------------------------------------------------------
|
| 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();
eps = RoR - 1.0;
tan2w = 0.25 * eps * eps / (sqrt(RoR) + RoR *(2.0 + sqrt(RoR)));
rp = sqrt(Ro.getLength()*R.getLength()) * (pow(cos(DNu * 0.25), 2) + tan2w);
if (DNu < core::PI64)
L = (pow(sin(DNu * 0.25), 2) + tan2w ) /
(pow(sin(DNu * 0.25), 2) + tan2w + cos(DNu * 0.5));
else
L = (pow(cos(DNu * 0.25), 2) + tan2w - cos(DNu * 0.5)) /
(pow(cos(DNu * 0.25), 2) + tan2w);
m = DtTU * DtTU / (8.0 * rp * rp * rp);
xn = 0.0; // 0 for par and hyp
chord = sqrt(Ro.getLength() * Ro.getLength() + R.getLength() * R.getLength() -
2.0 * Ro.getLength() * R.getLength() * cos(DNu));
s = (Ro.getLength() + R.getLength() + chord) * 0.5;
Loops = 1;
while (1 == 1)
{
x = xn;
Tempx = See(x);
Denom = 1.0 / ((1.0 + 2.0 * x + L) * (3.0 + x * (1.0 + 4.0 * Tempx)));
h1 = pow(L + x, 2) * ( 1.0 + (1.0 + 3.0 * x) * Tempx) * Denom;
h2 = m * ( 1.0 + (x - L) * Tempx) * Denom;
/* ------------------------ Evaluate CUBIC ------------------ */
b = 0.25 * 27.0 * h2 / pow(1.0 + h1, 3);
u = -0.5 * b / (1.0 + sqrt(1.0 + b));
k2 = k(u);
y = ((1.0 + h1) / 3.0) * (2.0 + sqrt(1.0 + b) /
(1.0 - 2.0 * u * k2 * k2));
xn = sqrt(pow((1.0 - L) * 0.5, 2) + m / (y * y)) - (1.0 + L) * 0.5;
Loops++;
if ((fabs(xn - x) < Small) || (Loops > 30))
break;
}
a = DtTU * DtTU / (16.0 * rp * rp * xn * y * y);
/* -------------------- Find Eccentric anomalies ---------------- */
/* -------------------------- Hyperbolic ------------------------ */
if (a < -Small)
{
arg1 = sqrt(s / (-2.0 * a));
arg2 = sqrt((s - chord) / (-2.0 * a));
/* -------- Evaluate f and g functions -------- */
//Visual Studio misses Hyperbolic Arc
/*
AlpH = 2.0 * asinh(arg1);
BetH = 2.0 * asinh(arg2);
*/
AlpH = 2.0 * log(arg1 + sqrt(arg1 * arg1 + 1.0));
BetH = 2.0 * log(arg2 + sqrt(arg2 * arg2 + 1.0));
DH = AlpH - BetH;
f = 1.0 - (a / Ro.getLength()) * (1.0 - cosh(DH));
GDot = 1.0 - (a / R.getLength()) * (1.0 - cosh(DH));
FDot = -sqrt(-a) * sinh(DH) / (Ro.getLength() * R.getLength());
}
else
{
/* ------------------------- Elliptical --------------------- */
if (a > Small)
{
arg1 = sqrt(s / (2.0 * a));
arg2 = sqrt((s - chord) / (2.0 * a));
Sinv = arg2;
Cosv = sqrt(1.0 - (Ro.getLength()+R.getLength() - chord) / (4.0 * a));
BetE = 2.0 * acos(Cosv);
BetE = 2.0 * asin(Sinv);
if (DNu > core::PI64)
BetE = -BetE;
Cosv = sqrt(1.0 - s / (2.0 * a));
Sinv = arg1;
am = s * 0.5;
ae = core::PI64;
be = 2.0 * asin(sqrt((s - chord) / s));
tm = sqrt(am * am * am) * (ae - (be - sin(be)));
if (DtTU > tm)
AlpE = 2.0 * core::PI64 - 2.0 * asin(Sinv);
else
AlpE = 2.0 * asin(Sinv);
DE = AlpE - BetE;
f = 1.0 - (a / Ro.getLength()) * (1.0 - cos(DE));
GDot = 1.0 - (a / R.getLength()) * (1.0 - cos(DE));
g = DtTU - sqrt(a * a * a) * (DE - sin(DE));
FDot = -sqrt(a) * sin(DE) / (Ro.getLength() * R.getLength());
}
else
{
/* ------------------------- Parabolic -------------------- */
arg1 = 0.0;
arg2 = 0.0;
strcpy(Error, "a = 0 ");
//if (FileOut != NULL)
//fprintf(FileOut, " a parabolic orbit \n");
}
}
/*
for (u32 i = 0; i <= 2; i++)
{
Vo[i] = (R[i] - f * Ro[i])/ g;
V[i] = (GDot * R[i] - Ro[i])/ g;
}
*/
Vo->X = (R.X - f * Ro.X)/ g;
Vo->Y = (R.Y - f * Ro.Y)/ g;
Vo->Z = (R.Z - f * Ro.Z)/ g;
V->X = (GDot * R.X - Ro.X)/ g;
V->Y = (GDot * R.Y - Ro.Y)/ g;
V->Z = (GDot * R.Z - Ro.Z)/ g;
/*
if (strcmp(Error, "ok") == 0)
Testamt = f * GDot - FDot * g;
else
Testamt = 2.0;
*/
//if (FileOut != NULL)
//fprintf(FileOut, "%8.5f %3d\n", Testamt, Loops);
//BigT = sqrt(8.0 / (s * s * s)) * DtTU;
}
/** 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);
f64 ftemp = evaluate(ahigh);
if ((fm * ftemp) >= 0.0)
{
printf(" initial guesses do not bound \n");
return;
}
//ZeroFinder regfalsi = new ZeroFinder(this, 10000, 1.0E-6, 1.0E-15);
f64 sma = regulaFalsi(amin, ahigh);
f64 alpha = getalpha(sma);
f64 beta = getbeta(sma);
f64 de = alpha - beta;
f64 f = 1.0 - (sma / magr0) * (1.0 - cos(de));
f64 g = dt - sqrt(sma * sma * sma / mu) * (de - sin(de));
core::vector3df newv0;
core::vector3df newvf;
newv0.X = (rf.X - f * r0.X) / g;
newv0.Y = (rf.Y - f * r0.Y) / g;
newv0.Z = (rf.Z - f * r0.Z) / g;
//if it wont work for you, take away the -1.0 multiplications
//I guess my coordinate system is little screwed...
*deltaV0 = (newv0 - (v0*-1.0))*-1.0;
*totalV0 = (newv0*-1.0);
if(debug_print)
printf("deltav-0 X=%.9f, Y=%.9f, Z=%.9f\n",deltaV0->X,deltaV0->Y,deltaV0->Z);
f64 dv0 = deltaV0->getLength();
f64 fdot = -1.0 * (sqrt(mu * sma) / (magr0 * magrf)) * sin(de);
f64 gdot = 1.0 - (sma / magrf) * (1.0 - cos(de));
newvf.X = fdot * r0.X + gdot * newv0.X;
newvf.Y = fdot * r0.Y + gdot * newv0.Y;
newvf.Z = fdot * r0.Z + gdot * newv0.Z;
//Same here:
//if it wont work for you, take away the -1.0 multiplications
//I guess my coordinate system is little screwed...
*deltaV1 = ((vf*-1.0) - newvf)*-1.0;
*totalV1 = (newvf*-1.0);
if(debug_print)
printf("deltav-f X=%.9f, Y=%.9f, Z=%.9f\n",deltaV1->X,deltaV1->Y,deltaV1->Z);
f64 dvf = deltaV1->getLength();
f64 totaldv = dv0 + dvf;
if(debug_print)
printf("\n\nInitial DeltaV dv0 = %.9f\nFinal DeltaV dvf = %.9f\nTotal DeltaV dv = %.9f\nSemi Major Axis = %.9f\n\n",dv0,dvf,totaldv,sma);
/*debug
printf("************************************************\n");
printf("alpha = %.9f\n",alpha);
printf("beta = %.9f\n",beta);
printf("de = %.9f\n",de);
printf("f = %.9f\n",f);
printf("g = %.9f\n",g);
printf("mu = %.9E\n",mu);
f64 firstPartOfG = sqrt(sma * sma * sma / mu);
f64 secondPartOfG = (de - sin(de));
printf("firstPart = %.9f\n",firstPartOfG);
printf("secondPart = %.9f\n",secondPartOfG);
f64 firstTimesSecond = firstPartOfG * secondPartOfG;
f64 timeMinusfirstTimesSecond = dt - test;
printf("firstTimesSecond = %.9E\n",firstTimesSecond);
printf("timeMinusfirstTimesSecond = %.9f\n",timeMinusfirstTimesSecond);
printf("start X=%.9f, Y=%.9f, Z=%.9f\n",r0.X,r0.Y,r0.Z);
printf("desti X=%.9f, Y=%.9f, Z=%.9f\n\n",rf.X,rf.Y,rf.Z);
printf("start Length: %.9Ef\n",r0.getLength());
printf("desti Length: %.9Ef\n\n",rf.getLength());
*/
}
