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