des.mc.core.skeleton source code

1 module des.mc.core.skeleton;
2 
3 import std.stdio;
4 import std.string;
5 import std.algorithm;
6 import std.traits;
7 public import des.math.linear;
8 
9 struct Joint
10 {
11     vec3 pos;
12     float qual = 0.0f;
13 }
14 
15 struct Skeleton
16 {
17     enum JointID
18     {
19         HEAD,
20         NECK,
21 
22         LEFT_SHOULDER,
23         RIGHT_SHOULDER,
24         LEFT_ELBOW,
25         RIGHT_ELBOW,
26         LEFT_HAND,
27         RIGHT_HAND,
28 
29         TORSO,
30 
31         LEFT_HIP,
32         RIGHT_HIP,
33         LEFT_KNEE,
34         RIGHT_KNEE,
35         LEFT_FOOT,
36         RIGHT_FOOT
37     }
38 
39     enum JointCount = [EnumMembers!JointID].length;
40 
41     private Joint[JointCount] joints;
42 
43     mixin( accessArray!("joints",Joint,JointID) );
44 
45     auto transform( in mat4 mtr, JointID[] jj = [] ) const
46     {
47         Skeleton ret;
48         auto en = cast(JointID[])[EnumMembers!JointID];
49         if( jj.length )
50             en = jj;
51         foreach( i; en )
52             ret.joints[i] = Joint( (mtr * vec4(joints[i].pos,1.0)).xyz, joints[i].qual );
53 
54         return ret;
55     }
56 
57     vec3 center() const
58     {
59         vec3 p;
60         foreach( joint; joints )
61             p += joint.pos;
62         return p / joints.length;
63     }
64 
65     Joint[JointCount] allJoints() const { return joints; }
66     void setJoints( in Joint[JointCount] j ) { joints = j; }
67 
68     vec3[] allBones() const
69     {
70         vec3[] ret;
71         ret ~= joints[JointID.HEAD].pos;
72         ret ~= joints[JointID.NECK].pos;
73 
74         ret ~= joints[JointID.NECK].pos;
75         ret ~= joints[JointID.LEFT_SHOULDER].pos;
76 
77         ret ~= joints[JointID.NECK].pos;
78         ret ~= joints[JointID.RIGHT_SHOULDER].pos;
79 
80         ret ~= joints[JointID.RIGHT_ELBOW].pos;
81         ret ~= joints[JointID.RIGHT_SHOULDER].pos;
82 
83         ret ~= joints[JointID.LEFT_ELBOW].pos;
84         ret ~= joints[JointID.LEFT_SHOULDER].pos;
85 
86         ret ~= joints[JointID.LEFT_ELBOW].pos;
87         ret ~= joints[JointID.LEFT_HAND].pos;
88 
89         ret ~= joints[JointID.RIGHT_ELBOW].pos;
90         ret ~= joints[JointID.RIGHT_HAND].pos;
91 
92         ret ~= joints[JointID.TORSO].pos;
93         ret ~= joints[JointID.NECK].pos;
94 
95         ret ~= joints[JointID.TORSO].pos;
96         ret ~= joints[JointID.LEFT_HIP].pos;
97 
98         ret ~= joints[JointID.TORSO].pos;
99         ret ~= joints[JointID.RIGHT_HIP].pos;
100 
101         ret ~= joints[JointID.RIGHT_KNEE].pos;
102         ret ~= joints[JointID.RIGHT_HIP].pos;
103 
104         ret ~= joints[JointID.LEFT_KNEE].pos;
105         ret ~= joints[JointID.LEFT_HIP].pos;
106 
107         ret ~= joints[JointID.LEFT_KNEE].pos;
108         ret ~= joints[JointID.LEFT_FOOT].pos;
109 
110         ret ~= joints[JointID.RIGHT_KNEE].pos;
111         ret ~= joints[JointID.RIGHT_FOOT].pos;
112         return ret;
113     }
114 
115     static Skeleton fromJoints( in Joint[JointCount] jj )
116     {
117         Skeleton ret;
118         ret.setJoints( jj );
119         return ret;
120     }
121 }
122 
123 version(unittest)
124 {
125     Joint[Skeleton.JointCount] pose_norm;
126     Skeleton utest_skeleton;
127     static this()
128     {
129         pose_norm[Skeleton.JointID.HEAD] = Joint( vec3(0,0,2), 1.0f );
130         pose_norm[Skeleton.JointID.NECK] = Joint( vec3(0,0,1.8), 1.0f );
131 
132         pose_norm[Skeleton.JointID.LEFT_SHOULDER]  = Joint( vec3(0, 0.2,1.7), 1.0f );
133         pose_norm[Skeleton.JointID.RIGHT_SHOULDER] = Joint( vec3(0,-0.2,1.7), 1.0f );
134         pose_norm[Skeleton.JointID.LEFT_ELBOW]     = Joint( vec3(0, 0.3,1.2), 1.0f );
135         pose_norm[Skeleton.JointID.RIGHT_ELBOW]    = Joint( vec3(0,-0.3,1.2), 1.0f );
136         pose_norm[Skeleton.JointID.LEFT_HAND]      = Joint( vec3(0.2,0.3,0.9), 1.0f );
137         pose_norm[Skeleton.JointID.RIGHT_HAND]     = Joint( vec3(0.2,-0.3,0.9), 1.0f );
138 
139         pose_norm[Skeleton.JointID.TORSO] = Joint( vec3(0,0,1.2), 1.0f );
140 
141         pose_norm[Skeleton.JointID.LEFT_HIP] = Joint( vec3(0,0.15,1), 1.0f );
142         pose_norm[Skeleton.JointID.RIGHT_HIP] = Joint( vec3(0,-0.15,1), 1.0f );
143         pose_norm[Skeleton.JointID.LEFT_KNEE] = Joint( vec3(0,0.15,0.5), 1.0f );
144         pose_norm[Skeleton.JointID.RIGHT_KNEE] = Joint( vec3(0,-0.15,0.5), 1.0f );
145         pose_norm[Skeleton.JointID.LEFT_FOOT] = Joint( vec3(0,0.15,0), 1.0f );
146         pose_norm[Skeleton.JointID.RIGHT_FOOT] = Joint( vec3(0,-0.15,0), 1.0f );
147         utest_skeleton = Skeleton.fromJoints( pose_norm );
148     }
149 
150     Skeleton[] getFakeSkeletons( vec3 global_offset = vec3(0,0,0),
151             vec3[] offsets = [ vec3(0,2,0), vec3(1,-1,0) ] )
152     {
153         Skeleton[] ret;
154         foreach( i, offset; offsets )
155             ret ~= skeleton_offset( utest_skeleton, global_offset + offset );
156         return ret;
157     }
158 }
159 
160 unittest
161 {
162     auto ts = Skeleton.fromJoints( pose_norm );
163     assert( pose_norm[Skeleton.JointID.HEAD] == ts.head );
164     assert( pose_norm[Skeleton.JointID.RIGHT_HAND] == ts.rightHand );
165 }
166 
167 unittest
168 {
169     auto tr = mat4( 1.0, 0.0, 0.0, 1.0,
170                     0.0, 1.0, 0.0, 0.0,
171                     0.0, 0.0, 1.0, 0.0,
172                     0.0, 0.0, 0.0, 1.0 );
173     auto skel = Skeleton.fromJoints( pose_norm );
174     auto tr_skel = skel.transform( tr );
175     foreach( i, j; tr_skel.joints )
176         assert( j.pos.x == skel.joints[i].pos.x + 1 );
177     tr_skel = skel.transform( tr, [Skeleton.JointID.RIGHT_HAND, Skeleton.JointID.LEFT_HAND] );
178 
179     foreach( i, j; tr_skel.joints )
180     {
181         if( i == Skeleton.JointID.RIGHT_HAND || i == Skeleton.JointID.LEFT_HAND )
182             assert( j.pos.x == skel.joints[i].pos.x + 1 );
183         else
184             assert( j.pos.x == skel.joints[i].pos.x );
185     }
186 }
187 
188 Skeleton skeleton_mlt( in Skeleton s, float v )
189 {
190     auto s_joints = s.allJoints();
191     foreach( ref joint; s_joints )
192     {
193         joint.pos *= v;
194         joint.qual *= v;
195     }
196     return Skeleton.fromJoints( s_joints );
197 }
198 
199 unittest
200 {
201     auto ts = Skeleton.fromJoints( pose_norm );
202     auto mts = skeleton_mlt( ts, 10 );
203     assert( pose_norm[Skeleton.JointID.HEAD].pos * 10 == mts.head.pos );
204 }
205 
206 Skeleton skeleton_add( in Skeleton a, in Skeleton b )
207 {
208     auto a_joints = a.allJoints();
209     auto b_joints = b.allJoints();
210     foreach( i, ref joint; a_joints )
211     {
212         joint.pos += b_joints[i].pos;
213         joint.qual += b_joints[i].qual;
214     }
215     return Skeleton.fromJoints( a_joints );
216 }
217 
218 unittest
219 {
220     auto ts1 = Skeleton.fromJoints( pose_norm );
221     auto ts2 = Skeleton.fromJoints( pose_norm );
222     auto rts = skeleton_add( ts1, ts2 );
223     assert( pose_norm[Skeleton.JointID.HEAD].pos * 2 == rts.head.pos );
224 }
225 
226 Skeleton skeleton_diff( in Skeleton a, in Skeleton b )
227 {
228     auto a_joints = a.allJoints();
229     auto b_joints = b.allJoints();
230     foreach( i, ref joint; a_joints )
231     {
232         joint.pos -= b_joints[i].pos;
233         joint.qual -= b_joints[i].qual;
234     }
235     return Skeleton.fromJoints( a_joints );
236 }
237 
238 unittest
239 {
240     auto ts1 = Skeleton.fromJoints( pose_norm );
241     auto ts2 = skeleton_mlt( ts1, 2 );
242     auto rts = skeleton_diff( ts2, ts1 );
243     assert( pose_norm[Skeleton.JointID.HEAD].pos == rts.head.pos );
244 }
245 
246 Skeleton skeleton_div( in Skeleton s, float v )
247 { return skeleton_mlt( s, 1.0f / v ); }
248 
249 Skeleton skeleton_offset( in Skeleton a, in vec3 offset )
250 {
251     auto a_joints = a.allJoints();
252     foreach( ref joint; a_joints )
253         joint.pos += offset;
254     return Skeleton.fromJoints( a_joints );
255 }
256 
257 private
258 {
259     @property string accessArray(string arrayName,arrayType,Enum)()
260     {
261         string[] ret;
262 
263         string type = arrayType.stringof;
264 
265         foreach( e; [EnumMembers!(Skeleton.JointID)] )
266         {
267             string id = format("%s.%s",Enum.stringof,e);
268             string name = toProperName( id );
269             string access = format( "%s[%s]", arrayName, id );
270             ret ~= format( "@property ref %1$s %2$s() { return %3$s; }\n" ~ 
271                            "@property ref const(%1$s) %2$s() const { return %3$s; }\n",
272                         type, name, access );
273         }
274 
275         return ret.join("\n");
276     }
277 
278     string toProperName( string str )
279     {
280         auto words = str.split(".")[$-1].split("_");
281         auto res = toLowerPure( words[0] );
282         if( words.length > 1 )
283             res = reduce!( (a,b) => a ~ b )( res, map!( a => a.capitalize )(words[1 .. $]) );
284         return res;
285     }
286 
287     unittest
288     {
289         assert( toProperName( "RIGHT_HAND" ) == "rightHand" );
290         assert( toProperName( "RIGHT_HAND_FIRST_FINGER" ) == "rightHandFirstFinger" );
291         assert( toProperName( "HEAD" ) == "head" );
292     }
293 
294     pure string toLowerPure( string str )
295     {
296         auto diff = cast(ubyte)('A') - cast(ubyte)('a');
297         char[] buf;
298         auto upperStart = cast(ubyte)('A');
299         auto upperEnd = cast(ubyte)('Z');
300         foreach( c; str )
301         {
302             auto bc = cast(ubyte)(c);
303             if( bc >= upperStart && bc <= upperEnd )
304                 buf ~= cast(char)(bc-diff);
305             else
306                 buf ~= c;
307         }
308 
309         return buf.idup;
310     }
311 
312     unittest
313     {
314         assert( toLowerPure( "OlolOlo" ) == "olololo" );
315         assert( toLowerPure( "RIGHT" ) == "right" );
316         assert( toLowerPure( "r10%Z" ) == "r10%z" );
317         string rndstr = "`1234567890-=qwertyuiop[]\\asdfghjkl;'zxcvbnm,./~!@#$%^&*()_+QWERTYUIOP{}|ASDFGHJKL:\"ZXCVBNM<>?";
318         assert( toLowerPure( rndstr ) == rndstr.toLower );
319     }
320 }