3d Math functions

From Unify Community Wiki
(Difference between revisions)
Jump to: navigation, search
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
+
    //get the vector length
float magnitude = Vector3.Magnitude(vector);
+
    float magnitude = Vector3.Magnitude(vector);
+
   
//change the length
+
    //change the length
magnitude += size;
+
    magnitude += size;
+
   
//normalize the vector
+
    //normalize the vector
Vector3 vectorNormalized = Vector3.Normalize(vector);
+
    Vector3 vectorNormalized = Vector3.Normalize(vector);
+
   
//scale the vector
+
    //scale the vector
return Vector3.Scale(vectorNormalized, new Vector3(magnitude, magnitude, magnitude));
+
    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
+
    //normalize the vector
Vector3 vectorNormalized = Vector3.Normalize(vector);
+
    Vector3 vectorNormalized = Vector3.Normalize(vector);
+
   
//scale the vector
+
    //scale the vector
// return Vector3.Scale(vectorNormalized, new Vector3(size, size, size));
+
    return vectorNormalized *= size;
return vectorNormalized *= size;
+
 
}
 
}
  
  
//calculate the rotational difference from A to B
+
//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;
+
    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 inputs are two game objects which represent the planes. The plane is assumed to be made
+
//the function outputs true, otherwise false.
//up of the x and z axis of the game object.
+
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.
+
   
void PlanePlaneIntersection(out Vector3 linePoint, out Vector3 lineVec, Vector3 plane1Normal, Vector3 plane1Position, Vector3 plane2Normal, Vector3 plane2Position){
+
    linePoint = Vector3.zero;
+
    lineVec = Vector3.zero;
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
//We can get the direction of the line of intersection of the two planes by calculating the  
+
    //is not fixed in space yet. We need a point for that to go with the line vector.
//cross product of the normals of the two planes. Note that this is just a direction and the line
+
    lineVec = Vector3.Cross(plane1Normal, plane2Normal);
//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
//Next is to calculate a point on the line to fix it's position in space. This is done by finding a vector from
+
    //errors, this vector also has to be perpendicular to lineDirection. To get this vector, calculate
//the plane2 location, moving parallel to it's plane, and intersecting plane1. To prevent rounding
+
    //the cross product of the normal of plane2 and the lineDirection.
//errors, this vector also has to be perpendicular to lineDirection. To get this vector, calculate
+
    Vector3 ldir = Vector3.Cross(plane2Normal, lineVec);
//the cross product of the normal of plane2 and the lineDirection.
+
   
Vector3 ldir = Vector3.Cross(plane2Normal, lineVec);
+
    float denominator = Vector3.Dot(plane1Normal, ldir);
+
   
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){
//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;
Vector3 plane1ToPlane2 = plane1Position - plane2Position;
+
        linePoint = plane2Position + t * ldir;
float t = Vector3.Dot(plane1Normal, plane1ToPlane2) / denominator;
+
       
linePoint = plane2Position + t * ldir;
+
        return true;
}
+
    }
 +
   
 +
    //output not valid
 +
    else{
 +
        return false;
 +
    }
 
}
 
}
  
//Get the coordinates (world space) of the intersection between a line and a plane
+
//Get the intersection between a line and a plane.
Vector3 LinePlaneIntersection(Vector3 linePoint, Vector3 lineVec, Vector3 planeNormal, Vector3 planePoint){
+
//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 length;
float dotDenominator;
+
    float dotNumerator;
Vector3 vector;
+
    float dotDenominator;
+
    Vector3 vector;
//calculate the distance between the linePoint and the line-plane intersection point
+
    intersection = Vector3.zero;
dotNumerator = Vector3.Dot((planePoint - linePoint), planeNormal);
+
   
dotDenominator = Vector3.Dot(lineVec, planeNormal);
+
    //calculate the distance between the linePoint and the line-plane intersection point
+
    dotNumerator = Vector3.Dot((planePoint - linePoint), planeNormal);
//line and plane are not parallel
+
    dotDenominator = Vector3.Dot(lineVec, planeNormal);
if(dotDenominator != 0.0f){
+
   
length =  dotNumerator / dotDenominator;
+
    //line and plane are not parallel
+
    if(dotDenominator != 0.0f){
//create a vector from the linePoint to the intersection point
+
        length =  dotNumerator / dotDenominator;
vector = setVectorLength(lineVec, length);
+
       
+
        //create a vector from the linePoint to the intersection point
//get the coordinates of the line-plane intersection point
+
        vector = SetVectorLength(lineVec, length);
return linePoint + vector;
+
       
}
+
        //get the coordinates of the line-plane intersection point
+
        intersection = linePoint + vector;
else{
+
       
return Vector3.zero;
+
        return true;
}
+
    }
+
   
 +
    //output not valid
 +
    else{
 +
        return false;
 +
    }
 
}
 
}
  
