Skip to content

Commit ccc9ea5

Browse files
dooly123dooly123
committed
breaking down bone driver
1 parent 4d4406f commit ccc9ea5

10 files changed

Lines changed: 588 additions & 383 deletions

File tree

‎Basis/Packages/com.basis.framework/Avatar/TransformBinders/BoneControl/BasisLocalBoneControl.cs‎

Lines changed: 35 additions & 90 deletions
Original file line numberDiff line numberDiff line change
@@ -124,111 +124,56 @@ public BasisHasRigLayer HasRigLayer
124124
/// <summary>Scaled T-pose local-space reference (e.g., by avatar height change).</summary>
125125
[SerializeField] public BasisCalibratedCoords TposeLocalScaled = new BasisCalibratedCoords();
126126

127-
/// <summary>
128-
/// Computes the outgoing local and world pose for this bone.
129-
/// If tracking is present, copies (or offset-corrects) incoming data; otherwise,
130-
/// lerps toward the <see cref="Target"/> plus <see cref="ScaledOffset"/>.
131-
/// </summary>
132-
/// <param name="parentMatrix">Parent transform matrix for world conversion.</param>
133-
/// <param name="DeltaTime">Frame delta time (unscaled).</param>
134-
public void ComputeMovementLocal(Matrix4x4 parentMatrix, float DeltaTime)
127+
public void SetIncoming(Vector3 position, Quaternion rotation)
135128
{
136-
if (hasTrackerDriver == BasisHasTracked.HasTracker)
137-
{
138-
if (UseInverseOffset)
139-
{
140-
Vector3 DestinationPosition = IncomingData.position + IncomingData.rotation * InverseOffsetFromBone.position;
141-
Quaternion DestinationRotation = IncomingData.rotation * InverseOffsetFromBone.rotation;
142-
143-
// Smooth toward destination
144-
OutGoingData.position = Vector3.Lerp(LastRunData.position, DestinationPosition, trackersmooth);
145-
OutGoingData.rotation = Quaternion.Slerp(LastRunData.rotation, DestinationRotation, trackersmooth);
146-
}
147-
else
148-
{
149-
// Directly use the incoming tracker pose
150-
OutGoingData.rotation = IncomingData.rotation;
151-
OutGoingData.position = IncomingData.position;
152-
}
153-
154-
ApplyWorldAndLast(parentMatrix);
155-
}
156-
else
157-
{
158-
if (!HasVirtualOverride && HasTarget)
159-
{
160-
OutGoingData.rotation = ApplyLerpToQuaternion(DeltaTime, LastRunData.rotation, Target.OutGoingData.rotation);
161-
162-
// Offset relative to target’s rotation
163-
Vector3 customDirection = Target.OutGoingData.rotation * ScaledOffset;
164-
Vector3 targetPosition = Target.OutGoingData.position + customDirection;
165-
166-
float lerpFactor = ClampInterpolationFactor(PositionLerpAmount, DeltaTime);
167-
OutGoingData.position = Vector3.Lerp(LastRunData.position, targetPosition, lerpFactor);
168-
169-
ApplyWorldAndLast(parentMatrix);
170-
}
171-
}
129+
IncomingData.position = position;
130+
IncomingData.rotation = rotation;
172131
}
173132

174-
/// <summary>
175-
/// Interpolates between two rotations using a speed that increases with angular difference.
176-
/// </summary>
177-
/// <param name="DeltaTime">Frame delta time.</param>
178-
/// <param name="CurrentRotation">Current rotation.</param>
179-
/// <param name="FutureRotation">Target rotation.</param>
180-
/// <returns>Interpolated rotation.</returns>
181-
public Quaternion ApplyLerpToQuaternion(float DeltaTime, Quaternion CurrentRotation, Quaternion FutureRotation)
133+
public void SetOutgoing(Vector3 position, Quaternion rotation)
182134
{
183-
// Dot product ≈ cosine of half-angle; detects similarity
184-
float dotProduct = math.dot(CurrentRotation, FutureRotation);
185-
186-
// Early-outs for near-identical rotations
187-
if (dotProduct > 0.999999f)
188-
{
189-
return FutureRotation;
190-
}
135+
OutGoingData.position = position;
136+
OutGoingData.rotation = rotation;
137+
}
191138

