private IEnumerator StopSolver(FABRIK solver)
{
yield return(new WaitForSeconds(FABRIKSolveTimeSeconds));
solver.enabled = false;
SetEnd(fromCap, rootLink);
SetEnd(toCap, lastLink);
Rigidbody parentRigid = rootLink.GetComponent <Rigidbody>();
ConfigurableJoint parentJoint = parentRigid.GetComponent <ConfigurableJoint>();
for (int i = 0; i < numLinks; i++)
{
parentRigid.isKinematic = false;
parentRigid.transform.SetParent(rootLink.parent);
Transform childT = parentRigid.transform.GetChild(0);
if (childT.CompareTag("joint"))
{
Rigidbody child = childT.GetComponent <Rigidbody>();
parentJoint.connectedBody = child;
parentJoint = child.GetComponent <ConfigurableJoint>();
parentRigid = child;
}
}
//set the FABRIK root to under this for easier cleanup
rootLink.parent.SetParent(this.transform);
}
public IkModifier(ModifierInfo mi, Entity casterEntity, Entity targetEntity,
Environment environment,
CollectionOfInteractions modifierInteractionCollection) : base(mi, casterEntity, targetEntity, environment, modifierInteractionCollection)
{
this.environment = environment;
this.info = (IkInfo)mi;
mapCollider = environment.MapColliders();
caster = casterEntity.GetComponent <SkillComponent>().Character;
FrameAndSecondsConverter fasc = FrameAndSecondsConverter._30Fps;
aimAt = 0;
aimDuration = fasc.FramesToSeconds(info.Config.aimDuration);
aimLogicDuration = fasc.FramesToSeconds(info.Config.aimLogicDuration);
aimInterpolationDuration = fasc.FramesToSeconds(info.Config.aimInterpolationDuration);
startupAt = aimAt + aimDuration;
startupDuration = fasc.FramesToSeconds(info.Config.startupDuration);
activeAt = startupAt + startupDuration;
activeDuration = fasc.FramesToSeconds(info.Config.activeDuration);
recoveryAt = activeAt + activeDuration;
recoveryDuration = fasc.FramesToSeconds(info.Config.recoveryDuration);
totalDuration = recoveryAt + recoveryDuration;
target = environment.FindNearbyCharacters(
caster, Vector3.zero, 999,
new[] { FindingFilter.ExcludeMe, FindingFilter.ExcludeDead, FindingFilter.ExcludeAllies }
)[0];
fabrik = caster.GameObject().GetComponent <FABRIK>();
IKSolverFABRIK solverFabrik = (IKSolverFABRIK)fabrik.GetIKSolver();
ikJoint = solverFabrik.bones[solverFabrik.bones.Length - 1].transform;
ikJointParent = solverFabrik.bones[solverFabrik.bones.Length - 2].transform;
}
public override void OnInspectorGUI()
{
DrawDefaultInspector();
FABRIK fabrik = target as FABRIK;
if (GUILayout.Button("Generate FABRIK Effector System"))
{
fabrik.CreateSystem();
}
}
public static void Solve(HandState[] states, IKPointTarget[] pointTargets, IKGapTarget[] gapTargets)
{
if (!states[0].active && !states[1].active)
{
return;
}
FABRIK data = new FABRIK(states, pointTargets, gapTargets);
for (int i = 0; i < 5; i++)
{
data.SolveStep();
}
data.Apply(states);
}
protected override void ShowVisuals(IPowerable g1, IPowerable g2)
{
if (g1 is RoverStation && g2 is RoverInput)
{
station = g1 as RoverStation;
station.OnRoverAttachedChange(g2 as RoverInput);
}
else if (g2 is RoverStation && g1 is RoverInput)
{
station = g2 as RoverStation;
station.OnRoverAttachedChange(g1 as RoverInput);
}
fromCap = CreateCap(Data.fromPos, Data.fromRot, Data.fromScale);
toCap = CreateCap(Data.toPos, Data.toRot, Data.toScale);
Vector3 fromPos = fromCap.position,
toPos = toCap.position;
float distanceBetween = Vector3.Distance(fromPos, toPos);
numLinks = Mathf.RoundToInt(distanceBetween / linkSeparation) + LinkNumberAdjustment;
rootLink = GameObject.Instantiate <Transform>(UmbilicalLinkPrefab);
rootLink.SetParent(this.transform);
rootLink.position = fromPos;
rootLink.rotation = Data.fromRot;
Transform parent = rootLink;
for (int i = 0; i < numLinks - 1; i++)
{
Transform link = GameObject.Instantiate <Transform>(UmbilicalLinkPrefab);
link.position = parent.position + Vector3.forward * linkSeparation;
link.SetParent(parent);
link.SetAsFirstSibling();
parent = link;
}
lastLink = parent;
solver = rootLink.gameObject.AddComponent <FABRIK>();
solver.Constrain = true;
solver.ConstrainAngle = FABRIKAngleConstraint;
solver.initialTarget = toCap;
StartCoroutine(StopSolver(solver));
}
public void Execute(Entity entity, int index, ref ArmComponent armComponent,
[ReadOnly] ref Translation translation, [ReadOnly] ref Rotation rotation)
{
float armBoneLength = armComponent.armBoneLength;
float3 handUp = armComponent.handUp;
float armBendStrength = armComponent.armBendStrength;
Matrix4x4[] matrices = new Matrix4x4[17];
DynamicBuffer <ArmChainsBuffer> armChainsBuffer = armChainsBufferLookup[entity];
DynamicBuffer <FingerChainsBuffer> fingerChainsBuffer = fingerChainsBufferLookup[entity];
DynamicBuffer <ThumbChainsBuffer> thumbChainsBuffer = thumbChainsBufferLookup[entity];
///////////////////////////
// Resting position for hand
float time = worldTime + armComponent.timeOffset;
// solve the arm IK chain first
float3 anchor = translation.Value;
//float3 anchor = new float3();
FABRIK.SolveViaBuffer(armChainsBuffer.Reinterpret <float3>(), armBoneLength, anchor, armComponent.handTarget, handUp * armBendStrength);
Quaternion q = rotation.Value;
float3 transformRight = math.normalize(math.mul(q, directions.right));
// figure out our current "hand vectors" from our arm orientation
float3 handForward = math.normalize(util.Last(armChainsBuffer.Reinterpret <float3>(), 0) - util.Last(armChainsBuffer.Reinterpret <float3>(), 1));
handUp = math.normalize(math.cross(handForward, transformRight));
float3 handRight = math.cross(handUp, handForward);
// create handspace-to-worldspace matrix
armComponent.handMatrix = Matrix4x4.TRS(util.Last(armChainsBuffer.Reinterpret <float3>(), 0), Quaternion.LookRotation(handForward, handUp), directions.one);
// how much are our fingers gripping?
// (during a reach, this is based on the reach timer)
float fingerGrabT = armComponent.savedGrabT;
if (armComponent.heldRock != Entity.Null)
{
//Translation heldRockTrans = translationsFromEntity[armComponent.heldRock];
fingerGrabT = 1.0f; //When holding the rock, we're fully gripped.
var held = new RockHeldComponent();
held.rockInHandPosition = armComponent.handMatrix.MultiplyPoint3x4(armComponent.heldRockOffset);
ecb.AddComponent(index, armComponent.heldRock, held);
// armComponent.lastIntendedRockPos =
}
// create rendering matrices for arm bones
util.UpdateMatrices(matrices, armChainsBuffer.Reinterpret <float3>(), 0, armComponent.armBoneThickness, handUp);
int matrixIndex = armChainsBuffer.Reinterpret <float3>().Length - 1;
// next: fingers
float3 handPos = util.Last(armChainsBuffer.Reinterpret <float3>(), 0);
// fingers spread out during a throw
float openPalm = throwCurve.Evaluate(armComponent.throwTimer);
//TODO add these to arm component?
float fingerXOffset = -0.12f;
float fingerSpacing = 0.08f;
float[] fingerBoneLengths = { 0.2f, 0.22f, 0.2f, 0.16f };
float[] fingerThicknesses = { 0.05f, 0.05f, 0.05f, 0.05f };
float fingerBendStrength = 0.2f;
for (int i = 0; i < 4; i++)
{
float3 handRightTemp = handRight * (fingerXOffset + i * fingerSpacing);
// find knuckle position for this finger
float3 fingerPos = handPos + handRightTemp;
// find resting position for this fingertip
float3 fingerTarget = fingerPos + handForward * (.5f - .1f * fingerGrabT);
// spooky finger wiggling while we're idle
fingerTarget += handUp * Mathf.Sin((time + i * .2f) * 3f) * .2f * (1f - fingerGrabT);
// if we're gripping, move this fingertip onto the surface of our rock
float3 rockFingerDelta = fingerTarget - armComponent.lastIntendedRockPos;
float3 rockFingerPos = armComponent.lastIntendedRockPos + math.normalize(rockFingerDelta) * (armComponent.lastIntendedRockSize * .5f + fingerThicknesses[i]);
fingerTarget = math.lerp(fingerTarget, rockFingerPos, fingerGrabT);
// apply finger-spreading during throw animation
fingerTarget += (handUp * .3f + handForward * .1f + handRight * (i - 1.5f) * .1f) * openPalm;
// solve this finger's IK chain
// FABRIK.Solve(fingerChains[i],fingerBoneLengths[i],fingerPos,fingerTarget,handUp*fingerBendStrength);
int startIdx = i * 4;
int stopIdx = startIdx + 4;
FABRIK.SolveViaBufferSliced(fingerChainsBuffer.Reinterpret <float3>(), fingerBoneLengths[i], fingerPos, fingerTarget, handUp * fingerBendStrength, startIdx, stopIdx);
// update this finger's rendering matrices
util.UpdateMatricesSliced(matrices, fingerChainsBuffer.Reinterpret <float3>(), matrixIndex, fingerThicknesses[i], handUp, startIdx, stopIdx);
matrixIndex += 4 - 1;
}
// the thumb is pretty much the same as the fingers
// (but pointing in a strange direction)
float thumbXOffset = -0.05f;
float thumbThickness = 0.06f;
float thumbBendStrength = 0.1f;
float thumbBoneLength = 0.13f;
float3 thumbPos = handPos + handRight * thumbXOffset;
float3 thumbTarget = thumbPos - handRight * .15f + handForward * (.2f + .1f * fingerGrabT) - handUp * .1f;
thumbTarget += handRight * Mathf.Sin(time * 3f + .5f) * .1f * (1f - fingerGrabT);
// thumb bends away from the palm, instead of "upward" like the fingers
float3 thumbBendHint = (-handRight - handForward * .5f);
float3 rockThumbDelta = thumbTarget - armComponent.lastIntendedRockPos;
float3 rockThumbPos = armComponent.lastIntendedRockPos + math.normalize(rockThumbDelta) * (armComponent.lastIntendedRockSize * .5f);
thumbTarget = math.lerp(thumbTarget, rockThumbPos, fingerGrabT);
FABRIK.SolveViaBuffer(thumbChainsBuffer.Reinterpret <float3>(), thumbBoneLength, thumbPos, thumbTarget, thumbBendHint * thumbBendStrength);
util.UpdateMatrices(matrices, thumbChainsBuffer.Reinterpret <float3>(), matrixIndex, thumbThickness, thumbBendHint);
DynamicBuffer <ArmMatrixBuffer> buffer = matrixBufferLookup[entity];
buffer.Clear();
for (int i = 0; i < matrices.Length; i++)
{
buffer.Add(new ArmMatrixBuffer()
{
Value = matrices[i]
});
}
}
// Start is called before the first frame update
void Start()
{
fabrik = new FABRIK(this.transform, iterationLimit, boneChainLength);
}
void Start()
{
fabRik = GetComponent <FABRIK>();
}
void Update()
{
float time = Time.time + timeOffset;
// resting position
Vector3 idleHandTarget = transform.position + new Vector3(Mathf.Sin(time) * .35f, 1f + Mathf.Cos(time * 1.618f) * .5f, 1.5f);
if (heldRock == null && windupTimer <= 0f)
{
if (intendedRock == null && reachTimer == 0f)
{
// we're idle - see if we can grab a rock
Rock nearestRock = RockManager.NearestConveyorRock(transform.position - Vector3.right * .5f);
if (nearestRock != null)
{
if ((nearestRock.position - transform.position).sqrMagnitude < maxReachLength * maxReachLength)
{
// found a rock to grab!
// mark it as reserved so other hands don't reach for it
intendedRock = nearestRock;
intendedRock.reserved = true;
lastIntendedRockSize = intendedRock.size;
}
}
}
else if (intendedRock == null)
{
// stop reaching if we've lost our target
reachTimer -= Time.deltaTime / reachDuration;
}
if (intendedRock != null)
{
// we're reaching for a rock (but we haven't grabbed it yet)
Vector3 delta = intendedRock.position - transform.position;
if (delta.sqrMagnitude < maxReachLength * maxReachLength)
{
// figure out where we want to put our wrist
// in order to grab the rock
Vector3 flatDelta = delta;
flatDelta.y = 0f;
flatDelta.Normalize();
grabHandTarget = intendedRock.position + Vector3.up * intendedRock.size * .5f - flatDelta * intendedRock.size * .5f;
lastIntendedRockPos = intendedRock.position;
reachTimer += Time.deltaTime / reachDuration;
if (reachTimer >= 1f)
{
// we've arrived at the rock - pick it up
heldRock = intendedRock;
RockManager.RemoveFromConveyor(heldRock);
heldRock.state = Rock.State.Held;
// remember the rock's position in "hand space"
// (so we can position the rock while holding it)
heldRockOffset = handMatrix.inverse.MultiplyPoint3x4(heldRock.position);
intendedRock = null;
// random minimum delay before starting the windup
windupTimer = Random.Range(-1f, 0f);
throwTimer = 0f;
}
}
else
{
// we didn't grab the rock in time - forget it
intendedRock.reserved = false;
intendedRock = null;
}
}
}
if (heldRock != null)
{
// stop reaching after we've successfully grabbed a rock
reachTimer -= Time.deltaTime / reachDuration;
if (targetCan == null)
{
// find a target
targetCan = TinCanManager.GetNearestCan(transform.position, true, targetXRange);
}
if (targetCan != null)
{
// found a target - prepare to throw
targetCan.reserved = true;
windupTimer += Time.deltaTime / windupDuration;
}
}
reachTimer = Mathf.Clamp01(reachTimer);
// smoothed reach timer
float grabT = reachTimer;
grabT = 3f * grabT * grabT - 2f * grabT * grabT * grabT;
// reaching overrides our idle hand position
handTarget = Vector3.Lerp(idleHandTarget, grabHandTarget, grabT);
if (targetCan != null)
{
// we've got a target, which means we're currently throwing
if (windupTimer < 1f)
{
// still winding up...
float windupT = Mathf.Clamp01(windupTimer) - Mathf.Clamp01(throwTimer * 2f);
windupT = 3f * windupT * windupT - 2f * windupT * windupT * windupT;
handTarget = Vector3.Lerp(handTarget, windupHandTarget, windupT);
Vector3 flatTargetDelta = targetCan.position - transform.position;
flatTargetDelta.y = 0f;
flatTargetDelta.Normalize();
// windup position is "behind us," relative to the target position
windupHandTarget = transform.position - flatTargetDelta * 2f + Vector3.up * (3f - windupT * 2.5f);
}
else
{
// done winding up - actual throw, plus resetting to idle
throwTimer += Time.deltaTime / throwDuration;
// update our aim until we release the rock
if (heldRock != null)
{
aimVector = AimAtCan(targetCan, lastIntendedRockPos);
}
// we start this animation in our windup position,
// and end it by returning to our default idle pose
Vector3 restingPos = Vector3.Lerp(windupHandTarget, handTarget, throwTimer);
// find the hand's target position to perform the throw
// (somewhere forward and upward from the windup position)
Vector3 throwHandTarget = windupHandTarget + aimVector.normalized * 2.5f;
handTarget = Vector3.LerpUnclamped(restingPos, throwHandTarget, throwCurve.Evaluate(throwTimer));
if (throwTimer > .15f && heldRock != null)
{
// release the rock
heldRock.reserved = false;
heldRock.state = Rock.State.Thrown;
heldRock.velocity = aimVector;
heldRock = null;
}
if (throwTimer >= 1f)
{
// we've completed the animation - return to idle
windupTimer = 0f;
throwTimer = 0f;
TinCanManager.UnreserveCanAfterDelay(targetCan, 3f);
targetCan = null;
}
}
}
// solve the arm IK chain first
FABRIK.Solve(armChain, armBoneLength, transform.position, handTarget, handUp * armBendStrength);
// figure out our current "hand vectors" from our arm orientation
handForward = (armChain.Last(0) - armChain.Last(1)).normalized;
handUp = Vector3.Cross(handForward, transform.right).normalized;
handRight = Vector3.Cross(handUp, handForward);
// create handspace-to-worldspace matrix
handMatrix = Matrix4x4.TRS(armChain.Last(), Quaternion.LookRotation(handForward, handUp), Vector3.one);
// how much are our fingers gripping?
// (during a reach, this is based on the reach timer)
float fingerGrabT = grabT;
if (heldRock != null)
{
// move our held rock to match our new hand position
heldRock.position = handMatrix.MultiplyPoint3x4(heldRockOffset);
lastIntendedRockPos = heldRock.position;
// if we're holding a rock, we're always gripping
fingerGrabT = 1f;
}
// create rendering matrices for arm bones
UpdateMatrices(armChain, 0, armBoneThickness, handUp);
int matrixIndex = armChain.Length - 1;
// next: fingers
Vector3 handPos = armChain.Last();
// fingers spread out during a throw
float openPalm = throwCurve.Evaluate(throwTimer);
for (int i = 0; i < fingerChains.Length; i++)
{
// find knuckle position for this finger
Vector3 fingerPos = handPos + handRight * (fingerXOffset + i * fingerSpacing);
// find resting position for this fingertip
Vector3 fingerTarget = fingerPos + handForward * (.5f - .1f * fingerGrabT);
// spooky finger wiggling while we're idle
fingerTarget += handUp * Mathf.Sin((time + i * .2f) * 3f) * .2f * (1f - fingerGrabT);
// if we're gripping, move this fingertip onto the surface of our rock
Vector3 rockFingerDelta = fingerTarget - lastIntendedRockPos;
Vector3 rockFingerPos = lastIntendedRockPos + rockFingerDelta.normalized * (lastIntendedRockSize * .5f + fingerThicknesses[i]);
fingerTarget = Vector3.Lerp(fingerTarget, rockFingerPos, fingerGrabT);
// apply finger-spreading during throw animation
fingerTarget += (handUp * .3f + handForward * .1f + handRight * (i - 1.5f) * .1f) * openPalm;
// solve this finger's IK chain
FABRIK.Solve(fingerChains[i], fingerBoneLengths[i], fingerPos, fingerTarget, handUp * fingerBendStrength);
// update this finger's rendering matrices
UpdateMatrices(fingerChains[i], matrixIndex, fingerThicknesses[i], handUp);
matrixIndex += fingerChains[i].Length - 1;
}
// the thumb is pretty much the same as the fingers
// (but pointing in a strange direction)
Vector3 thumbPos = handPos + handRight * thumbXOffset;
Vector3 thumbTarget = thumbPos - handRight * .15f + handForward * (.2f + .1f * fingerGrabT) - handUp * .1f;
thumbTarget += handRight * Mathf.Sin(time * 3f + .5f) * .1f * (1f - fingerGrabT);
// thumb bends away from the palm, instead of "upward" like the fingers
Vector3 thumbBendHint = (-handRight - handForward * .5f);
Vector3 rockThumbDelta = thumbTarget - lastIntendedRockPos;
Vector3 rockThumbPos = lastIntendedRockPos + rockThumbDelta.normalized * (lastIntendedRockSize * .5f);
thumbTarget = Vector3.Lerp(thumbTarget, rockThumbPos, fingerGrabT);
FABRIK.Solve(thumbChain, thumbBoneLength, thumbPos, thumbTarget, thumbBendHint * thumbBendStrength);
UpdateMatrices(thumbChain, matrixIndex, thumbThickness, thumbBendHint);
// draw all of our bones
Graphics.DrawMeshInstanced(boneMesh, 0, material, matrices);
}
