int AABB::UniqueEdgeDirections(vec *out) const
{
out[0] = DIR_VEC(1,0,0);
out[1] = DIR_VEC(0,1,0);
out[2] = DIR_VEC(0,0,1);
return 3;
}
int AABB::UniqueFaceNormals(vec *out) const
{
out[0] = DIR_VEC(1,0,0);
out[1] = DIR_VEC(0,1,0);
out[2] = DIR_VEC(0,0,1);
return 3;
}
void OBB::SetFrom(const Sphere &sphere)
{
pos = sphere.pos;
r.SetFromScalar(sphere.r);
axis[0] = DIR_VEC(1,0,0);
axis[1] = DIR_VEC(0,1,0);
axis[2] = DIR_VEC(0,0,1);
}
void OBB::SetFrom(const AABB &aabb)
{
pos = aabb.CenterPoint();
r = aabb.HalfSize();
axis[0] = DIR_VEC(1, 0, 0);
axis[1] = DIR_VEC(0, 1, 0);
axis[2] = DIR_VEC(0, 0, 1);
}
void OBB::SetNegativeInfinity()
{
pos = POINT_VEC_SCALAR(0.f);
r.SetFromScalar(-FLOAT_INF);
axis[0] = DIR_VEC(1,0,0);
axis[1] = DIR_VEC(0, 1, 0);
axis[2] = DIR_VEC(0, 0, 1);
}
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.
}
AABB Sphere::MaximalContainedAABB() const
{
AABB aabb;
static const float recipSqrt3 = RSqrt(3);
float halfSideLength = r * recipSqrt3;
aabb.SetFromCenterAndSize(pos, DIR_VEC(halfSideLength,halfSideLength,halfSideLength));
return aabb;
}
vec Quat::WorldZ() const
{
#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
return FLOAT4_TO_DIR(quat_transform_vec4(q, float4::unitZ));
#else
return DIR_VEC(this->Transform(0.f, 0.f, 1.f));
#endif
}
/// For Plane-float3x4 transform code, see Eric Lengyel's Mathematics for 3D Game Programming And Computer Graphics 2nd ed., p.110, chapter 4.2.3. [groupSyntax]
void Plane::Transform(const float3x4 &transform)
{
///@todo Could optimize this function by switching to plane convention ax+by+cz+d=0 instead of ax+by+cz=d.
float3x3 r = transform.Float3x3Part();
bool success = r.Inverse(); ///@todo Can optimize the inverse here by assuming orthogonality or orthonormality.
assume(success);
MARK_UNUSED(success);
d = d + normal.Dot(DIR_VEC(r * transform.TranslatePart()));
normal = normal * r;
}
void AABBTransformAsAABB(AABB &aabb, Matrix &m)
{
const vec centerPoint = (aabb.minPoint + aabb.maxPoint) * 0.5f;
const vec halfSize = centerPoint - aabb.minPoint;
vec newCenter = m.MulPos(centerPoint);
// The following is equal to taking the absolute value of the whole matrix m.
vec newDir = DIR_VEC(ABSDOT3(m[0], halfSize), ABSDOT3(m[1], halfSize), ABSDOT3(m[2], halfSize));
aabb.minPoint = newCenter - newDir;
aabb.maxPoint = newCenter + newDir;
}
void OBBSetFrom(OBB &obb, const AABB &aabb, const Matrix &m)
{
assume(m.IsColOrthogonal()); // We cannot convert transform an AABB to OBB if it gets sheared in the process.
assume(m.HasUniformScale()); // Nonuniform scale will produce shear as well.
obb.pos = m.MulPos(aabb.CenterPoint());
obb.r = aabb.HalfSize();
obb.axis[0] = DIR_VEC(m.Col(0));
obb.axis[1] = DIR_VEC(m.Col(1));
obb.axis[2] = DIR_VEC(m.Col(2));
// If the matrix m contains scaling, propagate the scaling from the axis vectors to the half-length vectors,
// since we want to keep the axis vectors always normalized in our representation.
float matrixScale = obb.axis[0].LengthSq();
matrixScale = Sqrt(matrixScale);
obb.r *= matrixScale;
matrixScale = 1.f / matrixScale;
obb.axis[0] *= matrixScale;
obb.axis[1] *= matrixScale;
obb.axis[2] *= matrixScale;
// mathassert(vec::AreOrthogonal(obb.axis[0], obb.axis[1], obb.axis[2]));
// mathassert(vec::AreOrthonormal(obb.axis[0], obb.axis[1], obb.axis[2]));
///@todo Would like to simply do the above, but instead numerical stability requires to do the following:
vec::Orthonormalize(obb.axis[0], obb.axis[1], obb.axis[2]);
}
vec AABB::FaceNormal(int faceIndex) const
{
assume(0 <= faceIndex && faceIndex <= 5);
switch(faceIndex)
{
default: // For release builds where assume() is disabled, return always the first option if out-of-bounds.
case 0: return DIR_VEC(-1, 0, 0);
case 1: return DIR_VEC( 1, 0, 0);
case 2: return DIR_VEC( 0, -1, 0);
case 3: return DIR_VEC( 0, 1, 0);
case 4: return DIR_VEC( 0, 0, -1);
case 5: return DIR_VEC( 0, 0, 1);
}
}
vec Sphere::RandomPointOnSurface(LCG &lcg)
{
vec v = vec::zero;
// Rejection sampling analysis: The unit sphere fills ~52.4% of the volume of its enclosing box, so this
// loop is expected to take only very few iterations before succeeding.
for(int i = 0; i < 1000; ++i)
{
v.x = lcg.FloatNeg1_1();
v.y = lcg.FloatNeg1_1();
v.z = lcg.FloatNeg1_1();
float lenSq = v.LengthSq();
if (lenSq >= 1e-6f && lenSq <= 1.f)
return pos + (r / Sqrt(lenSq)) * v;
}
// Astronomically small probability to reach here, and if we do so, the provided random number generator must have been in a bad state.
assume(false && "Sphere::RandomPointOnSurface failed!");
// Failed to generate a point inside this sphere. Return an arbitrary point on the surface as fallback.
return pos + DIR_VEC(r, 0, 0);
}
Plane Polygon::PlaneCCW() const
{
if (p.size() > 3)
{
Plane plane;
for(size_t i = 0; i < p.size()-2; ++i)
for(size_t j = i+1; j < p.size()-1; ++j)
{
vec pij = vec(p[j])-vec(p[i]);
for(size_t k = j+1; k < p.size(); ++k)
{
plane.normal = pij.Cross(vec(p[k])-vec(p[i]));
float lenSq = plane.normal.LengthSq();
if (lenSq > 1e-8f)
{
plane.normal /= Sqrt(lenSq);
plane.d = plane.normal.Dot(vec(p[i]));
return plane;
}
}
}
#ifndef MATH_SILENT_ASSUME
LOGW("Polygon contains %d points, but they are all collinear! Cannot form a plane for the Polygon using three points! %s", (int)p.size(), this->SerializeToString().c_str());
#endif
// Polygon contains multiple points, but they are all collinear.
// Pick an arbitrary plane along the line as the polygon plane (as if the polygon had only two points)
vec dir = (vec(p[1])-vec(p[0])).Normalized();
return Plane(Line(p[0], dir), dir.Perpendicular());
}
if (p.size() == 3)
return Plane(p[0], p[1], p[2]);
if (p.size() == 2)
{
vec dir = (vec(p[1])-vec(p[0])).Normalized();
return Plane(Line(p[0], dir), dir.Perpendicular());
}
if (p.size() == 1)
return Plane(p[0], DIR_VEC(0,1,0));
return Plane();
}
vec Quat::Axis() const
{
assume2(this->IsNormalized(), *this, this->Length());
#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
// Best: 6.145 nsecs / 16.88 ticks, Avg: 6.367 nsecs, Worst: 6.529 nsecs
assume2(this->IsNormalized(), *this, this->Length());
simd4f cosAngle = _mm_shuffle_ps(q, q, _MM_SHUFFLE(3, 3, 3, 3));
simd4f rcpSinAngle = rsqrt_ps(sub_ps(set1_ps(1.f), mul_ps(cosAngle, cosAngle)));
simd4f a = mul_ps(q, rcpSinAngle);
// Set the w component to zero.
simd4f highPart = _mm_unpackhi_ps(a, zero_ps()); // [_ _ 0 z]
a = _mm_movelh_ps(a, highPart); // [0 z y x]
return FLOAT4_TO_DIR(a);
#else
// Best: 6.529 nsecs / 18.152 ticks, Avg: 6.851 nsecs, Worst: 8.065 nsecs
// Convert cos to sin via the identity sin^2 + cos^2 = 1, and fuse reciprocal and square root to the same instruction,
// since we are about to divide by it.
float rcpSinAngle = RSqrt(1.f - w*w);
return DIR_VEC(x, y, z) * rcpSinAngle;
#endif
}
void AABB::Scale(const vec ¢erPoint, float scaleFactor)
{
return Scale(centerPoint, DIR_VEC(float3(scaleFactor, scaleFactor, scaleFactor)));
}
ScaleOp::ScaleOp(float sx, float sy, float sz)
:scale(DIR_VEC(sx, sy, sz))
{
}
ScaleOp::ScaleOp(const float2 &scaleXY, float scaleZ)
:scale(DIR_VEC(scaleXY.x, scaleXY.y, scaleZ))
{
}
ScaleOp::ScaleOp(const float3 &scale)
:scale(DIR_VEC(scale))
{
}
MATH_BEGIN_NAMESPACE
TranslateOp::TranslateOp(float tx, float ty, float tz)
:offset(DIR_VEC(tx, ty, tz))
{
}
bool OBB::Intersects(const OBB &b, float epsilon) const
{
assume(pos.IsFinite());
assume(b.pos.IsFinite());
assume(vec::AreOrthogonal(axis[0], axis[1], axis[2]));
assume(vec::AreOrthogonal(b.axis[0], b.axis[1], b.axis[2]));
// Generate a rotation matrix that transforms from world space to this OBB's coordinate space.
float3x3 R;
for(int i = 0; i < 3; ++i)
for(int j = 0; j < 3; ++j)
R[i][j] = Dot(axis[i], b.axis[j]);
vec t = b.pos - pos;
// Express the translation vector in a's coordinate frame.
t = DIR_VEC(Dot(t, axis[0]), Dot(t, axis[1]), Dot(t, axis[2]));
float3x3 AbsR;
for(int i = 0; i < 3; ++i)
for(int j = 0; j < 3; ++j)
AbsR[i][j] = Abs(R[i][j]) + epsilon;
// Test the three major axes of this OBB.
for(int i = 0; i < 3; ++i)
{
float ra = r[i];
float rb = DOT3(b.r, AbsR[i]);
if (Abs(t[i]) > ra + rb)
return false;
}
// Test the three major axes of the OBB b.
for(int i = 0; i < 3; ++i)
{
float ra = r[0] * AbsR[0][i] + r[1] * AbsR[1][i] + r[2] * AbsR[2][i];
float rb = b.r[i];
if (Abs(t.x * R[0][i] + t.y * R[1][i] + t.z * R[2][i]) > ra + rb)
return false;
}
// Test the 9 different cross-axes.
// A.x <cross> B.x
float ra = r.y * AbsR[2][0] + r.z * AbsR[1][0];
float rb = b.r.y * AbsR[0][2] + b.r.z * AbsR[0][1];
if (Abs(t.z * R[1][0] - t.y * R[2][0]) > ra + rb)
return false;
// A.x < cross> B.y
ra = r.y * AbsR[2][1] + r.z * AbsR[1][1];
rb = b.r.x * AbsR[0][2] + b.r.z * AbsR[0][0];
if (Abs(t.z * R[1][1] - t.y * R[2][1]) > ra + rb)
return false;
// A.x <cross> B.z
ra = r.y * AbsR[2][2] + r.z * AbsR[1][2];
rb = b.r.x * AbsR[0][1] + b.r.y * AbsR[0][0];
if (Abs(t.z * R[1][2] - t.y * R[2][2]) > ra + rb)
return false;
// A.y <cross> B.x
ra = r.x * AbsR[2][0] + r.z * AbsR[0][0];
rb = b.r.y * AbsR[1][2] + b.r.z * AbsR[1][1];
if (Abs(t.x * R[2][0] - t.z * R[0][0]) > ra + rb)
return false;
// A.y <cross> B.y
ra = r.x * AbsR[2][1] + r.z * AbsR[0][1];
rb = b.r.x * AbsR[1][2] + b.r.z * AbsR[1][0];
if (Abs(t.x * R[2][1] - t.z * R[0][1]) > ra + rb)
return false;
// A.y <cross> B.z
ra = r.x * AbsR[2][2] + r.z * AbsR[0][2];
rb = b.r.x * AbsR[1][1] + b.r.y * AbsR[1][0];
if (Abs(t.x * R[2][2] - t.z * R[0][2]) > ra + rb)
return false;
// A.z <cross> B.x
ra = r.x * AbsR[1][0] + r.y * AbsR[0][0];
rb = b.r.y * AbsR[2][2] + b.r.z * AbsR[2][1];
if (Abs(t.y * R[0][0] - t.x * R[1][0]) > ra + rb)
return false;
// A.z <cross> B.y
ra = r.x * AbsR[1][1] + r.y * AbsR[0][1];
rb = b.r.x * AbsR[2][2] + b.r.z * AbsR[2][0];
if (Abs(t.y * R[0][1] - t.x * R[1][1]) > ra + rb)
return false;
// A.z <cross> B.z
ra = r.x * AbsR[1][2] + r.y * AbsR[0][2];
rb = b.r.x * AbsR[2][1] + b.r.y * AbsR[2][0];
if (Abs(t.y * R[0][2] - t.x * R[1][2]) > ra + rb)
return false;
// No separating axis exists, so the two OBB don't intersect.
return true;
}
TranslateOp::TranslateOp(const float3 &trans)
:offset(DIR_VEC(trans))
{
}
void AABB::SetFrom(const Sphere &s)
{
vec d = DIR_VEC(float3::FromScalar(s.r));
minPoint = s.pos - d;
maxPoint = s.pos + d;
}
