--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Renderer/Engine/spe/ChainCal.cc	Mon Oct 12 09:39:35 2009 +0900
@@ -0,0 +1,93 @@
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+#include "ChainCal.h"
+#include "Func.h"
+#include "types.h"
+
+/* これは必須 */
+SchedDefineTask(ChainCal);
+
+#define CHAIN_LEN 50
+
+static const    double m = 100.0;
+static const    double k = 7000.0;
+static const    double g = 9.8;
+static const    double dt = 0.003;
+static const    double chain_width = 10;
+static const    double safe = 0.995;
+
+typedef struct {
+    double x, y, next_x, next_y;
+    double vx, vy, next_vx, next_vy;
+    double angle[3];
+    int can_move;
+    uint32 parent;
+    int id;
+    //int parent;
+} ChainProperty, *ChainPropertyPtr;
+
+static int
+run(SchedTask *s,void *rbuf, void *wbuf)
+{
+	ChainPropertyPtr property = (ChainPropertyPtr)s->get_input(rbuf, 0);
+    ChainPropertyPtr update_property = (ChainPropertyPtr)s->get_output(wbuf, 0);
+
+//    ChainPropertyPtr property = (ChainPropertyPtr)rbuf;
+//    int id = get_param(0);
+    
+    //ChainPropertyPtr o_property = (ChainPropertyPtr)wbuf;
+    
+    for(int cnt = 0; cnt < 600; cnt++) {
+		for(int i = 0; i < CHAIN_LEN; i++) {
+			if(property[i].can_move) {
+				double dx = property[i-1].x - property[i].x;
+				double dy = property[i-1].y - property[i].y;
+				double l = sqrt(dx * dx + dy * dy);
+				double a = k * (l - chain_width) / m;
+				double ax = a * dx / l;
+				double ay = a * dy / l;
+				if(i < CHAIN_LEN - 1) {
+					dx = property[i+1].x - property[i].x;
+					dy = property[i+1].y - property[i].y;
+					l = sqrt(dx * dx + dy * dy);
+					a = k * (l - chain_width) / m;
+					ax += a * dx / l;
+					ay += a * dy / l;
+				}
+				ay += g;
+				property[i].vx *= safe;
+				property[i].vy *= safe;
+				property[i].next_vx = property[i].vx + ax * dt;
+				property[i].next_vy = property[i].vy + ay * dt;
+				property[i].next_x = property[i].x + property[i].vx * dt;
+				property[i].next_y = property[i].y + property[i].vy * dt;
+			} else {
+				property[i].next_x = property[i].x;
+				property[i].next_y = property[i].y;
+			}
+		}
+		for(int i = 0; i < CHAIN_LEN; i++) {
+			property[i].vx = property[i].next_vx;
+			property[i].vy = property[i].next_vy;
+			property[i].x = property[i].next_x;
+			property[i].y = property[i].next_y;
+		}
+    }
+	
+    for (int j = 0; j < CHAIN_LEN; j++) {
+		int p, n;
+		int id = property[j].id;
+		p = n = id;
+		if(p != 0) {
+			p--;
+		}
+		if(n != CHAIN_LEN - 1) {
+			n++;
+		}
+		property[j].angle[2-(id%2)*2]
+			= 90 + atan((property[p].next_y - property[n].next_y) / (property[p].next_x - property[n].next_x)) * 180 / M_PI;
+    }
+    memcpy((void*)update_property, (void*)property, sizeof(ChainProperty) * CHAIN_LEN);    
+    return 0;
+}