3d Math functions

I made a bunch of 3d Math functions for your entertainment.
These include:

Code (csharp):

1.  
2. //increase or decrease the length of vector by size
3. Vector3 AddVectorLength(Vector3 vector, float size){
4.  
5. //create a vector of direction "vector" with length "size"
6. Vector3 SetVectorLength(Vector3 vector, float size){
7.  
8. //caclulate the rotational difference from A to B
9. Quaternion SubtractRotation(Quaternion B, Quaternion A){
10.  
11. //Find the line of intersection between two planes.
12. bool PlanePlaneIntersection(out Vector3 linePoint, out Vector3 lineVec, Vector3 plane1Normal, Vector3 plane1Position, Vector3 plane2Normal, Vector3 plane2Position){
13.  
14. //Get the coordinates of the intersection between a line and a plane
15. bool LinePlaneIntersection(out Vector3 intersection, Vector3 linePoint, Vector3 lineVec, Vector3 planeNormal, Vector3 planePoint){
16.  
17. //Calculate the instersection point of two lines. Returns true if lines intersect, otherwise false.
18. bool LineLineIntersection(out Vector3 intersection, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2){
19.  
20. //Two non-parallel lines which may or may not touch each other have a point on each line which lays closest
21. //to each other. This function finds those two points.
22. bool ClosestPointsOnTwoLines(out Vector3 closestPointLine1, out Vector3 closestPointLine2, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2){
23.  
24. //This function returns a point which is a projection from a point to a line.
25. Vector3 ProjectPointOnLine(Vector3 linePoint, Vector3 lineVec, Vector3 point){
26.  
27. //This function returns a point which is a projection from a point to a line segment.
28. Vector3 ProjectPointOnLineSegment(Vector3 linePoint1, Vector3 linePoint2, Vector3 point){
29.  
30. //This function returns a point which is a projection from a point to a plane.
31. Vector3 ProjectPointOnPlane(Vector3 planeNormal, Vector3 planePoint, Vector3 point){
32.  
33. //Projects a vector onto a plane. The output is not normalized.
34. Vector3 ProjectVectorOnPlane(Vector3 planeNormal, Vector3 vector){
35.  
36. //Get the shortest distance between a point and a plane. The output is signed so it holds information
37. //as to which side of the plane normal the point is.
38. float SignedDistancePlanePoint(Vector3 planeNormal, Vector3 planePoint, Vector3 point){
39.  
40. //This fuction calculates a signed (+ or - sign instead of being ambiguous) dot product. It is basically used
41. //to figure out whether a vector is positioned to the left or right of another vector.
42. float SignedDotProduct(Vector3 vectorA, Vector3 vectorB, Vector3 normal){
43.  
44. //Calculate the angle between a vector and a plane. The plane is made by a normal vector.
45. //Output is in radians.
46. float AngleVectorPlane(Vector3 vector, Vector3 normal){
47.  
48. //Calculate the dot product as an angle
49. float DotProductAngle(Vector3 vec1, Vector3 vec2){
50.  
51. //Convert a plane defined by 3 points to a plane defined by a vector and a point.
52. //The plane point is the middle of the triangle defined by the 3 points.
53. void PlaneFrom3Points(out Vector3 planeNormal, out Vector3 planePoint, Vector3 pointA, Vector3 pointB, Vector3 pointC){
54.  
55. //Returns the forward vector of a quaternion
56. Vector3 GetForwardVector(Quaternion q)
57.  
58. //Returns the up vector of a quaternion
59. Vector3 GetUpVector(Quaternion q)
60.  
61. //Returns the right vector of a quaternion
62. Vector3 GetRightVector(Quaternion q)
63.  
64. //Gets a quaternion from a matrix
65. Quaternion QuaternionFromMatrix(Matrix4x4 m)
66.  
67. //Gets a position from a matrix
68. Vector3 PositionFromMatrix(Matrix4x4 m)
69.  
70. //This function translates one object as if it was parented to the other.
71. //Before using this function, the Init() function must be called
72. void TransformWithParent(out Quaternion childRotation, out Vector3 childPosition, Quaternion parentRotation, Vector3 parentPosition, Quaternion startParentRotation, Vector3 startParentPosition, Quaternion startChildRotation, Vector3 startChildPosition)
73.  
74. //With this function you can align a triangle of an object with any transform.
75. void PreciseAlign(ref GameObject gameObjectInOut, Vector3 alignWithVector, Vector3 alignWithNormal, Vector3 alignWithPosition, Vector3 triangleForward, Vector3 triangleNormal, Vector3 trianglePosition)
76.  
77. //This is an alternative for Quaternion.LookRotation. Instead of aligning the forward and up vector of the game
78. //object with the input vectors, a custom direction can be used instead of the fixed forward and up vectors.
79. void LookRotationExtended(ref GameObject gameObjectInOut, Vector3 alignWithVector, Vector3 alignWithNormal, Vector3 customForward, Vector3 customUp)
80.  
81. //Convert a position, direction, and normal vector to a transform
82. void VectorsToTransform(ref GameObject gameObjectInOut, Vector3 positionVector, Vector3 directionVector, Vector3 normalVector){
83.  
84. //This function finds out on which side of a line segment the point is located.
85. int PointOnWhichSideOfLineSegment(Vector3 linePoint1, Vector3 linePoint2, Vector3 point){
86.  
87. //Returns true if a line segment (made up of linePoint1 and linePoint2) is fully or partially in a rectangle
88. bool IsLineInRectangle(Vector3 linePoint1, Vector3 linePoint2, Vector3 rectA, Vector3 rectB, Vector3 rectC, Vector3 rectD){
89.  
90. //Returns true if "point" is in a rectangle made up of RectA to RectD. The line point is assumed to be on the same
91. //plane as the rectangle. If the point is not on the plane, use ProjectPointOnPlane() first.
92. bool IsPointInRectangle(Vector3 point, Vector3 rectA, Vector3 rectC, Vector3 rectB, Vector3 rectD){
93.  
94. //Returns true if line segment made up of pointA1 and pointA2 is crossing line segment made up of
95. //pointB1 and pointB2. The two lines are assumed to be in the same plane.
96. bool AreLineSegmentsCrossing(Vector3 pointA1, Vector3 pointA2, Vector3 pointB1, Vector3 pointB2){
97.  
98. //Returns the pixel distance from the mouse pointer to a line.
99. //Alternative for HandleUtility.DistanceToLine(). Works both in Editor mode and Play mode.
100. float MouseDistanceToLine(Vector3 linePoint1, Vector3 linePoint2){
101.  
102. //Returns the pixel distance from the mouse pointer to a camera facing circle.
103. //Alternative for HandleUtility.DistanceToCircle(). Works both in Editor mode and Play mode.
104. float MouseDistanceToCircle(Vector3 point, float radius){
105.  
106. //This function calculates the acceleration vector in meter/second^2.
107. //Input: position.
108. bool LinearAcceleration(out Vector3 vector, Vector3 position, int samples){
109.  
110. //This function calculates angular acceleration in object space as deg/second^2, encoded as a vector.
111. bool AngularAcceleration(out Vector3 vector, Quaternion rotation, int samples){
112.  
113. //Get y from a linear function, with x as an input. The linear function goes through points
114. //0,0 on the left ,and Qxy on the right.
115. float LinearFunction2DBasic(float x, float Qx, float Qy){
116.  
117. //Get y from a linear function, with x as an input. The linear function goes through points
118. //Pxy on the left ,and Qxy on the right.
119. float LinearFunction2DFull(float x, float Px, float Py, float Qx, float Qy){
120.  
121. //Add rotation B to rotation A.
122. Quaternion AddRotation(Quaternion A, Quaternion B){
123.  
124. //Same as the build in TransformDirection(), but using a rotation instead of a transform
125. Vector3 TransformDirectionMath(Quaternion rotation, Vector3 vector){
126.  
127. //Same as the build in InverseTransformDirection(), but using a rotation instead of a transform
128. Vector3 InverseTransformDirectionMath(Quaternion rotation, Vector3 vector){
129.  
130. //Rotate a vector as if it is attached to an object with rotation "from", which is then rotated to rotation "to".
131. //Similar to TransformWithParent(), but rotating a vector instead of a transform.
132. Vector3 RotateVectorFromTo(Quaternion from, Quaternion to, Vector3 vector){
133.  
134. //Get a point on a Catmull-Rom spline.
135. Vector2 GetPointOnSpline(float percentage, Vector2[] cPoints) {
136.  
137. //Finds the intersection points between a straight line and a spline. Solves a Cubic polynomial equation.
138. float[] GetLineSplineIntersections(Vector2[] linePoints, Vector2[] cPoints) {
139.  

Full code here:
https://drive.google.com/file/d/1O0d_pqitpgbFYJG06JO6uAJjudI-QHwn/view?usp=sharing

Make sure all input vectors are normalized.

 

YAY thanks man

 

thanks man

 

Nice, thanks!

 

Why not add an Version using extentsion methods?

i.e.

Code (csharp):

1.  
2. public static class Math3D {
3.     public static Vector3 addLength(this Vector3 vector, float size){
4.        ....
5.     }
6. }
7.  

then you can use this in your code

Code (csharp):

1.  
2.    Vector3 position = new Vector3(1,1,1);
3.    position.addLength(10); // Call the extension method
4.  
5.    // instead of
6.    position = Math3D.addVectorLength(position, 10);
7.  

It's much shorter and more intuitive, because the extended method will only be displayed in vector3 objects, shorter (addLength vs addVectorLength) and you don't have to pass the vector which is being modiifed to the method ^^

 

Using extensions is a good idea indeed. I never used it but I will look into it.
Thanks.

 

Yea, Extension methods are lovely and allow you to extends "sealed" classes, which can't be extended otherwise or having derive from a class and use the new class instead (who would like to use MyVector3 etc. instead of Vector3 )

On and when we're on it, we in C# use CamelCase for public Methods (and Properties) name

Right: public void AddLength(...)
Wrong: public void addLength(...)

It's a naming convention adopted by most serious C# developers, so it's easier to work and read the Code

 

I never looked up the proper C# naming conventions but I am aware of it's existence. Maybe this is a good time to get to grips with it.

 

As in unity the Vector3 is a struct, it's passed by value. So you'll still have to do a position = position.AddLength(10);. But I agree it's still nicer and faster to find using code completion this way.

 

I added these functions:

Code (csharp):

1.  
2. //Calculate the instersection point of two lines. Returns true if lines intersect, otherwise false.
3. bool LineLineIntersection(out Vector3 intersection, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2){
4.  
5. //Convert a plane defined by 3 points to a plane defined by a vector and a point.
6. //The plane point is the middle of the triangle defined by the 3 points.
7. void PlaneFrom3Points(out Vector3 planeNormal, out Vector3 planePoint, Vector3 pointA, Vector3 pointB, Vector3 pointC){
8.  

I still didn't get around to make it work as a extension method...

 

Code (csharp):

1. //Get the coordinates (world space) of the intersection between a line and a plane
2. Vector3 linePlaneIntersection(Vector3 linePoint, Vector3 lineVec, Vector3 planeNormal, Vector3 planePoint){
3. ...
4.     //line and plane are not parallel
5.     if(dotDenominator != 0.0f){
6. ...
7.     }
8.     else{
9.         return Vector3.zero;
10.     }
11. }

Vector3.zero can be a valid intersection point.

--
Anyway, respect! It's a very useful set of functions.

 

I am not sure what you mean. Do you mean there should be a separate variable indicating whether the output is valid or not? There probably should...

Edit:
Ok, I made all functions output bool false when the output is not valid instead of setting the output to zero or max it out.

Edit2:
Some functions added

Edit 3:
Added

Code (csharp):

1. PointOnWhichSideOfLineSegment()

Edit 4:
6 more functions added

 

I added a function called LinearAcceleration() which calculates the acceleration / deceleration of any object using only the position as an input. It doesn't require a rigidbody, just a position vector will do.

A nice "side effect" is that it automatically calculates centripetal force as well, without the object having to rotate in a perfect circle. It works purely on the change of position over time.

Edit:
I also added 2 linear functions. These are 2D instead of 3D but I find them quite useful so I included them anyway. It is based on this function:



Edit 2:
Added AngularAcceleration() function.

 

Update: some new functions added:

Code (CSharp):

1. //Add rotation B to rotation A.
2. public static Quaternion AddRotation(Quaternion A, Quaternion B){
3.  
4.     Quaternion C = A * B;    
5.     return C;
6. }
7.  
8. //Same as the build in TransformDirection(), but using a rotation instead of a transform
9. public static Vector3 TransformDirectionMath(Quaternion rotation, Vector3 vector){
10.  
11.     Vector3 output = rotation * vector;
12.     return output;
13. }
14.  
15. //Same as the build in InverseTransformDirection(), but using a rotation instead of a transform
16. public static Vector3 InverseTransformDirectionMath(Quaternion rotation, Vector3 vector){
17.  
18.     Vector3 output = Quaternion.Inverse(rotation) * vector;
19.     return output;
20. }
21.  
22. //Rotate a vector as if it is attached to an object with rotation "from", which is then rotated to rotation "to".
23. //Similar to TransformWithParent(), but rotating a vector instead of a transform.
24. public static Vector3 RotateVectorFromTo(Quaternion from, Quaternion to, Vector3 vector){
25.                                                             //Note: comments are in case all inputs are in World Space.
26.     Quaternion Q = SubtractRotation(to, from);                //Output is in object space.
27.     Vector3 A = InverseTransformDirectionMath(from, vector);//Output is in object space.
28.     Vector3 B = Q * A;                                        //Output is in local space.
29.     Vector3 C = TransformDirectionMath(from, B);            //Output is in world space.
30.     return C;
31. }

Note that the RotateVectorFromTo() function might be optimized. Let me know if you have any suggestions.

 

Thanks for sharing your work! Small bit of feedback: RotDiffToSpeedVec() is called a couple of times, but isn't implemented here.

 

Thanks, I forgot about that one. Fixed it now.

 

Nice work. When you're ready to speed it up a ton, check this out:

http://www.performancesimulations.c...ses-in-unitys-physics-or-any-math-heavy-code/

 

Wow, very interesting article. Thanks for the link!

 

I tested the LineLineIntersection function and it seems not to work properly.
These 2 lines should return "true" but the function says there is no intersection:

linepoint1: (5.0, 0.0, -10.0), lineVec1: (5.0, 0.0, 10.0)
linepoint2: (10.0, 0.0, 5.0), lineVec2: (-10.0, 0.0, 5.0)

Or did I get the parameters wrong?

 

I never had much luck with that function in 3D and instead use ClosestPointsOnTwoLines. If the lines truly intersect you'll get the point from it.

 

There's a flaw in the algorithm.

2 lines in 3-space intersect if they're coplanar and NOT parallel.

The algorithm determines if they're coplanar... but then goes on to get the distance along line 1 from its point that line 2 intersects it... and it assumes if that value is not between 0 and 1, they don't intersect... what?

This forces the inputs to have to have their points defined really near each other.

How you can test if they're not parallel is if crossVec1and2 is not near a zero vector (sqrMagnitude won't be 0), since a zero vector results when you cross 2 vectors that are parallel.

Rewritten:

Code (csharp):

1.  
2.     public static bool LineLineIntersection(out Vector3 intersection, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2){
3.  
4.         Vector3 lineVec3 = linePoint2 - linePoint1;
5.         Vector3 crossVec1and2 = Vector3.Cross(lineVec1, lineVec2);
6.         Vector3 crossVec3and2 = Vector3.Cross(lineVec3, lineVec2);
7.  
8.         float planarFactor = Vector3.Dot(lineVec3, crossVec1and2);
9.  
10.         //is coplanar, and not parrallel
11.         if(Mathf.Abs(planarFactor) < 0.0001f && crossVec1and2.sqrMagnitude > 0.0001f)
12.         {
13.             float s = Vector3.Dot(crossVec3and2, crossVec1and2) / crossVec1and2.sqrMagnitude;
14.             intersection = linePoint1 + (lineVec1 * s);
15.             return true;
16.         }
17.         else
18.         {
19.             intersection = Vector3.zero;
20.             return false;
21.         }
22.     }
23.  

Which you can also see here in my framework:
https://github.com/lordofduct/spacepuppy-unity-framework/blob/master/SpacepuppyBase/Geom/Line.cs

 

Thanks for pointing that out. I have changed it in the wiki.

 

Is there a function that create rectangle points from a rectangle position and size ? it will be very powerful, So this will simplify the use of IsPointInRectangle function. thanks

 

No, but feel free to add it to the wiki.

 

Added a function which finds the intersection points between a line and a Catmull-rom spline. It basically reverse solves a spline function. So instead of providing a value (0-1) to get the location on a spline, you input the location on a spline and get the corresponding 0-1 value.

Sounds easy but this is not trivial. Implementations based on this:
http://lifeinacubicleblog.com/2016/...ne-intersection-part-2-mathematical-approach/

Use case1:
Create a gauge with a non-linear scale by defining an array with needle angles vs the number it should point at. The array creates a spline. Driving the needle with a float in range 0 to 1 gives an unpredictable result. Instead, use the GetLineSplineIntersections() function to find the angle the gauge needle should have for a given number it should point at. In this case, cPoints should contain x for angle and y for scale number. Make a horizontal line at the given scale number (y) you want to find the needle angle for. The returned float is a percentage location on the spline (range 0 to 1).
Plug this value into the GetPointOnSpline() function to get the x coordinate which represents the needle angle.

Use case 2:
Make a gauge follow a non-linear animation, for example the (Exhaust Gas Temperature) EGT of a jet engine during startup. A video of a the event would be recorded and used to capture sample points consisting of EGT vs time. The time (x) and EGT (y) values would then be used to create a spline, allowing smooth interpolation between the original sample points. The line-spline intersection function can then be used to get the EGT for any point in time.

More info and a demo project can be found on my blog:
https://bitbarrelmedia.wordpress.com/2017/11/07/driving-non-linear-gauges/

 

Awesome library!!! Should one simply copy the code from the wiki page or is there a git repo or Unity Asset?

 

Thank you! Very useful @Elecman !! You made my day

 

Just copy the code. There is no git repo or Unity asset.

 

Please note, all content published on the http://wiki.unity3d.com/index.php/3d_Math_functions is is available under Creative Commons Attribution Share Alike. That means the source code of your project also must be published if you use it.
Elecman, it would be nice if you publish your code under MIT license or similar

 

Which project? All my projects which use it are open source already.

 

And that is the problem Unity Wiki has now moved, the links have broken and ths link is still the main hit from Google

 

web archive:
https://web.archive.org/web/20210507045029/http://wiki.unity3d.com/index.php/3d_Math_functions

unity wiki backup zip from 2017
https://github.com/UnityCommunity/UnityLibrary/issues/16#issuecomment-321150087

 

I uploaded the latest file to my google drive. You can get the link from the first post or from my signature.