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