Rodrigues绕任意轴旋转

huangapple
go评论91阅读模式
英文:

Rodrigues Rotation about an arbitrary axis

问题

Sure, here's the translated code:

  1. 假设我有一个候选向量 `v(vx, vy, vz)`。我想围绕起始向量 `s(sx,sy,sz)` 到结束向量 `e(ex, ey, ez)` 的任意轴旋转 `theta` 度，当坐标轴的原点位于 `o(ox, oy, oz)` 处时。
  3. 因此，在以下源代码中，我正在执行**罗德里格斯旋转**，但结果不正确：
  5. ```C#
  6. public class RotoTranslation
  7. {
  8.     private readonly Vec3 origin;
  9.     private readonly double cosTheta;
  10.     private readonly double sinTheta;
  11.     private readonly Vec3 axis;
  12.     private readonly Matrix3x3 rotationMatrix;
  13.     private readonly Matrix3x3 translationMatrix;
  14.     private readonly Matrix3x3 invTranslationMatrix;
  16.     public RotoTranslation(
  17.                 Vec3 origin, 
  18.                 Vec3 start, 
  19.                 Vec3 end, 
  20.                 double angle_rad)
  21.     {
  22.         this.origin = origin;
  23.         this.axis = Vec3.Normalize(end - start);
  24.         this.cosTheta = Math.Cos(angle_rad);
  25.         this.sinTheta = Math.Sin(angle_rad);
  27.         Matrix3x3 uOuter = 
  28.             Vec3.OuterProduct_mat(axis, axis);
  29.         Matrix3x3 uCross = 
  30.             new Matrix3x3(0.0f, -axis.z, axis.y,                   
  31.                         axis.z, 0.0f, -axis.x,                
  32.                         -axis.y, axis.x, 0.0f);
  34.         rotationMatrix = 
  35.              cosTheta * Matrix3x3.Identity()
  36.              + (1.0f - cosTheta) * uOuter
  37.              + sinTheta * uCross;
  39.         translationMatrix = 
  40.              Matrix3x3.CreateTranslation(-origin);
  41.         rotationMatrix = 
  42.              rotationMatrix * translationMatrix;
  43.         invTranslationMatrix = 
  44.             Matrix3x3.CreateTranslation(origin);
  45.     }
  46.     public Vec3 RotateVector(Vec3 vector)
  47.     {
  48.         Vec3 transformedVector = 
  49.             Vec3.TransformNormal(
  50. 			    vector - origin, 
  51. 				rotationMatrix);
  52.         return Vec3.Transform(
  53.                 transformedVector, 
  54.                 invTranslationMatrix);
  55.     }
  56. }

测试

  1. [TestMethod]
  2. public void TestMethod1()
  3. {
  4.     Vec3 origin = new Vec3(1, 1, 1);
  5.     Vec3 start = new Vec3(1, 1, 1);
  6.     Vec3 end = new Vec3(4, 4, 4);
  7.     Vec3 candidate = new Vec3(3,3,3);
  9.     double degrees = 360;
  10.     degrees = degrees * (Math.PI / 180.0);
  12.     RotoTranslation rot = 
  13.         new RotoTranslation(origin, 
  14.                             start, 
  15.                             end, 
  16.                             degrees);
  18.     Vec3 rotated = rot.RotateVector(candidate);
  20.     Assert.AreEqual(candidate[0], rotated[0]);
  21.     Assert.AreEqual(candidate[1], rotated[1]);
  22.     Assert.AreEqual(candidate[2], rotated[2]);
  23. }

如果我围绕轴旋转一个向量360度，它应该位于初始向量的相同位置。然而，在这里并非如此。

你能告诉我哪里出错了吗？

N.B. 我必须使用 Matrix3x3，而不是 Matrix4x4（增广矩阵）。

  2. 请注意，这是代码的翻译部分，如果你需要任何其他帮助或有其他问题，请告诉我。
  4. <details>
  5. <summary>英文:</summary>
  7. Suppose, I have a candidate vector `v(vx, vy, vz)`. I want to rotate it `theta` degrees about an arbitrary axis that starts at vector `s(sx,sy,sz)` and ends at vector `e(ex, ey, ez)` when the origin of the axes is located at `o(ox, oy, oz)`.  
  9. So, I am doing the **Rodrigues rotation** in the following source code, but it is not giving the correct results:
  11. ```C#
  12. public class RotoTranslation
  13. {
  14.     private readonly Vec3 origin;
  15.     private readonly double cosTheta;
  16.     private readonly double sinTheta;
  17.     private readonly Vec3 axis;
  18.     private readonly Matrix3x3 rotationMatrix;
  19.     private readonly Matrix3x3 translationMatrix;
  20.     private readonly Matrix3x3 invTranslationMatrix;
  21.     public RotoTranslation(
  22.                 Vec3 origin, 
  23.                 Vec3 start, 
  24.                 Vec3 end, 
  25.                 double angle_rad)
  26.     {
  27.         this.origin = origin;
  28.         this.axis = Vec3.Normalize(end - start);
  29.         this.cosTheta = Math.Cos(angle_rad);
  30.         this.sinTheta = Math.Sin(angle_rad);
  32.         Matrix3x3 uOuter = 
  33.             Vec3.OuterProduct_mat(axis, axis);
  34.         Matrix3x3 uCross = 
  35.             new Matrix3x3(0.0f, -axis.z, axis.y,                   
  36.                         axis.z, 0.0f, -axis.x,                
  37.                         -axis.y, axis.x, 0.0f);
  39.         rotationMatrix = 
  40.              cosTheta * Matrix3x3.Identity()
  41.              + (1.0f - cosTheta) * uOuter
  42.              + sinTheta * uCross;
  44.         translationMatrix = 
  45.              Matrix3x3.CreateTranslation(-origin);
  46.         rotationMatrix = 
  47.              rotationMatrix * translationMatrix;
  48.         invTranslationMatrix = 
  49.             Matrix3x3.CreateTranslation(origin);
  50.     }
  51.     public Vec3 RotateVector(Vec3 vector)
  52.     {
  53.         Vec3 transformedVector = 
  54.             Vec3.TransformNormal(
  55. 			    vector - origin, 
  56. 				rotationMatrix);
  57.         return Vec3.Transform(
  58.                 transformedVector, 
  59.                 invTranslationMatrix);
  60.     }
  61. }

