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