Main Page   Class Hierarchy   Compound List   File List   Compound Members   File Members  

scene.C

Go to the documentation of this file.
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 #include <LEDA/REDEFINE_NAMES.h>
00026 
00027 // Constructor
00028 Scene::Scene(Problem *problem, string path = "") {
00029 
00030   SetProblem(problem);
00031 
00032   if ((path.length() > 0)&&(path[path.length()-1] != '/'))
00033     path += "/";
00034 
00035   FilePath = path;
00036 
00037   NumBodies = P->NumBodies;
00038 
00039   if (is_file(FilePath+"LowerWorld")) {
00040     file_istream fin(FilePath + "LowerWorld");
00041     fin >> LowerWorld;
00042     fin.close();
00043   }
00044   else {
00045     LowerWorld = vector(-50.0,-50.0,-50.0);
00046   }
00047 
00048   if (is_file(FilePath+"UpperWorld")) {
00049     file_istream fin2(FilePath + "UpperWorld");
00050     fin2 >> UpperWorld;
00051     fin2.close();
00052   }
00053   else {
00054     UpperWorld = vector(50.0,50.0,50.0);
00055   }
00056 
00057   if (is_file(FilePath+"GlobalCameraPosition")) {
00058     file_istream fin3(FilePath + "GlobalCameraPosition");
00059     fin3 >> GlobalCameraPosition;
00060     fin3.close();
00061   }
00062   else {
00063     GlobalCameraPosition = vector(3);
00064 
00065     GlobalCameraPosition[0] = (UpperWorld[0] + LowerWorld[0])/2.0;  
00066     GlobalCameraPosition[1] = (UpperWorld[1] + LowerWorld[1])/2.0;  
00067     GlobalCameraPosition[2] = UpperWorld[2] + (UpperWorld[1]-LowerWorld[1])/tan(38.0/180.0*PI);
00068     //    GlobalCameraPosition = vector(0.0,0.0,200.0);
00069   }
00070 
00071   if (is_file(FilePath+"GlobalCameraDirection")) {
00072     file_istream fin4(FilePath + "GlobalCameraDirection");
00073     fin4 >> GlobalCameraDirection;
00074     GlobalCameraDirection = GlobalCameraDirection.norm();
00075     fin4.close();
00076   }
00077   else {
00078     GlobalCameraDirection = vector(0.0,0.0,-1.0);
00079   }
00080 
00081   if (is_file(FilePath+"GlobalCameraZenith")) {
00082     file_istream fin4(FilePath + "GlobalCameraZenith");
00083     fin4 >> GlobalCameraZenith;
00084     GlobalCameraZenith = GlobalCameraZenith.norm();
00085     fin4.close();
00086   }
00087   else {
00088     GlobalCameraZenith = vector(0.0,1.0,0.0);
00089   }
00090 
00091 
00092   if (is_file(FilePath+"AttachedCameraPosition")) {
00093     file_istream fin3(FilePath + "AttachedCameraPosition");
00094     fin3 >> AttachedCameraPosition;
00095     fin3.close();
00096   }
00097   else {
00098     AttachedCameraPosition = vector(0.0,0.0,6.0);
00099   }
00100 
00101   if (is_file(FilePath+"AttachedCameraDirection")) {
00102     file_istream fin4(FilePath + "AttachedCameraDirection");
00103     fin4 >> AttachedCameraDirection;
00104     AttachedCameraDirection = AttachedCameraDirection.norm();
00105     fin4.close();
00106   }
00107   else {
00108     AttachedCameraDirection = vector(1.0,0.0,0.0);
00109   }
00110 
00111   if (is_file(FilePath+"AttachedCameraZenith")) {
00112     file_istream fin4(FilePath + "AttachedCameraZenith");
00113     fin4 >> AttachedCameraZenith;
00114     AttachedCameraZenith = AttachedCameraZenith.norm();
00115     fin4.close();
00116   }
00117   else {
00118     AttachedCameraZenith = vector(0.0,0.0,1.0);
00119   }
00120 
00121   if (is_file(FilePath+"AttachedCameraBody")) {
00122     file_istream fin4(FilePath + "AttachedCameraBody");
00123     fin4 >> AttachedCameraBody;
00124     fin4.close();
00125   }
00126   else {
00127     AttachedCameraBody = 0;
00128   }
00129 
00130 }
00131 
00132 
00133 
00134 void Scene::SetProblem(Problem *problem) {
00135   P = problem;
00136   NumBodies = P->NumBodies;
00137   GeomDim = P->GeomDim;
00138   SceneConfigurationDim = NumBodies * 6;  // 6 DOF for each body
00139 }
00140 
00141 
00142 // By default, don't change anything
00143 vector Scene::StateToSceneConfiguration(const vector &x) {
00144   vector sc,q;
00145   int i;
00146   if (GeomDim == 3)
00147     return P->StateToConfiguration(x);
00148   else { // GeomDim == 2
00149     q = P->StateToConfiguration(x); // Three parameters per body
00150     // Blow out the vector to make 6 parameters per body
00151     sc = vector(NumBodies*6);
00152     for (i = 0; i < NumBodies; i++) {
00153       sc[6*i] = q[3*i];
00154       sc[6*i+1] = q[3*i+1];
00155       sc[6*i+2] = 0.0;
00156       sc[6*i+3] = 0.0;
00157       sc[6*i+4] = 0.0;
00158       sc[6*i+5] = q[3*i+2];
00159     }
00160     return sc;
00161   }
00162 }
00163 
00164 
00165 vector Scene::InterpolatedSceneConfiguration(const vector &x1, 
00166                                              const vector &x2, 
00167                                              const double &a) {
00168 
00169   return StateToSceneConfiguration(P->InterpolateState(x1,x2,a));
00170 }
00171 
00172 #include <LEDA/UNDEFINE_NAMES.h>
Motion Strategy Library


Web page maintained by Steve LaValle
Partial support provided by NSF CAREER Award IRI-970228 (LaValle), Honda Research, and Iowa State University.
Contributors: Anna Atramentov, Peng Cheng, James Kuffner, Steve LaValle, and Libo Yang.