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);
}
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);
}
}
}
