开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 图形图像 > 2D图形编程 > 查看源码
NewtonCustomJoints_Math.pas
资源名称：asphyre410.rar　[点击查看]
上传用户：ctlcnc
上传日期：2021-12-10
资源大小：4933k
文件大小：17k
源码类别：

2D图形编程

开发平台：

Delphi

NewtonCustomJoints_Math.pas：源码内容
  1. {*******************************************************************************}
  2. {                                                                               }
  3. {      Math helper unit for NewtonCustomJoints.pas                              }
  4. {                                                                               }
  5. {      Copyright (c) 2005,06 Sascha Willems                                     }
  6. {                                                                               }
  7. {*******************************************************************************}
  8. {                                                                               }
  9. { License :                                                                     }
  10. {                                                                               }
  11. {  The contents of this file are used with permission, subject to               }
  12. {  the Mozilla Public License Version 1.1 (the "License"); you may              }
  13. {  not use this file except in compliance with the License. You may             }
  14. {  obtain a copy of the License at                                              }
  15. {  http://www.mozilla.org/MPL/MPL-1.1.html                                      }
  16. {                                                                               }
  17. {  Software distributed under the License is distributed on an                  }
  18. {  "AS IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or               }
  19. {  implied. See the License for the specific language governing                 }
  20. {  rights and limitations under the License.                                    }
  21. {                                                                               }
  22. {*******************************************************************************}
  24. unit NewtonCustomJoints_Math;
  26. interface
  28. uses
  29.  Math;
  31. type
  32.  TMatrix4f = array[0..3, 0..3] of Single;
  33.  TMatrix4d = array[0..3, 0..3] of Double;
  34.  TVector3f = array[0..2] of Single;
  35.  TVector4f = array[0..3] of Single;
  37. var
  38.  NullMatrix4f   : TMatrix4f = ((0, 0, 0, 0), (0, 0, 0, 0), (0, 0, 0, 0), (0, 0, 0, 0));
  39.  IdentityMatrix : TMatrix4f = ((1, 0, 0, 0), (0, 1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1));
  40.  ZeroVector     : TVector3f = (0, 0, 0);
  42. const
  43.   X = 0;
  44.   Y = 1;
  45.   Z = 2;
  46.   W = 3;
  47.   Epsilon = 0.0001;
  49. function  Matrix_Multiply(m1 : TMatrix4f; m2 : TMatrix4f) : TMatrix4f;
  50. procedure Matrix_SetIdentity(var M : TMatrix4f);
  51. procedure Matrix_SetTransform(var M : TMatrix4f; V : TVector3f);
  52. procedure Matrix_SetRotation(var M : TMatrix4f; V : TVector3f);
  53. procedure Matrix_RotateVect(const M : TMatrix4f; var pVect : TVector3f);
  54. procedure Matrix_Inverse(var M: TMatrix4f);
  55. function  Matrix_TansformVector(const M : TMatrix4f; V : TVector3f) : TVector3f;
  56. function  Matrix_UntransformVector(const M : TMatrix4f; V : TVector3f) : TVector3f;
  57. function  Matrix_UnRotateVect(const M : TMatrix4f; pVect : TVector3f) : TVector3f;
  58. procedure Matrix_SetColumn(var M : TMatrix4f; pColumn : Byte;pVect : TVector4f);
  61. function V4(x,y,z : Single) : TVector4f; overload;
  62. function V4(x,y,z,w : Single) : TVector4f; overload;
  63. function V4(pV3 : TVector3f; w : Single) : TVector4f; overload;
  64. function V4(pMatrix : TMatrix4f; pC : Integer) : TVector4f; overload;
  65. function V3(x,y,z : Single) : TVector3f; overload;
  66. function V3(pMatrix : TMatrix4f; pC : Integer) : TVector3f; overload;
  67. function VCross(pV1, pV2 : TVector3f) : TVector3f; overload;
  68. function VCross(pV1, pV2 : TVector4f) : TVector4f; overload;
  69. function VDot(pV1, pV2 : TVector3f) : Single; overload;
  70. function VDot(pV1, pV2 : TVector4f) : Single; overload;
  71. function VTransform(pV1 : TVector3f; pM : TMatrix4f) : TVector3f;
  72. function VSub(pV1, pV2 : TVector3f) : TVector3f; overload;
  73. function VSub(pV1, pV2 : TVector4f) : TVector4f; overload;
  74. function VAdd(pV1, pV2 : TVector3f) : TVector3f; overload;
  75. function VAdd(pV1, pV2 : TVector4f) : TVector4f; overload;
  76. function VNormalize(pV : TVector3f) : TVector3f;
  77. function VScale(pV : TVector3f; pScale : Single) : TVector3f; overload;
  78. function VScale(pV : TVector4f; pScale : Single) : TVector4f; overload;
  79. function VDistance(pV1, pV2 : TVector3f) : Single;
  81. implementation
  83. // =============================================================================
  84. //  V4
  85. // =============================================================================
  86. function V4(x,y,z : Single) : TVector4f;
  87. begin
  88. Result[0] := x;
  89. Result[1] := y;
  90. Result[2] := z;
  91. Result[3] := 0;
  92. end;
  94. function V4(x,y,z,w : Single) : TVector4f;
  95. begin
  96. Result[0] := x;
  97. Result[1] := y;
  98. Result[2] := z;
  99. Result[3] := w;
  100. end;
  102. function V4(pV3 : TVector3f; w : Single) : TVector4f; overload;
  103. begin
  104. Result[0] := pV3[0];
  105. Result[1] := pV3[1];
  106. Result[2] := pV3[2];
  107. Result[3] := w;
  108. end;
  110. function V4(pMatrix : TMatrix4f; pC : Integer) : TVector4f; overload;
  111. begin
  112. Result[0] := pMatrix[pC, 0];
  113. Result[1] := pMatrix[pC, 1];
  114. Result[2] := pMatrix[pC, 2];
  115. Result[3] := pMatrix[pC, 3];
  116. end;
  119. // =============================================================================
  120. //  V3
  121. // =============================================================================
  122. function V3(x,y,z : Single) : TVector3f;
  123. begin
  124. Result[0] := x;
  125. Result[1] := y;
  126. Result[2] := z;
  127. end;
  129. function V3(pMatrix : TMatrix4f; pC : Integer) : TVector3f; overload;
  130. begin
  131. Result[0] := pMatrix[pC, 0];
  132. Result[1] := pMatrix[pC, 1];
  133. Result[2] := pMatrix[pC, 2];
  134. end;
  136. // =============================================================================
  137. //  VCross
  138. // =============================================================================
  139. function VCross(pV1, pV2 : TVector3f) : TVector3f;
  140. begin
  141. Result[0] := (pV1[1]*pV2[2]) - (pV1[2]*pV2[1]);
  142. Result[1] := (pV1[2]*pV2[0]) - (pV1[0]*pV2[2]);
  143. Result[2] := (pV1[0]*pV2[1]) - (pV1[1]*pV2[0]);
  144. end;
  146. function VCross(pV1, pV2 : TVector4f) : TVector4f;
  147. begin
  148. Result[0] := (pV1[1]*pV2[2]) - (pV1[2]*pV2[1]);
  149. Result[1] := (pV1[2]*pV2[0]) - (pV1[0]*pV2[2]);
  150. Result[2] := (pV1[0]*pV2[1]) - (pV1[1]*pV2[0]);
  151. Result[3] := 0;
  152. end;
  154. // =============================================================================
  155. //  VDot
  156. // =============================================================================
  157. function VDot(pV1, pV2 : TVector3f) : Single;
  158. begin
  159. Result := (pV1[0]*pV2[0]) + (pV1[1]*pV2[1]) + (pV1[2]*pV2[2]);
  160. end;
  162. function VDot(pV1, pV2 : TVector4f) : Single;
  163. begin
  164. Result := (pV1[0]*pV2[0]) + (pV1[1]*pV2[1]) + (pV1[2]*pV2[2]) + (pV1[3]*pV2[3]);
  165. end;
  167. // =============================================================================
  168. //  VTransform
  169. // =============================================================================
  170. function VTransform(pV1 : TVector3f; pM : TMatrix4f) : TVector3f;
  171. var
  172.  TV : TVector3f;
  173. begin
  174. TV[X] := pV1[X] * pM[X, X] + pV1[Y] * pM[Y, X] + pV1[Z] * pM[Z, X] + pM[W, X];
  175. TV[Y] := pV1[X] * pM[X, Y] + pV1[Y] * pM[Y, Y] + pV1[Z] * pM[Z, Y] + pM[W, Y];
  176. TV[Z] := pV1[X] * pM[X, Z] + pV1[Y] * pM[Y, Z] + pV1[Z] * pM[Z, Z] + pM[W, Z];
  177. Result := TV
  178. end;
  180. // =============================================================================
  181. //  VSub
  182. // =============================================================================
  183. function VSub(pV1, pV2 : TVector3f) : TVector3f;
  184. begin
  185. Result := V3(pV1[0]-pV2[0], pV1[1]-pV2[1], pV1[2]-pV2[2]);
  186. end;
  188. function VSub(pV1, pV2 : TVector4f) : TVector4f; overload;
  189. begin
  190. Result := V4(pV1[0]-pV2[0], pV1[1]-pV2[1], pV1[2]-pV2[2], pV1[3]-pV2[3]);
  191. end;
  193. // =============================================================================
  194. //  VAdd
  195. // =============================================================================
  196. function VAdd(pV1, pV2 : TVector3f) : TVector3f;
  197. begin
  198. Result := V3(pV1[0]+pV2[0], pV1[1]+pV2[1], pV1[2]+pV2[2]);
  199. end;
  201. function VAdd(pV1, pV2 : TVector4f) : TVector4f; overload;
  202. begin
  203. Result := V4(pV1[0]+pV2[0], pV1[1]+pV2[1], pV1[2]+pV2[2], pV1[3]+pV2[3]);
  204. end;
  207. // =============================================================================
  208. //  VNormalize
  209. // =============================================================================
  210. function VNormalize(pV : TVector3f) : TVector3f;
  211. var
  212.  l : Single;
  213. begin
  214. l := Sqrt(pV[0]*pV[0] + pV[1]*pV[1] + pV[2]*pV[2]);
  215. if l = 0 then
  216.  exit;
  217. Result[0] := pV[0]/l;
  218. Result[1] := pV[1]/l;
  219. Result[2] := pV[2]/l;
  220. end;
  222. // =============================================================================
  223. //  VScale
  224. // =============================================================================
  225. function VScale(pV : TVector3f; pScale : Single) : TVector3f;
  226. begin
  227. Result[0] := pV[0] * pScale;
  228. Result[1] := pV[1] * pScale;
  229. Result[2] := pV[2] * pScale;
  230. end;
  232. function VScale(pV : TVector4f; pScale : Single) : TVector4f; overload;
  233. begin
  234. Result[0] := pV[0] * pScale;
  235. Result[1] := pV[1] * pScale;
  236. Result[2] := pV[2] * pScale;
  237. Result[3] := pV[3] * pScale;
  238. end;
  240. // =============================================================================
  241. //  VDistance
  242. // =============================================================================
  243. function VDistance(pV1, pV2 : TVector3f) : Single;
  244. begin
  245. Result := Sqrt(Sqr(pV1[0]-pV2[0])+Sqr(pV1[1]-pV2[1])+Sqr(pV1[2]-pV2[2]));
  246. end;
  248. // =============================================================================
  249. //  Matrix_SetIdentity
  250. // =============================================================================
  251. procedure Matrix_SetIdentity(var M : TMatrix4f);
  252. begin
  253. M[0,0] := 1; M[1,0] := 0; M[2,0] := 0; M[3,0] := 0;
  254. M[0,1] := 0; M[1,1] := 1; M[2,1] := 0; M[3,1] := 0;
  255. M[0,2] := 0; M[1,2] := 0; M[2,2] := 1; M[3,2] := 0;
  256. M[0,3] := 0; M[1,3] := 0; M[2,3] := 0; M[3,3] := 1;
  257. end;
  259. // =============================================================================
  260. //  Matrix_SetTransform
  261. // =============================================================================
  262. procedure Matrix_SetTransform(var M : TMatrix4f; V : TVector3f);
  263. begin
  264. M[3][0] := V[0];
  265. M[3][1] := V[1];
  266. M[3][2] := V[2];
  267. end;
  269. // =============================================================================
  270. //  Matrix_SetRotation
  271. // =============================================================================
  272. procedure Matrix_SetRotation(var M : TMatrix4f; V : TVector3f);
  273. var
  274.  cr , sr , cp , sp , cy , sy , srsp , crsp : single;
  275. begin
  276. V[0] := DegToRad(V[0]);
  277. V[1] := DegToRad(V[1]);
  278. V[2] := DegToRad(V[2]);
  280. cr := cos(V[0]);
  281. sr := sin(V[0]);
  282. cp := cos(V[1]);
  283. sp := sin(V[1]);
  284. cy := cos(V[2]);
  285. sy := sin(V[2]);
  287. M[0,0] := cp*cy;
  288. M[1,0] := cp*sy;
  289. M[2,0] := -sp;
  291. srsp := sr*sp;
  292. crsp := cr*sp;
  294. M[0,1] := srsp*cy-cr*sy ;
  295. M[1,1] := srsp*sy+cr*cy ;
  296. M[2,1] := sr*cp ;
  297. M[0,2] := crsp*cy+sr*sy ;
  298. M[1,2] := crsp*sy-sr*cy ;
  299. M[2,2] := cr*cp ;
  300. end;
  302. // =============================================================================
  303. //  Matrix_RotateVect
  304. // =============================================================================
  305. procedure Matrix_RotateVect(const M : TMatrix4f; var pVect : TVector3f);
  306. var
  307.  vec : array [0..2] of single;
  308. begin
  309. vec[0] := pVect[0]*M[0,0] + pVect[1]*M[1,0] + pVect[2]*M[2,0];
  310. vec[1] := pVect[0]*M[0,1] + pVect[1]*M[1,1] + pVect[2]*M[2,1];
  311. vec[2] := pVect[0]*M[0,2] + pVect[1]*M[1,2] + pVect[2]*M[2,2];
  312. pVect[0] := vec[0];
  313. pVect[1] := vec[1];
  314. pVect[2] := vec[2];
  315. end;
  317. // =============================================================================
  318. //  Matrix_UnRotateVect
  319. // =============================================================================
  320. function Matrix_UnRotateVect(const M : TMatrix4f; pVect : TVector3f) : TVector3f;
  321. var
  322.  V : TVector3f;
  323. begin
  324. V := V3(VDot(V, V3(M[0,0], M[0,1], M[0,2])),
  325.         VDot(V, V3(M[1,0], M[1,1], M[1,2])),
  326.         VDot(V, V3(M[2,0], M[2,1], M[2,2])));
  327. Result := V;
  328. end;
  330. // =============================================================================
  331. //  Matrix_Multiply
  332. // =============================================================================
  333. function Matrix_Multiply(m1 : TMatrix4f; m2 : TMatrix4f) : TMatrix4f;
  334. var
  335.   r, c, i: Byte;
  336.   t: TMatrix4f;
  337. begin
  338. // Multiply two matrices.
  339. t := NullMatrix4f;
  340. for r := 0 to 3 do
  341.  for c := 0 to 3 do
  342.   for i := 0 to 3 do
  343.    t[r,c] := t[r,c] + (m1[r,i]*m2[i,c]);
  344. Result := t;
  345. end;
  347. // internal version for the determinant of a 3x3 matrix
  348. function MatrixDetInternal(a1, a2, a3, b1, b2, b3, c1, c2, c3: Single): Single;
  349. begin
  350. Result := a1 * (b2 * c3 - b3 * c2) -
  351.           b1 * (a2 * c3 - a3 * c2) +
  352.           c1 * (a2 * b3 - a3 * b2);
  353. end;
  355. // Determinant of a 4x4 matrix
  356. function MatrixDeterminant(M: TMatrix4f): Single; register;
  357. var a1, a2, a3, a4,
  358.     b1, b2, b3, b4,
  359.     c1, c2, c3, c4,
  360.     d1, d2, d3, d4  : Single;
  361. begin
  362.   a1 := M[X, X];  b1 := M[X, Y];  c1 := M[X, Z];  d1 := M[X, W];
  363.   a2 := M[Y, X];  b2 := M[Y, Y];  c2 := M[Y, Z];  d2 := M[Y, W];
  364.   a3 := M[Z, X];  b3 := M[Z, Y];  c3 := M[Z, Z];  d3 := M[Z, W];
  365.   a4 := M[W, X];  b4 := M[W, Y];  c4 := M[W, Z];  d4 := M[W, W];
  366.   Result := a1 * MatrixDetInternal(b2, b3, b4, c2, c3, c4, d2, d3, d4) -
  367.             b1 * MatrixDetInternal(a2, a3, a4, c2, c3, c4, d2, d3, d4) +
  368.             c1 * MatrixDetInternal(a2, a3, a4, b2, b3, b4, d2, d3, d4) -
  369.             d1 * MatrixDetInternal(a2, a3, a4, b2, b3, b4, c2, c3, c4);
  370. end;
  372. // Adjoint of a 4x4 matrix - used in the computation of the inverse
  373. // of a 4x4 matrix
  374. procedure MatrixAdjoint(var M: TMatrix4f); register;
  375. var a1, a2, a3, a4,
  376.     b1, b2, b3, b4,
  377.     c1, c2, c3, c4,
  378.     d1, d2, d3, d4: Single;
  379. begin
  380.     a1 :=  M[X, X]; b1 :=  M[X, Y];
  381.     c1 :=  M[X, Z]; d1 :=  M[X, W];
  382.     a2 :=  M[Y, X]; b2 :=  M[Y, Y];
  383.     c2 :=  M[Y, Z]; d2 :=  M[Y, W];
  384.     a3 :=  M[Z, X]; b3 :=  M[Z, Y];
  385.     c3 :=  M[Z, Z]; d3 :=  M[Z, W];
  386.     a4 :=  M[W, X]; b4 :=  M[W, Y];
  387.     c4 :=  M[W, Z]; d4 :=  M[W, W];
  388.     // row column labeling reversed since we transpose rows & columns
  389.     M[X, X] :=  MatrixDetInternal(b2, b3, b4, c2, c3, c4, d2, d3, d4);
  390.     M[Y, X] := -MatrixDetInternal(a2, a3, a4, c2, c3, c4, d2, d3, d4);
  391.     M[Z, X] :=  MatrixDetInternal(a2, a3, a4, b2, b3, b4, d2, d3, d4);
  392.     M[W, X] := -MatrixDetInternal(a2, a3, a4, b2, b3, b4, c2, c3, c4);
  393.     M[X, Y] := -MatrixDetInternal(b1, b3, b4, c1, c3, c4, d1, d3, d4);
  394.     M[Y, Y] :=  MatrixDetInternal(a1, a3, a4, c1, c3, c4, d1, d3, d4);
  395.     M[Z, Y] := -MatrixDetInternal(a1, a3, a4, b1, b3, b4, d1, d3, d4);
  396.     M[W, Y] :=  MatrixDetInternal(a1, a3, a4, b1, b3, b4, c1, c3, c4);
  397.     M[X, Z] :=  MatrixDetInternal(b1, b2, b4, c1, c2, c4, d1, d2, d4);
  398.     M[Y, Z] := -MatrixDetInternal(a1, a2, a4, c1, c2, c4, d1, d2, d4);
  399.     M[Z, Z] :=  MatrixDetInternal(a1, a2, a4, b1, b2, b4, d1, d2, d4);
  400.     M[W, Z] := -MatrixDetInternal(a1, a2, a4, b1, b2, b4, c1, c2, c4);
  401.     M[X, W] := -MatrixDetInternal(b1, b2, b3, c1, c2, c3, d1, d2, d3);
  402.     M[Y, W] :=  MatrixDetInternal(a1, a2, a3, c1, c2, c3, d1, d2, d3);
  403.     M[Z, W] := -MatrixDetInternal(a1, a2, a3, b1, b2, b3, d1, d2, d3);
  404.     M[W, W] :=  MatrixDetInternal(a1, a2, a3, b1, b2, b3, c1, c2, c3);
  405. end;
  407. // multiplies all elements of a 4x4 matrix with a factor
  408. procedure MatrixScale(var M: TMatrix4f; Factor: Single); register;
  409. var
  410.  I, J: Integer;
  411. begin
  412.   for I := 0 to 3 do
  413.     for J := 0 to 3 do M[I, J] := M[I, J] * Factor;
  414. end;
  416. // finds the inverse of a 4x4 matrix
  417. procedure Matrix_Inverse(var M: TMatrix4f); register;
  418. var
  419.  Det: Single;
  420. begin
  421.   Det := MatrixDeterminant(M);
  422.   if Abs(Det) < EPSILON then M := IdentityMatrix
  423.                         else
  424.   begin
  425.     MatrixAdjoint(M);
  426.     MatrixScale(M, 1 / Det);
  427.   end;
  428. end;
  430. // =============================================================================
  431. //  Matrix_TansformVector
  432. // =============================================================================
  433. function Matrix_TansformVector(const M : TMatrix4f; V : TVector3f) : TVector3f;
  434. begin
  435. Matrix_RotateVect(M, V);
  436. Result := V3(V[0]+M[3,0], V[1]+M[3,1], V[2]+M[3,2]);
  437. end;
  439. // =============================================================================
  440. //  Matrix_UntransformVector
  441. // =============================================================================
  442. function Matrix_UntransformVector(const M : TMatrix4f; V : TVector3f) : TVector3f;
  443. begin
  444. V := V3(V[0]-M[3,0], V[1]-M[3,1], V[2]-M[3,2]);
  445. V := V3(VDot(V, V3(M[0,0], M[0,1], M[0,2])),
  446.         VDot(V, V3(M[1,0], M[1,1], M[1,2])),
  447.         VDot(V, V3(M[2,0], M[2,1], M[2,2])));
  448. Result := V;
  449. end;
  451. // =============================================================================
  452. //  Matrix_SetColumn
  453. // =============================================================================
  454. procedure Matrix_SetColumn(var M : TMatrix4f; pColumn : Byte;pVect : TVector4f);
  455. begin
  456. M[pColumn, 0] := pVect[0];
  457. M[pColumn, 1] := pVect[1];
  458. M[pColumn, 2] := pVect[2];
  459. M[pColumn, 3] := pVect[3];
  460. end;
  463. end.