git.droids-corp.org
/
aversive.git
/ blobdiff
commit
grep
author
committer
pickaxe
?
search:
re
summary
|
shortlog
|
log
|
commit
|
commitdiff
|
tree
raw
|
inline
| side by side
work on trajectory, update cobboard and ballboard too
[aversive.git]
/
modules
/
base
/
math
/
geometry
/
lines.c
diff --git
a/modules/base/math/geometry/lines.c
b/modules/base/math/geometry/lines.c
index
09ad1a1
..
935d012
100755
(executable)
--- a/
modules/base/math/geometry/lines.c
+++ b/
modules/base/math/geometry/lines.c
@@
-46,24
+46,24
@@
* p argument is the crossing point coordinates (dummy for 0 or 2
* result)
*/
* p argument is the crossing point coordinates (dummy for 0 or 2
* result)
*/
-uint8_t
+uint8_t
intersect_line(const line_t *l1, const line_t *l2, point_t *p)
intersect_line(const line_t *l1, const line_t *l2, point_t *p)
-{
+{
double tmp1, tmp2;
double tmp1, tmp2;
- debug_printf("l1:%2.2f,%2.2f,%2.2f l2:%2.2f,%2.2f,%2.2f\n",
+ debug_printf("l1:%2.2f,%2.2f,%2.2f l2:%2.2f,%2.2f,%2.2f\n",
l1->a, l1->b, l1->c, l2->a, l2->b, l2->c);
/* if dummy lines */
if ((l1->a == 0 && l1->b == 0) || (l2->a == 0 && l2->b == 0))
return 0;
l1->a, l1->b, l1->c, l2->a, l2->b, l2->c);
/* if dummy lines */
if ((l1->a == 0 && l1->b == 0) || (l2->a == 0 && l2->b == 0))
return 0;
-
+
if (l1->a == 0) {
if (l2->a == 0) {
if (l1->b*l2->c == l2->b*l1->c)
return 2;
return 0;
}
if (l1->a == 0) {
if (l2->a == 0) {
if (l1->b*l2->c == l2->b*l1->c)
return 2;
return 0;
}
-
+
/* by + c = 0
* a'x + b'y + c' = 0 */
/*
/* by + c = 0
* a'x + b'y + c' = 0 */
/*
@@
-74,7
+74,7
@@
intersect_line(const line_t *l1, const line_t *l2, point_t *p)
p->x = -(l2->b*(-l1->c) + l2->c*l1->b)/(l2->a*l1->b);
return 1;
}
p->x = -(l2->b*(-l1->c) + l2->c*l1->b)/(l2->a*l1->b);
return 1;
}
-
+
if (l1->b == 0) {
if (l2->b == 0) {
if (l1->a*l2->c == l2->a*l1->c)
if (l1->b == 0) {
if (l2->b == 0) {
if (l1->a*l2->c == l2->a*l1->c)
@@
-118,12
+118,12
@@
void pts2line(const point_t *p1, const point_t *p2, line_t *l)
p1y = p1->y;
p2x = p2->x;
p2y = p2->y;
p1y = p1->y;
p2x = p2->x;
p2y = p2->y;
-
+
l->a = -(p2y - p1y);
l->b = (p2x - p1x);
l->c = -(l->a * p1x + l->b * p1y);
l->a = -(p2y - p1y);
l->b = (p2x - p1x);
l->c = -(l->a * p1x + l->b * p1y);
-
+
debug_printf("%s: %2.2f, %2.2f, %2.2f\r\n",
__FUNCTION__, l->a, l->b, l->c);
}
debug_printf("%s: %2.2f, %2.2f, %2.2f\r\n",
__FUNCTION__, l->a, l->b, l->c);
}
@@
-145,16
+145,16
@@
void proj_pt_line(const point_t * p, const line_t * l, point_t * p_out)
/* return values:
* 0 dont cross
/* return values:
* 0 dont cross
- * 1 cross
+ * 1 cross
* 2 cross on point
* 3 parallel and one point in
*
* p argument is the crossing point coordinates (dummy for 0 1 or 3
* result)
*/
* 2 cross on point
* 3 parallel and one point in
*
* p argument is the crossing point coordinates (dummy for 0 1 or 3
* result)
*/
-uint8_t
-intersect_segment(const point_t *s1, const point_t *s2,
- const point_t *t1, const point_t *t2,
+uint8_t
+intersect_segment(const point_t *s1, const point_t *s2,
+ const point_t *t1, const point_t *t2,
point_t *p)
{
line_t l1, l2;
point_t *p)
{
line_t l1, l2;
@@
-210,7
+210,7
@@
intersect_segment(const point_t *s1, const point_t *s2,
*p = *s2;
return 2;
}
*p = *s2;
return 2;
}
-
+
debug_printf("px=%" PRIi32 " py=%" PRIi32 "\n", p->x, p->y);
/* Consider as parallel if intersection is too far */
debug_printf("px=%" PRIi32 " py=%" PRIi32 "\n", p->x, p->y);
/* Consider as parallel if intersection is too far */
@@
-242,3
+242,8
@@
intersect_segment(const point_t *s1, const point_t *s2,
return 1;
}
return 1;
}
+
+void line_translate(line_t *l, vect_t *v)
+{
+ l->c -= (l->a * v->x + l->b * v->y);
+}