www.pudn.com > mfcopentree.rar > 3DMath.cpp
#include "stdafx.h" #include// We include math.h so we can use the sqrt() function #include "3DMath.h" /////// * /////////// * /////////// * NEW * /////// * /////////// * /////////// * #include // This is so we can use _isnan() for acos() #include "resource.h" // // // This file was build from the Ray Plane Collision tutorial. // We added 5 new functions to this math file: // // This returns the dot product between 2 vectors // float Dot(CVector3 vVector1, CVector3 vVector2); // // This returns the angle between 2 vectors // double AngleBetweenVectors(CVector3 Vector1, CVector3 Vector2); // // This returns an intersection point of a polygon and a line (assuming intersects the plane) // CVector3 IntersectionPoint(CVector3 vNormal, CVector3 vLine[], double distance); // // This returns true if the intersection point is inside of the polygon // bool InsidePolygon(CVector3 vIntersection, CVector3 Poly[], long verticeCount); // // Use this function to test collision between a line and polygon // bool IntersectedPolygon(CVector3 vPoly[], CVector3 vLine[], int verticeCount); // // These will enable to check if we internet not just the plane of a polygon, // but the actual polygon itself. Once the line is outside the permiter, it will fail // on a collision test. // // //武汉华中科技大学，2002，（C）版权所有 //作者：金德才 联系方式：iskyflying@163.com //请不要将本软件的任何一部分用于商业用途 //如果您觉得任何地方有用或者错误，请告诉作者，谢谢！ //如果你想引用部分源程序，请注明作者信息 /////// * /////////// * /////////// * NEW * /////// * /////////// * /////////// * int whichnode = 1;//处理到第几个子节点 long zongshu = 0;//均匀切分的子节点总数 double zongshu_d = 0;//跟踪zongshu的double值 /////////////////////////////////////// CROSS \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This returns a perpendicular vector from 2 given vectors by taking the cross product. ///// /////////////////////////////////////// CROSS \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* CVector3 Cross(CVector3 vVector1, CVector3 vVector2) { CVector3 vNormal; // The vector to hold the cross product // Once again, if we are given 2 vectors (directions of 2 sides of a polygon) // then we have a plane define. The cross product finds a vector that is perpendicular // to that plane, which means it's point straight out of the plane at a 90 degree angle. // The X value for the vector is: (V1.y * V2.z) - (V1.z * V2.y) // Get the X value vNormal.x = ((vVector1.y * vVector2.z) - (vVector1.z * vVector2.y)) ; if(vNormal.x < 0.000001 && vNormal.x > -0.000001) vNormal.x = 0; // The Y value for the vector is: (V1.z * V2.x) - (V1.x * V2.z) vNormal.y = ((vVector1.z * vVector2.x) - (vVector1.x * vVector2.z)); if(vNormal.y < 0.000001 && vNormal.y > -0.000001) vNormal.y = 0; // The Z value for the vector is: (V1.x * V2.y) - (V1.y * V2.x) vNormal.z = ((vVector1.x * vVector2.y) - (vVector1.y * vVector2.x)); if(vNormal.z < 0.000001 && vNormal.z > -0.000001) vNormal.z = 0; return vNormal; // Return the cross product (Direction the polygon is facing - Normal) } /////////////////////////////////////// VECTOR \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This returns a vector between 2 points ///// /////////////////////////////////////// VECTOR \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* CVector3 Vector(CVector3 vPoint1, CVector3 vPoint2) { CVector3 vVector = {0}; // Initialize our variable to zero // In order to get a vector from 2 points (a direction) we need to // subtract the second point from the first point. vVector.x = vPoint1.x - vPoint2.x; // Get the X value of our new vector vVector.y = vPoint1.y - vPoint2.y; // Get the Y value of our new vector vVector.z = vPoint1.z - vPoint2.z; // Get the Z value of our new vector return vVector; // Return our new vector } /////////////////////////////////////// MAGNITUDE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This returns the magnitude of a normal (or any other vector) ///// /////////////////////////////////////// MAGNITUDE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* float Magnitude(CVector3 vNormal) { // This will give us the magnitude or "Norm" as some say, of our normal. // Here is the equation: magnitude = sqrt(V.x^2 + V.y^2 + V.z^2) Where V is the vector return (float)sqrt( (vNormal.x * vNormal.x) + (vNormal.y * vNormal.y) + (vNormal.z * vNormal.z) ); } /////////////////////////////////////// NORMALIZE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This returns a normalize vector (A vector exactly of length 1) ///// /////////////////////////////////////// NORMALIZE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* CVector3 Normalize(CVector3 vNormal) { float magnitude = Magnitude(vNormal); // Get the magnitude of our normal // Now that we have the magnitude, we can divide our normal by that magnitude. // That will make our normal a total length of 1. This makes it easier to work with too. vNormal.x /= magnitude; // Divide the X value of our normal by it's magnitude vNormal.y /= magnitude; // Divide the Y value of our normal by it's magnitude vNormal.z /= magnitude; // Divide the Z value of our normal by it's magnitude // Finally, return our normalized normal. return vNormal; // Return the new normal of length 1. } /////////////////////////////////////// NORMAL \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This returns the normal of a polygon (The direction the polygon is facing) ///// /////////////////////////////////////// NORMAL \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* CVector3 NormalVector(CVector3 vTriangle[]) { // Get 2 vectors from the polygon (2 sides), Remember the order! CVector3 vVector1 = Vector(vTriangle[2], vTriangle[0]); CVector3 vVector2 = Vector(vTriangle[1], vTriangle[0]); CVector3 vNormal = Cross(vVector1, vVector2); // Take the cross product of our 2 vectors to get a perpendicular vector // Now we have a normal, but it's at a strange length, so let's make it length 1. vNormal = Normalize(vNormal); // Use our function we created to normalize the normal (Makes it a length of one) return vNormal; // Return our normal at our desired length } /////////////////////////////////// PLANE DISTANCE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This returns the distance between a plane and the origin ///// /////////////////////////////////// PLANE DISTANCE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* float PlaneDistance(CVector3 Normal, CVector3 Point) { float distance = 0; // This variable holds the distance from the plane tot he origin // Use the plane equation to find the distance (Ax + By + Cz + D = 0) We want to find D. // So, we come up with D = -(Ax + By + Cz) // Basically, the negated dot product of the normal of the plane and the point. (More about the dot product in another tutorial) distance = - ((Normal.x * Point.x) + (Normal.y * Point.y) + (Normal.z * Point.z)); return distance; // Return the distance } /////// * /////////// * /////////// * NEW * /////// * /////////// * /////////// * // Since the last tutorial, we added 2 more parameters for the normal and the distance // from the origin. This is so we don't have to recalculate it 3 times in our IntersectionPoint() // IntersectedPolygon() functions. We would probably make 2 different functions for // this so we have the choice of getting the normal and distance back, or not. // I also changed the vTriangle to "vPoly" because it isn't always a triangle. // The code doesn't change, it's just more correct (though we only need 3 points anyway). // For C programmers, the '&' is called a reference and is the same concept as the '*' for addressing. /////////////////////////////////// INTERSECTED PLANE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This checks to see if a line intersects a plane ///// /////////////////////////////////// INTERSECTED PLANE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* bool IntersectedPlane_2(CVector3 vLine[], CVector3 vNormal, float originDistance) { float distance1=0, distance2=0; distance1 = ((vNormal.x * vLine[0].x) + // Ax + (vNormal.y * vLine[0].y) + // By + (vNormal.z * vLine[0].z)) + originDistance; // Cz + D // Get the distance from point2 from the plane using Ax + By + Cz + D = (The distance from the plane) distance2 = ((vNormal.x * vLine[1].x) + // Ax + (vNormal.y * vLine[1].y) + // By + (vNormal.z * vLine[1].z)) + originDistance; // Cz + D if(distance1 * distance2 > 0) // Check to see if both point's distances are both negative or both positive return false; // Return false if each point has the same sign. -1 and 1 would mean each point is on either side of the plane. -1 -2 or 3 4 wouldn't... else return true; /* if(distance1 == 0 && distance2 ==0) return true; return true; */ } bool IntersectedPlane(CVector3 vPoly[], CVector3 vLine[], CVector3 &vNormal, float &originDistance) { float distance1=0, distance2=0; // The distances from the 2 points of the line from the plane vNormal = NormalVector(vPoly); // We need to get the normal of our plane to go any further // Let's find the distance our plane is from the origin. We can find this value // from the normal to the plane (polygon) and any point that lies on that plane (Any vertice) originDistance = PlaneDistance(vNormal, vPoly[0]); // Get the distance from point1 from the plane using: Ax + By + Cz + D = (The distance from the plane) distance1 = ((vNormal.x * vLine[0].x) + // Ax + (vNormal.y * vLine[0].y) + // Bx + (vNormal.z * vLine[0].z)) + originDistance; // Cz + D // Get the distance from point2 from the plane using Ax + By + Cz + D = (The distance from the plane) distance2 = ((vNormal.x * vLine[1].x) + // Ax + (vNormal.y * vLine[1].y) + // Bx + (vNormal.z * vLine[1].z)) + originDistance; // Cz + D // Now that we have 2 distances from the plane, if we times them together we either // get a positive or negative number. If it's a negative number, that means we collided! // This is because the 2 points must be on either side of the plane (IE. -1 * 1 = -1). if(distance1 * distance2 >= 0) // Check to see if both point's distances are both negative or both positive return false; // Return false if each point has the same sign. -1 and 1 would mean each point is on either side of the plane. -1 -2 or 3 4 wouldn't... return true; // The line intersected the plane, Return TRUE } /////////////////////////////////// DOT \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This computers the dot product of 2 vectors ///// /////////////////////////////////// DOT \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* float Dot(CVector3 vVector1, CVector3 vVector2) { // The dot product is this equation: V1.V2 = (V1.x * V2.x + V1.y * V2.y + V1.z * V2.z) // In math terms, it looks like this: V1.V2 = ||V1|| ||V2|| cos(theta) // The '.' means DOT. The || || is magnitude. So the magnitude of V1 times the magnitude // of V2 times the cosine of the angle. It seems confusing now, but it will become more clear. // This function is used for a ton of things, which we will cover in other tutorials. // For this tutorial, we use it to compute the angle between 2 vectors. If the vectors // are normalize, the dot product returns the cosine of the angle between the 2 vectors. // What does that mean? Well, it doesn't return the actual angle, it returns the value of: // cos(angle). Well, what if we want to get the actual angle? Then we use the arc cosine. // There is more on this in the below function AngleBetweenVectors(). Let's give some // applications of using the dot product. How would you tell if the angle between the // 2 vectors is perpendicular (90 degrees)? Well, if we normalize the vectors we can // get rid of the ||V1|| * ||V2|| in front, which just leaves us with: cos(theta). // If a vector is normalize, it's magnitude is 1, so it would be: 1 * 1 * cos(theta) , // which is pointless, so we discard that part of the equation. So, What is the cosine of 90? // If you punch it in your calculator you will find that it's 0. So that means // if the dot product of 2 angles is 0, then they are perpendicular. What we did in // our mind is take the arc cosine of 0, which is 90 (or PI/2 in radians). More on this below. // (V1.x * V2.x + V1.y * V2.y + V1.z * V2.z) return ( (vVector1.x * vVector2.x) + (vVector1.y * vVector2.y) + (vVector1.z * vVector2.z) ); } /////////////////////////////////// ANGLE BETWEEN VECTORS \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This checks to see if a point is inside the ranges of a polygon ///// /////////////////////////////////// ANGLE BETWEEN VECTORS \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* double AngleBetweenVectors(CVector3 Vector1, CVector3 Vector2) { // Remember, above we said that the Dot Product of returns the cosine of the angle // between 2 vectors? Well, that is assuming they are unit vectors (normalize vectors). // So, if we don't have a unit vector, then instead of just saying arcCos(DotProduct(A, B)) // We need to divide the dot product by the magnitude of the 2 vectors multiplied by each other. // Here is the equation: arc cosine of (V . W / || V || * || W || ) // the || V || means the magnitude of V. This then cancels out the magnitudes dot product magnitudes. // But basically, if you have normalize vectors already, you can forget about the magnitude part. // Get the dot product of the vectors float dotProduct = Dot(Vector1, Vector2); // Get the product of both of the vectors magnitudes float vectorsMagnitude = Magnitude(Vector1) * Magnitude(Vector2) ; // Get the arc cosine of the (dotProduct / vectorsMagnitude) which is the angle in RADIANS. // (IE. PI/2 radians = 90 degrees PI radians = 180 degrees 2*PI radians = 360 degrees) // To convert radians to degress use this equation: radians * (PI / 180) // TO convert degrees to radians use this equation: degrees * (180 / PI) double angle = acos( dotProduct / vectorsMagnitude ); // Here we make sure that the angle is not a -1.#IND0000000 number, which means indefinate. // acos() thinks it's funny when it returns -1.#IND0000000. If we don't do this check, // our collision results will sometimes say we are colliding when we aren't. I found this // out the hard way after MANY hours and already wrong written tutorials :) Usually // this value is found when the dot product and the maginitude are the same value. // We want to return 0 when this happens. if(_isnan(angle)) return 0; // Return the angle in radians return( angle ); } /////////////////////////////////// INTERSECTION POINT \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This returns the intersection point of the line that intersects the plane ///// /////////////////////////////////// INTERSECTION POINT \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* CVector3 IntersectionPoint(CVector3 vNormal, CVector3 vLine[], double distance) { CVector3 vPoint = {0}, vLineDir = {0}; // Variables to hold the point and the line's direction double Numerator = 0.0, Denominator = 0.0, dist = 0.0; // Here comes the confusing part. We need to find the 3D point that is actually // on the plane. Here are some steps to do that: // 1) First we need to get the vector of our line, Then normalize it so it's a length of 1 vLineDir = Vector(vLine[1], vLine[0]); // Get the Vector of the line vLineDir = Normalize(vLineDir); // Normalize the lines vector // 2) Use the plane equation (distance = Ax + By + Cz + D) to find the distance from one of our points to the plane. // Here I just chose a arbitrary point as the point to find that distance. You notice we negate that // distance. We negate the distance because we want to eventually go BACKWARDS from our point to the plane. // By doing this is will basically bring us back to the plane to find our intersection point. Numerator = - (vNormal.x * vLine[0].x + // Use the plane equation with the normal and the line vNormal.y * vLine[0].y + vNormal.z * vLine[0].z + distance); // 3) If we take the dot product between our line vector and the normal of the polygon, // this will give us the cosine of the angle between the 2 (since they are both normalized - length 1). // We will then divide our Numerator by this value to find the offset towards the plane from our arbitrary point. Denominator = Dot(vNormal, vLineDir); // Get the dot product of the line's vector and the normal of the plane // Since we are using division, we need to make sure we don't get a divide by zero error // If we do get a 0, that means that there are INFINATE points because the the line is // on the plane (the normal is perpendicular to the line - (Normal.Vector = 0)). // In this case, we should just return any point on the line. if( Denominator == 0.0) // Check so we don't divide by zero return vLine[0]; // Return an arbitrary point on the line // We divide the (distance from the point to the plane) by (the dot product) // to get the distance (dist) that we need to move from our arbitrary point. We need // to then times this distance (dist) by our line's vector (direction). When you times // a scalar (single number) by a vector you move along that vector. That is what we are // doing. We are moving from our arbitrary point we chose from the line BACK to the plane // along the lines vector. It seems logical to just get the numerator, which is the distance // from the point to the line, and then just move back that much along the line's vector. // Well, the distance from the plane means the SHORTEST distance. What about in the case that // the line is almost parallel with the polygon, but doesn't actually intersect it until half // way down the line's length. The distance from the plane is short, but the distance from // the actual intersection point is pretty long. If we divide the distance by the dot product // of our line vector and the normal of the plane, we get the correct length. Cool huh? dist = Numerator / Denominator; // Divide to get the multiplying (percentage) factor // Now, like we said above, we times the dist by the vector, then add our arbitrary point. // This essentially moves the point along the vector to a certain distance. This now gives // us the intersection point. Yay! vPoint.x = (float)(vLine[0].x + (vLineDir.x * dist)); vPoint.y = (float)(vLine[0].y + (vLineDir.y * dist)); vPoint.z = (float)(vLine[0].z + (vLineDir.z * dist)); return vPoint; // Return the intersection point } /////////////////////////////////// INSIDE POLYGON \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This checks to see if a point is inside the ranges of a polygon ///// /////////////////////////////////// INSIDE POLYGON \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* bool InsidePolygon(CVector3 vIntersection, CVector3 Poly[], long verticeCount) { const double MATCH_FACTOR = 0.9999; // Used to cover up the error in floating point double Angle = 0.0; // Initialize the angle CVector3 vA, vB; // Create temp vectors // Just because we intersected the plane, doesn't mean we were anywhere near the polygon. // This functions checks our intersection point to make sure it is inside of the polygon. // This is another tough function to grasp at first, but let me try and explain. // It's a brilliant method really, what it does is create triangles within the polygon // from the intersection point. It then adds up the inner angle of each of those triangles. // If the angles together add up to 360 degrees (or 2 * PI in radians) then we are inside! // If the angle is under that value, we must be outside of polygon. To further // understand why this works, take a pencil and draw a perfect triangle. Draw a dot in // the middle of the triangle. Now, from that dot, draw a line to each of the vertices. // Now, we have 3 triangles within that triangle right? Now, we know that if we add up // all of the angles in a triangle we get 360 right? Well, that is kinda what we are doing, // but the inverse of that. Say your triangle is an isosceles triangle, so add up the angles // and you will get 360 degree angles. 90 + 90 + 90 is 360. for (int i = 0; i < verticeCount; i++) // Go in a circle to each vertex and get the angle between { vA = Vector(Poly[i], vIntersection); // Subtract the intersection point from the current vertex // Subtract the point from the next vertex vB = Vector(Poly[(i + 1) % verticeCount], vIntersection); Angle += AngleBetweenVectors(vA, vB); // Find the angle between the 2 vectors and add them all up as we go along } // Now that we have the total angles added up, we need to check if they add up to 360 degrees. // Since we are using the dot product, we are working in radians, so we check if the angles // equals 2*PI. We defined PI in 3DMath.h. You will notice that we use a MATCH_FACTOR // in conjunction with our desired degree. This is because of the inaccuracy when working // with floating point numbers. It usually won't always be perfectly 2 * PI, so we need // to use a little twiddling. I use .9999, but you can change this to fit your own desired accuracy. if(Angle >= (MATCH_FACTOR * (2.0 * PI)) ) // If the angle is greater than 2 PI, (360 degrees) return TRUE; // The point is inside of the polygon return FALSE; // If you get here, it obviously wasn't inside the polygon, so Return FALSE } /////////////////////////////////// INTERSECTED POLYGON \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This checks if a line is intersecting a polygon ///// /////////////////////////////////// INTERSECTED POLYGON \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* bool IntersectedPolygon(CVector3 vPoly[], CVector3 vLine[], int verticeCount) { CVector3 vNormal = {0}; float originDistance = 0; // First we check to see if our line intersected the plane. If this isn't true // there is no need to go on, so return false immediately. // We pass in address of vNormal and originDistance so we only calculate it once // Reference // Reference if(!IntersectedPlane(vPoly, vLine, vNormal, originDistance)) return false; // Now that we have our normal and distance passed back from IntersectedPlane(), // we can use it to calculate the intersection point. The intersection point // is the point that actually is ON the plane. It is between the line. We need // this point test next, if we are inside the polygon. To get the I-Point, we // give our function the normal of the plan, the points of the line, and the originDistance. CVector3 vIntersection = IntersectionPoint(vNormal, vLine, originDistance); //排除端点的情形 for(int i=0; i<2; i++) { if(vIntersection.x == vLine[i].x && vIntersection.y == vLine[i].y && vIntersection.z == vLine[i].z) return FALSE; } // Now that we have the intersection point, we need to test if it's inside the polygon. // To do this, we pass in : // (our intersection point, the polygon, and the number of vertices our polygon has) if(InsidePolygon(vIntersection, vPoly, verticeCount)) return true; // We collided! Return success // If we get here, we must have NOT collided return false; // There was no collision, so return false } bool IntersectedPolygon2(CVector3 tri[3], CVector3 nor, CVector3 vLine[], float dis)//三角形平面方程已知 { CVector3 vNormal = {0}; if(!IntersectedPlane_2(vLine, nor, dis)) return false; CVector3 vIntersection = IntersectionPoint(nor, vLine, dis); /* //排除端点的情形 for(int i=0; i<2; i++) { if(vIntersection.x == vLine[i].x && vIntersection.y == vLine[i].y && vIntersection.z == vLine[i].z) return FALSE; } */ // Now that we have the intersection point, we need to test if it's inside the polygon. // To do this, we pass in : // (our intersection point, the polygon, and the number of vertices our polygon has) if(InsidePolygon(vIntersection, tri, 3)) return true; // We collided! Return success // If we get here, we must have NOT collided return false; // There was no collision, so return false } bool IntersectedPolygon3(CVector3 vPoly[], CVector3 nor, CVector3 vLine[], int verticeCount, float dis) { CVector3 vNormal = {0}; if(!IntersectedPlane_2(vLine, nor, dis)) return false; CVector3 vIntersection = IntersectionPoint(nor, vLine, dis); // Now that we have the intersection point, we need to test if it's inside the polygon. // To do this, we pass in : // (our intersection point, the polygon, and the number of vertices our polygon has) if(InsidePolygon(vIntersection, vPoly, verticeCount)) return true; // We collided! Return success // If we get here, we must have NOT collided return false; } /////// * /////////// * /////////// * NEW * /////// * /////////// * /////////// * ////////////////////////////////////// //八叉树划分空间算法 ////////////////////////////////////// void CreateNode(STLOctTree *node, STLArray *data, float precision, int i) { if(node->IsHave == 0 || node->m_Width <= precision) { zongshu_d = zongshu_d-pow(node->m_Width/precision, 3); return; } if(whichnode==1)//计算另一棵树 { zongshu_d = pow(node->m_Width/precision, 3); } whichnode++; if(zongshu_d>1000) { if(whichnode%8==0) { zongshu = long(zongshu_d); ::SendMessage(AfxGetApp()->GetMainWnd()->m_hWnd, WM_DISPPROG, whichnode+(i<<16), zongshu); } } else if(zongshu_d<=1000 && zongshu_d>=600) { if(whichnode%4==0) { zongshu = long(zongshu_d); ::SendMessage(AfxGetApp()->GetMainWnd()->m_hWnd, WM_DISPPROG, whichnode+(i<<16), zongshu); } } else { if(whichnode%2==0) { zongshu = long(zongshu_d); ::SendMessage(AfxGetApp()->GetMainWnd()->m_hWnd, WM_DISPPROG, whichnode+(i<<16), zongshu); } } STLOctTree *childnode1 = new STLOctTree; STLOctTree *childnode2 = new STLOctTree; STLOctTree *childnode3 = new STLOctTree; STLOctTree *childnode4 = new STLOctTree; STLOctTree *childnode5 = new STLOctTree; STLOctTree *childnode6 = new STLOctTree; STLOctTree *childnode7 = new STLOctTree; STLOctTree *childnode8 = new STLOctTree; for(int vv=0; vv<8; vv++) { childnode1->pchild[vv] = NULL; childnode2->pchild[vv] = NULL; childnode3->pchild[vv] = NULL; childnode4->pchild[vv] = NULL; childnode5->pchild[vv] = NULL; childnode6->pchild[vv] = NULL; childnode7->pchild[vv] = NULL; childnode8->pchild[vv] = NULL; } node->pchild[0] = childnode1; node->pchild[1] = childnode2; node->pchild[2] = childnode3; node->pchild[3] = childnode4; node->pchild[4] = childnode5; node->pchild[5] = childnode6; node->pchild[6] = childnode7; node->pchild[7] = childnode8; childnode1->m_vCenter.x = node->m_vCenter.x - node->m_Width/4.0f; childnode1->m_vCenter.y = node->m_vCenter.y + node->m_Width/4.0f; childnode1->m_vCenter.z = node->m_vCenter.z + node->m_Width/4.0f; childnode1->m_Width = node->m_Width/2.0f; childnode1->IsHave = ChildIsHave(childnode1, data); CreateNode(childnode1, data, precision, i); childnode2->m_vCenter.x = node->m_vCenter.x - node->m_Width/4.0f; childnode2->m_vCenter.y = node->m_vCenter.y + node->m_Width/4.0f; childnode2->m_vCenter.z = node->m_vCenter.z - node->m_Width/4.0f; childnode2->m_Width = node->m_Width/2.0f; childnode2->IsHave = ChildIsHave(childnode2, data); CreateNode(childnode2, data, precision, i); childnode3->m_vCenter.x = node->m_vCenter.x + node->m_Width/4.0f; childnode3->m_vCenter.y = node->m_vCenter.y + node->m_Width/4.0f; childnode3->m_vCenter.z = node->m_vCenter.z - node->m_Width/4.0f; childnode3->m_Width = node->m_Width/2.0f; childnode3->IsHave = ChildIsHave(childnode3, data); CreateNode(childnode3, data, precision, i); childnode4->m_vCenter.x = node->m_vCenter.x + node->m_Width/4.0f; childnode4->m_vCenter.y = node->m_vCenter.y + node->m_Width/4.0f; childnode4->m_vCenter.z = node->m_vCenter.z + node->m_Width/4.0f; childnode4->m_Width = node->m_Width/2.0f; childnode4->IsHave = ChildIsHave(childnode4, data); CreateNode(childnode4, data, precision, i); childnode5->m_vCenter.x = node->m_vCenter.x - node->m_Width/4.0f; childnode5->m_vCenter.y = node->m_vCenter.y - node->m_Width/4.0f; childnode5->m_vCenter.z = node->m_vCenter.z + node->m_Width/4.0f; childnode5->m_Width = node->m_Width/2.0f; childnode5->IsHave = ChildIsHave(childnode5, data); CreateNode(childnode5, data, precision, i); childnode6->m_vCenter.x = node->m_vCenter.x - node->m_Width/4.0f; childnode6->m_vCenter.y = node->m_vCenter.y - node->m_Width/4.0f; childnode6->m_vCenter.z = node->m_vCenter.z - node->m_Width/4.0f; childnode6->m_Width = node->m_Width/2.0f; childnode6->IsHave = ChildIsHave(childnode6, data); CreateNode(childnode6, data, precision, i); childnode7->m_vCenter.x = node->m_vCenter.x + node->m_Width/4.0f; childnode7->m_vCenter.y = node->m_vCenter.y - node->m_Width/4.0f; childnode7->m_vCenter.z = node->m_vCenter.z - node->m_Width/4.0f; childnode7->m_Width = node->m_Width/2.0f; childnode7->IsHave = ChildIsHave(childnode7, data); CreateNode(childnode7, data, precision, i); childnode8->m_vCenter.x = node->m_vCenter.x + node->m_Width/4.0f; childnode8->m_vCenter.y = node->m_vCenter.y - node->m_Width/4.0f; childnode8->m_vCenter.z = node->m_vCenter.z + node->m_Width/4.0f; childnode8->m_Width = node->m_Width/2.0f; childnode8->IsHave = ChildIsHave(childnode8, data); CreateNode(childnode8, data, precision, i); } int ChildIsHave(STLOctTree *node, STLArray *data) { STLData *tri = data->stl->next; while( tri != NULL ) { //三角形有一点在立方体内 if((tri->fPoint1[0] <= node->m_vCenter.x+node->m_Width/2.0f+0.00001f && tri->fPoint1[0] >= node->m_vCenter.x-node->m_Width/2.0f-0.00001f && tri->fPoint1[1] <= node->m_vCenter.y+node->m_Width/2.0f+0.00001f && tri->fPoint1[1] >= node->m_vCenter.y-node->m_Width/2.0f-0.00001f && tri->fPoint1[2] <= node->m_vCenter.z+node->m_Width/2.0f+0.00001f && tri->fPoint1[2] >= node->m_vCenter.z-node->m_Width/2.0f-0.00001f) || (tri->fPoint2[0] <= node->m_vCenter.x+node->m_Width/2.0f+0.00001f && tri->fPoint2[0] >= node->m_vCenter.x-node->m_Width/2.0f-0.00001f && tri->fPoint2[1] <= node->m_vCenter.y+node->m_Width/2.0f+0.00001f && tri->fPoint2[1] >= node->m_vCenter.y-node->m_Width/2.0f-0.00001f && tri->fPoint2[2] <= node->m_vCenter.z+node->m_Width/2.0f+0.00001f && tri->fPoint2[2] >= node->m_vCenter.z-node->m_Width/2.0f-0.00001f) || (tri->fPoint3[0] <= node->m_vCenter.x+node->m_Width/2.0f+0.00001f && tri->fPoint3[0] >= node->m_vCenter.x-node->m_Width/2.0f-0.00001f && tri->fPoint3[1] <= node->m_vCenter.y+node->m_Width/2.0f+0.00001f && tri->fPoint3[1] >= node->m_vCenter.y-node->m_Width/2.0f-0.00001f && tri->fPoint3[2] <= node->m_vCenter.z+node->m_Width/2.0f+0.00001f && tri->fPoint3[2] >= node->m_vCenter.z-node->m_Width/2.0f-0.00001f)) return 1; //三角形平面方程 float tri_a, tri_b, tri_c, tri_d; CVector3 triangle[3] = { {tri->fPoint1[0], tri->fPoint1[1], tri->fPoint1[2]}, {tri->fPoint2[0], tri->fPoint2[1], tri->fPoint2[2]}, {tri->fPoint3[0], tri->fPoint3[1], tri->fPoint3[2]} }; CVector3 nor = {tri->fNormal[0], tri->fNormal[1], tri->fNormal[2]}; tri_a = nor.x; tri_b = nor.y; tri_c = nor.z; tri_d = PlaneDistance(nor, triangle[0]); float cube_center_tri = tri_a * node->m_vCenter.x + tri_b * node->m_vCenter.y + tri_c * node->m_vCenter.z + tri_d; if(fabs(cube_center_tri) > node->m_Width) { tri = tri->next; continue; } else { //三角形有可能和立方体相交，结果取决于立方体的12条边和三角形的相交情况 CVector3 cubeside[12][2] = {{ {node->m_vCenter.x-node->m_Width/2.0f, node->m_vCenter.y-node->m_Width/2.0f, node->m_vCenter.z+node->m_Width/2.0f}, {node->m_vCenter.x-node->m_Width/2.0f, node->m_vCenter.y+node->m_Width/2.0f, node->m_vCenter.z+node->m_Width/2.0f} }, //前面的四条边 { {node->m_vCenter.x-node->m_Width/2.0f, node->m_vCenter.y+node->m_Width/2.0f, node->m_vCenter.z+node->m_Width/2.0f}, {node->m_vCenter.x+node->m_Width/2.0f, node->m_vCenter.y+node->m_Width/2.0f, node->m_vCenter.z+node->m_Width/2.0f} }, // { {node->m_vCenter.x+node->m_Width/2.0f, node->m_vCenter.y+node->m_Width/2.0f, node->m_vCenter.z+node->m_Width/2.0f}, {node->m_vCenter.x+node->m_Width/2.0f, node->m_vCenter.y-node->m_Width/2.0f, node->m_vCenter.z+node->m_Width/2.0f} }, // { {node->m_vCenter.x+node->m_Width/2.0f, node->m_vCenter.y-node->m_Width/2.0f, node->m_vCenter.z+node->m_Width/2.0f}, {node->m_vCenter.x-node->m_Width/2.0f, node->m_vCenter.y-node->m_Width/2.0f, node->m_vCenter.z+node->m_Width/2.0f} }, // { {node->m_vCenter.x+node->m_Width/2.0f, node->m_vCenter.y-node->m_Width/2.0f, node->m_vCenter.z-node->m_Width/2.0f}, {node->m_vCenter.x+node->m_Width/2.0f, node->m_vCenter.y+node->m_Width/2.0f, node->m_vCenter.z-node->m_Width/2.0f} }, //后面的四条边 { {node->m_vCenter.x+node->m_Width/2.0f, node->m_vCenter.y+node->m_Width/2.0f, node->m_vCenter.z-node->m_Width/2.0f}, {node->m_vCenter.x-node->m_Width/2.0f, node->m_vCenter.y+node->m_Width/2.0f, node->m_vCenter.z-node->m_Width/2.0f} }, // { {node->m_vCenter.x-node->m_Width/2.0f, node->m_vCenter.y+node->m_Width/2.0f, node->m_vCenter.z-node->m_Width/2.0f}, {node->m_vCenter.x-node->m_Width/2.0f, node->m_vCenter.y-node->m_Width/2.0f, node->m_vCenter.z-node->m_Width/2.0f} }, // { {node->m_vCenter.x-node->m_Width/2.0f, node->m_vCenter.y-node->m_Width/2.0f, node->m_vCenter.z-node->m_Width/2.0f}, {node->m_vCenter.x+node->m_Width/2.0f, node->m_vCenter.y-node->m_Width/2.0f, node->m_vCenter.z-node->m_Width/2.0f} }, // { {node->m_vCenter.x-node->m_Width/2.0f, node->m_vCenter.y+node->m_Width/2.0f, node->m_vCenter.z-node->m_Width/2.0f}, {node->m_vCenter.x-node->m_Width/2.0f, node->m_vCenter.y+node->m_Width/2.0f, node->m_vCenter.z+node->m_Width/2.0f} }, //左侧面的两条边 { {node->m_vCenter.x-node->m_Width/2.0f, node->m_vCenter.y-node->m_Width/2.0f, node->m_vCenter.z+node->m_Width/2.0f}, {node->m_vCenter.x-node->m_Width/2.0f, node->m_vCenter.y-node->m_Width/2.0f, node->m_vCenter.z-node->m_Width/2.0f} }, // { {node->m_vCenter.x+node->m_Width/2.0f, node->m_vCenter.y+node->m_Width/2.0f, node->m_vCenter.z+node->m_Width/2.0f}, {node->m_vCenter.x+node->m_Width/2.0f, node->m_vCenter.y+node->m_Width/2.0f, node->m_vCenter.z-node->m_Width/2.0f} }, //右侧面的两条边 { {node->m_vCenter.x+node->m_Width/2.0f, node->m_vCenter.y-node->m_Width/2.0f, node->m_vCenter.z-node->m_Width/2.0f}, {node->m_vCenter.x+node->m_Width/2.0f, node->m_vCenter.y-node->m_Width/2.0f, node->m_vCenter.z+node->m_Width/2.0f} } }; for(int kk=0; kk<12; kk++) { bool m_binter = IntersectedPolygon2(triangle, nor, cubeside[kk], tri_d);//(triangle, cubeside[kk], 3); if(m_binter) return 1; } } tri = tri->next; } return 0; }