/** Moves the object by applying forces and torque. */ public override void MoveTo(ReadonlyList <Hand> hands, PhysicsMoveInfo info, Vector3 solvedPosition, Quaternion solvedRotation) { if (info.shouldTeleport) { _obj.warper.Teleport(solvedPosition, solvedRotation); } else { Vector3 targetVelocity = PhysicsUtility.ToLinearVelocity(_obj.warper.RigidbodyPosition, solvedPosition, Time.fixedDeltaTime); Vector3 targetAngularVelocity = PhysicsUtility.ToAngularVelocity(_obj.warper.RigidbodyRotation, solvedRotation, Time.fixedDeltaTime); float targetSpeedSqrd = targetVelocity.sqrMagnitude; if (targetSpeedSqrd > _maxVelocitySqrd) { float targetPercent = (_maxVelocity * _obj.Manager.SimulationScale) / Mathf.Sqrt(targetSpeedSqrd); targetVelocity *= targetPercent; targetAngularVelocity *= targetPercent; } float followStrength = _strengthByDistance.Evaluate(info.remainingDistanceLastFrame / _obj.Manager.SimulationScale); Vector3 lerpedVelocity = Vector3.Lerp(_obj.rigidbody.velocity, targetVelocity, followStrength); Vector3 lerpedAngularVelocity = Vector3.Lerp(_obj.rigidbody.angularVelocity, targetAngularVelocity, followStrength); Vector3 centerOfMassOffset = _obj.warper.RigidbodyRotation * _obj.rigidbody.centerOfMass; _obj.rigidbody.velocity = lerpedVelocity + Vector3.Cross(lerpedAngularVelocity, centerOfMassOffset); _obj.rigidbody.angularVelocity = lerpedAngularVelocity; } }
/** Transfers the averaged velocity to the released object. */ public override void OnThrow(Hand throwingHand) { if (_velocityQueue.Count < 2) { _obj.rigidbody.velocity = Vector3.zero; _obj.rigidbody.angularVelocity = Vector3.zero; return; } float windowEnd = Time.fixedTime - _windowDelay; float windowStart = windowEnd - _windowLength; //0 occurs before 1 //start occurs before end VelocitySample start0, start1; VelocitySample end0, end1; VelocitySample s0, s1; s0 = s1 = start0 = start1 = end0 = end1 = _velocityQueue.Dequeue(); while (_velocityQueue.Count != 0) { s0 = s1; s1 = _velocityQueue.Dequeue(); if (s0.time < windowStart && s1.time >= windowStart) { start0 = s0; start1 = s1; } if (s0.time < windowEnd && s1.time >= windowEnd) { end0 = s0; end1 = s1; //We have assigned both start and end and can break out of loop _velocityQueue.Clear(); break; } } VelocitySample start = VelocitySample.Interpolate(start0, start1, windowStart); VelocitySample end = VelocitySample.Interpolate(end0, end1, windowEnd); Vector3 interpolatedVelocity = PhysicsUtility.ToLinearVelocity(start.position, end.position, _windowLength); //If trying to throw the object backwards into the hand Vector3 relativeVelocity = interpolatedVelocity - throwingHand.PalmVelocity.ToVector3(); if (Vector3.Dot(relativeVelocity, throwingHand.PalmNormal.ToVector3()) < 0) { interpolatedVelocity -= Vector3.Project(relativeVelocity, throwingHand.PalmNormal.ToVector3()); } _obj.rigidbody.velocity = interpolatedVelocity; _obj.rigidbody.angularVelocity = PhysicsUtility.ToAngularVelocity(start.rotation, end.rotation, _windowLength); _obj.rigidbody.velocity *= _velocityMultiplierCurve.Evaluate(_obj.rigidbody.velocity.magnitude); }
protected virtual void simulateFrame(Frame frame) { _activityManager.UpdateState(frame); var active = _activityManager.ActiveBehaviours; for (int i = 0; i < active.Count; i++) { active[i].NotifyPreSolve(); } dispatchOnHandsHoldingAll(frame, isPhysics: true); //Apply soft contacts from both hands in unified solve //(this will clear softContacts and originalVelocities as well) if (softContacts.Count > 0) { using (new ProfilerSample("Apply Soft Contacts", this)) { PhysicsUtility.applySoftContacts(softContacts, originalVelocities); } } updateInteractionStateChanges(frame); for (int i = 0; i < active.Count; i++) { active[i].NotifyPostSolve(); } }
/// <summary> /// Transfers the averaged velocity to the released object. /// </summary> public void OnThrow(InteractionBehaviour intObj, InteractionController throwingController) { if (_velocityQueue.Count < 2) { intObj.rigidbody.velocity = Vector3.zero; intObj.rigidbody.angularVelocity = Vector3.zero; return; } float windowEnd = Time.fixedTime - _windowDelay; float windowStart = windowEnd - _windowLength; // 0 occurs before 1, // start occurs before end. VelocitySample start0, start1; VelocitySample end0, end1; VelocitySample s0, s1; s0 = s1 = start0 = start1 = end0 = end1 = _velocityQueue.Dequeue(); while (_velocityQueue.Count != 0) { s0 = s1; s1 = _velocityQueue.Dequeue(); if (s0.time < windowStart && s1.time >= windowStart) { start0 = s0; start1 = s1; } if (s0.time < windowEnd && s1.time >= windowEnd) { end0 = s0; end1 = s1; // We have assigned both start and end and can break out of the loop. _velocityQueue.Clear(); break; } } VelocitySample start = VelocitySample.Interpolate(start0, start1, windowStart); VelocitySample end = VelocitySample.Interpolate(end0, end1, windowEnd); Vector3 interpolatedVelocity = PhysicsUtility.ToLinearVelocity(start.position, end.position, _windowLength); intObj.rigidbody.velocity = interpolatedVelocity; intObj.rigidbody.angularVelocity = PhysicsUtility.ToAngularVelocity(start.rotation, end.rotation, _windowLength); intObj.rigidbody.velocity *= _velocityMultiplierCurve.Evaluate(intObj.rigidbody.velocity.magnitude); }
void FixedUpdate() { OnPrePhysicalUpdate(); refreshInteractionControllers(); #if UNITY_EDITOR if (!Application.isPlaying) { return; } #endif using (new ProfilerSample("Interaction Manager FixedUpdate", this.gameObject)) { // Ensure scale information is up-to-date. _scale = this.transform.lossyScale.x; // Update each interaction controller (Leap hands or supported VR controllers). fixedUpdateInteractionControllers(); // Perform each interaction object's FixedUpdateObject. using (new ProfilerSample("FixedUpdateObject per-InteractionBehaviour")) { foreach (var interactionObj in _interactionObjects) { interactionObj.FixedUpdateObject(); } } // Apply soft contacts from all controllers in a unified solve. // (This will clear softContacts and originalVelocities as well.) using (new ProfilerSample("Apply Soft Contacts")) { if (_softContacts.Count > 0) { PhysicsUtility.applySoftContacts(_softContacts, _softContactOriginalVelocities); } } } OnPostPhysicalUpdate(); updateMovingFrameOfReferenceSupport(); }
public void MoveTo(Vector3 solvedPosition, Quaternion solvedRotation, InteractionBehaviour intObj, bool justGrasped) { Vector3 solvedCenterOfMass = solvedRotation * intObj.rigidbody.centerOfMass + solvedPosition; Vector3 currCenterOfMass = intObj.rigidbody.rotation * intObj.rigidbody.centerOfMass + intObj.rigidbody.position; Vector3 targetVelocity = PhysicsUtility.ToLinearVelocity(currCenterOfMass, solvedCenterOfMass, Time.fixedDeltaTime); Vector3 targetAngularVelocity = PhysicsUtility.ToAngularVelocity(intObj.rigidbody.rotation, solvedRotation, Time.fixedDeltaTime); // Clamp targetVelocity by _maxVelocity. float maxScaledVelocity = _maxVelocity * intObj.manager.SimulationScale; float targetSpeedSqrd = targetVelocity.sqrMagnitude; if (targetSpeedSqrd > maxScaledVelocity * maxScaledVelocity) { float targetPercent = maxScaledVelocity / Mathf.Sqrt(targetSpeedSqrd); targetVelocity *= targetPercent; targetAngularVelocity *= targetPercent; } float followStrength = 1F; if (!justGrasped) { float remainingDistanceLastFrame = Vector3.Distance(_lastSolvedCoMPosition, currCenterOfMass); followStrength = _strengthByDistance.Evaluate(remainingDistanceLastFrame / intObj.manager.SimulationScale); } Vector3 lerpedVelocity = Vector3.Lerp(intObj.rigidbody.velocity, targetVelocity, followStrength); Vector3 lerpedAngularVelocity = Vector3.Lerp(intObj.rigidbody.angularVelocity, targetAngularVelocity, followStrength); intObj.rigidbody.velocity = lerpedVelocity; intObj.rigidbody.angularVelocity = lerpedAngularVelocity; _lastSolvedCoMPosition = solvedCenterOfMass; // Store the target position and rotation to prevent slippage in SwapGrasp // scenarios. intObj.latestScheduledGraspPose = new Pose(solvedPosition, solvedRotation); }
public void MoveTo(Vector3 solvedPosition, Quaternion solvedRotation, InteractionBehaviour intObj, bool justGrasped) { Vector3 targetVelocity = PhysicsUtility.ToLinearVelocity(intObj.rigidbody.position, solvedPosition, Time.fixedDeltaTime); Vector3 targetAngularVelocity = PhysicsUtility.ToAngularVelocity(intObj.rigidbody.rotation, solvedRotation, Time.fixedDeltaTime); // Clamp by _maxVelocity. float maxScaledVelocity = _maxVelocity * intObj.manager.SimulationScale; float targetSpeedSqrd = targetVelocity.sqrMagnitude; if (targetSpeedSqrd > maxScaledVelocity * maxScaledVelocity) { float targetPercent = maxScaledVelocity / Mathf.Sqrt(targetSpeedSqrd); targetVelocity *= targetPercent; targetAngularVelocity *= targetPercent; } _useLastSolvedPosition = !justGrasped; float followStrength = 1F; if (_useLastSolvedPosition) { float remainingDistanceLastFrame = Vector3.Distance(_lastSolvedPosition, intObj.rigidbody.position); followStrength = _strengthByDistance.Evaluate(remainingDistanceLastFrame / intObj.manager.SimulationScale); } Vector3 lerpedVelocity = Vector3.Lerp(intObj.rigidbody.velocity, targetVelocity, followStrength); Vector3 lerpedAngularVelocity = Vector3.Lerp(intObj.rigidbody.angularVelocity, targetAngularVelocity, followStrength); Vector3 centerOfMassOffset = intObj.rigidbody.rotation * intObj.rigidbody.centerOfMass; intObj.rigidbody.velocity = lerpedVelocity + Vector3.Cross(lerpedAngularVelocity, centerOfMassOffset); intObj.rigidbody.angularVelocity = lerpedAngularVelocity; _lastSolvedPosition = solvedPosition; }
void FixedUpdate() { OnPrePhysicalUpdate(); // Physics should only be synced once at the beginning of the physics simulation. // (Will be re-set to its original value at the end of the update.) #if UNITY_2017_2_OR_NEWER var preUpdateAutoSyncTransforms = Physics.autoSyncTransforms; Physics.autoSyncTransforms = false; #endif try { refreshInteractionControllers(); #if UNITY_EDITOR if (!Application.isPlaying) { return; } #endif using (new ProfilerSample("Interaction Manager FixedUpdate", this.gameObject)) { // Ensure scale information is up-to-date. _scale = this.transform.lossyScale.x; // Update each interaction controller (Leap hands or supported VR controllers). fixedUpdateInteractionControllers(); // Perform each interaction object's FixedUpdateObject. using (new ProfilerSample("FixedUpdateObject per-InteractionBehaviour")) { foreach (var interactionObj in _interactionObjects) { interactionObj.FixedUpdateObject(); } } // Apply soft contacts from all controllers in a unified solve. // (This will clear softContacts and originalVelocities as well.) using (new ProfilerSample("Apply Soft Contacts")) { if (_drawControllerRuntimeGizmos) { _softContactsToDraw = new List <PhysicsUtility.SoftContact>(_softContacts); } if (_softContacts.Count > 0) { PhysicsUtility.applySoftContacts(_softContacts, _softContactOriginalVelocities); } } } OnPostPhysicalUpdate(); updateMovingFrameOfReferenceSupport(); if (autoGenerateLayers) { autoUpdateContactBoneLayerCollision(); } } finally { #if UNITY_2017_2_OR_NEWER // Restore the autoSyncTransforms setting to whatever the user had it as before // the Manager FixedUpdate. Physics.autoSyncTransforms = preUpdateAutoSyncTransforms; #endif } }
/** Updates this hand model. */ public override void UpdateHand() { #if UNITY_EDITOR if (!EditorApplication.isPlaying) { return; } #endif using (new ProfilerSample("Update InteractionBrushHand", this)) { using (new ProfilerSample("Update InteractionBrushBones", this)) { float deadzone = DEAD_ZONE_FRACTION * _hand.Fingers[1].Bone((Bone.BoneType) 1).Width; for (int fingerIndex = 0; fingerIndex < N_FINGERS; fingerIndex++) { for (int jointIndex = 0; jointIndex < N_ACTIVE_BONES; jointIndex++) { Bone bone = _hand.Fingers[fingerIndex].Bone((Bone.BoneType)(jointIndex) + 1); int boneArrayIndex = fingerIndex * N_ACTIVE_BONES + jointIndex; UpdateBone(bone, boneArrayIndex, deadzone); } } //Update Palm UpdateBone(_hand.Fingers[(int)Finger.FingerType.TYPE_MIDDLE].Bone(Bone.BoneType.TYPE_METACARPAL), N_FINGERS * N_ACTIVE_BONES, deadzone); } if (_softContactEnabled) { //SOFT CONTACT COLLISIONS using (new ProfilerSample("Update Soft Contact", this)) { //Generate Contacts bool softlyContacting = false; using (new ProfilerSample("Generate Soft Contacts", this)) { for (int fingerIndex = 0; fingerIndex < 5; fingerIndex++) { for (int jointIndex = 0; jointIndex < 4; jointIndex++) { Bone bone = _hand.Fingers[fingerIndex].Bone((Bone.BoneType)(jointIndex)); int boneArrayIndex = fingerIndex * 4 + jointIndex; Vector3 boneCenter = bone.Center.ToVector3(); ////Generate and Fill softContacts with SoftContacts that are intersecting a sphere at boneCenter, with radius softContactBoneRadius bool sphereIntersecting; using (new ProfilerSample("Generate Soft Contact", this)) { if (_manager != null) { sphereIntersecting = PhysicsUtility.generateSphereContacts(boneCenter, softContactBoneRadius, (boneCenter - previousBoneCenters[boneArrayIndex]) / Time.fixedDeltaTime, 1 << _manager.InteractionLayer, ref _manager.softContacts, ref _manager.originalVelocities, ref tempColliderArray); } else { sphereIntersecting = PhysicsUtility.generateSphereContacts(boneCenter, softContactBoneRadius, (boneCenter - previousBoneCenters[boneArrayIndex]) / Time.fixedDeltaTime, ~(1 << 2), ref softContacts, ref originalVelocities, ref tempColliderArray); } } softlyContacting = sphereIntersecting ? true : softlyContacting; } } } if (softlyContacting) { //(If we have a manager, let it handle resolving the contacts of both hands in one unified solve) if (_manager == null) { using (new ProfilerSample("Apply Per Hand Soft Contacts", this)) { PhysicsUtility.applySoftContacts(softContacts, originalVelocities); } } disableSoftContactEnqueued = false; } else { //If there are no detected Contacts, exit soft contact mode disableSoftContact(); } } } //Update the last positions of the bones with this frame for (int fingerIndex = 0; fingerIndex < 5; fingerIndex++) { for (int jointIndex = 0; jointIndex < 4; jointIndex++) { Bone bone = _hand.Fingers[fingerIndex].Bone((Bone.BoneType)(jointIndex)); int boneArrayIndex = fingerIndex * 4 + jointIndex; previousBoneCenters[boneArrayIndex] = bone.Center.ToVector3(); } } } }