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