void v0::decay::operator()(vectorn& c) const
{
c.setSize(mnSize);
m_real totalTime=mnSize-1;
m_real currTime;
for(int i=0; i<mnSize; i++)
{
currTime=(m_real)i/totalTime;
m_real t;
switch(mType)
{
case TRANSITION:
t=1.0-(-2.f*CUBIC(currTime)+3.f*SQR(currTime));
break;
case COS:
t=cos(currTime*M_PI*0.5);
break;
case LINEAR:
t=1.0-currTime;
break;
}
c[i]=t*mStart;
}
}
void Conversions::ecef2wgs(double ecef_x, double ecef_y, double ecef_z, double* wgs_lat, double* wgs_lon, double* altitude)
{
// ECEF 2 Wold Grid System 1984
double Xe = ecef_x;
double Ye = ecef_y;
double Ze = ecef_z;
// WGS 84 earth shape model
double f = 1/298.257223563; // Ellipsoid's flatness
double e = sqrt(f*(2-f)); // Ellipsoid's Eccentricity
double rp = 6357752.3142; // [m] ; semiminor axis: radius polar // b=6356752:31424518
double re = 6378137; // [m] ; semimajor axis: radius equator // a=6378137
// Earth position from ECEF to NorthEastDown (Local)
double lon = atan2(Ye,Xe);
double p = sqrt( SQ(Xe)+SQ(Ye) );
double h[2] = {0, 0};
double lamdum[2] = {0, 0};
double N[2] = {re, re};
double alt=0, E, F, G, c, s, P, Q, r0, V, z0, e_a, lat;
for (int i=0;i<100;i++)
{
lamdum[1] = atan( (Ze + SQ(e) * N[0] * sin( lamdum[0] ) ) / p);
N[1] = re / sqrt(1 - SQ(e) * SQ( sin(lamdum[0]) ) );
h[1] = p / cos( lamdum[0] ) - N[0];
if ( (fabs(h[1])-h[0]) == 0 )
{
alt=h[1];
break;
}
h[0]=h[1];
N[0]=N[1];
lamdum[0] = lamdum[1];
}
E = sqrt(re*re-rp*rp);
F = 54 * SQ(rp) * SQ(Ze);
G = SQ(p) + (1 - SQ(e)) * SQ(Ze) - SQ(e) * SQ(E);
c = ( SQ(SQ(e)) * F * SQ(p) ) / CUBIC(G);
s = pow(( 1 + c + sqrt( SQ(c) + 2 * c) ), (1/3));
P = F / (3* SQ(s+(1/s)+1) * SQ(G));
Q = sqrt(1+2* SQ(SQ(e)) * P);
r0= -(P * SQ(e) * p ) / (1+Q) + sqrt(0.5 * SQ(re) * (1+(1/Q))-(P*(1-SQ(e))*SQ(Ze))/(Q*(1+Q))-0.5 * P * SQ(p));
// U = sqrt( SQ(p- SQ(e) * r0) + SQ(Ze) );
V = sqrt( SQ(p- SQ(e) * r0) + (1-SQ(e)) * SQ(Ze) );
z0= ( SQ(rp) * Ze) / (re * V);
e_a = re * e / rp;
lat = atan( ( Ze + SQ(e_a) * z0) / p );
*wgs_lat = lat * 180.0 / 3.1415926535897932384626433832795;
*wgs_lon = lon * 180.0 / 3.1415926535897932384626433832795;
*altitude = alt;
}
void v0::transition::calc(vectorn& c) const
{
c.setSize(mnSize);
m_real totalTime=mnSize-1;
m_real currTime;
for(int i=0; i<mnSize; i++)
{
currTime=(float)i/totalTime;
float t=-2.f*CUBIC(currTime)+3.f*SQR(currTime);
c[i]=sop::interpolate(t, mStart, mEnd);
}
}
void vector3N::transition(const vector3& a, const vector3& b, int duration)
{
setSize(duration);
float totalTime=duration+1;
float currTime;
for(int i=0; i<duration; i++)
{
currTime=(float)(i+1)/totalTime;
float t=-2.f*CUBIC(currTime)+3.f*SQR(currTime);
row(i).lerp(a, b, t);
}
}
void quater::bezier(const quater& q0, const quater& q1, const quater& q2, const quater& q3, double t)
{
// kim myung soo siggraph 96 paper
// cumulative bases
double b1, b2, b3;
b1=1.f-CUBIC(1.f-t);
b2=3.f*SQR(t)-2.f*CUBIC(t);
b3=CUBIC(t);
// This paper follows directX convention of the definition of w (angularVelocity)
// Furthermore, its length is twice the common definition. (This means that this paper is actually wrong.)
// I modified to support common definition (OpenGL convention)
// ->
// q(t)=( PI_n^1 exp(2*wi*bi) ) * q0
vector3 w1, w2, w3;
w1.angularVelocity(q0, q1);
w2.angularVelocity(q1, q2);
w3.angularVelocity(q2, q3);
this->mult( (w3*b3).quaternion()*(w2*b2).quaternion()*(w1*b1).quaternion() , q0);
this->normalize();
}
void quaterN::transition(const quater& aa, const quater& b, int duration)
{
quater a(aa);
a.align(b);
setSize(duration);
float totalTime=duration+1;
float currTime;
for(int i=0; i<duration; i++)
{
currTime=(float)(i+1)/totalTime;
float t=-2.f*CUBIC(currTime)+3.f*SQR(currTime);
row(i).slerp(a, b, t);
}
}
vectorn& vectorn::curvature(const matrixn& pos)
{
vectorn& c=*this;
//http://mathworld.wolfram.com/Curvature.html
//k = (| r.^ x r^..|)/(| r^. |^3)
matrixn vel;
matrixn acc;
vel.derivative(pos);
acc.derivative(vel);
c.setSize(pos.rows());
for(int i=0; i<c.size(); i++)
{
c[i]= vel.row(i).toVector3().cross(acc.row(i).toVector3()).length();
c[i]/=CUBIC(vel.row(i).length());
}
return *this;
}
void quaterN::transition0(const quater& aa, const quater& bb, int duration)
{
quater a(aa);
quater b(bb);
quater qid;
qid.identity();
a.align(qid);
b.align(qid);
// kovar paper (prefer identity quaternion (more safe))
setSize(duration);
float totalTime=duration+1;
float currTime;
quater c, d, qi;
qi.identity();
for(int i=0; i<duration; i++)
{
currTime=(float)(i+1)/totalTime;
float t=-2.f*CUBIC(currTime)+3.f*SQR(currTime);
c.slerp(a, qi, t);
d.slerp(qi, b, t);
row(i).slerp(c, d, currTime);
}
}
void Conversions::setOriginEcef(double _x0,double _y0,double _z0)
{
double Xe = _x0;
double Ye = _y0;
double Ze = _z0;
x0_ecef(0,0,_x0);
x0_ecef(1,0,_y0);
x0_ecef(2,0,_z0);
// WGS 84 earth shape model
double f = 1/298.257223563; // Ellipsoid's flatness
double e = sqrt(f*(2-f)); // Ellipsoid's Eccentricity
double rp = 6357752.3142; // [m] ; semiminor axis: radius polar // b=6356752:31424518
double re = 6378137; // [m] ; semimajor axis: radius equator // a=6378137
// Earth position from ECEF to NorthEastDown (Local)
double lon = atan2(Ye,Xe);
double p = sqrt( SQ(Xe)+SQ(Ye) );
double h[2] = {0, 0};
double lamdum[2] = {0, 0};
double N[2] = {re, re};
double alt=0, E, F, G, c, s, P, Q, r0, V, z0, e_a, lat;
for (int i=0;i<100;i++)
{
lamdum[1] = atan( (Ze + SQ(e) * N[0] * sin( lamdum[0] ) ) / p);
N[1] = re / sqrt(1 - SQ(e) * SQ( sin(lamdum[0]) ) );
h[1] = p / cos( lamdum[0] ) - N[0];
if ( (fabs(h[1])-h[0]) == 0 )
{
alt=h[1];
break;
}
h[0]=h[1];
N[0]=N[1];
lamdum[0] = lamdum[1];
}
E = sqrt(re*re-rp*rp);
F = 54 * SQ(rp) * SQ(Ze);
G = SQ(p) + (1 - SQ(e)) * SQ(Ze) - SQ(e) * SQ(E);
c = ( SQ(SQ(e)) * F * SQ(p) ) / CUBIC(G);
s = pow(( 1 + c + sqrt( SQ(c) + 2 * c) ), (1/3));
P = F / (3* SQ(s+(1/s)+1) * SQ(G));
Q = sqrt(1+2* SQ(SQ(e)) * P);
r0= -(P * SQ(e) * p ) / (1+Q) + sqrt(0.5 * SQ(re) * (1+(1/Q))-(P*(1-SQ(e))*SQ(Ze))/(Q*(1+Q))-0.5 * P * SQ(p));
// U = sqrt( SQ(p- SQ(e) * r0) + SQ(Ze) );
V = sqrt( SQ(p- SQ(e) * r0) + (1-SQ(e)) * SQ(Ze) );
z0= ( SQ(rp) * Ze) / (re * V);
e_a = re * e / rp;
lat = atan( ( Ze + SQ(e_a) * z0) / p );
// printf("Conversions::setOrigin(): X0[deg] %f N %f E %f h\n",lat*180/3.141596,lon*180/3.141596,alt);
// Rotation from ECEF to Local
T_el(0,0,-cos(lon)*sin(lat));
T_el(1,0,-sin(lon));
T_el(2,0,-cos(lon)*cos(lat));
T_el(0,1,-sin(lon)*sin(lat));
T_el(1,1, cos(lon));
T_el(2,1,-sin(lon)*cos(lat));
T_el(0,2, cos(lat));
T_el(1,2, 0);
T_el(2,2,-sin(lat));
this->originset = true;
// std::cout << "T_el = " << T_el << std::endl;
}
