package org.goko.core.math;
import java.math.BigDecimal;
import javax.vecmath.Matrix3d;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;
import org.goko.core.common.measure.quantity.Angle;
import org.goko.core.common.measure.quantity.AngleUnit;
import org.goko.core.common.measure.quantity.Length;
import org.goko.core.common.measure.quantity.LengthUnit;
import org.goko.core.common.measure.units.Unit;
public class Arc3b {
private Tuple6b start;
private Tuple6b center;
private Tuple6b end;
private Point3d pStart;
private Point3d pCenter;
private Point3d pEnd;
private Vector3d axis;
private Angle angle;
private Length radius;
private Unit<Length> unit;
public Arc3b(Tuple6b start, Tuple6b center, Tuple6b end, Vector3d axis, boolean clockwise) {
super();
this.start = start;
this.center = center;
this.end = end;
this.axis = axis;
this.radius = start.distance(center);//new Vector3d(start.x - center.x, start.y - center.y, start.z - center.z).length();
// Internal fields
this.unit = start.getX().getUnit();
this.pStart = start.toPoint3d(unit);
this.pCenter= center.toPoint3d(unit);
this.pEnd = end.toPoint3d(unit);
build(clockwise);
}
private Matrix3d getBaseChangeMatrix(){
Vector3d v1 = new Vector3d();
v1.sub(pStart, pCenter);
Vector3d v3 = new Vector3d(axis);
v1.normalize();
v3.normalize();
Vector3d v2 = new Vector3d();
v2.cross(v3, v1);
Matrix3d m = new Matrix3d( v1.x, v1.y, v1.z,
v2.x, v2.y, v2.z,
v3.x, v3.y, v3.z);
return m;
}
public void build(boolean clockwise){
Matrix3d matrix = getBaseChangeMatrix();
Matrix3d invMatrix = new Matrix3d();
invMatrix.invert(matrix);
Point3d lStart = new Point3d(pStart);
Point3d lCenter = new Point3d(pCenter);
Point3d lEnd = new Point3d(pEnd);
matrix.transform(lStart);
matrix.transform(lCenter);
matrix.transform(lEnd);
Vector3d v1 = new Vector3d(lStart.x - lCenter.x, lStart.y - lCenter.y, lStart.z - lCenter.z);
Vector3d v2 = new Vector3d(lEnd.x - lCenter.x, lEnd.y - lCenter.y, lEnd.z - lCenter.z);
double smallestAngle = StrictMath.atan2(v1.y,v1.x) - StrictMath.atan2(v2.y,v2.x);
double angle = smallestAngle ;
// If smallestAngle < 0 then it is a counterclockwise angle.
if(smallestAngle < 0){
if(clockwise){ // The angle is CCW but the command is CCW
angle = - ( 2*Math.PI - Math.abs(smallestAngle) ); // In OpenGl when rotating, CW rotation = negative angle
}else{
angle = Math.abs(smallestAngle); // In OpenGl when rotating, CCW rotation = positive angle
}
}else{
if(clockwise){ // The angle is CW and we have a CW command
angle = - Math.abs(smallestAngle); // In OpenGl when rotating, CW rotation = negative angle
}else{ // The angle is CW but we want the CCW command
angle = smallestAngle;//2*Math.PI - smallestAngle;
}
}
this.angle = Angle.valueOf(BigDecimal.valueOf(angle), AngleUnit.RADIAN);
}
public Matrix3d getRotationMatrix(Angle rAngle) {
Matrix3d m = new Matrix3d();
double c = Math.cos(rAngle.doubleValue(AngleUnit.RADIAN));
double s = Math.sin(rAngle.doubleValue(AngleUnit.RADIAN));
double t = 1.0 - c;
// if axis is not already normalised then uncomment this
// double magnitude = Math.sqrt(a1.x*a1.x + a1.y*a1.y + a1.z*a1.z);
// if (magnitude==0) throw error;
// a1.x /= magnitude;
// a1.y /= magnitude;
// a1.z /= magnitude;
m.m00 = c + axis.x * axis.x * t;
m.m11 = c + axis.y * axis.y * t;
m.m22 = c + axis.z * axis.z * t;
double tmp1 = axis.x * axis.y * t;
double tmp2 = axis.z * s;
m.m10 = tmp1 + tmp2;
m.m01 = tmp1 - tmp2;
tmp1 = axis.x * axis.z * t;
tmp2 = axis.y * s;
m.m20 = tmp1 - tmp2;
m.m02 = tmp1 + tmp2;
tmp1 = axis.y * axis.z * t;
tmp2 = axis.x * s;
m.m21 = tmp1 + tmp2;
m.m12 = tmp1 - tmp2;
return m;
}
public Length getLength(){
return radius.multiply(angle.value(AngleUnit.RADIAN).abs());
}
public Tuple6b point(double i){
Matrix3d rMat = getRotationMatrix(angle.multiply(BigDecimal.valueOf(i)));
Point3d res = new Point3d(pStart);
res.sub(pCenter);
rMat.transform(res);
res.add(pCenter);
// along axis motion
Vector3d delta = new Vector3d(pEnd.x - pStart.x, pEnd.y - pStart.y, pEnd.z - pStart.z);
double scale = delta.dot(axis) * i;
delta.x = scale*axis.x;
delta.y = scale*axis.y;
delta.z = scale*axis.z;
res.add(delta);
return new Tuple6b(res.x, res.y, res.z, unit);
}
public static void main(String[] args) {
Tuple6b start = new Tuple6b(Length.valueOf(-3, LengthUnit.MILLIMETRE), Length.ZERO, Length.ZERO);
Tuple6b center = new Tuple6b(Length.ZERO, Length.ZERO, Length.ZERO);
Tuple6b end = new Tuple6b(Length.valueOf(3, LengthUnit.MILLIMETRE), Length.ZERO, Length.ZERO);
Tuple6b startx = new Tuple6b(Length.ZERO, Length.valueOf(-3, LengthUnit.MILLIMETRE), Length.ZERO);
Tuple6b endx = new Tuple6b(Length.ZERO, Length.valueOf(3, LengthUnit.MILLIMETRE), Length.ZERO);
Vector3d axisx = new Vector3d(1,0,0);
Vector3d axisy = new Vector3d(0,1,0);
Vector3d axisz = new Vector3d(0,0,1);
Arc3b arcx = new Arc3b(startx, center, endx, axisx, true);
Arc3b arcy = new Arc3b(start, center, end, axisy, true);
Arc3b arcz = new Arc3b(start, center, end, axisz, true);
Tuple6b px = arcx.point(0.5);
Tuple6b py = arcy.point(0.5);
Tuple6b pz = arcz.point(0.5);
System.err.println();
}
public BoundingTuple6b getBound(){
return null;
}
/**
* @return the start
*/
public Tuple6b getStart() {
return start;
}
/**
* @return the center
*/
public Tuple6b getCenter() {
return center;
}
/**
* @return the end
*/
public Tuple6b getEnd() {
return end;
}
/**
* @return the axis
*/
public Vector3d getAxis() {
return axis;
}
/**
* @return the angle
*/
public Angle getAngle() {
return angle;
}
/**
* @return the radius
*/
public Length getRadius() {
return radius;
}
}