|
539
|
1 #include <stdio.h>
|
|
|
2 #include <string.h>
|
|
|
3 #include <math.h>
|
|
|
4 #include "ChainCal.h"
|
|
|
5 #include "Func.h"
|
|
|
6 #include "types.h"
|
|
|
7
|
|
|
8 /* これは必須 */
|
|
|
9 SchedDefineTask(ChainCal);
|
|
|
10
|
|
|
11 #define CHAIN_LEN 50
|
|
|
12
|
|
|
13 static const double m = 100.0;
|
|
|
14 static const double k = 7000.0;
|
|
|
15 static const double g = 9.8;
|
|
|
16 static const double dt = 0.003;
|
|
|
17 static const double chain_width = 10;
|
|
|
18 static const double safe = 0.995;
|
|
|
19
|
|
|
20 typedef struct {
|
|
|
21 double x, y, next_x, next_y;
|
|
|
22 double vx, vy, next_vx, next_vy;
|
|
|
23 double angle[3];
|
|
|
24 int can_move;
|
|
|
25 uint32 parent;
|
|
|
26 int id;
|
|
|
27 //int parent;
|
|
|
28 } ChainProperty, *ChainPropertyPtr;
|
|
|
29
|
|
|
30 static int
|
|
|
31 run(SchedTask *s,void *rbuf, void *wbuf)
|
|
|
32 {
|
|
|
33 ChainPropertyPtr property = (ChainPropertyPtr)s->get_input(rbuf, 0);
|
|
|
34 ChainPropertyPtr update_property = (ChainPropertyPtr)s->get_output(wbuf, 0);
|
|
|
35
|
|
|
36 // ChainPropertyPtr property = (ChainPropertyPtr)rbuf;
|
|
|
37 // int id = get_param(0);
|
|
|
38
|
|
|
39 //ChainPropertyPtr o_property = (ChainPropertyPtr)wbuf;
|
|
|
40
|
|
|
41 for(int cnt = 0; cnt < 600; cnt++) {
|
|
|
42 for(int i = 0; i < CHAIN_LEN; i++) {
|
|
|
43 if(property[i].can_move) {
|
|
|
44 double dx = property[i-1].x - property[i].x;
|
|
|
45 double dy = property[i-1].y - property[i].y;
|
|
|
46 double l = sqrt(dx * dx + dy * dy);
|
|
|
47 double a = k * (l - chain_width) / m;
|
|
|
48 double ax = a * dx / l;
|
|
|
49 double ay = a * dy / l;
|
|
|
50 if(i < CHAIN_LEN - 1) {
|
|
|
51 dx = property[i+1].x - property[i].x;
|
|
|
52 dy = property[i+1].y - property[i].y;
|
|
|
53 l = sqrt(dx * dx + dy * dy);
|
|
|
54 a = k * (l - chain_width) / m;
|
|
|
55 ax += a * dx / l;
|
|
|
56 ay += a * dy / l;
|
|
|
57 }
|
|
|
58 ay += g;
|
|
|
59 property[i].vx *= safe;
|
|
|
60 property[i].vy *= safe;
|
|
|
61 property[i].next_vx = property[i].vx + ax * dt;
|
|
|
62 property[i].next_vy = property[i].vy + ay * dt;
|
|
|
63 property[i].next_x = property[i].x + property[i].vx * dt;
|
|
|
64 property[i].next_y = property[i].y + property[i].vy * dt;
|
|
|
65 } else {
|
|
|
66 property[i].next_x = property[i].x;
|
|
|
67 property[i].next_y = property[i].y;
|
|
|
68 }
|
|
|
69 }
|
|
|
70 for(int i = 0; i < CHAIN_LEN; i++) {
|
|
|
71 property[i].vx = property[i].next_vx;
|
|
|
72 property[i].vy = property[i].next_vy;
|
|
|
73 property[i].x = property[i].next_x;
|
|
|
74 property[i].y = property[i].next_y;
|
|
|
75 }
|
|
|
76 }
|
|
|
77
|
|
|
78 for (int j = 0; j < CHAIN_LEN; j++) {
|
|
|
79 int p, n;
|
|
|
80 int id = property[j].id;
|
|
|
81 p = n = id;
|
|
|
82 if(p != 0) {
|
|
|
83 p--;
|
|
|
84 }
|
|
|
85 if(n != CHAIN_LEN - 1) {
|
|
|
86 n++;
|
|
|
87 }
|
|
|
88 property[j].angle[2-(id%2)*2]
|
|
|
89 = 90 + atan((property[p].next_y - property[n].next_y) / (property[p].next_x - property[n].next_x)) * 180 / M_PI;
|
|
|
90 }
|
|
|
91 memcpy((void*)update_property, (void*)property, sizeof(ChainProperty) * CHAIN_LEN);
|
|
|
92 return 0;
|
|
|
93 }
|
