Advertisement
Guest User

Untitled

a guest
Apr 23rd, 2017
59
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 24.45 KB | None | 0 0
  1. //world rotation * up
  2. Vector3f spatialY = control.getPhsicsRotation().mult(Vector3f.UNIT_Y);
  3. Vector3f cross = spatialY.crossLocal(Vector3f.UNIT_Y).normalizeLocal();
  4.  
  5. Vector3f startVector;
  6. Vector3f currentVector;
  7. Vector3f endVector;
  8. Vector3f upVector;
  9. Vector3f tempVector;
  10. Vector3f spatialY;
  11. Vector3f cross;
  12. Vector3f v;
  13. public void autoLevel(String action, boolean isPressed){
  14. autoLevel = (autoLevel == false);
  15. if (autoLevel){
  16. upVector = Vector3f.UNIT_Y;
  17. //start
  18. float[] rad = new float[3];
  19. spatial.getWorldRotation().toAngles(rad);//local
  20. //start vector
  21. startVector = new Vector3f(rad[0], rad[1], rad[2]);
  22. //current vector
  23. currentVector = new Vector3f(rad[0], rad[1], rad[2]);
  24. //end vector
  25. endVector = new Vector3f(rad[0], rad[1], rad[2]);
  26. //test
  27. spatialY = endVector.mult(Vector3f.UNIT_Y);
  28. cross = spatialY.cross(Vector3f.UNIT_Y);
  29. }
  30. }
  31.  
  32.  
  33. private void doAutoLevel(float tpf){
  34. //current
  35. float[] rad = new float[3];
  36. spatial.getWorldRotation().toAngles(rad);
  37. currentVector = new Vector3f(rad[0], rad[1], rad[2]);
  38. //if cross it the difference between my current and end rotations...
  39. massControl.rotate(cross);
  40. autoLevel = false;//shut off for testing
  41. if (true) return;
  42. //Or...
  43. if (currentVector.x < endVector.x - 0.01f)
  44. pitchDown("PitchUp", true);
  45. else if (currentVector.x > endVector.x + 0.01f)
  46. pitchUp("PitchDown", true);
  47. else{
  48. currentVector.x = endVector.x;
  49. pitchDown("PitchDown", false);
  50. pitchUp("PitchUp", false);
  51. massControl.setSpin(0, 0f);
  52. }
  53.  
  54. if (currentVector.y < endVector.y - 0.01f)
  55. yawRight("YawRight", true);
  56. else if (currentVector.y > endVector.y + 0.01f)
  57. yawLeft("YawLeft", true);
  58. else{
  59. massControl.setSpin(1, 0f);
  60. yawLeft("YawLeft", false);
  61. yawRight("YawRight", false);
  62. currentVector.y = endVector.y;
  63. }
  64. //
  65. if (currentVector.z < endVector.z - 0.01f)
  66. rollLeft("RollLeft", true);
  67. else if (currentVector.z > endVector.z + 0.01f)
  68. rollRight("RollRight", true);
  69. else{
  70. massControl.setSpin(2, 0f);
  71. rollLeft("RollLeft", false);
  72. rollRight("RollRight", false);
  73. currentVector.z = endVector.z;
  74. }
  75. }//doAutoLevel
  76.  
  77. //And the gravity routine that I found after searching for years...
  78.  
  79. private synchronized void doGravity(){
  80. //delay -= 1.0f;
  81. //if (delay > 0) return;
  82. //delay = app.getTimer().getFrameRate();
  83. //ArrayList<Node> bodyList = getBodyList();
  84. List list = getMasses(rootNode);
  85. for (int a = 0;a < list.size();a++){
  86. Node p1 = (Node)list.get(a);
  87. MassControl massControl = p1.getControl(MassControl.class);
  88. float m1 = massControl.getMass();
  89. //System.out.println("EnvironmentalControl.doGravity()" + ", processing p1=" + p1.getName());
  90. Vector3f acc = new Vector3f();
  91. if (list.size() > 1){
  92. //if (p.getName().equals("Moon")) System.out.print("acc=");
  93. for (int b = 0;b < list.size();b++){
  94. Node p2 = (Node)list.get(b);
  95. if (p1.equals(p2)) continue;
  96. Vector3f unit = new Vector3f(p2.getWorldTranslation().subtract(p1.getWorldTranslation()));
  97. float magnitude = (float)Math.sqrt((unit.x * unit.x) + (unit.y * unit.y) + (unit.z * unit.z));
  98. float m2 = p2.getControl(MassControl.class).getMass();
  99. //if (p1.getName().equals("Ship 01") && p2.getName().equals("Earth") || p2.getName().equals("Ship 01") && p1.getName().equals("Earth"))
  100. //System.out.println("OrbitMaker.doGravity(" + p1.getName() + ")(" + p2.getName() + ") m1=" + m1 + ", m2=" + m2 + ", unit=" + unit + ", magnitude=" + magnitude);//do gravity
  101. if (magnitude != 0.0f && (m1 * m2 != 0.0)){
  102. float factor = (G * (
  103. (m1 * m2) / (magnitude * magnitude * magnitude)
  104. )) / m1;
  105. unit = unit.mult(factor);
  106. //if (p1.getName().equals("Ship 01")) System.out.println("OrbitMaker.doGravity(" + p1.getName() + ")(" + p2.getName() + "): factor=" + factor + ", unit=" + unit + ", acc=" + acc);
  107. }
  108. else{
  109. unit.set(0.0f, 0.0f, 0.0f);
  110. }
  111. acc = acc.add(unit.clone());//.mult(tpf));
  112. //collision check
  113. if (areClose(p1, p2)){
  114. collisionQueue.add(new CollisionGroup(p1, p2));
  115. }
  116. }
  117. }
  118.  
  119. //if (p.getName().equals("Moon")) System.out.println(acc + ", ");
  120. //Vector3f velocity = p.getControl(MassControl.class).getVelocity();
  121. //if (p.getName().equals("Moon")) System.out.println("OrbitMaker.doGravity(" + p.getName() + "): acc=" + acc);
  122. massControl.addToVelocity(acc);
  123. }
  124. //System.out.println();
  125. }//doGravity
  126.  
  127. Matrix GetNewWorldMatrix(Matrix lastFrameMatrix, Vector3 newShipPosition, Vector3 planetCenter)
  128. {
  129. Matrix result = Matrix.Identity;
  130.  
  131. Vector3 directionShipIsTraveling = lasteFrameMatrix.Forward;
  132. Vector3 newMatrixUp = newShipPosition - planetCenter;
  133. Vector3 newMatrixRight = Vector3.Cross(directionShipIsTraveling, newMatrixUp);
  134. newMatrixUp.Normalize();
  135. newMatrixRight.Normalize();
  136. Vector3 newMatrixForward = Vector3.Cross(newMatrixUp, newMatrixRight);
  137.  
  138. result.Up = newMatrixUp;
  139. result.Right = newMatrixRight;
  140. result.Forward = newMatrixForward;
  141. result.Translation = newShipPosition;
  142.  
  143. return result;
  144.  
  145. }
  146.  
  147. public void autoLevel(){
  148. currentMatrix = getNewWorldMatrix(currentMatrix, spatial.getWorldTranslation(), parent.getWorldTranslation());
  149. }
  150.  
  151. //static final int LR = 0;//right/left
  152. //static final int UD = 1;//up/down
  153. //static final int FR = 2;//front/back
  154.  
  155. public Matrix3f getNewWorldMatrix(Matrix3f oldMatrix, Vector3f currentTranslation, Vector3f parentTranslation){
  156. Matrix3f result = new Matrix3f();
  157. //lastFrameMatrix = spatial.getLocalRotation().toRotationMatrix();
  158. Vector3f walkDirection = oldMatrix.getColumn(FR);
  159. Vector3f newUp = currentTranslation.subtract(parentTranslation).normalize();
  160. Vector3f newRight = walkDirection.cross(newUp).normalize();
  161. Vector3f newForward = newUp.cross(newRight).normalize();
  162. result.setColumn(LR, newRight); //LR = 0
  163. result.setColumn(UD, newUp); //UD = 1
  164. result.setColumn(FR, newForward); //FR = 2
  165. result.normalizeLocal();
  166. return result;
  167. }
  168.  
  169. public void initAutoLevel(){
  170. //start
  171. float[] rad = new float[3];
  172. spatial.getWorldRotation().toAngles(rad);//local
  173. startVector = new Vector3f(rad[0], rad[1], rad[2]);
  174. //end
  175. endMatrix = spatial.getWorldRotation().toRotationMatrix();
  176. endMatrix = getNewWorldMatrix(endMatrix, spatial.getWorldTranslation(), parent.getWorldTranslation());
  177. //Quaternion endQ = new Quaternion();
  178. endQ.fromRotationMatrix(endMatrix);
  179. endQ.toAngles(rad);
  180. endVector = new Vector3f(rad[0], rad[1], rad[2]);
  181. //
  182. getDirection(startVector.x, endVector.x, X);//set dist
  183. getDirection(startVector.y, endVector.y, Y);
  184. getDirection(startVector.z, endVector.z, Z);
  185.  
  186. autoLevelData[X][DIRECTION] = DONE;
  187. //autoLevelData[Y][DIRECTION] = DONE;
  188. autoLevelData[Z][DIRECTION] = DONE;
  189.  
  190. solarAppState.autoLevelPanel.initAutoLevelPanel(autoLevelData);
  191. println();
  192. }//initAutoLevel
  193.  
  194.  
  195.  
  196.  
  197. public Matrix3f getNewWorldMatrix(Matrix3f oldMatrix, Vector3f currentTranslation, Vector3f parentTranslation){
  198. Matrix3f result = new Matrix3f();
  199. System.out.println("CC.getNWM()" + ", oldMatrix=" + oldMatrix);
  200. //lastFrameMatrix = spatial.getLocalRotation().toRotationMatrix();
  201. Vector3f walkDirection = oldMatrix.getColumn(Z);
  202. Vector3f newUp = currentTranslation.subtract(parentTranslation).normalize();
  203. Vector3f newRight = walkDirection.cross(newUp).normalize();
  204. Vector3f newForward = newUp.cross(newRight).normalize();
  205.  
  206. result.setColumn(X, newRight); //LR = 0
  207. result.setColumn(Y, newUp); //UD = 1
  208. result.setColumn(Z, newForward); //FR = 2
  209. result.normalizeLocal();
  210. System.out.println("CC.getNWM()" + ", result=" + result);
  211. println();
  212. return result;
  213. }//getNewWorldMatrix
  214.  
  215.  
  216. private void doAutoLevel(float tpf){
  217.  
  218. doAutoLevelEuler(tpf);
  219. }
  220.  
  221.  
  222. public void doAutoLevelEuler(float tpf){
  223. float[] rad = new float[3];
  224. int dim;
  225. //int dir;
  226. int turn = 100000;
  227. spatial.getWorldRotation().toAngles(rad);//local
  228. autoLevelData[X][CURRENT] = rad[X];
  229. autoLevelData[Y][CURRENT] = rad[Y];
  230. autoLevelData[Z][CURRENT] = rad[Z];
  231. //Vector3f currentVector = new Vector3f(rad[0], rad[1], rad[2]);
  232.  
  233. dim = X;
  234. //dir = ;//cant do it here
  235. if ((int)autoLevelData[dim][DIRECTION] != DONE){
  236. turn = getTurnEuler((int)autoLevelData[dim][DIRECTION], rad[X], spin.x, X);
  237. if (turn == INCREASE){
  238. //println("CC.doAutoLevel()" + ", pitch up" + ", dir=" + (int)autoLevelData[dim][DIRECTION] + ", turn=" + (int)autoLevelData[dim][TURN]);
  239. pitchUp("PitchUp", true);
  240. pitchDown("PitchDown", false);
  241. }
  242. else if (turn == DECREASE){
  243. //println("CC.doAutoLevel()" + ", pitch down" + ", dir=" + (int)autoLevelData[dim][DIRECTION] + ", turn=" + (int)autoLevelData[dim][TURN]);
  244. pitchUp("PitchUp", false);
  245. pitchDown("PitchDown", true);
  246. }
  247. else if (turn == STOPTHRUST){
  248. //println("CC.doAutoLevel()" + ", stop pitching" + ", dir=" + YDir);
  249. pitchUp("PitchUp", false);
  250. pitchDown("PitchDown", false);
  251. }
  252. else if (turn == COAST){
  253. //autoLevelData[dim][DIRECTION] = DONE;
  254. }
  255. else if (turn == STOP){
  256. //println("CC.doAutoLevel()" + ", stop pitch" + ", dir=" + XDir);
  257. pitchUp("PitchUp", false);
  258. pitchDown("PitchDown", false);
  259. //stopSpin("StopSpin", true);
  260. autoLevelData[dim][TURN] = DONE;
  261. autoLevelData[dim][DIRECTION] = DONE;
  262. }
  263. }
  264.  
  265.  
  266. //if (true) return;
  267. //yaw
  268. //if (currentVector.y != endVector.y){
  269. dim = Y;
  270. if (((int)autoLevelData[dim][DIRECTION]) != DONE){
  271. turn = getTurnEuler((int)autoLevelData[dim][DIRECTION], rad[Y], spin.y, Y);
  272. if (turn == INCREASE){
  273. //println("CC.doAutoLevel()" + ", yaw right" + ", dir=" + YDir);
  274. yawRight("YawRight", true);
  275. yawLeft("YawLeft", false);
  276. }
  277. else if (turn == DECREASE){
  278. //println("CC.doAutoLevel()" + ", yaw left" + ", dir=" + YDir);
  279. yawRight("YawRight", false);
  280. yawLeft("YawLeft", true);
  281. }
  282. else if (turn == STOPTHRUST){
  283. //println("CC.doAutoLevel()" + ", stop yawing" + ", dir=" + YDir);
  284. yawRight("YawRight", false);
  285. yawLeft("YawLeft", false);
  286. }
  287. else if (turn == COAST){
  288. //autoLevelData[dim][DIRECTION] = DONE;
  289. //println("CC.doAutoLevel()" + ", coast yawing" + ", dir=" + YDir);
  290. }
  291. else if (turn == STOP){
  292. yawRight("YawRight", false);
  293. yawLeft("YawLeft", false);
  294. //stopSpin("StopSpin", true);
  295. autoLevelData[dim][TURN] = DONE;
  296. autoLevelData[dim][DIRECTION] = DONE;
  297. println("CC.doAutoLevel()" + ", stop yaw" + ", dir=" + (int)autoLevelData[dim][DIRECTION]);
  298. }
  299. }
  300.  
  301.  
  302. //roll
  303. //if (currentVector.z != endVector.z){
  304. dim = Z;
  305. if (((int)autoLevelData[dim][DIRECTION]) != DONE){
  306. turn = getTurnEuler((int)autoLevelData[dim][DIRECTION], rad[Z], spin.z, Z);
  307. if (turn == INCREASE){
  308. //println("CC.doAutoLevel()" + ", roll right" + ", dir=" + ZDir);
  309. rollRight("RollRight", true);
  310. rollLeft("rollLeft", false);
  311. }
  312. else if (turn == DECREASE){
  313. //println("CC.doAutoLevel()" + ", roll left" + ", dir=" + ZDir);
  314. rollRight("RollRight", false);
  315. rollLeft("RollLeft", true);
  316. }
  317. else if (turn == STOPTHRUST){
  318. //println("CC.doAutoLevel()" + ", stop rolling" + ", dir=" + ZDir);
  319. rollRight("RollRight", false);
  320. rollLeft("rollLeft", false);
  321. }
  322. else if (turn == COAST){
  323. //println("CC.doAutoLevel()" + ", continue rolling" + ", dir=" + ZDir);
  324. //autoLevelData[dim][DIRECTION] = DONE;
  325. }
  326. else if (turn == STOP){
  327. //println("CC.doAutoLevel()" + "stop roll" + ", dir=STOP");
  328. rollRight("RollRight", false);
  329. rollLeft("RollLeft", false);
  330. //stopSpin("StopSpin", true);
  331. autoLevelData[dim][TURN] = DONE;
  332. autoLevelData[dim][DIRECTION] = DONE;
  333. }
  334. }//done?
  335. }//doAutoLevel
  336.  
  337.  
  338. private void getDirection(float start, float end, int dim){
  339. if (start < end){//inc
  340. if (end < start + PI){
  341. autoLevelData[dim][DIRECTION] = INCREASE;
  342. autoLevelData[dim][START] = start;//update start
  343. autoLevelData[dim][END] = (end - start);//update end
  344. float dist = autoLevelData[dim][DIST] = (end - start);//total dist//10f
  345. autoLevelData[dim][MAX] = (dist * 0.1f);
  346. }
  347. else if (end > start - PI){
  348. autoLevelData[dim][DIRECTION] = INCREASE2;
  349. autoLevelData[dim][START] = start;
  350. autoLevelData[dim][END] = ((end - start) - PI2);
  351. float dist = autoLevelData[dim][DIST] = ((end - start) - PI2);//total dist///10f
  352. autoLevelData[dim][MAX] = (dist * 0.1f);
  353. }
  354. }
  355. else if (start > end){//dec
  356. if (end > start - PI){
  357. autoLevelData[dim][DIRECTION] = DECREASE;
  358. autoLevelData[dim][START] = start;//update start
  359. autoLevelData[dim][END] = (end - start);//update end
  360. float dist = autoLevelData[dim][DIST] = (end - start);//total dist///10f
  361. autoLevelData[dim][MAX] = (dist * 0.1f);
  362. }
  363. else if (end < start - PI){//???
  364. autoLevelData[dim][DIRECTION] = DECREASE2;
  365. autoLevelData[dim][START] = start;
  366. autoLevelData[dim][END] = ((end - start) + PI2);
  367. float dist = autoLevelData[dim][DIST] = ((end - start) + PI2);//total dist//10f
  368. autoLevelData[dim][MAX] = (dist * 0.1f);
  369. }
  370. }
  371. else{
  372. println("CC.getDir()" + ", Error: dim=" + dim + ", start=" + start + ", end=" + end + ", dir=" + DONE);
  373. autoLevelData[dim][DIRECTION] = DONE;
  374. autoLevelData[dim][START] = start;
  375. autoLevelData[dim][END] = end;
  376. autoLevelData[dim][DIST] = (end - start);//total dist
  377. autoLevelData[dim][MAX] = 0f;
  378. }
  379. 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]);
  380. }//getDirection
  381.  
  382.  
  383. private int getTurnEuler(int dir, float current, float spin, int dim){
  384. float remainder = 0f;
  385. float start = autoLevelData[dim][START];
  386. float end = autoLevelData[dim][END];
  387.  
  388. if (dir != STOP){
  389. if (dir == INCREASE){//inc
  390. remainder = autoLevelData[dim][REM] = (autoLevelData[dim][DIST] - (current - start));
  391. 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);
  392. if (isBiggerThan(remainder, autoLevelData[dim][MAX]))
  393. remainder = autoLevelData[dim][MAX];
  394. //turn......................
  395. if (remainder == 0f){//autoLevelData[dim][DIST] < 0f ? -0.01f:0.01f) && spin == 0.0f
  396. //if (current >= autoLevelData[dim][DIST]){
  397. autoLevelData[dim][TURN] = STOP;
  398. }
  399. else if (isSmallerThan(spin, remainder - 0.01f))//1:1// + 0.001
  400. autoLevelData[dim][TURN] = INCREASE;
  401. else if (isBiggerThan(spin, remainder + 0.01f))
  402. autoLevelData[dim][TURN] = DECREASE;
  403. else{
  404. if (autoLevelData[dim][TURN] != STOPTHRUST && autoLevelData[dim][TURN] != COAST)
  405. autoLevelData[dim][TURN] = STOPTHRUST;//10
  406. else
  407. autoLevelData[dim][TURN] = COAST;//11
  408. }
  409. autoLevelData[dim][PERCENT] = (autoLevelData[dim][DIST] - (current - start))/autoLevelData[dim][DIST];
  410. }
  411. else if (dir == DECREASE){//dec
  412. remainder = (autoLevelData[dim][DIST] - (current - start));//
  413. autoLevelData[dim][REM] = remainder;
  414. 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);
  415. if (isBiggerThan(remainder, autoLevelData[dim][MAX])){
  416. 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);
  417. remainder = autoLevelData[dim][MAX];
  418. }
  419. else{
  420. 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);
  421. }
  422. //turn................
  423. //if (isSmallerThan(remainder, autoLevelData[dim][DIST] < 0f ? -0.01f:0.01f) && spin == 0.0f){
  424. if (remainder == 0f){
  425. autoLevelData[dim][TURN] = STOP;
  426. 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]);
  427. }
  428. else if (isSmallerThan(spin, remainder + 0.01f)){//1:1//
  429. autoLevelData[dim][TURN] = DECREASE;
  430. 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]);
  431. }
  432. else if (isBiggerThan(spin, remainder - 0.01f)){//1:1//
  433. autoLevelData[dim][TURN] = INCREASE;
  434. 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]);
  435. }
  436. else{//spin != 0.0f//
  437. if (autoLevelData[dim][TURN] != STOPTHRUST && autoLevelData[dim][TURN] != COAST)
  438. autoLevelData[dim][TURN] = STOPTHRUST;//10
  439. else
  440. autoLevelData[dim][TURN] = COAST;
  441. 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]);
  442. }
  443.  
  444. autoLevelData[dim][PERCENT] = (autoLevelData[dim][DIST] - (current - start))/autoLevelData[dim][DIST];
  445. }
  446. //
  447. else if (dir == INCREASE2){//inc2
  448. remainder = (autoLevelData[dim][DIST] - (current - start));
  449. autoLevelData[dim][REM] = remainder;
  450. 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);
  451. if (isBiggerThan(remainder, autoLevelData[dim][MAX]))
  452. remainder = autoLevelData[dim][MAX];
  453. //turn....................
  454. float sign = autoLevelData[dim][DIST] < 0f ? -0.01f:0.01f;
  455. if (isSmallerThan(remainder, 0.05f) && isSmallerThan(spin, 0.001f)){//autoLevelData[dim][DIST] < 0f ? -0.0001f:0.0001f
  456. //if (remainder < 0.6f * sign){
  457. autoLevelData[dim][TURN] = STOP;
  458. }
  459. else if (isBiggerThan(spin, remainder - (0.01f)))//1:1// + 0.001
  460. autoLevelData[dim][TURN] = DECREASE;
  461. else if (isSmallerThan(spin, remainder + (0.01f)))//1:1// + 0.001
  462. autoLevelData[dim][TURN] = INCREASE;
  463. else{
  464. if (autoLevelData[dim][TURN] != STOPTHRUST && autoLevelData[dim][TURN] != COAST)
  465. autoLevelData[dim][TURN] = STOPTHRUST;
  466. else
  467. autoLevelData[dim][TURN] = COAST;
  468. }
  469. autoLevelData[dim][PERCENT] = (autoLevelData[dim][DIST] - (current - start))/autoLevelData[dim][DIST];
  470. }
  471. else if (dir == DECREASE2){//dec2
  472. remainder = (autoLevelData[dim][DIST] - (current - start));
  473. autoLevelData[dim][REM] = remainder;
  474. //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);
  475. if (isBiggerThan(remainder, autoLevelData[dim][MAX]))
  476. remainder = autoLevelData[dim][MAX];
  477. //turn....................
  478. //if (isSmallerThan(remainder, autoLevelData[dim][DIST] < 0f ? -0.01f:0.01f) && spin == 0.0f){
  479. if (remainder == 0f){
  480. autoLevelData[dim][TURN] = STOP;
  481. }
  482. else if (isSmallerThan(spin, remainder + 0.01f))//1:1// + 0.001
  483. autoLevelData[dim][TURN] = INCREASE;
  484. else if (isBiggerThan(spin, remainder - 0.01f))//1:1// + 0.001
  485. autoLevelData[dim][TURN] = DECREASE;
  486. else{
  487. if (autoLevelData[dim][TURN] != STOPTHRUST && autoLevelData[dim][TURN] != COAST)
  488. autoLevelData[dim][TURN] = STOPTHRUST;
  489. else
  490. autoLevelData[dim][TURN] = COAST;
  491. }
  492. autoLevelData[dim][PERCENT] = (autoLevelData[dim][DIST] - (current - start))/autoLevelData[dim][DIST];
  493. }
  494.  
  495. else{
  496. System.out.println("CC.getTurn2()" + ", error: should not get here. dir=(" + dir + ")");
  497. //turn = 1000000;
  498. //remainder = 0f;
  499. }
  500. //if (dim == X)
  501. 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]);
  502. }//dir not stop
  503. return((int)autoLevelData[dim][TURN]);
  504. }//getTurn2
  505.  
  506.  
  507. public boolean isBiggerThan(float f1, float f2){
  508. boolean b = false;
  509. String tdir = "--";
  510. if ((f1 >= 0f && f2 > 0f) || (f1 <= 0f && f2 < 0f)){
  511. if (Math.abs(f1) > Math.abs(f2)){
  512. b = true;
  513. tdir = ">>";
  514. }
  515. }
  516. //System.out.println("CC.isBiggerThan()" + ", f1=" + f1 + " " + tdir + " " + "f2=" + f2 + "=" + b);
  517. return (b);
  518. }
  519.  
  520.  
  521. public boolean isSmallerThan(float f1, float f2){
  522. boolean b = false;
  523. String tdir = "--";
  524. if ((f1 >= 0f && f2 > 0f) || (f1 <= 0f && f2 < 0f)){
  525. if (Math.abs(f1) < Math.abs(f2)){
  526. b = true;
  527. tdir = "<<";
  528. }
  529. }
  530. System.out.println("CC.isSmallerThan()" + ", f1=" + f1 + " " + tdir + " " + "f2=" + f2 + "=" + b);
  531. return (b);
  532. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement