void Point3DTest::testConstructor() {
/*
* Test if all instances are created correctly.
*/
CPPUNIT_ASSERT((point000->getX() - 0) < maxTolerance);
CPPUNIT_ASSERT((point000->getY() - 0) < maxTolerance);
CPPUNIT_ASSERT((point000->getZ() - 0) < maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, point000->getX(), maxTolerance); //same as before but with CppUnit macro
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, point000->getY(), maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, point000->getZ(), maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, point111->getX(), maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, point111->getY(), maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, point111->getZ(), maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, pointMinus123->getX(), maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-2.0, pointMinus123->getY(), maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-3.0, pointMinus123->getZ(), maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(maxCoordValue, pointMax->getX(), maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(maxCoordValue, pointMax->getY(), maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(maxCoordValue, pointMax->getZ(), maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(minCoordValue, pointMin->getX(), maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(minCoordValue, pointMin->getY(), maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(minCoordValue, pointMin->getZ(), maxTolerance);
/* copy constructor */
Point3D *newPoint0;
newPoint0 = new Point3D(point111);
CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, newPoint0->getX(), maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, newPoint0->getY(), maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, newPoint0->getZ(), maxTolerance);
delete newPoint0;
Point3D newPoint1(point111);
CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, newPoint1.getX(), maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, newPoint1.getY(), maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, newPoint1.getZ(), maxTolerance);
}
WaypointList*
PathFinder::SmoothWaypointList( WaypointList* pWaypointList )
{
if (!pWaypointList)
return NULL;
if( pWaypointList->size() > 2 )
{
WORLD_POSITION newPoint0( 0.0f, 0.0f, 0.0f );
WORLD_POSITION newPoint1( 0.0f, 0.0f, 0.0f );
WaypointList oldList;
for( WaypointList::iterator i = pWaypointList->begin(); i != pWaypointList->end(); i++ )
{
oldList.push_back( *i );
}
pWaypointList->clear();
WORLD_POSITION p0, p1, p2, p3;
//First
WaypointList::iterator iter = oldList.begin();
p0 = p1 = p2 = *iter;
iter++;
p3 = *iter;
iter++;
const int divisions = 5;
while( iter != oldList.end() )
{
p0 = p1;
p1 = p2;
p2 = p3;
p3 = *iter;
for( int count=1; count<divisions+1; count++)
{
vec3 newPoint = CatmullRom( p0, p1, p2, p3, (float)count/(float)divisions );
if( pWaypointList->size() > 1 )
{
vec3 lastDir = newPoint1 - newPoint0;
vec3 curDir = newPoint - newPoint1;
lastDir.Normalize();
curDir.Normalize();
float dot = lastDir.Dot( curDir );
if( dot > 0.99f )
{
pWaypointList->pop_back();
}
}
newPoint0 = newPoint1;
newPoint1 = newPoint;
pWaypointList->push_back( newPoint );
}
iter++;
}
//Last
p0 = p1;
p1 = p2;
p2 = p3;
p3 = p3; //Redundant, but put here to make it clear what is intended
for( int count=1; count<divisions+1; count++)
{
WORLD_POSITION newPoint = CatmullRom( p0, p1, p2, p3, (float)count/(float)divisions );
if( pWaypointList->size() > 1 )
{
vec3 lastDir = newPoint1 - newPoint0;
vec3 curDir = newPoint - newPoint1;
lastDir.Normalize();
curDir.Normalize();
float dot = lastDir.Dot( curDir );
if( dot > 0.99f )
{
pWaypointList->pop_back();
}
}
newPoint0 = newPoint1;
newPoint1 = newPoint;
pWaypointList->push_back( newPoint );
}
}
return pWaypointList;
}
UInt32 NormalQuantifier::getSubIndex(Vec3f point,
Vec3f point1,
Vec3f point2,
Vec3f point3,
UInt32 number) const
{
int intersect = -1, index = 0;
Vec3f newPoint1(point1);
Vec3f newPoint2(point1);
Vec3f newPoint3(point2);
//newPoint1 = (point1+point2)/2; newPoint1.normalize();
//newPoint2 = (point1+point3)/2; newPoint2.normalize();
//newPoint3 = (point2+point3)/2; newPoint3.normalize();
newPoint1 += point2;
newPoint1 /= 2;
newPoint1.normalize();
newPoint2 += point3;
newPoint2 /=2;
newPoint2.normalize();
newPoint3 += point3;
newPoint3 /= 2;
newPoint3.normalize();
number--;
if (rayTriangle(point, point1, newPoint1, newPoint2))
intersect = 0;
else
if (rayTriangle(point, newPoint1, point2, newPoint3))
intersect = 1;
else
if ( rayTriangle(point, newPoint1, newPoint2, newPoint3))
intersect = 2;
else
if ( rayTriangle(point, newPoint2, newPoint3, point3))
intersect = 3;
if ( intersect >= 0 ) {
index = intersect;
if (number != 0) {
index = index << (number*2);
switch (intersect) {
case 0:
index += getSubIndex(point, point1, newPoint1, newPoint2, number);
break;
case 1:
index += getSubIndex(point, newPoint1, point2, newPoint3, number);
break;
case 2:
index += getSubIndex(point, newPoint1, newPoint2, newPoint3, number);
break;
case 3:
index += getSubIndex(point, newPoint2, newPoint3, point3, number);
break;
}
}
}
else {
FFATAL (( "Intersect < 0 in NormalQuantifier::getSubIndex()\n"));
}
return index;
}
