10 package com.generalrobotix.ui.view.graph;
14 import javax.vecmath.AxisAngle4d;
15 import javax.vecmath.Matrix3d;
37 private static final double LOG10 = Math.log(10);
95 StringBuffer graphName =
new StringBuffer(
"Graph");
111 throw new ArrayIndexOutOfBoundsException();
113 StringBuffer graphName =
new StringBuffer(
"Graph");
114 graphName.append(index);
115 return graphName.toString();
152 while (itr.hasNext()) {
159 while (itr0.hasNext()) {
161 for(
int i=0;
i<3;
i++){
272 while (itr.hasNext()) {
277 while (itr0.hasNext()) {
279 for(
int i=0;
i<3;
i++){
324 if(key.contains(
"attitude")){
326 if(attitudeDataModel==
null){
332 for(
int i=0;
i<4;
i++){
356 if(key.contains(
"attitude"))
378 if(key.contains(
"attitude")){
386 for(
int i=0;
i<4;
i++)
421 int eMin = (
int)Math.floor(Math.log(sMin) /
LOG10);
425 boolean found =
false;
428 for (
int i = 1;
i <= 3;
i++) {
429 step = m * Math.pow(10.0, e);
432 char[]
c =
new char[-e + 2];
435 for (
int j = 0; j < -e; j++) {
438 format =
new String(
c);
473 }
catch (Exception e) {
490 private void _getData(
long origin,
int offset,
int count){
497 int counter = changePos - ((
int)origin + offset);
500 if( changePos >= 0 ){
506 if( counter <= count ){
514 if(refWorld ==
null){
517 double[] dbArray = ds.
getData();
518 int localOffset = (ds.
getHeadPos() + offset) % dbArray.length;
519 for (
int i = 0;
i < count; ++
i, ++localOffset){
520 if(localOffset +
i >= dbArray.length){
523 dbArray[localOffset] = Double.NaN;
529 long recNo = origin + offset;
534 double[] dbArray = ds.
getData();
535 int localOffset = (ds.
getHeadPos() + offset) % dbArray.length;
537 localDataItem.
node +
"." + localDataItem.
attribute + (localDataItem.
index >= 0 ?
"." + localDataItem.
index :
"") );
538 ArrayList<Integer> arryLength =
new ArrayList<Integer>();
539 arryLength.add(refCharacter.
position.length * 7);
540 arryLength.add(arryLength.get(0) + refCharacter.
sensorState.q.length);
541 arryLength.add(arryLength.get(1) + refCharacter.
sensorState.u.length);
543 arryLength.add( arryLength.get(2));
545 arryLength.add( arryLength.get(2) +
550 arryLength.add( arryLength.get(3));
552 arryLength.add( arryLength.get(3) +
557 arryLength.add( arryLength.get(4));
559 arryLength.add( arryLength.get(4) +
563 arryLength.add( arryLength.get(5));
565 arryLength.add( arryLength.get(5) +
569 for (
int i = 0;
i < count; ++
i, ++recNo, ++localOffset){
570 if(localOffset >= dbArray.length){
574 dbArray[localOffset] = Double.NaN;
577 if (index <= arryLength.get(0)){
580 }
else if (index <= arryLength.get(2)){
581 if( (index - 1) % 2 == 0 ){
582 dbArray[localOffset] = localCharacter.
sensorState.q[(index - arryLength.get(0) - 1)/2];
584 dbArray[localOffset] = localCharacter.
sensorState.u[(index - arryLength.get(0) - 2)/2];
586 }
else if (index <= arryLength.get(3) && localCharacter.
sensorState.force.length > 0){
587 int dim1 = (index - arryLength.get(2) - 1) / localCharacter.
sensorState.force[0].length ;
588 int dim2 = (index - arryLength.get(2) - 1) % localCharacter.
sensorState.force[0].length ;
589 dbArray[localOffset] = localCharacter.
sensorState.force[dim1][dim2];
591 }
else if (index <= arryLength.get(4) && localCharacter.
sensorState.rateGyro.length > 0){
592 int dim1 = (index - arryLength.get(3) - 1) / localCharacter.
sensorState.rateGyro[0].length ;
593 int dim2 = (index - arryLength.get(3) - 1) % localCharacter.
sensorState.rateGyro[0].length ;
594 dbArray[localOffset] = localCharacter.
sensorState.rateGyro[dim1][dim2];
595 }
else if (index <= arryLength.get(5) && localCharacter.
sensorState.accel.length > 0){
596 int dim1 = (index - arryLength.get(4) - 1) / localCharacter.
sensorState.accel[0].length ;
597 int dim2 = (index - arryLength.get(4) - 1) % localCharacter.
sensorState.accel[0].length ;
598 dbArray[localOffset] = localCharacter.
sensorState.accel[dim1][dim2];
599 }
else if(localCharacter.
sensorState.range.length > 0) {
600 int dim1 = (index - arryLength.get(5) - 1) / localCharacter.
sensorState.range[0].length ;
601 int dim2 = (index - arryLength.get(5) - 1) % localCharacter.
sensorState.range[0].length ;
602 dbArray[localOffset] = localCharacter.
sensorState.range[dim1][dim2];
614 double[][] aa =
new double[4][];
615 int[] aapos =
new int[4];
616 int[] aaSize =
new int[4];
617 for(
int i=0;
i<4;
i++){
623 double[][] rpy = {
null,
null,
null};
624 int[] rpypos =
new int[3];
625 int[] rpySize =
new int[3];
626 for(
int i=0;
i<3;
i++){
634 for (
int i = 0;
i < count;
i++) {
635 if(aa[0][aapos[0]] == Double.NaN || aa[1][aapos[1]] == Double.NaN || aa[2][aapos[2]] == Double.NaN || aa[3][aapos[3]] == Double.NaN){
636 if(rpy[0]!=
null) rpy[0][rpypos[0]] = Double.NaN;
637 if(rpy[1]!=
null) rpy[1][rpypos[1]] = Double.NaN;
638 if(rpy[2]!=
null) rpy[2][rpypos[2]] = Double.NaN;
641 for(
int j=0; j<4; j++){
642 if(aapos[j] < aaSize[j]-1)
647 for(
int j=0; j<3; j++){
648 if(rpypos[j] < rpySize[j]-1)
681 private void AxisAngleToRPY(
double[][] aa,
double[][] rpy,
int[] aapos,
int[] rpypos){
682 Matrix3d m =
new Matrix3d();
683 m.set(
new AxisAngle4d(aa[0][aapos[0]], aa[1][aapos[1]], aa[2][aapos[2]], aa[3][aapos[3]]));
686 double roll, pitch, yaw;
687 if ((Math.abs(m.m00)<Math.abs(m.m20)) && (Math.abs(m.m10)<Math.abs(m.m20))) {
691 }
else if (sp > 1.0) {
694 pitch = Math.asin(sp);
696 roll = Math.atan2(sp*m.m01+m.m12,
705 double sr=Math.sin(roll), cr=Math.cos(roll);
707 yaw = Math.atan2(sr*m.m11+cr*m.m12,
710 yaw = Math.atan2(-sr*m.m11-cr*m.m12,
714 yaw = Math.atan2(m.m10, m.m00);
715 double sa = Math.sin(yaw);
716 double ca = Math.cos(yaw);
717 pitch = Math.atan2(-m.m20, ca*m.m00+sa*m.m10);
718 roll = Math.atan2(sa*m.m02-ca*m.m12, -sa*m.m01+ca*m.m11);
721 if(rpy[0]!=
null) rpy[0][rpypos[0]] = roll;
722 if(rpy[1]!=
null) rpy[1][rpypos[1]] = pitch;
723 if(rpy[2]!=
null) rpy[2][rpypos[2]] = yaw;