192-
float angleDifference = math.acos(math.clamp(dotProduct, -1f, 1f));
193-
if (angleDifference < math.EPSILON)
194-
{
195-
return FutureRotation;
196-
}
139+
public void SetOutgoingWorld(Vector3 position, Quaternion rotation)
140+
{
141+
OutgoingWorldData.position = position;
142+
OutgoingWorldData.rotation = rotation;
143+
}
197144

198-
// Blend rate between normal and fast movement using normalized angle fraction
199-
float lerpAmountNormal = QuaternionLerp;
200-
float timing = math.min(angleDifference / AngleBeforeSpeedup, 1f);
201-
float lerpAmount = lerpAmountNormal + (QuaternionLerpFastMovement - lerpAmountNormal) * timing;
145+
public void SetOutgoingWorldPosition(Vector3 position)
146+
{
147+
OutgoingWorldData.position = position;
148+
}
202149

203-
// Frame-rate-independent factor
204-
float lerpFactor = ClampInterpolationFactor(lerpAmount, DeltaTime);
150+
public void SetLastRun(Vector3 position, Quaternion rotation)
151+
{
152+
LastRunData.position = position;
153+
LastRunData.rotation = rotation;
154+
}
205155

206-
return math.slerp(CurrentRotation, FutureRotation, lerpFactor);
156+
public void SetInverseOffset(Vector3 position, Quaternion rotation)
157+
{
158+
InverseOffsetFromBone.position = position;
159+
InverseOffsetFromBone.rotation = rotation;
207160
}
208161

209-
/// <summary>
210-
/// Converts a per-second interpolation rate into a clamped [0,1] fraction.
211-
/// </summary>
212-
/// <param name="lerpAmount">Interpolation rate (per second).</param>
213-
/// <param name="DeltaTime">Frame delta time.</param>
214-
/// <returns>Clamped interpolation factor in [0,1].</returns>
215-
private float ClampInterpolationFactor(float lerpAmount, float DeltaTime)
162+
public void SetInverseOffset(in BasisCalibratedCoords value)
216163
{
217-
return math.clamp(lerpAmount * DeltaTime, 0f, 1f);
164+
InverseOffsetFromBone = value;
218165
}
219166

220-
/// <summary>
221-
/// Writes <see cref="OutGoingData"/> to <see cref="LastRunData"/>,
222-
/// computes world-space pose into <see cref="OutgoingWorldData"/> using <paramref name="parentMatrix"/>.
223-
/// </summary>
224-
/// <param name="parentMatrix">Parent transform matrix.</param>
225-
public void ApplyWorldAndLast(Matrix4x4 parentMatrix)
167+
public void SetTposeLocal(Vector3 position, Quaternion rotation)
226168
{
227-
LastRunData.position = OutGoingData.position;
228-
LastRunData.rotation = OutGoingData.rotation;
169+
TposeLocal.position = position;
170+
TposeLocal.rotation = rotation;
171+
}
229172

230-
OutgoingWorldData.position = parentMatrix.MultiplyPoint3x4(OutGoingData.position);
231-
OutgoingWorldData.rotation = parentMatrix.rotation * OutGoingData.rotation;
173+
public void SetTposeScaled(Vector3 position, Quaternion rotation)
174+
{
175+
TposeLocalScaled.position = position;
176+
TposeLocalScaled.rotation = rotation;
232177
}
233178
}
234179
}

