From 9815079a748b3b626342e51a2ffe442248ed9f44 Mon Sep 17 00:00:00 2001
From: Daniel Flickinger <flikx@flux.utah.edu>
Date: Thu, 16 Dec 2004 09:31:38 +0000
Subject: [PATCH] (hopefully) fixed up the completely twisted and fucked up
left handed world coordinate thing. (Transpose x,y)
---
robots/rmcd/rmcd.c | 24 +++++++++++++++++-------
1 file changed, 17 insertions(+), 7 deletions(-)
diff --git a/robots/rmcd/rmcd.c b/robots/rmcd/rmcd.c
index 44a948e733..a29adca1b8 100644
--- a/robots/rmcd/rmcd.c
+++ b/robots/rmcd/rmcd.c
@@ -206,11 +206,17 @@ static void conv_a2r(struct position *rel,
ct = cos(abs_start->theta);
st = sin(abs_start->theta);
- rel->x = ct*(abs_finish->x - abs_start->x) +
- st*(abs_finish->y - abs_start->y);
- rel->y = ct*(abs_finish->y - abs_start->y) +
- st*(abs_start->x - abs_finish->x);
+ //rel->x = ct*(abs_finish->x - abs_start->x) +
+ // st*(abs_finish->y - abs_start->y);
+ //rel->y = ct*(abs_finish->y - abs_start->y) +
+ // st*(abs_start->x - abs_finish->x);
+ // Transpose x, y
+ rel->x = ct*(abs_finish->y - abs_start->y) +
+ st*(abs_finish->x - abs_start->x);
+ rel->y = ct*(abs_finish->x - abs_start->x) +
+ st*(abs_start->y - abs_finish->y);
+
rel->theta = abs_finish->theta - abs_start->theta;
rel->timestamp = abs_finish->timestamp;
@@ -243,9 +249,13 @@ static void conv_r2a(struct position *abs_finish,
ct = cos(abs_start->theta);
st = sin(abs_start->theta);
- abs_finish->x = ct*rel->x - st*rel->y + abs_start->x;
- abs_finish->y = ct*rel->y + st*rel->x + abs_start->y;
-
+ // abs_finish->x = ct*rel->x - st*rel->y + abs_start->x;
+ // abs_finish->y = ct*rel->y + st*rel->x + abs_start->y;
+
+ // transpose x,y
+ abs_finish->x = ct*rel->y - st*rel->x + abs_start->y;
+ abs_finish->y = ct*rel->x + st*rel->y + abs_start->x;
+
abs_finish->theta = abs_start->theta + rel->theta;
abs_finish->timestamp = rel->timestamp;
--
GitLab