190 lines
6.6 KiB
C++
190 lines
6.6 KiB
C++
// Copyright (c) 2012-2023 Wojciech Figat. All rights reserved.
|
|
|
|
#include "Plane.h"
|
|
#include "Matrix.h"
|
|
#include "Vector3.h"
|
|
#include "Vector4.h"
|
|
#include "Quaternion.h"
|
|
#include "../Types/String.h"
|
|
|
|
const Real Plane::DistanceEpsilon = 0.0001f;
|
|
const Real Plane::NormalEpsilon = 1.0f / 65535.0f;
|
|
|
|
Plane::Plane(const Vector3& point1, const Vector3& point2, const Vector3& point3)
|
|
{
|
|
Vector3 cross;
|
|
|
|
const Vector3 t1 = point2 - point1;
|
|
const Vector3 t2 = point3 - point1;
|
|
|
|
Vector3::Cross(t1, t2, cross);
|
|
const Real invPyth = cross.InvLength();
|
|
|
|
Normal = cross * invPyth;
|
|
D = -(Normal.X * point1.X + Normal.Y * point1.Y + Normal.Z * point1.Z);
|
|
}
|
|
|
|
String Plane::ToString() const
|
|
{
|
|
return String::Format(TEXT("{}"), *this);
|
|
}
|
|
|
|
void Plane::Normalize()
|
|
{
|
|
const Real length = Normal.Length();
|
|
if (!Math::IsZero(length))
|
|
{
|
|
const Real rcp = 1.0f / length;
|
|
Normal *= rcp;
|
|
D *= rcp;
|
|
}
|
|
}
|
|
|
|
Vector3 Plane::Intersection(const Plane& inPlane1, const Plane& inPlane2, const Plane& inPlane3)
|
|
{
|
|
// intersection point with 3 planes
|
|
// {
|
|
// x = -( c2*b1*d3-c2*b3*d1+b3*c1*d2+c3*b2*d1-b1*c3*d2-c1*b2*d3)/
|
|
// (-c2*b3*a1+c3*b2*a1-b1*c3*a2-c1*b2*a3+b3*c1*a2+c2*b1*a3),
|
|
// y = ( c3*a2*d1-c3*a1*d2-c2*a3*d1+d2*c1*a3-a2*c1*d3+c2*d3*a1)/
|
|
// (-c2*b3*a1+c3*b2*a1-b1*c3*a2-c1*b2*a3+b3*c1*a2+c2*b1*a3),
|
|
// z = -(-a2*b1*d3+a2*b3*d1-a3*b2*d1+d3*b2*a1-d2*b3*a1+d2*b1*a3)/
|
|
// (-c2*b3*a1+c3*b2*a1-b1*c3*a2-c1*b2*a3+b3*c1*a2+c2*b1*a3)
|
|
// }
|
|
|
|
// TODO: convet into cros products, dot products etc. ???
|
|
|
|
const Real bc1 = inPlane1.Normal.Y * inPlane3.Normal.Z - inPlane3.Normal.Y * inPlane1.Normal.Z;
|
|
const Real bc2 = inPlane2.Normal.Y * inPlane1.Normal.Z - inPlane1.Normal.Y * inPlane2.Normal.Z;
|
|
const Real bc3 = inPlane3.Normal.Y * inPlane2.Normal.Z - inPlane2.Normal.Y * inPlane3.Normal.Z;
|
|
|
|
const Real ad1 = inPlane1.Normal.X * inPlane3.D - inPlane3.Normal.X * inPlane1.D;
|
|
const Real ad2 = inPlane2.Normal.X * inPlane1.D - inPlane1.Normal.X * inPlane2.D;
|
|
const Real ad3 = inPlane3.Normal.X * inPlane2.D - inPlane2.Normal.X * inPlane3.D;
|
|
|
|
const Real x = -(inPlane1.D * bc3 + inPlane2.D * bc1 + inPlane3.D * bc2);
|
|
const Real y = -(inPlane1.Normal.Z * ad3 + inPlane2.Normal.Z * ad1 + inPlane3.Normal.Z * ad2);
|
|
const Real z = +(inPlane1.Normal.Y * ad3 + inPlane2.Normal.Y * ad1 + inPlane3.Normal.Y * ad2);
|
|
const Real w = -(inPlane1.Normal.X * bc3 + inPlane2.Normal.X * bc1 + inPlane3.Normal.X * bc2);
|
|
|
|
// better to have detectable invalid values than to have reaaaaaaally big values
|
|
if (w > -NormalEpsilon && w < NormalEpsilon)
|
|
{
|
|
return Vector3(NAN);
|
|
}
|
|
return Vector3(x / w, y / w, z / w);
|
|
}
|
|
|
|
void Plane::Multiply(const Plane& value, Real scale, Plane& result)
|
|
{
|
|
result.Normal.X = value.Normal.X * scale;
|
|
result.Normal.Y = value.Normal.Y * scale;
|
|
result.Normal.Z = value.Normal.Z * scale;
|
|
result.D = value.D * scale;
|
|
}
|
|
|
|
Plane Plane::Multiply(const Plane& value, Real scale)
|
|
{
|
|
return Plane(value.Normal * scale, value.D * scale);
|
|
}
|
|
|
|
void Plane::Dot(const Plane& left, const Vector4& right, Real& result)
|
|
{
|
|
result = left.Normal.X * right.X + left.Normal.Y * right.Y + left.Normal.Z * right.Z + left.D * right.W;
|
|
}
|
|
|
|
Real Plane::Dot(const Plane& left, const Vector4& right)
|
|
{
|
|
return left.Normal.X * right.X + left.Normal.Y * right.Y + left.Normal.Z * right.Z + left.D * right.W;
|
|
}
|
|
|
|
void Plane::DotCoordinate(const Plane& left, const Vector3& right, Real& result)
|
|
{
|
|
result = left.Normal.X * right.X + left.Normal.Y * right.Y + left.Normal.Z * right.Z + left.D;
|
|
}
|
|
|
|
Real Plane::DotCoordinate(const Plane& left, const Vector3& right)
|
|
{
|
|
return left.Normal.X * right.X + left.Normal.Y * right.Y + left.Normal.Z * right.Z + left.D;
|
|
}
|
|
|
|
void Plane::DotNormal(const Plane& left, const Vector3& right, Real& result)
|
|
{
|
|
result = left.Normal.X * right.X + left.Normal.Y * right.Y + left.Normal.Z * right.Z;
|
|
}
|
|
|
|
Real Plane::DotNormal(const Plane& left, const Vector3& right)
|
|
{
|
|
return left.Normal.X * right.X + left.Normal.Y * right.Y + left.Normal.Z * right.Z;
|
|
}
|
|
|
|
void Plane::Normalize(const Plane& plane, Plane& result)
|
|
{
|
|
const Real magnitude = 1.0f / Math::Sqrt(plane.Normal.X * plane.Normal.X + plane.Normal.Y * plane.Normal.Y + plane.Normal.Z * plane.Normal.Z);
|
|
result.Normal.X = plane.Normal.X * magnitude;
|
|
result.Normal.Y = plane.Normal.Y * magnitude;
|
|
result.Normal.Z = plane.Normal.Z * magnitude;
|
|
result.D = plane.D * magnitude;
|
|
}
|
|
|
|
Plane Plane::Normalize(const Plane& plane)
|
|
{
|
|
const Real magnitude = 1.0f / Math::Sqrt(plane.Normal.X * plane.Normal.X + plane.Normal.Y * plane.Normal.Y + plane.Normal.Z * plane.Normal.Z);
|
|
return Plane(plane.Normal * magnitude, plane.D * magnitude);
|
|
}
|
|
|
|
void Plane::Transform(const Plane& plane, const Quaternion& rotation, Plane& result)
|
|
{
|
|
const Real x2 = rotation.X + rotation.X;
|
|
const Real y2 = rotation.Y + rotation.Y;
|
|
const Real z2 = rotation.Z + rotation.Z;
|
|
const Real wx = rotation.W * x2;
|
|
const Real wy = rotation.W * y2;
|
|
const Real wz = rotation.W * z2;
|
|
const Real xx = rotation.X * x2;
|
|
const Real xy = rotation.X * y2;
|
|
const Real xz = rotation.X * z2;
|
|
const Real yy = rotation.Y * y2;
|
|
const Real yz = rotation.Y * z2;
|
|
const Real zz = rotation.Z * z2;
|
|
|
|
const Real x = plane.Normal.X;
|
|
const Real y = plane.Normal.Y;
|
|
const Real z = plane.Normal.Z;
|
|
|
|
result.Normal.X = x * (1.0f - yy - zz) + y * (xy - wz) + z * (xz + wy);
|
|
result.Normal.Y = x * (xy + wz) + y * (1.0f - xx - zz) + z * (yz - wx);
|
|
result.Normal.Z = x * (xz - wy) + y * (yz + wx) + z * (1.0f - xx - yy);
|
|
result.D = plane.D;
|
|
}
|
|
|
|
Plane Plane::Transform(const Plane& plane, const Quaternion& rotation)
|
|
{
|
|
Plane result;
|
|
Transform(plane, rotation, result);
|
|
return result;
|
|
}
|
|
|
|
void Plane::Transform(const Plane& plane, const Matrix& transformation, Plane& result)
|
|
{
|
|
const Real x = plane.Normal.X;
|
|
const Real y = plane.Normal.Y;
|
|
const Real z = plane.Normal.Z;
|
|
const Real d = plane.D;
|
|
|
|
Matrix inverse;
|
|
Matrix::Invert(transformation, inverse);
|
|
|
|
result.Normal.X = x * inverse.M11 + y * inverse.M12 + z * inverse.M13 + d * inverse.M14;
|
|
result.Normal.Y = x * inverse.M21 + y * inverse.M22 + z * inverse.M23 + d * inverse.M24;
|
|
result.Normal.Z = x * inverse.M31 + y * inverse.M32 + z * inverse.M33 + d * inverse.M34;
|
|
result.D = x * inverse.M41 + y * inverse.M42 + z * inverse.M43 + d * inverse.M44;
|
|
}
|
|
|
|
Plane Plane::Transform(const Plane& plane, const Matrix& transformation)
|
|
{
|
|
Plane result;
|
|
Transform(plane, transformation, result);
|
|
return result;
|
|
}
|