# 3d Math functions

From Unify Community Wiki

(Difference between revisions)

Line 1: | Line 1: | ||

<syntaxhighlight lang="csharp"> | <syntaxhighlight lang="csharp"> | ||

+ | |||

//increase or decrease the length of vector by size | //increase or decrease the length of vector by size | ||

Vector3 AddVectorLength(Vector3 vector, float size){ | Vector3 AddVectorLength(Vector3 vector, float size){ | ||

− | + | ||

− | + | //get the vector length | |

− | + | float magnitude = Vector3.Magnitude(vector); | |

− | + | ||

− | + | //change the length | |

− | + | magnitude += size; | |

− | + | ||

− | + | //normalize the vector | |

− | + | Vector3 vectorNormalized = Vector3.Normalize(vector); | |

− | + | ||

− | + | //scale the vector | |

− | + | return Vector3.Scale(vectorNormalized, new Vector3(magnitude, magnitude, magnitude)); | |

} | } | ||

//create a vector of direction "vector" with length "size" | //create a vector of direction "vector" with length "size" | ||

Vector3 SetVectorLength(Vector3 vector, float size){ | Vector3 SetVectorLength(Vector3 vector, float size){ | ||

− | + | ||

− | + | //normalize the vector | |

− | + | Vector3 vectorNormalized = Vector3.Normalize(vector); | |

− | + | ||

− | + | //scale the vector | |

− | + | return vectorNormalized *= size; | |

− | + | ||

} | } | ||

− | // | + | //caclulate the rotational difference from A to B |

− | Quaternion SubtractRotation(Quaternion B, Quaternion A){ | + | Quaternion SubtractRotation(Quaternion B, Quaternion A){ |

− | + | ||

− | + | Quaternion C = Quaternion.Inverse(A) * B; | |

+ | return C; | ||

} | } | ||

− | + | //Find the line of intersection between two planes. The planes are defined by a normal and a point on that plane. | |

− | //Find the line of intersection between two planes. | + | //The outputs are a point on the line and a vector which indicates it's direction. If the planes are not parallel, |

− | + | //the function outputs true, otherwise false. | |

− | + | bool PlanePlaneIntersection(out Vector3 linePoint, out Vector3 lineVec, Vector3 plane1Normal, Vector3 plane1Position, Vector3 plane2Normal, Vector3 plane2Position){ | |

− | //The outputs are a point on the line and a vector which indicates it's direction. | + | |

− | + | linePoint = Vector3.zero; | |

− | + | lineVec = Vector3.zero; | |

− | + | ||

− | + | //We can get the direction of the line of intersection of the two planes by calculating the | |

− | + | //cross product of the normals of the two planes. Note that this is just a direction and the line | |

− | + | //is not fixed in space yet. We need a point for that to go with the line vector. | |

− | + | lineVec = Vector3.Cross(plane1Normal, plane2Normal); | |

− | + | ||

− | + | //Next is to calculate a point on the line to fix it's position in space. This is done by finding a vector from | |

− | + | //the plane2 location, moving parallel to it's plane, and intersecting plane1. To prevent rounding | |

− | + | //errors, this vector also has to be perpendicular to lineDirection. To get this vector, calculate | |

− | + | //the cross product of the normal of plane2 and the lineDirection. | |

− | + | Vector3 ldir = Vector3.Cross(plane2Normal, lineVec); | |

− | + | ||

− | + | float denominator = Vector3.Dot(plane1Normal, ldir); | |

− | + | ||

− | + | //Prevent divide by zero and rounding errors by requiring about 5 degrees angle between the planes. | |

− | + | if(Mathf.Abs(denominator) > 0.006f){ | |

− | + | ||

− | + | Vector3 plane1ToPlane2 = plane1Position - plane2Position; | |

− | + | float t = Vector3.Dot(plane1Normal, plane1ToPlane2) / denominator; | |

− | + | linePoint = plane2Position + t * ldir; | |

− | + | ||

− | + | return true; | |

− | + | } | |

+ | |||

+ | //output not valid | ||

+ | else{ | ||

+ | return false; | ||

+ | } | ||

} | } | ||

