//Apply the hinge rotation limit
private Quat LimitHinge(Quat rotation)
{
// If limit is zero return rotation fixed to axis
if (min == 0 && max == 0 && useLimits)
{
return(Quat.AngleAxis(0, axis));
}
// Get 1 degree of freedom rotation along axis
Quat free1DOF = Limit1DOF(rotation, axis);
if (!useLimits)
{
return(free1DOF);
}
// Get offset from last rotation in angle-axis representation
Quat addR = free1DOF * Quat.Inverse(lastRotation);
float addA = Quat.Angle(Quat.identity, addR);
Vec3 secondaryAxis = new Vec3(axis.z, axis.x, axis.y);
Vec3 cross = secondaryAxis.CrossProduct(axis);
if (cross.DotProduct(addR * secondaryAxis) > 0f)
{
addA = -addA;
}
// Clamp to limits
lastAngle = Mathf.Clamp(lastAngle + addA, min, max);
return(Quat.AngleAxis(lastAngle, axis));
}
public static Vec3 SlerpUnclamped(Vec3 from, Vec3 to, float t)
{
float scale0, scale1;
float len2 = to.Magnitude();
float len1 = from.Magnitude();
Vec3 v2 = to / len2;
Vec3 v1 = from / len1;
float len = (len2 - len1) * t + len1;
float cosom = Dot(v1, v2);
if (cosom > 1 - 1e-6)
{
scale0 = 1 - t;
scale1 = t;
}
else if (cosom < -1 + 1e-6)
{
Vec3 axis = OrthoNormalVector(from);
Quat q = Quat.AngleAxis(180.0f * t, axis);
Vec3 v = q * from * len;
return(v);
}
else
{
float omega = MathUtils.Acos(cosom);
float sinom = MathUtils.Sin(omega);
scale0 = MathUtils.Sin((1 - t) * omega) / sinom;
scale1 = MathUtils.Sin(t * omega) / sinom;
}
v2 = (v2 * scale1 + v1 * scale0) * len;
return(v2);
}
public static Vec3 RotateTowards(Vec3 current, Vec3 target, float maxRadiansDelta, float maxMagnitudeDelta)
{
float len1 = current.Magnitude();
float len2 = target.Magnitude();
if (len1 > 1e-6 && len2 > 1e-6)
{
Vec3 from = current / len1;
Vec3 to = target / len2;
float cosom = Dot(from, to);
if (cosom > 1 - 1e-6)
{
return(MoveTowards(current, target, maxMagnitudeDelta));
}
if (cosom < -1 + 1e-6)
{
Quat q = Quat.AngleAxis(maxRadiansDelta * MathUtils.RAD_TO_DEG, OrthoNormalVector(from));
return(q * from * ClampedMove(len1, len2, maxMagnitudeDelta));
}
else
{
float angle = MathUtils.Acos(cosom);
Quat q = Quat.AngleAxis(MathUtils.Min(maxRadiansDelta, angle) * MathUtils.RAD_TO_DEG,
Normalize(Cross(from, to)));
return(q * from * ClampedMove(len1, len2, maxMagnitudeDelta));
}
}
return(MoveTowards(current, target, maxMagnitudeDelta));
}
/* Simulates the forward kinematics,
* given a solution. */
public PositionRotation ForwardKinematics(float[] Solution)
{
Vec3 prevPoint = (Vec3)Joints[0].transform.position;
//Quaternion rotation = Quaternion.identity;
// Takes object initial rotation into account
Quat rotation = (Quat)transform.rotation;
for (int i = 1; i < Joints.Length; i++)
{
// Rotates around a new axis
rotation *= Quat.AngleAxis(Solution[i - 1], (Vec3)Joints[i - 1].Axis);
Vec3 nextPoint = prevPoint + rotation * Joints[i].StartOffset;
if (DebugDraw)
{
Debug.DrawLine((Vector3)prevPoint, (Vector3)nextPoint, Color.blue);
}
prevPoint = nextPoint;
}
// The end of the effector
return(new PositionRotation(prevPoint, rotation));
}
/// <summary>
/// Rotate the frame around a pivot point and axis
/// </summary>
/// <param name="point">pivot point</param>
/// <param name="axis">rotation axis</param>
/// <param name="angle">rotation angle</param>
public void RotateAround(Vec3 point, Vec3 axis, double angle)
{
var rotation = Quat.AngleAxis(axis, angle);
var position = ((Transformation)rotation) * (this.LocalPosition - point);
this.LocalPosition = position + point;
Rotate(rotation);
}
public void TestRotation()
{
var F1 = new Frame(Quat.Identity, Vec3.Zero);
var F2 = new Frame(Quat.AngleAxis(Vec3.K, -Math.PI / 2), Vec3.Zero); // rotated 90deg
var v1 = Vec3.One;
var v2 = F1.LocalToFramePoint(v1, F2);
Assert.AreEqual(new Vec3(-1, 1, 1).ToString(), v2.ToString());
}
public void TestNestedRotation()
{
var F1 = new Frame(Quat.AngleAxis(Vec3.K, -Math.PI / 2), Vec3.Zero); // rotated 90deg
F1.Parent = new Frame(Quat.AngleAxis(Vec3.K, -Math.PI / 2), Vec3.Zero); // rotated 90deg
var v1 = Vec3.One;
var v2 = F1.LocalToGlobalPoint(v1);
Assert.AreEqual(new Vec3(-1, -1, 1).ToString(), v2.ToString());
}
public Vec3 RotateAround(float angle, Vec3 axis)
{
// rotate into world space
Quat quaternion = Quat.AngleAxis(0, axis);
quaternion = quaternion.Conjugate();
Vec3 worldSpaceVector = quaternion.Transform(this);
// rotate back to vector space
quaternion = Quat.AngleAxis(angle, axis);
worldSpaceVector = quaternion.Transform(worldSpaceVector);
return(worldSpaceVector);
}
public Mat3 RotateAround(float angle, Vec3 axis)
{
// rotate into world space
Quat quaternion = Quat.AngleAxis(0, axis).Conjugate();
Mat3 worldSpaceMatrix = FromQuaternion(quaternion) * this;
// rotate back to matrix space
quaternion = Quat.AngleAxis(angle, axis);
Mat3 qMat = FromQuaternion(quaternion);
worldSpaceMatrix = qMat * worldSpaceMatrix;
return(worldSpaceMatrix);
}
//Before rendering a frame
void Update()
{
if (this.transform.position.y < -50)
{
CheckPoint();
}
switch (state)
{
case State.DIRECTION:
VisualDebug.DisableRenderer(VisualDebug.Vectors.GREEN, transform);
VisualDebug.DisableRenderer(VisualDebug.Vectors.RED, transform);
redArrow.gameObject.SetActive(true);
text.text = "Angle: " + Mathf.Round(ang).ToString();
// TIMER - ang
if (increaseAngle)
{
ang += Time.deltaTime * turnSpeed;
if (ang >= maxAngle)
{
increaseAngle = false;
}
}
else if (!increaseAngle)
{
ang -= Time.deltaTime * turnSpeed;
if (ang <= -maxAngle)
{
increaseAngle = true;
}
}
if (InputManager.Space())
{
Quat rotate = Quat.AngleAxis(ang, (Vec3)this.transform.up);
this.transform.rotation = (Quaternion)(rotate * (Quat)this.transform.rotation);
redArrow.gameObject.SetActive(false);
velocity = minForce;
state = State.PROPULSION;
previousPos = (Vec3)transform.position;
}
float angArrow = ang;
Quat rotateArrow = Quat.AngleAxis(ang - 90, (Vec3)this.transform.forward * -1);
redArrow.transform.rotation = (Quaternion)(rotateArrow * (Quat)this.transform.rotation);
break;
case State.PROPULSION:
powerBar.gameObject.SetActive(true);
text.text = "Force: " + Mathf.Round(velocity).ToString();
// TIMER - FORCE
if (increase)
{
velocity += Time.deltaTime * powerSpeed;
if (velocity >= maxForce)
{
increase = false;
}
}
else if (!increase)
{
velocity -= Time.deltaTime * powerSpeed;
if (velocity <= minForce)
{
increase = true;
}
}
powerBar.value = velocity;
// INPUT
if (InputManager.Space())
{
state = State.STRINGS;
grounded = false;
powerBar.gameObject.SetActive(false);
direction += (Vec3)transform.forward;
}
break;
case State.STRINGS:
direction = Physics_functions.Molla(direction, transform);
state = State.JUMP;
break;
case State.JUMP:
if (!grounded)
{
transform.position = (Vector3)Physics_functions.TirParabolic(direction, this.transform);
direction.y -= 9.81f * Time.deltaTime;
}
VisualDebug.DrawVector(direction, VisualDebug.Vectors.GREEN, transform);
break;
case State.LANDING:
Vec3 friction = Physics_functions.FrictionJump(direction, drag);
VisualDebug.DrawVector(friction, VisualDebug.Vectors.RED, transform);
//x = x0 + (v0 + Ft) * t
direction += Physics_functions.TirParabolic(friction, transform) * Time.deltaTime;
float aux = (direction * Time.deltaTime).Module();
if (aux >= 0.0001f && (direction.z * Time.deltaTime) >= 0)
{
transform.position += transform.forward * aux;
}
else
{
state = State.DIRECTION;
transform.rotation = (Quaternion)inicialRotation;
direction = auxVel;
}
break;
default:
break;
}
}
//Before rendering a frame
void Update()
{
if (this.transform.position.y < -100)
{
CheckPoint();
}
switch (state)
{
case State.DIRECTION:
VisualDebug.DisableRenderer(VisualDebug.Vectors.GREEN, transform);
redArrow.gameObject.SetActive(true);
text.text = "Angle: " + Mathf.Round(ang).ToString();
// TIMER - ang
if (increaseAngle)
{
ang += Time.deltaTime * turnSpeed;
if (ang >= maxAngle)
{
increaseAngle = false;
}
}
else if (!increaseAngle)
{
ang -= Time.deltaTime * turnSpeed;
if (ang <= -maxAngle)
{
increaseAngle = true;
}
}
if (InputManager.Space())
{
Quat rotate = Quat.AngleAxis(ang, (Vec3)this.transform.up);
this.transform.rotation = (Quaternion)(rotate * (Quat)this.transform.rotation);
redArrow.gameObject.SetActive(false);
velocity = minForce;
state = State.PROPULSION;
previousPos = (Vec3)transform.position;
}
float angArrow = ang;
Quat rotateArrow = Quat.AngleAxis(ang - 90, (Vec3)this.transform.forward * -1);
redArrow.transform.rotation = (Quaternion)(rotateArrow * (Quat)this.transform.rotation);
break;
case State.PROPULSION:
powerBar.gameObject.SetActive(true);
text.text = "Force: " + Mathf.Round(velocity).ToString();
// TIMER - FORCE
if (increase)
{
velocity += Time.deltaTime * powerSpeed;
if (velocity >= maxForce)
{
increase = false;
}
}
else if (!increase)
{
velocity -= Time.deltaTime * powerSpeed;
if (velocity <= minForce)
{
increase = true;
}
}
powerBar.value = velocity;
// INPUT
if (InputManager.Space())
{
state = State.STRINGS;
grounded = false;
powerBar.gameObject.SetActive(false);
direction += (Vec3)transform.forward;
//Vec3 endPos = Physics_functions.TrobarPosicioFinalTir((direction + (Vec3)this.transform.forward) * velocity, this.transform);
//GameObject.Find("base").GetComponent<ENTICourse.IK.InverseKinematics>().target = endPos;
}
break;
case State.STRINGS:
direction = Physics_functions.Molla(direction, transform);
state = State.JUMP;
break;
case State.JUMP:
if (!grounded)
{
transform.position = (Vector3)Physics_functions.TirParabolic(direction, this.transform);
direction.y -= 9.81f * Time.deltaTime;
//endPos = Physics_functions.TrobarPosicioFinalTir(direction, (Vec3)this.transform.position);
}
VisualDebug.DrawVector(direction, VisualDebug.Vectors.GREEN, transform);
break;
case State.LANDING:
Vec3 friction = Physics_functions.FrictionJump(direction, drag);
VisualDebug.DrawVector(friction, VisualDebug.Vectors.RED, transform);
direction += friction * Time.deltaTime;
if (direction.x > 0.1f && direction.z > 0.1f)
{
transform.position += (Vector3)direction;
}
else
{
state = State.DIRECTION;
}
break;
default:
break;
}
}
public void Quat4()
{
FQuat fq = FQuat.Euler(( Fix64 )45, ( Fix64 )(-23), ( Fix64 )(-48.88));
FQuat fq2 = FQuat.Euler(( Fix64 )23, ( Fix64 )(-78), ( Fix64 )(-132.43f));
Quat q = Quat.Euler(45, -23, -48.88f);
Quat q2 = Quat.Euler(23, -78, -132.43f);
FVec3 fv = new FVec3(12.5f, 9, 8);
FVec3 fv2 = new FVec3(1, 0, 0);
Vec3 v = new Vec3(12.5f, 9, 8);
Vec3 v2 = new Vec3(1, 0, 0);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
this._output.WriteLine(fq2.ToString());
this._output.WriteLine(q2.ToString());
Fix64 fa = FQuat.Angle(fq, fq2);
float a = Quat.Angle(q, q2);
this._output.WriteLine(fa.ToString());
this._output.WriteLine(a.ToString());
fq = FQuat.AngleAxis(( Fix64 )(-123.324), fv);
q = Quat.AngleAxis(-123.324f, v);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fa = FQuat.Dot(fq, fq2);
a = Quat.Dot(q, q2);
this._output.WriteLine(fa.ToString());
this._output.WriteLine(a.ToString());
fq = FQuat.FromToRotation(FVec3.Normalize(fv), fv2);
q = Quat.FromToRotation(Vec3.Normalize(v), v2);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fq = FQuat.Lerp(fq, fq2, ( Fix64 )0.66);
q = Quat.Lerp(q, q2, 0.66f);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fq = FQuat.Normalize(fq);
q.Normalize();
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fq.Inverse();
q = Quat.Inverse(q);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fv = FQuat.Orthogonal(fv);
v = Quat.Orthogonal(v);
this._output.WriteLine(fv.ToString());
this._output.WriteLine(v.ToString());
fq = FQuat.Slerp(fq, fq2, ( Fix64 )0.66);
q = Quat.Slerp(q, q2, 0.66f);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fq = FQuat.LookRotation(FVec3.Normalize(fv), fv2);
q = Quat.LookRotation(Vec3.Normalize(v), v2);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fq.ToAngleAxis(out fa, out fv);
q.ToAngleAxis(out a, out v);
this._output.WriteLine(fa.ToString());
this._output.WriteLine(a.ToString());
this._output.WriteLine(fv.ToString());
this._output.WriteLine(v.ToString());
fq = fq.Conjugate();
q = q.Conjugate();
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fq.SetLookRotation(FVec3.Normalize(fv), fv2);
q.SetLookRotation(Vec3.Normalize(v), v2);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
}
// Running the solver - all the joints are iterated through once every frame
void Update()
{
// if the target hasn't been reached
if (!done)
{
// if the Max number of tries hasn't been reached
if (tries <= Mtries)
{
// starting from the second last joint (the last being the end effector)
// going back up to the root
for (int i = joints.Length - 2; i >= 0; i--)
{
// The vector from the ith joint to the end effector
Vec3 r1 = joints[joints.Length - 1].transform.position - joints[i].transform.position;
// The vector from the ith joint to the target
//Vector3 r2 = TODO2
Vec3 r2 = tpos - new Vec3(joints[i].transform.position);
// to avoid dividing by tiny numbers
if (r1.Module() * r2.Module() <= 0.001f)
{
// cos will be 1 and sin will be 0
cos[i] = 1;
sin[i] = 0;
}
else
{
// find the components (cos i sin) using dot and cross product
cos[i] = r1.DotProduct(r2) / (r1.Module() * r2.Module());
sin[i] = r1.CrossProduct(r2).Module() / (r1.Module() * r2.Module());
}
// The axis of rotation
Vec3 axis = r1.CrossProduct(r2).Normalize();
// find the angle between r1 and r2 (and clamp values if needed avoid errors)
theta[i] = Mathf.Acos(Mathf.Max(-1, Mathf.Min(1, cos[i])));
//Optional. correct angles if needed, depending on angles invert angle if sin component is negative
if (sin[i] < 0)
{
theta[i] = -theta[i];
}
// obtain an angle value between -pi and pi, and then convert to degrees
//theta[i] = TODO8
theta[i] = (float)SimpleAngle(theta[i]) * Mathf.Rad2Deg;
// rotate the ith joint along the axis by theta degrees in the world space.
// TODO9
joints[i].transform.rotation = Quat.AngleAxis(theta[i], axis) * new Quat(joints[i].transform.rotation);
rotationLimit[i].Apply();
}
// increment tries
tries++;
}
}
// find the difference in the positions of the end effector and the target
// TODO10
//float f = (tpos - new Vec3(joints[joints.Length - 1].transform.position)).Module();
float x = Mathf.Abs(joints[joints.Length - 1].transform.position.x - targ.transform.position.x);
float y = Mathf.Abs(joints[joints.Length - 1].transform.position.y - targ.transform.position.y);
float z = Mathf.Abs(joints[joints.Length - 1].transform.position.z - targ.transform.position.z);
// if target is within reach (within epsilon) then the process is done
if (x < epsilon && y < epsilon && z < epsilon)
{
done = true;
}
// if it isn't, then the process should be repeated
else
{
done = false;
}
// the target has moved, reset tries to 0 and change tpos
if (targ.transform.position != tpos)
{
tries = 0;
tpos = targ.transform.position;
}
}
public static Mat3 FromRotationAxis(float angle, Vec3 axis)
{
Quat quaternion = Quat.AngleAxis(angle, axis);
return(FromQuaternion(quaternion));
}
// ApplyBoneOrientationConstraints and constrain rotations.
public void Constrain(ref Matrix4x4[] jointOrientations, ref bool[] jointTracked)
{
// Constraints are defined as a vector with respect to the parent bone vector, and a constraint angle,
// which is the maximum angle with respect to the constraint axis that the bone can move through.
// Calculate constraint values. 0.0-1.0 means the bone is within the constraint cone. Greater than 1.0 means
// it lies outside the constraint cone.
for (int i = 0; i < this.jointConstraints.Count; i++)
{
BoneOrientationConstraint jc = this.jointConstraints[i];
if (!jointTracked[i] || jc.joint == (int)NuiSkeletonPositionIndex.HipCenter)
{
// End joint is not tracked or Hip Center has no parent to perform this calculation with.
continue;
}
// If the joint has a parent, constrain the bone direction to be within the constraint cone
int parentIdx = GetSkeletonJointParent(jc.joint);
// Local bone orientation relative to parent
Matrix4x4 localOrientationMatrix = Invert(jointOrientations[parentIdx]) * jointOrientations[jc.joint];
Vector3 localOrientationZ = (Vector3)localOrientationMatrix.GetColumn(2);
Vector3 localOrientationY = (Vector3)localOrientationMatrix.GetColumn(1);
if (localOrientationZ == Vector3.zero || localOrientationY == Vector3.zero)
{
continue;
}
Quat localRotation = Quat.LookRotation(localOrientationZ, localOrientationY);
Vector3 eulerAngles = localRotation.eulerAngles;
bool isConstrained = false;
//Matrix4x4 globalOrientationMatrix = jointOrientations[jc.joint];
//Quaternion globalRotation = Quaternion.LookRotation(globalOrientationMatrix.GetColumn(2), globalOrientationMatrix.GetColumn(1));
for (int a = 0; a < jc.axisConstrainrs.Count; a++)
{
AxisOrientationConstraint ac = jc.axisConstrainrs[a];
Quat axisRotation = Quat.AngleAxis(localRotation.eulerAngles[ac.axis], ac.rotateAround);
//Quaternion axisRotation = Quaternion.AngleAxis(globalRotation.eulerAngles[ac.axis], ac.rotateAround);
float angleFromMin = Quat.Angle(axisRotation, quatToQuat2(ac.minQuaternion));
float angleFromMax = Quat.Angle(axisRotation, quatToQuat2(ac.maxQuaternion));
if (!(angleFromMin <= ac.angleRange && angleFromMax <= ac.angleRange))
{
// Keep the current rotations around other axes and only
// correct the axis that has fallen out of range.
//Vector3 euler = globalRotation.eulerAngles;
if (angleFromMin > angleFromMax)
{
eulerAngles[ac.axis] = ac.angleMax;
}
else
{
eulerAngles[ac.axis] = ac.angleMin;
}
isConstrained = true;
}
}
if (isConstrained)
{
Quat constrainedRotation = Quat.Euler(eulerAngles);
// Put it back into the bone orientations
localOrientationMatrix = Transform(localOrientationMatrix, quatToQuat(constrainedRotation));
jointOrientations[jc.joint] = jointOrientations[parentIdx] * localOrientationMatrix;
//globalOrientationMatrix.SetTRS(Vector3.zero, constrainedRotation, Vector3.one);
//jointOrientations[jc.joint] = globalOrientationMatrix;
switch (jc.joint)
{
case (int)NuiSkeletonPositionIndex.ShoulderCenter:
jointOrientations[(int)NuiSkeletonPositionIndex.Head] = jointOrientations[jc.joint];
break;
case (int)NuiSkeletonPositionIndex.WristLeft:
jointOrientations[(int)NuiSkeletonPositionIndex.HandLeft] = jointOrientations[jc.joint];
break;
case (int)NuiSkeletonPositionIndex.WristRight:
jointOrientations[(int)NuiSkeletonPositionIndex.HandRight] = jointOrientations[jc.joint];
break;
case (int)NuiSkeletonPositionIndex.AnkleLeft:
jointOrientations[(int)NuiSkeletonPositionIndex.FootLeft] = jointOrientations[jc.joint];
break;
case (int)NuiSkeletonPositionIndex.AnkleRight:
jointOrientations[(int)NuiSkeletonPositionIndex.FootRight] = jointOrientations[jc.joint];
break;
}
}
// globalRotation = Quaternion.LookRotation(globalOrientationMatrix.GetColumn(2), globalOrientationMatrix.GetColumn(1));
// string stringToDebug = string.Format("{0}, {2}", (KinectWrapper.NuiSkeletonPositionIndex)jc.joint,
// globalRotation.eulerAngles, localRotation.eulerAngles);
// Debug.Log(stringToDebug);
//
// if(debugText != null)
// debugText.guiText.text = stringToDebug;
}
}
