Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- //world rotation * up
- Vector3f spatialY = control.getPhsicsRotation().mult(Vector3f.UNIT_Y);
- Vector3f cross = spatialY.crossLocal(Vector3f.UNIT_Y).normalizeLocal();
- Vector3f startVector;
- Vector3f currentVector;
- Vector3f endVector;
- Vector3f upVector;
- Vector3f tempVector;
- Vector3f spatialY;
- Vector3f cross;
- Vector3f v;
- public void autoLevel(String action, boolean isPressed){
- autoLevel = (autoLevel == false);
- if (autoLevel){
- upVector = Vector3f.UNIT_Y;
- //start
- float[] rad = new float[3];
- spatial.getWorldRotation().toAngles(rad);//local
- //start vector
- startVector = new Vector3f(rad[0], rad[1], rad[2]);
- //current vector
- currentVector = new Vector3f(rad[0], rad[1], rad[2]);
- //end vector
- endVector = new Vector3f(rad[0], rad[1], rad[2]);
- //test
- spatialY = endVector.mult(Vector3f.UNIT_Y);
- cross = spatialY.cross(Vector3f.UNIT_Y);
- }
- }
- private void doAutoLevel(float tpf){
- //current
- float[] rad = new float[3];
- spatial.getWorldRotation().toAngles(rad);
- currentVector = new Vector3f(rad[0], rad[1], rad[2]);
- //if cross it the difference between my current and end rotations...
- massControl.rotate(cross);
- autoLevel = false;//shut off for testing
- if (true) return;
- //Or...
- if (currentVector.x < endVector.x - 0.01f)
- pitchDown("PitchUp", true);
- else if (currentVector.x > endVector.x + 0.01f)
- pitchUp("PitchDown", true);
- else{
- currentVector.x = endVector.x;
- pitchDown("PitchDown", false);
- pitchUp("PitchUp", false);
- massControl.setSpin(0, 0f);
- }
- if (currentVector.y < endVector.y - 0.01f)
- yawRight("YawRight", true);
- else if (currentVector.y > endVector.y + 0.01f)
- yawLeft("YawLeft", true);
- else{
- massControl.setSpin(1, 0f);
- yawLeft("YawLeft", false);
- yawRight("YawRight", false);
- currentVector.y = endVector.y;
- }
- //
- if (currentVector.z < endVector.z - 0.01f)
- rollLeft("RollLeft", true);
- else if (currentVector.z > endVector.z + 0.01f)
- rollRight("RollRight", true);
- else{
- massControl.setSpin(2, 0f);
- rollLeft("RollLeft", false);
- rollRight("RollRight", false);
- currentVector.z = endVector.z;
- }
- }//doAutoLevel
- //And the gravity routine that I found after searching for years...
- private synchronized void doGravity(){
- //delay -= 1.0f;
- //if (delay > 0) return;
- //delay = app.getTimer().getFrameRate();
- //ArrayList<Node> bodyList = getBodyList();
- List list = getMasses(rootNode);
- for (int a = 0;a < list.size();a++){
- Node p1 = (Node)list.get(a);
- MassControl massControl = p1.getControl(MassControl.class);
- float m1 = massControl.getMass();
- //System.out.println("EnvironmentalControl.doGravity()" + ", processing p1=" + p1.getName());
- Vector3f acc = new Vector3f();
- if (list.size() > 1){
- //if (p.getName().equals("Moon")) System.out.print("acc=");
- for (int b = 0;b < list.size();b++){
- Node p2 = (Node)list.get(b);
- if (p1.equals(p2)) continue;
- Vector3f unit = new Vector3f(p2.getWorldTranslation().subtract(p1.getWorldTranslation()));
- float magnitude = (float)Math.sqrt((unit.x * unit.x) + (unit.y * unit.y) + (unit.z * unit.z));
- float m2 = p2.getControl(MassControl.class).getMass();
- //if (p1.getName().equals("Ship 01") && p2.getName().equals("Earth") || p2.getName().equals("Ship 01") && p1.getName().equals("Earth"))
- //System.out.println("OrbitMaker.doGravity(" + p1.getName() + ")(" + p2.getName() + ") m1=" + m1 + ", m2=" + m2 + ", unit=" + unit + ", magnitude=" + magnitude);//do gravity
- if (magnitude != 0.0f && (m1 * m2 != 0.0)){
- float factor = (G * (
- (m1 * m2) / (magnitude * magnitude * magnitude)
- )) / m1;
- unit = unit.mult(factor);
- //if (p1.getName().equals("Ship 01")) System.out.println("OrbitMaker.doGravity(" + p1.getName() + ")(" + p2.getName() + "): factor=" + factor + ", unit=" + unit + ", acc=" + acc);
- }
- else{
- unit.set(0.0f, 0.0f, 0.0f);
- }
- acc = acc.add(unit.clone());//.mult(tpf));
- //collision check
- if (areClose(p1, p2)){
- collisionQueue.add(new CollisionGroup(p1, p2));
- }
- }
- }
- //if (p.getName().equals("Moon")) System.out.println(acc + ", ");
- //Vector3f velocity = p.getControl(MassControl.class).getVelocity();
- //if (p.getName().equals("Moon")) System.out.println("OrbitMaker.doGravity(" + p.getName() + "): acc=" + acc);
- massControl.addToVelocity(acc);
- }
- //System.out.println();
- }//doGravity
- Matrix GetNewWorldMatrix(Matrix lastFrameMatrix, Vector3 newShipPosition, Vector3 planetCenter)
- {
- Matrix result = Matrix.Identity;
- Vector3 directionShipIsTraveling = lasteFrameMatrix.Forward;
- Vector3 newMatrixUp = newShipPosition - planetCenter;
- Vector3 newMatrixRight = Vector3.Cross(directionShipIsTraveling, newMatrixUp);
- newMatrixUp.Normalize();
- newMatrixRight.Normalize();
- Vector3 newMatrixForward = Vector3.Cross(newMatrixUp, newMatrixRight);
- result.Up = newMatrixUp;
- result.Right = newMatrixRight;
- result.Forward = newMatrixForward;
- result.Translation = newShipPosition;
- return result;
- }
- public void autoLevel(){
- currentMatrix = getNewWorldMatrix(currentMatrix, spatial.getWorldTranslation(), parent.getWorldTranslation());
- }
- //static final int LR = 0;//right/left
- //static final int UD = 1;//up/down
- //static final int FR = 2;//front/back
- public Matrix3f getNewWorldMatrix(Matrix3f oldMatrix, Vector3f currentTranslation, Vector3f parentTranslation){
- Matrix3f result = new Matrix3f();
- //lastFrameMatrix = spatial.getLocalRotation().toRotationMatrix();
- Vector3f walkDirection = oldMatrix.getColumn(FR);
- Vector3f newUp = currentTranslation.subtract(parentTranslation).normalize();
- Vector3f newRight = walkDirection.cross(newUp).normalize();
- Vector3f newForward = newUp.cross(newRight).normalize();
- result.setColumn(LR, newRight); //LR = 0
- result.setColumn(UD, newUp); //UD = 1
- result.setColumn(FR, newForward); //FR = 2
- result.normalizeLocal();
- return result;
- }
- public void initAutoLevel(){
- //start
- float[] rad = new float[3];
- spatial.getWorldRotation().toAngles(rad);//local
- startVector = new Vector3f(rad[0], rad[1], rad[2]);
- //end
- endMatrix = spatial.getWorldRotation().toRotationMatrix();
- endMatrix = getNewWorldMatrix(endMatrix, spatial.getWorldTranslation(), parent.getWorldTranslation());
- //Quaternion endQ = new Quaternion();
- endQ.fromRotationMatrix(endMatrix);
- endQ.toAngles(rad);
- endVector = new Vector3f(rad[0], rad[1], rad[2]);
- //
- getDirection(startVector.x, endVector.x, X);//set dist
- getDirection(startVector.y, endVector.y, Y);
- getDirection(startVector.z, endVector.z, Z);
- autoLevelData[X][DIRECTION] = DONE;
- //autoLevelData[Y][DIRECTION] = DONE;
- autoLevelData[Z][DIRECTION] = DONE;
- solarAppState.autoLevelPanel.initAutoLevelPanel(autoLevelData);
- println();
- }//initAutoLevel
- public Matrix3f getNewWorldMatrix(Matrix3f oldMatrix, Vector3f currentTranslation, Vector3f parentTranslation){
- Matrix3f result = new Matrix3f();
- System.out.println("CC.getNWM()" + ", oldMatrix=" + oldMatrix);
- //lastFrameMatrix = spatial.getLocalRotation().toRotationMatrix();
- Vector3f walkDirection = oldMatrix.getColumn(Z);
- Vector3f newUp = currentTranslation.subtract(parentTranslation).normalize();
- Vector3f newRight = walkDirection.cross(newUp).normalize();
- Vector3f newForward = newUp.cross(newRight).normalize();
- result.setColumn(X, newRight); //LR = 0
- result.setColumn(Y, newUp); //UD = 1
- result.setColumn(Z, newForward); //FR = 2
- result.normalizeLocal();
- System.out.println("CC.getNWM()" + ", result=" + result);
- println();
- return result;
- }//getNewWorldMatrix
- private void doAutoLevel(float tpf){
- doAutoLevelEuler(tpf);
- }
- public void doAutoLevelEuler(float tpf){
- float[] rad = new float[3];
- int dim;
- //int dir;
- int turn = 100000;
- spatial.getWorldRotation().toAngles(rad);//local
- autoLevelData[X][CURRENT] = rad[X];
- autoLevelData[Y][CURRENT] = rad[Y];
- autoLevelData[Z][CURRENT] = rad[Z];
- //Vector3f currentVector = new Vector3f(rad[0], rad[1], rad[2]);
- dim = X;
- //dir = ;//cant do it here
- if ((int)autoLevelData[dim][DIRECTION] != DONE){
- turn = getTurnEuler((int)autoLevelData[dim][DIRECTION], rad[X], spin.x, X);
- if (turn == INCREASE){
- //println("CC.doAutoLevel()" + ", pitch up" + ", dir=" + (int)autoLevelData[dim][DIRECTION] + ", turn=" + (int)autoLevelData[dim][TURN]);
- pitchUp("PitchUp", true);
- pitchDown("PitchDown", false);
- }
- else if (turn == DECREASE){
- //println("CC.doAutoLevel()" + ", pitch down" + ", dir=" + (int)autoLevelData[dim][DIRECTION] + ", turn=" + (int)autoLevelData[dim][TURN]);
- pitchUp("PitchUp", false);
- pitchDown("PitchDown", true);
- }
- else if (turn == STOPTHRUST){
- //println("CC.doAutoLevel()" + ", stop pitching" + ", dir=" + YDir);
- pitchUp("PitchUp", false);
- pitchDown("PitchDown", false);
- }
- else if (turn == COAST){
- //autoLevelData[dim][DIRECTION] = DONE;
- }
- else if (turn == STOP){
- //println("CC.doAutoLevel()" + ", stop pitch" + ", dir=" + XDir);
- pitchUp("PitchUp", false);
- pitchDown("PitchDown", false);
- //stopSpin("StopSpin", true);
- autoLevelData[dim][TURN] = DONE;
- autoLevelData[dim][DIRECTION] = DONE;
- }
- }
- //if (true) return;
- //yaw
- //if (currentVector.y != endVector.y){
- dim = Y;
- if (((int)autoLevelData[dim][DIRECTION]) != DONE){
- turn = getTurnEuler((int)autoLevelData[dim][DIRECTION], rad[Y], spin.y, Y);
- if (turn == INCREASE){
- //println("CC.doAutoLevel()" + ", yaw right" + ", dir=" + YDir);
- yawRight("YawRight", true);
- yawLeft("YawLeft", false);
- }
- else if (turn == DECREASE){
- //println("CC.doAutoLevel()" + ", yaw left" + ", dir=" + YDir);
- yawRight("YawRight", false);
- yawLeft("YawLeft", true);
- }
- else if (turn == STOPTHRUST){
- //println("CC.doAutoLevel()" + ", stop yawing" + ", dir=" + YDir);
- yawRight("YawRight", false);
- yawLeft("YawLeft", false);
- }
- else if (turn == COAST){
- //autoLevelData[dim][DIRECTION] = DONE;
- //println("CC.doAutoLevel()" + ", coast yawing" + ", dir=" + YDir);
- }
- else if (turn == STOP){
- yawRight("YawRight", false);
- yawLeft("YawLeft", false);
- //stopSpin("StopSpin", true);
- autoLevelData[dim][TURN] = DONE;
- autoLevelData[dim][DIRECTION] = DONE;
- println("CC.doAutoLevel()" + ", stop yaw" + ", dir=" + (int)autoLevelData[dim][DIRECTION]);
- }
- }
- //roll
- //if (currentVector.z != endVector.z){
- dim = Z;
- if (((int)autoLevelData[dim][DIRECTION]) != DONE){
- turn = getTurnEuler((int)autoLevelData[dim][DIRECTION], rad[Z], spin.z, Z);
- if (turn == INCREASE){
- //println("CC.doAutoLevel()" + ", roll right" + ", dir=" + ZDir);
- rollRight("RollRight", true);
- rollLeft("rollLeft", false);
- }
- else if (turn == DECREASE){
- //println("CC.doAutoLevel()" + ", roll left" + ", dir=" + ZDir);
- rollRight("RollRight", false);
- rollLeft("RollLeft", true);
- }
- else if (turn == STOPTHRUST){
- //println("CC.doAutoLevel()" + ", stop rolling" + ", dir=" + ZDir);
- rollRight("RollRight", false);
- rollLeft("rollLeft", false);
- }
- else if (turn == COAST){
- //println("CC.doAutoLevel()" + ", continue rolling" + ", dir=" + ZDir);
- //autoLevelData[dim][DIRECTION] = DONE;
- }
- else if (turn == STOP){
- //println("CC.doAutoLevel()" + "stop roll" + ", dir=STOP");
- rollRight("RollRight", false);
- rollLeft("RollLeft", false);
- //stopSpin("StopSpin", true);
- autoLevelData[dim][TURN] = DONE;
- autoLevelData[dim][DIRECTION] = DONE;
- }
- }//done?
- }//doAutoLevel
- private void getDirection(float start, float end, int dim){
- if (start < end){//inc
- if (end < start + PI){
- autoLevelData[dim][DIRECTION] = INCREASE;
- autoLevelData[dim][START] = start;//update start
- autoLevelData[dim][END] = (end - start);//update end
- float dist = autoLevelData[dim][DIST] = (end - start);//total dist//10f
- autoLevelData[dim][MAX] = (dist * 0.1f);
- }
- else if (end > start - PI){
- autoLevelData[dim][DIRECTION] = INCREASE2;
- autoLevelData[dim][START] = start;
- autoLevelData[dim][END] = ((end - start) - PI2);
- float dist = autoLevelData[dim][DIST] = ((end - start) - PI2);//total dist///10f
- autoLevelData[dim][MAX] = (dist * 0.1f);
- }
- }
- else if (start > end){//dec
- if (end > start - PI){
- autoLevelData[dim][DIRECTION] = DECREASE;
- autoLevelData[dim][START] = start;//update start
- autoLevelData[dim][END] = (end - start);//update end
- float dist = autoLevelData[dim][DIST] = (end - start);//total dist///10f
- autoLevelData[dim][MAX] = (dist * 0.1f);
- }
- else if (end < start - PI){//???
- autoLevelData[dim][DIRECTION] = DECREASE2;
- autoLevelData[dim][START] = start;
- autoLevelData[dim][END] = ((end - start) + PI2);
- float dist = autoLevelData[dim][DIST] = ((end - start) + PI2);//total dist//10f
- autoLevelData[dim][MAX] = (dist * 0.1f);
- }
- }
- else{
- println("CC.getDir()" + ", Error: dim=" + dim + ", start=" + start + ", end=" + end + ", dir=" + DONE);
- autoLevelData[dim][DIRECTION] = DONE;
- autoLevelData[dim][START] = start;
- autoLevelData[dim][END] = end;
- autoLevelData[dim][DIST] = (end - start);//total dist
- autoLevelData[dim][MAX] = 0f;
- }
- println("CC.getDirection(euler)" + ", dim=" + dim + ", start=" + autoLevelData[dim][START] + ", end=" + autoLevelData[dim][END] + ", dist=" + autoLevelData[dim][DIST] + ", max=" + autoLevelData[dim][MAX] + ", ---start=" + autoLevelData[dim][START] * FastMath.RAD_TO_DEG + ", end=" + autoLevelData[dim][END] * FastMath.RAD_TO_DEG + ", dist=" + autoLevelData[dim][DIST] * FastMath.RAD_TO_DEG + ", max=" + autoLevelData[dim][MAX] + ", dir=" + (int)autoLevelData[dim][DIRECTION]);
- }//getDirection
- private int getTurnEuler(int dir, float current, float spin, int dim){
- float remainder = 0f;
- float start = autoLevelData[dim][START];
- float end = autoLevelData[dim][END];
- if (dir != STOP){
- if (dir == INCREASE){//inc
- remainder = autoLevelData[dim][REM] = (autoLevelData[dim][DIST] - (current - start));
- System.out.println("CC.getTurn2()1.1" + ", dim=" + dim + ", dir=" + dir + ", start=" + start * FastMath.RAD_TO_DEG + ", current=" + (current * FastMath.RAD_TO_DEG) + ", end=" + end * FastMath.RAD_TO_DEG + ", remainder=" + remainder * FastMath.RAD_TO_DEG + ", max=" + autoLevelData[dim][MAX] * FastMath.RAD_TO_DEG + ", == var=0.01" + ", spin=" + spin);
- if (isBiggerThan(remainder, autoLevelData[dim][MAX]))
- remainder = autoLevelData[dim][MAX];
- //turn......................
- if (remainder == 0f){//autoLevelData[dim][DIST] < 0f ? -0.01f:0.01f) && spin == 0.0f
- //if (current >= autoLevelData[dim][DIST]){
- autoLevelData[dim][TURN] = STOP;
- }
- else if (isSmallerThan(spin, remainder - 0.01f))//1:1// + 0.001
- autoLevelData[dim][TURN] = INCREASE;
- else if (isBiggerThan(spin, remainder + 0.01f))
- autoLevelData[dim][TURN] = DECREASE;
- else{
- if (autoLevelData[dim][TURN] != STOPTHRUST && autoLevelData[dim][TURN] != COAST)
- autoLevelData[dim][TURN] = STOPTHRUST;//10
- else
- autoLevelData[dim][TURN] = COAST;//11
- }
- autoLevelData[dim][PERCENT] = (autoLevelData[dim][DIST] - (current - start))/autoLevelData[dim][DIST];
- }
- else if (dir == DECREASE){//dec
- remainder = (autoLevelData[dim][DIST] - (current - start));//
- autoLevelData[dim][REM] = remainder;
- System.out.println("CC.getTurn2()2.1" + ", dim=" + dim + ", dir=" + dir + ", start=" + start * FastMath.RAD_TO_DEG + ", current=" + (current * FastMath.RAD_TO_DEG) + ", end=" + end * FastMath.RAD_TO_DEG + ", max=" + autoLevelData[dim][MAX] * FastMath.RAD_TO_DEG + ", remainder=" + remainder * FastMath.RAD_TO_DEG + ", == var=0.01" + ", spin=" + spin);
- if (isBiggerThan(remainder, autoLevelData[dim][MAX])){
- System.out.println("CC.getTurn2()2.2a" + ", dim=" + dim + ", dir=" + dir + ", remainder=" + remainder + ", >> max=" + autoLevelData[dim][MAX] + ", max deg=" + autoLevelData[dim][MAX] * FastMath.RAD_TO_DEG);
- remainder = autoLevelData[dim][MAX];
- }
- else{
- System.out.println("CC.getTurn2()2.2b" + ", dim=" + dim + ", dir=" + dir + ", remainder=" + remainder * FastMath.RAD_TO_DEG + ", << max=" + autoLevelData[dim][MAX] * FastMath.RAD_TO_DEG + ", remainder=" + remainder * FastMath.RAD_TO_DEG);
- }
- //turn................
- //if (isSmallerThan(remainder, autoLevelData[dim][DIST] < 0f ? -0.01f:0.01f) && spin == 0.0f){
- if (remainder == 0f){
- autoLevelData[dim][TURN] = STOP;
- System.out.println("CC.getTurn2()2.3x" + ", dim=" + dim + ", dir=" + dir + ", remainder=" + remainder * FastMath.RAD_TO_DEG + ", == var=0.01" + ", spin=" + spin * FastMath.RAD_TO_DEG + ", turn=" + (int)autoLevelData[dim][TURN]);
- }
- else if (isSmallerThan(spin, remainder + 0.01f)){//1:1//
- autoLevelData[dim][TURN] = DECREASE;
- System.out.println("CC.getTurn2()2.3a" + ", dim=" + dim + ", dir=" + dir + ", spin=" + spin/* * FastMath.RAD_TO_DEG*/ + ", << remainder=" + remainder/* * FastMath.RAD_TO_DEG*/ + ", turn=" + (int)autoLevelData[dim][TURN]);
- }
- else if (isBiggerThan(spin, remainder - 0.01f)){//1:1//
- autoLevelData[dim][TURN] = INCREASE;
- System.out.println("CC.getTurn2()2.3b" + ", dim=" + dim + ", dir=" + dir + ", spin=" + spin * FastMath.RAD_TO_DEG + ", >> remainder=" + remainder * FastMath.RAD_TO_DEG + ", turn=" + (int)autoLevelData[dim][TURN]);
- }
- else{//spin != 0.0f//
- if (autoLevelData[dim][TURN] != STOPTHRUST && autoLevelData[dim][TURN] != COAST)
- autoLevelData[dim][TURN] = STOPTHRUST;//10
- else
- autoLevelData[dim][TURN] = COAST;
- System.out.println("CC.getTurn2()2.3c" + ", dim=" + dim + ", dir=" + dir + ", spin=" + spin * FastMath.RAD_TO_DEG + ", == remainder=" + remainder * FastMath.RAD_TO_DEG + ", turn=" + autoLevelData[dim][TURN]);
- }
- autoLevelData[dim][PERCENT] = (autoLevelData[dim][DIST] - (current - start))/autoLevelData[dim][DIST];
- }
- //
- else if (dir == INCREASE2){//inc2
- remainder = (autoLevelData[dim][DIST] - (current - start));
- autoLevelData[dim][REM] = remainder;
- System.out.println("CC.getTurn2()3.1" + ", dim=" + dim + ", dir=" + dir + ", start=" + start * FastMath.RAD_TO_DEG + ", current=" + (current * FastMath.RAD_TO_DEG) + ", end=" + end * FastMath.RAD_TO_DEG + ", remainder=" + remainder * FastMath.RAD_TO_DEG + ", max=" + autoLevelData[dim][MAX] * FastMath.RAD_TO_DEG + ", == var=0.01" + ", spin=" + spin);
- if (isBiggerThan(remainder, autoLevelData[dim][MAX]))
- remainder = autoLevelData[dim][MAX];
- //turn....................
- float sign = autoLevelData[dim][DIST] < 0f ? -0.01f:0.01f;
- if (isSmallerThan(remainder, 0.05f) && isSmallerThan(spin, 0.001f)){//autoLevelData[dim][DIST] < 0f ? -0.0001f:0.0001f
- //if (remainder < 0.6f * sign){
- autoLevelData[dim][TURN] = STOP;
- }
- else if (isBiggerThan(spin, remainder - (0.01f)))//1:1// + 0.001
- autoLevelData[dim][TURN] = DECREASE;
- else if (isSmallerThan(spin, remainder + (0.01f)))//1:1// + 0.001
- autoLevelData[dim][TURN] = INCREASE;
- else{
- if (autoLevelData[dim][TURN] != STOPTHRUST && autoLevelData[dim][TURN] != COAST)
- autoLevelData[dim][TURN] = STOPTHRUST;
- else
- autoLevelData[dim][TURN] = COAST;
- }
- autoLevelData[dim][PERCENT] = (autoLevelData[dim][DIST] - (current - start))/autoLevelData[dim][DIST];
- }
- else if (dir == DECREASE2){//dec2
- remainder = (autoLevelData[dim][DIST] - (current - start));
- autoLevelData[dim][REM] = remainder;
- //System.out.println("CC.getTurn2()4.1" + ", dim=" + dim + ", dir=" + dir + ", start=" + start * FastMath.RAD_TO_DEG + ", current=" + current * FastMath.RAD_TO_DEG + ", end=" + end * FastMath.RAD_TO_DEG + ", remainder=" + remainder * FastMath.RAD_TO_DEG + ", max=" + autoLevelData[dim][MAX] * FastMath.RAD_TO_DEG + ", == var=0.01" + ", spin=" + spin);
- if (isBiggerThan(remainder, autoLevelData[dim][MAX]))
- remainder = autoLevelData[dim][MAX];
- //turn....................
- //if (isSmallerThan(remainder, autoLevelData[dim][DIST] < 0f ? -0.01f:0.01f) && spin == 0.0f){
- if (remainder == 0f){
- autoLevelData[dim][TURN] = STOP;
- }
- else if (isSmallerThan(spin, remainder + 0.01f))//1:1// + 0.001
- autoLevelData[dim][TURN] = INCREASE;
- else if (isBiggerThan(spin, remainder - 0.01f))//1:1// + 0.001
- autoLevelData[dim][TURN] = DECREASE;
- else{
- if (autoLevelData[dim][TURN] != STOPTHRUST && autoLevelData[dim][TURN] != COAST)
- autoLevelData[dim][TURN] = STOPTHRUST;
- else
- autoLevelData[dim][TURN] = COAST;
- }
- autoLevelData[dim][PERCENT] = (autoLevelData[dim][DIST] - (current - start))/autoLevelData[dim][DIST];
- }
- else{
- System.out.println("CC.getTurn2()" + ", error: should not get here. dir=(" + dir + ")");
- //turn = 1000000;
- //remainder = 0f;
- }
- //if (dim == X)
- System.out.println("CC.getTurn2()5x " + ", dim=" + dim + ", dir=" + (int)autoLevelData[dim][DIRECTION] + ", start=" + (autoLevelData[dim][START] * FastMath.RAD_TO_DEG) + ", current=" + (current * FastMath.RAD_TO_DEG) + " " + getDirCode((int)autoLevelData[dim][TURN]) + ", end=" + (autoLevelData[dim][END] * FastMath.RAD_TO_DEG) + ", remainder=" + (remainder * FastMath.RAD_TO_DEG) + ", dist=" + (autoLevelData[dim][DIST] * FastMath.RAD_TO_DEG) + ", max=" + autoLevelData[dim][MAX] * FastMath.RAD_TO_DEG + ", spin=" + spin * FastMath.RAD_TO_DEG + ", percent=" + autoLevelData[dim][PERCENT] + ", dim=" + dim + ", turn=" + (int)autoLevelData[dim][TURN]);
- }//dir not stop
- return((int)autoLevelData[dim][TURN]);
- }//getTurn2
- public boolean isBiggerThan(float f1, float f2){
- boolean b = false;
- String tdir = "--";
- if ((f1 >= 0f && f2 > 0f) || (f1 <= 0f && f2 < 0f)){
- if (Math.abs(f1) > Math.abs(f2)){
- b = true;
- tdir = ">>";
- }
- }
- //System.out.println("CC.isBiggerThan()" + ", f1=" + f1 + " " + tdir + " " + "f2=" + f2 + "=" + b);
- return (b);
- }
- public boolean isSmallerThan(float f1, float f2){
- boolean b = false;
- String tdir = "--";
- if ((f1 >= 0f && f2 > 0f) || (f1 <= 0f && f2 < 0f)){
- if (Math.abs(f1) < Math.abs(f2)){
- b = true;
- tdir = "<<";
- }
- }
- System.out.println("CC.isSmallerThan()" + ", f1=" + f1 + " " + tdir + " " + "f2=" + f2 + "=" + b);
- return (b);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement