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>