3d Math functions

From Unify Community Wiki
(Difference between revisions)
Jump to: navigation, search
(Usage)
(Code)
Line 13: Line 13:
  
 
-If you want to use the TransformWithParent() function, you have to call Math3d.Init() first.
 
-If you want to use the TransformWithParent() function, you have to call Math3d.Init() first.
 
==Code==
 
<syntaxhighlight lang="csharp">
 
  
 
using UnityEngine;
 
using UnityEngine;
Line 25: Line 22:
 
private static GameObject tempChild;
 
private static GameObject tempChild;
 
private static GameObject tempParent;
 
private static GameObject tempParent;
+
 
 
public static void Init(){
 
public static void Init(){
  
 
tempChild = new GameObject("TempChild");
 
tempChild = new GameObject("TempChild");
 
tempParent = new GameObject("TempParent");
 
tempParent = new GameObject("TempParent");
+
 
 
//set the parent
 
//set the parent
 
tempChild.transform.parent = tempParent.transform;
 
tempChild.transform.parent = tempParent.transform;
 
}
 
}
  
 
    //increase or decrease the length of vector by size
 
    public static 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"
+
//increase or decrease the length of vector by size
    public static Vector3 SetVectorLength(Vector3 vector, float size){
+
public static Vector3 AddVectorLength(Vector3 vector, float size){
       
+
        //normalize the vector
+
//get the vector length
        Vector3 vectorNormalized = Vector3.Normalize(vector);
+
float magnitude = Vector3.Magnitude(vector);
       
+
 
        //scale the vector
+
//change the length
        return vectorNormalized *= size;
+
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"
 +
public static Vector3 SetVectorLength(Vector3 vector, float size){
 +
 +
//normalize the vector
 +
Vector3 vectorNormalized = Vector3.Normalize(vector);
 +
 
 +
//scale the vector
 +
return vectorNormalized *= size;
 +
}
  
  
Line 70: Line 67:
 
}
 
}
  
    //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 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 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.
