AbstractStraightGeometryBuilder.java

package org.goko.core.gcode.rs274ngcv3.jogl.renderer.builder;

import java.util.ArrayList;
import java.util.List;

import javax.vecmath.Matrix4d;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import org.goko.core.common.exception.GkException;
import org.goko.core.common.measure.quantity.Angle;
import org.goko.core.common.measure.quantity.AngleUnit;
import org.goko.core.gcode.element.IInstructionType;
import org.goko.core.gcode.rs274ngcv3.context.GCodeContext;
import org.goko.core.gcode.rs274ngcv3.instruction.AbstractStraightInstruction;
import org.goko.core.math.Tuple6b;
import org.goko.tools.viewer.jogl.preferences.JoglViewerPreference;
import org.goko.tools.viewer.jogl.service.JoglUtils;

public abstract class AbstractStraightGeometryBuilder<T extends AbstractStraightInstruction> extends AbstractInstructionGeometryBuilder<T> {

	/**
	 * Constructor
	 * @param type
	 */
	public AbstractStraightGeometryBuilder(IInstructionType type) {
		super(type);
	}

	/** (inheritDoc)
	 * @see org.goko.core.gcode.rs274ngcv3.jogl.renderer.builder.AbstractInstructionGeometryBuilder#buildInstructionGeometry(org.goko.core.gcode.rs274ngcv3.context.GCodeContext, org.goko.core.gcode.element.IInstruction)
	 */
	@Override
	public List<Point3d> buildInstructionGeometry(GCodeContext context, T instruction) throws GkException {
		JoglViewerPreference settings = JoglViewerPreference.getInstance();		
		if(instruction != null){			
			if(settings.isRotaryAxisEnabled()){
				GCodeContext postContext = new GCodeContext(context);
				instruction.apply(postContext);
				// The complete angle around the 4th axis
				Angle deltaAngle = Angle.ZERO;
				// FIXME Use a setting to define the 4th axis and then do a dynamic angle detection around the axe A,B or C
				if( postContext.getA() != null){
					deltaAngle = context.getA().subtract(postContext.getA());
				}

				if(deltaAngle.abs().lowerThan(Angle.valueOf("0.0001", AngleUnit.DEGREE_ANGLE))){
					return renderLinearLine(context, instruction);
				}else{
					return renderRotaryLine(context, instruction);
				}
			}else{
				return renderLinearLine(context, instruction);
			}
		}
		return new ArrayList<Point3d>();
	}

//verifier la prise en compte des coordinates systems dans toutes les commandes

	private List<Point3d> renderLinearLine(GCodeContext context, T instruction) throws GkException {
		JoglViewerPreference settings = JoglViewerPreference.getInstance();
		List<Point3d> vertices = new ArrayList<Point3d>();
		Tuple6b offset = context.getCoordinateSystemData(context.getCoordinateSystem());
		Tuple6b startTuple = context.getPosition();//new Tuple6b(context.getX(), context.getY(), context.getZ(), context.getA(), context.getB(), context.getC());
		GCodeContext postContext = new GCodeContext(context);
		instruction.apply(postContext);
		Tuple6b 		endTuple 	=  postContext.getPosition();
		//Tuple6b endTuple   = new Tuple6b(instruction.getX(), instruction.getY(), instruction.getZ(), instruction.getA(), instruction.getB(), instruction.getC());

		startTuple = startTuple.add(offset);
		endTuple = endTuple.add(offset);

		Point3d startPoint 	= startTuple.toPoint3d(JoglUtils.JOGL_UNIT);
		Point3d endPoint 	=   endTuple.toPoint3d(JoglUtils.JOGL_UNIT);

		if(settings.isRotaryAxisEnabled()){
			// It's a simple line, but it doesn't mean the rotary (A, B or C) value is at 0
			if(postContext.getA() != null){
				Point3d endAngle 	=   endTuple.angleToPoint3d(AngleUnit.DEGREE_ANGLE);

				Matrix4d rotationMatrix = new Matrix4d();
				rotateMatrix(rotationMatrix, Math.toRadians(endAngle.x));

				if(settings.getRotaryAxisPosition() != null){
					startPoint.sub(settings.getRotaryAxisPosition().toPoint3d(JoglUtils.JOGL_UNIT));
					endPoint.sub(settings.getRotaryAxisPosition().toPoint3d(JoglUtils.JOGL_UNIT));
				}
				rotationMatrix.transform(startPoint);
				rotationMatrix.transform(endPoint);
				if(settings.getRotaryAxisPosition() != null){
					startPoint.add(settings.getRotaryAxisPosition().toPoint3d(JoglUtils.JOGL_UNIT));
					endPoint.add(settings.getRotaryAxisPosition().toPoint3d(JoglUtils.JOGL_UNIT));
				}
			}
		}


		vertices.add(startPoint);
		vertices.add(endPoint);
		return vertices;
	}