//Two non-parallel lines which may or may not touch each other have a point on each line which lays closest
+
//Calculate the instersection point of two lines. Returns true if lines intersect, otherwise false.
//to each other. This function finds those two points.
+
//Note that in 3d, two lines do not intersect most of the time. So if the two lines are not in the
void ClosestPointsOnTwoLines(out Vector3 closestPointLine1, out Vector3 closestPointLine2, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2){
+
//same plane, use ClosestPointsOnTwoLines() instead.
 +
bool LineLineIntersection(out Vector3 intersection, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2){
  
closestPointLine1 = Vector3.zero;
+
    intersection = Vector3.zero;
closestPointLine2 = Vector3.zero;
+
   
+
    Vector3 lineVec3 = linePoint2 - linePoint1;
float a = Vector3.Dot(lineVec1, lineVec1);
+
    Vector3 crossVec1and2 = Vector3.Cross(lineVec1, lineVec2);
float b = Vector3.Dot(lineVec1, lineVec2);
+
    Vector3 crossVec3and2 = Vector3.Cross(lineVec3, lineVec2);
float e = Vector3.Dot(lineVec2, lineVec2);
+
 
+
    float planarFactor = Vector3.Dot(lineVec3, crossVec1and2);
float d = a*e - b*b;
+
   
+
    //Lines are not coplanar. Take into account rounding errors.
//lines are not parallel
+
    if((planarFactor >= 0.00001f) || (planarFactor <= -0.00001f)){
if(d != 0.0f){
+
 
+
        return false;
Vector3 r = linePoint1 - linePoint2;
+
    }
float c = Vector3.Dot(lineVec1, r);
+
 
float f = Vector3.Dot(lineVec2, r);
+
    //Note: sqrMagnitude does x*x+y*y+z*z on the input vector.
+
    float s = Vector3.Dot(crossVec3and2, crossVec1and2) / crossVec1and2.sqrMagnitude;
float s = (b*f - c*e) / d;
+
   
float t = (a*f - c*b) / d;
+
    if((s >= 0.0f) && (s <= 1.0f)){
+
 
closestPointLine1 = linePoint1 + lineVec1 * s;
+
        intersection = linePoint1 + (lineVec1 * s);
closestPointLine2 = linePoint2 + lineVec2 * t;
+
        return true;
}
+
    }
+
   
else{
+
    else{
closestPointLine1 = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
+
        return false;     
closestPointLine2 = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
+
    }
}
+
}
 +
 
 +
//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 3d point in space which is a projection from point "point" to a line, consisting
+
//This function returns a point which is a projection from a point to a line.
//of a vector (lineVec) and a point on that line (linePoint).
+
 
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
+
    //get vector from point on line to point in space
Vector3 linePointToPoint = point - linePoint;
+
    Vector3 linePointToPoint = point - linePoint;
  
float t = Vector3.Dot(linePointToPoint, lineVec);
+
    float t = Vector3.Dot(linePointToPoint, lineVec);
+
   
return linePoint + lineVec * t;
+
    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;
+
    float distance;
Vector3 translationVector;
+
    Vector3 translationVector;
+
   
//First calculate the distance from the point to the plane:
+
    //First calculate the distance from the point to the plane:
distance = signedDistancePlanePoint(planeNormal, planePoint, point);
+
    distance = SignedDistancePlanePoint(planeNormal, planePoint, point);
+
   
//Reverse the sign of the distance
+
    //Reverse the sign of the distance
distance *= -1;
+
    distance *= -1;
+
   
//Get a translation vector
+
    //Get a translation vector
translationVector = setVectorLength(planeNormal, distance);
+
    translationVector = SetVectorLength(planeNormal, distance);
+
   
//Translate the point to form a projection
+
    //Translate the point to form a projection
return point + translationVector;
+
    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);
+
    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));
+
    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;
+
    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);
+
    //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);
+
    //Now calculate the dot product between the perpendicular vector (perpVector) and the other input vector
+
    dot = Vector3.Dot(perpVector, vectorB);
return dot;
+
   
 +
    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 dot;
float angle;
+
    float angle;
+
   
//calculate the the dot product between the two input vectors. This gives the cosine between the two vectors
+
    //calculate the the dot product between the two input vectors. This gives the cosine between the two vectors
dot = Vector3.Dot(vector, normal);
+
    dot = Vector3.Dot(vector, normal);
+
   
//this is in radians
+
    //this is in radians
angle = (float)Math.Acos(dot);
+
    angle = (float)Math.Acos(dot);
+
   
return 1.570796326794897f - angle; //90
+
    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);
 
 
//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;
 
}
 
 
//Calculate the instersection point of two lines. Returns true if lines intersect, otherwise false.
 
bool LineLineIntersection(out Vector3 intersection, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2){
 
 
    intersection = Vector3.zero;
 
 
      
 
      
     Vector3 lineVec3 = linePoint2 - linePoint1;
+
     double dot;
     Vector3 crossVec1and2 = Vector3.Cross(lineVec1, lineVec2);
+
     double angle;
    Vector3 crossVec3and2 = Vector3.Cross(lineVec3, lineVec2);
+
 
+
    float planarFactor = Vector3.Dot(lineVec3, crossVec1and2);
+
 
      
 
      
     //Lines are not coplanar. Take into account rounding errors.
+
     //get the dot product
    if((planarFactor >= 0.00001f) || (planarFactor <= -0.00001f)){
+
     dot = Vector3.Dot(vec1, vec2);
 
+
        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)){
+
    //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){
         intersection = linePoint1 + (lineVec1 * s);
+
         dot = -1.0f;
         return true;
+
    }
 +
    if(dot > 1.0f){
 +
         dot =1.0f;
 
     }
 
     }
 
      
 
      
     else{
+
     //Calculate the angle. The output is in radians
        return false;      
+
    //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);
}
Personal tools
Namespaces

Variants
Actions
Navigation
Extras
Toolbox