private IEnumerator Climb(ClimbingNode nextNode)
{
UpdateAnimator();
elapsedTime = 0;
while (elapsedTime <= 1)
{
if (Input.GetButtonDown("Jump"))
{
Jump();
yield break;
}
//Move Right Side
if (movePolarity == RIGHT)
{
IKController.LerpIKTarget(IK.RightHand, IKTarget.FromTransform(currentNodes[RIGHT].rightHand.transform), IKTarget.FromTransform(nextNode.rightHand.transform), elapsedTime);
IKController.LerpIKTarget(IK.RightFoot, IKTarget.FromTransform(currentNodes[RIGHT].rightFoot.transform), IKTarget.FromTransform(nextNode.rightFoot.transform), elapsedTime);
}
else
{
IK.RightHand.Set(currentNodes[RIGHT].rightHand);
IK.RightFoot.Set(currentNodes[RIGHT].rightFoot);
}
//Move Left Side
if (movePolarity == LEFT)
{
IKController.LerpIKTarget(IK.LeftHand, IKTarget.FromTransform(currentNodes[LEFT].leftHand.transform), IKTarget.FromTransform(nextNode.leftHand.transform), elapsedTime);
IKController.LerpIKTarget(IK.LeftFoot, IKTarget.FromTransform(currentNodes[LEFT].leftFoot.transform), IKTarget.FromTransform(nextNode.leftFoot.transform), elapsedTime);
}
else
{
IK.LeftHand.Set(currentNodes[LEFT].leftHand);
IK.LeftFoot.Set(currentNodes[LEFT].leftFoot);
}
//Root Rotaion - While moving
Quaternion desiredRotation = Quaternion.Lerp(
Quaternion.Lerp(currentNodes[LEFT].transform.rotation, currentNodes[RIGHT].transform.rotation, 0.5f),
Quaternion.Lerp(currentNodes[(movePolarity + 1) % 2].transform.rotation, nextNode.transform.rotation, 0.5f),
elapsedTime
);
if (freehang)
{
desiredRotation = Quaternion.Euler(0, desiredRotation.eulerAngles.y, desiredRotation.eulerAngles.z);
}
rb.transform.rotation = desiredRotation;
//Root Position - While moving
rb.transform.position = Vector3.Lerp(
(currentNodes[LEFT].PlayerPosition + currentNodes[RIGHT].PlayerPosition) / 2,
(currentNodes[(movePolarity + 1) % 2].PlayerPosition + nextNode.PlayerPosition) / 2,
elapsedTime);
elapsedTime += Time.deltaTime * movementSpeed;
yield return(null);
}
currentNodes[movePolarity] = nextNode;
moving = false;
}
void Parse(Stream s)
{
var br = new BinaryReader(s);
this.version = br.ReadUInt32();
this.unknown1 = br.ReadUInt32();
this.duration = br.ReadSingle();
this.unknown_floats = new float[8];
for (int i = 0; i < this.unknown_floats.Length; i++)
{
this.unknown_floats[i] = br.ReadSingle();
}
this.unknown_hashes = new uint[3];
for (int i = 0; i < this.unknown_hashes.Length; i++)
{
this.unknown_hashes[i] = br.ReadUInt32();
}
this.clip_name = br.ReadString32();
this.actor_name = br.ReadString32();
var extra_actor_count = br.ReadInt32();
var extra_actors = new string[extra_actor_count];
for (int i = 0; i < extra_actor_count; i++)
{
extra_actors[i] = br.ReadString32();
}
this.actor_list = string.Join(",", extra_actors);
var ik_target_count = br.ReadInt32();
this.IKChainCount = br.ReadInt32();
var ik_targets = new IKTarget[ik_target_count];
for (int i = 0; i < ik_target_count; i++)
{
ik_targets[i] = new IKTarget(RecommendedApiVersion, this.OnResourceChanged, s);
}
this.ik_targets = new IKTargetList(this.OnResourceChanged, ik_targets);
UInt32 next = br.ReadUInt32();
bool end = false;
var events = new List <ClipEvent>();
while (stream.Position + next != stream.Length)
{
var evt = ClipEvent.Create(next, this.OnResourceChanged, next);
var evt_end = br.ReadInt32() + br.BaseStream.Position;
evt.Parse(s);
events.Add(evt);
if (Settings.Settings.Checking && br.BaseStream.Position != evt_end)
{
throw new InvalidDataException();
}
next = br.ReadUInt32();
}
this.clip_events = new ClipEventList(this.OnResourceChanged, events);
this.s3clip = new byte[next];
s.Read(this.s3clip, 0, this.s3clip.Length);
}
/// <summary>
/// 测试和代码的位置
/// </summary>
///
public void Test()
{
skillController.equipSkill(actorParts.GetLearnableSkill()[0]);
skillController.equipSkill(actorParts.GetLearnableSkill()[1]);
skillController.equipSkill(actorParts.GetLearnableSkill()[2]);
targetTest = GetComponentInChildren <IKTarget>();
}
public static IKTarget createTarget(string name, IKBone targetRoot)
{
GameObject go = new GameObject(name);
IKTarget newTarget = go.AddComponent <IKTarget>();
newTarget.transform.parent = targetRoot.transform;
newTarget.targetRoot = targetRoot;
return(newTarget);
}
public IKSolverTwoBone(string name, IKBone chainRoot, IKBone targetRoot, float length, float width, float thickness, float proportion) : base(name, chainRoot, targetRoot)
{
neutralDir = Vector3.right;
hingeAxis = Vector3.up;
upper = chainRoot.addBone(name + "_upper", length * proportion, width * proportion, thickness * proportion);
float proportionInv = 1.0f - proportion;
lower = upper.addBone(name + "_lower", length * proportionInv, width * proportionInv, thickness * proportionInv);
float maxWT = Mathf.Max(width, thickness);
end = lower.addBone(name + "_end", maxWT * proportionInv, maxWT * proportionInv, maxWT * proportionInv);
target = IKTarget.createTarget(name + "_target", targetRoot);
target.transform.position = upper.getStartPointWorld() + neutralDir * (upper.length + lower.length);
}
// Use this for initialization
void Start()
{
base.Start();
Wilson = GameObject.Find("Wilson");
Diana = GameObject.Find("Diana");
animator = Wilson.GetComponent <Animator>();
relationTracker = GameObject.Find("BehaviorController").GetComponent <RelationTracker>();
eventManager = GameObject.Find("BehaviorController").GetComponent <EventManager>();
inputController = GameObject.Find("IOController").GetComponent <InputController>();
leftGrasper = animator.GetBoneTransform(HumanBodyBones.LeftHand).transform.gameObject;
rightGrasper = animator.GetBoneTransform(HumanBodyBones.RightHand).transform.gameObject;
graspController = Wilson.GetComponent <GraspScript>();
ikControl = Wilson.GetComponent <IKControl>();
leftTarget = ikControl.leftHandObj.GetComponent <IKTarget>();
rightTarget = ikControl.rightHandObj.GetComponent <IKTarget>();
headTarget = ikControl.lookObj.GetComponent <IKTarget>();
outputModality = GameObject.Find("OutputModality").GetComponent <OutputModality>();
goBack = false;
currentStep = ScriptStep.Step0;
waitTimer = new Timer(WAIT_TIME);
waitTimer.Enabled = false;
waitTimer.Elapsed += Proceed;
humanMoveComplete = false;
leftAtTarget = false;
rightAtTarget = false;
inputController.InputReceived += HumanInputReceived;
eventManager.QueueEmpty += HumanMoveComplete;
eventManager.ForceClear += EventsForceCleared;
leftTarget.AtTarget += LeftAtTarget;
rightTarget.AtTarget += RightAtTarget;
OpenLog(demoName, outputModality.modality);
}
void followObjectIK(IKTarget target)
{
var cmd = NativeMethods.b3CalculateInverseKinematicsCommandInit(pybullet, b3RobotId);
var targetPosRos = target.targetTF.position.Unity2Ros();
var targetOrnRos = target.targetTF.rotation;
targetOrnRos *= Quaternion.AngleAxis(90, new Vector3(0, 1, 0));
targetOrnRos *= Quaternion.AngleAxis(180, new Vector3(1, 0, 0));
targetOrnRos = targetOrnRos.Unity2Ros();
double[] targetPos = { targetPosRos.x, targetPosRos.y, targetPosRos.z };
double[] targetOrn = { targetOrnRos.x, targetOrnRos.y, targetOrnRos.z, targetOrnRos.w };
NativeMethods.b3CalculateInverseKinematicsAddTargetPositionWithOrientation(cmd, target.endeffectorLinkId, ref targetPos[0], ref targetOrn[0]);
//NativeMethods.b3CalculateInverseKinematicsAddTargetPurePosition(cmd, target.endeffectorLinkId, ref targetPos[0]);
var statusHandle = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
int dofCount = 0;
double[] jointTargets = new double[freeJoints.Count];
int bodyId = -1;
var status = NativeMethods.b3GetStatusInverseKinematicsJointPositions(statusHandle, ref bodyId, ref dofCount, ref jointTargets[0]);
status = NativeMethods.b3GetStatusInverseKinematicsJointPositions(statusHandle, ref bodyId, ref dofCount, ref jointTargets[0]);
cmd = NativeMethods.b3JointControlCommandInit2(pybullet, b3RobotId, (int)EnumControlMode.CONTROL_MODE_POSITION_VELOCITY_PD);
for (int i = 0; i < jointTargets.Length; i++)
{
if (!target.kinematicChain.Contains(freeJoints[i]))
{
continue;
}
bb.SetJointPosition(ref cmd, b3RobotId, freeJoints[i], jointTargets[i]);
}
var st = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
}
public void addTarget(IKTarget target) //, Quaternion rotation
{
sumTargets += target.transform.position;
numTargets++;
}
// Use this for initialization
void Start()
{
Wilson = GameObject.Find ("Wilson");
Diana = GameObject.Find ("Diana");
animator = Wilson.GetComponent<Animator> ();
relationTracker = GameObject.Find ("BehaviorController").GetComponent<RelationTracker> ();
eventManager = GameObject.Find ("BehaviorController").GetComponent<EventManager> ();
leftGrasper = animator.GetBoneTransform (HumanBodyBones.LeftHand).transform.gameObject;
rightGrasper = animator.GetBoneTransform (HumanBodyBones.RightHand).transform.gameObject;
graspController = Wilson.GetComponent<GraspScript> ();
ikControl = Wilson.GetComponent<IKControl> ();
leftTarget = ikControl.leftHandObj.GetComponent<IKTarget> ();
rightTarget = ikControl.rightHandObj.GetComponent<IKTarget> ();
headTarget = ikControl.lookObj.GetComponent<IKTarget> ();
outputModality = GameObject.Find ("OutputModality").GetComponent<OutputModality>();
goBack = false;
currentStep = DemoStep.Step0;
waitTimer = new Timer (WAIT_TIME);
waitTimer.Enabled = false;
waitTimer.Elapsed += Proceed;
humanMoveComplete = false;
leftAtTarget = false;
rightAtTarget = false;
eventManager.EventComplete += HumanMoveComplete;
leftTarget.AtTarget += LeftAtTarget;
rightTarget.AtTarget += RightAtTarget;
}
public static void LerpIKTarget(IKTarget Target, IKTarget fromTarget, IKTarget toTarget, float delta)
{
Target.position = Vector3.Lerp(fromTarget.position, toTarget.position, delta);
Target.rotation = Quaternion.Lerp(fromTarget.rotation, toTarget.rotation, delta);
}
//LerpIK
public static void LerpIKTarget(IKTarget Target, IKTarget fromTarget, IKTarget toTarget, float delta, float fromWeight, float toWeight)
{
Target.position = Vector3.Lerp(fromTarget.position, toTarget.position, delta);
Target.rotation = Quaternion.Lerp(fromTarget.rotation, toTarget.rotation, delta);
Target.weight = Mathf.Lerp(fromWeight, toWeight, delta);
}
public static void MoveIKTarget(IKTarget Target, Transform NewTarget, float delta)
{
Target.position = Vector3.MoveTowards(Target.position, NewTarget.position, delta);
Target.rotation = Quaternion.Lerp(Target.rotation, NewTarget.rotation, delta * 4);
}
//MoveIK
public static void MoveIKTarget(IKTarget Target, Transform NewTarget, float delta, float EndWeight, float weightDelta)
{
Target.position = Vector3.MoveTowards(Target.position, NewTarget.position, delta);
Target.rotation = Quaternion.Lerp(Target.rotation, NewTarget.rotation, delta * 4);
Target.weight = Mathf.MoveTowards(Target.weight, EndWeight, weightDelta);
}
