|
0
|
1 #include <stdio.h>
|
|
|
2 #include <string.h>
|
|
|
3 #include <math.h>
|
|
|
4 #include "chain_move.h"
|
|
|
5 #include "Func.h"
|
|
|
6 #include "types.h"
|
|
|
7
|
|
|
8 /* これは必須 */
|
|
|
9 SchedDefineTask(ChainTask);
|
|
|
10
|
|
|
11 static const int PROPERTY_LENGTH = 50;
|
|
|
12 static float m = 100.0;
|
|
|
13 static float k = 7000.0;
|
|
|
14 static float g = 9.8;
|
|
|
15 static float dt = 0.003;
|
|
|
16 static float chain_width = 10;
|
|
|
17 static float safe = 0.995;
|
|
|
18
|
|
|
19 typedef struct {
|
|
|
20 float xyz[3];
|
|
|
21 float angle[3];
|
|
|
22 float stack_xyz[3];
|
|
|
23 float next_xyz[3];
|
|
|
24 float v_xyz[3];
|
|
|
25 float next_v_xyz[3];
|
|
|
26 int property_index;
|
|
|
27 int parent_index;
|
|
|
28 int have_parent;
|
|
|
29 int can_move;
|
|
|
30 memaddr parent;
|
|
|
31 memaddr children;
|
|
|
32 memaddr node;
|
|
|
33 int sgid;
|
|
|
34 } *PropertyPtr, Property;
|
|
|
35
|
|
|
36 static int
|
|
|
37 run(SchedTask *s,void *rbuf, void *wbuf)
|
|
|
38 {
|
|
|
39 PropertyPtr property = (PropertyPtr)s->get_input(rbuf, 0);
|
|
|
40 PropertyPtr update_property = (PropertyPtr)s->get_output(wbuf, 0);
|
|
|
41
|
|
|
42 property[0].can_move = 0;
|
|
|
43 //property[1].can_move = 0;
|
|
|
44
|
|
|
45 for(int cnt = 0; cnt < 600; cnt++) {
|
|
|
46 for(int i = 0; i < PROPERTY_LENGTH; i++) {
|
|
|
47 if(property[i].can_move) {
|
|
|
48 #if 0
|
|
|
49 vector float v_x1 __attribute__((aligned(16))) = { property[i-1].xyz[0],
|
|
|
50 property[i].xyz[0],
|
|
|
51 property[i+1].xyz[0],
|
|
|
52 property[i+2].xyz[0]};
|
|
|
53 vector float v_x2 __attribute__((aligned(16))) = { property[i].xyz[0],
|
|
|
54 property[i+1].xyz[0],
|
|
|
55 property[i+2].xyz[0],
|
|
|
56 property[i+3].xyz[0]};
|
|
|
57 vector float v_dx __attribute__((aligned(16))) = spu_sub(v_x1, v_x2);
|
|
|
58
|
|
|
59 vector float v_y1 __attribute__((aligned(16))) = { property[i-1].xyz[1],
|
|
|
60 property[i].xyz[1],
|
|
|
61 property[i+1].xyz[1],
|
|
|
62 property[i+2].xyz[1]};
|
|
|
63 vector float v_y2 __attribute__((aligned(16))) = { property[i].xyz[1],
|
|
|
64 property[i+1].xyz[1],
|
|
|
65 property[i+2].xyz[1],
|
|
|
66 property[i+3].xyz[1]};
|
|
|
67 vector float v_dy __attribute__((aligned(16))) = spu_sub(v_x1, v_x2);
|
|
|
68
|
|
|
69 dx = spu_mule(dx, dx);
|
|
|
70 dy = spu_mule(dy, dy);
|
|
|
71 vector float v_l __attribute__((aligned(16)));
|
|
|
72
|
|
|
73 #else
|
|
|
74 float dx = property[i-1].xyz[0] - property[i].xyz[0];
|
|
|
75 float dy = property[i-1].xyz[1] - property[i].xyz[1];
|
|
|
76 float l = sqrt(dx * dx + dy * dy);
|
|
|
77 float a = k * (l - chain_width) / m;
|
|
|
78 float ax = a * dx / l;
|
|
|
79 float ay = a * dy / l;
|
|
|
80 if(i < PROPERTY_LENGTH - 1) {
|
|
|
81 dx = property[i+1].xyz[0] - property[i].xyz[0];
|
|
|
82 dy = property[i+1].xyz[1] - property[i].xyz[1];
|
|
|
83 l = sqrt(dx * dx + dy * dy);
|
|
|
84 a = k * (l - chain_width) / m;
|
|
|
85 ax += a * dx / l;
|
|
|
86 ay += a * dy / l;
|
|
|
87 }
|
|
|
88 ay += g;
|
|
|
89 property[i].v_xyz[0] *= safe;
|
|
|
90 property[i].v_xyz[1] *= safe;
|
|
|
91 property[i].next_v_xyz[0] = property[i].v_xyz[0] + ax * dt;
|
|
|
92 property[i].next_v_xyz[1] = property[i].v_xyz[1] + ay * dt;
|
|
|
93 property[i].next_xyz[0] = property[i].xyz[0] + property[i].v_xyz[0] * dt;
|
|
|
94 property[i].next_xyz[1] = property[i].xyz[1] + property[i].v_xyz[1] * dt;
|
|
|
95 } else {
|
|
|
96 property[i].next_xyz[0] = property[i].xyz[0];
|
|
|
97 property[i].next_xyz[1] = property[i].xyz[1];
|
|
|
98 }
|
|
|
99 #endif
|
|
|
100 }
|
|
|
101
|
|
|
102 for(int i = 0; i < PROPERTY_LENGTH; i++) {
|
|
|
103 property[i].v_xyz[0] = property[i].next_v_xyz[0];
|
|
|
104 property[i].v_xyz[1] = property[i].next_v_xyz[1];
|
|
|
105 property[i].xyz[0] = property[i].next_xyz[0];
|
|
|
106 property[i].xyz[1] = property[i].next_xyz[1];
|
|
|
107 }
|
|
|
108 }
|
|
|
109
|
|
|
110 for(int i = 0; i < PROPERTY_LENGTH; i++) {
|
|
|
111 int id = property[i].property_index;
|
|
|
112 int p, n;
|
|
|
113 p = n = id;
|
|
|
114 if(p != 0) {
|
|
|
115 p--;
|
|
|
116 }
|
|
|
117 if(n != PROPERTY_LENGTH - 1) {
|
|
|
118 n++;
|
|
|
119 }
|
|
|
120 property[i].angle[2-(id%2)*2]
|
|
|
121 = 90 + atan((property[p].next_xyz[1] - property[n].next_xyz[1])
|
|
|
122 / (property[p].next_xyz[0] - property[n].next_xyz[0])) * 180 / M_PI;
|
|
|
123 }
|
|
|
124
|
|
|
125 memcpy((void*)update_property, (void*)property, sizeof(Property)*PROPERTY_LENGTH);
|
|
|
126
|
|
|
127 return 0;
|
|
|
128 }
|
