float3 float3::AnotherPerpendicular(const float3 &hint, const float3 &hint2) const { assume(!this->IsZero()); assume(hint.IsNormalized()); assume(hint2.IsNormalized()); float3 firstPerpendicular = Perpendicular(hint, hint2); float3 v = this->Cross(firstPerpendicular); return v.Normalized(); }
bool MUST_USE_RESULT float3::AreOrthonormal(const float3 &a, const float3 &b, const float3 &c, float epsilon) { return a.IsPerpendicular(b, epsilon) && a.IsPerpendicular(c, epsilon) && b.IsPerpendicular(c, epsilon) && a.IsNormalized(epsilon*epsilon) && b.IsNormalized(epsilon*epsilon) && c.IsNormalized(epsilon*epsilon); }
float3x4 float3x4::RotateFromTo(const float3 &sourceDirection, const float3 &targetDirection) { assume(sourceDirection.IsNormalized()); assume(targetDirection.IsNormalized()); float3x4 r; r.SetRotatePart(Quat::RotateFromTo(sourceDirection, targetDirection)); r.SetTranslatePart(0, 0, 0); return r; }
float3 float3::Perpendicular(const float3 &hint, const float3 &hint2) const { assume(!this->IsZero()); assume(hint.IsNormalized()); assume(hint2.IsNormalized()); float3 v = this->Cross(hint); float len = v.Normalize(); if (len == 0) return hint2; else return v; }
Quat MUST_USE_RESULT Quat::RotateFromTo(const float3 &sourceDirection, const float3 &targetDirection) { assume(sourceDirection.IsNormalized()); assume(targetDirection.IsNormalized()); float angle = sourceDirection.AngleBetweenNorm(targetDirection); assume(angle >= 0.f); // If sourceDirection == targetDirection, the cross product comes out zero, and normalization would fail. In that case, pick an arbitrary axis. float3 axis = sourceDirection.Cross(targetDirection); float oldLength = axis.Normalize(); if (oldLength == 0) axis = float3(1, 0, 0); return Quat(axis, angle); }
float4 float4::Perpendicular3(const float3 &hint, const float3 &hint2) const { assume(!this->IsZero3()); assume(EqualAbs(w, 0)); assume(hint.IsNormalized()); assume(hint2.IsNormalized()); float3 v = this->Cross3(hint).xyz(); float len = v.Normalize(); if (len == 0) return float4(hint2, 0); else return float4(v, 0); }
bool AABB::IntersectRayAABB(const float3 &rayPos, const float3 &rayDir, float &tNear, float &tFar) const { assume(rayDir.IsNormalized()); tNear = -FLOAT_INF; tFar = FLOAT_INF; for(int i = 0; i < 3; ++i) // loop for each AABB plane (X,Y,Z) { if (EqualAbs(rayDir[i], 0.f)) // ray is parallel to plane in question if (rayPos[i] < minPoint[i] || rayPos[i] > maxPoint[i]) // early-out if the ray can't possibly enter the box. return false; // intersection distances to plane. float recipDir = 1.f / rayDir[i]; float t1 = (minPoint[i] - rayPos[i]) * recipDir; float t2 = (maxPoint[i] - rayPos[i]) * recipDir; if (t1 > t2) Swap(t1, t2); // swap so that t1 is the distance to nearer of the two planes. if (t1 > tNear) tNear = t1; // tNear tracks distance to intersect the AABB. if (t2 < tFar) tFar = t2; // tFar tracks the distance to exit the AABB. if (tNear > tFar) // Box is missed since we "exit" before entering it. return false; if (tFar < 0) // Box is behind the ray. return false; } return true; }
Quat MUST_USE_RESULT Quat::RotateFromTo(const float3 &sourceDirection, const float3 &targetDirection) { assume(sourceDirection.IsNormalized()); assume(targetDirection.IsNormalized()); // If sourceDirection == targetDirection, the cross product comes out zero, and normalization would fail. In that case, pick an arbitrary axis. float3 axis = sourceDirection.Cross(targetDirection); float oldLength = axis.Normalize(); if (oldLength != 0.f) { float halfCosAngle = 0.5f*sourceDirection.Dot(targetDirection); float cosHalfAngle = Sqrt(0.5f + halfCosAngle); float sinHalfAngle = Sqrt(0.5f - halfCosAngle); return Quat(axis.x * sinHalfAngle, axis.y * sinHalfAngle, axis.z * sinHalfAngle, cosHalfAngle); } else return Quat(1.f, 0.f, 0.f, 0.f); }
void Quat::SetFromAxisAngle(const float3 &axis, float angle) { assume(axis.IsNormalized()); assume(MATH_NS::IsFinite(angle)); float cosz = Cos(angle/2.f); float sinz = Sin(angle/2.f); x = axis.x * sinz; y = axis.y * sinz; z = axis.z * sinz; w = cosz; }
float float3::AngleBetweenNorm(const float3 &other) const { assume(this->IsNormalized()); assume(other.IsNormalized()); float cosa = Dot(other); if (cosa >= 1.f) return 0.f; else if (cosa <= -1.f) return pi; else return acos(cosa); }
void Capsule::ProjectToAxis(const float3 &direction, float &outMin, float &outMax) const { outMin = Dot(direction, l.a); outMax = Dot(direction, l.b); if (outMax < outMin) Swap(outMin, outMax); // The following requires that direction is normalized, otherwise we would have to sub/add 'r * direction.Length()', but // don't want to do that for performance reasons. assume(direction.IsNormalized()); outMin -= r; outMax += r; }
void Quat::SetFromAxisAngle(const float3 &axis, float angle) { #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE) SetFromAxisAngle(load_vec3(axis.ptr(), 0.f), angle); #else assume1(axis.IsNormalized(), axis); assume1(MATH_NS::IsFinite(angle), angle); float sinz, cosz; SinCos(angle*0.5f, sinz, cosz); x = axis.x * sinz; y = axis.y * sinz; z = axis.z * sinz; w = cosz; #endif }
void float3::Decompose(const float3 &direction, float3 &outParallel, float3 &outPerpendicular) const { assume(direction.IsNormalized()); outParallel = this->ProjectToNorm(direction); outPerpendicular = *this - outParallel; }
float3 float3::ProjectToNorm(const float3 &direction) const { assume(direction.IsNormalized()); return direction * this->Dot(direction); }
float3 float3::Reflect(const float3 &normal) const { assume(normal.IsNormalized()); return 2.f * this->ProjectToNorm(normal) - *this; }
float3 float3::Reflect(const float3 &normal) const { assume2(normal.IsNormalized(), normal.SerializeToCodeString(), normal.Length()); return 2.f * this->ProjectToNorm(normal) - *this; }
float4 float4::Reflect3(const float3 &normal) const { assume(normal.IsNormalized()); assume(EqualAbs(w, 0)); return 2.f * this->ProjectToNorm3(normal) - *this; }
float4 float4::Reflect3(const float3 &normal) const { assume2(normal.IsNormalized(), normal.SerializeToCodeString(), normal.Length()); assume(EqualAbs(w, 0)); return 2.f * this->ProjectToNorm3(normal) - *this; }
bool Plane::IsInPositiveDirection(const float3 &directionVector) const { assume(directionVector.IsNormalized()); return normal.Dot(directionVector) >= 0.f; }
int Sphere::IntersectLine(const float3 &linePos, const float3 &lineDir, const float3 &sphereCenter, float sphereRadius, float &t1, float &t2) { assume(lineDir.IsNormalized()); assume(sphereRadius >= 0.f); /* A line is represented explicitly by the set { linePos + t * lineDir }, where t is an arbitrary float. A sphere is represented implictly by the set of vectors that satisfy ||v - sphereCenter|| == sphereRadius. To solve which points on the line are also points on the sphere, substitute v <- linePos + t * lineDir to obtain: || linePos + t * lineDir - sphereCenter || == sphereRadius, and squaring both sides we get || linePos + t * lineDir - sphereCenter ||^2 == sphereRadius^2, or rearranging: || (linePos - sphereCenter) + t * lineDir ||^2 == sphereRadius^2. */ // This equation represents the set of points which lie both on the line and the sphere. There is only one // unknown variable, t, for which we solve to get the actual points of intersection. // Compute variables from the above equation: const float3 a = linePos - sphereCenter; const float radSq = sphereRadius * sphereRadius; /* so now the equation looks like || a + t * lineDir ||^2 == radSq. Since ||x||^2 == <x,x> (i.e. the square of a vector norm equals the dot product with itself), we get <a + t * lineDir, a + t * lineDir> == radSq, and using the identity <a+b, a+b> == <a,a> + 2*<a,b> + <b,b> (which holds for dot product when a and b are reals), we have <a,a> + 2 * <a, t * lineDir> + <t * lineDir, t * lineDir> == radSq, or <a,a> - radSq + 2 * <a, lineDir> * t + <lineDir, lineDir> * t^2 == 0, or C + Bt + At^2 == 0, where C = <a,a> - radSq, B = 2 * <a, lineDir>, and A = <lineDir, lineDir> == 1, since we assumed lineDir is normalized. */ const float C = Dot(a,a) - radSq; const float B = 2.f * Dot(a, lineDir); /* The equation A + Bt + Ct^2 == 0 is a second degree equation on t, which is easily solvable using the known formula, and we obtain t = [-B +/- Sqrt(B^2 - 4AC)] / 2A. */ float D = B*B - 4.f * C; // D = B^2 - 4AC. if (D < 0.f) // There is no solution to the square root, so the ray doesn't intersect the sphere. return 0; if (D < 1e-4f) // The expression inside Sqrt is ~ 0. The line is tangent to the sphere, and we have one solution. { t1 = t2 = -B * 0.5f; return 1; } // The Sqrt expression is strictly positive, so we get two different solutions for t. D = Sqrt(D); t1 = (-B - D) * 0.5f; t2 = (-B + D) * 0.5f; return 2; }
float4 float4::ProjectToNorm3(const float3 &target) const { assume(target.IsNormalized()); assume(this->IsWZeroOrOne()); return float4(target * Dot(xyz(), target), w); }
bool AABB::IntersectLineAABB_CPP(const float3 &linePos, const float3 &lineDir, float &tNear, float &tFar) const { assume2(lineDir.IsNormalized(), lineDir, lineDir.LengthSq()); assume2(tNear <= tFar && "AABB::IntersectLineAABB: User gave a degenerate line as input for the intersection test!", tNear, tFar); // The user should have inputted values for tNear and tFar to specify the desired subrange [tNear, tFar] of the line // for this intersection test. // For a Line-AABB test, pass in // tNear = -FLOAT_INF; // tFar = FLOAT_INF; // For a Ray-AABB test, pass in // tNear = 0.f; // tFar = FLOAT_INF; // For a LineSegment-AABB test, pass in // tNear = 0.f; // tFar = LineSegment.Length(); // Test each cardinal plane (X, Y and Z) in turn. if (!EqualAbs(lineDir.x, 0.f)) { float recipDir = RecipFast(lineDir.x); float t1 = (minPoint.x - linePos.x) * recipDir; float t2 = (maxPoint.x - linePos.x) * recipDir; // tNear tracks distance to intersect (enter) the AABB. // tFar tracks the distance to exit the AABB. if (t1 < t2) tNear = Max(t1, tNear), tFar = Min(t2, tFar); else // Swap t1 and t2. tNear = Max(t2, tNear), tFar = Min(t1, tFar); if (tNear > tFar) return false; // Box is missed since we "exit" before entering it. } else if (linePos.x < minPoint.x || linePos.x > maxPoint.x) return false; // The ray can't possibly enter the box, abort. if (!EqualAbs(lineDir.y, 0.f)) { float recipDir = RecipFast(lineDir.y); float t1 = (minPoint.y - linePos.y) * recipDir; float t2 = (maxPoint.y - linePos.y) * recipDir; if (t1 < t2) tNear = Max(t1, tNear), tFar = Min(t2, tFar); else // Swap t1 and t2. tNear = Max(t2, tNear), tFar = Min(t1, tFar); if (tNear > tFar) return false; // Box is missed since we "exit" before entering it. } else if (linePos.y < minPoint.y || linePos.y > maxPoint.y) return false; // The ray can't possibly enter the box, abort. if (!EqualAbs(lineDir.z, 0.f)) // ray is parallel to plane in question { float recipDir = RecipFast(lineDir.z); float t1 = (minPoint.z - linePos.z) * recipDir; float t2 = (maxPoint.z - linePos.z) * recipDir; if (t1 < t2) tNear = Max(t1, tNear), tFar = Min(t2, tFar); else // Swap t1 and t2. tNear = Max(t2, tNear), tFar = Min(t1, tFar); } else if (linePos.z < minPoint.z || linePos.z > maxPoint.z) return false; // The ray can't possibly enter the box, abort. return tNear <= tFar; }