+
//the function outputs true, otherwise false.
    public static bool PlanePlaneIntersection(out Vector3 linePoint, out Vector3 lineVec, Vector3 plane1Normal, Vector3 plane1Position, Vector3 plane2Normal, Vector3 plane2Position){
+
public static bool PlanePlaneIntersection(out Vector3 linePoint, out Vector3 lineVec, Vector3 plane1Normal, Vector3 plane1Position, Vector3 plane2Normal, Vector3 plane2Position){
       
+
        linePoint = Vector3.zero;
+
linePoint = Vector3.zero;
        lineVec = 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.
+
//We can get the direction of the line of intersection of the two planes by calculating the  
    //If the line and plane are not parallel, the function outputs true, otherwise false.
+
//cross product of the normals of the two planes. Note that this is just a direction and the line
    public static bool LinePlaneIntersection(out Vector3 intersection, Vector3 linePoint, Vector3 lineVec, Vector3 planeNormal, Vector3 planePoint){
+
//is not fixed in space yet. We need a point for that to go with the line vector.
       
+
lineVec = Vector3.Cross(plane1Normal, plane2Normal);
        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 intersection point of two lines. Returns true if lines intersect, otherwise false.
+
//Next is to calculate a point on the line to fix it's position in space. This is done by finding a vector from
    //Note that in 3d, two lines do not intersect most of the time. So if the two lines are not in the
+
//the plane2 location, moving parallel to it's plane, and intersecting plane1. To prevent rounding
    //same plane, use ClosestPointsOnTwoLines() instead.
+
//errors, this vector also has to be perpendicular to lineDirection. To get this vector, calculate
    public static bool LineLineIntersection(out Vector3 intersection, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2){
+
//the cross product of the normal of plane2 and the lineDirection.
 +
Vector3 ldir = Vector3.Cross(plane2Normal, lineVec);
  
        intersection = Vector3.zero;
+
float denominator = Vector3.Dot(plane1Normal, ldir);
       
+
        Vector3 lineVec3 = linePoint2 - linePoint1;
+
        Vector3 crossVec1and2 = Vector3.Cross(lineVec1, lineVec2);
+
        Vector3 crossVec3and2 = Vector3.Cross(lineVec3, lineVec2);
+
  
        float planarFactor = Vector3.Dot(lineVec3, crossVec1and2);
+
//Prevent divide by zero and rounding errors by requiring about 5 degrees angle between the planes.
       
+
if(Mathf.Abs(denominator) > 0.006f){
        //Lines are not coplanar. Take into account rounding errors.
+
        if((planarFactor >= 0.00001f) || (planarFactor <= -0.00001f)){
+
Vector3 plane1ToPlane2 = plane1Position - plane2Position;
 +
float t = Vector3.Dot(plane1Normal, plane1ToPlane2) / denominator;
 +
linePoint = plane2Position + t * ldir;
 +
 +
return true;
 +
}
  
            return false;
+
//output not valid
        }
+
else{
 +
return false;
 +
}
 +
}
  
        //Note: sqrMagnitude does x*x+y*y+z*z on the input vector.
+
//Get the intersection between a line and a plane.  
        float s = Vector3.Dot(crossVec3and2, crossVec1and2) / crossVec1and2.sqrMagnitude;
+
//If the line and plane are not parallel, the function outputs true, otherwise false.
       
+
public static bool LinePlaneIntersection(out Vector3 intersection, Vector3 linePoint, Vector3 lineVec, Vector3 planeNormal, Vector3 planePoint){
        if((s >= 0.0f) && (s <= 1.0f)){
+
 +
float length;
 +
float dotNumerator;
 +
float dotDenominator;
 +
Vector3 vector;
 +
intersection = Vector3.zero;
  
            intersection = linePoint1 + (lineVec1 * s);
+
//calculate the distance between the linePoint and the line-plane intersection point
            return true;
+
dotNumerator = Vector3.Dot((planePoint - linePoint), planeNormal);
        }
+
dotDenominator = Vector3.Dot(lineVec, planeNormal);
       
+
        else{
+
            return false;     
+
        }
+
    }
+
  
    //Two non-parallel lines which may or may not touch each other have a point on each line which are closest
+
//line and plane are not parallel
    //to each other. This function finds those two points. If the lines are not parallel, the function
+
if(dotDenominator != 0.0f){
    //outputs true, otherwise false.
+
length =  dotNumerator / dotDenominator;
    public static bool ClosestPointsOnTwoLines(out Vector3 closestPointLine1, out Vector3 closestPointLine2, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2){
+
 +
//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;
 +
}
  
        closestPointLine1 = Vector3.zero;
+
//output not valid
        closestPointLine2 = Vector3.zero;
+
else{
       
+
return false;
        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.
+
//Calculate the intersection point of two lines. Returns true if lines intersect, otherwise false.
    public static Vector3 ProjectPointOnLine(Vector3 linePoint, Vector3 lineVec, Vector3 point){
+
//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.
        //get vector from point on line to point in space
+
public static bool LineLineIntersection(out Vector3 intersection, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2){
        Vector3 linePointToPoint = point - linePoint;
+
  
        float t = Vector3.Dot(linePointToPoint, lineVec);
+
intersection = Vector3.zero;
       
+
        return linePoint + lineVec * t;
+
    }
+
  
    //This function returns a point which is a projection from a point to a plane.
+
Vector3 lineVec3 = linePoint2 - linePoint1;
    public static Vector3 ProjectPointOnPlane(Vector3 planeNormal, Vector3 planePoint, Vector3 point){
+
Vector3 crossVec1and2 = Vector3.Cross(lineVec1, lineVec2);
       
+
Vector3 crossVec3and2 = Vector3.Cross(lineVec3, lineVec2);
        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.
+
float planarFactor = Vector3.Dot(lineVec3, crossVec1and2);
    public static 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
+
//Lines are not coplanar. Take into account rounding errors.
    //as to which side of the plane normal the point is.
+
if((planarFactor >= 0.00001f) || (planarFactor <= -0.00001f)){
    public static 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
+
return false;
    //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.
+
    public static float SignedDotProduct(Vector3 vectorA, Vector3 vectorB, Vector3 normal){
+
  
        Vector3 perpVector;
+
//Note: sqrMagnitude does x*x+y*y+z*z on the input vector.
        float dot;
+
float s = Vector3.Dot(crossVec3and2, crossVec1and2) / crossVec1and2.sqrMagnitude;
       
+
        //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.
+
if((s >= 0.0f) && (s <= 1.0f)){
    //Output is in radians.
+
    public static 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 degrees - angle
+
    }
+
  
    //Calculate the dot product as an angle
+
intersection = linePoint1 + (lineVec1 * s);
    public static float DotProductAngle(Vector3 vec1, Vector3 vec2){
+
return true;
       
+
}
        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;
+
else{
    }
+
return false;      
 +
}
 +
}
  
    //Convert a plane defined by 3 points to a plane defined by a vector and a point.
+
//Two non-parallel lines which may or may not touch each other have a point on each line which are closest
    //The plane point is the middle of the triangle defined by the 3 points.
+
//to each other. This function finds those two points. If the lines are not parallel, the function
    public static void PlaneFrom3Points(out Vector3 planeNormal, out Vector3 planePoint, Vector3 pointA, Vector3 pointB, Vector3 pointC){
+
//outputs true, otherwise false.
 +
public static bool ClosestPointsOnTwoLines(out Vector3 closestPointLine1, out Vector3 closestPointLine2, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2){
  
        planeNormal = Vector3.zero;
+
closestPointLine1 = Vector3.zero;
        planePoint = Vector3.zero;
+
closestPointLine2 = 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
+
float a = Vector3.Dot(lineVec1, lineVec1);
        planeNormal = Vector3.Normalize(Vector3.Cross(AB, AC));
+
float b = Vector3.Dot(lineVec1, lineVec2);
       
+
float e = Vector3.Dot(lineVec2, lineVec2);
        //Get the points in the middle AB and AC
+
 
        Vector3 middleAB = pointA + (AB / 2.0f);
+
float d = a*e - b*b;
        Vector3 middleAC = pointA + (AC / 2.0f);
+
 
       
+
//lines are not parallel
        //Get vectors from the middle of AB and AC to the point which is not on that line.
+
if(d != 0.0f){
        Vector3 middleABtoC = pointC - middleAB;
+
        Vector3 middleACtoB = pointB - middleAC;
+
Vector3 r = linePoint1 - linePoint2;
       
+
float c = Vector3.Dot(lineVec1, r);
        //Calculate the intersection between the two lines. This will be the center  
+
float f = Vector3.Dot(lineVec2, r);
        //of the triangle defined by the 3 points.
+
        LineLineIntersection(out planePoint, middleAB, middleABtoC, middleAC, middleACtoB);
+
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.
 +
public static 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.
 +
public static 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.
 +
public static 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.
 +
public static 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.
 +
public static 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.
 +
public static 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 degrees - angle
 +
}
 +
 
 +
//Calculate the dot product as an angle
 +
public static 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.
 +
public static 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);
 +
}
  
 
//Returns the forward vector of a quaternion
 
//Returns the forward vector of a quaternion
 
public static Vector3 GetForwardVector(Quaternion q){
 
public static Vector3 GetForwardVector(Quaternion q){
+
 
 
return new Vector3(2.0f * (q.x * q.z + q.w * q.y), 2.0f * (q.y * q.x - q.w * q.x), 1.0f - 2.0f * (q.x * q.x + q.y * q.y));
 
return new Vector3(2.0f * (q.x * q.z + q.w * q.y), 2.0f * (q.y * q.x - q.w * q.x), 1.0f - 2.0f * (q.x * q.x + q.y * q.y));
 
}
 
}
Line 345: Line 342:
 
//Returns the up vector of a quaternion
 
//Returns the up vector of a quaternion
 
public static Vector3 GetUpVector(Quaternion q){
 
public static Vector3 GetUpVector(Quaternion q){
+
 
return new Vector3(2.0f * (q.x * q.y - q.w * q.z), 1.0f - 2.0f * (q.x * q.x + q.z * q.z), 2.0f * (q.y * q.z + q.w * q.x));
+
return new Vector3(2.0f * (q.x * q.y - q.w * q.z), 1.0f - 2.0f * (q.x * q.x + q.z * q.z), 2.0f * (q.y * q.z + q.w * q.x));
 
}
 
}
  
 
//Returns the right vector of a quaternion
 
//Returns the right vector of a quaternion
 
public static Vector3 GetRightVector(Quaternion q){
 
public static Vector3 GetRightVector(Quaternion q){
+
 
 
return new Vector3(1.0f - 2.0f * (q.y * q.y + q.z * q.z), 2.0f * (q.x * q.y + q.w * q.z), 2.0f * (q.x * q.z - q.w * q.y));
 
return new Vector3(1.0f - 2.0f * (q.y * q.y + q.z * q.z), 2.0f * (q.x * q.y + q.w * q.z), 2.0f * (q.x * q.z - q.w * q.y));
 
}
 
}
+
 
 
//Gets a quaternion from a matrix
 
//Gets a quaternion from a matrix
 
public static Quaternion QuaternionFromMatrix(Matrix4x4 m){  
 
public static Quaternion QuaternionFromMatrix(Matrix4x4 m){  
+
 
 
return Quaternion.LookRotation(m.GetColumn(2), m.GetColumn(1));  
 
return Quaternion.LookRotation(m.GetColumn(2), m.GetColumn(1));  
 
}
 
}
+
 
 
//Gets a position from a matrix
 
//Gets a position from a matrix
 
public static Vector3 PositionFromMatrix(Matrix4x4 m){
 
public static Vector3 PositionFromMatrix(Matrix4x4 m){
+
 
 
Vector4 vector4Position = m.GetColumn(3);
 
Vector4 vector4Position = m.GetColumn(3);
 
return new Vector3(vector4Position.x, vector4Position.y, vector4Position.z);
 
return new Vector3(vector4Position.x, vector4Position.y, vector4Position.z);
 
}
 
}
+
 
    //This is an alternative for Quaternion.LookRotation. Instead of aligning the forward and up vector of the game  
+
//This is an alternative for Quaternion.LookRotation. Instead of aligning the forward and up vector of the game  
    //object with the input vectors, a custom direction can be used instead of the fixed forward and up vectors.
+
//object with the input vectors, a custom direction can be used instead of the fixed forward and up vectors.
    //alignWithVector and alignWithNormal are in world space.
+
//alignWithVector and alignWithNormal are in world space.
    //customForward and customUp are in object space.
+
//customForward and customUp are in object space.
    //Usage: use alignWithVector and alignWithNormal as if you are using the default LookRotation function.
+
//Usage: use alignWithVector and alignWithNormal as if you are using the default LookRotation function.
    //Set customForward and customUp to the vectors you wish to use instead of the default forward and up vectors.
+
//Set customForward and customUp to the vectors you wish to use instead of the default forward and up vectors.
    public static void LookRotationExtended(ref GameObject gameObjectInOut, Vector3 alignWithVector, Vector3 alignWithNormal, Vector3 customForward, Vector3 customUp){
+
public static void LookRotationExtended(ref GameObject gameObjectInOut, Vector3 alignWithVector, Vector3 alignWithNormal, Vector3 customForward, Vector3 customUp){
+
    //Set the rotation of the destination
+
    Quaternion rotationA = Quaternion.LookRotation(alignWithVector, alignWithNormal);
+
 
 
    //Set the rotation of the custom normal and up vectors.  
+
//Set the rotation of the destination
    //When using the default LookRotation function, this would be hard coded to the forward and up vector.
+
Quaternion rotationA = Quaternion.LookRotation(alignWithVector, alignWithNormal);
    Quaternion rotationB = Quaternion.LookRotation(customForward, customUp);
+
 
+
//Set the rotation of the custom normal and up vectors.  
    //Calculate the rotation
+
//When using the default LookRotation function, this would be hard coded to the forward and up vector.
    gameObjectInOut.transform.rotation =  rotationA * Quaternion.Inverse(rotationB);
+
Quaternion rotationB = Quaternion.LookRotation(customForward, customUp);
    }
+
 
+
//Calculate the rotation
 +
gameObjectInOut.transform.rotation =  rotationA * Quaternion.Inverse(rotationB);
 +
}
 +
 
 
//This function translates one object as if it was parented to the other.
 
//This function translates one object as if it was parented to the other.
 
//Before using this function, the Init() function must be called
 
//Before using this function, the Init() function must be called
Line 403: Line 400:
 
tempParent.transform.position = startParentPosition;
 
tempParent.transform.position = startParentPosition;
 
tempParent.transform.localScale = Vector3.one; //to prevent scale wandering
 
tempParent.transform.localScale = Vector3.one; //to prevent scale wandering
+
 
 
//set the child start transform
 
//set the child start transform
 
tempChild.transform.rotation = startChildRotation;
 
tempChild.transform.rotation = startChildRotation;
Line 412: Line 409:
 
tempParent.transform.rotation = parentRotation;
 
tempParent.transform.rotation = parentRotation;
 
tempParent.transform.position = parentPosition;
 
tempParent.transform.position = parentPosition;
+
 
 
//get the child transform
 
//get the child transform
 
childRotation = tempChild.transform.rotation;
 
childRotation = tempChild.transform.rotation;
 
childPosition = tempChild.transform.position;
 
childPosition = tempChild.transform.position;
 
}
 
}
+
 
 
//With this function you can align a triangle of an object with any transform.
 
//With this function you can align a triangle of an object with any transform.
 
//Usage: gameObjectInOut is the game object you want to transform.
 
//Usage: gameObjectInOut is the game object you want to transform.
 
//alignWithVector, alignWithNormal, and alignWithPosition is the transform with which the triangle of the object should be aligned with.
 
//alignWithVector, alignWithNormal, and alignWithPosition is the transform with which the triangle of the object should be aligned with.
    //triangleForward, triangleNormal, and trianglePosition is the transform of the triangle from the object.
+
//triangleForward, triangleNormal, and trianglePosition is the transform of the triangle from the object.
 
//alignWithVector, alignWithNormal, and alignWithPosition are in world space.
 
//alignWithVector, alignWithNormal, and alignWithPosition are in world space.
    //triangleForward, triangleNormal, and trianglePosition are in object space.
+
//triangleForward, triangleNormal, and trianglePosition are in object space.
 
//trianglePosition is the mesh position of the triangle. The effect of the scale of the object is handled automatically.
 
//trianglePosition is the mesh position of the triangle. The effect of the scale of the object is handled automatically.
 
//trianglePosition can be set at any position, it does not have to be at a vertex or in the middle of the triangle.
 
//trianglePosition can be set at any position, it does not have to be at a vertex or in the middle of the triangle.
 
public static void PreciseAlign(ref GameObject gameObjectInOut, Vector3 alignWithVector, Vector3 alignWithNormal, Vector3 alignWithPosition, Vector3 triangleForward, Vector3 triangleNormal, Vector3 trianglePosition){
 
public static void PreciseAlign(ref GameObject gameObjectInOut, Vector3 alignWithVector, Vector3 alignWithNormal, Vector3 alignWithPosition, Vector3 triangleForward, Vector3 triangleNormal, Vector3 trianglePosition){
+
 
 
//Set the rotation.
 
//Set the rotation.
 
LookRotationExtended(ref gameObjectInOut, alignWithVector, alignWithNormal, triangleForward, triangleNormal);
 
LookRotationExtended(ref gameObjectInOut, alignWithVector, alignWithNormal, triangleForward, triangleNormal);
+
 
 
//Get the world space position of trianglePosition
 
//Get the world space position of trianglePosition
 
Vector3 trianglePositionWorld = gameObjectInOut.transform.TransformPoint(trianglePosition);
 
Vector3 trianglePositionWorld = gameObjectInOut.transform.TransformPoint(trianglePosition);
+
 
 
//Get a vector from trianglePosition to alignWithPosition
 
//Get a vector from trianglePosition to alignWithPosition
 
Vector3 translateVector = alignWithPosition - trianglePositionWorld;
 
Vector3 translateVector = alignWithPosition - trianglePositionWorld;
+
 
 
//Now transform the object so the triangle lines up correctly.
 
//Now transform the object so the triangle lines up correctly.
 
gameObjectInOut.transform.Translate(translateVector, Space.World);
 
gameObjectInOut.transform.Translate(translateVector, Space.World);
 
}
 
}
 
}
 
}
 
</syntaxhighlight>
 
  
 
==Categories==
 
==Categories==
 
3d Math
 
3d Math

Revision as of 10:39, 12 January 2013

This article is a stub.

You can help UnifyWiki by expanding it.


Contents

Author

Tjeerd Schouten

Description

This is a collection of generic 3d math functions such as line plane intersection, closest points on two lines, etc.

Usage

-Place the Math3d.cs script in the scripts folder.

-To call a function from another script, place "Math3d." of the function, for example: Math3d.LookRotationExtended(...)

-If you want to use the TransformWithParent() function, you have to call Math3d.Init() first.

using UnityEngine; using System.Collections; using System;

public class Math3d : MonoBehaviour {

private static GameObject tempChild; private static GameObject tempParent;

public static void Init(){

tempChild = new GameObject("TempChild"); tempParent = new GameObject("TempParent");

//set the parent tempChild.transform.parent = tempParent.transform; }


//increase or decrease the length of vector by size public static 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" public static 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 public static 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. public static 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. public static 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 intersection 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. public static 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. public static 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. public static 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. public static 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. public static 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. public static 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. public static 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. public static 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 degrees - angle }

//Calculate the dot product as an angle public static 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. public static 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); }

//Returns the forward vector of a quaternion public static Vector3 GetForwardVector(Quaternion q){

return new Vector3(2.0f * (q.x * q.z + q.w * q.y), 2.0f * (q.y * q.x - q.w * q.x), 1.0f - 2.0f * (q.x * q.x + q.y * q.y)); }

//Returns the up vector of a quaternion public static Vector3 GetUpVector(Quaternion q){

return new Vector3(2.0f * (q.x * q.y - q.w * q.z), 1.0f - 2.0f * (q.x * q.x + q.z * q.z), 2.0f * (q.y * q.z + q.w * q.x)); }

//Returns the right vector of a quaternion public static Vector3 GetRightVector(Quaternion q){

return new Vector3(1.0f - 2.0f * (q.y * q.y + q.z * q.z), 2.0f * (q.x * q.y + q.w * q.z), 2.0f * (q.x * q.z - q.w * q.y)); }

//Gets a quaternion from a matrix public static Quaternion QuaternionFromMatrix(Matrix4x4 m){

return Quaternion.LookRotation(m.GetColumn(2), m.GetColumn(1)); }

//Gets a position from a matrix public static Vector3 PositionFromMatrix(Matrix4x4 m){

Vector4 vector4Position = m.GetColumn(3); return new Vector3(vector4Position.x, vector4Position.y, vector4Position.z); }