− | //Get | + | //Get the intersection between a line and a plane. |

− | + | //If the line and plane are not parallel, the function outputs true, otherwise false. | |

− | + | bool LinePlaneIntersection(out Vector3 intersection, Vector3 linePoint, Vector3 lineVec, Vector3 planeNormal, Vector3 planePoint){ | |

− | + | ||

− | + | float length; | |

− | + | float dotNumerator; | |

− | + | float dotDenominator; | |

− | + | Vector3 vector; | |

− | + | intersection = Vector3.zero; | |

− | + | ||

− | + | //calculate the distance between the linePoint and the line-plane intersection point | |

− | + | dotNumerator = Vector3.Dot((planePoint - linePoint), planeNormal); | |

− | + | dotDenominator = Vector3.Dot(lineVec, planeNormal); | |

− | + | ||

− | + | //line and plane are not parallel | |

− | + | if(dotDenominator != 0.0f){ | |

− | + | length = dotNumerator / dotDenominator; | |

− | + | ||

− | + | //create a vector from the linePoint to the intersection point | |

− | + | vector = SetVectorLength(lineVec, length); | |

− | + | ||

− | + | //get the coordinates of the line-plane intersection point | |

− | + | intersection = linePoint + vector; | |

− | + | ||

− | + | return true; | |

− | + | } | |

− | + | ||

+ | //output not valid | ||

+ | else{ | ||

+ | return false; | ||

+ | } | ||

} | } | ||

− | // | + | //Calculate the instersection point of two lines. Returns true if lines intersect, otherwise false. |

− | // | + | //Note that in 3d, two lines do not intersect most of the time. So if the two lines are not in the |

− | + | //same plane, use ClosestPointsOnTwoLines() instead. | |

+ | bool LineLineIntersection(out Vector3 intersection, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2){ | ||

− | + | intersection = Vector3.zero; | |

− | + | ||

− | + | Vector3 lineVec3 = linePoint2 - linePoint1; | |

− | + | Vector3 crossVec1and2 = Vector3.Cross(lineVec1, lineVec2); | |

− | + | Vector3 crossVec3and2 = Vector3.Cross(lineVec3, lineVec2); | |

− | + | ||

− | + | float planarFactor = Vector3.Dot(lineVec3, crossVec1and2); | |

− | + | ||

− | + | //Lines are not coplanar. Take into account rounding errors. | |

− | + | if((planarFactor >= 0.00001f) || (planarFactor <= -0.00001f)){ | |

− | + | ||

− | + | return false; | |

− | + | } | |

− | + | ||

− | + | //Note: sqrMagnitude does x*x+y*y+z*z on the input vector. | |

− | + | float s = Vector3.Dot(crossVec3and2, crossVec1and2) / crossVec1and2.sqrMagnitude; | |

− | + | ||

− | + | if((s >= 0.0f) && (s <= 1.0f)){ | |

− | + | ||

− | + | intersection = linePoint1 + (lineVec1 * s); | |

− | + | return true; | |

− | + | } | |

− | + | ||

− | + | else{ | |

− | + | return false; | |

− | + | } | |

− | + | } | |

+ | |||

+ | //Two non-parallel lines which may or may not touch each other have a point on each line which are closest | ||

+ | //to each other. This function finds those two points. If the lines are not parallel, the function | ||

+ | //outputs true, otherwise false. | ||

