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;
}
float3x4 Frustum::WorldMatrix() const
{
assume(up.IsNormalized());
assume(front.IsNormalized());
float3x4 m;
m.SetCol(0, DIR_TO_FLOAT3(WorldRight().Normalized()));
m.SetCol(1, DIR_TO_FLOAT3(up));
if (handedness == FrustumRightHanded)
m.SetCol(2, DIR_TO_FLOAT3(-front)); // In right-handed convention, the -Z axis must map towards the front vector. (so +Z maps to -front)
else
m.SetCol(2, DIR_TO_FLOAT3(front)); // In left-handed convention, the +Z axis must map towards the front vector.
m.SetCol(3, POINT_TO_FLOAT3(pos));
assume(!m.HasNegativeScale());
return m;
}
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;
}
float3x4 operator *(const float3x4 &lhs, const TranslateOp &rhs)
{
float3x4 r = lhs;
r.SetTranslatePart(lhs.TransformPos(DIR_TO_FLOAT3(rhs.Offset())));
// Our optimized form of multiplication must be the same as this.
assume4(r.Equals(lhs * (float3x4)rhs), lhs, rhs, r, lhs * (float3x4)rhs);
return r;
}
float3x4 operator *(const TranslateOp &lhs, const float3x4 &rhs)
{
float3x4 r = rhs;
r.SetTranslatePart(r.TranslatePart() + DIR_TO_FLOAT3(lhs.Offset()));
// Our optimized form of multiplication must be the same as this.
mathassert(r.Equals((float3x4)lhs * rhs));
return r;
}
float4x4 operator *(const TranslateOp &lhs, const float4x4 &rhs)
{
// This function is based on the optimized assumption that the last row of rhs is [0,0,0,1].
// If this does not hold and you are hitting the check below, explicitly cast TranslateOp lhs to float4x4 before multiplication!
assume(rhs.Row(3).Equals(0.f, 0.f, 0.f, 1.f));
float4x4 r = rhs;
r.SetTranslatePart(r.TranslatePart() + DIR_TO_FLOAT3(lhs.Offset()));
return r;
}
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
}
float4x4 operator *(const float4x4 &lhs, const TranslateOp &rhs)
{
float4x4 r = lhs;
r.SetTranslatePart(lhs.TransformPos(DIR_TO_FLOAT3(rhs.Offset())));
return r;
}