‎Basis/Packages/com.basis.framework/Device Management/BasisDeviceManagement.cs‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -588,7 +588,7 @@ private IEnumerator RestoreInversetOffsets(BasisInput input, BasisStoredPrevious
588588
}
589589
if (prev.hasRoleAssigned)
590590
{
591-
input.Control.InverseOffsetFromBone = prev.InverseOffsetFromBone;
591+
input.Control.SetInverseOffset(prev.InverseOffsetFromBone);
592592
}
593593
if (input.HasControl)
594594
{

‎Basis/Packages/com.basis.framework/Device Management/Devices/Base/BasisInput.cs‎

Lines changed: 6 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -311,8 +311,9 @@ public void CalculateOffset()
311311
BasisInverseOffsetData.InitialControlRotation = Control.OutgoingWorldData.rotation;
312312

313313
Vector3 Offset = Control.OutgoingWorldData.position - BasisInverseOffsetData.TrackerPosition;
314-
Control.InverseOffsetFromBone.position = BasisInverseOffsetData.InitialInverseTrackRotation * (Offset);
315-
Control.InverseOffsetFromBone.rotation = BasisInverseOffsetData.InitialInverseTrackRotation * BasisInverseOffsetData.InitialControlRotation;
314+
Control.SetInverseOffset(
315+
BasisInverseOffsetData.InitialInverseTrackRotation * (Offset),
316+
BasisInverseOffsetData.InitialInverseTrackRotation * BasisInverseOffsetData.InitialControlRotation);
316317
Control.UseInverseOffset = true;
317318
}
318319

@@ -323,8 +324,7 @@ public void UnAssignRoleAndTracker()
323324
{
324325
if (Control != null)
325326
{
326-
Control.IncomingData.position = Vector3.zero;
327-
Control.IncomingData.rotation = Quaternion.identity;
327+
Control.SetIncoming(Vector3.zero, Quaternion.identity);
328328
SetRealTrackers(BasisHasTracked.HasNoTracker, BasisHasRigLayer.HasNoRigLayer, UniqueDeviceIdentifier);
329329
}
330330
if (DeviceMatchSettings == null || DeviceMatchSettings.HasTrackedRole == false)
@@ -392,8 +392,7 @@ public void UnAssignTracker()
392392
if (HasControl)
393393
{
394394
BasisDebug.Log($"UnAssigning Tracker {Control.name}", BasisDebug.LogTag.Input);
395-
Control.InverseOffsetFromBone.position = Vector3.zero;
396-
Control.InverseOffsetFromBone.rotation = Quaternion.identity;
395+
Control.SetInverseOffset(Vector3.zero, Quaternion.identity);
397396
Control.UseInverseOffset = false;
398397
}
399398
UnAssignRoleAndTracker();
@@ -736,11 +735,7 @@ public void ControlOnlyAsDevice()
736735
{
737736
if (hasRoleAssigned && Control.HasTracked != BasisHasTracked.HasNoTracker)
738737
{
739-
// Apply position offset using math.mul for quaternion-vector multiplication
740-
Control.IncomingData.position = ScaledDeviceCoord.position;
741-
742-
// Apply rotation offset using math.mul for quaternion multiplication
743-
Control.IncomingData.rotation = ScaledDeviceCoord.rotation;
738+
Control.SetIncoming(ScaledDeviceCoord.position, ScaledDeviceCoord.rotation);
744739
}
745740

746741
}

‎Basis/Packages/com.basis.framework/Device Management/Devices/Base/BasisInputController.cs‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -83,8 +83,7 @@ public void ControlOnlyAsHand(Vector3 Position,Quaternion Rotation)
8383
{
8484
if (hasRoleAssigned && Control.HasTracked != BasisHasTracked.HasNoTracker)
8585
{
86-
Control.IncomingData.position = Position;
87-
Control.IncomingData.rotation = Rotation;
86+
Control.SetIncoming(Position, Rotation);
8887
}
8988
}
9089
public Vector3 ChangeHandYHeight(Vector3 position)

‎Basis/Packages/com.basis.framework/Device Management/Devices/Simulation/BasisInputXRSimulate.cs‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -99,8 +99,7 @@ public override void LateDoPollData()
9999

100100
if (hasRoleAssigned && Control.HasTracked != BasisHasTracked.HasNoTracker)
101101
{
102-
Control.IncomingData.position = ScaledDeviceCoord.position;
103-
Control.IncomingData.rotation = ScaledDeviceCoord.rotation;
102+
Control.SetIncoming(ScaledDeviceCoord.position, ScaledDeviceCoord.rotation);
104103
this.transform.name = Control.name;
105104
this.FollowMovement.name = $"{Control.name} Moveable transform";
106105
}

‎Basis/Packages/com.basis.framework/Drivers/Common/BasisHeightDriver.cs‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -136,8 +136,7 @@ public static void ApplyAvatarScale(float ScaleFactor)
136136
{
137137
BasisLocalBoneControl c = boneDriver.Controls[Index];
138138

139-
c.TposeLocalScaled.position = c.TposeLocal.position * ScaleFactor;
140-
c.TposeLocalScaled.rotation = c.TposeLocal.rotation;
139+
c.SetTposeScaled(c.TposeLocal.position * ScaleFactor, c.TposeLocal.rotation);
141140
c.ScaledOffset = c.Offset * ScaleFactor;
142141
}
143142
}

