make traj_eval_trajnsform more efficient and add comment

bitcraze · Jun 3, 2024 · 182455b · 182455b
1 parent ac94caf
commit 182455b
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 3 deletions.
diff --git a/src/modules/interface/pptraj.h b/src/modules/interface/pptraj.h
@@ -137,6 +137,7 @@ bool is_traj_eval_valid(struct traj_eval const *ev);
 // evaluate a single polynomial piece
 struct traj_eval poly4d_eval(struct poly4d const *p, float t);
 
+// rotate and then translate a traj_eval object
 void traj_eval_transform(struct traj_eval *ev, struct vec shift, float rotation);
 
 // ----------------------------------//

diff --git a/src/modules/src/pptraj.c b/src/modules/src/pptraj.c
@@ -331,10 +331,12 @@ struct traj_eval poly4d_eval(struct poly4d const *p, float t)
 
 void traj_eval_transform(struct traj_eval *ev, struct vec shift, float rotation)
 {
+	struct mat33 rotator = mrotz(normalize_radians(rotation));
+
 	// rotate position, velocity, acceleration
-	ev->pos = mvmul(mrotz(normalize_radians(rotation)), ev->pos);
-	ev->vel = mvmul(mrotz(normalize_radians(rotation)), ev->vel);
-	ev->acc = mvmul(mrotz(normalize_radians(rotation)), ev->acc);
+	ev->pos = mvmul(rotator, ev->pos);
+	ev->vel = mvmul(rotator, ev->vel);
+	ev->acc = mvmul(rotator, ev->acc);
 
 	// shift
 	ev->yaw += normalize_radians(rotation);