/*
* Given vectors a and b, compute the angle between them in degrees
*/
double Lgm_VectorAngle(Lgm_Vector *a, Lgm_Vector *b) {
double ang;
Lgm_Vector u, v;
u = *a;
v = *b;
Lgm_NormalizeVector(&u);
Lgm_NormalizeVector(&v);
ang = acos(Lgm_DotProduct(&u, &v));
return (ang*DegPerRad);
}
/**
* \brief
* Compute the curl of B and it's parallel and perpendicular components at a given point.
*
* \details
* Computes \f$ \nabla\times B \f$, \f$ (\nabla\times B)_\parallel \f$, and \f$ (\nabla\times B)_\perp \f$.
*
*
* \param[in] u0 Position (in GSM) to use.
* \param[out] CurlB The computed curl of B at position u0.
* \param[out] CurlB_para The computed parallel component of curl of B at position u0.
* \param[out] CurlB_perp The computed perpendicular component of curl of B at position u0.
* \param[in] DerivScheme Derivative scheme to use (can be one of LGM_DERIV_SIX_POINT, LGM_DERIV_FOUR_POINT, or LGM_DERIV_TWO_POINT).
* \param[in] h The delta (in Re) to use for grid spacing in the derivative scheme.
* \param[in,out] m A properly initialized and configured Lgm_MagModelInfo structure.
*
*
* \author Mike Henderson
* \date 2011
*
*/
void Lgm_CurlB2( Lgm_Vector *u0, Lgm_Vector *CurlB, Lgm_Vector *CurlB_para, Lgm_Vector *CurlB_perp, int DerivScheme, double h, Lgm_MagModelInfo *m ) {
double g;
Lgm_Vector Bvec;
Lgm_CurlB( u0, CurlB, DerivScheme, h, m );
m->Bfield( u0, &Bvec, m );
Lgm_NormalizeVector( &Bvec );
// Compute parallel component of CurlB
g = Lgm_DotProduct( &Bvec, CurlB );
*CurlB_para = *CurlB;
Lgm_ScaleVector( CurlB_para, g );
// Compute perpendicular component of CurlB (CurlB_perp = CurlB - CurlB_para)
Lgm_VecSub( CurlB_perp, CurlB, CurlB_para );
return;
}
/**
* \brief
* Compute the gradient of B and it's parallel and perpendicular components at a given point.
*
* \details
* Computes \f$ \nabla B \f$, \f$ (\nabla B)_\parallel \f$, and \f$ (\nabla B)_\perp \f$.
*
*
* \param[in] u0 Position (in GSM) to use.
* \param[out] GradB The computed curl of B at position u0.
* \param[out] GradB_para The computed parallel component of curl of B at position u0.
* \param[out] GradB_perp The computed perpendicular component of curl of B at position u0.
* \param[in] DerivScheme Derivative scheme to use (can be one of LGM_DERIV_SIX_POINT, LGM_DERIV_FOUR_POINT, or LGM_DERIV_TWO_POINT).
* \param[in] h The delta (in Re) to use for grid spacing in the derivative scheme.
* \param[in,out] m A properly initialized and configured Lgm_MagModelInfo structure.
*
*
* \author Mike Henderson
* \date 2011
*
*/
void Lgm_GradB2( Lgm_Vector *u0, Lgm_Vector *GradB, Lgm_Vector *GradB_para, Lgm_Vector *GradB_perp, int DerivScheme, double h, Lgm_MagModelInfo *m ) {
double g;
Lgm_Vector Bvec;
Lgm_GradB( u0, GradB, DerivScheme, h, m );
m->Bfield( u0, &Bvec, m );
Lgm_NormalizeVector( &Bvec );
// Compute parallel component of GradB
g = Lgm_DotProduct( &Bvec, GradB );
*GradB_para = *GradB;
Lgm_ScaleVector( GradB_para, g );
// Compute perpendicular component of GradB (GradB_perp = GradB - GradB_para)
Lgm_VecSub( GradB_perp, GradB, GradB_para );
return;
}
int main(){
Lgm_Vector a, b, z1, z2;
Lgm_SlerpInfo s;
double alpha;
a.x = 1.0; a.y = 1.0; a.z = 1.0;
b.x = 0.0; b.y = -1.0; b.z = 1.0;
Lgm_NormalizeVector( &a );
Lgm_NormalizeVector( &b );
Lgm_InitSlerp( &a, &b, &s );
alpha = 0.1;
Lgm_Slerp( &a, &b, &z1, alpha, &s );
printf("Results of slerping:\n");
Lgm_PrintVector(&a);
Lgm_PrintVector(&b);
Lgm_PrintVector(&z1);
z2.x = (1.0-alpha)*a.x + alpha*b.x;
z2.y = (1.0-alpha)*a.y + alpha*b.y;
z2.z = (1.0-alpha)*a.z + alpha*b.z;
Lgm_NormalizeVector( &z2 );
printf("Results of interpolating components (and re-normalizing):\n");
Lgm_PrintVector(&a);
Lgm_PrintVector(&b);
Lgm_PrintVector(&z2);
Lgm_VecSub(&a, &z1, &z2);
printf("\nThey are different by:\n");
Lgm_PrintVector(&a);
return(0);
}
/*
* This routinem determines the angle and axis of rotation implied
* by a (normalized) quaternion.
*
* Input:
* Q[4] -- four element array containing Qx, Qy, Qz, Qw
*
* Output:
*
* Angle -- angle (in degrees) of rotation.
* u -- unit vector of rotation axis.
*
*
*/
void Lgm_QuatToAxisAngle( double *Q, double *Angle, Lgm_Vector *u ) {
double Aover2, SinAover2, SinAover2_inv;
/*
* Force normalization of Q
*/
Lgm_NormalizeQuat( Q );
if ( (1.0-fabs(Q[3])) < 1e-8 ) {
/*
* Rotation angle is close to zero
* (Its either A/2 = 0 (i.e. A = 0) or
* A/2 = 180 (i.e. A = 360))
* Rotation axis is arbitrary
*/
*Angle = 0.0;
u->x = 0.0;
u->y = 0.0;
u->z = 1.0;
} else {
/*
* Determine Angle/2
*/
Aover2 = acos( Q[3] );
*Angle = 2.0*Aover2*DegPerRad;
/*
* Compute 1.0/sin(Angle/2)
*/
SinAover2 = sqrt( 1.0 - Q[3]*Q[3] );
SinAover2_inv = 1.0/SinAover2;
/*
* Compute components of rotation axis
*/
u->x = Q[0]*SinAover2;
u->y = Q[1]*SinAover2;
u->z = Q[2]*SinAover2;
Lgm_NormalizeVector( u );
}
}
/*
* Take exp() of a purely imaginary quaternion.
*/
void Lgm_QuatExp( double Q[4], double expQ[4] ) {
double Theta, SinTheta, CosTheta;
Lgm_Vector u;
u.x = Q[0];
u.y = Q[1];
u.z = Q[2];
Theta = Lgm_NormalizeVector( &u );
SinTheta = sin( Theta );
CosTheta = cos( Theta );
expQ[0] = SinTheta*u.x;
expQ[1] = SinTheta*u.y;
expQ[2] = SinTheta*u.z;
expQ[3] = CosTheta;
}
/*
* makes the given vector have a magnitude of mag
*/
void Lgm_ForceMagnitude(Lgm_Vector *a, double mag) {
Lgm_NormalizeVector(a);
Lgm_ScaleVector(a, mag);
}
void ComputeZPSTransMatrix( Lgm_Vector *Rgeo, long int Date, double UT, double Agsm_to_ins[3][3], double Ains_to_gsm[3][3], double Qins_to_gsm[4], double Qgsm_to_ins[4] ) {
double Glon, Alpha, A, B, ca, sa, sum;
Lgm_Vector Xgeo, Ygeo, Zgeo, Xgsm, Ygsm, Zgsm;
Lgm_Vector Ngeo, Egeo, Ngsm, Egsm;
Lgm_Vector Xsc_geo,Ysc_geo, Zsc_geo;
Lgm_Vector Xsc_gsm,Ysc_gsm, Zsc_gsm;
double Agsm_to_sc[3][3], Asc_to_gsm[3][3], Asc_to_r3[3][3], Ar3_to_sc[3][3], Asc_to_dom[3][3], Adom_to_sc[3][3];
Lgm_Vector X1, Y1, Z1;
Lgm_Vector X2, Y2, Z2;
Lgm_Vector X3, Y3, Z3;
Lgm_Vector X4, Y4, Z4;
int i, j, k;
Lgm_CTrans c;
Lgm_Vector Xins, Yins, Zins, Xsc, Ysc, Zsc;
Lgm_Vector Xins_new, Yins_new, Zins_new;
double Q1[4], Q2[4], Q3[4], QQ[4], QT1[4];
Glon = atan2( Rgeo->y, Rgeo->x );
/*
* Compute transformation matrix from dome coords to GSM
* Assume spacecraft is located at Rgeo.
*/
/*
* Nadir in GEO coords.
*/
Ngeo.x = -Rgeo->x;
Ngeo.y = -Rgeo->y;
Ngeo.z = -Rgeo->z;
Lgm_NormalizeVector( &Ngeo );
/*
* East in GEO coords. Since, r = {cos(phi), sin(phi)}
* east direction will be dr/dphi which is;
*
* east = {-sin(phi), cos(phi)}
*
* note that phi is just glon in GEO coords.
*
*/
Egeo.x = -1.0*sin(Glon);
Egeo.y = cos(Glon);
Egeo.z = 0.0;
/*
* North in GEO coords.
*/
Zgeo.x = 0.0;
Zgeo.y = 0.0;
Zgeo.z = 1.0;
/*
* Define S/C coords as:
* X: East
* Y: South
* Z: Nadir
*/
Xsc_geo = Egeo;
Ysc_geo.x = -Zgeo.x; Ysc_geo.y = -Zgeo.y; Ysc_geo.z = -Zgeo.z;
Zsc_geo = Ngeo;
/*
* Convert to GSM
*/
Lgm_Set_Coord_Transforms( Date, UT, &c );
Lgm_Convert_Coords( &Xsc_geo, &Xsc_gsm, GEO_TO_GSM, &c );
Lgm_Convert_Coords( &Ysc_geo, &Ysc_gsm, GEO_TO_GSM, &c );
Lgm_Convert_Coords( &Zsc_geo, &Zsc_gsm, GEO_TO_GSM, &c );
Xsc_gsm = Xsc_geo;
Ysc_gsm = Ysc_geo;
Zsc_gsm = Zsc_geo;
/*
* Construct Transformation Matrix from GSM to S/C
*/
Agsm_to_sc[0][0] = Xsc_gsm.x, Agsm_to_sc[1][0] = Xsc_gsm.y, Agsm_to_sc[2][0] = Xsc_gsm.z;
Agsm_to_sc[0][1] = Ysc_gsm.x, Agsm_to_sc[1][1] = Ysc_gsm.y, Agsm_to_sc[2][1] = Ysc_gsm.z;
Agsm_to_sc[0][2] = Zsc_gsm.x, Agsm_to_sc[1][2] = Zsc_gsm.y, Agsm_to_sc[2][2] = Zsc_gsm.z;
Asc_to_gsm[0][0] = Xsc_gsm.x, Asc_to_gsm[1][0] = Ysc_gsm.x, Asc_to_gsm[2][0] = Zsc_gsm.x;
Asc_to_gsm[0][1] = Xsc_gsm.y, Asc_to_gsm[1][1] = Ysc_gsm.y, Asc_to_gsm[2][1] = Zsc_gsm.y;
Asc_to_gsm[0][2] = Xsc_gsm.z, Asc_to_gsm[1][2] = Ysc_gsm.z, Asc_to_gsm[2][2] = Zsc_gsm.z;
printf(" ( %5g %5g %5g )\n", Asc_to_gsm[0][0], Asc_to_gsm[1][0], Asc_to_gsm[2][0] );
printf("Asc_to_gsm = ( %5g %5g %5g )\n", Asc_to_gsm[0][1], Asc_to_gsm[1][1], Asc_to_gsm[2][1] );
printf(" ( %5g %5g %5g )\n", Asc_to_gsm[0][2], Asc_to_gsm[1][2], Asc_to_gsm[2][2] );
/*
* Now construct trans matrix between S/C and INSTRUMENT
*
* Lets define the instrument coord system to be as follows:
* Yins: parallel to instrument cylindrical symmetry axis.
* Zins: parallel to FOV of
*/
/*
* Here is the prescription to orient ZPS:
*
* 1. Start with Yins parallel to Ysc and Zins parallel to Zsc
* 2. Rotate by -29 deg. about Zsc
* 3. Then rotate by +45.5 deg. about Yins
* 4. Then roate by +30.0 deg. about Zins
*
* The easiest way to do this is to use Quaternions
*
*/
// Step 1
Xins = Xsc_gsm;
Yins = Ysc_gsm;
Zins = Zsc_gsm;
// Step2
//Lgm_AxisAngleToQuat( &Zins, -29.0, Q1 );
Lgm_AxisAngleToQuat( &Zins, 0.0, Q1 );
Lgm_QuatRotateVector( Q1, &Xins, &Xins_new ); Xins = Xins_new;
Lgm_QuatRotateVector( Q1, &Yins, &Yins_new ); Yins = Yins_new;
Lgm_QuatRotateVector( Q1, &Zins, &Zins_new ); Zins = Zins_new;
// Step3
//Lgm_AxisAngleToQuat( &Yins, 45.5, Q2 );
Lgm_AxisAngleToQuat( &Yins, 0.0, Q2 );
Lgm_QuatRotateVector( Q2, &Xins, &Xins_new ); Xins = Xins_new;
Lgm_QuatRotateVector( Q2, &Yins, &Yins_new ); Yins = Yins_new;
Lgm_QuatRotateVector( Q2, &Zins, &Zins_new ); Zins = Zins_new;
// Step4
//Lgm_AxisAngleToQuat( &Zins, 30.0, Q3 );
Lgm_AxisAngleToQuat( &Zins, 0.0, Q3 );
Lgm_QuatRotateVector( Q3, &Xins, &Xins_new ); Xins = Xins_new;
Lgm_QuatRotateVector( Q3, &Yins, &Yins_new ); Yins = Yins_new;
Lgm_QuatRotateVector( Q3, &Zins, &Zins_new ); Zins = Zins_new;
/*
* Now, Xins, Yins, Zins will be oriented in their final positions.
* We can construct the sc -> ins trans matrix in one of two wyas now.
* 1. combine all of the quats and then convert Q->matrix
* or 2. use the final unit vectors to manually construct the matri.
* Try both...
*/
Lgm_QuatCombineQuats( Q1, Q2, QT1 );
Lgm_QuatCombineQuats( QT1, Q3, QQ );
for (i=0; i<4; i++) Qins_to_gsm[i] = QQ[i];
Lgm_Quat_To_Matrix( QQ, Ains_to_gsm );
for (i=0; i<3; ++i){
for (j=0; j<3; ++j){
Agsm_to_ins[i][j] = Ains_to_gsm[j][i];
}
}
Lgm_MatrixToQuat( Agsm_to_ins, Qgsm_to_ins );
Lgm_MatrixToQuat( Agsm_to_sc, Qgsm_to_ins );
Lgm_MatrixToQuat( Asc_to_gsm, Qins_to_gsm );
}
int main(void) {
Lgm_Vector *v, *v2;
double Lat, Lon, r, ang;
double Arr[3];
printf("Make a Vector:\n");
v = Lgm_CreateVector(1,2,3);
Lgm_PrintVector(v);
printf("Normalize it:\n");
Lgm_NormalizeVector(v);
Lgm_PrintVector(v);
printf("Force it to have a magnitude:\n");
Lgm_ForceMagnitude(v, 2);
Lgm_PrintVector(v);
printf("Set its value:\n");
Lgm_SetVecVal(v, 3);
Lgm_PrintVector(v);
printf("Set its elements:\n");
Lgm_SetVecElements(v, 1,2,3);
Lgm_PrintVector(v);
printf("Change it to spherical:\n");
Lgm_CartToSphCoords(v, &Lat, &Lon, &r);
printf("Lat:%lf Lon:%lf r:%lf\n", Lat, Lon, r);
printf("And back to Cart:\n");
Lgm_SphToCartCoords(Lat, Lon, r, v );
Lgm_PrintVector(v);
printf("Make it an array:\n");
Lgm_VecToArr(v, Arr);
printf("Arr[0]:%lf Arr[1]:%lf Arr[2]:%lf\n", Arr[0], Arr[1], Arr[2]);
printf("Edit the array and make it a vector:\n");
Arr[0] = -1;
Lgm_ArrToVec(Arr, v);
Lgm_PrintVector(v);
printf("Divide it by a scalar:\n");
Lgm_VecDivideScalar(v, 2);
Lgm_PrintVector(v);
printf("Multiply by a scalar:\n");
Lgm_VecMultiplyScalar(v, 2);
Lgm_PrintVector(v);
printf("Make a new vector and get the angle between them:\n");
v2 = Lgm_CreateVector(1,0,0);
Lgm_SetVecElements(v, 0, 1, 0);
ang = Lgm_VectorAngle(v, v2);
printf("Angle:%lf\n", ang);
printf("Make a new vector and get the angle between them:\n");
Lgm_SetVecElements(v, 0, 1, 0);
Lgm_SetVecElements(v2, 0, 1, 0);
ang = Lgm_VectorAngle(v, v2);
printf("Angle:%lf\n", ang);
Lgm_FreeVector(v);
Lgm_FreeVector(v2);
return(0);
}
