// Copied from irrlicht
void calculateTangents(
core::vector3df& normal,
core::vector3df& tangent,
core::vector3df& binormal,
const core::vector3df& vt1, const core::vector3df& vt2, const core::vector3df& vt3, // vertices
const core::vector2df& tc1, const core::vector2df& tc2, const core::vector2df& tc3) // texture coords
{
core::vector3df v1 = vt1 - vt2;
core::vector3df v2 = vt3 - vt1;
normal = v2.crossProduct(v1);
normal.normalize();
// binormal
f32 deltaX1 = tc1.X - tc2.X;
f32 deltaX2 = tc3.X - tc1.X;
binormal = (v1 * deltaX2) - (v2 * deltaX1);
binormal.normalize();
// tangent
f32 deltaY1 = tc1.Y - tc2.Y;
f32 deltaY2 = tc3.Y - tc1.Y;
tangent = (v1 * deltaY2) - (v2 * deltaY1);
tangent.normalize();
// adjust
core::vector3df txb = tangent.crossProduct(binormal);
if (txb.dotProduct(normal) < 0.0f)
{
tangent *= -1.0f;
binormal *= -1.0f;
}
}
void CFreeCamera::Update(unsigned uDeltaTime)
{
scene::ICameraSceneNode *pCamera = m_pSceneNode;
// Check if dt is not 0 and we are animating active camera
if(!uDeltaTime || pCamera->getSceneManager()->getActiveCamera() != pCamera)
return;
// Calculate some useful vectors
core::vector3df vPos = pCamera->getPosition();
const core::vector3df vForward = (pCamera->getTarget() - pCamera->getPosition()).normalize();
const core::vector3df &vUp = pCamera->getUpVector();
const core::vector3df vRight = vForward.crossProduct(vUp);
float fSpeed = uDeltaTime / 100.0f;
// Shift makes camera faster
if(m_Controls[CTRL_FASTER])
fSpeed *= 5.0f;
// Calculate direction of movement
core::vector3df vMovement(0.0f, 0.0f, 0.0f);
if(m_Controls[CTRL_FORWARD])
vMovement += vForward * fSpeed;
if(m_Controls[CTRL_BACKWARD])
vMovement -= vForward * fSpeed;
if(m_Controls[CTRL_LEFT])
vMovement += vRight * fSpeed;
if(m_Controls[CTRL_RIGHT])
vMovement -= vRight * fSpeed;
// update camera velocity
float fFactor = min(uDeltaTime / 200.0f, 1.0f);
m_vVelocity = vMovement * fFactor + m_vVelocity * (1.0f - fFactor);
vPos += m_vVelocity;
pCamera->setPosition(vPos);
// Update pitch and yaw
if(m_vCursorPos != m_vCursorCenter)
{
core::vector2df vOffset = m_vCursorPos - m_vCursorCenter;
m_fYaw = fmod(m_fYaw + vOffset.X, 2.0f * M_PI);
const float fPitchMax = M_PI_2 - 0.1f;
m_fPitch -= vOffset.Y;
if(m_fPitch > fPitchMax)
m_fPitch = fPitchMax;
else if(m_fPitch < -fPitchMax)
m_fPitch = -fPitchMax;
m_pCursorCtrl->setPosition(0.5f, 0.5f);
m_vCursorCenter = m_pCursorCtrl->getRelativePosition();
}
// Set camera target
core::vector3df vTarget(sinf(m_fYaw) * cosf(m_fPitch), sinf(m_fPitch), cosf(m_fYaw) * cosf(m_fPitch));
vTarget += vPos;
pCamera->setTarget(vTarget);
}
core::vector3df RotatePositionByDirectionalVector(core::vector3df vPos, core::vector3df vNormal )
{
//OPTIMIZE Isn't there a much faster way to do this?
//calculate rotated z
core::vector3df vFinal = vNormal * vPos.Z;
//calculate rotation x
vFinal = vFinal + (vNormal.crossProduct(core::vector3df(0,1,0)) * vPos.X);
//y will just be up.. yeah, not really right
vFinal.Y += vPos.Y;
return vFinal;
}
/*------------------------------------------------------------------------------
|
| 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;
}