/** 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); }
/** 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; } }
/// <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); }
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; }