Menu
▾
▴
[simspark-svn] SF.net SVN: simspark:[190] trunk
|
From: <yx...@us...> - 2010-03-30 11:33:09
|
Revision: 190
http://simspark.svn.sourceforge.net/simspark/?rev=190&view=rev
Author: yxu
Date: 2010-03-30 11:33:03 +0000 (Tue, 30 Mar 2010)
Log Message:
-----------
Sense lines on the field (the first working version)
Modified Paths:
--------------
trunk/rcssserver3d/data/rsg/agent/nao/soccer.rsg
trunk/rcssserver3d/plugin/soccer/CMakeLists.txt
trunk/rcssserver3d/plugin/soccer/export.cpp
trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.cpp
trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.h
trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor_c.cpp
trunk/spark/lib/salt/CMakeLists.txt
Added Paths:
-----------
trunk/rcssserver3d/data/rsg/agent/fieldline.rsg
trunk/rcssserver3d/data/rsg/agent/fieldring.rsg
trunk/rcssserver3d/plugin/soccer/line/
trunk/rcssserver3d/plugin/soccer/line/line.cpp
trunk/rcssserver3d/plugin/soccer/line/line.h
trunk/rcssserver3d/plugin/soccer/line/line_c.cpp
trunk/spark/lib/salt/line2.h
trunk/spark/lib/salt/linesegment2.h
Added: trunk/rcssserver3d/data/rsg/agent/fieldline.rsg
===================================================================
--- trunk/rcssserver3d/data/rsg/agent/fieldline.rsg (rev 0)
+++ trunk/rcssserver3d/data/rsg/agent/fieldline.rsg 2010-03-30 11:33:03 UTC (rev 190)
@@ -0,0 +1,27 @@
+; -*- mode: lisp; -*-
+;
+; create a field line from ($BegX $BegY) to ($EndX $EndY)
+;
+
+(RSG 0 1)
+(
+ (templ $BegX $BegY $EndX $EndY)
+
+ (nd Transform
+ (nd Line
+ (setBeginPoint $BegX $BegY 0)
+ (setEndPoint $EndX $EndY 0)
+ )
+ )
+
+ ;; (def $centerX (eval (eval $BegX + $EndX) / 2.0))
+ ;; (def $centerY (eval (eval $BegY + $EndY) / 2.0))
+ ;; (def $lengthY (eval (eval $EndY - $BegY) / 2.0))
+
+ ;; (nd Transform
+ ;; (setLocalPos $centerX $centerY 0)
+ ;; (nd Box
+ ;; (setExtents 0.1 $lengthY 0.1)
+ ;; )
+ ;; )
+ )
\ No newline at end of file
Added: trunk/rcssserver3d/data/rsg/agent/fieldring.rsg
===================================================================
--- trunk/rcssserver3d/data/rsg/agent/fieldring.rsg (rev 0)
+++ trunk/rcssserver3d/data/rsg/agent/fieldring.rsg 2010-03-30 11:33:03 UTC (rev 190)
@@ -0,0 +1,89 @@
+; -*- mode: lisp; -*-
+;
+; create a field ring with radius $Radius
+;
+
+(RSG 0 1)
+(
+ (templ $Radius)
+
+ (def $px0 1.0)
+ (def $py0 0.0)
+
+ (def $px36 0.80901699437494745)
+ (def $py36 0.58778525229247314)
+
+ (def $px72 0.30901699437494745)
+ (def $py72 0.95105651629515353)
+
+ (def $px108 -0.30901699437494734)
+ (def $py108 0.95105651629515364)
+
+ (def $px144 -0.80901699437494734)
+ (def $py144 0.58778525229247325)
+
+ (def $px180 -1.0)
+ (def $py180 0.0)
+
+ (def $px216 $px144)
+ (def $py216 (eval -1 * $py144))
+
+ (def $px252 $px108)
+ (def $py252 (eval -1 * $py108))
+
+ (def $px288 $px72)
+ (def $py288 (eval -1 * $py72))
+
+ (def $px324 $px36)
+ (def $py324 (eval -1 * $py36))
+
+ (importScene rsg/agent/fieldline.rsg
+ $px0 $py0
+ $px36 $py36
+ )
+
+ (importScene rsg/agent/fieldline.rsg
+ $px36 $py36
+ $px72 $py72
+ )
+
+ (importScene rsg/agent/fieldline.rsg
+ $px72 $py72
+ $px108 $py108
+ )
+
+ (importScene rsg/agent/fieldline.rsg
+ $px108 $py108
+ $px144 $py144
+ )
+
+ (importScene rsg/agent/fieldline.rsg
+ $px144 $py144
+ $px180 $py180
+ )
+
+ (importScene rsg/agent/fieldline.rsg
+ $px180 $py180
+ $px216 $py216
+ )
+
+ (importScene rsg/agent/fieldline.rsg
+ $px216 $py216
+ $px252 $py252
+ )
+
+ (importScene rsg/agent/fieldline.rsg
+ $px252 $py252
+ $px288 $py288
+ )
+
+ (importScene rsg/agent/fieldline.rsg
+ $px288 $py288
+ $px324 $py324
+ )
+
+ (importScene rsg/agent/fieldline.rsg
+ $px324 $py324
+ $px0 $py0
+ )
+ )
Modified: trunk/rcssserver3d/data/rsg/agent/nao/soccer.rsg
===================================================================
--- trunk/rcssserver3d/data/rsg/agent/nao/soccer.rsg 2010-03-28 08:27:22 UTC (rev 189)
+++ trunk/rcssserver3d/data/rsg/agent/nao/soccer.rsg 2010-03-30 11:33:03 UTC (rev 190)
@@ -10,6 +10,8 @@
(def $FieldWidth (eval Soccer.FieldWidth))
(def $FieldHeight (eval Soccer.FieldHeight))
(def $GoalDepth (eval Soccer.GoalDepth))
+ (def $PenaltyLength (eval Soccer.PenaltyLength))
+ (def $PenaltyWidth (eval Soccer.PenaltyWidth))
;; height of the field ground plane
(def $FieldMaterial matGrass)
@@ -22,6 +24,8 @@
(def $GoalHalfDepth (eval $GoalDepth / 2.0))
(def $FieldLengthTextureSize (eval $FieldLength / 12.0 ))
(def $FieldWidthTextureSize (eval $FieldWidth / 8.0 ))
+ (def $PenaltyX (eval $FieldHalfLength - $PenaltyLength ))
+ (def $PenaltyHalfWidth (eval $PenaltyWidth * 0.5 ))
(def $BorderLength (eval $FieldLength * 0.333))
(def $BorderWidth (eval $FieldWidth * 0.75))
@@ -193,7 +197,72 @@
0
F2R
)
+;;
+;; add field lines
+;;
+;; middle line
+(importScene rsg/agent/fieldline.rsg
+ 0 (eval -1 * $FieldHalfWidth)
+ 0 $FieldHalfWidth
+ )
+;; ground lines
+(importScene rsg/agent/fieldline.rsg
+ $FieldHalfLength (eval -1 * $FieldHalfWidth)
+ $FieldHalfLength $FieldHalfWidth
+ )
+
+(importScene rsg/agent/fieldline.rsg
+ (eval -1 * $FieldHalfLength) (eval -1 * $FieldHalfWidth)
+ (eval -1 * $FieldHalfLength) $FieldHalfWidth
+ )
+
+;; side lines
+(importScene rsg/agent/fieldline.rsg
+ $FieldHalfLength $FieldHalfWidth
+ (eval -1 * $FieldHalfLength) $FieldHalfWidth
+ )
+
+(importScene rsg/agent/fieldline.rsg
+ $FieldHalfLength (eval -1 * $FieldHalfWidth)
+ (eval -1 * $FieldHalfLength) (eval -1 * $FieldHalfWidth)
+ )
+
+;; penalty lines
+(importScene rsg/agent/fieldline.rsg
+ $PenaltyX $PenaltyHalfWidth
+ $PenaltyX (eval -1 * $PenaltyHalfWidth)
+ )
+
+(importScene rsg/agent/fieldline.rsg
+ $PenaltyX $PenaltyHalfWidth
+ $FieldHalfLength $PenaltyHalfWidth
+ )
+
+(importScene rsg/agent/fieldline.rsg
+ $PenaltyX (eval -1 * $PenaltyHalfWidth)
+ $FieldHalfLength (eval -1 * $PenaltyHalfWidth)
+ )
+
+(importScene rsg/agent/fieldline.rsg
+ (eval -1 * $PenaltyX) $PenaltyHalfWidth
+ (eval -1 * $PenaltyX) (eval -1 * $PenaltyHalfWidth)
+ )
+
+(importScene rsg/agent/fieldline.rsg
+ (eval -1 * $PenaltyX) $PenaltyHalfWidth
+ (eval -1 * $FieldHalfLength) $PenaltyHalfWidth
+ )
+
+(importScene rsg/agent/fieldline.rsg
+ (eval -1 * $PenaltyX) (eval -1 * $PenaltyHalfWidth)
+ (eval -1 * $FieldHalfLength) (eval -1 * $PenaltyHalfWidth)
+ )
+
+;; ring
+(importScene rsg/agent/fieldring.rsg
+ 1.8)
+
;;
;; add the ball
;;
Modified: trunk/rcssserver3d/plugin/soccer/CMakeLists.txt
===================================================================
--- trunk/rcssserver3d/plugin/soccer/CMakeLists.txt 2010-03-28 08:27:22 UTC (rev 189)
+++ trunk/rcssserver3d/plugin/soccer/CMakeLists.txt 2010-03-30 11:33:03 UTC (rev 190)
@@ -44,7 +44,8 @@
hmdp_effector/hmdpaction.h
hmdp_effector/hmdpeffector.h
hmdp_effector/hmdpperceptor.h
- hmdp_effector/naospecific.h
+ hmdp_effector/naospecific.h
+ line/line.h
)
set(soccer_LIB_SRCS
@@ -117,6 +118,8 @@
hmdp_effector/hmdpperceptor.cpp
hmdp_effector/hmdpwrapper.cpp
hmdp_effector/naospecific.cpp
+ line/line.cpp
+ line/line_c.cpp
)
include_directories(${CMAKE_CURRENT_SOURCE_DIR} ${FREETYPE_INCLUDE_DIRS}
Modified: trunk/rcssserver3d/plugin/soccer/export.cpp
===================================================================
--- trunk/rcssserver3d/plugin/soccer/export.cpp 2010-03-28 08:27:22 UTC (rev 189)
+++ trunk/rcssserver3d/plugin/soccer/export.cpp 2010-03-30 11:33:03 UTC (rev 190)
@@ -53,6 +53,7 @@
#include "agentintegration/soccerbotbehavior.h"
#include "hmdp_effector/hmdpeffector.h"
#include "hmdp_effector/hmdpperceptor.h"
+#include "line/line.h"
ZEITGEIST_EXPORT_BEGIN()
ZEITGEIST_EXPORT(SoccerControlAspect);
@@ -88,5 +89,6 @@
ZEITGEIST_EXPORT(SoccerbotBehavior);
ZEITGEIST_EXPORT(HMDPPerceptor);
ZEITGEIST_EXPORT(HMDPEffector);
+ ZEITGEIST_EXPORT(Line);
ZEITGEIST_EXPORT_END()
Added: trunk/rcssserver3d/plugin/soccer/line/line.cpp
===================================================================
--- trunk/rcssserver3d/plugin/soccer/line/line.cpp (rev 0)
+++ trunk/rcssserver3d/plugin/soccer/line/line.cpp 2010-03-30 11:33:03 UTC (rev 190)
@@ -0,0 +1,31 @@
+/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+ this file is part of rcssserver3D
+ Copyright (C) 2010 RoboCup Soccer Server 3D Maintenance Group
+ $Id$
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; version 2 of the License.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+*/
+
+#include "line.h"
+
+Line::Line()
+{
+
+}
+
+Line::~Line()
+{
+
+}
Added: trunk/rcssserver3d/plugin/soccer/line/line.h
===================================================================
--- trunk/rcssserver3d/plugin/soccer/line/line.h (rev 0)
+++ trunk/rcssserver3d/plugin/soccer/line/line.h 2010-03-30 11:33:03 UTC (rev 190)
@@ -0,0 +1,44 @@
+/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+ this file is part of rcssserver3D
+ Copyright (C) 2010 RoboCup Soccer Server 3D Maintenance Group
+ $Id$
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; version 2 of the License.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+*/
+#ifndef LINE_H
+#define LINE_H
+
+#include "../soccernode/soccernode.h"
+
+class Line : public SoccerNode
+{
+public:
+ Line();
+ virtual ~Line();
+
+ salt::Vector3f& BeginPoint() { return mBeginPoint; }
+ salt::Vector3f& EndPoint() { return mEndPoint; }
+ const salt::Vector3f& BeginPoint() const { return mBeginPoint; }
+ const salt::Vector3f& EndPoint() const { return mEndPoint; }
+
+protected:
+
+ salt::Vector3f mBeginPoint;
+ salt::Vector3f mEndPoint;
+};
+
+DECLARE_CLASS(Line);
+
+#endif // LINE_H
Added: trunk/rcssserver3d/plugin/soccer/line/line_c.cpp
===================================================================
--- trunk/rcssserver3d/plugin/soccer/line/line_c.cpp (rev 0)
+++ trunk/rcssserver3d/plugin/soccer/line/line_c.cpp 2010-03-30 11:33:03 UTC (rev 190)
@@ -0,0 +1,65 @@
+/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+ this file is part of rcssserver3D
+ Copyright (C) 2010 RoboCup Soccer Server 3D Maintenance Group
+ $Id$
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; version 2 of the License.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+*/
+
+#include "line.h"
+
+using namespace boost;
+using namespace oxygen;
+using namespace std;
+
+FUNCTION(Line,setBeginPoint)
+{
+ salt::Vector3f& value = obj->BeginPoint();
+
+ if (
+ (in.GetSize() != 3) ||
+ (! in.GetValue(in[0], value[0])) ||
+ (! in.GetValue(in[1], value[1])) ||
+ (! in.GetValue(in[2], value[2]))
+ )
+ {
+ return false;
+ }
+ return true;
+}
+
+FUNCTION(Line,setEndPoint)
+{
+ salt::Vector3f& value = obj->EndPoint();
+
+ if (
+ (in.GetSize() != 3) ||
+ (! in.GetValue(in[0], value[0])) ||
+ (! in.GetValue(in[1], value[1])) ||
+ (! in.GetValue(in[2], value[2]))
+ )
+ {
+ return false;
+ }
+ return true;
+}
+
+void
+CLASS(Line)::DefineClass()
+{
+ DEFINE_BASECLASS(SoccerNode);
+ DEFINE_FUNCTION(setBeginPoint);
+ DEFINE_FUNCTION(setEndPoint);
+}
Modified: trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.cpp
===================================================================
--- trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.cpp 2010-03-28 08:27:22 UTC (rev 189)
+++ trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.cpp 2010-03-30 11:33:03 UTC (rev 190)
@@ -25,6 +25,12 @@
#include <oxygen/sceneserver/transform.h>
#include <soccerbase/soccerbase.h>
#include <salt/gmath.h>
+#include <list>
+#include <X11/X.h>
+#include <math.h>
+#include <salt/linesegment2.h>
+#include <limits>
+#include <iostream>
using namespace zeitgeist;
using namespace oxygen;
@@ -34,7 +40,8 @@
RestrictedVisionPerceptor::RestrictedVisionPerceptor() : Perceptor(),
mSenseMyPos(false),
mAddNoise(true),
- mStaticSenseAxis(true)
+ mStaticSenseAxis(true),
+ mSenseLine(false)
{
// set predicate name
SetPredicateName("See");
@@ -454,6 +461,11 @@
element.AddValue(sensedMyPos[2]);
}
+ if (mSenseLine)
+ {
+ SenseLine(predicate);
+ }
+
return true;
}
@@ -551,6 +563,11 @@
element.AddValue(sensedMyPos[2]);
}
+ if (mSenseLine)
+ {
+ SenseLine(predicate);
+ }
+
return true;
}
@@ -616,3 +633,233 @@
{
mSenseMyPos = sense;
}
+
+bool RestrictedVisionPerceptor::checkVisuable(RestrictedVisionPerceptor::ObjectData& od) const
+{
+ // theta is the angle in horizontal plane, with fwAngle as 0 degree
+ od.mTheta = gNormalizeDeg(gRadToDeg(gNormalizeRad(
+ gArcTan2(od.mRelPos[1], od.mRelPos[0])
+ )) - 90);
+
+ // latitude with fwPhi as 0 degreee
+ od.mPhi = gRadToDeg(gNormalizeRad(
+ gArcTan2(od.mRelPos[2],
+ Vector2f(od.mRelPos[0], od.mRelPos[1]).Length()
+ )
+ )
+ );
+
+ od.mDist = od.mRelPos.Length();
+ if (od.mDist <= 0.1)
+ {
+ // object is too close
+ return false;
+ }
+
+ const int hAngle_2 = mHViewCone >> 1;
+ if (gAbs(od.mTheta) > hAngle_2)
+ {
+ // object is out of view range
+ return false;
+ }
+
+ const int vAngle_2 = mVViewCone >> 1;
+ if (gAbs(od.mPhi) > vAngle_2)
+ {
+ return false;
+ }
+
+ return true;
+}
+
+void RestrictedVisionPerceptor::SenseLine(Predicate& predicate)
+{
+ const float focalLength = 0.1f;
+ TLineList visibleLines;
+ SetupLines(visibleLines);
+
+ // TODO: precalculate it
+ // visual range
+ Vector2f ru(tan(gDegToRad<double>(mHViewCone*0.5))*focalLength, tan(gDegToRad<double>(mVViewCone*0.5))*focalLength);
+ Vector2f rb(ru.x(), -ru.y());
+ Vector2f lu(-ru.x(), ru.y());
+ Vector2f lb(-ru.x(), -ru.y());
+ LineSegment2f viewRangeLine[4];
+ viewRangeLine[0] = LineSegment2f(lu, ru);
+ viewRangeLine[1] = LineSegment2f(lb, rb);
+ viewRangeLine[2] = LineSegment2f(lu, lb);
+ viewRangeLine[3] = LineSegment2f(ru, rb);
+
+ for (TLineList::iterator i = visibleLines.begin(); i != visibleLines.end(); )
+ {
+ LineData& ld = (*i);
+
+ bool seeBeginPoint = checkVisuable(ld.mBeginPoint);
+ bool seeEndPoint = checkVisuable(ld.mEndPoint);
+
+ if (!(seeBeginPoint && seeEndPoint))
+ {
+ Vector3f bp3 = ld.mBeginPoint.mRelPos;
+ Vector3f ep3 = ld.mEndPoint.mRelPos;
+
+ if ( bp3.y() < focalLength && ep3.y() >= focalLength )
+ {
+ float t = ( focalLength - ep3.y() ) / ( bp3.y() - ep3.y() );
+ bp3 = ep3 + ( bp3 - ep3 ) * t;
+ }
+ else if ( ep3.y() < focalLength && bp3.y() >= focalLength )
+ {
+ float t = ( focalLength - bp3.y() ) / ( ep3.y() - bp3.y() );
+ ep3 = bp3 + ( ep3 - bp3 ) * t;
+ }
+
+ if ( bp3.y() > 0 && ep3.y() > 0 )
+ {
+ Vector2f bp = Vector2f(bp3.x(), bp3.z()) * focalLength / bp3.y();
+ Vector2f ep = Vector2f(ep3.x(), ep3.z()) * focalLength / ep3.y();
+ LineSegment2f l(bp, ep);
+
+ float CB = bp3.Length();
+ float CE = ep3.Length();
+ float BE = (bp3 - ep3).Length();
+ float CEB = acos((BE * BE + CE * CE - CB * CB) / (2 * BE * CE));
+ Vector3f Ef(ep.x(), focalLength, ep.y());
+ float CEf = Ef.Length();
+
+ Vector3f X[2];
+ int inum = 0;
+ for ( int i=0; i<4 && inum < 2; i++)
+ {
+ Vector2f ip;
+ if ( l.Intersection(viewRangeLine[i], ip) )
+ {
+ Vector3f Xf(ip.x(), focalLength, ip.y());
+ float CXf = Xf.Length();
+ float XfEf = (Xf - Ef).Length();
+ float XfCEf = acos((CXf * CXf + CEf * CEf - XfEf * XfEf) / (2 * CXf * CEf));
+ float CXE = gPI - CEB - XfCEf;
+ float EX = CE / sin(CXE) * sin(XfCEf);
+ X[inum] = ep3 + (bp3 - ep3) * (EX / BE);
+ inum++;
+ }
+ }
+
+ if ( inum == 2 )
+ {
+ ld.mBeginPoint.mRelPos = X[0];
+ checkVisuable(ld.mBeginPoint);
+ seeBeginPoint = true;
+
+ ld.mEndPoint.mRelPos = X[1];
+ checkVisuable(ld.mEndPoint);
+ seeEndPoint = true;
+ }
+ else if ( inum == 1 )
+ {
+ if ( !seeBeginPoint )
+ {
+ ld.mBeginPoint.mRelPos = X[0];
+ checkVisuable(ld.mBeginPoint);
+ seeBeginPoint = true;
+ }
+ else
+ {
+ ld.mEndPoint.mRelPos = X[0];
+ checkVisuable(ld.mEndPoint);
+ seeEndPoint = true;
+ }
+ }
+ }
+ }
+
+ if (seeBeginPoint && seeEndPoint)
+ {
+ // make some noise
+ ApplyNoise(ld.mBeginPoint);
+ ApplyNoise(ld.mEndPoint);
+
+ ++i;
+ }
+ else
+ {
+ i = visibleLines.erase(i);
+ }
+ }
+
+ // generate a sense entry
+ AddSense(predicate, visibleLines);
+}
+
+void
+RestrictedVisionPerceptor::SetupLines(TLineList& visibleLines)
+{
+ TLeafList lineList;
+ mActiveScene->GetChildrenOfClass("Line", lineList, true);
+
+ // determine position relative to the local reference frame
+ const Matrix& mat = mTransformParent->GetWorldTransform();
+
+ // get the transformation matrix describing the current orientation
+ Vector3f myPos = mat.Pos();
+ if (mAddNoise)
+ {
+ myPos -= mError;
+ }
+
+
+ for (TLeafList::iterator i = lineList.begin(); i != lineList.end(); ++i)
+ {
+ LineData ld;
+
+ ld.mLine = shared_static_cast<Line > (*i);
+
+ if (ld.mLine.get() == 0)
+ {
+ GetLog()->Error() << "Error: (RestrictedVisionPerceptor) skipped line: "
+ << (*i)->GetName() << "\n";
+ continue; // this should never happen
+ }
+
+ boost::shared_ptr<Transform> j = ld.mLine->GetTransformParent();
+
+ if (j.get() == 0)
+ {
+ GetLog()->Error() << "Error: (RestrictedVisionPerceptor) skipped line (2): "
+ << (*i)->GetName() << "\n";
+ continue; // this should never happen
+ }
+
+ const salt::Matrix& t = j->GetWorldTransform();
+ ld.mBeginPoint.mRelPos = mat.InverseRotate( t * ld.mLine->BeginPoint() - myPos );
+ ld.mEndPoint.mRelPos = mat.InverseRotate( t * ld.mLine->EndPoint() - myPos );
+
+ visibleLines.push_back(ld);
+ }
+}
+
+void RestrictedVisionPerceptor::AddSense(Predicate& predicate, const TLineList& lineList) const
+{
+ for (TLineList::const_iterator i = lineList.begin(); i != lineList.end(); ++i)
+ {
+ const LineData& ld = (*i);
+ ParameterList& element = predicate.parameter.AddList();
+ element.AddValue(std::string("L"));
+
+ ParameterList& begPos = element.AddList();
+ begPos.AddValue(std::string("pol"));
+ begPos.AddValue(ld.mBeginPoint.mDist);
+ begPos.AddValue(ld.mBeginPoint.mTheta);
+ begPos.AddValue(ld.mBeginPoint.mPhi);
+
+ ParameterList& endPos = element.AddList();
+ endPos.AddValue(std::string("pol"));
+ endPos.AddValue(ld.mEndPoint.mDist);
+ endPos.AddValue(ld.mEndPoint.mTheta);
+ endPos.AddValue(ld.mEndPoint.mPhi);
+ }
+}
+
+void RestrictedVisionPerceptor::SetSenseLine(bool sense)
+{
+ mSenseLine = sense;
+}
\ No newline at end of file
Modified: trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.h
===================================================================
--- trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.h 2010-03-28 08:27:22 UTC (rev 189)
+++ trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.h 2010-03-30 11:33:03 UTC (rev 190)
@@ -29,6 +29,7 @@
#include <oxygen/sceneserver/transform.h>
#include <oxygen/agentaspect/agentaspect.h>
#include <agentstate/agentstate.h>
+#include "../line/line.h"
class RestrictedVisionPerceptor : public oxygen::Perceptor
{
@@ -67,6 +68,15 @@
typedef std::map<boost::shared_ptr<oxygen::BaseNode>, TObjectList> TNodeObjectsMap;
+ struct LineData
+ {
+ boost::shared_ptr<Line> mLine;
+ ObjectData mBeginPoint;
+ ObjectData mEndPoint;
+ };
+
+ typedef std::list<LineData> TLineList;
+
public:
RestrictedVisionPerceptor();
virtual ~RestrictedVisionPerceptor();
@@ -94,6 +104,9 @@
//! Turn sensing of agent position on/off
void SetSenseMyPos(bool sense);
+ // turn sensing of lines on/off
+ void SetSenseLine(bool sense);
+
/** Turn noise off/on.
\param add_noise flag if noise should be used at all.
*/
@@ -162,6 +175,13 @@
*/
bool DynamicAxisPercept(boost::shared_ptr<oxygen::PredicateList> predList);
+ /** Percept lines in the world */
+ void SenseLine(oxygen::Predicate& predicate);
+
+ void SetupLines(TLineList& visibleLines);
+
+ bool checkVisuable(ObjectData& od) const;
+
/** Checks if the given object is occluded, seen from from my_pos */
bool CheckOcclusion(const salt::Vector3f& my_pos, const ObjectData& od) const;
@@ -172,6 +192,9 @@
boost::shared_ptr<BaseNode> node,
TObjectList& objectList) const;
+ void AddSense(oxygen::Predicate& predicate,
+ const TLineList& lineList) const;
+
/** applies noise to the setup ObjectData */
void ApplyNoise(ObjectData& od) const;
@@ -201,6 +224,9 @@
*/
bool mStaticSenseAxis;
+ /** flag if the lines can be sensed */
+ bool mSenseLine;
+
//! horizontal opening of the vision cone
unsigned int mHViewCone;
//! vertical opening of the vision cone
Modified: trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor_c.cpp
===================================================================
--- trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor_c.cpp 2010-03-28 08:27:22 UTC (rev 189)
+++ trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor_c.cpp 2010-03-30 11:33:03 UTC (rev 190)
@@ -150,6 +150,22 @@
return true;
}
+FUNCTION(RestrictedVisionPerceptor,setSenseLine)
+{
+ bool inSenseLine;
+
+ if (
+ (in.GetSize() != 1) ||
+ (! in.GetValue(in.begin(),inSenseLine))
+ )
+ {
+ return false;
+ }
+
+ obj->SetSenseLine(inSenseLine);
+ return true;
+}
+
void CLASS(RestrictedVisionPerceptor)::DefineClass()
{
DEFINE_BASECLASS(oxygen/Perceptor);
@@ -160,4 +176,5 @@
DEFINE_FUNCTION(setViewCones);
DEFINE_FUNCTION(setPanRange);
DEFINE_FUNCTION(setTiltRange);
+ DEFINE_FUNCTION(setSenseLine);
}
Modified: trunk/spark/lib/salt/CMakeLists.txt
===================================================================
--- trunk/spark/lib/salt/CMakeLists.txt 2010-03-28 08:27:22 UTC (rev 189)
+++ trunk/spark/lib/salt/CMakeLists.txt 2010-03-30 11:33:03 UTC (rev 190)
@@ -15,6 +15,8 @@
sharedlibrary.h
vector.h
salt_defines.h
+ line2.h
+ linesegment2.h
)
set(salt_LIB_SRCS
Added: trunk/spark/lib/salt/line2.h
===================================================================
--- trunk/spark/lib/salt/line2.h (rev 0)
+++ trunk/spark/lib/salt/line2.h 2010-03-30 11:33:03 UTC (rev 190)
@@ -0,0 +1,305 @@
+/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+ this file is part of rcssserver3D
+ Copyright (C) 2008,2009 SEU RoboCup Simulation Team, Southeast University , Nanjing, China.
+ Copyright (C) 2010 RoboCup Soccer Server 3D Maintenance Group
+ $Id$
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; version 2 of the License.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _LINE2_H
+#define _LINE2_H
+
+#include <limits>
+#include "tvector.h"
+
+namespace salt {
+
+template <typename DATATYPE>
+class TLine2
+{
+ // a line is defined by the formula: ay + bx + c = 0
+ DATATYPE mA;
+ DATATYPE mB;
+ DATATYPE mC;
+public:
+
+ /**
+ * create line by given coefficients
+ *
+ * @param a the a coefficients
+ * @param b the b coefficients
+ * @param c the c coefficients
+ */
+ TLine2(DATATYPE a = 1, DATATYPE b = 0, DATATYPE c = -1)
+ : mA(a), mB(b), mC(c)
+ {
+ }
+
+ /**
+ * create line from tow points
+ *
+ * @param pos1 the one point
+ * @param pos2 the other point
+ */
+ template <typename P_T>
+ TLine2(const P_T &pos1,
+ const P_T &pos2)
+ {
+ // 1*y + bx + c = 0 => y = -bx - c
+ // with -b the direction coefficient (or slope)
+ // and c = - y - bx
+ DATATYPE dTemp = pos2[0] - pos1[0]; // determine the slope
+ if (fabs(dTemp) < std::numeric_limits<DATATYPE>::epsilon() ) {
+ // ay + bx + c = 0 with vertical slope=> a = 0, b = 1
+ mA = 0.0;
+ mB = 1.0;
+ } else {
+ // y = (-b)x -c with -b the slope of the line
+ mA = 1.0;
+ mB = -(pos2[1] - pos1[1]) / dTemp;
+ }
+ // ay + bx + c = 0 ==> c = -a*y - b*x
+ mC = -mA * pos2.y() - mB * pos2.x();
+ }
+
+ /**
+ * create line from one point and an angle
+ *
+ * @param pos the point in line
+ * @param angle the sclope angle
+ */
+ template <typename P_T>
+ TLine2(const P_T &pos, DATATYPE angle)
+ {
+ // calculate point somewhat further in direction 'angle'
+ // and make line from these two points.
+ P_T pos2 = pos;
+ pos2.x() += cosDeg(angle);
+ pos2.y() += sinDeg(angle);
+ *this = TLine2(pos, pos2);
+ }
+
+ DATATYPE a() const
+ {
+ return mA;
+ }
+
+ DATATYPE b() const
+ {
+ return mB;
+ }
+
+ DATATYPE c() const
+ {
+ return mC;
+ }
+
+ DATATYPE& a()
+ {
+ return mA;
+ }
+
+ DATATYPE& b()
+ {
+ return mB;
+ }
+
+ DATATYPE& c()
+ {
+ return mC;
+ }
+
+ /**
+ * calculate the intersection of two lines
+ *
+ * @param line the other line
+ *
+ * @return the intersection
+ */
+ template <typename P_T>
+ bool Intersection(const TLine2 &line, P_T &p) const
+ {
+ DATATYPE L = line.a() * mB - mA * line.b();
+ if (abs(L) < std::numeric_limits<DATATYPE>::epsilon()) {
+ // lines are parallel, no intersection
+ return false;
+ }
+ if (mA == 0) {
+ // bx + c = 0 and a2*y + b2*x + c2 = 0 ==> x = -c/b
+ // calculate x using the current line
+ p.x() = -mC / mB; // and calculate the y using the second line
+ p.y() = line.YGivenX(p.x());
+ } else if (line.a() == 0) {
+ // ay + bx + c = 0 and b2*x + c2 = 0 ==> x = -c2/b2
+ p.x() = -line.c() / line.b();
+ // calculate x using
+ p.y() = YGivenX(p.x());
+ // 2nd line and calculate y using current line
+ } else {
+ // ay + bx + c = 0 and a2y + b2*x + c2 = 0
+ // y = (-b2/a2)x - c2/a2
+ // bx = -a*y - c => bx = -a*(-b2/a2)x -a*(-c2/a2) - c
+ // ==> a2*bx = a*b2*x + a*c2 - a2*c
+ // ==> x = (a*c2 - a2*c)/(a2*b - a*b2)
+ p.x() = (mA * line.c() - line.a() * mC) / L;
+ p.y() = YGivenX(p.x());
+ }
+ return true;
+ }
+
+ /**
+ * ay + bx + c = 0 -> y = (-b/a)x + (-c/a)
+ * tangent: y = (a/b)*x + C1 -> by - ax + C2 = 0 => C2 = ax - by
+ * with pos.y = y, pos.x = x
+ *
+ * @param pos the tangent intersection
+ *
+ * @return the tangent line of this line
+ */
+ template<typename P_T>
+ TLine2 TangentLine(const P_T &pos) const
+ {
+ return TLine2(mB, -mA, mA * pos.x() - mB * pos.y());
+ }
+
+ /**
+ * calculate the closest point of a given point to this line
+ *
+ * @param pos the given point
+ *
+ * @return the closest point
+ */
+ template<typename P_T>
+ P_T PointOnLineClosestTo(const P_T &pos) const
+ {
+ TLine2 l2 = TangentLine(pos); // get tangent line
+ // and intersection between the two lines
+ return Intersection(l2);
+ }
+
+ /**
+ * calculate the distance from a given point to this line
+ *
+ * @param pos the given point
+ *
+ * @return the distance from the pos to this line
+ */
+ template<typename P_T>
+ DATATYPE DistanceToPoint(const P_T &pos) const
+ {
+ return (PointOnLineClosestTo(pos) - pos).length();
+ }
+
+ /**
+ * calculate the Y value by given X value
+ *
+ * @param x the given x coordinate value
+ *
+ * @return the Y coordinate value
+ */
+ DATATYPE YGivenX(DATATYPE x) const
+ {
+ if (mA == 0) {
+ std::cerr << __FILE__ << __LINE__ << __FUNCTION__
+ << " Cannot calculate Y coordinate: " << std::endl;
+ return 0;
+ }
+ // ay + bx + c = 0 ==> y = -(b*x + c)/a
+ return -(mB * x + mC) / mA;
+ }
+
+ /**
+ * calculate the X value by given Y value
+ *
+ * @param y the given Y coordinate value
+ *
+ * @return the X coordinate value
+ */
+ DATATYPE XGivenY(DATATYPE y) const
+ {
+ if (mB == 0) {
+ std::cerr << __FILE__ << __LINE__ << __FUNCTION__
+ << "Cannot calculate X coordinate\n" << std::endl;
+ return 0;
+ }
+ // ay + bx + c = 0 ==> x = -(a*y + c)/b
+ return -(mA * y + mC) / mB;
+ }
+
+ /**
+ * calculate the location of point to the line
+ *
+ * @param p the given point
+ *
+ * @return +1 means upper the line
+ * 0 means on the line
+ * -1 means lower the line
+ */
+ template <typename P_T>
+ int Location(const P_T & p)const
+ {
+ return sign(mA * p.y() + mB * p.x() + mC);
+ }
+
+ /**
+ * calculate the ang for line
+ */
+ DATATYPE GetAngle()
+ {
+ return atan2(-mB, mA);
+ }
+
+ /**
+ * @param[in] pos1 first point
+ * @param pos2 second point
+ * @return
+ */
+ template<typename P_T>
+ static TLine2 MidperpendicularFromTwoPoints(const P_T &pos1, const P_T &pos2)
+ {
+ TLine2 l1(pos1, pos2);
+ P_T posMid = (pos1 + pos2)*0.5;
+ return l1.TangentLine(posMid);
+ }
+
+};
+
+/// dump
+
+template<typename DATATYPE>
+std::ostream & operator <<(std::ostream & os, TLine2<DATATYPE> l)
+{
+ DATATYPE a = l.a();
+ DATATYPE b = l.b();
+ DATATYPE c = l.c();
+ // ay + bx + c = 0 -> y = -b/a x - c/a
+ if (a == 0) {
+ os << "x = " << -c / b;
+ } else {
+ os << "y = ";
+ if (b != 0)
+ os << -b / a << "x ";
+ if (c > 0)
+ os << "- " << fabs(c / a);
+ else if (c < 0)
+ os << "+ " << fabs(c / a);
+ }
+ return os;
+}
+
+} // namespace salt
+#endif /* _LINE2_H */
+
Added: trunk/spark/lib/salt/linesegment2.h
===================================================================
--- trunk/spark/lib/salt/linesegment2.h (rev 0)
+++ trunk/spark/lib/salt/linesegment2.h 2010-03-30 11:33:03 UTC (rev 190)
@@ -0,0 +1,129 @@
+/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+ this file is part of rcssserver3D
+ Copyright (C) 2008,2009 SEU RoboCup Simulation Team, Southeast University , Nanjing, China.
+ Copyright (C) 2010 RoboCup Soccer Server 3D Maintenance Group
+ $Id$
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; version 2 of the License.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _LINESEGMENT2_H
+#define _LINESEGMENT2_H
+
+#include "line2.h"
+
+namespace salt {
+
+template <typename DATATYPE, typename P_T>
+class TLineSegment2 : public TLine2<DATATYPE>
+{
+ /// endpoints
+ P_T mP[2];
+public:
+
+ TLineSegment2()
+ {
+ }
+
+ TLineSegment2(const P_T & p0, const P_T & p1)
+ : TLine2<DATATYPE>(p0, p1)
+ {
+ mP[0] = p0;
+ mP[1] = p1;
+ }
+
+ const P_T & p0() const
+ {
+ return mP[0];
+ }
+
+ const P_T & p1() const
+ {
+ return mP[1];
+ }
+
+ bool IsBetween(const P_T & p) const
+ {
+ return p[0] <= std::max(mP[0][0], mP[1][0])
+ && p[0] >= std::min(mP[0][0], mP[1][0])
+ && p[1] <= std::max(mP[0][1], mP[1][1])
+ && p[1] >= std::min(mP[0][1], mP[1][1]);
+ }
+
+ DATATYPE Length() const
+ {
+ return ( mP[0] - mP[1]).Length();
+ }
+
+ bool IsContain(const P_T & p) const
+ {
+ return (p - mP[0]).Length() + (p - mP[1]).Length()
+ < Length() + std::numeric_limits<DATATYPE>::epsilon();
+ }
+
+ /**
+ * calculate the intersection of the segment and a given line
+ *
+ * @param line the given line
+ * @param intersection retrun the intersection
+ *
+ * @return if the segment and line have intersection
+ */
+ bool IntersectionWithLine(const TLine2<DATATYPE>& line,
+ P_T & intersection)const
+ {
+ if ( TLine2<DATATYPE>::Intersection(line, intersection) )
+ {
+ return IsBetween(intersection);
+ }
+ return false;
+ }
+
+ /**
+ * calculate the interseciton of two segment
+ *
+ * @param seg2 the other segment
+ * @param intersection return the intersection
+ *
+ * @return if the tow segments have intersection
+ */
+ bool Intersection(const TLineSegment2& seg2,
+ P_T & intersection)const
+ {
+ if (IntersectionWithLine(seg2, intersection)) {
+ return seg2.IsBetween(intersection);
+ }
+ return false;
+ }
+
+ /**
+ * if there is an intersection of two segment
+ *
+ * @param seg2 the other segment
+ *
+ * @return if the tow segments have intersection
+ */
+ bool IsIntersect(const TLineSegment2& seg2) const
+ {
+ P_T intersection;
+ return Intersection(seg2, intersection);
+ }
+};
+
+typedef TLineSegment2<float, Vector2f> LineSegment2f;
+
+} // namespace salt
+#endif /* _LINESEGMENT2_H */
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
