# HG changeset patch
# User kazz@kazzone.cr.ie.u-ryukyu.ac.jp
# Date 1249240931 -32400
# Node ID 12099d1ce7fc1cf86283d89e551b52fad32a5dca
# Parent  0f4576210e9ff633243d0b7e63750a6a1d10c0d1
chain.cpp update

diff -r 0f4576210e9f -r 12099d1ce7fc TaskManager/Test/test_render/chain.cpp
--- a/TaskManager/Test/test_render/chain.cpp	Sat Aug 01 11:55:50 2009 +0900
+++ b/TaskManager/Test/test_render/chain.cpp	Mon Aug 03 04:22:11 2009 +0900
@@ -1,16 +1,17 @@
+#include <iostream.h>
 #include <math.h>
 #include "SceneGraphRoot.h"
 #include "SGList.h"
 
 #define FALSE 0
 #define TRUE !FALSE
-#define CHAIN_LEN 30
+#define CHAIN_LEN 50
 
 static double m = 100.0;
-static double k = 1000.0;
+static double k = 7000.0;
 static double g = 9.8;
-static double dt = 0.01;
-static double chain_width = 5;
+static double dt = 0.003;
+static double chain_width = 10;
 static double safe = 0.995;
 
 typedef struct {
@@ -44,15 +45,15 @@
     if (pad->circle.isHold()) {
         cv[CHAIN_LEN-1].can_move = FALSE;
         if (pad->left.isHold()) {
-            cv[CHAIN_LEN-1].x += -1.0;
+            cv[CHAIN_LEN-1].x += -5.0;
         } else if (pad->right.isHold()) {
-            cv[CHAIN_LEN-1].x += 1.0;
+            cv[CHAIN_LEN-1].x += 5.0;
         }
 
         if (pad->up.isHold()) {
-            cv[CHAIN_LEN-1].y += -1.0;
+            cv[CHAIN_LEN-1].y += -5.0;
         } else if (pad->down.isHold()) {
-            cv[CHAIN_LEN-1].y += 1.0;
+            cv[CHAIN_LEN-1].y += 5.0;
         }
     } else {
         cv[CHAIN_LEN-1].can_move = TRUE;
@@ -63,42 +64,56 @@
 chain_move(SceneGraphPtr sg, int w, int h)
 {
     int id = sg->id;
-    if(cv[id].can_move) {
-        double dx = cv[id-1].x - cv[id].x;
-        double dy = cv[id-1].y - cv[id].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(id < CHAIN_LEN - 1) {
-            dx = cv[id+1].x - cv[id].x;
-            dy = cv[id+1].y - cv[id].y;
-            l = sqrt(dx * dx + dy * dy);
-            a = k * (l - chain_width) / m;
-            ax += a * dx / l;
-            ay += a * dy / l;
+    if(id == 0) {
+        for(int cnt = 0; cnt < 600; cnt++) {
+            for(int i = 0; i < CHAIN_LEN; i++) {
+                if(cv[i].can_move) {
+                    double dx = cv[i-1].x - cv[i].x;
+                    double dy = cv[i-1].y - cv[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 = cv[i+1].x - cv[i].x;
+                        dy = cv[i+1].y - cv[i].y;
+                        l = sqrt(dx * dx + dy * dy);
+                        a = k * (l - chain_width) / m;
+                        ax += a * dx / l;
+                        ay += a * dy / l;
+                    }
+                    ay += g;
+                    cv[i].vx *= safe;
+                    cv[i].vy *= safe;
+                    cv[i].next_vx = cv[i].vx + ax * dt;
+                    cv[i].next_vy = cv[i].vy + ay * dt;
+                    cv[i].next_x = cv[i].x + cv[i].vx * dt;
+                    cv[i].next_y = cv[i].y + cv[i].vy * dt;
+                } else {
+                    cv[i].next_x = cv[i].x;
+                    cv[i].next_y = cv[i].y;
+                }
+            }
+            for(int i = 0; i < CHAIN_LEN; i++) {
+                cv[i].vx = cv[i].next_vx;
+                cv[i].vy = cv[i].next_vy;
+                cv[i].x = cv[i].next_x;
+                cv[i].y = cv[i].next_y;
+            }
         }
-        ay += g;
-        cv[id].vx *= safe;
-        cv[id].vy *= safe;
-        cv[id].next_vx = cv[id].vx + ax * dt;
-        cv[id].next_vy = cv[id].vy + ay * dt;
-        cv[id].next_x = cv[id].x + cv[id].vx * dt;
-        cv[id].next_y = cv[id].y + cv[id].vy * dt;
-    } else {
-        cv[id].next_x = cv[id].x;
-        cv[id].next_y = cv[id].y;
+        //    cout << id << ", " << sg->xyz[1] << endl;
     }
     set_vector(&cv[id], sg);
-    if(id == CHAIN_LEN -1) {
-        for(int i = 0; i < CHAIN_LEN; i++) {
-            cv[i].vx = cv[i].next_vx;
-            cv[i].vy = cv[i].next_vy;
-            cv[i].x = cv[i].next_x;
-            cv[i].y = cv[i].next_y;
-        }
+    int p, n;
+    p = n = id;
+    if(p != 0) {
+        p--;
     }
-    //    cout << id << ", " << sg->xyz[1] << endl;
+    if(n != CHAIN_LEN - 1) {
+        n++;
+    }
+    sg->angle[2-(id%2)*2]
+        = 90 + atan((cv[p].next_y - cv[n].next_y) / (cv[p].next_x - cv[n].next_x)) * 180 / M_PI;
 }
 
 void
@@ -126,16 +141,15 @@
         chain = sgroot->createSceneGraph(CHAIN);
         chain->id = i;
         init_chain_vars(&cv[i]);
-        if(i == 0)
-            cv[i].can_move = FALSE;
         cv[i].x = 0;
         cv[i].y = chain_width * i;
         set_vector(&cv[i], chain);
-        chain->angle[1] = 90 * (i % 2);
+        chain->angle[1] = -90 * (i % 2);
         chain->set_move_collision(chain_move, chain_collision);
 
         root_chain->addChild(chain);
     }
+    cv[0].can_move = FALSE;
 
     sgroot->setSceneData(root_chain);
 }