diff options
Diffstat (limited to 'indra/llmath/llvolume.cpp')
-rw-r--r-- | indra/llmath/llvolume.cpp | 625 |
1 files changed, 491 insertions, 134 deletions
diff --git a/indra/llmath/llvolume.cpp b/indra/llmath/llvolume.cpp index 9b6e2488e6..d261811aa2 100644 --- a/indra/llmath/llvolume.cpp +++ b/indra/llmath/llvolume.cpp @@ -45,6 +45,7 @@ #include "m4math.h" #include "m3math.h" #include "llmatrix4a.h" +#include "lloctree.h" #include "lldarray.h" #include "llvolume.h" #include "llstl.h" @@ -132,6 +133,51 @@ BOOL LLLineSegmentBoxIntersect(const F32* start, const F32* end, const F32* cent return true; } +BOOL LLLineSegmentBoxIntersect(const LLVector4a& start, const LLVector4a& end, const LLVector4a& center, const LLVector4a& size) +{ + LLVector4a fAWdU; + LLVector4a dir; + LLVector4a diff; + + dir.setSub(end, start); + dir.mul(0.5f); + + diff.setAdd(end,start); + diff.mul(0.5f); + diff.sub(center); + fAWdU.setAbs(dir); + + LLVector4a rhs; + rhs.setAdd(size, fAWdU); + + LLVector4a lhs; + lhs.setAbs(diff); + + S32 grt = lhs.greaterThan4(rhs).getComparisonMask(); + + if (grt & 0x7) + { + return false; + } + + LLVector4a f; + f.setCross3(dir, diff); + f.setAbs(f); + + LLVector4a v0; v0.mQ = _mm_shuffle_ps(size.mQ, size.mQ, _MM_SHUFFLE(3,1,0,0)); + LLVector4a v1; v1.mQ = _mm_shuffle_ps(fAWdU.mQ, fAWdU.mQ, _MM_SHUFFLE(3,2,2,1)); + lhs.setMul(v0, v1); + + v0.mQ = _mm_shuffle_ps(size.mQ, size.mQ, _MM_SHUFFLE(3,2,2,1)); + v1.mQ = _mm_shuffle_ps(fAWdU.mQ, fAWdU.mQ, _MM_SHUFFLE(3,1,0,0)); + rhs.setMul(v0, v1); + rhs.add(lhs); + + grt = f.greaterThan4(rhs).getComparisonMask(); + + return (grt & 0x7) ? false : true; +} + // intersect test between triangle vert0, vert1, vert2 and a ray from orig in direction dir. // returns TRUE if intersecting and returns barycentric coordinates in intersection_a, intersection_b, @@ -139,15 +185,13 @@ BOOL LLLineSegmentBoxIntersect(const F32* start, const F32* end, const F32* cent // Moller-Trumbore algorithm BOOL LLTriangleRayIntersect(const LLVector4a& vert0, const LLVector4a& vert1, const LLVector4a& vert2, const LLVector4a& orig, const LLVector4a& dir, - F32* intersection_a, F32* intersection_b, F32* intersection_t, BOOL two_sided) + F32& intersection_a, F32& intersection_b, F32& intersection_t) { - F32 u, v, t; /* find vectors for two edges sharing vert0 */ LLVector4a edge1; edge1.setSub(vert1, vert0); - LLVector4a edge2; edge2.setSub(vert2, vert0); @@ -156,87 +200,116 @@ BOOL LLTriangleRayIntersect(const LLVector4a& vert0, const LLVector4a& vert1, co pvec.setCross3(dir, edge2); /* if determinant is near zero, ray lies in plane of triangle */ - F32 det = edge1.dot3(pvec); - - if (!two_sided) + LLVector4a det; + det.setAllDot3(edge1, pvec); + + if (det.greaterEqual4(LLVector4a::getApproximatelyZero()).getComparisonMask()) { - if (det < F_APPROXIMATELY_ZERO) - { - return FALSE; - } - /* calculate distance from vert0 to ray origin */ LLVector4a tvec; tvec.setSub(orig, vert0); /* calculate U parameter and test bounds */ - u = tvec.dot3(pvec); + LLVector4a u; + u.setAllDot3(tvec,pvec); - if (u < 0.f || u > det) + if (u.greaterEqual4(LLVector4a::getZero()).getComparisonMask() && + u.lessEqual4(det).getComparisonMask()) { - return FALSE; + /* prepare to test V parameter */ + LLVector4a qvec; + qvec.setCross3(tvec, edge1); + + /* calculate V parameter and test bounds */ + LLVector4a v; + v.setAllDot3(dir, qvec); + + + //if (!(v < 0.f || u + v > det)) + + LLVector4a sum_uv; + sum_uv.setAdd(u, v); + + S32 v_gequal = v.greaterEqual4(LLVector4a::getZero()).getComparisonMask(); + S32 sum_lequal = sum_uv.lessEqual4(det).getComparisonMask(); + + if (v_gequal && sum_lequal) + { + /* calculate t, scale parameters, ray intersects triangle */ + LLVector4a t; + t.setAllDot3(edge2,qvec); + + t.div(det); + u.div(det); + v.div(det); + + intersection_a = u[0]; + intersection_b = v[0]; + intersection_t = t[0]; + return TRUE; + } } - - /* prepare to test V parameter */ - LLVector4a qvec; - qvec.setCross3(tvec, edge1); + } - /* calculate V parameter and test bounds */ - v = dir.dot3(qvec); - if (v < 0.f || u + v > det) - { - return FALSE; - } + return FALSE; +} - /* calculate t, scale parameters, ray intersects triangle */ - t = edge2.dot3(qvec); - F32 inv_det = 1.0 / det; - t *= inv_det; - u *= inv_det; - v *= inv_det; - } +BOOL LLTriangleRayIntersectTwoSided(const LLVector4a& vert0, const LLVector4a& vert1, const LLVector4a& vert2, const LLVector4a& orig, const LLVector4a& dir, + F32& intersection_a, F32& intersection_b, F32& intersection_t) +{ + F32 u, v, t; - else // two sided - { - if (det > -F_APPROXIMATELY_ZERO && det < F_APPROXIMATELY_ZERO) - { - return FALSE; - } - F32 inv_det = 1.0 / det; + /* find vectors for two edges sharing vert0 */ + LLVector4a edge1; + edge1.setSub(vert1, vert0); + + + LLVector4a edge2; + edge2.setSub(vert2, vert0); - /* calculate distance from vert0 to ray origin */ - LLVector4a tvec; - tvec.setSub(orig, vert0); - - /* calculate U parameter and test bounds */ - u = (tvec.dot3(pvec)) * inv_det; - if (u < 0.f || u > 1.f) - { - return FALSE; - } + /* begin calculating determinant - also used to calculate U parameter */ + LLVector4a pvec; + pvec.setCross3(dir, edge2); - /* prepare to test V parameter */ - LLVector4a qvec; - qvec.setSub(tvec, edge1); - - /* calculate V parameter and test bounds */ - v = (dir.dot3(qvec)) * inv_det; - - if (v < 0.f || u + v > 1.f) - { - return FALSE; - } + /* if determinant is near zero, ray lies in plane of triangle */ + F32 det = edge1.dot3(pvec); + + + if (det > -F_APPROXIMATELY_ZERO && det < F_APPROXIMATELY_ZERO) + { + return FALSE; + } + + F32 inv_det = 1.f / det; + + /* calculate distance from vert0 to ray origin */ + LLVector4a tvec; + tvec.setSub(orig, vert0); + + /* calculate U parameter and test bounds */ + u = (tvec.dot3(pvec)) * inv_det; + if (u < 0.f || u > 1.f) + { + return FALSE; + } - /* calculate t, ray intersects triangle */ - t = (edge2.dot3(qvec)) * inv_det; + /* prepare to test V parameter */ + tvec.sub(edge1); + + /* calculate V parameter and test bounds */ + v = (dir.dot3(tvec)) * inv_det; + + if (v < 0.f || u + v > 1.f) + { + return FALSE; } + + /* calculate t, ray intersects triangle */ + t = (edge2.dot3(tvec)) * inv_det; - if (intersection_a != NULL) - *intersection_a = u; - if (intersection_b != NULL) - *intersection_b = v; - if (intersection_t != NULL) - *intersection_t = t; + intersection_a = u; + intersection_b = v; + intersection_t = t; return TRUE; @@ -244,7 +317,7 @@ BOOL LLTriangleRayIntersect(const LLVector4a& vert0, const LLVector4a& vert1, co //helper for non-aligned vectors BOOL LLTriangleRayIntersect(const LLVector3& vert0, const LLVector3& vert1, const LLVector3& vert2, const LLVector3& orig, const LLVector3& dir, - F32* intersection_a, F32* intersection_b, F32* intersection_t, BOOL two_sided) + F32& intersection_a, F32& intersection_b, F32& intersection_t, BOOL two_sided) { LLVector4a vert0a, vert1a, vert2a, origa, dira; vert0a.load3(vert0.mV); @@ -253,11 +326,130 @@ BOOL LLTriangleRayIntersect(const LLVector3& vert0, const LLVector3& vert1, cons origa.load3(orig.mV); dira.load3(dir.mV); - return LLTriangleRayIntersect(vert0a, vert1a, vert2a, origa, dira, - intersection_a, intersection_b, intersection_t, two_sided); + if (two_sided) + { + return LLTriangleRayIntersectTwoSided(vert0a, vert1a, vert2a, origa, dira, + intersection_a, intersection_b, intersection_t); + } + else + { + return LLTriangleRayIntersect(vert0a, vert1a, vert2a, origa, dira, + intersection_a, intersection_b, intersection_t); + } } +class LLVolumeOctreeListener : public LLOctreeListener<LLVolumeFace::Triangle> +{ +public: + + LLVolumeOctreeListener(LLOctreeNode<LLVolumeFace::Triangle>* node) + { + node->addListener(this); + + mBounds = (LLVector4a*) _mm_malloc(sizeof(LLVector4a)*4, 16); + mExtents = mBounds+2; + } + + ~LLVolumeOctreeListener() + { + _mm_free(mBounds); + } + + //LISTENER FUNCTIONS + virtual void handleChildAddition(const LLOctreeNode<LLVolumeFace::Triangle>* parent, + LLOctreeNode<LLVolumeFace::Triangle>* child) + { + new LLVolumeOctreeListener(child); + } + + virtual void handleStateChange(const LLTreeNode<LLVolumeFace::Triangle>* node) { } + virtual void handleChildRemoval(const LLOctreeNode<LLVolumeFace::Triangle>* parent, + const LLOctreeNode<LLVolumeFace::Triangle>* child) { } + virtual void handleInsertion(const LLTreeNode<LLVolumeFace::Triangle>* node, LLVolumeFace::Triangle* tri) { } + virtual void handleRemoval(const LLTreeNode<LLVolumeFace::Triangle>* node, LLVolumeFace::Triangle* tri) { } + virtual void handleDestruction(const LLTreeNode<LLVolumeFace::Triangle>* node) { } + + +public: + LLVector4a* mBounds; // bounding box (center, size) of this node and all its children (tight fit to objects) + LLVector4a* mExtents; // extents (min, max) of this node and all its children +}; + +class LLVolumeOctreeRebound : public LLOctreeTravelerDepthFirst<LLVolumeFace::Triangle> +{ +public: + const LLVolumeFace* mFace; + + LLVolumeOctreeRebound(const LLVolumeFace* face) + { + mFace = face; + } + + virtual void visit(const LLOctreeNode<LLVolumeFace::Triangle>* branch) + { + LLVolumeOctreeListener* node = (LLVolumeOctreeListener*) branch->getListener(0); + + LLVector4a& min = node->mExtents[0]; + LLVector4a& max = node->mExtents[1]; + + if (branch->getElementCount() != 0) + { + const LLVolumeFace::Triangle* tri = *(branch->getData().begin()); + + min = *(tri->mV[0]); + max = *(tri->mV[0]); + + for (LLOctreeNode<LLVolumeFace::Triangle>::const_element_iter iter = + branch->getData().begin(); iter != branch->getData().end(); ++iter) + { + //stretch by triangles in node + tri = *iter; + + min.setMin(*tri->mV[0]); + min.setMin(*tri->mV[1]); + min.setMin(*tri->mV[2]); + + max.setMax(*tri->mV[0]); + max.setMax(*tri->mV[1]); + max.setMax(*tri->mV[2]); + } + + for (S32 i = 0; i < branch->getChildCount(); ++i) + { //stretch by child extents + LLVolumeOctreeListener* child = (LLVolumeOctreeListener*) branch->getChild(i)->getListener(0); + min.setMin(child->mExtents[0]); + max.setMax(child->mExtents[1]); + } + } + else if (branch->getChildCount() != 0) + { + LLVolumeOctreeListener* child = (LLVolumeOctreeListener*) branch->getChild(0)->getListener(0); + + min = child->mExtents[0]; + max = child->mExtents[1]; + + for (S32 i = 1; i < branch->getChildCount(); ++i) + { //stretch by child extents + child = (LLVolumeOctreeListener*) branch->getChild(i)->getListener(0); + min.setMin(child->mExtents[0]); + max.setMax(child->mExtents[1]); + } + } + else + { + llerrs << "WTF? Empty leaf" << llendl; + } + + node->mBounds[0].setAdd(min, max); + node->mBounds[0].mul(0.5f); + + node->mBounds[1].setSub(max,min); + node->mBounds[1].mul(0.5f); + } +}; + + //------------------------------------------------------------------- // statics //------------------------------------------------------------------- @@ -4244,6 +4436,114 @@ S32 LLVolume::lineSegmentIntersect(const LLVector3& start, const LLVector3& end, } +class LLOctreeTriangleRayIntersect : public LLOctreeTraveler<LLVolumeFace::Triangle> +{ +public: + const LLVolumeFace* mFace; + LLVector4a mStart; + LLVector4a mDir; + LLVector4a mEnd; + LLVector3* mIntersection; + LLVector2* mTexCoord; + LLVector3* mNormal; + LLVector3* mBinormal; + F32* mClosestT; + bool mHitFace; + + LLOctreeTriangleRayIntersect(const LLVector4a& start, const LLVector4a& dir, + const LLVolumeFace* face, F32* closest_t, + LLVector3* intersection,LLVector2* tex_coord, LLVector3* normal, LLVector3* bi_normal) + : mFace(face), + mStart(start), + mDir(dir), + mIntersection(intersection), + mTexCoord(tex_coord), + mNormal(normal), + mBinormal(bi_normal), + mClosestT(closest_t), + mHitFace(false) + { + mEnd.setAdd(mStart, mDir); + } + + void traverse(const LLOctreeNode<LLVolumeFace::Triangle>* node) + { + LLVolumeOctreeListener* vl = (LLVolumeOctreeListener*) node->getListener(0); + + /*const F32* start = mStart.getF32(); + const F32* end = mEnd.getF32(); + const F32* center = vl->mBounds[0].getF32(); + const F32* size = vl->mBounds[1].getF32();*/ + + if (LLLineSegmentBoxIntersect(mStart, mEnd, vl->mBounds[0], vl->mBounds[1])) + { + node->accept(this); + for (S32 i = 0; i < node->getChildCount(); ++i) + { + traverse(node->getChild(i)); + } + } + } + + void visit(const LLOctreeNode<LLVolumeFace::Triangle>* node) + { + for (LLOctreeNode<LLVolumeFace::Triangle>::const_element_iter iter = + node->getData().begin(); iter != node->getData().end(); ++iter) + { + const LLVolumeFace::Triangle* tri = *iter; + + F32 a, b, t; + + if (LLTriangleRayIntersect(*tri->mV[0], *tri->mV[1], *tri->mV[2], + mStart, mDir, a, b, t)) + { + if ((t >= 0.f) && // if hit is after start + (t <= 1.f) && // and before end + (t < *mClosestT)) // and this hit is closer + { + *mClosestT = t; + mHitFace = true; + + if (mIntersection != NULL) + { + LLVector4a intersect = mDir; + intersect.mul(*mClosestT); + intersect.add(mStart); + mIntersection->set(intersect.getF32()); + } + + + if (mTexCoord != NULL) + { + LLVector2* tc = (LLVector2*) mFace->mTexCoords; + *mTexCoord = ((1.f - a - b) * tc[tri->mIndex[0]] + + a * tc[tri->mIndex[1]] + + b * tc[tri->mIndex[2]]); + + } + + if (mNormal != NULL) + { + LLVector4* norm = (LLVector4*) mFace->mNormals; + + *mNormal = ((1.f - a - b) * LLVector3(norm[tri->mIndex[0]]) + + a * LLVector3(norm[tri->mIndex[1]]) + + b * LLVector3(norm[tri->mIndex[2]])); + } + + if (mBinormal != NULL) + { + LLVector4* binormal = (LLVector4*) mFace->mBinormals; + *mBinormal = ((1.f - a - b) * LLVector3(binormal[tri->mIndex[0]]) + + a * LLVector3(binormal[tri->mIndex[1]]) + + b * LLVector3(binormal[tri->mIndex[2]])); + } + } + } + } + } +}; + S32 LLVolume::lineSegmentIntersect(const LLVector4a& start, const LLVector4a& end, S32 face, LLVector3* intersection,LLVector2* tex_coord, LLVector3* normal, LLVector3* bi_normal) @@ -4288,66 +4588,19 @@ S32 LLVolume::lineSegmentIntersect(const LLVector4a& start, const LLVector4a& en { genBinormals(i); } - - LLVector4a* p = (LLVector4a*) face.mPositions; - for (U32 tri = 0; tri < face.mNumIndices/3; tri++) + if (!face.mOctree) { - S32 index1 = face.mIndices[tri*3+0]; - S32 index2 = face.mIndices[tri*3+1]; - S32 index3 = face.mIndices[tri*3+2]; - - F32 a, b, t; + face.createOctree(); + } - if (LLTriangleRayIntersect(p[index1], - p[index2], - p[index3], - start, dir, &a, &b, &t, FALSE)) - { - if ((t >= 0.f) && // if hit is after start - (t <= 1.f) && // and before end - (t < closest_t)) // and this hit is closer - { - closest_t = t; - hit_face = i; - - if (intersection != NULL) - { - LLVector4a intersect = dir; - intersect.mul(closest_t); - intersect.add(start); - intersection->set(intersect.getF32()); - } - - - if (tex_coord != NULL) - { - LLVector2* tc = (LLVector2*) face.mTexCoords; - *tex_coord = ((1.f - a - b) * tc[index1] + - a * tc[index2] + - b * tc[index3]); - - } - - if (normal != NULL) - { - LLVector4* norm = (LLVector4*) face.mNormals; - - *normal = ((1.f - a - b) * LLVector3(norm[index1]) + - a * LLVector3(norm[index2]) + - b * LLVector3(norm[index3])); - } - - if (bi_normal != NULL) - { - LLVector4* binormal = (LLVector4*) face.mBinormals; - *bi_normal = ((1.f - a - b) * LLVector3(binormal[index1]) + - a * LLVector3(binormal[index2]) + - b * LLVector3(binormal[index3])); - } + LLVector4a* p = (LLVector4a*) face.mPositions; - } - } + LLOctreeTriangleRayIntersect intersect(start, dir, &face, &closest_t, intersection, tex_coord, normal, bi_normal); + intersect.traverse(face.mOctree); + if (intersect.mHitFace) + { + hit_face = i; } } } @@ -5128,13 +5381,29 @@ LLVolumeFace::LLVolumeFace() : mBinormals(NULL), mTexCoords(NULL), mIndices(NULL), - mWeights(NULL) + mWeights(NULL), + mOctree(NULL) { mExtents = (LLVector4a*) _mm_malloc(48, 16); mCenter = mExtents+2; } LLVolumeFace::LLVolumeFace(const LLVolumeFace& src) +: mID(0), + mTypeMask(0), + mBeginS(0), + mBeginT(0), + mNumS(0), + mNumT(0), + mNumVertices(0), + mNumIndices(0), + mPositions(NULL), + mNormals(NULL), + mBinormals(NULL), + mTexCoords(NULL), + mIndices(NULL), + mWeights(NULL), + mOctree(NULL) { mExtents = (LLVector4a*) _mm_malloc(48, 16); mCenter = mExtents+2; @@ -5157,13 +5426,9 @@ LLVolumeFace& LLVolumeFace::operator=(const LLVolumeFace& src) mNumVertices = 0; mNumIndices = 0; - mPositions = NULL; - mNormals = NULL; - mBinormals = NULL; - mTexCoords = NULL; - mWeights = NULL; - mIndices = NULL; + freeData(); + LLVector4a::memcpyNonAliased16((F32*) mExtents, (F32*) src.mExtents, 12); resizeVertices(src.mNumVertices); @@ -5179,6 +5444,7 @@ LLVolumeFace& LLVolumeFace::operator=(const LLVolumeFace& src) LLVector4a::memcpyNonAliased16((F32*) mNormals, (F32*) src.mNormals, vert_size); LLVector4a::memcpyNonAliased16((F32*) mTexCoords, (F32*) src.mTexCoords, vert_size); + if (src.mBinormals) { allocateBinormals(src.mNumVertices); @@ -5217,17 +5483,37 @@ LLVolumeFace& LLVolumeFace::operator=(const LLVolumeFace& src) LLVolumeFace::~LLVolumeFace() { + _mm_free(mExtents); + mExtents = NULL; + + freeData(); +} + +void LLVolumeFace::freeData() +{ _mm_free(mPositions); + mPositions = NULL; _mm_free(mNormals); + mNormals = NULL; _mm_free(mTexCoords); + mTexCoords = NULL; _mm_free(mIndices); + mIndices = NULL; _mm_free(mBinormals); + mBinormals = NULL; _mm_free(mWeights); - _mm_free(mExtents); + mWeights = NULL; + + delete mOctree; + mOctree = NULL; } BOOL LLVolumeFace::create(LLVolume* volume, BOOL partial_build) { + //tree for this face is no longer valid + delete mOctree; + mOctree = NULL; + if (mTypeMask & CAP_MASK) { return createCap(volume, partial_build); @@ -5250,6 +5536,18 @@ void LLVolumeFace::getVertexData(U16 index, LLVolumeFace::VertexData& cv) cv.mTexCoord = mTexCoords[index]; } +bool LLVolumeFace::VertexMapData::operator==(const LLVolumeFace::VertexData& rhs) const +{ + return getPosition().equal3(rhs.getPosition()) && + mTexCoord == rhs.mTexCoord && + getNormal().equal3(rhs.getNormal()); +} + +bool LLVolumeFace::VertexMapData::ComparePosition::operator()(const LLVector4a& a, const LLVector4a& b) const +{ + return a.less3(b); +} + void LLVolumeFace::optimize(F32 angle_cutoff) { LLVolumeFace new_face; @@ -5305,6 +5603,65 @@ void LLVolumeFace::optimize(F32 angle_cutoff) swapData(new_face); } + +void LLVolumeFace::createOctree() +{ + mOctree = new LLOctreeRoot<Triangle>(LLVector3d(0,0,0), LLVector3d(1,1,1), NULL); + new LLVolumeOctreeListener(mOctree); + + for (U32 i = 0; i < mNumIndices; i+= 3) + { + Triangle* tri = new Triangle(); + + const LLVector4a& v0 = mPositions[mIndices[i]]; + const LLVector4a& v1 = mPositions[mIndices[i+1]]; + const LLVector4a& v2 = mPositions[mIndices[i+2]]; + + tri->mV[0] = &v0; + tri->mV[1] = &v1; + tri->mV[2] = &v2; + + tri->mIndex[0] = mIndices[i]; + tri->mIndex[1] = mIndices[i+1]; + tri->mIndex[2] = mIndices[i+2]; + + LLVector4a min = v0; + min.setMin(v1); + min.setMin(v2); + + LLVector4a max = v0; + max.setMax(v1); + max.setMax(v2); + + LLVector4a center; + center.setAdd(min, max); + center.mul(0.5f); + + + tri->mPositionGroup.setVec(center[0], center[1], center[2]); + + LLVector4a size; + size.setSub(max,min); + + tri->mRadius = size.length3() * 0.5f; + + mOctree->insert(tri); + } + + LLVolumeOctreeRebound rebound(this); + rebound.traverse(mOctree); +} + +const LLVector3d& LLVolumeFace::Triangle::getPositionGroup() const +{ + return mPositionGroup; +} + +const F64& LLVolumeFace::Triangle::getBinRadius() const +{ + return mRadius; +} + void LLVolumeFace::swapData(LLVolumeFace& rhs) { llswap(rhs.mPositions, mPositions); |