‎Basis/Packages/com.basis.framework/Drivers/Local/BasisLocalAvatarDriver.cs‎

Lines changed: 6 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -634,19 +634,17 @@ public bool IsNull(UnityEngine.Object obj)
634634
/// <param name="WorldTpose">World-space T-pose position to convert to avatar space.</param>
635635
public void SetInitialData(Transform Transform, BasisLocalBoneControl bone, BasisBoneTrackedRole Role, Vector3 WorldTpose, Quaternion WorldTposeRotation)
636636
{
637-
bone.OutGoingData.position = BasisLocalBoneDriver.ConvertToAvatarSpaceInitial(Transform, WorldTpose);
638-
bone.OutGoingData.rotation = Quaternion.Inverse(Transform.rotation) * WorldTposeRotation;
637+
Vector3 outgoingPosition = BasisLocalBoneDriver.ConvertToAvatarSpaceInitial(Transform, WorldTpose);
638+
Quaternion outgoingRotation = Quaternion.Inverse(Transform.rotation) * WorldTposeRotation;
639639

640640
if (IsApartOfSpineVertical(Role))
641641
{
642-
bone.OutGoingData.position.x = 0;
642+
outgoingPosition.x = 0;
643643
}
644644

645-
bone.TposeLocal.rotation = bone.OutGoingData.rotation;
646-
bone.TposeLocal.position = bone.OutGoingData.position;
647-
648-
bone.TposeLocalScaled.position = bone.TposeLocal.position;
649-
bone.TposeLocalScaled.rotation = bone.TposeLocal.rotation;
645+
bone.SetOutgoing(outgoingPosition, outgoingRotation);
646+
bone.SetTposeLocal(outgoingPosition, outgoingRotation);
647+
bone.SetTposeScaled(outgoingPosition, outgoingRotation);
650648
}
651649

652650
/// <summary>

‎Basis/Packages/com.basis.framework/Drivers/Local/BasisLocalBoneDriver.cs‎

Lines changed: 88 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -125,6 +125,10 @@ public class BasisLocalBoneDriver
125125
// Cached target indices: _cachedTargetIndices[i] = array index of Controls[i].Target, or -1.
126126
private int[] _cachedTargetIndices = Array.Empty<int>();
127127

128+
// role -> Controls index, rebuilt alongside chain data. -1 = role not present.
129+
private static readonly int RoleCount = Enum.GetValues(typeof(BasisBoneTrackedRole)).Length;
130+
private int[] _roleToIndex;
131+
128132
// Chain index arrays — built once after calibration.
129133
// Spine chain runs first (serially). The four limb chains run in parallel after spine.
130134
private NativeArray<int> _spineChainIndices;
@@ -362,6 +366,24 @@ private void BuildChainData()
362366
_cachedTargetIndices[i] = (c.Target != null) ? Array.IndexOf(Controls, c.Target) : -1;
363367
}
364368

369+
// Cache role -> index for the public request API.
370+
if (_roleToIndex == null || _roleToIndex.Length < RoleCount)
371+
{
372+
_roleToIndex = new int[RoleCount];
373+
}
374+
for (int r = 0; r < _roleToIndex.Length; r++)
375+
{
376+
_roleToIndex[r] = -1;
377+
}
378+
for (int i = 0; i < ControlsLength; i++)
379+
{
380+
int role = (int)trackedRoles[i];
381+
if (role >= 0 && role < _roleToIndex.Length)
382+
{
383+
_roleToIndex[role] = i;
384+
}
385+
}
386+
365387
DisposeChainArrays();
366388

367389
HashSet<int> covered = new HashSet<int>();
@@ -485,6 +507,69 @@ public void InvalidateChainData()
485507
_chainsBuilt = false;
486508
}
487509

