--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Renderer/Test/spe/chain_move.cc	Fri Aug 06 19:59:23 2010 +0900
@@ -0,0 +1,128 @@
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+#include "chain_move.h"
+#include "Func.h"
+#include "types.h"
+
+/* これは必須 */
+SchedDefineTask(ChainTask);
+
+static const int PROPERTY_LENGTH = 50;
+static float m = 100.0;
+static float k = 7000.0;
+static float g = 9.8;
+static float dt = 0.003;
+static float chain_width = 10;
+static float safe = 0.995;
+
+typedef struct {
+    float xyz[3];
+    float angle[3];
+    float stack_xyz[3];
+    float next_xyz[3];
+    float v_xyz[3];
+    float next_v_xyz[3];
+    int property_index;
+    int parent_index;
+    int have_parent;
+    int can_move;
+    memaddr parent;
+    memaddr children;
+    memaddr node;
+    int sgid;
+} *PropertyPtr, Property;
+
+static int
+run(SchedTask *s,void *rbuf, void *wbuf)
+{
+    PropertyPtr	property	= (PropertyPtr)s->get_input(rbuf, 0);
+    PropertyPtr	update_property = (PropertyPtr)s->get_output(wbuf, 0);
+
+    property[0].can_move = 0;
+    //property[1].can_move = 0;
+
+    for(int cnt = 0; cnt < 600; cnt++) {
+	for(int i = 0; i < PROPERTY_LENGTH; i++) {
+	    if(property[i].can_move) {
+#if 0
+		vector float v_x1 __attribute__((aligned(16))) = { property[i-1].xyz[0],
+								   property[i].xyz[0],
+								   property[i+1].xyz[0], 
+								   property[i+2].xyz[0]};
+		vector float v_x2 __attribute__((aligned(16))) = { property[i].xyz[0],
+								   property[i+1].xyz[0],
+								   property[i+2].xyz[0],
+								   property[i+3].xyz[0]};
+		vector float v_dx __attribute__((aligned(16))) = spu_sub(v_x1, v_x2);
+
+		vector float v_y1 __attribute__((aligned(16))) = { property[i-1].xyz[1],
+								   property[i].xyz[1],
+								   property[i+1].xyz[1], 
+								   property[i+2].xyz[1]};
+		vector float v_y2 __attribute__((aligned(16))) = { property[i].xyz[1],
+								   property[i+1].xyz[1],
+								   property[i+2].xyz[1],
+								   property[i+3].xyz[1]};
+		vector float v_dy __attribute__((aligned(16))) = spu_sub(v_x1, v_x2);
+
+		dx = spu_mule(dx, dx);
+		dy = spu_mule(dy, dy);
+		vector float v_l __attribute__((aligned(16)));
+		
+#else
+		float dx = property[i-1].xyz[0] - property[i].xyz[0];
+		float dy = property[i-1].xyz[1] - property[i].xyz[1];
+		float l = sqrt(dx * dx + dy * dy);
+		float a = k * (l - chain_width) / m;
+		float ax = a * dx / l;
+		float ay = a * dy / l;
+		if(i < PROPERTY_LENGTH - 1) {
+		    dx = property[i+1].xyz[0] - property[i].xyz[0];
+		    dy = property[i+1].xyz[1] - property[i].xyz[1];
+		    l = sqrt(dx * dx + dy * dy);
+		    a = k * (l - chain_width) / m;
+		    ax += a * dx / l;
+		    ay += a * dy / l;
+		}
+		ay += g;
+		property[i].v_xyz[0] *= safe;
+		property[i].v_xyz[1] *= safe;
+		property[i].next_v_xyz[0] = property[i].v_xyz[0] + ax * dt;
+		property[i].next_v_xyz[1] = property[i].v_xyz[1] + ay * dt;
+		property[i].next_xyz[0] = property[i].xyz[0] + property[i].v_xyz[0] * dt;
+		property[i].next_xyz[1] = property[i].xyz[1] + property[i].v_xyz[1] * dt;
+	    } else {
+		property[i].next_xyz[0] = property[i].xyz[0];
+		property[i].next_xyz[1] = property[i].xyz[1];
+	    }
+#endif
+	}
+
+	for(int i = 0; i < PROPERTY_LENGTH; i++) {
+	    property[i].v_xyz[0] = property[i].next_v_xyz[0];
+	    property[i].v_xyz[1] = property[i].next_v_xyz[1];
+	    property[i].xyz[0] = property[i].next_xyz[0];
+	    property[i].xyz[1] = property[i].next_xyz[1];
+	}
+    }
+
+    for(int i = 0; i < PROPERTY_LENGTH; i++) {
+	int id = property[i].property_index;
+	int p, n;	
+	p = n = id;
+	if(p != 0) {
+	    p--;
+	}
+	if(n != PROPERTY_LENGTH - 1) {
+	    n++;
+	}
+	property[i].angle[2-(id%2)*2]
+	    = 90 + atan((property[p].next_xyz[1] - property[n].next_xyz[1])
+			/ (property[p].next_xyz[0] - property[n].next_xyz[0])) * 180 / M_PI;
+    }
+
+    memcpy((void*)update_property, (void*)property, sizeof(Property)*PROPERTY_LENGTH);
+
+    return 0;
+}