|
|
@ -1,6 +1,8 @@ |
|
|
|
package iflytop.app.utils; |
|
|
|
|
|
|
|
import iflytop.app.config.AppConstantConfig; |
|
|
|
import iflytop.app.type.XYPoint; |
|
|
|
import iflytop.app.type.protocol.TPMISensorValReport; |
|
|
|
import lombok.extern.slf4j.Slf4j; |
|
|
|
import org.springframework.util.Assert; |
|
|
|
|
|
|
@ -10,6 +12,24 @@ import static java.lang.Math.abs; |
|
|
|
@Slf4j |
|
|
|
public class CoordinateCalculator { |
|
|
|
|
|
|
|
|
|
|
|
static public XYPoint calculator(Double arm0Length, Double arm0Rad, Double arm1Length, Double arm1Rad) { |
|
|
|
XYPoint xyPoint = new XYPoint(); double lval = calculateThirdSide(arm0Length, arm1Length, Math.PI - arm1Rad); if (lval < 0.000001) { |
|
|
|
xyPoint.x = 0.0; xyPoint.y = 0.0; |
|
|
|
} else { |
|
|
|
double rad0 = calculateAngle(arm0Length, lval, arm1Length); double lrad = 0.0; if (arm1Rad > 0) { |
|
|
|
lrad = arm0Rad + rad0; |
|
|
|
} else { |
|
|
|
lrad = arm0Rad - rad0; |
|
|
|
} xyPoint.x = lval * Math.sin(lrad); xyPoint.y = lval * Math.cos(lrad); |
|
|
|
} return xyPoint; |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
// |
|
|
|
// PRIVATE |
|
|
|
// |
|
|
|
|
|
|
|
public static double calculateThirdSide(double a, double b, double angleInRadians) { |
|
|
|
// 计算第三条边的平方 |
|
|
|
double cSquared = a * a + b * b - 2 * a * b * Math.cos(angleInRadians); |
|
|
@ -38,18 +58,9 @@ public class CoordinateCalculator { |
|
|
|
} return ret; |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
static public XYPoint calculator(Double arm0Length, Double arm0Rad, Double arm1Length, Double arm1Rad) { |
|
|
|
XYPoint xyPoint = new XYPoint(); double lval = calculateThirdSide(arm0Length, arm1Length, Math.PI - arm1Rad); if (lval < 0.000001) { |
|
|
|
xyPoint.x = 0.0; xyPoint.y = 0.0; |
|
|
|
} else { |
|
|
|
double rad0 = calculateAngle(arm0Length, lval, arm1Length); double lrad = 0.0; if (arm1Rad > 0) { |
|
|
|
lrad = arm0Rad + rad0; |
|
|
|
} else { |
|
|
|
lrad = arm0Rad - rad0; |
|
|
|
} xyPoint.x = lval * Math.sin(lrad); xyPoint.y = lval * Math.cos(lrad); |
|
|
|
} return xyPoint; |
|
|
|
} |
|
|
|
// |
|
|
|
// TEST |
|
|
|
// |
|
|
|
|
|
|
|
static void testCalculateThirdSide(double a, double b, double angleInRadians, double expected) { |
|
|
|
double c = calculateThirdSide(a, b, angleInRadians); |
|
|
@ -62,7 +73,6 @@ public class CoordinateCalculator { |
|
|
|
String.format("testcalculateAngle: side1=%+.4f,side2=%+.4f,oppositeSide=%+.4f --> angle=%+.4f(%+.4f)", side1, side2, oppositeSide, angle, Math.toDegrees(angle))); |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
static void testCalculator(double arm0Length, double arm0Rad, double arm1Length, double arm1Rad, double expectedX, double expectedY) { |
|
|
|
XYPoint xyPoint = calculator(arm0Length, arm0Rad, arm1Length, arm1Rad); |
|
|
|
Assert.isTrue(abs(xyPoint.x - expectedX) < 0.0001 && abs(xyPoint.y - expectedY) < 0.0001, |
|
|
@ -70,7 +80,7 @@ public class CoordinateCalculator { |
|
|
|
} |
|
|
|
|
|
|
|
public static void main(String[] args) { |
|
|
|
CoordinateCalculator.testCalculateThirdSide(1, 1, 0,0.0); |
|
|
|
CoordinateCalculator.testCalculateThirdSide(1, 1, 0, 0.0); |
|
|
|
CoordinateCalculator.testCalculateThirdSide(1, 1, Math.PI / 4, 0.7654); |
|
|
|
CoordinateCalculator.testCalculateThirdSide(1, 1, Math.PI / 2, 1.4142); |
|
|
|
CoordinateCalculator.testCalculateThirdSide(1, 1, Math.PI * 3 / 4, 1.8478); |
|
|
|