///////////////////////////////////////////////////////////////////////////

//

// Copyright (c) 2006, Industrial Light & Magic, a division of Lucas

// Digital Ltd. LLC

// 

// All rights reserved.

// 

// Redistribution and use in source and binary forms, with or without

// modification, are permitted provided that the following conditions are

// met:

// *       Redistributions of source code must retain the above copyright

// notice, this list of conditions and the following disclaimer.

// *       Redistributions in binary form must reproduce the above

// copyright notice, this list of conditions and the following disclaimer

// in the documentation and/or other materials provided with the

// distribution.

// *       Neither the name of Industrial Light & Magic nor the names of

// its contributors may be used to endorse or promote products derived

// from this software without specific prior written permission. 

// 

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR

// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT

// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,

// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT

// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,

// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY

// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT

// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE

// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

//

///////////////////////////////////////////////////////////////////////////

#include <testLineAlgo.h>

#include "ImathLineAlgo.h"

#include "ImathRandom.h"

#include <iostream>

#include <assert.h>

using namespace std;

using namespace IMATH_INTERNAL_NAMESPACE;

namespace {

void

testClosestPoints

    (const Line3f &line1,

     const Line3f &line2,

     bool returnValue,

     const V3f &point1,

     const V3f &point2)

{

    V3f p1;

    V3f p2;

    bool rv = closestPoints (line1, line2, p1, p2);

    assert (rv == returnValue);

    if (rv)

    {

	float e = 10 * limits<float>::epsilon();

	assert (point1.equalWithAbsError (p1, e));

	assert (point2.equalWithAbsError (p2, e));

    }

}

void

testClosestPoints ()

{

    cout << "closest points on two lines" << endl;

    cout << "  non-intersecting, non-parallel lines" << endl;

    testClosestPoints

	(Line3f (V3f ( 0, -1, -1), V3f ( 0,  1, -1)),

	 Line3f (V3f (-1,  0,  1), V3f ( 1,  0,  1)),

	 true,

	 V3f ( 0,  0, -1),

	 V3f ( 0,  0,  1));

    testClosestPoints

	(Line3f (V3f ( 2, -1, -1), V3f ( 2,  1, -1)),

	 Line3f (V3f (-1,  3,  1), V3f ( 1,  3,  1)),

	 true,

	 V3f ( 2,  3, -1),

	 V3f ( 2,  3,  1));

    cout << "  intersecting, non-parallel lines" << endl;

    testClosestPoints

	(Line3f (V3f ( 2, -1,  0), V3f ( 2,  1,  0)),

	 Line3f (V3f (-1,  3,  0), V3f ( 1,  3,  0)),

	 true,

	 V3f ( 2,  3,  0),

	 V3f ( 2,  3,  0));

    cout << "  parallel lines" << endl;

    testClosestPoints

	(Line3f (V3f ( 2, -1,  0), V3f ( 2,  1,  0)),

	 Line3f (V3f ( 2, -1,  1), V3f ( 2,  1,  1)),

	 false,

	 V3f ( 0,  0,  0),

	 V3f ( 0,  0,  0));

    testClosestPoints

	(Line3f (V3f ( 2, -1,  0), V3f ( 2,  1,  0)),

	 Line3f (V3f ( 2,  1,  1), V3f ( 2, -1,  1)),

	 false,

	 V3f ( 0,  0,  0),

	 V3f ( 0,  0,  0));

    cout << "  coincident lines" << endl;

    testClosestPoints

	(Line3f (V3f ( 2, -1,  0), V3f ( 2, -1,  1)),

	 Line3f (V3f ( 2, -1,  0), V3f ( 2, -1,  1)),

	 false,

	 V3f ( 0,  0,  0),

	 V3f ( 0,  0,  0));

    cout << "  random lines" << endl;

    Rand48 rand (7);

    for (int i = 0; i < 10000; ++i)

    {

	Line3f line1 (solidSphereRand<V3f> (rand) * 100.f,

		      solidSphereRand<V3f> (rand) * 100.f);

	Line3f line2 (solidSphereRand<V3f> (rand) * 100.f,

		      solidSphereRand<V3f> (rand) * 100.f);

	V3f point1;

	V3f point2;

	bool rv = closestPoints (line1, line2, point1, point2);

	if (rv)

	{

	    //

	    // We test if the line that connects point1 and point2

	    // is perpendicular to line1 and line2.  The numerical

	    // accuracy of point1 and point2 depends strongly on

	    // the relative directions of line1 and line2; accuracy

	    // degrades rather quickly if line1 and line2 become

	    // close to parallel.

	    //

	    float e = 2000 * limits<float>::epsilon();

	    float d = 1 - (line1.dir ^ line2.dir) * (line1.dir ^ line2.dir);

	    V3f n = point1 - point2;

	    assert (equalWithAbsError (0.0f, (line1.dir ^ n) * d, e));

	    assert (equalWithAbsError (0.0f, (line2.dir ^ n) * d, e));

	}

    }

}

void

testIntersect

