00001 /* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*- 00002 00003 this file is part of rcssserver3D 00004 Fri May 9 2003 00005 Copyright (C) 2002,2003 Koblenz University 00006 Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group 00007 $Id: perfectvisionperceptor.cpp,v 1.7 2004/04/05 14:51:36 rollmark Exp $ 00008 00009 This program is free software; you can redistribute it and/or modify 00010 it under the terms of the GNU General Public License as published by 00011 the Free Software Foundation; version 2 of the License. 00012 00013 This program is distributed in the hope that it will be useful, 00014 but WITHOUT ANY WARRANTY; without even the implied warranty of 00015 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00016 GNU General Public License for more details. 00017 00018 You should have received a copy of the GNU General Public License 00019 along with this program; if not, write to the Free Software 00020 Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. 00021 */ 00022 #include "perfectvisionperceptor.h" 00023 #include <zeitgeist/logserver/logserver.h> 00024 #include <oxygen/sceneserver/scene.h> 00025 #include <oxygen/sceneserver/transform.h> 00026 00027 #define TEST_ME 0 00028 00029 #if TEST_ME 00030 #include <iostream> 00031 #endif 00032 00033 using namespace oxygen; 00034 using namespace boost; 00035 using namespace zeitgeist; 00036 00037 PerfectVisionPerceptor::PerfectVisionPerceptor() : oxygen::Perceptor() 00038 { 00039 mInvertX = mInvertY = mInvertZ = false; 00040 } 00041 00042 PerfectVisionPerceptor::~PerfectVisionPerceptor() 00043 { 00044 } 00045 00046 bool 00047 PerfectVisionPerceptor::Percept(boost::shared_ptr<PredicateList> predList) 00048 { 00049 if (mSceneServer.get() == 0) 00050 { 00051 mSceneServer = shared_static_cast<SceneServer> 00052 (GetCore()->Get("/sys/server/scene")); 00053 } 00054 00055 if (mSceneServer.get() == 0) 00056 { 00057 return false; 00058 } 00059 00060 shared_ptr<Scene> activeScene = mSceneServer->GetActiveScene(); 00061 if (activeScene.get() == 0) 00062 { 00063 GetLog()->Error() 00064 << "ERROR: (PerfectVisionPerceptor) SceneServer reports no active scene\n"; 00065 return false; 00066 } 00067 00068 Predicate& predicate = predList->AddPredicate(); 00069 predicate.name = "PerfectVision"; 00070 predicate.parameter.Clear(); 00071 00072 // we want positions relative to the closest parent transform node 00073 shared_ptr<Transform> parent = shared_dynamic_cast<Transform> 00074 (make_shared(FindParentSupportingClass<Transform>())); 00075 00076 salt::Vector3f myPos(0,0,0); 00077 if (parent.get() == 0) 00078 { 00079 GetLog()->Warning() 00080 << "WARNING: (PerfectVisionPerceptor) parent node is " 00081 << "not derived from TransformNode\n"; 00082 } else 00083 { 00084 myPos = parent->GetWorldTransform().Pos(); 00085 } 00086 00087 TLeafList transformList; 00088 activeScene->ListChildrenSupportingClass<Transform>(transformList, true); 00089 00090 for (TLeafList::iterator i = transformList.begin(); 00091 i != transformList.end(); ++i) 00092 { 00093 shared_ptr<Transform> j = shared_static_cast<Transform>(*i); 00094 const salt::Vector3f& pos = j->GetWorldTransform().Pos() - myPos; 00095 00096 ParameterList& element = predicate.parameter.AddList(); 00097 element.AddValue((*i)->GetName()); 00098 00099 ParameterList& posElement = element.AddList(); 00100 posElement.AddValue(std::string("pos")); 00101 posElement.AddValue(mInvertX ? -pos[0] : pos[0]); 00102 posElement.AddValue(mInvertY ? -pos[1] : pos[1]); 00103 posElement.AddValue(mInvertZ ? -pos[2] : pos[2]); 00104 } 00105 00106 return true; 00107 } 00108 00109 void 00110 PerfectVisionPerceptor::SetInversion(bool x, bool y, bool z) 00111 { 00112 mInvertX = x; 00113 mInvertY = y; 00114 mInvertZ = z; 00115 } 00116