Math3d.cs 50 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373
  1. using UnityEngine;
  2. using System.Collections;
  3. using System.Collections.Generic;
  4. using System;
  5. public class Math3d
  6. {
  7. private static Transform tempChild = null;
  8. private static Transform tempParent = null;
  9. private static Vector3[] positionRegister;
  10. private static float[] posTimeRegister;
  11. private static int positionSamplesTaken = 0;
  12. private static Quaternion[] rotationRegister;
  13. private static float[] rotTimeRegister;
  14. private static int rotationSamplesTaken = 0;
  15. public static void Init()
  16. {
  17. tempChild = (new GameObject("Math3d_TempChild")).transform;
  18. tempParent = (new GameObject("Math3d_TempParent")).transform;
  19. tempChild.gameObject.hideFlags = HideFlags.HideAndDontSave;
  20. MonoBehaviour.DontDestroyOnLoad(tempChild.gameObject);
  21. tempParent.gameObject.hideFlags = HideFlags.HideAndDontSave;
  22. MonoBehaviour.DontDestroyOnLoad(tempParent.gameObject);
  23. //set the parent
  24. tempChild.parent = tempParent;
  25. }
  26. //Get a point on a Catmull-Rom spline.
  27. //The percentage is in range 0 to 1, which starts at the second control point and ends at the second last control point.
  28. //The array cPoints should contain all control points. The minimum amount of control points should be 4.
  29. //Source: https://forum.unity.com/threads/waypoints-and-constant-variable-speed-problems.32954/#post-213942
  30. public static Vector2 GetPointOnSpline(float percentage, Vector2[] cPoints)
  31. {
  32. //Minimum size is 4
  33. if (cPoints.Length >= 4)
  34. {
  35. //Convert the input range (0 to 1) to range (0 to numSections)
  36. int numSections = cPoints.Length - 3;
  37. int curPoint = Mathf.Min(Mathf.FloorToInt(percentage * (float)numSections), numSections - 1);
  38. float t = percentage * (float)numSections - (float)curPoint;
  39. //Get the 4 control points around the location to be sampled.
  40. Vector2 p0 = cPoints[curPoint];
  41. Vector2 p1 = cPoints[curPoint + 1];
  42. Vector2 p2 = cPoints[curPoint + 2];
  43. Vector2 p3 = cPoints[curPoint + 3];
  44. //The Catmull-Rom spline can be written as:
  45. // 0.5 * (2*P1 + (-P0 + P2) * t + (2*P0 - 5*P1 + 4*P2 - P3) * t^2 + (-P0 + 3*P1 - 3*P2 + P3) * t^3)
  46. //Variables P0 to P3 are the control points.
  47. //Variable t is the position on the spline, with a range of 0 to numSections.
  48. //C# way of writing the function. Note that f means float (to force precision).
  49. Vector2 result = .5f * (2f * p1 + (-p0 + p2) * t + (2f * p0 - 5f * p1 + 4f * p2 - p3) * (t * t) + (-p0 + 3f * p1 - 3f * p2 + p3) * (t * t * t));
  50. return new Vector2(result.x, result.y);
  51. }
  52. else
  53. {
  54. return new Vector2(0, 0);
  55. }
  56. }
  57. //Finds the intersection points between a straight line and a spline. Solves a Cubic polynomial equation
  58. //The output is in the form of a percentage along the length of the spline (range 0 to 1).
  59. //The linePoints array should contain two points which form a straight line.
  60. //The cPoints array should contain all the control points of the spline.
  61. //Use case: 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.
  62. //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
  63. //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.
  64. //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).
  65. //Plug this value into the GetPointOnSpline() function to get the x coordinate which represents the needle angle.
  66. //Source: https://medium.com/@csaba.apagyi/finding-catmull-rom-spline-and-line-intersection-part-2-mathematical-approach-dfb969019746
  67. public static float[] GetLineSplineIntersections(Vector2[] linePoints, Vector2[] cPoints)
  68. {
  69. List<float> list = new List<float>();
  70. float[] crossings;
  71. int numSections = cPoints.Length - 3;
  72. //The line spline intersection can only be calculated for one segment of a spline, meaning 4 control points,
  73. //with a spline segment between the middle two control points. So check all spline segments.
  74. for (int i = 0; i < numSections; i++)
  75. {
  76. //Get the 4 control points around the location to be sampled.
  77. Vector2 p0 = cPoints[i];
  78. Vector2 p1 = cPoints[i + 1];
  79. Vector2 p2 = cPoints[i + 2];
  80. Vector2 p3 = cPoints[i + 3];
  81. //The Catmull-Rom spline can be written as:
  82. // 0.5 * (2P1 + (-P0 + P2) * t + (2P0 - 5P1 + 4P2 - P3) * t^2 + (-P0 + 3P1 - 3P2 + P3) * t^3)
  83. //Variables P0 to P3 are the control points.
  84. //Notation: 2P1 means 2*controlPoint1
  85. //Variable t is the position on the spline, converted from a range of 0 to 1.
  86. //C# way of writing the function is below. Note that f means float (to force precision).
  87. //Vector2 result = .5f * (2f * p1 + (-p0 + p2) * t + (2f * p0 - 5f * p1 + 4f * p2 - p3) * (t * t) + (-p0 + 3f * p1 - 3f * p2 + p3) * (t * t * t));
  88. //The variable t is the only unknown, so the rest can be substituted:
  89. //a = 0.5 * (-p0 + 3*p1 - 3*p2 + p3)
  90. //b = 0.5 * (2*p0 - 5*p1 + 4*p2 - p3)
  91. //c = 0.5 * (-p0 + p2)
  92. //d = 0.5 * (2*p1)
  93. //This gives rise to the following Cubic equation:
  94. //a * t^3 + b * t^2 + c * t + d = 0
  95. //The spline control points (p0-3) consist of two variables: the x and y coordinates. They are independent so we can handle them separately.
  96. //Below, a1 is substitution a where the x coordinate of each point is used, like so: a1 = 0.5 * (-p0.x + 3*p1.x - 3*p2.x + p3.x)
  97. //Below, a2 is substitution a where the y coordinate of each point is used, like so: a2 = 0.5 * (-p0.y + 3*p1.y - 3*p2.y + p3.y)
  98. //The same logic applies for substitutions b, c, and d.
  99. float a1 = 0.5f * (-p0.x + 3f * p1.x - 3f * p2.x + p3.x);
  100. float a2 = 0.5f * (-p0.y + 3f * p1.y - 3f * p2.y + p3.y);
  101. float b1 = 0.5f * (2f * p0.x - 5f * p1.x + 4f * p2.x - p3.x);
  102. float b2 = 0.5f * (2f * p0.y - 5f * p1.y + 4f * p2.y - p3.y);
  103. float c1 = 0.5f * (-p0.x + p2.x);
  104. float c2 = 0.5f * (-p0.y + p2.y);
  105. float d1 = 0.5f * (2f * p1.x);
  106. float d2 = 0.5f * (2f * p1.y);
  107. //We now have two Cubic functions. One for x and one for y.
  108. //Note that a, b, c, and d are not vector variables itself but substituted functions.
  109. //x = a1 * t^3 + b1 * t^2 + c1 * t + d1
  110. //y = a2 * t^3 + b2 * t^2 + c2 * t + d2
  111. //Line formula, standard form:
  112. //Ax + By + C = 0
  113. float A = linePoints[0].y - linePoints[1].y;
  114. float B = linePoints[1].x - linePoints[0].x;
  115. float C = (linePoints[0].x - linePoints[1].x) * linePoints[0].y + (linePoints[1].y - linePoints[0].y) * linePoints[0].x;
  116. //Substituting the values of x and y from the separated Spline formula into the Line formula, we get:
  117. //A * (a1 * t^3 + b1 * t^2 + c1 * t + d1) + B * (a2 * t^3 + b2 * t^2 + c2 * t + d2) + C = 0
  118. //Rearranged version:
  119. //(A * a1 + B * a2) * t^3 + (A * b1 + B * b2) * t^2 + (A * c1 + B * c2) * t + (A * d1 + B * d2 + C) = 0
  120. //Substituting gives rise to a Cubic function:
  121. //a * t^3 + b * t^2 + c * t + d = 0
  122. float a = A * a1 + B * a2;
  123. float b = A * b1 + B * b2;
  124. float c = A * c1 + B * c2;
  125. float d = A * d1 + B * d2 + C;
  126. //This is again a Cubic equation, combined from the Line and the Spline equation. If you solve this you can get up to 3 line-spline cross points.
  127. //How to solve a Cubic equation is described here:
  128. //https://www.cs.rit.edu/~ark/pj/lib/edu/rit/numeric/Cubic.shtml
  129. //https://www.codeproject.com/Articles/798474/To-Solve-a-Cubic-Equation
  130. int crossAmount;
  131. float cross1;
  132. float cross2;
  133. float cross3;
  134. float crossCorrected;
  135. //Two different implementations of solving a Cubic equation.
  136. // SolveCubic2(out crossAmount, out cross1, out cross2, out cross3, a, b, c, d);
  137. SolveCubic(out crossAmount, out cross1, out cross2, out cross3, a, b, c, d);
  138. //Get the highest and lowest value (in range 0 to 1) of the current section and calculate the difference.
  139. float currentSectionLowest = (float)i / (float)numSections;
  140. float currentSectionHighest = ((float)i + 1f) / (float)numSections;
  141. float diff = currentSectionHighest - currentSectionLowest;
  142. //Only use the result if it is within range 0 to 1.
  143. //The range 0 to 1 is within the current segment. It has to be converted to the range of the entire spline,
  144. //which still uses a range of 0 to 1.
  145. if (cross1 >= 0 && cross1 <= 1)
  146. {
  147. //Map an intermediate range (0 to 1) to the lowest and highest section values.
  148. crossCorrected = (cross1 * diff) + currentSectionLowest;
  149. //Add the result to the list.
  150. list.Add(crossCorrected);
  151. }
  152. if (cross2 >= 0 && cross2 <= 1)
  153. {
  154. //Map an intermediate range (0 to 1) to the lowest and highest section values.
  155. crossCorrected = (cross2 * diff) + currentSectionLowest;
  156. //Add the result to the list.
  157. list.Add(crossCorrected);
  158. }
  159. if (cross3 >= 0 && cross3 <= 1)
  160. {
  161. //Map an intermediate range (0 to 1) to the lowest and highest section values.
  162. crossCorrected = (cross3 * diff) + currentSectionLowest;
  163. //Add the result to the list.
  164. list.Add(crossCorrected);
  165. }
  166. }
  167. //Convert the list to an array.
  168. crossings = list.ToArray();
  169. return crossings;
  170. }
  171. //Solve cubic equation according to Cardano.
  172. //Source: https://www.cs.rit.edu/~ark/pj/lib/edu/rit/numeric/Cubic.shtml
  173. private static void SolveCubic(out int nRoots, out float x1, out float x2, out float x3, float a, float b, float c, float d)
  174. {
  175. float TWO_PI = 2f * Mathf.PI;
  176. float FOUR_PI = 4f * Mathf.PI;
  177. // Normalize coefficients.
  178. float denom = a;
  179. a = b / denom;
  180. b = c / denom;
  181. c = d / denom;
  182. // Commence solution.
  183. float a_over_3 = a / 3f;
  184. float Q = (3f * b - a * a) / 9f;
  185. float Q_CUBE = Q * Q * Q;
  186. float R = (9f * a * b - 27f * c - 2f * a * a * a) / 54f;
  187. float R_SQR = R * R;
  188. float D = Q_CUBE + R_SQR;
  189. if (D < 0.0f)
  190. {
  191. // Three unequal real roots.
  192. nRoots = 3;
  193. float theta = Mathf.Acos(R / Mathf.Sqrt(-Q_CUBE));
  194. float SQRT_Q = Mathf.Sqrt(-Q);
  195. x1 = 2f * SQRT_Q * Mathf.Cos(theta / 3f) - a_over_3;
  196. x2 = 2f * SQRT_Q * Mathf.Cos((theta + TWO_PI) / 3f) - a_over_3;
  197. x3 = 2f * SQRT_Q * Mathf.Cos((theta + FOUR_PI) / 3f) - a_over_3;
  198. }
  199. else if (D > 0.0f)
  200. {
  201. // One real root.
  202. nRoots = 1;
  203. float SQRT_D = Mathf.Sqrt(D);
  204. float S = CubeRoot(R + SQRT_D);
  205. float T = CubeRoot(R - SQRT_D);
  206. x1 = (S + T) - a_over_3;
  207. x2 = float.NaN;
  208. x3 = float.NaN;
  209. }
  210. else
  211. {
  212. // Three real roots, at least two equal.
  213. nRoots = 3;
  214. float CBRT_R = CubeRoot(R);
  215. x1 = 2 * CBRT_R - a_over_3;
  216. x2 = CBRT_R - a_over_3;
  217. x3 = x2;
  218. }
  219. }
  220. //Mathf.Pow is used as an alternative for cube root (Math.cbrt) here.
  221. private static float CubeRoot(float d)
  222. {
  223. if (d < 0.0f)
  224. {
  225. return -Mathf.Pow(-d, 1f / 3f);
  226. }
  227. else
  228. {
  229. return Mathf.Pow(d, 1f / 3f);
  230. }
  231. }
  232. //increase or decrease the length of vector by size
  233. public static Vector3 AddVectorLength(Vector3 vector, float size)
  234. {
  235. //get the vector length
  236. float magnitude = Vector3.Magnitude(vector);
  237. //calculate new vector length
  238. float newMagnitude = magnitude + size;
  239. //calculate the ratio of the new length to the old length
  240. float scale = newMagnitude / magnitude;
  241. //scale the vector
  242. return vector * scale;
  243. }
  244. //create a vector of direction "vector" with length "size"
  245. public static Vector3 SetVectorLength(Vector3 vector, float size)
  246. {
  247. //normalize the vector
  248. Vector3 vectorNormalized = Vector3.Normalize(vector);
  249. //scale the vector
  250. return vectorNormalized *= size;
  251. }
  252. //caclulate the rotational difference from A to B
  253. public static Quaternion SubtractRotation(Quaternion B, Quaternion A)
  254. {
  255. Quaternion C = Quaternion.Inverse(A) * B;
  256. return C;
  257. }
  258. //Add rotation B to rotation A.
  259. public static Quaternion AddRotation(Quaternion A, Quaternion B)
  260. {
  261. Quaternion C = A * B;
  262. return C;
  263. }
  264. //Same as the build in TransformDirection(), but using a rotation instead of a transform.
  265. public static Vector3 TransformDirectionMath(Quaternion rotation, Vector3 vector)
  266. {
  267. Vector3 output = rotation * vector;
  268. return output;
  269. }
  270. //Same as the build in InverseTransformDirection(), but using a rotation instead of a transform.
  271. public static Vector3 InverseTransformDirectionMath(Quaternion rotation, Vector3 vector)
  272. {
  273. Vector3 output = Quaternion.Inverse(rotation) * vector;
  274. return output;
  275. }
  276. //Rotate a vector as if it is attached to an object with rotation "from", which is then rotated to rotation "to".
  277. //Similar to TransformWithParent(), but rotating a vector instead of a transform.
  278. public static Vector3 RotateVectorFromTo(Quaternion from, Quaternion to, Vector3 vector)
  279. {
  280. //Note: comments are in case all inputs are in World Space.
  281. Quaternion Q = SubtractRotation(to, from); //Output is in object space.
  282. Vector3 A = InverseTransformDirectionMath(from, vector);//Output is in object space.
  283. Vector3 B = Q * A; //Output is in local space.
  284. Vector3 C = TransformDirectionMath(from, B); //Output is in world space.
  285. return C;
  286. }
  287. //Find the line of intersection between two planes. The planes are defined by a normal and a point on that plane.
  288. //The outputs are a point on the line and a vector which indicates it's direction. If the planes are not parallel,
  289. //the function outputs true, otherwise false.
  290. public static bool PlanePlaneIntersection(out Vector3 linePoint, out Vector3 lineVec, Vector3 plane1Normal, Vector3 plane1Position, Vector3 plane2Normal, Vector3 plane2Position)
  291. {
  292. linePoint = Vector3.zero;
  293. lineVec = Vector3.zero;
  294. //We can get the direction of the line of intersection of the two planes by calculating the
  295. //cross product of the normals of the two planes. Note that this is just a direction and the line
  296. //is not fixed in space yet. We need a point for that to go with the line vector.
  297. lineVec = Vector3.Cross(plane1Normal, plane2Normal);
  298. //Next is to calculate a point on the line to fix it's position in space. This is done by finding a vector from
  299. //the plane2 location, moving parallel to it's plane, and intersecting plane1. To prevent rounding
  300. //errors, this vector also has to be perpendicular to lineDirection. To get this vector, calculate
  301. //the cross product of the normal of plane2 and the lineDirection.
  302. Vector3 ldir = Vector3.Cross(plane2Normal, lineVec);
  303. float denominator = Vector3.Dot(plane1Normal, ldir);
  304. //Prevent divide by zero and rounding errors by requiring about 5 degrees angle between the planes.
  305. if (Mathf.Abs(denominator) > 0.006f)
  306. {
  307. Vector3 plane1ToPlane2 = plane1Position - plane2Position;
  308. float t = Vector3.Dot(plane1Normal, plane1ToPlane2) / denominator;
  309. linePoint = plane2Position + t * ldir;
  310. return true;
  311. }
  312. //output not valid
  313. else
  314. {
  315. return false;
  316. }
  317. }
  318. //Get the intersection between a line and a plane.
  319. //If the line and plane are not parallel, the function outputs true, otherwise false.
  320. public static bool LinePlaneIntersection(out Vector3 intersection, Vector3 linePoint, Vector3 lineVec, Vector3 planeNormal, Vector3 planePoint)
  321. {
  322. float length;
  323. float dotNumerator;
  324. float dotDenominator;
  325. Vector3 vector;
  326. intersection = Vector3.zero;
  327. //calculate the distance between the linePoint and the line-plane intersection point
  328. dotNumerator = Vector3.Dot((planePoint - linePoint), planeNormal);
  329. dotDenominator = Vector3.Dot(lineVec, planeNormal);
  330. //line and plane are not parallel
  331. if (dotDenominator != 0.0f)
  332. {
  333. length = dotNumerator / dotDenominator;
  334. //create a vector from the linePoint to the intersection point
  335. vector = SetVectorLength(lineVec, length);
  336. //get the coordinates of the line-plane intersection point
  337. intersection = linePoint + vector;
  338. return true;
  339. }
  340. //output not valid
  341. else
  342. {
  343. return false;
  344. }
  345. }
  346. //Calculate the intersection point of two lines. Returns true if lines intersect, otherwise false.
  347. //Note that in 3d, two lines do not intersect most of the time. So if the two lines are not in the
  348. //same plane, use ClosestPointsOnTwoLines() instead.
  349. public static bool LineLineIntersection(out Vector3 intersection, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2)
  350. {
  351. Vector3 lineVec3 = linePoint2 - linePoint1;
  352. Vector3 crossVec1and2 = Vector3.Cross(lineVec1, lineVec2);
  353. Vector3 crossVec3and2 = Vector3.Cross(lineVec3, lineVec2);
  354. float planarFactor = Vector3.Dot(lineVec3, crossVec1and2);
  355. //is coplanar, and not parrallel
  356. if (Mathf.Abs(planarFactor) < 0.0001f && crossVec1and2.sqrMagnitude > 0.0001f)
  357. {
  358. float s = Vector3.Dot(crossVec3and2, crossVec1and2) / crossVec1and2.sqrMagnitude;
  359. intersection = linePoint1 + (lineVec1 * s);
  360. return true;
  361. }
  362. else
  363. {
  364. intersection = Vector3.zero;
  365. return false;
  366. }
  367. }
  368. //Two non-parallel lines which may or may not touch each other have a point on each line which are closest
  369. //to each other. This function finds those two points. If the lines are not parallel, the function
  370. //outputs true, otherwise false.
  371. public static bool ClosestPointsOnTwoLines(out Vector3 closestPointLine1, out Vector3 closestPointLine2, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2)
  372. {
  373. closestPointLine1 = Vector3.zero;
  374. closestPointLine2 = Vector3.zero;
  375. float a = Vector3.Dot(lineVec1, lineVec1);
  376. float b = Vector3.Dot(lineVec1, lineVec2);
  377. float e = Vector3.Dot(lineVec2, lineVec2);
  378. float d = a * e - b * b;
  379. //lines are not parallel
  380. if (d != 0.0f)
  381. {
  382. Vector3 r = linePoint1 - linePoint2;
  383. float c = Vector3.Dot(lineVec1, r);
  384. float f = Vector3.Dot(lineVec2, r);
  385. float s = (b * f - c * e) / d;
  386. float t = (a * f - c * b) / d;
  387. closestPointLine1 = linePoint1 + lineVec1 * s;
  388. closestPointLine2 = linePoint2 + lineVec2 * t;
  389. return true;
  390. }
  391. else
  392. {
  393. return false;
  394. }
  395. }
  396. //This function returns a point which is a projection from a point to a line.
  397. //The line is regarded infinite. If the line is finite, use ProjectPointOnLineSegment() instead.
  398. public static Vector3 ProjectPointOnLine(Vector3 linePoint, Vector3 lineVec, Vector3 point)
  399. {
  400. //get vector from point on line to point in space
  401. Vector3 linePointToPoint = point - linePoint;
  402. float t = Vector3.Dot(linePointToPoint, lineVec);
  403. return linePoint + lineVec * t;
  404. }
  405. //This function returns a point which is a projection from a point to a line segment.
  406. //If the projected point lies outside of the line segment, the projected point will
  407. //be clamped to the appropriate line edge.
  408. //If the line is infinite instead of a segment, use ProjectPointOnLine() instead.
  409. public static Vector3 ProjectPointOnLineSegment(Vector3 linePoint1, Vector3 linePoint2, Vector3 point)
  410. {
  411. Vector3 vector = linePoint2 - linePoint1;
  412. Vector3 projectedPoint = ProjectPointOnLine(linePoint1, vector.normalized, point);
  413. int side = PointOnWhichSideOfLineSegment(linePoint1, linePoint2, projectedPoint);
  414. //The projected point is on the line segment
  415. if (side == 0)
  416. {
  417. return projectedPoint;
  418. }
  419. if (side == 1)
  420. {
  421. return linePoint1;
  422. }
  423. if (side == 2)
  424. {
  425. return linePoint2;
  426. }
  427. //output is invalid
  428. return Vector3.zero;
  429. }
  430. //This function returns a point which is a projection from a point to a plane.
  431. public static Vector3 ProjectPointOnPlane(Vector3 planeNormal, Vector3 planePoint, Vector3 point)
  432. {
  433. float distance;
  434. Vector3 translationVector;
  435. //First calculate the distance from the point to the plane:
  436. distance = SignedDistancePlanePoint(planeNormal, planePoint, point);
  437. //Reverse the sign of the distance
  438. distance *= -1;
  439. //Get a translation vector
  440. translationVector = SetVectorLength(planeNormal, distance);
  441. //Translate the point to form a projection
  442. return point + translationVector;
  443. }
  444. //Projects a vector onto a plane. The output is not normalized.
  445. public static Vector3 ProjectVectorOnPlane(Vector3 planeNormal, Vector3 vector)
  446. {
  447. return vector - (Vector3.Dot(vector, planeNormal) * planeNormal);
  448. }
  449. //Get the shortest distance between a point and a plane. The output is signed so it holds information
  450. //as to which side of the plane normal the point is.
  451. public static float SignedDistancePlanePoint(Vector3 planeNormal, Vector3 planePoint, Vector3 point)
  452. {
  453. return Vector3.Dot(planeNormal, (point - planePoint));
  454. }
  455. //This function calculates a signed (+ or - sign instead of being ambiguous) dot product. It is basically used
  456. //to figure out whether a vector is positioned to the left or right of another vector. The way this is done is
  457. //by calculating a vector perpendicular to one of the vectors and using that as a reference. This is because
  458. //the result of a dot product only has signed information when an angle is transitioning between more or less
  459. //than 90 degrees.
  460. public static float SignedDotProduct(Vector3 vectorA, Vector3 vectorB, Vector3 normal)
  461. {
  462. Vector3 perpVector;
  463. float dot;
  464. //Use the geometry object normal and one of the input vectors to calculate the perpendicular vector
  465. perpVector = Vector3.Cross(normal, vectorA);
  466. //Now calculate the dot product between the perpendicular vector (perpVector) and the other input vector
  467. dot = Vector3.Dot(perpVector, vectorB);
  468. return dot;
  469. }
  470. public static float SignedVectorAngle(Vector3 referenceVector, Vector3 otherVector, Vector3 normal)
  471. {
  472. Vector3 perpVector;
  473. float angle;
  474. //Use the geometry object normal and one of the input vectors to calculate the perpendicular vector
  475. perpVector = Vector3.Cross(normal, referenceVector);
  476. //Now calculate the dot product between the perpendicular vector (perpVector) and the other input vector
  477. angle = Vector3.Angle(referenceVector, otherVector);
  478. angle *= Mathf.Sign(Vector3.Dot(perpVector, otherVector));
  479. return angle;
  480. }
  481. //Calculate the angle between a vector and a plane. The plane is made by a normal vector.
  482. //Output is in radians.
  483. public static float AngleVectorPlane(Vector3 vector, Vector3 normal)
  484. {
  485. float dot;
  486. float angle;
  487. //calculate the the dot product between the two input vectors. This gives the cosine between the two vectors
  488. dot = Vector3.Dot(vector, normal);
  489. //this is in radians
  490. angle = (float)Math.Acos(dot);
  491. return 1.570796326794897f - angle; //90 degrees - angle
  492. }
  493. //Calculate the dot product as an angle
  494. public static float DotProductAngle(Vector3 vec1, Vector3 vec2)
  495. {
  496. double dot;
  497. double angle;
  498. //get the dot product
  499. dot = Vector3.Dot(vec1, vec2);
  500. //Clamp to prevent NaN error. Shouldn't need this in the first place, but there could be a rounding error issue.
  501. if (dot < -1.0f)
  502. {
  503. dot = -1.0f;
  504. }
  505. if (dot > 1.0f)
  506. {
  507. dot = 1.0f;
  508. }
  509. //Calculate the angle. The output is in radians
  510. //This step can be skipped for optimization...
  511. angle = Math.Acos(dot);
  512. return (float)angle;
  513. }
  514. //Convert a plane defined by 3 points to a plane defined by a vector and a point.
  515. //The plane point is the middle of the triangle defined by the 3 points.
  516. public static void PlaneFrom3Points(out Vector3 planeNormal, out Vector3 planePoint, Vector3 pointA, Vector3 pointB, Vector3 pointC)
  517. {
  518. planeNormal = Vector3.zero;
  519. planePoint = Vector3.zero;
  520. //Make two vectors from the 3 input points, originating from point A
  521. Vector3 AB = pointB - pointA;
  522. Vector3 AC = pointC - pointA;
  523. //Calculate the normal
  524. planeNormal = Vector3.Normalize(Vector3.Cross(AB, AC));
  525. //Get the points in the middle AB and AC
  526. Vector3 middleAB = pointA + (AB / 2.0f);
  527. Vector3 middleAC = pointA + (AC / 2.0f);
  528. //Get vectors from the middle of AB and AC to the point which is not on that line.
  529. Vector3 middleABtoC = pointC - middleAB;
  530. Vector3 middleACtoB = pointB - middleAC;
  531. //Calculate the intersection between the two lines. This will be the center
  532. //of the triangle defined by the 3 points.
  533. //We could use LineLineIntersection instead of ClosestPointsOnTwoLines but due to rounding errors
  534. //this sometimes doesn't work.
  535. Vector3 temp;
  536. ClosestPointsOnTwoLines(out planePoint, out temp, middleAB, middleABtoC, middleAC, middleACtoB);
  537. }
  538. //Returns the forward vector of a quaternion
  539. public static Vector3 GetForwardVector(Quaternion q)
  540. {
  541. return q * Vector3.forward;
  542. }
  543. //Returns the up vector of a quaternion
  544. public static Vector3 GetUpVector(Quaternion q)
  545. {
  546. return q * Vector3.up;
  547. }
  548. //Returns the right vector of a quaternion
  549. public static Vector3 GetRightVector(Quaternion q)
  550. {
  551. return q * Vector3.right;
  552. }
  553. //Gets a quaternion from a matrix
  554. public static Quaternion QuaternionFromMatrix(Matrix4x4 m)
  555. {
  556. return Quaternion.LookRotation(m.GetColumn(2), m.GetColumn(1));
  557. }
  558. //Gets a position from a matrix
  559. public static Vector3 PositionFromMatrix(Matrix4x4 m)
  560. {
  561. Vector4 vector4Position = m.GetColumn(3);
  562. return new Vector3(vector4Position.x, vector4Position.y, vector4Position.z);
  563. }
  564. //This is an alternative for Quaternion.LookRotation. Instead of aligning the forward and up vector of the game
  565. //object with the input vectors, a custom direction can be used instead of the fixed forward and up vectors.
  566. //alignWithVector and alignWithNormal are in world space.
  567. //customForward and customUp are in object space.
  568. //Usage: use alignWithVector and alignWithNormal as if you are using the default LookRotation function.
  569. //Set customForward and customUp to the vectors you wish to use instead of the default forward and up vectors.
  570. public static void LookRotationExtended(ref GameObject gameObjectInOut, Vector3 alignWithVector, Vector3 alignWithNormal, Vector3 customForward, Vector3 customUp)
  571. {
  572. //Set the rotation of the destination
  573. Quaternion rotationA = Quaternion.LookRotation(alignWithVector, alignWithNormal);
  574. //Set the rotation of the custom normal and up vectors.
  575. //When using the default LookRotation function, this would be hard coded to the forward and up vector.
  576. Quaternion rotationB = Quaternion.LookRotation(customForward, customUp);
  577. //Calculate the rotation
  578. gameObjectInOut.transform.rotation = rotationA * Quaternion.Inverse(rotationB);
  579. }
  580. //This function transforms one object as if it was parented to the other.
  581. //Before using this function, the Init() function must be called
  582. //Input: parentRotation and parentPosition: the current parent transform.
  583. //Input: startParentRotation and startParentPosition: the transform of the parent object at the time the objects are parented.
  584. //Input: startChildRotation and startChildPosition: the transform of the child object at the time the objects are parented.
  585. //Output: childRotation and childPosition.
  586. //All transforms are in world space.
  587. public static void TransformWithParent(out Quaternion childRotation, out Vector3 childPosition, Quaternion parentRotation, Vector3 parentPosition, Quaternion startParentRotation, Vector3 startParentPosition, Quaternion startChildRotation, Vector3 startChildPosition)
  588. {
  589. childRotation = Quaternion.identity;
  590. childPosition = Vector3.zero;
  591. //set the parent start transform
  592. tempParent.rotation = startParentRotation;
  593. tempParent.position = startParentPosition;
  594. tempParent.localScale = Vector3.one; //to prevent scale wandering
  595. //set the child start transform
  596. tempChild.rotation = startChildRotation;
  597. tempChild.position = startChildPosition;
  598. tempChild.localScale = Vector3.one; //to prevent scale wandering
  599. //translate and rotate the child by moving the parent
  600. tempParent.rotation = parentRotation;
  601. tempParent.position = parentPosition;
  602. //get the child transform
  603. childRotation = tempChild.rotation;
  604. childPosition = tempChild.position;
  605. }
  606. //With this function you can align a triangle of an object with any transform.
  607. //Usage: gameObjectInOut is the game object you want to transform.
  608. //alignWithVector, alignWithNormal, and alignWithPosition is the transform with which the triangle of the object should be aligned with.
  609. //triangleForward, triangleNormal, and trianglePosition is the transform of the triangle from the object.
  610. //alignWithVector, alignWithNormal, and alignWithPosition are in world space.
  611. //triangleForward, triangleNormal, and trianglePosition are in object space.
  612. //trianglePosition is the mesh position of the triangle. The effect of the scale of the object is handled automatically.
  613. //trianglePosition can be set at any position, it does not have to be at a vertex or in the middle of the triangle.
  614. public static void PreciseAlign(ref GameObject gameObjectInOut, Vector3 alignWithVector, Vector3 alignWithNormal, Vector3 alignWithPosition, Vector3 triangleForward, Vector3 triangleNormal, Vector3 trianglePosition)
  615. {
  616. //Set the rotation.
  617. LookRotationExtended(ref gameObjectInOut, alignWithVector, alignWithNormal, triangleForward, triangleNormal);
  618. //Get the world space position of trianglePosition
  619. Vector3 trianglePositionWorld = gameObjectInOut.transform.TransformPoint(trianglePosition);
  620. //Get a vector from trianglePosition to alignWithPosition
  621. Vector3 translateVector = alignWithPosition - trianglePositionWorld;
  622. //Now transform the object so the triangle lines up correctly.
  623. gameObjectInOut.transform.Translate(translateVector, Space.World);
  624. }
  625. //Convert a position, direction, and normal vector to a transform
  626. public static void VectorsToTransform(ref GameObject gameObjectInOut, Vector3 positionVector, Vector3 directionVector, Vector3 normalVector)
  627. {
  628. gameObjectInOut.transform.position = positionVector;
  629. gameObjectInOut.transform.rotation = Quaternion.LookRotation(directionVector, normalVector);
  630. }
  631. //This function finds out on which side of a line segment the point is located.
  632. //The point is assumed to be on a line created by linePoint1 and linePoint2. If the point is not on
  633. //the line segment, project it on the line using ProjectPointOnLine() first.
  634. //Returns 0 if point is on the line segment.
  635. //Returns 1 if point is outside of the line segment and located on the side of linePoint1.
  636. //Returns 2 if point is outside of the line segment and located on the side of linePoint2.
  637. public static int PointOnWhichSideOfLineSegment(Vector3 linePoint1, Vector3 linePoint2, Vector3 point)
  638. {
  639. Vector3 lineVec = linePoint2 - linePoint1;
  640. Vector3 pointVec = point - linePoint1;
  641. float dot = Vector3.Dot(pointVec, lineVec);
  642. //point is on side of linePoint2, compared to linePoint1
  643. if (dot > 0)
  644. {
  645. //point is on the line segment
  646. if (pointVec.magnitude <= lineVec.magnitude)
  647. {
  648. return 0;
  649. }
  650. //point is not on the line segment and it is on the side of linePoint2
  651. else
  652. {
  653. return 2;
  654. }
  655. }
  656. //Point is not on side of linePoint2, compared to linePoint1.
  657. //Point is not on the line segment and it is on the side of linePoint1.
  658. else
  659. {
  660. return 1;
  661. }
  662. }
  663. //Returns the pixel distance from the mouse pointer to a line.
  664. //Alternative for HandleUtility.DistanceToLine(). Works both in Editor mode and Play mode.
  665. //Do not call this function from OnGUI() as the mouse position will be wrong.
  666. public static float MouseDistanceToLine(Vector3 linePoint1, Vector3 linePoint2)
  667. {
  668. Camera currentCamera;
  669. Vector3 mousePosition;
  670. #if UNITY_EDITOR
  671. if (Camera.current != null)
  672. {
  673. currentCamera = Camera.current;
  674. }
  675. else
  676. {
  677. currentCamera = Camera.main;
  678. }
  679. //convert format because y is flipped
  680. mousePosition = new Vector3(Event.current.mousePosition.x, currentCamera.pixelHeight - Event.current.mousePosition.y, 0f);
  681. #else
  682. currentCamera = Camera.main;
  683. mousePosition = Input.mousePosition;
  684. #endif
  685. Vector3 screenPos1 = currentCamera.WorldToScreenPoint(linePoint1);
  686. Vector3 screenPos2 = currentCamera.WorldToScreenPoint(linePoint2);
  687. Vector3 projectedPoint = ProjectPointOnLineSegment(screenPos1, screenPos2, mousePosition);
  688. //set z to zero
  689. projectedPoint = new Vector3(projectedPoint.x, projectedPoint.y, 0f);
  690. Vector3 vector = projectedPoint - mousePosition;
  691. return vector.magnitude;
  692. }
  693. //Returns the pixel distance from the mouse pointer to a camera facing circle.
  694. //Alternative for HandleUtility.DistanceToCircle(). Works both in Editor mode and Play mode.
  695. //Do not call this function from OnGUI() as the mouse position will be wrong.
  696. //If you want the distance to a point instead of a circle, set the radius to 0.
  697. public static float MouseDistanceToCircle(Vector3 point, float radius)
  698. {
  699. Camera currentCamera;
  700. Vector3 mousePosition;
  701. #if UNITY_EDITOR
  702. if (Camera.current != null)
  703. {
  704. currentCamera = Camera.current;
  705. }
  706. else
  707. {
  708. currentCamera = Camera.main;
  709. }
  710. //convert format because y is flipped
  711. mousePosition = new Vector3(Event.current.mousePosition.x, currentCamera.pixelHeight - Event.current.mousePosition.y, 0f);
  712. #else
  713. currentCamera = Camera.main;
  714. mousePosition = Input.mousePosition;
  715. #endif
  716. Vector3 screenPos = currentCamera.WorldToScreenPoint(point);
  717. //set z to zero
  718. screenPos = new Vector3(screenPos.x, screenPos.y, 0f);
  719. Vector3 vector = screenPos - mousePosition;
  720. float fullDistance = vector.magnitude;
  721. float circleDistance = fullDistance - radius;
  722. return circleDistance;
  723. }
  724. //Returns true if a line segment (made up of linePoint1 and linePoint2) is fully or partially in a rectangle
  725. //made up of RectA to RectD. The line segment is assumed to be on the same plane as the rectangle. If the line is
  726. //not on the plane, use ProjectPointOnPlane() on linePoint1 and linePoint2 first.
  727. public static bool IsLineInRectangle(Vector3 linePoint1, Vector3 linePoint2, Vector3 rectA, Vector3 rectB, Vector3 rectC, Vector3 rectD)
  728. {
  729. bool pointAInside = false;
  730. bool pointBInside = false;
  731. pointAInside = IsPointInRectangle(linePoint1, rectA, rectC, rectB, rectD);
  732. if (!pointAInside)
  733. {
  734. pointBInside = IsPointInRectangle(linePoint2, rectA, rectC, rectB, rectD);
  735. }
  736. //none of the points are inside, so check if a line is crossing
  737. if (!pointAInside && !pointBInside)
  738. {
  739. bool lineACrossing = AreLineSegmentsCrossing(linePoint1, linePoint2, rectA, rectB);
  740. bool lineBCrossing = AreLineSegmentsCrossing(linePoint1, linePoint2, rectB, rectC);
  741. bool lineCCrossing = AreLineSegmentsCrossing(linePoint1, linePoint2, rectC, rectD);
  742. bool lineDCrossing = AreLineSegmentsCrossing(linePoint1, linePoint2, rectD, rectA);
  743. if (lineACrossing || lineBCrossing || lineCCrossing || lineDCrossing)
  744. {
  745. return true;
  746. }
  747. else
  748. {
  749. return false;
  750. }
  751. }
  752. else
  753. {
  754. return true;
  755. }
  756. }
  757. //Returns true if "point" is in a rectangle mad up of RectA to RectD. The line point is assumed to be on the same
  758. //plane as the rectangle. If the point is not on the plane, use ProjectPointOnPlane() first.
  759. public static bool IsPointInRectangle(Vector3 point, Vector3 rectA, Vector3 rectC, Vector3 rectB, Vector3 rectD)
  760. {
  761. Vector3 vector;
  762. Vector3 linePoint;
  763. //get the center of the rectangle
  764. vector = rectC - rectA;
  765. float size = -(vector.magnitude / 2f);
  766. vector = AddVectorLength(vector, size);
  767. Vector3 middle = rectA + vector;
  768. Vector3 xVector = rectB - rectA;
  769. float width = xVector.magnitude / 2f;
  770. Vector3 yVector = rectD - rectA;
  771. float height = yVector.magnitude / 2f;
  772. linePoint = ProjectPointOnLine(middle, xVector.normalized, point);
  773. vector = linePoint - point;
  774. float yDistance = vector.magnitude;
  775. linePoint = ProjectPointOnLine(middle, yVector.normalized, point);
  776. vector = linePoint - point;
  777. float xDistance = vector.magnitude;
  778. if ((xDistance <= width) && (yDistance <= height))
  779. {
  780. return true;
  781. }
  782. else
  783. {
  784. return false;
  785. }
  786. }
  787. //Returns true if line segment made up of pointA1 and pointA2 is crossing line segment made up of
  788. //pointB1 and pointB2. The two lines are assumed to be in the same plane.
  789. public static bool AreLineSegmentsCrossing(Vector3 pointA1, Vector3 pointA2, Vector3 pointB1, Vector3 pointB2)
  790. {
  791. Vector3 closestPointA;
  792. Vector3 closestPointB;
  793. int sideA;
  794. int sideB;
  795. Vector3 lineVecA = pointA2 - pointA1;
  796. Vector3 lineVecB = pointB2 - pointB1;
  797. bool valid = ClosestPointsOnTwoLines(out closestPointA, out closestPointB, pointA1, lineVecA.normalized, pointB1, lineVecB.normalized);
  798. //lines are not parallel
  799. if (valid)
  800. {
  801. sideA = PointOnWhichSideOfLineSegment(pointA1, pointA2, closestPointA);
  802. sideB = PointOnWhichSideOfLineSegment(pointB1, pointB2, closestPointB);
  803. if ((sideA == 0) && (sideB == 0))
  804. {
  805. return true;
  806. }
  807. else
  808. {
  809. return false;
  810. }
  811. }
  812. //lines are parallel
  813. else
  814. {
  815. return false;
  816. }
  817. }
  818. //This function calculates the acceleration vector in meter/second^2.
  819. //Input: position. If the output is used for motion simulation, the input transform
  820. //has to be located at the seat base, not at the vehicle CG. Attach an empty GameObject
  821. //at the correct location and use that as the input for this function.
  822. //Gravity is not taken into account but this can be added to the output if needed.
  823. //A low number of samples can give a jittery result due to rounding errors.
  824. //If more samples are used, the output is more smooth but has a higher latency.
  825. public static bool LinearAcceleration(out Vector3 vector, Vector3 position, int samples)
  826. {
  827. Vector3 averageSpeedChange = Vector3.zero;
  828. vector = Vector3.zero;
  829. Vector3 deltaDistance;
  830. float deltaTime;
  831. Vector3 speedA;
  832. Vector3 speedB;
  833. //Clamp sample amount. In order to calculate acceleration we need at least 2 changes
  834. //in speed, so we need at least 3 position samples.
  835. if (samples < 3)
  836. {
  837. samples = 3;
  838. }
  839. //Initialize
  840. if (positionRegister == null)
  841. {
  842. positionRegister = new Vector3[samples];
  843. posTimeRegister = new float[samples];
  844. }
  845. //Fill the position and time sample array and shift the location in the array to the left
  846. //each time a new sample is taken. This way index 0 will always hold the oldest sample and the
  847. //highest index will always hold the newest sample.
  848. for (int i = 0; i < positionRegister.Length - 1; i++)
  849. {
  850. positionRegister[i] = positionRegister[i + 1];
  851. posTimeRegister[i] = posTimeRegister[i + 1];
  852. }
  853. positionRegister[positionRegister.Length - 1] = position;
  854. posTimeRegister[posTimeRegister.Length - 1] = Time.time;
  855. positionSamplesTaken++;
  856. //The output acceleration can only be calculated if enough samples are taken.
  857. if (positionSamplesTaken >= samples)
  858. {
  859. //Calculate average speed change.
  860. for (int i = 0; i < positionRegister.Length - 2; i++)
  861. {
  862. deltaDistance = positionRegister[i + 1] - positionRegister[i];
  863. deltaTime = posTimeRegister[i + 1] - posTimeRegister[i];
  864. //If deltaTime is 0, the output is invalid.
  865. if (deltaTime == 0)
  866. {
  867. return false;
  868. }
  869. speedA = deltaDistance / deltaTime;
  870. deltaDistance = positionRegister[i + 2] - positionRegister[i + 1];
  871. deltaTime = posTimeRegister[i + 2] - posTimeRegister[i + 1];
  872. if (deltaTime == 0)
  873. {
  874. return false;
  875. }
  876. speedB = deltaDistance / deltaTime;
  877. //This is the accumulated speed change at this stage, not the average yet.
  878. averageSpeedChange += speedB - speedA;
  879. }
  880. //Now this is the average speed change.
  881. averageSpeedChange /= positionRegister.Length - 2;
  882. //Get the total time difference.
  883. float deltaTimeTotal = posTimeRegister[posTimeRegister.Length - 1] - posTimeRegister[0];
  884. //Now calculate the acceleration, which is an average over the amount of samples taken.
  885. vector = averageSpeedChange / deltaTimeTotal;
  886. return true;
  887. }
  888. else
  889. {
  890. return false;
  891. }
  892. }
  893. /*
  894. //This function calculates angular acceleration in object space as deg/second^2, encoded as a vector.
  895. //For example, if the output vector is 0,0,-5, the angular acceleration is 5 deg/second^2 around the object Z axis, to the left.
  896. //Input: rotation (quaternion). If the output is used for motion simulation, the input transform
  897. //has to be located at the seat base, not at the vehicle CG. Attach an empty GameObject
  898. //at the correct location and use that as the input for this function.
  899. //A low number of samples can give a jittery result due to rounding errors.
  900. //If more samples are used, the output is more smooth but has a higher latency.
  901. //Note: the result is only accurate if the rotational difference between two samples is less than 180 degrees.
  902. //Note: a suitable way to visualize the result is:
  903. Vector3 dir;
  904. float scale = 2f;
  905. dir = new Vector3(vector.x, 0, 0);
  906. dir = Math3d.SetVectorLength(dir, dir.magnitude * scale);
  907. dir = gameObject.transform.TransformDirection(dir);
  908. Debug.DrawRay(gameObject.transform.position, dir, Color.red);
  909. dir = new Vector3(0, vector.y, 0);
  910. dir = Math3d.SetVectorLength(dir, dir.magnitude * scale);
  911. dir = gameObject.transform.TransformDirection(dir);
  912. Debug.DrawRay(gameObject.transform.position, dir, Color.green);
  913. dir = new Vector3(0, 0, vector.z);
  914. dir = Math3d.SetVectorLength(dir, dir.magnitude * scale);
  915. dir = gameObject.transform.TransformDirection(dir);
  916. Debug.DrawRay(gameObject.transform.position, dir, Color.blue); */
  917. public static bool AngularAcceleration(out Vector3 vector, Quaternion rotation, int samples)
  918. {
  919. Vector3 averageSpeedChange = Vector3.zero;
  920. vector = Vector3.zero;
  921. Quaternion deltaRotation;
  922. float deltaTime;
  923. Vector3 speedA;
  924. Vector3 speedB;
  925. //Clamp sample amount. In order to calculate acceleration we need at least 2 changes
  926. //in speed, so we need at least 3 rotation samples.
  927. if (samples < 3)
  928. {
  929. samples = 3;
  930. }
  931. //Initialize
  932. if (rotationRegister == null)
  933. {
  934. rotationRegister = new Quaternion[samples];
  935. rotTimeRegister = new float[samples];
  936. }
  937. //Fill the rotation and time sample array and shift the location in the array to the left
  938. //each time a new sample is taken. This way index 0 will always hold the oldest sample and the
  939. //highest index will always hold the newest sample.
  940. for (int i = 0; i < rotationRegister.Length - 1; i++)
  941. {
  942. rotationRegister[i] = rotationRegister[i + 1];
  943. rotTimeRegister[i] = rotTimeRegister[i + 1];
  944. }
  945. rotationRegister[rotationRegister.Length - 1] = rotation;
  946. rotTimeRegister[rotTimeRegister.Length - 1] = Time.time;
  947. rotationSamplesTaken++;
  948. //The output acceleration can only be calculated if enough samples are taken.
  949. if (rotationSamplesTaken >= samples)
  950. {
  951. //Calculate average speed change.
  952. for (int i = 0; i < rotationRegister.Length - 2; i++)
  953. {
  954. deltaRotation = SubtractRotation(rotationRegister[i + 1], rotationRegister[i]);
  955. deltaTime = rotTimeRegister[i + 1] - rotTimeRegister[i];
  956. //If deltaTime is 0, the output is invalid.
  957. if (deltaTime == 0)
  958. {
  959. return false;
  960. }
  961. speedA = RotDiffToSpeedVec(deltaRotation, deltaTime);
  962. deltaRotation = SubtractRotation(rotationRegister[i + 2], rotationRegister[i + 1]);
  963. deltaTime = rotTimeRegister[i + 2] - rotTimeRegister[i + 1];
  964. if (deltaTime == 0)
  965. {
  966. return false;
  967. }
  968. speedB = RotDiffToSpeedVec(deltaRotation, deltaTime);
  969. //This is the accumulated speed change at this stage, not the average yet.
  970. averageSpeedChange += speedB - speedA;
  971. }
  972. //Now this is the average speed change.
  973. averageSpeedChange /= rotationRegister.Length - 2;
  974. //Get the total time difference.
  975. float deltaTimeTotal = rotTimeRegister[rotTimeRegister.Length - 1] - rotTimeRegister[0];
  976. //Now calculate the acceleration, which is an average over the amount of samples taken.
  977. vector = averageSpeedChange / deltaTimeTotal;
  978. return true;
  979. }
  980. else
  981. {
  982. return false;
  983. }
  984. }
  985. //Get y from a linear function, with x as an input. The linear function goes through points
  986. //0,0 on the left ,and Qxy on the right.
  987. public static float LinearFunction2DBasic(float x, float Qx, float Qy)
  988. {
  989. float y = x * (Qy / Qx);
  990. return y;
  991. }
  992. //Get y from a linear function, with x as an input. The linear function goes through points
  993. //Pxy on the left ,and Qxy on the right.
  994. public static float LinearFunction2DFull(float x, float Px, float Py, float Qx, float Qy)
  995. {
  996. float y = 0f;
  997. float A = Qy - Py;
  998. float B = Qx - Px;
  999. float C = A / B;
  1000. y = Py + (C * (x - Px));
  1001. return y;
  1002. }
  1003. //Convert a rotation difference to a speed vector.
  1004. //For internal use only.
  1005. private static Vector3 RotDiffToSpeedVec(Quaternion rotation, float deltaTime)
  1006. {
  1007. float x;
  1008. float y;
  1009. float z;
  1010. if (rotation.eulerAngles.x <= 180.0f)
  1011. {
  1012. x = rotation.eulerAngles.x;
  1013. }
  1014. else
  1015. {
  1016. x = rotation.eulerAngles.x - 360.0f;
  1017. }
  1018. if (rotation.eulerAngles.y <= 180.0f)
  1019. {
  1020. y = rotation.eulerAngles.y;
  1021. }
  1022. else
  1023. {
  1024. y = rotation.eulerAngles.y - 360.0f;
  1025. }
  1026. if (rotation.eulerAngles.z <= 180.0f)
  1027. {
  1028. z = rotation.eulerAngles.z;
  1029. }
  1030. else
  1031. {
  1032. z = rotation.eulerAngles.z - 360.0f;
  1033. }
  1034. return new Vector3(x / deltaTime, y / deltaTime, z / deltaTime);
  1035. }
  1036. }