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