Exemplo n.º 1
0
// 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;
}
Exemplo n.º 2
0
//-------------------------------------------------------------------------------
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;
}
Exemplo n.º 3
0
//---------------------------------------------------------------------------
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]);

}
Exemplo n.º 4
0
//-------------------------------------------------------------------------------
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;
}