3d Math functions

From Unify Community Wiki
Revision as of 16:12, 11 December 2012 by TomoI (Talk | contribs)

Jump to: navigation, search
This article is a stub.

You can help UnifyWiki by expanding it.



This is you the person writing the page, please include any credits to people here also.


A general description of what the script/code can do.


How to use the script/code (please give clear precise instructions).


//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
        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
        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;
        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;
        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);


Place the relevant category tags at the bottom of the page.

Personal tools