bool plTriUtils::IFastBarycentric(int iAx, const hsPoint3& p0, const hsPoint3& p1, const hsPoint3& p2, const hsPoint3&p, hsPoint3& out)
{
if( --iAx < 0 )
iAx = 2;
int jAx = iAx - 1;
if( jAx < 0 )
jAx = 2;
hsVector3 v02(&p0, &p2);
hsVector3 v12(&p1, &p2);
float totArea = v02[iAx] * v12[jAx] - v02[jAx] * v12[iAx];
hsAssert(totArea != 0, "Should have already filtered degerate tris and degenerate projection");
float invTotArea = 1.f / totArea;
hsVector3 vp2(&p, &p2);
float aArea = vp2[iAx] * v12[jAx] - vp2[jAx] * v12[iAx];
float bArea = v02[iAx] * vp2[jAx] - v02[jAx] * vp2[iAx];
out[0] = aArea * invTotArea;
out[1] = bArea * invTotArea;
out[2] = 1.f - out[0] - out[1];
return true;
}
bool plTriUtils::FastBarycentric(const hsPoint3& p0, const hsPoint3& p1, const hsPoint3& p2, const hsPoint3&p, hsPoint3& out)
{
hsVector3 v02(&p0, &p2);
hsVector3 v12(&p1, &p2);
int iAx = ISelectAxis(Cross(v12, v02));
if( iAx < 0 )
return false;
return IFastBarycentric(iAx, p0, p1, p2, p, out);
}
bool plTriUtils::ProjectOntoPlaneAlongVector(const hsPoint3& p0, const hsPoint3& p1, const hsPoint3& p2, const hsVector3& vec, hsPoint3& p)
{
hsVector3 v02(&p0, &p2);
hsVector3 v12(&p1, &p2);
hsVector3 norm = v12 % v02;
float dist = norm.InnerProduct(p0 - p);
return ProjectOntoPlaneAlongVector(norm, dist, vec, p);
}
plTriUtils::Bary plTriUtils::ComputeBarycentric(const hsPoint3& p0, const hsPoint3& p1, const hsPoint3& p2, const hsPoint3&p, hsPoint3& out)
{
hsVector3 v12(&p1, &p2);
hsVector3 v02(&p0, &p2);
hsVector3 norm = Cross(v12, v02);
float invLenSq12 = norm.MagnitudeSquared();
if( invLenSq12 < kAlmostZero )
return kDegenerateTri; // degenerate triangle
invLenSq12 = 1.f / invLenSq12;
hsVector3 vp2(&p, &p2);
hsVector3 v0 = Cross(v12, vp2);
hsVector3 v1 = Cross(vp2, v02);
return IComputeBarycentric(norm, invLenSq12, v0, v1, out);
}
int Poly::getObtuseCorner() const {
AVec2 v01(point[1]-point[0]),
v02(point[2]-point[0]),
v12(point[2]-point[1]);
if(v01.dot(v02) < 0)
return 0;
v01 *= -1;
if(v01.dot(v12) < 0)
return 1;
v12 *= -1;
v02 *= -1;
if(v02.dot(v12) < 0)
return 2;
return -1;
}
Real Pyramid5::volume () const
{
// The pyramid with a bilinear base has volume given by the
// formula in: "Calculation of the Volume of a General Hexahedron
// for Flow Predictions", AIAA Journal v.23, no.6, 1984, p.954-
Node * node0 = this->get_node(0);
Node * node1 = this->get_node(1);
Node * node2 = this->get_node(2);
Node * node3 = this->get_node(3);
Node * node4 = this->get_node(4);
// Construct Various edge and diagonal vectors
Point v40 ( *node0 - *node4 );
Point v13 ( *node3 - *node1 );
Point v02 ( *node2 - *node0 );
Point v03 ( *node3 - *node0 );
Point v01 ( *node1 - *node0 );
// Finally, ready to return the volume!
return (1./6.)*(v40*(v13.cross(v02))) + (1./12.)*(v02*(v01.cross(v03)));
}
bool plTriUtils::FastBarycentricProjection(const hsPoint3& p0, const hsPoint3& p1, const hsPoint3& p2, hsPoint3&p, hsPoint3& out)
{
hsVector3 v02(&p0, &p2);
hsVector3 v12(&p1, &p2);
hsVector3 norm = Cross(v12, v02);
float invLenSq12 = norm.MagnitudeSquared();
if( invLenSq12 < kAlmostZero )
return false; // degenerate triangle
invLenSq12 = 1.f / invLenSq12;
hsVector3 del(&p0, &p);
float delDotNormOverLenSq = del.InnerProduct(norm) * invLenSq12;
p += norm * delDotNormOverLenSq;
int iAx = ISelectAxis(norm);
hsAssert(iAx >= 0, "Should have already picked out degenerate tris");
return IFastBarycentric(iAx, p0, p1, p2, p, out);
}
/**
* @TODO: liczyc przeciecia odcinkow z kolkiem o promieniu aktora i z odcinkami miedzy nodami
*/
bool AIMap::insideObstacle(float x, float y) {
bool ret2 = false;
for(int i = 0; i < map_obstacles.size(); i++) {
bool ret = true;
std::vector<std::pair<float, float> > vertices = map_obstacles[i].vertex;
slm::vec2 v01(vertices.back().first - vertices.front().first,vertices.back().second - vertices.front().second);
slm::vec2 v02(vertices.back().first - x,vertices.back().second - y);
if(slm::dot(v01,v02) < 0) ret = false;
for(int j = 1; j < vertices.size(); j++) {
slm::vec2 v1(vertices[j-1].first - vertices[j].first,vertices[j-1].second - vertices[j].second);
slm::vec2 v2(vertices[j-1].first - x,vertices[j-1].second - y);
if(slm::dot(v1,v2) < 0) ret = false;
}
if(ret) ret2 = true;
}
return ret2;
}
bool Vec2::IsInClockwiseDirection( const Vec2& p0, const Vec2& p1, const Vec2& p2 )
{
Vec3 v02( p2 - p0 );
Vec3 v01( p1 - p0 );
return Vec3( v02 ).Cross( Vec3( v01 ) ).z >= 0.f;
}