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; }