+ | bool ClosestPointsOnTwoLines(out Vector3 closestPointLine1, out Vector3 closestPointLine2, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2){ | ||

+ | |||

+ | closestPointLine1 = Vector3.zero; | ||

+ | closestPointLine2 = Vector3.zero; | ||

+ | |||

+ | float a = Vector3.Dot(lineVec1, lineVec1); | ||

+ | float b = Vector3.Dot(lineVec1, lineVec2); | ||

+ | float e = Vector3.Dot(lineVec2, lineVec2); | ||

+ | |||

+ | float d = a*e - b*b; | ||

+ | |||

+ | //lines are not parallel | ||

+ | if(d != 0.0f){ | ||

+ | |||

+ | Vector3 r = linePoint1 - linePoint2; | ||

+ | float c = Vector3.Dot(lineVec1, r); | ||

+ | float f = Vector3.Dot(lineVec2, r); | ||

+ | |||

+ | float s = (b*f - c*e) / d; | ||

+ | float t = (a*f - c*b) / d; | ||

+ | |||

+ | closestPointLine1 = linePoint1 + lineVec1 * s; | ||

+ | closestPointLine2 = linePoint2 + lineVec2 * t; | ||

+ | |||

+ | return true; | ||

+ | } | ||

+ | |||

+ | else{ | ||

+ | return false; | ||

+ | } | ||

} | } | ||

− | //This function returns a | + | //This function returns a point which is a projection from a point to a line. |

− | + | ||

Vector3 ProjectPointOnLine(Vector3 linePoint, Vector3 lineVec, Vector3 point){ | Vector3 ProjectPointOnLine(Vector3 linePoint, Vector3 lineVec, Vector3 point){ | ||

− | + | ||

− | + | //get vector from point on line to point in space | |

− | + | Vector3 linePointToPoint = point - linePoint; | |

− | + | float t = Vector3.Dot(linePointToPoint, lineVec); | |

− | + | ||

− | + | return linePoint + lineVec * t; | |

} | } | ||

+ | //This function returns a point which is a projection from a point to a plane. | ||

Vector3 ProjectPointOnPlane(Vector3 planeNormal, Vector3 planePoint, Vector3 point){ | Vector3 ProjectPointOnPlane(Vector3 planeNormal, Vector3 planePoint, Vector3 point){ | ||

− | + | ||

− | + | float distance; | |

− | + | Vector3 translationVector; | |

− | + | ||

− | + | //First calculate the distance from the point to the plane: | |

− | + | distance = SignedDistancePlanePoint(planeNormal, planePoint, point); | |

− | + | ||

− | + | //Reverse the sign of the distance | |

− | + | distance *= -1; | |

− | + | ||

− | + | //Get a translation vector | |

− | + | translationVector = SetVectorLength(planeNormal, distance); | |

− | + | ||

− | + | //Translate the point to form a projection | |

− | + | return point + translationVector; | |

− | } | + | } |

− | //output is not normalized | + | //Projects a vector onto a plane. The output is not normalized. |

Vector3 ProjectVectorOnPlane(Vector3 planeNormal, Vector3 vector){ | Vector3 ProjectVectorOnPlane(Vector3 planeNormal, Vector3 vector){ | ||

− | + | ||

− | + | return vector - (Vector3.Dot(vector, planeNormal) * planeNormal); | |

− | } | + | } |

− | //Get the shortest distance between a point and a plane | + | //Get the shortest distance between a point and a plane. The output is signed so it holds information |

+ | //as to which side of the plane normal the point is. | ||

float SignedDistancePlanePoint(Vector3 planeNormal, Vector3 planePoint, Vector3 point){ | float SignedDistancePlanePoint(Vector3 planeNormal, Vector3 planePoint, Vector3 point){ | ||

− | + | ||

− | + | return Vector3.Dot(planeNormal, (point - planePoint)); | |

} | } | ||

Line 176: | Line 222: | ||

//then 90 degrees. | //then 90 degrees. | ||

