comparison Orchestland/Assets/LeapMotion/Scripts/Utils/GrabbingHand.cs @ 1:f7675884f2a1

Add Orchestland project
author Daiki OYAKAWA <e135764@ie.u-ryukyu.ac.jp>
date Fri, 17 Jul 2015 23:09:20 +0900
parents
children
comparison
equal deleted inserted replaced
0:347d21cdfc22 1:f7675884f2a1
1 /******************************************************************************\
2 * Copyright (C) Leap Motion, Inc. 2011-2014. *
3 * Leap Motion proprietary. Licensed under Apache 2.0 *
4 * Available at http://www.apache.org/licenses/LICENSE-2.0.html *
5 \******************************************************************************/
6
7 using UnityEngine;
8 using System.Collections;
9 using Leap;
10
11 // Leap Motion hand script that detects pinches and grabs the closest rigidbody.
12 public class GrabbingHand : MonoBehaviour {
13
14 public enum PinchState {
15 kPinched,
16 kReleased,
17 kReleasing
18 }
19
20 // Layers that we can grab.
21 public LayerMask grabbableLayers = ~0;
22
23 // Ratio of the length of the proximal bone of the thumb that will trigger a pinch.
24 public float grabTriggerDistance = 0.7f;
25
26 // Ratio of the length of the proximal bone of the thumb that will trigger a release.
27 public float releaseTriggerDistance = 1.2f;
28
29 // Maximum distance of an object that we can grab when pinching.
30 public float grabObjectDistance = 2.0f;
31
32 // If the object gets far from the pinch we'll break the bond.
33 public float releaseBreakDistance = 0.3f;
34
35 // Curve of the trailing off of strength as you release the object.
36 public AnimationCurve releaseStrengthCurve;
37
38 // Filtering the rotation of grabbed object.
39 public float rotationFiltering = 0.4f;
40
41 // Filtering the movement of grabbed object.
42 public float positionFiltering = 0.4f;
43
44 // Minimum tracking confidence of the hand that will cause a change of state.
45 public float minConfidence = 0.1f;
46
47 // Clamps the movement of the grabbed object.
48 public Vector3 maxMovement = new Vector3(Mathf.Infinity, Mathf.Infinity, Mathf.Infinity);
49 public Vector3 minMovement = new Vector3(-Mathf.Infinity, -Mathf.Infinity, -Mathf.Infinity);
50
51 protected PinchState pinch_state_;
52 protected Collider active_object_;
53
54 protected float last_max_angular_velocity_;
55 protected Quaternion rotation_from_palm_;
56
57 protected Vector3 current_pinch_position_;
58 protected Vector3 filtered_pinch_position_;
59 protected Vector3 object_pinch_offset_;
60 protected Quaternion palm_rotation_;
61
62 void Start() {
63 pinch_state_ = PinchState.kReleased;
64 active_object_ = null;
65 last_max_angular_velocity_ = 0.0f;
66 rotation_from_palm_ = Quaternion.identity;
67 current_pinch_position_ = Vector3.zero;
68 filtered_pinch_position_ = Vector3.zero;
69 object_pinch_offset_ = Vector3.zero;
70 palm_rotation_ = Quaternion.identity;
71 }
72
73 void OnDestroy() {
74 OnRelease();
75 }
76
77 // Finds the closest grabbable object within range of the pinch.
78 protected Collider FindClosestGrabbableObject(Vector3 pinch_position) {
79 Collider closest = null;
80 float closest_sqr_distance = grabObjectDistance * grabObjectDistance;
81 Collider[] close_things =
82 Physics.OverlapSphere(pinch_position, grabObjectDistance, grabbableLayers);
83
84 for (int j = 0; j < close_things.Length; ++j) {
85 float sqr_distance = (pinch_position - close_things[j].transform.position).sqrMagnitude;
86
87 if (close_things[j].GetComponent<Rigidbody>() != null && sqr_distance < closest_sqr_distance &&
88 !close_things[j].transform.IsChildOf(transform) &&
89 close_things[j].tag != "NotGrabbable") {
90
91 GrabbableObject grabbable = close_things[j].GetComponent<GrabbableObject>();
92 if (grabbable == null || !grabbable.IsGrabbed()) {
93 closest = close_things[j];
94 closest_sqr_distance = sqr_distance;
95 }
96 }
97 }
98
99 return closest;
100 }
101
102 // Notify grabbable objects when they are ready to grab :)
103 protected void Hover() {
104 Collider hover = FindClosestGrabbableObject(current_pinch_position_);
105
106 if (hover != active_object_ && active_object_ != null) {
107 GrabbableObject old_grabbable = active_object_.GetComponent<GrabbableObject>();
108
109 if (old_grabbable != null)
110 old_grabbable.OnStopHover();
111 }
112
113 if (hover != null) {
114 GrabbableObject new_grabbable = hover.GetComponent<GrabbableObject>();
115
116 if (new_grabbable != null)
117 new_grabbable.OnStartHover();
118 }
119
120 active_object_ = hover;
121 }
122
123 protected void StartPinch() {
124 // Only pinch if we're hovering over an object.
125 if (active_object_ == null)
126 return;
127
128 HandModel hand_model = GetComponent<HandModel>();
129 Leap.Utils.IgnoreCollisions(gameObject, active_object_.gameObject, true);
130 GrabbableObject grabbable = active_object_.GetComponent<GrabbableObject>();
131
132 // Setup initial position and rotation conditions.
133 palm_rotation_ = hand_model.GetPalmRotation();
134 object_pinch_offset_ = Vector3.zero;
135
136 // If we don't center the object, find the closest point in the collider for our grab point.
137 if (grabbable == null || !grabbable.centerGrabbedObject) {
138 Vector3 delta_position = active_object_.transform.position - current_pinch_position_;
139
140 Ray pinch_ray = new Ray(current_pinch_position_, delta_position);
141 RaycastHit pinch_hit;
142
143 // If we raycast hits the object, we're outside the collider so grab the hit point.
144 // If not, we're inside the collider so just use the pinch position.
145 if (active_object_.Raycast(pinch_ray, out pinch_hit, grabObjectDistance))
146 object_pinch_offset_ = active_object_.transform.position - pinch_hit.point;
147 else
148 object_pinch_offset_ = active_object_.transform.position - current_pinch_position_;
149 }
150
151 filtered_pinch_position_ = active_object_.transform.position - object_pinch_offset_;
152 object_pinch_offset_ = Quaternion.Inverse(active_object_.transform.rotation) *
153 object_pinch_offset_;
154 rotation_from_palm_ = Quaternion.Inverse(palm_rotation_) * active_object_.transform.rotation;
155
156 // If we can rotate the object quickly, increase max angular velocity for now.
157 if (grabbable == null || grabbable.rotateQuickly) {
158 last_max_angular_velocity_ = active_object_.GetComponent<Rigidbody>().maxAngularVelocity;
159 active_object_.GetComponent<Rigidbody>().maxAngularVelocity = Mathf.Infinity;
160 }
161
162 if (grabbable != null) {
163 // Notify grabbable object that it was grabbed.
164 grabbable.OnGrab();
165
166 if (grabbable.useAxisAlignment) {
167 // If this option is enabled we only want to align the object axis with the palm axis
168 // so we'll cancel out any rotation about the aligned axis.
169 Vector3 palm_vector = grabbable.rightHandAxis;
170 if (hand_model.GetLeapHand().IsLeft)
171 palm_vector = Vector3.Scale(palm_vector, new Vector3(-1, 1, 1));
172
173 Vector3 axis_in_palm = rotation_from_palm_ * grabbable.objectAxis;
174 Quaternion axis_correction = Quaternion.FromToRotation(axis_in_palm, palm_vector);
175 if (Vector3.Dot(axis_in_palm, palm_vector) < 0)
176 axis_correction = Quaternion.FromToRotation(axis_in_palm, -palm_vector);
177
178 rotation_from_palm_ = axis_correction * rotation_from_palm_;
179 }
180 }
181 }
182
183 protected void OnRelease() {
184 if (active_object_ != null) {
185 // Notify the grabbable object that is was released.
186 GrabbableObject grabbable = active_object_.GetComponent<GrabbableObject>();
187 if (grabbable != null)
188 grabbable.OnRelease();
189
190 if (grabbable == null || grabbable.rotateQuickly)
191 active_object_.GetComponent<Rigidbody>().maxAngularVelocity = last_max_angular_velocity_;
192
193 Leap.Utils.IgnoreCollisions(gameObject, active_object_.gameObject, false);
194 }
195 active_object_ = null;
196
197 Hover();
198 }
199
200 protected PinchState GetNewPinchState() {
201 HandModel hand_model = GetComponent<HandModel>();
202 Hand leap_hand = hand_model.GetLeapHand();
203
204 Vector leap_thumb_tip = leap_hand.Fingers[0].TipPosition;
205 float closest_distance = Mathf.Infinity;
206
207 // Check thumb tip distance to joints on all other fingers.
208 // If it's close enough, you're pinching.
209 for (int i = 1; i < HandModel.NUM_FINGERS; ++i) {
210 Finger finger = leap_hand.Fingers[i];
211
212 for (int j = 0; j < FingerModel.NUM_BONES; ++j) {
213 Vector leap_joint_position = finger.Bone((Bone.BoneType)j).NextJoint;
214
215 float thumb_tip_distance = leap_joint_position.DistanceTo(leap_thumb_tip);
216 closest_distance = Mathf.Min(closest_distance, thumb_tip_distance);
217 }
218 }
219
220 // Scale trigger distance by thumb proximal bone length.
221 float proximal_length = leap_hand.Fingers[0].Bone(Bone.BoneType.TYPE_PROXIMAL).Length;
222 float trigger_distance = proximal_length * grabTriggerDistance;
223 float release_distance = proximal_length * releaseTriggerDistance;
224
225 if (closest_distance <= trigger_distance)
226 return PinchState.kPinched;
227 if (closest_distance <= release_distance && pinch_state_ != PinchState.kReleased &&
228 !ObjectReleaseBreak(current_pinch_position_))
229 return PinchState.kReleasing;
230 return PinchState.kReleased;
231 }
232
233 protected void UpdatePinchPosition() {
234 HandModel hand_model = GetComponent<HandModel>();
235 current_pinch_position_ = 0.5f * (hand_model.fingers[0].GetTipPosition() +
236 hand_model.fingers[1].GetTipPosition());
237
238 Vector3 delta_pinch = current_pinch_position_ - filtered_pinch_position_;
239 filtered_pinch_position_ += (1.0f - positionFiltering) * delta_pinch;
240 }
241
242 protected void UpdatePalmRotation() {
243 HandModel hand_model = GetComponent<HandModel>();
244 palm_rotation_ = Quaternion.Slerp(palm_rotation_, hand_model.GetPalmRotation(),
245 1.0f - rotationFiltering);
246 }
247
248 protected bool ObjectReleaseBreak(Vector3 pinch_position) {
249 if (active_object_ == null)
250 return true;
251
252 Vector3 delta_position = pinch_position - active_object_.transform.position;
253 return delta_position.magnitude > releaseBreakDistance;
254 }
255
256 // If we're in a pinch state, just move the object to the right spot using velocities.
257 protected void ContinueHardPinch() {
258 Quaternion target_rotation = palm_rotation_ * rotation_from_palm_;
259
260 Vector3 target_position = filtered_pinch_position_ + target_rotation * object_pinch_offset_;
261 target_position.x = Mathf.Clamp(target_position.x, minMovement.x, maxMovement.x);
262 target_position.y = Mathf.Clamp(target_position.y, minMovement.y, maxMovement.y);
263 target_position.z = Mathf.Clamp(target_position.z, minMovement.z, maxMovement.z);
264 Vector3 velocity = (target_position - active_object_.transform.position) / Time.deltaTime;
265 active_object_.GetComponent<Rigidbody>().velocity = velocity;
266
267 Quaternion delta_rotation = target_rotation *
268 Quaternion.Inverse(active_object_.transform.rotation);
269
270 float angle = 0.0f;
271 Vector3 axis = Vector3.zero;
272 delta_rotation.ToAngleAxis(out angle, out axis);
273
274 if (angle >= 180) {
275 angle = 360 - angle;
276 axis = -axis;
277 }
278 if (angle != 0)
279 active_object_.GetComponent<Rigidbody>().angularVelocity = angle * axis;
280 }
281
282 // If we are releasing the object only apply a weaker force to the object
283 // like it's sliding through your fingers.
284 protected void ContinueSoftPinch() {
285 Quaternion target_rotation = palm_rotation_ * rotation_from_palm_;
286
287 Vector3 target_position = filtered_pinch_position_ + target_rotation * object_pinch_offset_;
288 Vector3 delta_position = target_position - active_object_.transform.position;
289
290 float strength = (releaseBreakDistance - delta_position.magnitude) / releaseBreakDistance;
291 strength = releaseStrengthCurve.Evaluate(strength);
292 active_object_.GetComponent<Rigidbody>().AddForce(delta_position.normalized * strength * positionFiltering,
293 ForceMode.Acceleration);
294
295 Quaternion delta_rotation = target_rotation *
296 Quaternion.Inverse(active_object_.transform.rotation);
297
298 float angle = 0.0f;
299 Vector3 axis = Vector3.zero;
300 delta_rotation.ToAngleAxis(out angle, out axis);
301
302 active_object_.GetComponent<Rigidbody>().AddTorque(strength * rotationFiltering * angle * axis,
303 ForceMode.Acceleration);
304 }
305
306 void FixedUpdate() {
307 UpdatePalmRotation();
308 UpdatePinchPosition();
309 HandModel hand_model = GetComponent<HandModel>();
310 Hand leap_hand = hand_model.GetLeapHand();
311
312 if (leap_hand == null)
313 return;
314
315 PinchState new_pinch_state = GetNewPinchState();
316 if (pinch_state_ == PinchState.kPinched) {
317 if (new_pinch_state == PinchState.kReleased)
318 OnRelease();
319 else if (active_object_ != null)
320 ContinueHardPinch();
321 }
322 else if (pinch_state_ == PinchState.kReleasing) {
323 if (new_pinch_state == PinchState.kReleased)
324 OnRelease();
325 else if (new_pinch_state == PinchState.kPinched)
326 StartPinch();
327 else if (active_object_ != null)
328 ContinueSoftPinch();
329 }
330 else {
331 if (new_pinch_state == PinchState.kPinched)
332 StartPinch();
333 else
334 Hover();
335 }
336 pinch_state_ = new_pinch_state;
337 }
338 }