Show kpathfollower.cpp syntax highlighted
/*
Copyright (C) 2003, 2004, 2005 by Luca Cappa
Written by Luca Cappa groton@users.sourceforge.net
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Library General Public
License as published by the Free Software Foundation; either
version 2 of the License, or (at your option) any later version.
This library 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
Library General Public License for more details.
You should have received a copy of the GNU Library General Public
License along with this library; if not, write to the Free
Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "cssysdef.h"
#include "csutil/sysfunc.h"
#include "csutil/cscolor.h"
#include "csutil/cmdhelp.h"
#include "csutil/cspmeter.h"
#include "csutil/csstring.h"
#include "csutil/scfstr.h"
#include "csutil/flags.h"
#include "csutil/xmltiny.h"
#include "csutil/dirtyaccessarray.h"
#include "cstool/csview.h"
#include "cstool/initapp.h"
#include "cstool/collider.h"
#include "csutil/array.h"
#include "iutil/vfs.h"
#include "iutil/eventq.h"
#include "iutil/event.h"
#include "iutil/objreg.h"
#include "iutil/csinput.h"
#include "iutil/virtclk.h"
#include "iutil/plugin.h"
#include "iutil/string.h"
#include "iengine/sector.h"
#include "iengine/engine.h"
#include "iengine/camera.h"
#include "iengine/light.h"
#include "iengine/texture.h"
#include "iengine/mesh.h"
#include "iengine/movable.h"
#include "iengine/material.h"
#include "iengine/viscull.h"
#include "imesh/thing.h"
#include "imesh/object.h"
#include "imesh/sprite3d.h"
#include "imesh/ball.h"
#include "ivideo/graph3d.h"
#include "ivideo/graph2d.h"
#include "ivideo/natwin.h"
#include "ivideo/txtmgr.h"
#include "ivideo/texture.h"
#include "ivideo/material.h"
#include "ivideo/fontserv.h"
#include "igraphic/imageio.h"
#include "imap/loader.h"
#include "ivaria/reporter.h"
#include "ivaria/stdrep.h"
#include "ivaria/conout.h"
#include "ivaria/reporter.h"
#include "ivaria/stdrep.h"
#include "ivaria/collider.h"
#include "csgeom/quaterni.h"
#include "csgeom/transfrm.h"
#include "csgeom/math3d_d.h"
#include "csgeom/math3d.h"
#include "csgeom/path.h"
#include "igeom/polymesh.h"
#include "igeom/objmodel.h"
#include "iaws/aws.h"
#include "iaws/awscnvs.h"
#include "csutil/array.h"
#include "ske.h"
#include "klandmark.h"
#include "ksprite3d.h"
#include "kobject3d.h"
#include "kpath.h"
#include "ksegment3.h"
#include "kpathfollower.h"
KPathFollower::KPathFollower (KPath* p_path,
const csVector3& p_initialPosition):
m_path (p_path), m_mistakenMeters (0.0f)
{
m_sequenceNumber = 0;
m_path->GetPoints (m_points);
//
//Copy the initial position.
m_lastPosition = p_initialPosition;
m_initPosition = p_initialPosition;
//
//Fill in the array of KSegment3.
size_t i = 1;
for (;i < m_points.Length (); i++)
m_segs.Push (KSegment3 (m_points[i - 1], m_points[i], i));
//Build an array of pointers.
m_segsFast = new KSegment3* [m_segs.Length ()];
for (i = 0;i < m_segs.Length (); i++)
m_segsFast[i] = & (m_segs[i]);
SetMetersTolerance (5.f);
}
KPathFollower::~KPathFollower ()
{
delete m_segsFast;
}
KPathFollower::KPathFollower (const KPathFollower & p_pF):
m_points (p_pF.m_points)
{
m_sequenceNumber = p_pF.m_sequenceNumber;
m_path = p_pF.m_path;
m_mistakenMeters = p_pF.m_mistakenMeters;
m_lastPosition = p_pF.m_lastPosition;
//
//Fill in the array of KSegment3.
size_t i = 1;
for (;i < m_points.Length (); i++)
m_segs.Push (KSegment3 (m_points[i - 1], m_points[i], i));
//Build an array of pointers.
m_segsFast = new KSegment3* [m_segs.Length ()];
for (i = 0; i < m_segs.Length (); i++)
m_segsFast[i] = & (m_segs[i]);
}
//This s_referencePoint is used VERY important for
//the ordering, since all segments are ordered
//on the distance to this point.
static csVector3 s_referencePoint;
static int Compare (const void* p_s1, const void* p_s2)
{
const KSegment3** l_s1 = (const KSegment3**)p_s1;
const KSegment3** l_s2 = (const KSegment3**)p_s2;
csVector3 l_crap;//??
float l_dist1 = PointSegmentDistance (s_referencePoint, **l_s1,
l_crap);
float l_dist2 = PointSegmentDistance (s_referencePoint, **l_s2,
l_crap);
if (l_dist1 < l_dist2)
return -1;
else
return 1;
}
bool KPathFollower::IsLastSegmentBeingTouched ()
{
return (m_sequenceNumber == m_segs.Length ());
}
void KPathFollower::AddPosition (csVector3& p_pos,/* csVector3& p_nor,*/
csVector3& p_intersection, float p_secondsElapsed)
{
//
//Find the closer and "touchable" segment following the right order
//(cfr. m_sequenceNumber).
//Order segment by their distance from the actual position.
s_referencePoint = p_pos;//s_fixedPoint is used for ordering (qsorting).
qsort (m_segsFast, m_segs.Length (), sizeof (KSegment3*), Compare);
csVector3 l_nearestPoint;
size_t i = 0;
csVector3 l_crap;
for (;i < m_segs.Length (); i++)
{
if (m_sequenceNumber + 1 < m_segsFast[i]->m_sequenceNumber)
continue;//the actual segsFast[i] is too much forward in the path (i.e. we
//have to "touch" the segments in ordering from 0 to end sequentially).
if (m_sequenceNumber > m_segsFast[i]->m_sequenceNumber)
continue;//the actual segsFast[i] is too much backward in the path (i.e. we
//have to "touch" the segments in ordering from 0 to end sequentially).
PointSegmentDistance (p_pos, *m_segsFast[i], p_intersection);
iVisibilityCuller* l_vC = g_ske->m_world->GetVisibilityCuller ();
csRef<iVisibilityObjectIterator> l_vOI = l_vC->IntersectSegment
(p_pos, p_intersection);
bool l_thingTouched = false;
while (l_vOI->HasNext ())
{
iVisibilityObject* l_vO = l_vOI->Next ();
iMeshWrapper* l_mW = l_vO->GetMeshWrapper ();
csRef<iThingState> l_tS = SCF_QUERY_INTERFACE (l_mW->GetMeshObject (),
iThingState);
if (l_tS != NULL)
{
l_thingTouched = true;
break;
}//if
}//while
if (l_thingTouched == false)
{
if (m_sequenceNumber + 1 == m_segsFast[i]->m_sequenceNumber)
m_sequenceNumber++;//Move forward the sequence, since we touched the next one.
break;
}//if
}//for
//If we cannot touch any segment we are not following the path,
//so return wrong way after 2 secs!
static float s_secElapsed = 0;//Time passed without touching any segment!
if (i >= m_segs.Length ())
{
s_secElapsed += p_secondsElapsed ;
if (s_secElapsed > 2.0f)
m_mistakenMeters = 2.1f;
}//if
else
{
//The secElapsed is zeroed!
s_secElapsed = 0;
//At this point we have found the closer and touchable segment, stored
//in m_segsFast[i].
//
//Test distance from the path (max 5 meters)
csVector3 l_unused;
float l_dist;
l_dist = PointSegmentDistance (p_pos, *m_segsFast[i], l_unused);
if (l_dist >= GetMetersTolerance ())
m_mistakenMeters = GetMetersTolerance ();
else
{
//
//Test for normals and add the space walked in the wrong direction to
//m_mistakenMeters.
csVector3 l_movement = p_pos - m_lastPosition;
csVector3 l_segDirection = m_segsFast[i]->End () -
m_segsFast[i]->Start ();
if (l_movement.Norm () > SMALL_EPSILON)//if moving.
{
if ((l_movement * l_segDirection) < 0)//If the versus are opposed.
m_mistakenMeters += l_movement.Norm ();
else
m_mistakenMeters = 0;
}//if
}//else
}//else
//
//Store last position.
m_lastPosition = p_pos;
//??m_lastNormal = p_nor;
}
bool KPathFollower::IsPathLeft ()
{
return (m_mistakenMeters >= GetMetersTolerance ());
}
void KPathFollower::Reset ()
{
m_sequenceNumber = 0;
m_mistakenMeters = 0;
m_lastPosition = m_initPosition;
}
See more files for this project here