00001 package edu.tum.cs.ias.knowrob.vis.items;
00002
00003 import java.io.BufferedReader;
00004 import java.io.File;
00005 import java.io.FileReader;
00006 import java.io.IOException;
00007 import java.util.ArrayList;
00008
00009 import edu.tum.cs.ias.knowrob.util.datastructures.Point;
00010 import edu.tum.cs.ias.knowrob.vis.Canvas;
00011 import edu.tum.cs.ias.knowrob.vis.DrawableAnimated;
00012
00013 import javax.vecmath.Vector3f;
00014
00020 public class BodyPoseSequence extends ItemBase implements DrawableAnimated {
00021
00022 protected ArrayList<Point[]> jointPositions = new ArrayList<Point[]>();
00023 protected Integer trackedBodyPart = null, trackedBodyPartStartTime;
00024 protected int color = 0xffff00ff;
00025
00026 protected final float armsThickness = 0.035f;
00027 protected final float spineThickness = 0.04f;
00028
00029 protected boolean wireMode = false;
00030
00031 public void addPose(float EPISODE_NR, float TIME, String action,
00032 float BECX, float BECY, float BECZ, float ULWX, float ULWY, float ULWZ,
00033 float OLWX, float OLWY, float OLWZ, float UBWX, float UBWY, float UBWZ,
00034 float OBWX, float OBWY, float OBWZ, float UHWX, float UHWY, float UHWZ,
00035 float BRKX, float BRKY, float BRKZ, float OHWX, float OHWY, float OHWZ,
00036 float KOX, float KOY, float KOZ, float SEHX, float SEHY, float SEHZ,
00037 float OSLX, float OSLY, float OSLZ, float USLX, float USLY, float USLZ,
00038 float FULX, float FULY, float FULZ, float FBLX, float FBLY, float FBLZ,
00039 float OSRX, float OSRY, float OSRZ, float USRX, float USRY, float USRZ,
00040 float FURX, float FURY, float FURZ, float FBRX, float FBRY, float FBRZ,
00041 float SBLX, float SBLY, float SBLZ, float OALX, float OALY, float OALZ,
00042 float UALX, float UALY, float UALZ, float HALX, float HALY, float HALZ,
00043 float FILX, float FILY, float FILZ, float SBRX, float SBRY, float SBRZ,
00044 float OARX, float OARY, float OARZ, float UARX, float UARY, float UARZ,
00045 float HARX, float HARY, float HARZ, float FIRX, float FIRY, float FIRZ ){
00046
00047 Point[] newPosition = new Point[28];
00048
00049 newPosition[0] = new Point(BECX, BECY, BECZ, EPISODE_NR, TIME, "BEC", action);
00050 newPosition[1] = new Point(ULWX, ULWY, ULWZ, EPISODE_NR, TIME, "ULW", action);
00051 newPosition[2] = new Point(OLWX, OLWY, OLWZ, EPISODE_NR, TIME, "OLW", action);
00052 newPosition[3] = new Point(UBWX, UBWY, UBWZ, EPISODE_NR, TIME, "UBW", action);
00053 newPosition[4] = new Point(OBWX, OBWY, OBWZ, EPISODE_NR, TIME, "OBW", action);
00054 newPosition[5] = new Point(UHWX, UHWY, UHWZ, EPISODE_NR, TIME, "UHW", action);
00055 newPosition[6] = new Point(BRKX, BRKY, BRKZ, EPISODE_NR, TIME, "BRK", action);
00056 newPosition[7] = new Point(OHWX, OHWY, OHWZ, EPISODE_NR, TIME, "OHW", action);
00057 newPosition[8] = new Point(KOX, KOY, KOZ, EPISODE_NR, TIME, "KO", action);
00058 newPosition[9] = new Point(SEHX, SEHY, SEHZ, EPISODE_NR, TIME, "SEH", action);
00059 newPosition[10] = new Point(OSLX, OSLY, OSLZ, EPISODE_NR, TIME, "OSL", action);
00060 newPosition[11] = new Point(USLX, USLY, USLZ, EPISODE_NR, TIME, "USL", action);
00061 newPosition[12] = new Point(FULX, FULY, FULZ, EPISODE_NR, TIME, "FUL", action);
00062 newPosition[13] = new Point(FBLX, FBLY, FBLZ, EPISODE_NR, TIME, "FBL", action);
00063 newPosition[14] = new Point(OSRX, OSRY, OSRZ, EPISODE_NR, TIME, "OSR", action);
00064 newPosition[15] = new Point(USRX, USRY, USRZ, EPISODE_NR, TIME, "USR", action);
00065 newPosition[16] = new Point(FURX, FURY, FURZ, EPISODE_NR, TIME, "FUR", action);
00066 newPosition[17] = new Point(FBRX, FBRY, FBRZ, EPISODE_NR, TIME, "FBR", action);
00067 newPosition[18] = new Point(SBLX, SBLY, SBLZ, EPISODE_NR, TIME, "SBL", action);
00068 newPosition[19] = new Point(OALX, OALY, OALZ, EPISODE_NR, TIME, "OAL", action);
00069 newPosition[20] = new Point(UALX, UALY, UALZ, EPISODE_NR, TIME, "UAL", action);
00070 newPosition[21] = new Point(HALX, HALY, HALZ, EPISODE_NR, TIME, "HAL", action);
00071 newPosition[22] = new Point(FILX, FILY, FILZ, EPISODE_NR, TIME, "FIL", action);
00072 newPosition[23] = new Point(SBRX, SBRY, SBRZ, EPISODE_NR, TIME, "SBR", action);
00073 newPosition[24] = new Point(OARX, OARY, OARZ, EPISODE_NR, TIME, "OAR", action);
00074 newPosition[25] = new Point(UARX, UARY, UARZ, EPISODE_NR, TIME, "UAR", action);
00075 newPosition[26] = new Point(HARX, HARY, HARZ, EPISODE_NR, TIME, "HAR", action);
00076 newPosition[27] = new Point(FIRX, FIRY, FIRZ, EPISODE_NR, TIME, "FIR", action);
00077
00078
00079
00080
00081
00082 jointPositions.add(newPosition);
00083 defaultColor = color;
00084 }
00085
00086 public ArrayList<Point[]> getSequence() {
00087 return jointPositions;
00088 }
00089
00090 public void addPose(float episode_nr, float time, String action, float[] coords) {
00091 addPose(episode_nr, time, action, coords[0], coords[1], coords[2], coords[3], coords[4], coords[5], coords[6], coords[7], coords[8], coords[9], coords[10], coords[11], coords[12], coords[13], coords[14], coords[15], coords[16], coords[17], coords[18], coords[19], coords[20], coords[21], coords[22], coords[23], coords[24], coords[25], coords[26], coords[27], coords[28], coords[29], coords[30], coords[31], coords[32], coords[33], coords[34], coords[35], coords[36], coords[37], coords[38], coords[39], coords[40], coords[41], coords[42], coords[43], coords[44], coords[45], coords[46], coords[47], coords[48], coords[49], coords[50], coords[51], coords[52], coords[53], coords[54], coords[55], coords[56], coords[57], coords[58], coords[59], coords[60], coords[61], coords[62], coords[63], coords[64], coords[65], coords[66], coords[67], coords[68], coords[69], coords[70], coords[71], coords[72], coords[73], coords[74], coords[75], coords[76], coords[77], coords[78], coords[79], coords[80], coords[81], coords[82], coords[83]);
00092 }
00093
00094 public void setColor(int color) {
00095 this.color = color;
00096 }
00097
00098 public void draw(Canvas c, int step) {
00099 drawPose(c, step);
00100
00101 if(trackedBodyPart != null)
00102 drawBodyPartTrajectory(c, trackedBodyPart, trackedBodyPartStartTime, step);
00103 }
00104
00105 public void drawPose(Canvas c, int step) {
00106 if(wireMode)
00107 drawPoseWire(c, step);
00108 else
00109 drawPoseFat(c, step);
00110 }
00111
00112 public void drawPoseWire(Canvas c, int step) {
00113 if(step > getMaxStep())
00114 return;
00115
00116 Point[] pose = this.jointPositions.get(step);
00117
00118
00119 c.stroke(this.color);
00120
00121 c.pushMatrix();
00122 c.scale(100);
00123
00124
00125 for(int i = 0; i < 9; i++)
00126 c.line(pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z);
00127
00128
00129 c.pushMatrix();
00130 c.translate(pose[8].x, pose[8].y, pose[8].z);
00131 c.sphere(0.09f);
00132 c.popMatrix();
00133
00134
00135 c.line(pose[0].x, pose[0].y, pose[0].z, pose[10].x, pose[10].y, pose[10].z);
00136 c.line(pose[0].x, pose[0].y, pose[0].z, pose[14].x, pose[14].y, pose[14].z);
00137 c.line(pose[6].x, pose[6].y, pose[6].z, pose[18].x, pose[18].y, pose[18].z);
00138 c.line(pose[6].x, pose[6].y, pose[6].z, pose[23].x, pose[23].y, pose[23].z);
00139
00140 for(int i = 10; i < 13; i++)
00141 c.line(pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z);
00142 for(int i = 14; i < 17; i++)
00143 c.line(pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z);
00144 for(int i = 18; i < 22; i++)
00145 c.line(pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z);
00146 for(int i = 23; i < 27; i++)
00147 c.line(pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z);
00148
00149 c.popMatrix();
00150 }
00151
00152 public void drawPoseFat(Canvas c, int step) {
00153 if(step > getMaxStep())
00154 return;
00155
00156 Point[] pose = this.jointPositions.get(step);
00157
00158
00159 c.color(color);
00160 c.fill(color);
00161
00162 c.pushMatrix();
00163 c.scale(100);
00164 c.sphereDetail(20);
00165
00166
00167 for(int i = 0; i < 8; i++){
00168 c.color(0xffff0000);
00169 cyl(c,pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z, spineThickness);
00170 new Sphere(pose[i].x, pose[i].y, pose[i].z, -spineThickness, this.color).draw(c);
00171 }
00172
00173
00174 new Sphere(pose[8].x, pose[8].y, pose[8].z, -spineThickness*2, color).draw(c);
00175
00176 float lambda = 1.1f;
00177 cone(c,pose[8].x, pose[8].y, pose[8].z, pose[8].x + lambda*(pose[9].x - pose[8].x), pose[8].y + lambda*(pose[9].y - pose[8].y), pose[8].z+ + lambda*(pose[9].z - pose[8].z), spineThickness, spineThickness * 0.05f);
00178
00179
00180 cyl(c,pose[0].x, pose[0].y, pose[0].z, pose[10].x, pose[10].y, pose[10].z, armsThickness);
00181 cyl(c,pose[0].x, pose[0].y, pose[0].z, pose[14].x, pose[14].y, pose[14].z, armsThickness);
00182 cyl(c,pose[6].x, pose[6].y, pose[6].z, pose[18].x, pose[18].y, pose[18].z, armsThickness);
00183 cyl(c,pose[6].x, pose[6].y, pose[6].z, pose[23].x, pose[23].y, pose[23].z, armsThickness);
00184
00185 for(int i = 10; i < 13; i++) {
00186 cyl(c,pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z, armsThickness);
00187 new Sphere(pose[i].x, pose[i].y, pose[i].z, -armsThickness, this.color).draw(c);
00188 }
00189 for(int i = 14; i < 17; i++){
00190 cyl(c,pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z, armsThickness);
00191 new Sphere(pose[i].x, pose[i].y, pose[i].z, -armsThickness, this.color).draw(c);
00192 }
00193 for(int i = 18; i < 21; i++){
00194 cyl(c,pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z, armsThickness);
00195 new Sphere(pose[i].x, pose[i].y, pose[i].z, -armsThickness, this.color).draw(c);
00196 }
00197
00198 cone(c,pose[21].x, pose[21].y, pose[21].z, pose[21].x + lambda*(pose[22].x - pose[21].x), pose[21].y + lambda*(pose[22].y - pose[21].y), pose[21].z+ + lambda*(pose[22].z - pose[21].z), armsThickness, armsThickness * 0.1f);
00199
00200 for(int i = 23; i < 26; i++){
00201 cyl(c,pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z, armsThickness);
00202 drawSphere(c, pose[i].x, pose[i].y, pose[i].z, armsThickness);
00203 }
00204
00205 cone(c,pose[26].x, pose[26].y, pose[26].z, pose[26].x + lambda*(pose[27].x - pose[26].x), pose[26].y + lambda*(pose[27].y - pose[26].y), pose[26].z+ + lambda*(pose[27].z - pose[26].z), armsThickness, armsThickness * 0.1f);
00206
00207
00208 drawSphere(c, pose[20].x, pose[20].y, pose[20].z, armsThickness);
00209 drawSphere(c, pose[25].x, pose[25].y, pose[25].z, armsThickness);
00210
00211 c.popMatrix();
00212
00213 }
00214
00215 protected void drawSphere(Canvas c, float x, float y, float z, float r) {
00216 new Sphere(x, y, z, -r, this.color).draw(c);
00217 }
00218
00219 protected void cyl(Canvas c, float x1, float y1, float z1, float x2, float y2, float z2, float r){
00220 ConePrimitive cyl = new ConePrimitive(new Vector3f(x1, y1, z1), new Vector3f(x2, y2, z2), r);
00221 cyl.draw(c);
00222 }
00223
00224
00225 protected void cone(Canvas c, float x1, float y1, float z1, float x2, float y2, float z2, float r1, float r2){
00226 ConePrimitive cyl = new ConePrimitive(new Vector3f(x1, y1, z1), new Vector3f(x2, y2, z2), r1, r2);
00227 cyl.draw(c);
00228 }
00229
00230 public void trackBodyPart(Integer bodyPart, int startTime) {
00231 this.trackedBodyPart = bodyPart;
00232 this.trackedBodyPartStartTime = startTime;
00233 }
00234
00235 public boolean isTrackingBodyPart() {
00236 return trackedBodyPart != null;
00237 }
00238
00239 public void drawBodyPartTrajectory(Canvas c, int bodyPart, int begin, int end) {
00240 c.pushMatrix();
00241 c.stroke(255,255,0);
00242 c.scale(100);
00243 if((bodyPart != -1) && (end > 0)) {
00244 for(int i = begin; i < end - 1; i++) {
00245 Point[] pose1 = jointPositions.get(i);
00246 Point[] pose2 = jointPositions.get(i+1);
00247 c.line(pose1[bodyPart].x, pose1[bodyPart].y, pose1[bodyPart].z, pose2[bodyPart].x, pose2[bodyPart].y, pose2[bodyPart].z);
00248 }
00249 }
00250 c.popMatrix();
00251 }
00252
00253 public int getMaxStep() {
00254 return jointPositions.size()-1;
00255 }
00256
00257 public boolean isEmpty() {
00258 return jointPositions.size() == 0;
00259 }
00260
00267 public void readData(File matlabAsciiFile) throws NumberFormatException, IOException {
00268 BufferedReader r = new BufferedReader(new FileReader(matlabAsciiFile));
00269 String line;
00270 while((line = r.readLine()) != null) {
00271 String[] parts = line.trim().split("\\s+");
00272 float[] coords = new float[parts.length];
00273 for(int i = 0; i < parts.length; i++)
00274 coords[i] = Float.parseFloat(parts[i]);
00275 this.addPose(1, 0, "", coords);
00276 }
00277 }
00278
00279 public void setMode(boolean wireOnly) {
00280 wireMode = wireOnly;
00281 }
00282
00283 @Override
00284 public void setColor(int color, int start, int end) {
00285 System.err.println("not availible");
00286
00287 }
00288 }
