Motion Strategy Library

00001 //----------------------------------------------------------------------
00002 //               The Motion Strategy Library (MSL)
00003 //----------------------------------------------------------------------
00004 //
00005 // Copyright (c) 1998-2000 Iowa State University and Steve LaValle.  
00006 // All Rights Reserved.
00007 // 
00008 // Permission to use, copy, and distribute this software and its 
00009 // documentation is hereby granted free of charge, provided that 
00010 // (1) it is not a component of a commercial product, and 
00011 // (2) this notice appears in all copies of the software and
00012 //     related documentation. 
00013 // 
00014 // Iowa State University and the author make no representations
00015 // about the suitability or fitness of this software for any purpose.  
00016 // It is provided "as is" without express or implied warranty.
00017 //----------------------------------------------------------------------
00018 
00019 #include <fstream.h>
00020 #include <math.h>
00021 
00022 #include "scene.h"
00023 #include "defs.h"
00024 
00025 // Constructor
00026 Scene::Scene(Problem *problem, string path = "") {
00027 
00028   SetProblem(problem);
00029 
00030   if ((path.length() > 0)&&(path[path.length()-1] != '/'))
00031     path += "/";
00032 
00033   FilePath = path;
00034 
00035   NumBodies = P->NumBodies;
00036   
00037   READ_PARAMETER_OR_DEFAULT(LowerWorld,MSLVector(-50.0,-50.0,-50.0));
00038   READ_PARAMETER_OR_DEFAULT(UpperWorld,MSLVector(50.0,50.0,50.0));
00039   READ_PARAMETER_OR_DEFAULT(GlobalCameraPosition,
00040                             MSLVector((UpperWorld[0] + LowerWorld[0])/2.0,
00041                                       (UpperWorld[1] + LowerWorld[1])/2.0,
00042                                       UpperWorld[2] + 
00043                                       (UpperWorld[1]- 
00044                                        LowerWorld[1])/tan(38.0/180.0*PI)));
00045   READ_PARAMETER_OR_DEFAULT(GlobalCameraDirection,MSLVector(0.0,0.0,-1.0));
00046   READ_PARAMETER_OR_DEFAULT(GlobalCameraZenith,MSLVector(0.0,1.0,0.0));
00047   READ_PARAMETER_OR_DEFAULT(AttachedCameraPosition,MSLVector(0.0,0.0,6.0));
00048   READ_PARAMETER_OR_DEFAULT(AttachedCameraDirection,MSLVector(1.0,0.0,0.0));
00049   READ_PARAMETER_OR_DEFAULT(AttachedCameraZenith,MSLVector(0.0,0.0,1.0));
00050   READ_PARAMETER_OR_DEFAULT(AttachedCameraBody,0);
00051 
00052   GlobalCameraDirection = GlobalCameraDirection.norm();
00053   GlobalCameraZenith = GlobalCameraZenith.norm();
00054   AttachedCameraDirection = AttachedCameraDirection.norm();
00055   AttachedCameraZenith = AttachedCameraZenith.norm();
00056   
00057 }
00058 
00059 
00060 
00061 void Scene::SetProblem(Problem *problem) {
00062   P = problem;
00063   NumBodies = P->NumBodies;
00064   GeomDim = P->GeomDim;
00065   SceneConfigurationDim = NumBodies * 6;  // 6 DOF for each body
00066 }
00067 
00068 
00069 // By default, don't change anything
00070 MSLVector Scene::StateToSceneConfiguration(const MSLVector &x) {
00071   MSLVector sc,q;
00072   int i;
00073   if (GeomDim == 3)
00074     return P->StateToConfiguration(x);
00075   else { // GeomDim == 2
00076     q = P->StateToConfiguration(x); // Three parameters per body
00077     // Blow out the MSLVector to make 6 parameters per body
00078     sc = MSLVector(NumBodies*6);
00079     for (i = 0; i < NumBodies; i++) {
00080       sc[6*i] = q[3*i];
00081       sc[6*i+1] = q[3*i+1];
00082       sc[6*i+2] = 0.0;
00083       sc[6*i+3] = 0.0;
00084       sc[6*i+4] = 0.0;
00085       sc[6*i+5] = q[3*i+2];
00086     }
00087     return sc;
00088   }
00089 }
00090 
00091 
00092 MSLVector Scene::InterpolatedSceneConfiguration(const MSLVector &x1, 
00093                                                 const MSLVector &x2, 
00094                                                 const double &a) {
00095 
00096   return StateToSceneConfiguration(P->InterpolateState(x1,x2,a));
00097 }
00098
scene.C