Test

  1. [TestMethod]
  2. public void TestMethod1()
  3. {
  4. 	Vec3 origin = new Vec3(1, 1, 1);
  5. 	Vec3 start = new Vec3(1, 1, 1);
  6. 	Vec3 end = new Vec3(4, 4, 4);
  7. 	Vec3 candidate = new Vec3(3,3,3);
  9. 	double degrees = 360;
  10. 	degrees = degrees * (Math.PI / 180.0);
  12. 	RotoTranslation rot = 
  13. 	    new RotoTranslation(origin, 
  14. 		                    start, 
  15. 							end, 
  16. 							degrees);
  18. 	Vec3 rotated = rot.RotateVector(candidate);
  20. 	Assert.AreEqual(candidate[0], rotated[0]);
  21. 	Assert.AreEqual(candidate[1], rotated[1]);
  22. 	Assert.AreEqual(candidate[2], rotated[2]);
  23. }

If I rotate a vector around an axis 360 degrees, it should be at the same position as the initial vector. However, that is not the case here.

Can you tell me what I am doing wrong?

N.B. I must use Matrix3x3, rather than a Matrix4x4 (augmented matrix).

Full Source code

  1. using System;
  2. using System.Collections.Generic;
  3. using Microsoft.VisualStudio.TestTools.UnitTesting;
  5. public class Vec3
  6. {
  7. 	public double x, y, z;
  9. 	public Vec3(double x, double y, double z)
  10. 	{
  11. 		this.x = x;
  12. 		this.y = y;
  13. 		this.z = z;
  14. 	}
  16. 	public static double DistanceSquared(Vec3 t1, Vec3 t2)
  17. 	{
  18. 		double x = t1.x - t2.x;
  19. 		double y = t1.y - t2.y;
  20. 		double z = t1.z - t2.z;
  21. 		return x * x + y * y + z * z;
  22. 	}
  24. 	public static double Distance(Vec3 t1, Vec3 t2)
  25. 	{
  26. 		if (t1 == null) throw new Exception(&quot;point1 is null&quot;);
  27. 		if (t2 == null) throw new Exception(&quot;point2 is null&quot;);
  29. 		return Math.Sqrt(DistanceSquared(t1, t2));
  30. 	}
  32. 	public Vec3 Subtract(Vec3 rhs)
  33. 	{
  34. 		return new Vec3(this.x - rhs.x, this.y - rhs.y, this.z - rhs.z);
  35. 	}
  37. 	public static Vec3 operator -(Vec3 a, Vec3 b)
  38. 	{
  39. 		return new Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
  40. 	}
  43. 	public Vec3 Scale(double rhs)
  44. 	{
  45. 		return new Vec3(this.x * rhs, this.y * rhs, this.z * rhs);
  46. 	}
  48. 	public double MagnitudeSquared()
  49. 	{
  50. 		return this.x * this.x + this.y * this.y + this.z * this.z;
  51. 	}
  53. 	public static double Dot(Vec3 a, Vec3 b)
  54. 	{
  55. 		return a.x * b.x + a.y * b.y + a.z * b.z;
  56. 	}
  59. 	public Vec3 Cross(Vec3 other)
  60. 	{
  61. 		double a = this.y * other.z - this.z * other.y;
  62. 		double b = this.z * other.x - this.x * other.z;
  63. 		double c = this.x * other.y - this.y * other.x;
  64. 		return new Vec3(a, b, c);
  65. 	}
  67. 	public override string ToString()
  68. 	{
  69. 		return $&quot;{x,8:0.000}{y,8:0.000}{z,8:0.000}&quot;;
  70. 	}
  72. 	public Vec3(string x, string y, string z)
  73. 	{
  74. 		this.x = Convert.ToDouble(x);
  75. 		this.y = Convert.ToDouble(y);
  76. 		this.z = Convert.ToDouble(z);
  77. 	}
  79. 	public Vec3(string xyz)
  80. 	{
  81. 		string[] vals = xyz.Split(new char[] { &#39; &#39; }, StringSplitOptions.RemoveEmptyEntries);
  83. 		this.x = Convert.ToDouble(vals[0].Trim());
  84. 		this.y = Convert.ToDouble(vals[1].Trim());
  85. 		this.z = Convert.ToDouble(vals[2].Trim());
  86. 	}
  88. 	public Vec3()
  89. 	{
  90. 	}
  92. 	public static Vec3 Normalize(Vec3 vec)
  93. 	{
  94. 		double mag = Math.Sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z);
  95. 		return new Vec3(vec.x / mag, vec.y / mag, vec.z / mag);
  96. 	}
  98. 	public static Vec3 OuterProduct(Vec3 a, Vec3 b)
  99. 	{
  100. 		double x = a.y * b.z - a.z * b.y;
  101. 		double y = a.z * b.x - a.x * b.z;
  102. 		double z = a.x * b.y - a.y * b.x;
  103. 		return new Vec3(x, y, z);
  104. 	}
  106. 	public static Matrix3x3 OuterProduct_mat(Vec3 lhs, Vec3 rhs)
  107. 	{
  108. 		double[,] data = new double[3, 3];
  110. 		data[0, 0] = lhs.x * rhs.x;
  111. 		data[0, 1] = lhs.x * rhs.y;
  112. 		data[0, 2] = lhs.x * rhs.z;
  114. 		data[1, 0] = lhs.y * rhs.x;
  115. 		data[1, 1] = lhs.y * rhs.y;
  116. 		data[1, 2] = lhs.y * rhs.z;
  118. 		data[2, 0] = lhs.z * rhs.x;
  119. 		data[2, 1] = lhs.z * rhs.y;
  120. 		data[2, 2] = lhs.z * rhs.z;
  122. 		return new Matrix3x3(data[0, 0], data[0, 1], data[0, 2],
  123. 							 data[1, 0], data[1, 1], data[1, 2],
  124. 							 data[2, 0], data[2, 1], data[2, 2]);
  125. 	}
  127. 	public static Vec3 operator -(Vec3 v)
  128. 	{
  129. 		return new Vec3(-v.x, -v.y, -v.z);
  130. 	}
  132. 	public static Vec3 Transform(Vec3 v, Matrix3x3 m)
  133. 	{
  134. 		double x = m[0] * v.x + m[1] * v.y + m[2] * v.z;
  135. 		double y = m[3] * v.x + m[4] * v.y + m[5] * v.z;
  136. 		double z = m[6] * v.x + m[7] * v.y + m[8] * v.z;
  137. 		return new Vec3(x, y, z);
  138. 	}
  140. 	public static Vec3 TransformNormal(Vec3 normal, Matrix3x3 matrix)
  141. 	{
  142. 		return new Vec3(
  143. 			matrix[0] * normal.x + matrix[3] * normal.y + matrix[6] * normal.z,
  144. 			matrix[1] * normal.x + matrix[4] * normal.y + matrix[7] * normal.z,
  145. 			matrix[2] * normal.x + matrix[5] * normal.y + matrix[8] * normal.z
  146. 		);
  147. 	}
  149. 	public static Vec3 operator +(Vec3 v1, Vec3 v2)
  150. 	{
  151. 		return new Vec3(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
  152. 	}
  154. 	public double this[int i]
  155. 	{
  156. 		get
  157. 		{
  158. 			switch (i)
  159. 			{
  160. 				case 0:
  161. 					return x;
  162. 				case 1:
  163. 					return y;
  164. 				case 2:
  165. 					return z;
  166. 				default:
  167. 					throw new IndexOutOfRangeException();
  168. 			}
  169. 		}
  170. 		set
  171. 		{
  172. 			switch (i)
  173. 			{
  174. 				case 0:
  175. 					x = value;
  176. 					break;
  177. 				case 1:
  178. 					y = value;
  179. 					break;
  180. 				case 2:
  181. 					z = value;
  182. 					break;
  183. 				default:
  184. 					throw new IndexOutOfRangeException();
  185. 			}
  186. 		}
  187. 	}
  188. }
  190. public class Matrix3x3
  191. {
  192. 	const int rows = 3;
  193. 	const int cols = 3;
  194. 	private List&lt;double&gt; data2d;
  196. 	public Matrix3x3()
  197. 	{
  198. 		data2d = new List&lt;double&gt;(new double[rows * cols]);
  199. 	}
  201. 	public Matrix3x3(double x1, double y1, double z1,
  202. 					 double x2, double y2, double z2,
  203. 					 double x3, double y3, double z3)
  204. 	{
  205. 		data2d = new List&lt;double&gt;(new double[rows * cols]);
  206. 		data2d[0] = x1; data2d[1] = y1; data2d[2] = z1;
  207. 		data2d[3] = x2; data2d[4] = y2; data2d[5] = z2;
  208. 		data2d[6] = x3; data2d[7] = y3; data2d[8] = z3;
  209. 	}
  211. 	public double this[int i]
  212. 	{
  213. 		get { return data2d[i]; }
  214. 		set { data2d[i] = value; }
  215. 	}
  217. 	public static Matrix3x3 Add(Matrix3x3 lhs, Matrix3x3 rhs)
  218. 	{
  219. 		Matrix3x3 result = new Matrix3x3();
  220. 		for (int i = 0; i &lt; rows * cols; i++)
  221. 		{
  222. 			result[i] = lhs[i] + rhs[i];
  223. 		}
  224. 		return result;
  225. 	}
  227. 	public static Matrix3x3 Sub(Matrix3x3 lhs, Matrix3x3 rhs)
  228. 	{
  229. 		Matrix3x3 result = new Matrix3x3();
  230. 		for (int i = 0; i &lt; rows * cols; i++)
  231. 		{
  232. 			result[i] = lhs[i] - rhs[i];
  233. 		}
  234. 		return result;
  235. 	}
  237. 	public static Matrix3x3 mul_scalar(Matrix3x3 lhs, double rhs)
  238. 	{
  239. 		Matrix3x3 result = new Matrix3x3();
  240. 		for (int i = 0; i &lt; rows * cols; i++)
  241. 		{
  242. 			result[i] = lhs[i] * rhs;
  243. 		}
  244. 		return result;
  245. 	}
  247. 	public static Matrix3x3 div_scalar(Matrix3x3 lhs, double rhs)
  248. 	{
  249. 		Matrix3x3 result = new Matrix3x3();
  250. 		for (int i = 0; i &lt; rows * cols; i++)
  251. 		{
  252. 			result[i] = lhs[i] / rhs;
  253. 		}
  254. 		return result;
  255. 	}
  257. 	public static Vec3 mul_vec_mut(Matrix3x3 lhs, Vec3 rhs)
  258. 	{
  259. 		double x = lhs[0] * rhs.x + lhs[1] * rhs.y + lhs[2] * rhs.z;
  260. 		double y = lhs[3] * rhs.x + lhs[4] * rhs.y + lhs[5] * rhs.z;
  261. 		double z = lhs[6] * rhs.x + lhs[7] * rhs.y + lhs[8] * rhs.z;
  262. 		return new Vec3(x, y, z);
  263. 	}
  265. 	public double det()
  266. 	{
  267. 		Matrix3x3 lhs = this;
  268. 		double a = lhs[0] * (lhs[4] * lhs[8] - lhs[5] * lhs[7]);
  269. 		double b = lhs[1] * (lhs[3] * lhs[8] - lhs[5] * lhs[6]);
  270. 		double c = lhs[2] * (lhs[3] * lhs[7] - lhs[4] * lhs[6]);
  271. 		double returns = a - b + c;
  272. 		return returns;
  273. 	}
  275. 	public static Matrix3x3 mul_mat_mut(Matrix3x3 lhs, Matrix3x3 rhs)
  276. 	{
  277. 		Matrix3x3 result = new Matrix3x3(
  278. 			lhs[0] * rhs[0] + lhs[1] * rhs[3] + lhs[2] * rhs[6], // row 1, column 1
  279. 			lhs[0] * rhs[1] + lhs[1] * rhs[4] + lhs[2] * rhs[7], // row 1, column 2
  280. 			lhs[0] * rhs[2] + lhs[1] * rhs[5] + lhs[2] * rhs[8], // row 1, column 3
  281. 			lhs[3] * rhs[0] + lhs[4] * rhs[3] + lhs[5] * rhs[6], // row 2, column 1
  282. 			lhs[3] * rhs[1] + lhs[4] * rhs[4] + lhs[5] * rhs[7], // row 2, column 2
  283. 			lhs[3] * rhs[2] + lhs[4] * rhs[5] + lhs[5] * rhs[8], // row 2, column 3
  284. 			lhs[6] * rhs[0] + lhs[7] * rhs[3] + lhs[8] * rhs[6], // row 3, column 1
  285. 			lhs[6] * rhs[1] + lhs[7] * rhs[4] + lhs[8] * rhs[7], // row 3, column 2
  286. 			lhs[6] * rhs[2] + lhs[7] * rhs[5] + lhs[8] * rhs[8]); // row 3, column 3
  288. 		return result;
  289. 	}
  291. 	public static Matrix3x3 inverse(Matrix3x3 lhs)
  292. 	{
  293. 		Matrix3x3 temp = null;
  294. 		double _det = lhs.det();
  295. 		if (_det != 0.0)
  296. 		{
  297. 			double inv_det = 1.0 / _det;
  298. 			temp = new Matrix3x3(
  299. 				lhs[4] * lhs[8] - lhs[5] * lhs[7],
  300. 				lhs[2] * lhs[7] - lhs[1] * lhs[8],
  301. 				lhs[1] * lhs[5] - lhs[2] * lhs[4],
  302. 				lhs[5] * lhs[6] - lhs[3] * lhs[8],
  303. 				lhs[0] * lhs[8] - lhs[2] * lhs[6],
  304. 				lhs[2] * lhs[3] - lhs[0] * lhs[5],
  305. 				lhs[3] * lhs[7] - lhs[4] * lhs[6],
  306. 				lhs[1] * lhs[6] - lhs[0] * lhs[7],
  307. 				lhs[0] * lhs[4] - lhs[1] * lhs[3]);
  308. 		}
  309. 		return temp;
  310. 	}
  312. 	public static Matrix3x3 Identity()
  313. 	{
  314. 		return new Matrix3x3(1.0, 0.0, 0.0,
  315. 										0.0, 1.0, 0.0,
  316. 										0.0, 0.0, 1.0);
  317. 	}
  319. 	public static Matrix3x3 OuterProduct(Matrix3x3 a, Matrix3x3 b)
  320. 	{
  321. 		Matrix3x3 result = new Matrix3x3();
  322. 		for (int i = 0; i &lt; rows; i++)
  323. 		{
  324. 			for (int j = 0; j &lt; cols; j++)
  325. 			{
  326. 				result[i * cols + j] = a[i] * b[j];
  327. 			}
  328. 		}
  329. 		return result;
  330. 	}
  332. 	public static Matrix3x3 operator *(Matrix3x3 lhs, double rhs)
  333. 	{
  334. 		Matrix3x3 result = new Matrix3x3();
  335. 		for (int i = 0; i &lt; rows * cols; i++)
  336. 		{
  337. 			result[i] = lhs[i] * rhs;
  338. 		}
  339. 		return result;
  340. 	}
  342. 	public static Matrix3x3 operator *(double lhs, Matrix3x3 rhs)
  343. 	{
  344. 		return rhs * lhs;
  345. 	}
  347. 	public static Matrix3x3 operator +(Matrix3x3 lhs, Matrix3x3 rhs)
  348. 	{
  349. 		Matrix3x3 result = new Matrix3x3();
  350. 		for (int i = 0; i &lt; rows * cols; i++)
  351. 		{
  352. 			result[i] = lhs[i] + rhs[i];
  353. 		}
  354. 		return result;
  355. 	}
  357. 	public static Matrix3x3 CreateTranslation(Vec3 translation)
  358. 	{
  359. 		return new Matrix3x3(
  360. 			1.0f, 0.0f, 0.0f,
  361. 			0.0f, 1.0f, 0.0f,
  362. 			translation.x, translation.y, translation.z
  363. 		);
  364. 	}
  366. 	public static Matrix3x3 operator *(Matrix3x3 lhs, Matrix3x3 rhs)
  367. 	{
  368. 		Matrix3x3 result = new Matrix3x3();
  369. 		for (int i = 0; i &lt; rows; i++)
  370. 		{
  371. 			for (int j = 0; j &lt; cols; j++)
  372. 			{
  373. 				double sum = 0;
  374. 				for (int k = 0; k &lt; cols; k++)
  375. 				{
  376. 					sum += lhs[i * cols + k] * rhs[k * cols + j];
  377. 				}
  378. 				result[i * cols + j] = sum;
  379. 			}
  380. 		}
  381. 		return result;
  382. 	}
  383. }
  385. public class RotoTranslation
  386. {
  387.     private readonly Vec3 origin;
  388.     private readonly double cosTheta;
  389.     private readonly double sinTheta;
  390.     private readonly Vec3 axis;
  391.     private readonly Matrix3x3 rotationMatrix;
  392.     private readonly Matrix3x3 translationMatrix;
  393.     private readonly Matrix3x3 invTranslationMatrix;
  395.     public RotoTranslation(Vec3 origin, Vec3 start, Vec3 end, double angle_rad)
  396.     {
  397.         this.origin = origin;
  398.         this.axis = Vec3.Normalize(end - start);
  399.         this.cosTheta = Math.Cos(angle_rad);
  400.         this.sinTheta = Math.Sin(angle_rad);
  402.         Matrix3x3 uOuter = Vec3.OuterProduct_mat(axis, axis);
  403.         Matrix3x3 uCross = new Matrix3x3(
  404.                                            0.0f, -axis.z, axis.y,
  405.                                            axis.z, 0.0f, -axis.x,
  406.                                           -axis.y, axis.x, 0.0f
  407.                                         );
  409.         rotationMatrix = cosTheta * Matrix3x3.Identity()
  410.                                  + (1.0f - cosTheta) * uOuter
  411.                                  + sinTheta * uCross;
  413.         translationMatrix = Matrix3x3.CreateTranslation(-origin);
  414.         rotationMatrix = rotationMatrix * translationMatrix;
  415.         invTranslationMatrix = Matrix3x3.CreateTranslation(origin);
  416.     }
  418.     public Vec3 RotateVector(Vec3 vector)
  419.     {
  420.         Vec3 transformedVector = Vec3.TransformNormal(vector - origin, rotationMatrix);
  421.         return Vec3.Transform(transformedVector, invTranslationMatrix);
  422.     }
  423. }
  425. [TestClass]
  426. public class RotoTranslationUnitTest
  427. {
  428.     [TestMethod]
  429.     public void TestMethod1()
  430.     {
  431.         Vec3 origin = new Vec3(1, 1, 1);
  432.         Vec3 start = new Vec3(1, 1, 1);
  433.         Vec3 end = new Vec3(4, 4, 4);
  434.         Vec3 candidate = new Vec3(3,3,3);
  436.         double degrees = 360;
  437.         degrees = degrees * (Math.PI / 180.0);
  439.         RotoTranslation rot = new RotoTranslation(origin, start, end, degrees);
  441.         Vec3 rotated = rot.RotateVector(candidate);
  443.         Assert.AreEqual(candidate[0], rotated[0]);
  444.         Assert.AreEqual(candidate[1], rotated[1]);
  445.         Assert.AreEqual(candidate[2], rotated[2]);
  446.     }
  447. }

答案1

得分: 1

以下是翻译好的代码部分：

  1. 你的问题出在这个方法中
  3. public static Matrix3x3 CreateTranslation(Vec3 translation)
  4. {
  5.     return new Matrix3x3(
  6.         1.0f, 0.0f, 0.0f,
  7.         0.0f, 1.0f, 0.0f,
  8.         translation.x, translation.y, translation.z
  9.     );
  10. }
  12. 要表示3D中的偏移量，你需要一个4x4的矩阵，排列如下
  14. | 1    0    0    x |
  15. | 0    1    0    y |
  16. | 0    0    1    z |
  17. | 0    0    0    1 |
  19. 不要使用矩阵运算处理平移，只需更改代码以使用线性代数
  21. public RotoTranslation(Vec3 origin, Vec3 start, Vec3 end, double angle_rad)
  22. {
  23.     this.origin = origin;
  24.     this.axis = Vec3.Normalize(end - start);
  25.     this.cosTheta = Math.Cos(angle_rad);
  26.     this.sinTheta = Math.Sin(angle_rad);
  28.     Matrix3x3 uOuter = Vec3.OuterProduct_mat(axis, axis);
  29.     Matrix3x3 uCross = new Matrix3x3(
  30.                                        0.0f, -axis.z, axis.y,
  31.                                        axis.z, 0.0f, -axis.x,
  32.                                       -axis.y, axis.x, 0.0f
  33.                                     );
  35.     rotationMatrix = cosTheta * Matrix3x3.Identity()
  36.                              + (1.0f - cosTheta) * uOuter
  37.                              + sinTheta * uCross;
  39. }
  41. public Vec3 RotateVector(Vec3 vector)
  42. {
  43.     Vec3 transformedVector = vector - origin;
  44.     transformedVector = Vec3.Transform(transformedVector, rotationMatrix );
  45.     return transformedVector + origin;
  46. }
  48. 'translationMatrix' 和 'InvTranslationMatrix' 不需要。你已经存储了 'origin' 向量，这就是你需要的一切。

如果还有其他需要翻译的内容，请继续提问。

英文:

Your problem is in this method

  1. public static Matrix3x3 CreateTranslation(Vec3 translation)
  2. {
  3. return new Matrix3x3(
  4. 1.0f, 0.0f, 0.0f,
  5. 0.0f, 1.0f, 0.0f,
  6. translation.x, translation.y, translation.z
  7. );
  8. }

To represent an offset in 3D with a matrix you need a 4×4 matrix arranged as follows

  1.                     | 1    0    0    x |
  2. translate3(x,y,z) = | 0    1    0    y |
  3. | 0    0    1    z |
  4. | 0    0    0    1 |

Instead of handling the translation with a matrix operation, just change the code to use linear algebra

  1. public RotoTranslation(Vec3 origin, Vec3 start, Vec3 end, double angle_rad)
  2. {
  3. this.origin = origin;
  4. this.axis = Vec3.Normalize(end - start);
  5. this.cosTheta = Math.Cos(angle_rad);
  6. this.sinTheta = Math.Sin(angle_rad);
  7. Matrix3x3 uOuter = Vec3.OuterProduct_mat(axis, axis);
  8. Matrix3x3 uCross = new Matrix3x3(
  9. 0.0f, -axis.z, axis.y,
  10. axis.z, 0.0f, -axis.x,
  11. -axis.y, axis.x, 0.0f
  12. );
  13. rotationMatrix = cosTheta * Matrix3x3.Identity()
  14. + (1.0f - cosTheta) * uOuter
  15. + sinTheta * uCross;
  16. }
  17. public Vec3 RotateVector(Vec3 vector)
  18. {
  19. Vec3 transformedVector = vector - origin;
  20. transformedVector = Vec3.Transform(transformedVector, rotationMatrix );
  21. return transformedVector + origin;
  22. }

translationMatrix and InvTranslationMatrix are not needed. You have the origin vector stored and that all you need.

huangapple
  • 本文由 发表于 2023年4月17日 22:32:05
  • 转载请务必保留本文链接：https://go.coder-hub.com/76036276.html
  • c#
  • matrix
  • rotational-matrices
匿名

发表评论

匿名网友

:?: :razz: :sad: :evil: :!: :smile: :oops: :grin: :eek: :shock: :???: :cool: :lol: :mad: :twisted: :roll: :wink: :idea: :arrow: :neutral: :cry: :mrgreen:

确定