//This is an alternative for Quaternion.LookRotation. Instead of aligning the forward and up vector of the game //object with the input vectors, a custom direction can be used instead of the fixed forward and up vectors. //alignWithVector and alignWithNormal are in world space. //customForward and customUp are in object space. //Usage: use alignWithVector and alignWithNormal as if you are using the default LookRotation function. //Set customForward and customUp to the vectors you wish to use instead of the default forward and up vectors. public static void LookRotationExtended(ref GameObject gameObjectInOut, Vector3 alignWithVector, Vector3 alignWithNormal, Vector3 customForward, Vector3 customUp){

//Set the rotation of the destination Quaternion rotationA = Quaternion.LookRotation(alignWithVector, alignWithNormal);

//Set the rotation of the custom normal and up vectors. //When using the default LookRotation function, this would be hard coded to the forward and up vector. Quaternion rotationB = Quaternion.LookRotation(customForward, customUp);

//Calculate the rotation gameObjectInOut.transform.rotation = rotationA * Quaternion.Inverse(rotationB); }

//This function translates one object as if it was parented to the other. //Before using this function, the Init() function must be called //Input: parentRotation and parentPosition: the current parent transform. //Input: startParentRotation and startParentPosition: the transform of the parent object at the time the objects are parented. //Input: startChildRotation and startChildPosition: the transform of the child object at the time the objects are parented. //Output: childRotation and childPosition. //All transforms are in world space. public static void TransformWithParent(out Quaternion childRotation, out Vector3 childPosition, Quaternion parentRotation, Vector3 parentPosition, Quaternion startParentRotation, Vector3 startParentPosition, Quaternion startChildRotation, Vector3 startChildPosition){