float SignedDotProduct(Vector3 vectorA, Vector3 vectorB, Vector3 normal){ | float SignedDotProduct(Vector3 vectorA, Vector3 vectorB, Vector3 normal){ | ||

− | + | ||

− | + | Vector3 perpVector; | |

− | + | float dot; | |

− | + | ||

− | + | //Use the geometry object normal and one of the input vectors to calculate the perpendicular vector | |

− | + | perpVector = Vector3.Cross(normal, vectorA); | |

− | + | ||

− | + | //Now calculate the dot product between the perpendicular vector (perpVector) and the other input vector | |

− | + | dot = Vector3.Dot(perpVector, vectorB); | |

− | + | ||

+ | return dot; | ||

} | } | ||

Line 191: | Line 238: | ||

//Output is in radians. | //Output is in radians. | ||

float AngleVectorPlane(Vector3 vector, Vector3 normal){ | float AngleVectorPlane(Vector3 vector, Vector3 normal){ | ||

− | + | ||

− | + | float dot; | |

− | + | float angle; | |

− | + | ||

− | + | //calculate the the dot product between the two input vectors. This gives the cosine between the two vectors | |

− | + | dot = Vector3.Dot(vector, normal); | |

− | + | ||

− | + | //this is in radians | |

− | + | angle = (float)Math.Acos(dot); | |

− | + | ||

− | + | return 1.570796326794897f - angle; //90 | |

} | } | ||

//Calculate the dot product as an angle | //Calculate the dot product as an angle | ||

float DotProductAngle(Vector3 vec1, Vector3 vec2){ | float DotProductAngle(Vector3 vec1, Vector3 vec2){ | ||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | |||

− | + | double dot; | |

− | + | double angle; | |

− | + | ||

− | + | ||

− | + | ||

− | // | + | //get the dot product |

− | + | dot = Vector3.Dot(vec1, vec2); | |

− | + | ||

− | + | ||

− | + | ||

− | + | ||

− | + | ||

− | + | ||

− | if( | + | //Clamp to prevent NaN error. Shouldn't need this in the first place, but there could be a rounding error issue. |

− | + | if(dot < -1.0f){ | |

− | + | dot = -1.0f; | |

− | + | } | |

+ | if(dot > 1.0f){ | ||

+ | dot =1.0f; | ||

} | } | ||

− | + | //Calculate the angle. The output is in radians | |

− | + | //This step can be skipped for optimization... | |

− | + | angle = Math.Acos(dot); | |

+ | |||

+ | return (float)angle; | ||

} | } | ||

## Revision as of 02:30, 25 June 2012