    (const Line3f &line,

     const V3f &v0,

     const V3f &v1,

     const V3f &v2,

     const V3f &point,

     bool front,

     bool returnValue)

{

    V3f pt;

    V3f bary;

    bool fr;

    bool rv = intersect (line, v0, v1, v2, pt, bary, fr);

    assert (rv == returnValue);

    float e = 10 * limits<float>::epsilon();

    if (rv)

    {

	assert (front == fr);

	assert (pt.equalWithAbsError (point, e));

	V3f pt2 = v0 * bary.x + v1 * bary.y + v2 * bary.z;

	assert (pt.equalWithAbsError (pt2, e));

    }

}

void

testIntersect ()

{

    cout << "line-triangle intersection" << endl;

    cout << "  line-plane intersection inside triangle" << endl;

    testIntersect (Line3f (V3f (0, 0, -1), V3f (0, 0, 7)),

		   V3f (-4, -4, 7), V3f (4, -4, 7), V3f (0, 6, 7),

		   V3f (0, 0, 7),

		   true,

		   true);

    testIntersect (Line3f (V3f (0, 0, -1), V3f (-1, -2, 7)),

		   V3f (-4, -4, 7), V3f (4, -4, 7), V3f (0, 6, 7),

		   V3f (-1, -2, 7),

		   true,

		   true);

    testIntersect (Line3f (V3f (0, 0, -1), V3f (-1, 1, 7)),

		   V3f (-4, -4, 7), V3f (4, -4, 7), V3f (0, 6, 7),

		   V3f (-1, 1, 7),

		   true,

		   true);

    testIntersect (Line3f (V3f (0, 0, -1), V3f (-1, 1, 7)),

		   V3f (4, -4, 7), V3f (-4, -4, 7), V3f (0, 6, 7),

		   V3f (-1, 1, 7),

		   false,

		   true);

    testIntersect (Line3f (V3f (1, 1, 2), V3f (0, 0, 7)),

		   V3f (-4, -4, 7), V3f (4, -4, 7), V3f (0, 6, 7),

		   V3f (0, 0, 7),

		   true,

		   true);

    testIntersect (Line3f (V3f (2, 3, -5), V3f (-1, -2, 7)),

		   V3f (-4, -4, 7), V3f (4, -4, 7), V3f (0, 6, 7),

		   V3f (-1, -2, 7),

		   true,

		   true);

    testIntersect (Line3f (V3f (2, 8, -10), V3f (-1, 1, 7)),

		   V3f (-4, -4, 7), V3f (4, -4, 7), V3f (0, 6, 7),

		   V3f (-1, 1, 7),

		   true,

		   true);

    testIntersect (Line3f (V3f (-10, 2, -1), V3f (-1, 1, 7)),

		   V3f (4, -4, 7), V3f (-4, -4, 7), V3f (0, 6, 7),

		   V3f (-1, 1, 7),

		   false,

		   true);

    cout << "  line-plane intersection outside triangle" << endl;

    testIntersect (Line3f (V3f (0, 0, -1), V3f (4, 0, 7)),

		   V3f (-4, -4, 7), V3f (4, -4, 7), V3f (0, 6, 7),

		   V3f (0, 0, 0),

		   false,

		   false);

    testIntersect (Line3f (V3f (0, 0, -1), V3f (-4, 1, 7)),

		   V3f (-4, -4, 7), V3f (4, -4, 7), V3f (0, 6, 7),

		   V3f (0, 0, 0),

		   false,

		   false);

    testIntersect (Line3f (V3f (0, 0, -1), V3f (0, -5, 7)),

		   V3f (-4, -4, 7), V3f (4, -4, 7), V3f (0, 6, 7),

		   V3f (0, 0, 0),

		   false,

		   false);

    testIntersect (Line3f (V3f (0, 0, -1), V3f (0, -7, 7)),

		   V3f (-4, -4, 7), V3f (4, -4, 7), V3f (0, 6, 7),

		   V3f (0, 0, 0),

		   false,

		   false);

    cout << "  line parallel to triangle" << endl;

    testIntersect (Line3f (V3f (0, 0, -1), V3f (4, 0, -1)),

		   V3f (-4, -4, 7), V3f (4, -4, 7), V3f (0, 6, 7),

		   V3f (0, 0, 0),

		   false,

		   false);

    testIntersect (Line3f (V3f (0, 4, 7), V3f (4, 0, 7)),

		   V3f (-4, -4, 7), V3f (4, -4, 7), V3f (0, 6, 7),

		   V3f (0, 0, 0),

		   false,

		   false);

    cout << "  zero-area triangle" << endl;

    testIntersect (Line3f (V3f (2, 3, -5), V3f (-1, -2, 7)),

		   V3f (0, 6, 7), V3f (4, -4, 7), V3f (0, 6, 7),

		   V3f (0, 0, 0),

		   false,

		   false);

    testIntersect (Line3f (V3f (2, 3, -5), V3f (-1, -2, 7)),

		   V3f (-4, -4, 7), V3f (-4, -4, 7), V3f (0, 6, 7),

		   V3f (0, 0, 0),

		   false,

		   false);

    testIntersect (Line3f (V3f (2, 3, -5), V3f (-1, -2, 7)),

		   V3f (-4, -4, 7), V3f (0, 6, 7), V3f (0, 6, 7),

		   V3f (0, 0, 0),

		   false,

		   false);

    testIntersect (Line3f (V3f (2, 3, -5), V3f (-1, -2, 7)),

		   V3f (-4, -4, 7), V3f (-4, -4, 7), V3f (-4, -4, 7),

		   V3f (0, 0, 0),

		   false,

		   false);

    cout << "  random lines and triangles" << endl;

    Rand48 rand (8);

    for (int i = 0; i < 10000; ++i)

    {

	//

	// Generate a random triangle with non-zero area

	//

	V3f v0, v1, v2;

	V3f normal;

	do

	{

	    v0 = solidSphereRand<V3f> (rand);

	    v1 = solidSphereRand<V3f> (rand);

	    v2 = solidSphereRand<V3f> (rand);

	    normal = (v2 - v1) % (v1 - v0);

	}

	while (normal.length() < 0.01);

	{

	    //

	    // Generate a line that intersects inside the triangle

	    //

	    V3f b;

	    do

	    {

		b.x = rand.nextf (0.001, 0.999);

		b.y = rand.nextf (0.001, 0.999);

		b.z = 1 - b.x - b.y;

	    }

	    while (b.x + b.y > 0.999);

	    V3f p1 = v0 * b.x + v1 * b.y + v2 * b.z;

	    V3f p0;

	    do

	    {

		p0 = solidSphereRand<V3f> (rand);

	    }

	    while (abs (normal.normalized() ^ (p1 - p0).normalized()) < 0.1);

	    //

	    // Test for intersection

	    //

	    V3f point;

	    V3f bary;

	    bool front;

	    bool rv = intersect (Line3f (p0, p1),

	                         v0, v1, v2, point,

				 bary, front);

	    assert (rv == true);

	    float nd = abs (normal.normalized() ^ (p1 - p0).normalized());

	    float ep = 20 * limits<float>::epsilon() / nd;

	    assert (point.equalWithAbsError (p1, ep));

	}

	{

	    //

	    // Generate a line that intersects the triangle's plane

	    // but outside the triangle

	    //

	    V3f b;

	    do

	    {

		b.x = rand.nextf (-3, 3);

		b.y = rand.nextf (-3, 3);

		b.z = 1 - b.x - b.y;

	    }

	    while (b.x > -0.001 && b.y > -0.001 && b.z > -0.001);

	    V3f p1 = v0 * b.x + v1 * b.y + v2 * b.z;

	    V3f p0;

	    int j = 0;

	    do

	    {

		p0 = solidSphereRand<V3f> (rand) * 10;

	    }

	    while (abs (normal.normalized() ^ (p1 - p0).normalized()) < 0.1);

	    //

	    // Test for intersection

	    //

	    V3f point;

	    V3f bary;

	    bool front;

	    bool rv = intersect (Line3f (p0, p1),

	                         v0, v1, v2, point,

				 bary, front);

	    assert (rv == false);

	}

    }

}

} // namespace

void

testLineAlgo ()

{

    cout << "Testing line algorithms" << endl;

    testClosestPoints();

    testIntersect();

    cout << "ok\n" << endl;

}