childRotation = Quaternion.identity; childPosition = Vector3.zero;

//set the parent start transform tempParent.transform.rotation = startParentRotation; tempParent.transform.position = startParentPosition; tempParent.transform.localScale = Vector3.one; //to prevent scale wandering

//set the child start transform tempChild.transform.rotation = startChildRotation; tempChild.transform.position = startChildPosition; tempChild.transform.localScale = Vector3.one; //to prevent scale wandering

//translate and rotate the child by moving the parent tempParent.transform.rotation = parentRotation; tempParent.transform.position = parentPosition;

//get the child transform childRotation = tempChild.transform.rotation; childPosition = tempChild.transform.position; }

//With this function you can align a triangle of an object with any transform. //Usage: gameObjectInOut is the game object you want to transform. //alignWithVector, alignWithNormal, and alignWithPosition is the transform with which the triangle of the object should be aligned with. //triangleForward, triangleNormal, and trianglePosition is the transform of the triangle from the object. //alignWithVector, alignWithNormal, and alignWithPosition are in world space. //triangleForward, triangleNormal, and trianglePosition are in object space. //trianglePosition is the mesh position of the triangle. The effect of the scale of the object is handled automatically. //trianglePosition can be set at any position, it does not have to be at a vertex or in the middle of the triangle. public static void PreciseAlign(ref GameObject gameObjectInOut, Vector3 alignWithVector, Vector3 alignWithNormal, Vector3 alignWithPosition, Vector3 triangleForward, Vector3 triangleNormal, Vector3 trianglePosition){

//Set the rotation. LookRotationExtended(ref gameObjectInOut, alignWithVector, alignWithNormal, triangleForward, triangleNormal);

//Get the world space position of trianglePosition Vector3 trianglePositionWorld = gameObjectInOut.transform.TransformPoint(trianglePosition);

//Get a vector from trianglePosition to alignWithPosition Vector3 translateVector = alignWithPosition - trianglePositionWorld;

//Now transform the object so the triangle lines up correctly. gameObjectInOut.transform.Translate(translateVector, Space.World); } }

Categories

3d Math

Personal tools
Namespaces

Variants
Actions
Navigation
Extras
Toolbox