	private void rotateMatrix(Matrix4d matrix, double angleRadians){
		JoglViewerPreference settings = JoglViewerPreference.getInstance();
		switch(settings.getRotaryAxisDirection()){
			case X:matrix.rotX( angleRadians );
			break;
			case Y:matrix.rotY( angleRadians );
			break;
			case Z:matrix.rotZ( angleRadians );
			break;
			default:matrix.rotY( angleRadians );
		}
	}
	private List<Point3d> renderRotaryLine(GCodeContext context, T instruction) throws GkException {
		JoglViewerPreference settings = JoglViewerPreference.getInstance();
		ArrayList<Point3d> vertices = new ArrayList<Point3d>();

		Tuple6b startTuple = context.getPosition();//new Tuple6b(context.getX(), context.getY(), context.getZ(), context.getA(), context.getB(), context.getC());
		GCodeContext postContext = new GCodeContext(context);
		instruction.apply(postContext);
		Tuple6b 		endTuple 	=  postContext.getPosition();
		
		Point3d startPoint 	= startTuple.toPoint3d(JoglUtils.JOGL_UNIT);
		Point3d endPoint 	=   endTuple.toPoint3d(JoglUtils.JOGL_UNIT);

		Matrix4d rotationMatrix = new Matrix4d();

		double deltaAngleDeg = 0;

		Vector3d deltaVector = new Vector3d(endPoint);
		deltaVector.sub(startPoint);

		Point3d startAngle3d	= startTuple.angleToPoint3d(AngleUnit.DEGREE_ANGLE);
		Point3d endAngle3d	 	=   endTuple.angleToPoint3d(AngleUnit.DEGREE_ANGLE);
		double startAngle = startAngle3d.x;
		deltaAngleDeg = endAngle3d.x - startAngle3d.x;

		int nbPoints = (int) Math.max(1,Math.abs(deltaAngleDeg) / 5);
		deltaVector.scale(1.0/nbPoints);
		double startAngleRad = Math.toRadians(startAngle);
		double stepAngleRad = Math.toRadians(deltaAngleDeg) / nbPoints;

		Point3d rotaryAxisPosition = settings.getRotaryAxisPosition().toPoint3d(JoglUtils.JOGL_UNIT);
		Point3d generatorVector = new Point3d(startPoint);
		generatorVector.sub(rotaryAxisPosition);
		Point3d tmpVector 		= new Point3d(generatorVector);


		for(int i = 0; i <= nbPoints; i++){
			rotationMatrix.setIdentity();
			rotateMatrix(rotationMatrix, startAngleRad + i*stepAngleRad);
			rotationMatrix.transform(generatorVector,tmpVector);
			tmpVector.add(rotaryAxisPosition);
			vertices.add( new Point3d(tmpVector.x, tmpVector.y, tmpVector.z) );
			generatorVector.add( deltaVector );
		}
		return vertices;
	}
}