bool LodOutsideMarker::isInsideTriangle(const Vector3& ptarget, const CHTriangle& tri){

    // The idea is that if all angle is smaller to that point, than it should be inside.
    // NOTE: We assume that the vertex is on the triangle plane!

    const Vector3& p0 = tri.vertex[0]->position;
    const Vector3& p1 = tri.vertex[1]->position;
    const Vector3& p2 = tri.vertex[2]->position;
    const Vector3& n = tri.normal;

    bool b0, b1, b2;

    // It should not contain malformed triangles!
    assert(!isSamePosition(p0, p1) && !isSamePosition(p1, p2) && !isSamePosition(p2, p0));

    {
        Real d0 = pointToLineDir(ptarget, p0, p1, p2, n);
        if (std::abs(d0) <= mEpsilon){
            //Vertex is on the edge, so we need to compare length.
            return isInsideLine(ptarget, p0, p1);
        }
        b0 = d0 < 0.0f;
    }

    {
        Real d1 = pointToLineDir(ptarget, p1, p2, p0, n);
        if (std::abs(d1) <= mEpsilon){
            //Vertex is on the edge, so we need to compare length.
            return isInsideLine(ptarget, p1, p2);
        }
        b1 = d1 < 0.0f;
    }

    if (b0 != b1) {
        return false;
    }

    {
        Real d2 = pointToLineDir(ptarget, p2, p0, p1, n);
        if (std::abs(d2) <= mEpsilon){
            //Vertex is on the edge, so we need to compare length.
            return isInsideLine(ptarget, p2, p0);
        }
        b2 = d2 < 0.0f;
    }

    return (b1 == b2);
}
Esempio n. 2
0
task main()
{
	manualCallibColor();
	resetGyro(Gyro);
	seekBounds();
	faceHeading(forwardsHeading);
	while(true) {

		if(isInsideLine()) {
			setMotorSpeed(LeftMotor, 30);
			setMotorSpeed(RightMotor, 30);
		}
		else {
			seekBounds();
			faceHeading(forwardsHeading);
			eraseDisplay();
			displayBigTextLine(0, "Head: %d", forwardsHeading);
		}


	}


}