#if UNITY_EDITOR using UnityEngine; using System; using System.Collections.Generic; namespace GSpawn { [Serializable] public struct OBB { [SerializeField] private Vector3 _size; [SerializeField] private Vector3 _center; [SerializeField] private Quaternion _rotation; [SerializeField] private bool _isValid; public bool isValid { get { return _isValid; } } public float volume { get { return _size.x * _size.y * _size.z; } } public Vector3 center { get { return _center; } set { _center = value; } } public Vector3 size { get { return _size; } set { _size = value.abs(); } } public Vector3 extents { get { return new Vector3(_size.x * 0.5f, _size.y * 0.5f, _size.z * 0.5f); } } public Quaternion rotation { get { return _rotation; } set { _rotation = value; } } public Matrix4x4 rotationMatrix { get { return Matrix4x4.TRS(Vector3.zero, _rotation, Vector3.one); } } public Matrix4x4 transformMatrix { get { return Matrix4x4.TRS(center, _rotation, size); } } public Vector3 right { get { return _rotation * Vector3.right; } } public Vector3 up { get { return _rotation * Vector3.up; } } public Vector3 look { get { return _rotation * Vector3.forward; } } public Vector3 min { get { Vector3 halfSize = extents; return center - right * halfSize.x - up * halfSize.y - look * halfSize.z; } } public Vector3 max { get { Vector3 halfSize = extents; return center + right * halfSize.x + up * halfSize.y + look * halfSize.z; } } public OBB(bool valid) { _center = Vector3.zero; _size = Vector3.zero; _rotation = Quaternion.identity; _isValid = valid; } public OBB(Bounds bounds) { _center = bounds.center; _size = bounds.size; _rotation = Quaternion.identity; _isValid = true; } public OBB(AABB aabb) { _center = aabb.center; _size = aabb.size; _rotation = Quaternion.identity; _isValid = true; } public OBB(Vector3 center) { _center = center; _size = Vector3.zero; _rotation = Quaternion.identity; _isValid = true; } public OBB(Vector3 center, Vector3 size) { _center = center; _size = size; _rotation = Quaternion.identity; _isValid = true; } public OBB(Vector3 center, Vector3 size, Quaternion rotation) { _center = center; _size = size; _rotation = rotation; _isValid = true; } public OBB(Vector3 center, Quaternion rotation) { _center = center; _size = Vector3.zero; _rotation = rotation; _isValid = true; } public OBB(Quaternion rotation) { _center = Vector3.zero; _size = Vector3.zero; _rotation = rotation; _isValid = true; } public OBB(Bounds bounds, Quaternion rotation) { _center = bounds.center; _size = bounds.size; _rotation = rotation; _isValid = true; } public OBB(AABB aabb, Quaternion rotation) { _center = aabb.center; _size = aabb.size; _rotation = rotation; _isValid = true; } public OBB(AABB modelSpaceAABB, Transform transform) { _size = Vector3.Scale(modelSpaceAABB.size.abs(), transform.lossyScale.abs()); _center = transform.TransformPoint(modelSpaceAABB.center); _rotation = transform.rotation; _isValid = true; } public OBB(OBB copy) { _size = copy._size; _center = copy._center; _rotation = copy._rotation; _isValid = copy._isValid; } public static OBB getInvalid() { return new OBB(); } public static OBB createFromSegment(Vector3 pt0, Vector3 pt1, float segmentThickness) { Vector3 segmentDir = pt1 - pt0; float segmentLength = segmentDir.magnitude; if (segmentLength < 1e-5f) return getInvalid(); Vector3 size = new Vector3(segmentThickness, segmentThickness, segmentLength); return new OBB(pt0 + segmentDir * 0.5f, size, Quaternion.LookRotation(segmentDir.normalized, segmentDir.getFirstUnalignedAxisVec())); } public void clipToAlignedOBB(OBB clipOBB) { Vector3 clipRight = clipOBB.right; Vector3 clipUp = clipOBB.up; Vector3 clipLook = clipOBB.look; Vector3 clipExtents = clipOBB.extents; Vector3 clipCenter = clipOBB.center; Vector3 dir = min - clipCenter; float dmin_x = Vector3.Dot(dir, clipRight); float dmin_y = Vector3.Dot(dir, clipUp); float dmin_z = Vector3.Dot(dir, clipLook); dir = max - clipCenter; float dmax_x = Vector3.Dot(dir, clipRight); float dmax_y = Vector3.Dot(dir, clipUp); float dmax_z = Vector3.Dot(dir, clipLook); bool outX = false; bool outY = false; bool outZ = false; if (dmin_x > clipExtents.x && dmax_x > clipExtents.x) { Plane plane = new Plane(clipRight, clipCenter + clipRight * clipExtents.x); _center = plane.projectPoint(_center); _size.x = 0.0f; outX = true; } else if (dmin_x < -clipExtents.x && dmax_x < -clipExtents.x) { Plane plane = new Plane(-clipRight, clipCenter - clipRight * clipExtents.x); _center = plane.projectPoint(_center); _size.x = 0.0f; outX = true; } if (dmin_y > clipExtents.y && dmax_y > clipExtents.y) { Plane plane = new Plane(clipUp, clipCenter + clipUp * clipExtents.y); _center = plane.projectPoint(_center); _size.y = 0.0f; outY = true; } else if (dmin_y < -clipExtents.y && dmax_y < -clipExtents.y) { Plane plane = new Plane(-clipUp, clipCenter - clipUp * clipExtents.y); _center = plane.projectPoint(_center); _size.y = 0.0f; outY = true; } if (dmin_z > clipExtents.z && dmax_z > clipExtents.z) { Plane plane = new Plane(clipLook, clipCenter + clipLook * clipExtents.z); _center = plane.projectPoint(_center); _size.z = 0.0f; outZ = true; } else if (dmin_z < -clipExtents.z && dmax_z < -clipExtents.z) { Plane plane = new Plane(-clipLook, clipCenter - clipLook * clipExtents.z); _center = plane.projectPoint(_center); _size.z = 0.0f; outZ = true; } if (!outX) { if (dmin_x < -clipExtents.x) { float clipAmount = -(dmin_x + clipExtents.x); _center += clipRight * clipAmount * 0.5f; _size.x -= clipAmount; } if (dmax_x > clipExtents.x) { float clipAmount = dmax_x - clipExtents.x; _center -= clipRight * clipAmount * 0.5f; _size.x -= clipAmount; } } if (!outY) { if (dmin_y < -clipExtents.y) { float clipAmount = -(dmin_y + clipExtents.y); _center += clipUp * clipAmount * 0.5f; _size.y -= clipAmount; } if (dmax_y > clipExtents.y) { float clipAmount = dmax_y - clipExtents.y; _center -= clipUp * clipAmount * 0.5f; _size.y -= clipAmount; } } if (!outZ) { if (dmin_z < -clipExtents.z) { float clipAmount = -(dmin_z + clipExtents.z); _center += clipLook * clipAmount * 0.5f; _size.z -= clipAmount; } if (dmax_z > clipExtents.z) { float clipAmount = dmax_z - clipExtents.z; _center -= clipLook * clipAmount * 0.5f; _size.z -= clipAmount; } } } public void inflate(float amount) { size += Vector3Ex.create(amount); } public void inflateClampZero(float amount) { size += Vector3Ex.create(amount); size = Vector3.Max(size, Vector3.zero); } public void inflate(float amount, int ignoreAxis) { Vector3 inflate = Vector3Ex.create(amount); inflate[ignoreAxis] = 0.0f; size += inflate; } public void transform(Matrix4x4 transformMtx) { center = transformMtx.MultiplyPoint(_center); rotation = QuaternionEx.create(transformMtx) * _rotation; size = Vector3.Scale(transformMtx.getPositiveScale(), _size); } public void rotateAroundPivot(Quaternion rotation, Vector3 pivot) { _rotation = rotation * _rotation; Vector3 toCenter = _center - pivot; toCenter = rotation * toCenter; center = pivot + toCenter; } public void calcCenterAndCorners(List centerAndCorners) { calcCorners(centerAndCorners, false); centerAndCorners.Add(center); } public void calcCorners(List cornerPoints, bool append) { Box3D.calcCorners(_center, _size, _rotation, cornerPoints, append); } public void enclosePoint(Vector3 point) { // X axis float dot = Vector3.Dot((point - _center), right); if (dot > extents.x) { float delta = dot - extents.x; _size.x += delta; _center += right * delta * 0.5f; } else if (dot < -extents.x) { float delta = dot + extents.x; _size.x += Mathf.Abs(delta); _center += right * delta * 0.5f; } // Y axis dot = Vector3.Dot((point - _center), up); if (dot > extents.y) { float delta = dot - extents.y; _size.y += delta; _center += up * delta * 0.5f; } else if (dot < -extents.y) { float delta = dot + extents.y; _size.y += Mathf.Abs(delta); _center += up * delta * 0.5f; } // Z axis dot = Vector3.Dot((point - _center), look); if (dot > extents.z) { float delta = dot - extents.z; _size.z += delta; _center += look * delta * 0.5f; } else if (dot < -extents.z) { float delta = dot + extents.z; _size.z += Mathf.Abs(delta); _center += look * delta * 0.5f; } } public void encloseOBB(OBB otherOBB) { Vector3 otherRightAxis = otherOBB.right; Vector3 otherUpAxis = otherOBB.up; Vector3 otherExtents = otherOBB.extents; Vector3 otherFaceCenter = otherOBB.center - otherOBB.look * otherExtents.z; enclosePoint(otherFaceCenter - otherRightAxis * otherExtents.x + otherUpAxis * otherExtents.y); enclosePoint(otherFaceCenter + otherRightAxis * otherExtents.x + otherUpAxis * otherExtents.y); enclosePoint(otherFaceCenter + otherRightAxis * otherExtents.x - otherUpAxis * otherExtents.y); enclosePoint(otherFaceCenter - otherRightAxis * otherExtents.x - otherUpAxis * otherExtents.y); otherFaceCenter = otherOBB.center + otherOBB.look * otherExtents.z; enclosePoint(otherFaceCenter + otherRightAxis * otherExtents.x + otherUpAxis * otherExtents.y); enclosePoint(otherFaceCenter - otherRightAxis * otherExtents.x + otherUpAxis * otherExtents.y); enclosePoint(otherFaceCenter - otherRightAxis * otherExtents.x - otherUpAxis * otherExtents.y); enclosePoint(otherFaceCenter + otherRightAxis * otherExtents.x - otherUpAxis * otherExtents.y); } public OBB calcInwardFaceExtrusion(Box3DFace face, float extrudeAmount) { Box3DFaceDesc faceDesc = Box3D.getFaceDesc(_center, _size, _rotation, face); Vector3 size = _size; if (face == Box3DFace.Left || face == Box3DFace.Right) size.x = extrudeAmount; else if (face == Box3DFace.Bottom || face == Box3DFace.Top) size.y = extrudeAmount; else size.z = extrudeAmount; return new OBB(faceDesc.center - faceDesc.plane.normal * extrudeAmount * 0.5f, size, _rotation); } public OBB calcOutwardFaceExtrusion(Box3DFace face, float extrudeAmount) { Box3DFaceDesc faceDesc = Box3D.getFaceDesc(_center, _size, _rotation, face); Vector3 size = _size; if (face == Box3DFace.Left || face == Box3DFace.Right) size.x = extrudeAmount; else if (face == Box3DFace.Bottom || face == Box3DFace.Top) size.y = extrudeAmount; else size.z = extrudeAmount; return new OBB(faceDesc.center + faceDesc.plane.normal * extrudeAmount * 0.5f, size, _rotation); } public OBB calcFaceExtrusionFromFaceCenter(Box3DFace face, float extrudeAmount) { Box3DFaceDesc faceDesc = Box3D.getFaceDesc(_center, _size, _rotation, face); Vector3 size = _size; if (face == Box3DFace.Left || face == Box3DFace.Right) size.x = extrudeAmount; else if (face == Box3DFace.Bottom || face == Box3DFace.Top) size.y = extrudeAmount; else size.z = extrudeAmount; return new OBB(faceDesc.center, size, _rotation); } public bool containsPoint(Vector3 point, Vector3Int axisMask) { Vector3 boxRight = right; Vector3 boxUp = up; Vector3 boxLook = look; // Bring point in box local space float ptX = point.x; float ptY = point.y; float ptZ = point.z; point.x = (ptX * boxRight.x + ptY * boxRight.y + ptZ * boxRight.z) - (_center.x * boxRight.x + _center.y * boxRight.y + _center.z * boxRight.z); point.y = (ptX * boxUp.x + ptY * boxUp.y + ptZ * boxUp.z) - (_center.x * boxUp.x + _center.y * boxUp.y + _center.z * boxUp.z); point.z = (ptX * boxLook.x + ptY * boxLook.y + ptZ * boxLook.z) - (_center.x * boxLook.x + _center.y * boxLook.y + _center.z * boxLook.z); Vector3 extents; extents.x = _size.x * 0.5f; extents.y = _size.y * 0.5f; extents.z = _size.z * 0.5f; if (axisMask.x != 0 && (point.x < -extents.x || point.x > extents.x)) return false; if (axisMask.y != 0 && (point.y < -extents.y || point.y > extents.y)) return false; if (axisMask.z != 0 && (point.z < -extents.z || point.z > extents.z)) return false; return true; } public bool containsPoint(Vector3 point) { Vector3 boxRight = right; Vector3 boxUp = up; Vector3 boxLook = look; // Bring point in box local space float ptX = point.x; float ptY = point.y; float ptZ = point.z; point.x = (ptX * boxRight.x + ptY * boxRight.y + ptZ * boxRight.z) - (_center.x * boxRight.x + _center.y * boxRight.y + _center.z * boxRight.z); point.y = (ptX * boxUp.x + ptY * boxUp.y + ptZ * boxUp.z) - (_center.x * boxUp.x + _center.y * boxUp.y + _center.z * boxUp.z); point.z = (ptX * boxLook.x + ptY * boxLook.y + ptZ * boxLook.z) - (_center.x * boxLook.x + _center.y * boxLook.y + _center.z * boxLook.z); Vector3 extents; extents.x = _size.x * 0.5f; extents.y = _size.y * 0.5f; extents.z = _size.z * 0.5f; if (point.x < -extents.x || point.x > extents.x) return false; if (point.y < -extents.y || point.y > extents.y) return false; if (point.z < -extents.z || point.z > extents.z) return false; return true; } public bool containsPoint(Vector3 point, Vector3 boxRight, Vector3 boxUp, Vector3 boxLook) { // Bring point in box local space float ptX = point.x; float ptY = point.y; float ptZ = point.z; point.x = (ptX * boxRight.x + ptY * boxRight.y + ptZ * boxRight.z) - (_center.x * boxRight.x + _center.y * boxRight.y + _center.z * boxRight.z); point.y = (ptX * boxUp.x + ptY * boxUp.y + ptZ * boxUp.z) - (_center.x * boxUp.x + _center.y * boxUp.y + _center.z * boxUp.z); point.z = (ptX * boxLook.x + ptY * boxLook.y + ptZ * boxLook.z) - (_center.x * boxLook.x + _center.y * boxLook.y + _center.z * boxLook.z); Vector3 extents; extents.x = _size.x * 0.5f; extents.y = _size.y * 0.5f; extents.z = _size.z * 0.5f; if (point.x < -extents.x || point.x > extents.x) return false; if (point.y < -extents.y || point.y > extents.y) return false; if (point.z < -extents.z || point.z > extents.z) return false; return true; } public bool containsPoints(IEnumerable points, Vector3Int axisMask) { foreach (var pt in points) if (!containsPoint(pt, axisMask)) return false; return true; } public bool containsPoints(IEnumerable points) { foreach (var pt in points) if (!containsPoint(pt)) return false; return true; } public Vector3 calcClosestPoint(Vector3 point) { Vector3 boxRight = right; Vector3 boxUp = up; Vector3 boxLook = look; // Bring point in box local space float ptX = point.x; float ptY = point.y; float ptZ = point.z; point.x = (ptX * boxRight.x + ptY * boxRight.y + ptZ * boxRight.z) - (_center.x * boxRight.x + _center.y * boxRight.y + _center.z * boxRight.z); point.y = (ptX * boxUp.x + ptY * boxUp.y + ptZ * boxUp.z) - (_center.x * boxUp.x + _center.y * boxUp.y + _center.z * boxUp.z); point.z = (ptX * boxLook.x + ptY * boxLook.y + ptZ * boxLook.z) - (_center.x * boxLook.x + _center.y * boxLook.y + _center.z * boxLook.z); Vector3 extents; extents.x = _size.x * 0.5f; extents.y = _size.y * 0.5f; extents.z = _size.z * 0.5f; Vector3 closestPt; closestPt.x = point.x; if (closestPt.x > extents.x) closestPt.x = extents.x; else if (closestPt.x < -extents.x) closestPt.x = -extents.x; closestPt.y = point.y; if (closestPt.y > extents.y) closestPt.y = extents.y; else if (closestPt.y < -extents.y) closestPt.y = -extents.y; closestPt.z = point.z; if (closestPt.z > extents.z) closestPt.z = extents.z; else if (closestPt.z < -extents.z) closestPt.z = -extents.z; point.x = boxRight.x * closestPt.x + boxUp.x * closestPt.y + boxLook.x * closestPt.z + _center.x; point.y = boxRight.y * closestPt.x + boxUp.y * closestPt.y + boxLook.y * closestPt.z + _center.y; point.z = boxRight.z * closestPt.x + boxUp.z * closestPt.y + boxLook.z * closestPt.z + _center.z; return point; } public Vector3 calcClosestPoint(Vector3 point, Vector3 boxRight, Vector3 boxUp, Vector3 boxLook) { // Bring point in box local space float ptX = point.x; float ptY = point.y; float ptZ = point.z; point.x = (ptX * boxRight.x + ptY * boxRight.y + ptZ * boxRight.z) - (_center.x * boxRight.x + _center.y * boxRight.y + _center.z * boxRight.z); point.y = (ptX * boxUp.x + ptY * boxUp.y + ptZ * boxUp.z) - (_center.x * boxUp.x + _center.y * boxUp.y + _center.z * boxUp.z); point.z = (ptX * boxLook.x + ptY * boxLook.y + ptZ * boxLook.z) - (_center.x * boxLook.x + _center.y * boxLook.y + _center.z * boxLook.z); Vector3 extents; extents.x = _size.x * 0.5f; extents.y = _size.y * 0.5f; extents.z = _size.z * 0.5f; Vector3 closestPt; closestPt.x = point.x; if (closestPt.x > extents.x) closestPt.x = extents.x; else if (closestPt.x < -extents.x) closestPt.x = -extents.x; closestPt.y = point.y; if (closestPt.y > extents.y) closestPt.y = extents.y; else if (closestPt.y < -extents.y) closestPt.y = -extents.y; closestPt.z = point.z; if (closestPt.z > extents.z) closestPt.z = extents.z; else if (closestPt.z < -extents.z) closestPt.z = -extents.z; point.x = boxRight.x * closestPt.x + boxUp.x * closestPt.y + boxLook.x * closestPt.z + _center.x; point.y = boxRight.y * closestPt.x + boxUp.y * closestPt.y + boxLook.y * closestPt.z + _center.y; point.z = boxRight.z * closestPt.x + boxUp.z * closestPt.y + boxLook.z * closestPt.z + _center.z; return point; } public bool raycast(Ray ray, out float t) { t = 0.0f; Matrix4x4 boxMatrix = Matrix4x4.TRS(_center, _rotation, _size.replaceZero(1e-6f)); Ray modelRay = boxMatrix.inverse.transformRay(ray); if (modelRay.direction.sqrMagnitude == 0.0f) return false; Bounds unitCube = new Bounds(Vector3.zero, Vector3.one); if (unitCube.IntersectRay(modelRay, out t)) { Vector3 intersectPt = boxMatrix.MultiplyPoint(modelRay.GetPoint(t)); t = (intersectPt - ray.origin).magnitude; return true; } return false; } public bool raycast(Ray ray) { float t; return raycast(ray, out t); } private static List _aabbCorners = new List(); public bool intersectsTriangle(Vector3 p0, Vector3 p1, Vector3 p2) { // TODO: Implement SAT? AABB aabb = new AABB(Vector3.zero, Vector3.one); // Check if the aabb contains any of the 3 triangle points Matrix4x4 inverseTransform = transformMatrix.inverse; p0 = inverseTransform.MultiplyPoint(p0); if (aabb.containsPoint(p0)) return true; p1 = inverseTransform.MultiplyPoint(p1); if (aabb.containsPoint(p1)) return true; p2 = inverseTransform.MultiplyPoint(p2); if (aabb.containsPoint(p2)) return true; // Check if the triangle edges intersect the AABB Vector3 edge = p1 - p0; Ray ray = new Ray(p0, edge); float t; if (aabb.raycast(ray, out t)) { if (t >= 0.0f && t <= edge.magnitude) return true; } edge = p2 - p0; ray = new Ray(p0, edge); if (aabb.raycast(ray, out t)) { if (t >= 0.0f && t <= edge.magnitude) return true; } edge = p2 - p1; ray = new Ray(p1, edge); if (aabb.raycast(ray, out t)) { if (t >= 0.0f && t <= edge.magnitude) return true; } // Check if the AABB intersects the triangle plane and if it does, check if // any one of the corner points of the AABB lies inside the triangle area. Plane trianglePlane = new Plane(p0, p1, p2); Box3D.calcCorners(Vector3.zero, Vector3.one, _aabbCorners, false); var result = Box3D.classifyAgainstPlane(_aabbCorners, trianglePlane); if (result == PlaneClassifyResult.Spanning || result == PlaneClassifyResult.OnPlane) { int numPts = _aabbCorners.Count; for (int i = 0; i < numPts; ++i) { if (Triangle3D.containsPoint(_aabbCorners[i], false, p0, p1, p2)) return true; } } return false; } #region BoxIntersectsBox Heap Alloc private static Vector3[] A = new Vector3[3]; private static Vector3[] B = new Vector3[3]; private static float[,] R = new float[3, 3]; private static float[,] absR = new float[3, 3]; #endregion public bool intersectsOBB(OBB other) { A[0] = _rotation * Vector3.right; A[1] = _rotation * Vector3.up; A[2] = _rotation * Vector3.forward; B[0] = other._rotation * Vector3.right; B[1] = other._rotation * Vector3.up; B[2] = other._rotation * Vector3.forward; // Note: We're using column major matrices. for (int row = 0; row < 3; ++row) { for (int column = 0; column < 3; ++column) { R[row, column] = Vector3.Dot(A[row], B[column]); } } Vector3 extents = _size * 0.5f; Vector3 AEx = new Vector3(extents.x, extents.y, extents.z); extents = other._size * 0.5f; Vector3 BEx = new Vector3(extents.x, extents.y, extents.z); // Construct absolute rotation error matrix to account for cases when 2 local axes are parallel const float epsilon = 1e-4f; for (int row = 0; row < 3; ++row) { for (int column = 0; column < 3; ++column) { absR[row, column] = MathEx.fastAbs(R[row, column]) + epsilon; } } Vector3 trVector = other._center - _center; Vector3 t = new Vector3(Vector3.Dot(trVector, A[0]), Vector3.Dot(trVector, A[1]), Vector3.Dot(trVector, A[2])); // Test extents projection on box A local axes (A0, A1, A2) for (int axisIndex = 0; axisIndex < 3; ++axisIndex) { float bExtents = BEx[0] * absR[axisIndex, 0] + BEx[1] * absR[axisIndex, 1] + BEx[2] * absR[axisIndex, 2]; if (MathEx.fastAbs(t[axisIndex]) > AEx[axisIndex] + bExtents) return false; } // Test extents projection on box B local axes (B0, B1, B2) for (int axisIndex = 0; axisIndex < 3; ++axisIndex) { float aExtents = AEx[0] * absR[0, axisIndex] + AEx[1] * absR[1, axisIndex] + AEx[2] * absR[2, axisIndex]; if (MathEx.fastAbs(t[0] * R[0, axisIndex] + t[1] * R[1, axisIndex] + t[2] * R[2, axisIndex]) > aExtents + BEx[axisIndex]) return false; } // Test axis A0 x B0 float ra = AEx[1] * absR[2, 0] + AEx[2] * absR[1, 0]; float rb = BEx[1] * absR[0, 2] + BEx[2] * absR[0, 1]; if (MathEx.fastAbs(t[2] * R[1, 0] - t[1] * R[2, 0]) > ra + rb) return false; // Test axis A0 x B1 ra = AEx[1] * absR[2, 1] + AEx[2] * absR[1, 1]; rb = BEx[0] * absR[0, 2] + BEx[2] * absR[0, 0]; if (MathEx.fastAbs(t[2] * R[1, 1] - t[1] * R[2, 1]) > ra + rb) return false; // Test axis A0 x B2 ra = AEx[1] * absR[2, 2] + AEx[2] * absR[1, 2]; rb = BEx[0] * absR[0, 1] + BEx[1] * absR[0, 0]; if (MathEx.fastAbs(t[2] * R[1, 2] - t[1] * R[2, 2]) > ra + rb) return false; // Test axis A1 x B0 ra = AEx[0] * absR[2, 0] + AEx[2] * absR[0, 0]; rb = BEx[1] * absR[1, 2] + BEx[2] * absR[1, 1]; if (MathEx.fastAbs(t[0] * R[2, 0] - t[2] * R[0, 0]) > ra + rb) return false; // Test axis A1 x B1 ra = AEx[0] * absR[2, 1] + AEx[2] * absR[0, 1]; rb = BEx[0] * absR[1, 2] + BEx[2] * absR[1, 0]; if (MathEx.fastAbs(t[0] * R[2, 1] - t[2] * R[0, 1]) > ra + rb) return false; // Test axis A1 x B2 ra = AEx[0] * absR[2, 2] + AEx[2] * absR[0, 2]; rb = BEx[0] * absR[1, 1] + BEx[1] * absR[1, 0]; if (MathEx.fastAbs(t[0] * R[2, 2] - t[2] * R[0, 2]) > ra + rb) return false; // Test axis A2 x B0 ra = AEx[0] * absR[1, 0] + AEx[1] * absR[0, 0]; rb = BEx[1] * absR[2, 2] + BEx[2] * absR[2, 1]; if (MathEx.fastAbs(t[1] * R[0, 0] - t[0] * R[1, 0]) > ra + rb) return false; // Test axis A2 x B1 ra = AEx[0] * absR[1, 1] + AEx[1] * absR[0, 1]; rb = BEx[0] * absR[2, 2] + BEx[2] * absR[2, 0]; if (MathEx.fastAbs(t[1] * R[0, 1] - t[0] * R[1, 1]) > ra + rb) return false; // Test axis A2 x B2 ra = AEx[0] * absR[1, 2] + AEx[1] * absR[0, 2]; rb = BEx[0] * absR[2, 1] + BEx[1] * absR[2, 0]; if (MathEx.fastAbs(t[1] * R[0, 2] - t[0] * R[1, 2]) > ra + rb) return false; return true; } } } #endif