// Inner function to rotate a vector around another vector by angle
Rvector3 Camera::RotateAround(Rvector3 vector, Real angle, Rvector3 axis){
// Store the old x,y,z values
Real oldX = vector[0], oldY = vector[1], oldZ = vector[2];
// Store the cos and sin values, for convenience
Real c = cos(angle), s = sin(angle);
// Store the x,y,z values of the axis, for convenience
axis.Normalize();
Real x = axis[0], y = axis[1], z = axis[2];
// Rotate the vector around the axis
vector.Set((1+(1-c)*(x*x-1))*oldX + (-z*s+(1-c)*x*y)*oldY + (y*s+(1-c)*x*z)*oldZ,
(z*s+(1-c)*x*y)*oldX + (1+(1-c)*(y*y-1))*oldY + (-x*s+(1-c)*y*z)*oldZ,
(-y*s+(1-c)*x*z)*oldX + (x*s+(1-c)*y*z)*oldY + (1+(1-c)*(z*z-1))*oldZ);
// Make sure the direction vectors are still orthogonal to each other
//ReorthogonalizeVectors();
return vector;
}
//-------------------------------------------------------------------------------
bool PolyhedronBody::FaceNormals()
{
PolygonFace face;
Rvector3 n;
Rvector3 r1, r2, A, B, C;
fn.clear();
// Create normal vector list based on facesList:
Real x, y, z;
for (UnsignedInt i = 0; i < facesList.size(); ++i)
{
// Get indices of triangle ith:
face = facesList[i];
A.Set(verticesList[face[0]].Get(0), verticesList[face[0]].Get(1), verticesList[face[0]].Get(2)); // vertex A of triangle ABC
B.Set(verticesList[face[1]].Get(0), verticesList[face[1]].Get(1), verticesList[face[1]].Get(2)); // vertex B of triangle ABC
C.Set(verticesList[face[2]].Get(0), verticesList[face[2]].Get(1), verticesList[face[2]].Get(2)); // vertex C of triangle ABC
r1 = B - A; // vector AB
r2 = C - B; // vector BC
x = r1[1]*r2[2] - r1[2]*r2[1];
y = r1[2]*r2[0] - r1[0]*r2[2];
z = r1[0]*r2[1] - r1[1]*r2[0];
n.Set(x,y,z); // n = AB x BC
if (n.Norm() < 1.0e-15)
return false;
n.Normalize(); // normal vector of triangle ABC
fn.push_back(n); // add normal vector to the normal vectors list
}
#ifdef DEBUG_FACENORMALS_CALCULATION
MessageInterface::ShowMessage("List of normal vectors:\n");
for (UnsignedInt i = 0; i < fn.size(); ++i)
{
MessageInterface::ShowMessage("%.15lf %.15lf %.15lf\n", fn[i][0], fn[i][1], fn[i][2]);
}
#endif
return true;
}
//---------------------------------------------------------------------------
void SpiceAttitudeKernelReader::GetTargetOrientation(const wxString &objectName,
Integer naifID,
Integer forFrameNaifId,
const A1Mjd &atTime,
// Real tolerance,
Rmatrix33 &r33,
Rvector3 &angVel,
const wxString &referenceFrame)
{
#ifdef DEBUG_CK_READING
MessageInterface::ShowMessage(wxT("Entering GetTargetOrientation for object %s, with NAIF ID %d, at time %12.10f, with frame = %s\n"),
objectName.c_str(), naifID, atTime.Get(), referenceFrame.c_str());
#endif
wxString objectNameToUse = objectName;
objectNameToUse = GmatStringUtil::ToUpper(objectNameToUse);
objectNameSPICE = objectNameToUse.char_str();
naifIDSPICE = naifID;
frameNaifIDSPICE = forFrameNaifId;
referenceFrameSPICE = referenceFrame.char_str();
etSPICE = A1ToSpiceTime(atTime.Get());
// boddef_c(objectNameSPICE, naifIDSPICE); // CSPICE method to set NAIF ID for an object - is this valid for spacecraft?
// Convert the time (in TDB) to spacecaft ticks
SpiceDouble scTime;
sce2c_c(naifIDSPICE, etSPICE, &scTime);
if (failed_c())
{
ConstSpiceChar option[] = "LONG"; // retrieve long error message, for now
SpiceInt numChar = MAX_LONG_MESSAGE_VALUE;
//SpiceChar err[MAX_LONG_MESSAGE_VALUE];
SpiceChar *err = new SpiceChar[MAX_LONG_MESSAGE_VALUE];
getmsg_c(option, numChar, err);
wxString errStr(wxString::FromAscii(err));
wxString errmsg = wxT("Error getting spacecraft time (ticks) for object \"");
errmsg += objectName + wxT("\". Message received from CSPICE is: ");
errmsg += errStr + wxT("\n");
reset_c();
delete [] err;
throw UtilityException(errmsg);
}
// get the tolerance in spacecraft clock ticks
wxString tolerance = wxT("01"); // this should probably be user input, or set as a constant
ConstSpiceChar *tol = tolerance.char_str();
SpiceDouble tolTicks;
sctiks_c(naifIDSPICE, tol, &tolTicks);
if (failed_c())
{
ConstSpiceChar option[] = "LONG"; // retrieve long error message, for now
SpiceInt numChar = MAX_LONG_MESSAGE_VALUE;
//SpiceChar err[MAX_LONG_MESSAGE_VALUE];
SpiceChar *err = new SpiceChar[MAX_LONG_MESSAGE_VALUE];
getmsg_c(option, numChar, err);
wxString errStr(wxString::FromAscii(err));
wxString errmsg = wxT("Error getting tolerance (ticks) for object \"");
errmsg += objectName + wxT("\". Message received from CSPICE is: ");
errmsg += errStr + wxT("\n");
reset_c();
delete [] err;
throw UtilityException(errmsg);
}
#ifdef DEBUG_CK_READING
MessageInterface::ShowMessage(wxT("First, check for coverage for object \"%s\", with NAIF ID %d\n"),
objectName.c_str(), naifID);
#endif
Real beginCov = 0.0;
Real endCov = 0.0;
GetCoverageStartAndEnd(loadedKernels, forFrameNaifId, beginCov, endCov, false);
// Now get the C-matrix and angular velocity at the requested time
SpiceDouble cmat[3][3];
SpiceDouble av[3];
SpiceBoolean found;
SpiceDouble clkout;
#ifdef DEBUG_CK_READING
MessageInterface::ShowMessage(wxT("about to call ckgpav: \n"));
MessageInterface::ShowMessage(wxT(" NAIF ID = %d\n")
wxT(" etSPICE = %12.10f\n")
wxT(" scTime = %12.10fn")
wxT(" tolTicks = %12.10f\n")
wxT(" refFrame = %s\n"),
(Integer) naifIDSPICE, (Real) etSPICE, (Real) scTime, (Real) tolTicks,
referenceFrame.c_str());
#endif
ckgpav_c(frameNaifIDSPICE, scTime, tolTicks, referenceFrameSPICE, cmat, av, &clkout, &found);
// ckgpav_c(naifIDSPICE, scTime, tolTicks, referenceFrameSPICE, cmat, av, &clkout, &found);
if (failed_c())
{
ConstSpiceChar option[] = "LONG"; // retrieve long error message, for now
SpiceInt numChar = MAX_LONG_MESSAGE_VALUE;
//SpiceChar err[MAX_LONG_MESSAGE_VALUE];
SpiceChar *err = new SpiceChar[MAX_LONG_MESSAGE_VALUE];
getmsg_c(option, numChar, err);
wxString errStr(wxString::FromAscii(err));
wxString errmsg = wxT("Error getting C-matrix and/or angular velocity for object \"");
errmsg += objectName + wxT("\". Message received from CSPICE is: ");
errmsg += errStr + wxT("\n");
reset_c();
delete [] err;
throw UtilityException(errmsg);
}
if (found == SPICEFALSE)
{
wxString errmsg = wxT("Pointing data for object ");
errmsg += objectName + wxT(" not found on loaded CK/SCLK kernels.\n");
throw UtilityException(errmsg);
}
#ifdef DEBUG_CK_READING
MessageInterface::ShowMessage(wxT("results from ckgpav: \n"));
MessageInterface::ShowMessage(wxT(" cosMat = %12.10f %12.10f %12.10f\n")
wxT(" %12.10f %12.10f %12.10f\n")
wxT(" %12.10f %12.10f %12.10f\n"),
(Real)cmat[0][0], (Real)cmat[0][1], (Real)cmat[0][2],
(Real)cmat[1][0], (Real)cmat[1][1], (Real)cmat[1][2],
(Real)cmat[2][0], (Real)cmat[2][1], (Real)cmat[2][2]);
MessageInterface::ShowMessage(wxT(" angvel = %12.10f %12.10f %12.10f\n"),
(Real)av[0], (Real)av[1], (Real)av[2]);
MessageInterface::ShowMessage(wxT(" and clkout = %12.10f\n"), (Real) clkout);
#endif
// Set output values
r33.Set(cmat[0][0], cmat[0][1], cmat[0][2],
cmat[1][0], cmat[1][1], cmat[1][2],
cmat[2][0], cmat[2][1], cmat[2][2]);
angVel.Set(av[0], av[1], av[2]);
}
//-------------------------------------------------------------------------------
bool PolyhedronBody::LoadBodyShape()
{
if (isLoad)
return true;
// 1. Open data file:
std::ifstream bsFile(bodyShapeFilename.c_str());
if (!bsFile)
throw UtilityException("Error opening file: " +
bodyShapeFilename);
bsFile.setf(std::ios::skipws);
std::string line, subline;
// 2. Read all vertices and store them to pointsList:
Integer numvertices;
Integer index;
Real x, y, z;
Rvector3 vertex;
// 2.1. Read number of vertices:
getline(bsFile,line);
GmatStringUtil::ToInteger(line,&numvertices);
// 2.2. Read all vertices:
Integer first, last;
for(Integer i=0; (i < numvertices)&&(!bsFile.eof()); ++i)
{
// get a line from data file:
getline(bsFile,line);
// get record's index:
line = GmatStringUtil::Trim(line, LEADING);
GmatStringUtil::FindFirstAndLast(line,' ',first,last);
subline = line.substr(0,first);
GmatStringUtil::ToInteger(subline, &index);
line = line.substr(first, line.size()-first);
// get vertex's x-coordinate:
line = GmatStringUtil::Trim(line, LEADING);
GmatStringUtil::FindFirstAndLast(line,' ',first,last);
subline = line.substr(0,first);
GmatStringUtil::ToReal(subline, &x);
line = line.substr(first, line.size()-first);
// get vertex's y-coordinate:
line = GmatStringUtil::Trim(line, LEADING);
GmatStringUtil::FindFirstAndLast(line,' ',first,last);
subline = line.substr(0,first);
GmatStringUtil::ToReal(subline, &y);
line = line.substr(first, line.size()-first);
// get vertex's z-coordinate:
line = GmatStringUtil::Trim(line, LEADING);
GmatStringUtil::FindFirstAndLast(line,' ',first,last);
subline = line.substr(0,first);
GmatStringUtil::ToReal(subline, &z);
// push the vertex to verticesList:
vertex.Set(x, y,z); // unit: km
verticesList.push_back(vertex);
}
if (bsFile.eof())
throw UtilityException("Error: missing data in file: " +
bodyShapeFilename);
#ifdef DEBUG_READ_DATAFILE
MessageInterface::ShowMessage("number of vertices = %d\n", verticesList.size());
for (UnsignedInt i = 0; i < verticesList.size(); ++i)
MessageInterface::ShowMessage("%.15lf %.15lf %.15lf\n", verticesList[i].Get(0), verticesList[i].Get(1), verticesList[i].Get(2) );
MessageInterface::ShowMessage("\n");
#endif
// 3. Read all faces and store them to facesList:
Integer numfaces;
Integer ix, iy, iz;
PolygonFace triangularface;
// 3.1. Read number of faces:
getline(bsFile,line);
GmatStringUtil::ToInteger(line,&numfaces);
// 3.2. Read all faces:
for(Integer i=0; (i < numfaces)&&(!bsFile.eof()); ++i)
{
// get a line from data file:
getline(bsFile,line);
// get record's index:
line = GmatStringUtil::Trim(line, LEADING);
GmatStringUtil::FindFirstAndLast(line,' ',first,last);
subline = line.substr(0,first);
GmatStringUtil::ToInteger(subline, &index);
line = line.substr(first, line.size()-first);
// get the index of the first vertex of triangle :
line = GmatStringUtil::Trim(line, LEADING);
GmatStringUtil::FindFirstAndLast(line,' ',first,last);
subline = line.substr(0,first);
GmatStringUtil::ToInteger(subline, &ix);
line = line.substr(first, line.size()-first);
// get the index of the second vertex of triangle :
line = GmatStringUtil::Trim(line, LEADING);
GmatStringUtil::FindFirstAndLast(line,' ',first,last);
subline = line.substr(0,first);
GmatStringUtil::ToInteger(subline, &iy);
line = line.substr(first, line.size()-first);
// get the index of the third vertex of triangle :
line = GmatStringUtil::Trim(line, LEADING);
GmatStringUtil::FindFirstAndLast(line,' ',first,last);
subline = line.substr(0,first);
GmatStringUtil::ToInteger(subline, &iz);
// define a triangular face:
triangularface.push_back(ix-1); // GMAT index starts from 0; MatLab index starts from 1
triangularface.push_back(iy-1); // GMAT index starts from 0; MatLab index starts from 1
triangularface.push_back(iz-1); // GMAT index starts from 0; MatLab index starts from 1
// add the triangular face into facesList:
facesList.push_back(triangularface);
triangularface.clear();
}
if (bsFile.eof())
throw UtilityException("Error: missing data in file: " +
bodyShapeFilename);
// 4. Close data file:
if (bsFile.is_open()) bsFile.close();
isLoad = true;
#ifdef DEBUG_READ_DATAFILE
MessageInterface::ShowMessage("number of faces = %d\n", facesList.size());
for (UnsignedInt i = 0; i < facesList.size(); ++i)
MessageInterface::ShowMessage("%d: %d %d %d\n", i, facesList[i].at(0), facesList[i].at(1), facesList[i].at(2));
MessageInterface::ShowMessage("\n");
#endif
return true;
}