LineSegment Polygon::Edge2D(int i) const
{
if (p.empty())
return LineSegment(vec::nan, vec::nan);
if (p.size() == 1)
return LineSegment(vec::zero, vec::zero);
return LineSegment(POINT_VEC(MapTo2D(i), 0), POINT_VEC(MapTo2D((i+1)%p.size()), 0));
}
MATH_IGNORE_UNUSED_VARS_WARNING
UNIQUE_TEST(TrickyAABBCapsuleNoIntersect)
{
Capsule a(POINT_VEC(-37.3521881f,-61.0987396f,77.0996475f), POINT_VEC(46.2122498f,-61.2913399f,15.9034805f) ,53.990406f);
AABB b(POINT_VEC(51.6529007f,-28.0629959f,65.9745636f),POINT_VEC(59.8043518f,-18.9434891f,69.0897827f));
for(int i = 0; i < 8; ++i)
LOGI("Distance: %f", a.Distance(b.CornerPoint(i)));
LOGI("D: %f", b.Distance(a.Centroid()));
LOGI("D: %f", b.Distance(a.SphereA()));
LOGI("D: %f", b.Distance(a.SphereB()));
assert(!a.Intersects(b));
}
bool Plane::Intersects(const Plane &plane, Line *outLine) const
{
vec perp = normal.Perpendicular(plane.normal);//vec::Perpendicular Cross(normal, plane.normal);
float3x3 m;
m.SetRow(0, DIR_TO_FLOAT3(normal));
m.SetRow(1, DIR_TO_FLOAT3(plane.normal));
m.SetRow(2, DIR_TO_FLOAT3(perp)); // This is arbitrarily chosen, to produce m invertible.
float3 intersectionPos;
bool success = m.SolveAxb(float3(d, plane.d, 0.f),intersectionPos);
if (!success) // Inverse failed, so the planes must be parallel.
{
float normalDir = Dot(normal,plane.normal);
if ((normalDir > 0.f && EqualAbs(d, plane.d)) || (normalDir < 0.f && EqualAbs(d, -plane.d)))
{
if (outLine)
*outLine = Line(normal*d, plane.normal.Perpendicular());
return true;
}
else
return false;
}
if (outLine)
*outLine = Line(POINT_VEC(intersectionPos), perp.Normalized());
return true;
}
vec AABB::ExtremePoint(const vec &direction) const
{
float3 pt;
pt.x = (direction.x >= 0.f ? maxPoint.x : minPoint.x);
pt.y = (direction.y >= 0.f ? maxPoint.y : minPoint.y);
pt.z = (direction.z >= 0.f ? maxPoint.z : minPoint.z);
return POINT_VEC(pt);
}
vec AABB::PointInside(float x, float y, float z) const
{
assume(0.f <= x && x <= 1.f);
assume(0.f <= y && y <= 1.f);
assume(0.f <= z && z <= 1.f);
vec d = maxPoint - minPoint;
return minPoint + d.Mul(POINT_VEC(x, y, z));
}
bool Polygon::Intersects(const LineSegment &lineSegment) const
{
Plane plane = PlaneCCW();
// Compute line-plane intersection (unroll Plane::IntersectLinePlane())
float denom = Dot(plane.normal, lineSegment.b - lineSegment.a);
if (Abs(denom) < 1e-4f) // The plane of the polygon and the line are planar? Do the test in 2D.
return Intersects2D(LineSegment(POINT_VEC(MapTo2D(lineSegment.a), 0), POINT_VEC(MapTo2D(lineSegment.b), 0)));
// The line segment properly intersects the plane of the polygon, so there is exactly one
// point of intersection between the plane of the polygon and the line segment. Test that intersection point against
// the line segment end points.
float t = (plane.d - Dot(plane.normal, lineSegment.a)) / denom;
if (t < 0.f || t > 1.f)
return false;
return Contains(lineSegment.GetPoint(t));
}
void Frustum::DeserializeFromXml(TiXmlElement *e)
{
type = StrCaseEq(e->Attribute("orthographic"), "true") ? OrthographicFrustum : PerspectiveFrustum;
pos = POINT_VEC(float3::FromString(e->Attribute("pos")));
front = DIR_VEC(float3::FromString(e->Attribute("front")));
up = DIR_VEC(float3::FromString(e->Attribute("up")));
e->QueryFloatAttribute("nearPlaneDistance", &nearPlaneDistance);
e->QueryFloatAttribute("farPlaneDistance", &farPlaneDistance);
e->QueryFloatAttribute("horizontalFov", &horizontalFov);
e->QueryFloatAttribute("verticalFov", &verticalFov);
}
void Frustum::SetWorldMatrix(const float3x4 &worldTransform)
{
pos = POINT_VEC(worldTransform.TranslatePart());
if (handedness == FrustumRightHanded)
front = -DIR_VEC(worldTransform.Col(2)); // The camera looks towards -Z axis of the given transform.
else
front = DIR_VEC(worldTransform.Col(2)); // The camera looks towards +Z axis of the given transform.
up = DIR_VEC(worldTransform.Col(1)); // The camera up points towards +Y of the given transform.
assume(pos.IsFinite());
assume(front.IsNormalized());
assume(up.IsNormalized());
assume(worldTransform.IsColOrthogonal3()); // Front and up must be orthogonal to each other.
assume(EqualAbs(worldTransform.Determinant(), 1.f)); // The matrix cannot contain mirroring.
}
bool Polygon::Intersects2D(const LineSegment &localSpaceLineSegment) const
{
if (p.size() < 3)
return false;
const vec basisU = BasisU();
const vec basisV = BasisV();
const vec origin = p[0];
LineSegment edge;
edge.a = POINT_VEC(Dot(p.back(), basisU), Dot(p.back(), basisV), 0); // map to 2D
for (int i = 0; i < (int)p.size(); ++i)
{
edge.b = POINT_VEC(Dot(p[i], basisU), Dot(p[i], basisV), 0); // map to 2D
if (edge.Intersects(localSpaceLineSegment))
return true;
edge.a = edge.b;
}
// The line segment did not intersect with any of the polygon edges, so either the whole line segment is inside
// the polygon, or it is fully outside the polygon. Test one point of the line segment to determine which.
return Contains(MapFrom2D(localSpaceLineSegment.a.xy()));
}
vec AABB::CornerPoint(int cornerIndex) const
{
assume(0 <= cornerIndex && cornerIndex <= 7);
switch(cornerIndex)
{
default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
case 0: return POINT_VEC(float3(minPoint.x, minPoint.y, minPoint.z));
case 1: return POINT_VEC(float3(minPoint.x, minPoint.y, maxPoint.z));
case 2: return POINT_VEC(float3(minPoint.x, maxPoint.y, minPoint.z));
case 3: return POINT_VEC(float3(minPoint.x, maxPoint.y, maxPoint.z));
case 4: return POINT_VEC(float3(maxPoint.x, minPoint.y, minPoint.z));
case 5: return POINT_VEC(float3(maxPoint.x, minPoint.y, maxPoint.z));
case 6: return POINT_VEC(float3(maxPoint.x, maxPoint.y, minPoint.z));
case 7: return POINT_VEC(float3(maxPoint.x, maxPoint.y, maxPoint.z));
}
}
bool Plane::Intersects(const Plane &plane, const Plane &plane2, Line *outLine, vec *outPoint) const
{
Line dummy;
if (!outLine)
outLine = &dummy;
// First check all planes for parallel pairs.
if (this->IsParallel(plane) || this->IsParallel(plane2))
{
if (EqualAbs(d, plane.d) || EqualAbs(d, plane2.d))
{
bool intersect = plane.Intersects(plane2, outLine);
if (intersect && outPoint)
*outPoint = outLine->GetPoint(0);
return intersect;
}
else
return false;
}
if (plane.IsParallel(plane2))
{
if (EqualAbs(plane.d, plane2.d))
{
bool intersect = this->Intersects(plane, outLine);
if (intersect && outPoint)
*outPoint = outLine->GetPoint(0);
return intersect;
}
else
return false;
}
// All planes point to different directions.
float3x3 m;
m.SetRow(0, DIR_TO_FLOAT3(normal));
m.SetRow(1, DIR_TO_FLOAT3(plane.normal));
m.SetRow(2, DIR_TO_FLOAT3(plane2.normal));
float3 intersectionPos;
bool success = m.SolveAxb(float3(d, plane.d, plane2.d), intersectionPos);
if (!success)
return false;
if (outPoint)
*outPoint = POINT_VEC(intersectionPos);
return true;
}
Polygon Polygon::FromString(const char *str, const char **outEndStr)
{
MATH_SKIP_WORD(str, "Polygon");
MATH_SKIP_WORD(str, "(");
Polygon p;
while(*str == '(' || *str == ',')
{
MATH_SKIP_WORD(str, ",");
float3 pt = float3::FromString(str, &str);
p.p.push_back(POINT_VEC(pt));
}
MATH_SKIP_WORD(str, ")");
if (outEndStr)
*outEndStr = str;
return p;
}
vec AABB::FaceCenterPoint(int faceIndex) const
{
assume(0 <= faceIndex && faceIndex <= 5);
vec center = (minPoint + maxPoint) * 0.5f;
switch(faceIndex)
{
default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
case 0: return POINT_VEC(minPoint.x, center.y, center.z);
case 1: return POINT_VEC(maxPoint.x, center.y, center.z);
case 2: return POINT_VEC(center.x, minPoint.y, center.z);
case 3: return POINT_VEC(center.x, maxPoint.y, center.z);
case 4: return POINT_VEC(center.x, center.y, minPoint.z);
case 5: return POINT_VEC(center.x, center.y, maxPoint.z);
}
}
vec AABB::FacePoint(int faceIndex, float u, float v) const
{
assume(0 <= faceIndex && faceIndex <= 5);
assume(0 <= u && u <= 1.f);
assume(0 <= v && v <= 1.f);
vec d = maxPoint - minPoint;
switch(faceIndex)
{
default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
case 0: return POINT_VEC(minPoint.x, minPoint.y + u * d.y, minPoint.z + v * d.z);
case 1: return POINT_VEC(maxPoint.x, minPoint.y + u * d.y, minPoint.z + v * d.z);
case 2: return POINT_VEC(minPoint.x + u * d.x, minPoint.y, minPoint.z + v * d.z);
case 3: return POINT_VEC(minPoint.x + u * d.x, maxPoint.y, minPoint.z + v * d.z);
case 4: return POINT_VEC(minPoint.x + u * d.x, minPoint.y + v * d.y, minPoint.z);
case 5: return POINT_VEC(minPoint.x + u * d.x, minPoint.y + v * d.y, maxPoint.z);
}
}
int Sphere::Triangulate(vec *outPos, vec *outNormal, float2 *outUV, int numVertices, bool ccwIsFrontFacing) const
{
assume(outPos);
assume(numVertices >= 24 && "At minimum, sphere triangulation will contain at least 8 triangles, which is 24 vertices, but fewer were specified!");
assume(numVertices % 3 == 0 && "Warning:: The size of output should be divisible by 3 (each triangle takes up 3 vertices!)");
#ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
if (!outPos)
return 0;
#endif
assume(this->r > 0.f);
if (numVertices < 24)
return 0;
#ifdef MATH_ENABLE_STL_SUPPORT
TriangleArray temp;
#else
Array<Triangle> temp;
#endif
// Start subdividing from a diamond shape.
vec xp = POINT_VEC(r,0,0);
vec xn = POINT_VEC(-r, 0, 0);
vec yp = POINT_VEC(0, r, 0);
vec yn = POINT_VEC(0, -r, 0);
vec zp = POINT_VEC(0, 0, r);
vec zn = POINT_VEC(0, 0, -r);
if (ccwIsFrontFacing)
{
temp.push_back(Triangle(yp,xp,zp));
temp.push_back(Triangle(xp,yp,zn));
temp.push_back(Triangle(yn,zp,xp));
temp.push_back(Triangle(yn,xp,zn));
temp.push_back(Triangle(zp,xn,yp));
temp.push_back(Triangle(yp,xn,zn));
temp.push_back(Triangle(yn,xn,zp));
temp.push_back(Triangle(xn,yn,zn));
}
else
{
temp.push_back(Triangle(yp,zp,xp));
temp.push_back(Triangle(xp,zn,yp));
temp.push_back(Triangle(yn,xp,zp));
temp.push_back(Triangle(yn,zn,xp));
temp.push_back(Triangle(zp,yp,xn));
temp.push_back(Triangle(yp,zn,xn));
temp.push_back(Triangle(yn,zp,xn));
temp.push_back(Triangle(xn,zn,yn));
}
int oldEnd = 0;
while(((int)temp.size()-oldEnd+3)*3 <= numVertices)
{
Triangle cur = temp[oldEnd];
vec a = ((cur.a + cur.b) * 0.5f).ScaledToLength(this->r);
vec b = ((cur.a + cur.c) * 0.5f).ScaledToLength(this->r);
vec c = ((cur.b + cur.c) * 0.5f).ScaledToLength(this->r);
temp.push_back(Triangle(cur.a, a, b));
temp.push_back(Triangle(cur.b, c, a));
temp.push_back(Triangle(cur.c, b, c));
temp.push_back(Triangle(a, c, b));
++oldEnd;
}
// Check that we really did tessellate as many new triangles as possible.
assert(((int)temp.size()-oldEnd)*3 <= numVertices && ((int)temp.size()-oldEnd)*3 + 9 > numVertices);
for(size_t i = oldEnd, j = 0; i < temp.size(); ++i, ++j)
{
outPos[3*j] = this->pos + TRIANGLE(temp[i]).a;
outPos[3*j+1] = this->pos + TRIANGLE(temp[i]).b;
outPos[3*j+2] = this->pos + TRIANGLE(temp[i]).c;
}
if (outNormal)
for(size_t i = oldEnd, j = 0; i < temp.size(); ++i, ++j)
{
outNormal[3*j] = TRIANGLE(temp[i]).a.Normalized();
outNormal[3*j+1] = TRIANGLE(temp[i]).b.Normalized();
outNormal[3*j+2] = TRIANGLE(temp[i]).c.Normalized();
}
if (outUV)
for(size_t i = oldEnd, j = 0; i < temp.size(); ++i, ++j)
{
outUV[3*j] = float2(atan2(TRIANGLE(temp[i]).a.y, TRIANGLE(temp[i]).a.x) / (2.f * 3.141592654f) + 0.5f, (TRIANGLE(temp[i]).a.z + r) / (2.f * r));
outUV[3*j+1] = float2(atan2(TRIANGLE(temp[i]).b.y, TRIANGLE(temp[i]).b.x) / (2.f * 3.141592654f) + 0.5f, (TRIANGLE(temp[i]).b.z + r) / (2.f * r));
outUV[3*j+2] = float2(atan2(TRIANGLE(temp[i]).c.y, TRIANGLE(temp[i]).c.x) / (2.f * 3.141592654f) + 0.5f, (TRIANGLE(temp[i]).c.z + r) / (2.f * r));
}
return ((int)temp.size() - oldEnd) * 3;
}
void Sphere::SetNegativeInfinity()
{
pos = POINT_VEC(0,0,0);
r = -FLOAT_INF;
}
void AABB::Scale(const vec ¢erPoint, const vec &scaleFactor)
{
float3x4 transform = float3x4::Scale(DIR_TO_FLOAT3(scaleFactor), POINT_TO_FLOAT3(centerPoint)); ///\todo mat
minPoint = POINT_VEC(transform.MulPos(POINT_TO_FLOAT3(minPoint))); ///\todo mat
maxPoint = POINT_VEC(transform.MulPos(POINT_TO_FLOAT3(maxPoint))); ///\todo mat
}
vec AABB::PointOnEdge(int edgeIndex, float u) const
{
assume(0 <= edgeIndex && edgeIndex <= 11);
assume(0 <= u && u <= 1.f);
vec d = maxPoint - minPoint;
switch(edgeIndex)
{
default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
case 0: return POINT_VEC(minPoint.x, minPoint.y, minPoint.z + u * d.z);
case 1: return POINT_VEC(minPoint.x, maxPoint.y, minPoint.z + u * d.z);
case 2: return POINT_VEC(maxPoint.x, minPoint.y, minPoint.z + u * d.z);
case 3: return POINT_VEC(maxPoint.x, maxPoint.y, minPoint.z + u * d.z);
case 4: return POINT_VEC(minPoint.x, minPoint.y + u * d.y, minPoint.z);
case 5: return POINT_VEC(maxPoint.x, minPoint.y + u * d.y, minPoint.z);
case 6: return POINT_VEC(minPoint.x, minPoint.y + u * d.y, maxPoint.z);
case 7: return POINT_VEC(maxPoint.x, minPoint.y + u * d.y, maxPoint.z);
case 8: return POINT_VEC(minPoint.x + u * d.x, minPoint.y, minPoint.z);
case 9: return POINT_VEC(minPoint.x + u * d.x, minPoint.y, maxPoint.z);
case 10: return POINT_VEC(minPoint.x + u * d.x, maxPoint.y, minPoint.z);
case 11: return POINT_VEC(minPoint.x + u * d.x, maxPoint.y, maxPoint.z);
}
}
vec AABB::ExtremePoint(const vec &direction) const
{
return POINT_VEC((direction.x >= 0.f ? maxPoint.x : minPoint.x),
(direction.y >= 0.f ? maxPoint.y : minPoint.y),
(direction.z >= 0.f ? maxPoint.z : minPoint.z));
}
LineSegment AABB::Edge(int edgeIndex) const
{
assume(0 <= edgeIndex && edgeIndex <= 11);
switch(edgeIndex)
{
default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
/* For documentation, here's the segments that are returned:
case 0: return LineSegment(CornerPoint(0), CornerPoint(1));
case 1: return LineSegment(CornerPoint(0), CornerPoint(2));
case 2: return LineSegment(CornerPoint(0), CornerPoint(4));
case 3: return LineSegment(CornerPoint(1), CornerPoint(3));
case 4: return LineSegment(CornerPoint(1), CornerPoint(5));
case 5: return LineSegment(CornerPoint(2), CornerPoint(3));
case 6: return LineSegment(CornerPoint(2), CornerPoint(6));
case 7: return LineSegment(CornerPoint(3), CornerPoint(7));
case 8: return LineSegment(CornerPoint(4), CornerPoint(5));
case 9: return LineSegment(CornerPoint(4), CornerPoint(6));
case 10: return LineSegment(CornerPoint(5), CornerPoint(7));
case 11: return LineSegment(CornerPoint(6), CornerPoint(7));
*/
// Force-optimize to avoid calling to CornerPoint for another switch-case statement.
case 0: return LineSegment(minPoint, POINT_VEC(minPoint.x, minPoint.y, maxPoint.z));
case 1: return LineSegment(minPoint, POINT_VEC(minPoint.x, maxPoint.y, minPoint.z));
case 2: return LineSegment(minPoint, POINT_VEC(maxPoint.x, minPoint.y, minPoint.z));
case 3: return LineSegment(POINT_VEC(minPoint.x, minPoint.y, maxPoint.z), POINT_VEC(minPoint.x, maxPoint.y, maxPoint.z));
case 4: return LineSegment(POINT_VEC(minPoint.x, minPoint.y, maxPoint.z), POINT_VEC(maxPoint.x, minPoint.y, maxPoint.z));
case 5: return LineSegment(POINT_VEC(minPoint.x, maxPoint.y, minPoint.z), POINT_VEC(minPoint.x, maxPoint.y, maxPoint.z));
case 6: return LineSegment(POINT_VEC(minPoint.x, maxPoint.y, minPoint.z), POINT_VEC(maxPoint.x, maxPoint.y, minPoint.z));
case 7: return LineSegment(POINT_VEC(minPoint.x, maxPoint.y, maxPoint.z), maxPoint);
case 8: return LineSegment(POINT_VEC(maxPoint.x, minPoint.y, minPoint.z), POINT_VEC(maxPoint.x, minPoint.y, maxPoint.z));
case 9: return LineSegment(POINT_VEC(maxPoint.x, minPoint.y, minPoint.z), POINT_VEC(maxPoint.x, maxPoint.y, minPoint.z));
case 10: return LineSegment(POINT_VEC(maxPoint.x, minPoint.y, maxPoint.z), maxPoint);
case 11: return LineSegment(POINT_VEC(maxPoint.x, maxPoint.y, minPoint.z), maxPoint);
}
}
