From ba8202d54a03fb4f7fb712d8bc0bb63ba6087f83 Mon Sep 17 00:00:00 2001
From: Jonathan Westhues <jwesthues@cq.cx>
Date: Mon, 8 Jun 2015 11:19:29 -0700
Subject: [PATCH] Don't falsely detect inconsistent systems with small angle
 constraints.

---
 src/constrainteq.cpp | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/src/constrainteq.cpp b/src/constrainteq.cpp
index c62c350..e5022cb 100644
--- a/src/constrainteq.cpp
+++ b/src/constrainteq.cpp
@@ -628,8 +628,15 @@ void ConstraintBase::GenerateReal(IdList<Equation,hEquation> *l) {
             if(type == ANGLE) {
                 // The direction cosine is equal to the cosine of the
                 // specified angle
-                Expr *rads = exA->Times(Expr::From(PI/180));
-                AddEq(l, c->Minus(rads->Cos()), 0);
+                Expr *rads = exA->Times(Expr::From(PI/180)),
+                     *rc   = rads->Cos();
+                double arc = fabs(rc->Eval());
+                // avoid false detection of inconsistent systems by gaining
+                // up as the difference in dot products gets small at small
+                // angles; doubles still have plenty of precision, only
+                // problem is that rank test
+                Expr *mult = Expr::From(arc > 0.99 ? 0.01/(1.00001 - arc) : 1);
+                AddEq(l, (c->Minus(rc))->Times(mult), 0);
             } else {
                 // The dot product (and therefore the direction cosine)
                 // is equal to zero, perpendicular.