//increase or decrease the length of vector by size Vector3 AddVectorLength(Vector3 vector, float size){ //get the vector length float magnitude = Vector3.Magnitude(vector); //change the length magnitude += size; //normalize the vector Vector3 vectorNormalized = Vector3.Normalize(vector); //scale the vector return Vector3.Scale(vectorNormalized, new Vector3(magnitude, magnitude, magnitude)); } //create a vector of direction "vector" with length "size" Vector3 SetVectorLength(Vector3 vector, float size){ //normalize the vector Vector3 vectorNormalized = Vector3.Normalize(vector); //scale the vector return vectorNormalized *= size; } //caclulate the rotational difference from A to B Quaternion SubtractRotation(Quaternion B, Quaternion A){ Quaternion C = Quaternion.Inverse(A) * B; return C; } //Find the line of intersection between two planes. The planes are defined by a normal and a point on that plane. //The outputs are a point on the line and a vector which indicates it's direction. If the planes are not parallel, //the function outputs true, otherwise false. bool PlanePlaneIntersection(out Vector3 linePoint, out Vector3 lineVec, Vector3 plane1Normal, Vector3 plane1Position, Vector3 plane2Normal, Vector3 plane2Position){ linePoint = Vector3.zero; lineVec = Vector3.zero; //We can get the direction of the line of intersection of the two planes by calculating the //cross product of the normals of the two planes. Note that this is just a direction and the line //is not fixed in space yet. We need a point for that to go with the line vector. lineVec = Vector3.Cross(plane1Normal, plane2Normal); //Next is to calculate a point on the line to fix it's position in space. This is done by finding a vector from //the plane2 location, moving parallel to it's plane, and intersecting plane1. To prevent rounding //errors, this vector also has to be perpendicular to lineDirection. To get this vector, calculate //the cross product of the normal of plane2 and the lineDirection. Vector3 ldir = Vector3.Cross(plane2Normal, lineVec); float denominator = Vector3.Dot(plane1Normal, ldir); //Prevent divide by zero and rounding errors by requiring about 5 degrees angle between the planes. if(Mathf.Abs(denominator) > 0.006f){ Vector3 plane1ToPlane2 = plane1Position - plane2Position; float t = Vector3.Dot(plane1Normal, plane1ToPlane2) / denominator; linePoint = plane2Position + t * ldir; return true; } //output not valid else{ return false; } } //Get the intersection between a line and a plane. //If the line and plane are not parallel, the function outputs true, otherwise false. bool LinePlaneIntersection(out Vector3 intersection, Vector3 linePoint, Vector3 lineVec, Vector3 planeNormal, Vector3 planePoint){ float length; float dotNumerator; float dotDenominator; Vector3 vector; intersection = Vector3.zero; //calculate the distance between the linePoint and the line-plane intersection point dotNumerator = Vector3.Dot((planePoint - linePoint), planeNormal); dotDenominator = Vector3.Dot(lineVec, planeNormal); //line and plane are not parallel if(dotDenominator != 0.0f){ length = dotNumerator / dotDenominator; //create a vector from the linePoint to the intersection point vector = SetVectorLength(lineVec, length); //get the coordinates of the line-plane intersection point intersection = linePoint + vector; return true; } //output not valid else{ return false; } } //Calculate the instersection point of two lines. Returns true if lines intersect, otherwise false. //Note that in 3d, two lines do not intersect most of the time. So if the two lines are not in the //same plane, use ClosestPointsOnTwoLines() instead. bool LineLineIntersection(out Vector3 intersection, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2){ intersection = Vector3.zero; Vector3 lineVec3 = linePoint2 - linePoint1; Vector3 crossVec1and2 = Vector3.Cross(lineVec1, lineVec2); Vector3 crossVec3and2 = Vector3.Cross(lineVec3, lineVec2); float planarFactor = Vector3.Dot(lineVec3, crossVec1and2); //Lines are not coplanar. Take into account rounding errors. if((planarFactor >= 0.00001f) || (planarFactor <= -0.00001f)){ return false; } //Note: sqrMagnitude does x*x+y*y+z*z on the input vector. float s = Vector3.Dot(crossVec3and2, crossVec1and2) / crossVec1and2.sqrMagnitude; if((s >= 0.0f) && (s <= 1.0f)){ intersection = linePoint1 + (lineVec1 * s); return true; } else{ return false; } } //Two non-parallel lines which may or may not touch each other have a point on each line which are closest //to each other. This function finds those two points. If the lines are not parallel, the function //outputs true, otherwise false. bool ClosestPointsOnTwoLines(out Vector3 closestPointLine1, out Vector3 closestPointLine2, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2){ closestPointLine1 = Vector3.zero; closestPointLine2 = Vector3.zero; float a = Vector3.Dot(lineVec1, lineVec1); float b = Vector3.Dot(lineVec1, lineVec2); float e = Vector3.Dot(lineVec2, lineVec2); float d = a*e - b*b; //lines are not parallel if(d != 0.0f){ Vector3 r = linePoint1 - linePoint2; float c = Vector3.Dot(lineVec1, r); float f = Vector3.Dot(lineVec2, r); float s = (b*f - c*e) / d; float t = (a*f - c*b) / d; closestPointLine1 = linePoint1 + lineVec1 * s; closestPointLine2 = linePoint2 + lineVec2 * t; return true; } else{ return false; } } //This function returns a point which is a projection from a point to a line. Vector3 ProjectPointOnLine(Vector3 linePoint, Vector3 lineVec, Vector3 point){ //get vector from point on line to point in space Vector3 linePointToPoint = point - linePoint; float t = Vector3.Dot(linePointToPoint, lineVec); return linePoint + lineVec * t; } //This function returns a point which is a projection from a point to a plane. Vector3 ProjectPointOnPlane(Vector3 planeNormal, Vector3 planePoint, Vector3 point){ float distance; Vector3 translationVector; //First calculate the distance from the point to the plane: distance = SignedDistancePlanePoint(planeNormal, planePoint, point); //Reverse the sign of the distance distance *= -1; //Get a translation vector translationVector = SetVectorLength(planeNormal, distance); //Translate the point to form a projection return point + translationVector; } //Projects a vector onto a plane. The output is not normalized. Vector3 ProjectVectorOnPlane(Vector3 planeNormal, Vector3 vector){ return vector - (Vector3.Dot(vector, planeNormal) * planeNormal); } //Get the shortest distance between a point and a plane. The output is signed so it holds information //as to which side of the plane normal the point is. float SignedDistancePlanePoint(Vector3 planeNormal, Vector3 planePoint, Vector3 point){ return Vector3.Dot(planeNormal, (point - planePoint)); } //This function calculates a signed (+ or - sign instead of being ambiguous) dot product. It is basically used //to figure out whether a vector is positioned to the left or right of another vector. The way this is done is //by calculating a vector perpendicular to one of the vectors and using that as a reference. This is because //the result of a dot product only has signed information when an angle is transitioning between more or less //then 90 degrees. float SignedDotProduct(Vector3 vectorA, Vector3 vectorB, Vector3 normal){ Vector3 perpVector; float dot; //Use the geometry object normal and one of the input vectors to calculate the perpendicular vector perpVector = Vector3.Cross(normal, vectorA); //Now calculate the dot product between the perpendicular vector (perpVector) and the other input vector dot = Vector3.Dot(perpVector, vectorB); return dot; } //Calculate the angle between a vector and a plane. The plane is made by a normal vector. //Output is in radians. float AngleVectorPlane(Vector3 vector, Vector3 normal){ float dot; float angle; //calculate the the dot product between the two input vectors. This gives the cosine between the two vectors dot = Vector3.Dot(vector, normal); //this is in radians angle = (float)Math.Acos(dot); return 1.570796326794897f - angle; //90 } //Calculate the dot product as an angle float DotProductAngle(Vector3 vec1, Vector3 vec2){ double dot; double angle; //get the dot product dot = Vector3.Dot(vec1, vec2); //Clamp to prevent NaN error. Shouldn't need this in the first place, but there could be a rounding error issue. if(dot < -1.0f){ dot = -1.0f; } if(dot > 1.0f){ dot =1.0f; } //Calculate the angle. The output is in radians //This step can be skipped for optimization... angle = Math.Acos(dot); return (float)angle; } //Convert a plane defined by 3 points to a plane defined by a vector and a point. //The plane point is the middle of the triangle defined by the 3 points. void PlaneFrom3Points(out Vector3 planeNormal, out Vector3 planePoint, Vector3 pointA, Vector3 pointB, Vector3 pointC){ planeNormal = Vector3.zero; planePoint = Vector3.zero; //Make two vectors from the 3 input points, originating from point A Vector3 AB = pointB - pointA; Vector3 AC = pointC - pointA; //Calculate the normal planeNormal = Vector3.Normalize(Vector3.Cross(AB, AC)); //Get the points in the middle AB and AC Vector3 middleAB = pointA + (AB / 2.0f); Vector3 middleAC = pointA + (AC / 2.0f); //Get vectors from the middle of AB and AC to the point which is not on that line. Vector3 middleABtoC = pointC - middleAB; Vector3 middleACtoB = pointB - middleAC; //Calculate the intersection between the two lines. This will be the center //of the triangle defined by the 3 points. LineLineIntersection(out planePoint, middleAB, middleABtoC, middleAC, middleACtoB); }