1+ using System ;
2+ using System . Collections ;
3+ using System . Collections . Generic ;
4+ using System . IO ;
5+ using UnityEngine ;
6+ using UnityEngine . Serialization ;
7+ using WebSocketSharp ;
8+ using WebSocketSharp . Net ;
9+ 10+ public class BoneController : MonoBehaviour
11+ {
12+ private BodyParts bodyParts ;
13+ private string receivedJson ;
14+ private WebSocket ws ;
15+ 16+ [ SerializeField , Range ( 10 , 120 ) ] private float frameRate ;
17+ public List < Transform > boneList = new List < Transform > ( ) ;
18+ private GameObject fullbodyIK ;
19+ private Vector3 [ ] points = new Vector3 [ 17 ] ;
20+ private Vector3 [ ] normalizeBone = new Vector3 [ 12 ] ;
21+ private float [ ] boneDistance = new float [ 12 ] ;
22+ private float timer ;
23+ 24+ private int [ , ] joints = new int [ , ] {
25+ { 0 , 1 } , { 1 , 2 } , { 2 , 3 } , { 0 , 4 } , { 4 , 5 } , { 5 , 6 } , { 0 , 7 } , { 7 , 8 } , { 8 , 9 } , { 9 , 10 } , { 8 , 11 } , { 11 , 12 } , { 12 , 13 } , { 8 , 14 } , { 14 , 15 } , { 15 , 16 }
26+ } ;
27+ 28+ private int [ , ] boneJoint = new int [ , ] { { 0 , 2 } , { 2 , 3 } , { 0 , 5 } , { 5 , 6 } , { 0 , 9 } , { 9 , 10 } , { 9 , 11 } , { 11 , 12 } , { 12 , 13 } , { 9 , 14 } , { 14 , 15 } , { 15 , 16 } } ;
29+ 30+ private int [ , ] normalizeJoint = new int [ , ] { { 0 , 1 } , { 1 , 2 } , { 0 , 3 } , { 3 , 4 } , { 0 , 5 } , { 5 , 6 } , { 5 , 7 } , { 7 , 8 } , { 8 , 9 } , { 5 , 10 } , { 10 , 11 } , { 11 , 12 } } ;
31+ 32+ private int nowFrame = 0 ;
33+ 34+ private float [ ] x = new float [ 17 ] ;
35+ private float [ ] y = new float [ 17 ] ;
36+ private float [ ] z = new float [ 17 ] ;
37+ 38+ private bool isReceived = false ;
39+ 40+ // Use this for initialization
41+ void Start ( )
42+ {
43+ ws = new WebSocket ( "ws://localhost:5000/" ) ;
44+ ws . OnOpen += ( sender , e ) =>
45+ {
46+ Debug . Log ( "WebSocket Open" ) ;
47+ } ;
48+ ws . OnMessage += ( sender , e ) =>
49+ {
50+ receivedJson = e . Data ;
51+ Debug . Log ( "Data: " + e . Data ) ;
52+ isReceived = true ;
53+ } ;
54+ ws . OnError += ( sender , e ) =>
55+ {
56+ Debug . Log ( "WebSocket Error Message: " + e . Message ) ;
57+ } ;
58+ ws . OnClose += ( sender , e ) =>
59+ {
60+ Debug . Log ( "WebSocket Close" ) ;
61+ } ;
62+ ws . Connect ( ) ;
63+ 64+ ws . Send ( "" ) ;
65+ }
66+ 67+ // Update is called once per frame
68+ void Update ( )
69+ {
70+ timer += Time . deltaTime ;
71+ ws . Send ( "" ) ;
72+ 73+ if ( timer > ( 1 / frameRate ) )
74+ {
75+ timer = 0 ;
76+ PointUpdate ( ) ;
77+ }
78+ 79+ if ( ! fullbodyIK )
80+ {
81+ IKFind ( ) ;
82+ }
83+ else
84+ {
85+ IKSet ( ) ;
86+ }
87+ }
88+ 89+ private void OnDestroy ( )
90+ {
91+ ws . Close ( ) ;
92+ ws = null ;
93+ }
94+ 95+ private void PointUpdate ( )
96+ {
97+ if ( nowFrame < 100 )
98+ {
99+ nowFrame ++ ;
100+ if ( isReceived )
101+ {
102+ bodyParts = JsonUtility . FromJson < BodyParts > ( receivedJson ) ;
103+ for ( int i = 0 ; i < 17 ; i ++ )
104+ {
105+ x [ i ] = bodyParts . parts [ i ] . x ;
106+ y [ i ] = bodyParts . parts [ i ] . y ;
107+ z [ i ] = bodyParts . parts [ i ] . z ;
108+ }
109+ isReceived = false ;
110+ }
111+ 112+ for ( int i = 0 ; i < 17 ; i ++ )
113+ {
114+ points [ i ] = new Vector3 ( x [ i ] , y [ i ] , - z [ i ] ) ;
115+ }
116+ 117+ for ( int i = 0 ; i < 12 ; i ++ )
118+ {
119+ normalizeBone [ i ] = ( points [ boneJoint [ i , 1 ] ] - points [ boneJoint [ i , 0 ] ] ) . normalized ;
120+ }
121+ }
122+ }
123+ 124+ private void IKFind ( )
125+ {
126+ fullbodyIK = GameObject . Find ( "FullBodyIK" ) ;
127+ if ( fullbodyIK )
128+ {
129+ for ( int i = 0 ; i < Enum . GetNames ( typeof ( OpenPoseRef ) ) . Length ; i ++ )
130+ {
131+ Transform obj = GameObject . Find ( Enum . GetName ( typeof ( OpenPoseRef ) , i ) ) . transform ;
132+ if ( obj )
133+ {
134+ boneList . Add ( obj ) ;
135+ }
136+ }
137+ 138+ for ( int i = 0 ; i < Enum . GetNames ( typeof ( NormalizeBoneRef ) ) . Length ; i ++ )
139+ {
140+ boneDistance [ i ] = Vector3 . Distance ( boneList [ normalizeJoint [ i , 0 ] ] . position ,
141+ boneList [ normalizeJoint [ i , 1 ] ] . position ) ;
142+ }
143+ }
144+ }
145+ 146+ private void IKSet ( )
147+ {
148+ if ( Math . Abs ( points [ 0 ] . x ) < 1000 && Math . Abs ( points [ 0 ] . y ) < 1000 && Math . Abs ( points [ 0 ] . z ) < 1000 )
149+ {
150+ boneList [ 0 ] . position = points [ 0 ] * 0.001f + Vector3 . up * 0.8f ;
151+ }
152+ 153+ for ( int i = 0 ; i < 12 ; i ++ )
154+ {
155+ boneList [ normalizeJoint [ i , 1 ] ] . position = Vector3 . Lerp (
156+ boneList [ normalizeJoint [ i , 1 ] ] . position ,
157+ boneList [ normalizeJoint [ i , 0 ] ] . position + boneDistance [ i ] * normalizeBone [ i ]
158+ , 0.05f
159+ ) ;
160+ DrawLine ( boneList [ normalizeJoint [ i , 0 ] ] . position , boneList [ normalizeJoint [ i , 1 ] ] . position , Color . red ) ;
161+ }
162+ 163+ for ( int i = 0 ; i < joints . Length / 2 ; i ++ )
164+ {
165+ DrawLine ( points [ joints [ i , 0 ] ] * 0.001f + new Vector3 ( - 1 , 0.8f , 0 ) ,
166+ points [ joints [ i , 1 ] ] * 0.001f + new Vector3 ( - 1 , 0.8f , 0 ) , Color . blue ) ;
167+ }
168+ }
169+ 170+ private void DrawLine ( Vector3 s , Vector3 e , Color c )
171+ {
172+ Debug . DrawLine ( s , e , c ) ;
173+ }
174+ }
175+ 176+ internal enum OpenPoseRef
177+ {
178+ Hips ,
179+ LeftKnee ,
180+ LeftFoot ,
181+ RightKnee ,
182+ RightFoot ,
183+ Neck ,
184+ Head ,
185+ RightArm ,
186+ RightElbow ,
187+ RightWrist ,
188+ LeftArm ,
189+ LeftElbow ,
190+ LeftWrist
191+ } ;
192+ 193+ internal enum NormalizeBoneRef
194+ {
195+ Hip2LeftKnee ,
196+ LeftKnee2LeftFoot ,
197+ Hip2RightKnee ,
198+ RightKnee2RightFoot ,
199+ Hip2Neck ,
200+ Neck2Head ,
201+ Neck2RightArm ,
202+ RightArm2RightElbow ,
203+ RightElbow2RightWrist ,
204+ Neck2LeftArm ,
205+ LeftArm2LeftElbow ,
206+ LeftElbow2LeftWrist
207+ } ;
208+ 209+ [ System . Serializable ]
210+ public class BodyParts
211+ {
212+ public Position [ ] parts ;
213+ }
214+ 215+ [ System . Serializable ]
216+ public class Position
217+ {
218+ public float x ;
219+ public float y ;
220+ public float z ;
221+ }
0 commit comments