510+
/// <summary>
511+
/// Resolves a tracked role to its <see cref="Controls"/> index, or -1 if absent.
512+
/// Available once chain data has been built (post-calibration).
513+
/// </summary>
514+
public int GetBoneIndex(BasisBoneTrackedRole role)
515+
{
516+
int r = (int)role;
517+
if (_roleToIndex != null && r >= 0 && r < _roleToIndex.Length)
518+
{
519+
return _roleToIndex[r];
520+
}
521+
return -1;
522+
}
523+
524+
/// <summary>
525+
/// Requests the latest simulated pose for a bone by role without dereferencing the
526+
/// managed <see cref="BasisLocalBoneControl"/>. Reads the native sim store once it has
527+
/// been populated; before the first simulate it falls back to the managed control.
528+
/// </summary>
529+
public bool TryGetSimState(BasisBoneTrackedRole role, out BasisBoneSimState state)
530+
{
531+
int index = GetBoneIndex(role);
532+
if (index < 0 || index >= ControlsLength)
533+
{
534+
state = default;
535+
return false;
536+
}
537+
538+
if (_nativeAllocated && index < _simStates.Length)
539+
{
540+
state = _simStates[index];
541+
return true;
542+
}
543+
544+
BasisLocalBoneControl c = Controls[index];
545+
state = new BasisBoneSimState
546+
{
547+
OutgoingPosition = c.OutGoingData.position,
548+
OutgoingRotation = c.OutGoingData.rotation,
549+
LastRunPosition = c.LastRunData.position,
550+
LastRunRotation = c.LastRunData.rotation,
551+
OutgoingWorldPosition = c.OutgoingWorldData.position,
552+
OutgoingWorldRotation = c.OutgoingWorldData.rotation,
553+
};
554+
return true;
555+
}
556+
557+
/// <summary>
558+
/// Exposes the native per-bone state buffer for systems that schedule their own Burst
559+
/// job against bone poses. Returns false until the native store is allocated. The buffer
560+
/// is refreshed each <see cref="Simulate"/>; index entries via <see cref="GetBoneIndex"/>.
561+
/// </summary>
562+
public bool TryGetSimStates(out NativeArray<BasisBoneSimState> states)
563+
{
564+
if (_nativeAllocated)
565+
{
566+
states = _simStates;
567+
return true;
568+
}
569+
states = default;
570+
return false;
571+
}
572+
488573
/// <summary>
489574
/// Releases all native containers. Call when the driver is being torn down.
490575
/// </summary>
@@ -601,7 +686,7 @@ public void SimulateWorldDestinations(Matrix4x4 localToWorldMatrix)
601686
for (int Index = 0; Index < ControlsLength; Index++)
602687
{
603688
// Apply local transform to parent's world transform
604-
Controls[Index].OutgoingWorldData.position = localToWorldMatrix.MultiplyPoint3x4(Controls[Index].OutGoingData.position);
689+
Controls[Index].SetOutgoingWorldPosition(localToWorldMatrix.MultiplyPoint3x4(Controls[Index].OutGoingData.position));
605690
}
606691
}
607692

@@ -720,10 +805,8 @@ public void SetupRole(int Index, Color Color, out BasisLocalBoneControl BasisBon
720805
{
721806
role = (BasisBoneTrackedRole)Index;
722807
BasisBoneControl = new BasisLocalBoneControl();
723-
BasisBoneControl.OutgoingWorldData.position = Vector3.zero;
724-
BasisBoneControl.OutgoingWorldData.rotation = Quaternion.identity;
725-
BasisBoneControl.LastRunData.position = BasisBoneControl.OutGoingData.position;
726-
BasisBoneControl.LastRunData.rotation = BasisBoneControl.OutGoingData.rotation;
808+
BasisBoneControl.SetOutgoingWorld(Vector3.zero, Quaternion.identity);
809+
BasisBoneControl.SetLastRun(BasisBoneControl.OutGoingData.position, BasisBoneControl.OutGoingData.rotation);
727810
FillOutBasicInformation(BasisBoneControl, role.ToString(), Color);
728811
}
729812

0 commit comments

Comments
 (0)