private Matrix4x4 RotateAroundAxis(double angle, Vector3D axis)
{
if (VMath.Dist(new Vector3D(0), axis) <= 0)
{
return(VMath.IdentityMatrix);
}
axis = 1 * axis / VMath.Dist(new Vector3D(0), axis);
double c = Math.Cos(angle);
double s = Math.Sin(angle);
Matrix4x4 ret = new Matrix4x4();
ret.m11 = axis.x * axis.x * (1 - c) + c;
ret.m12 = axis.x * axis.y * (1 - c) - axis.z * s;
ret.m13 = axis.x * axis.z * (1 - c) + axis.y * s;
ret.m14 = 0;
ret.m21 = axis.y * axis.x * (1 - c) + axis.z * s;
ret.m22 = axis.y * axis.y * (1 - c) + c;
ret.m23 = axis.y * axis.z * (1 - c) - axis.x * s;
ret.m24 = 0;
ret.m31 = axis.x * axis.z * (1 - c) - axis.y * s;
ret.m32 = axis.y * axis.z * (1 - c) + axis.x * s;
ret.m33 = axis.z * axis.z * (1 - c) + c;
ret.m34 = 0;
ret.m41 = 0;
ret.m42 = 0;
ret.m43 = 0;
ret.m44 = 1;
return(ret);
}
//called when data for any output pin is requested
public void Evaluate(int SpreadMax)
{
//for each frame empty the lists
vertices.Clear();
indices.Clear();
distance.Clear();
float radius = IRadius[0];
//for every incoming vector...
for (int i = 0; i < SpreadMax; i++)
{
//check every other incoming vector...
for (int j = i + 1; j < SpreadMax; j++)
{
float dist = (float)VMath.Dist(IInput[i], IInput[j]);
if (dist < radius)
{
vertices.Add(IInput[i]);
vertices.Add(IInput[j]);
indices.Add(i);
indices.Add(j);
distance.Add(dist / radius);
}
}
}
OVertices.AssignFrom(vertices);
OIndices.AssignFrom(indices);
ODistance.AssignFrom(distance);
}
public static Vector2D CalculateTransform(TransformState state, TransformType type)
{
var blobs = state.Blobs;
var pBlobs = state.PBlobs;
if (pBlobs.Count() != blobs.Count())
{
return(new Vector2D());
}
var value = new Vector2D();
var pValue = new Vector2D();
var delta = new Vector2D();
switch (type)
{
case TransformType.Scale:
if (blobs.Count() < 2)
{
Debug.WriteLine("[WARNING] Scale state have less than 2 fingers");
return(new Vector2D());
}
value.x = value.y = VMath.Dist(blobs.First().Position, blobs.Last().Position);
pValue.x = pValue.y = VMath.Dist(pBlobs.First().Position, pBlobs.Last().Position);
delta = value - pValue;
//Debug.WriteLine(blobs.Count());
break;
case TransformType.Rotate:
if (blobs.Count() < 2)
{
//Debug.WriteLine("[WARNING] Rotate state have less than 2 fingers");
return(new Vector2D());
}
value.x = value.y = FindAngle(blobs.First(), blobs.Last());
pValue.x = pValue.y = FindAngle(pBlobs.First(), pBlobs.Last());
delta.x = delta.y = SubtractCycles(value.x, pValue.x);
break;
case TransformType.Translate:
value = FindCentroid(blobs);
pValue = FindCentroid(pBlobs);
delta = value - pValue;
break;
}
if (Math.Abs(delta.x) > 0.1 || Math.Abs(delta.y) > 0.1)
{
delta = new Vector2D();
}
return(delta);
//return delta.LinearEasing(pDelta);
}
private double vectorAngle(Vector3D a, Vector3D b)
{
double adotb = a.x * b.x + a.y * b.y + a.z * b.z;
double maga = VMath.Dist(new Vector3D(0), a);
double magb = VMath.Dist(new Vector3D(0), b);
if (maga * magb == 0)
{
return(0);
}
return(Math.Acos(adotb / (maga * magb)));
}
protected override void Behave(IEnumerable <IAgent> agents)
{
FCurrentAgentCount.Sync();
FMaxAgentCount.Sync();
FStarveCount.Sync();
FCloneCount.Sync();
FRadius.Sync();
FCloneOut.SliceCount = 0;
var diff = FMaxAgentCount[0] - FCurrentAgentCount[0];
var starve = FStarveCount[0];
var clone = FCloneCount[0];
var radius = FRadius[0];
foreach (dynamic agent in agents)
{
var closeBy = from peer in agents
where peer != agent
where VMath.Dist((Vector3D)peer["Position"].First, (Vector3D)agent["Position"].First) < radius
select peer;
if (closeBy.Count() < starve)
{
agent.Feed(0.005);
if (agent.Health < 0.1)
{
agent.Killed = true;
agent.ReturnCode = ReturnCodeEnum.Failure;
}
}
if ((closeBy.Count() >= clone))
{
if (agent.Health >= 0.9)
{
if (diff > 0)
{
agent.Burn(0.5);
FCloneOut.Add((Agent)agent.Clone());
diff--;
}
}
}
FCloneOut.Flush();
}
}
//called when data for any output pin is requested
public void Evaluate(int SpreadMax)
{
//for each frame empty the lists
FDots.Clear();
FIndices.Clear();
FDistances.Clear();
//for every incoming vector...
for (int i = 0; i < SpreadMax; i++)
{
//check every other incoming vector...
for (int j = i + 1; j < SpreadMax; j++)
{
//if the distance is less than the given radius...
var d = VMath.Dist(FInput[i], FInput[j]);
if (d <= FMaxRadius[0])
{
//add both vecotors to the list
FDots.Add(FInput[i]);
FDots.Add(FInput[j]);
//add index i
FIndices.Add(i);
//add the distace between the two vectors to the list
FDistances.Add(d / FMaxRadius[0]);
}
}
}
//in beta>24.1 you can directly assign lists to pins
FOutput.AssignFrom(FDots);
FIndex.AssignFrom(FIndices);
FDistance.AssignFrom(FDistances);
//in beta24.1 you still have to move the list entries
//to the output slices manually
// FOutput.SliceCount = FDots.Count;
// FDistance.SliceCount = FDistances.Count;
// FIndex.SliceCount = FIndices.Count;
//
// for (int i = 0; i<FDots.Count; i++)
// FOutput[i] = FDots[i];
//
// for (int i = 0; i<FDistances.Count; i++)
// FDistance[i] = FDistances[i];
//
// for (int i = 0; i<FIndices.Count; i++)
// FIndex[i] = FIndices[i];
}
//the function that we pass to the threads
private void CalcPressure(object input)
{
//split up the parameter object we defined as parameter:
//new object[] {pressure, start, length}
//first we cast it to an object array
object[] inArray = (object[])input;
//then we get the objects in the array and cast them back to our parameters
Vector4D extPressure = (Vector4D)inArray[0];
int start = Math.Min((int)inArray[1], FSize - 1);
//calc the end = start + length
int end = Math.Min(start + (int)inArray[2], FSize);
for (int slice = start; slice < end; slice++)
{
//calc the 3d indices
int i = slice % FSizeX;
int j = (slice / FSizeX) % FSizeY;
int k = slice / (FSizeX * FSizeY);
//get earlier states
float p1 = FLastState[slice];
float p2 = FBeforeLastState[slice];
//get neighbours
float n1 = Get3D(FLastState, i + 1, j, k);
float n2 = Get3D(FLastState, i - 1, j, k);
float n3 = Get3D(FLastState, i, j + 1, k);
float n4 = Get3D(FLastState, i, j - 1, k);
float n5 = Get3D(FLastState, i, j, k + 1);
float n6 = Get3D(FLastState, i, j, k - 1);
//sum up the neighbours
float p1n = (n1 + n2 + n3 + n4 + n5 + n6) * 0.1666f;
//wave distribution formula
FCurrentState[slice] = (p1 + (float)FDecay * (p1 - p2)) + (float)FAttack * (p1n - p1);
//add external pressure
Vector3D pos = (new Vector3D(i, j, k) / new Vector3D(FSizeX * 0.5, FSizeY * 0.5, FSizeZ * 0.5)) - 1;
if (VMath.Dist(extPressure.xyz, pos) < 0.2)
{
FCurrentState[slice] += (float)extPressure.w;
}
}
}
private bool hasClicked(IJoint currJoint, Vector3D mousePos)
{
if (!jointPositions.ContainsKey(currJoint.Name))
{
return(false);
}
if (VMath.Dist(mousePos, jointPositions[currJoint.Name]) < 5)
{
bool alreadySelected = false;
for (int i = 0; i < selectedJoints.Count; i++)
{
if (selectedJoints[i] == currJoint)
{
alreadySelected = true;
break;
}
}
if (Control.ModifierKeys != Keys.Control)
{
selectedJoints.Clear();
}
if (alreadySelected && Control.ModifierKeys == Keys.Control)
{
selectedJoints.Remove(currJoint);
selectedJoint = null;
}
else
{
selectedJoints.Add(currJoint);
selectedJoint = currJoint;
}
Invalidate();
return(true);
}
for (int i = 0; i < currJoint.Children.Count; i++)
{
if (hasClicked(currJoint.Children[i], mousePos))
{
return(true);
}
}
return(false);
}
protected override void Behave(IEnumerable <IAgent> agents)
{
int i = 0;
FCenter.Sync();
FCenterMass.Sync();
foreach (var a in agents)
{
var agent = a.Face <IGravityAgent>();
var distance = VMath.Dist(agent.Position, FCenter[i]);
var otherMass = FCenterMass[i];
agent.ForceSum += FGravitationalConstant[0] * agent.Mass * otherMass / distance * distance; // one of Newtons Laws
i++;
agent.ReturnCode = ReturnCodeEnum.Success;
}
}
private bool hasHovered(IJoint currJoint, Vector3D mousePos)
{
if (!jointPositions.ContainsKey(currJoint.Name))
{
return(false);
}
if (VMath.Dist(mousePos, jointPositions[currJoint.Name]) < 5)
{
hoveredJoint = currJoint;
return(true);
}
for (int i = 0; i < currJoint.Children.Count; i++)
{
if (hasHovered(currJoint.Children[i], mousePos))
{
return(true);
}
}
return(false);
}
protected override void OnMouseMove(MouseEventArgs e)
{
if (!AllowDrag || IsDragging)
{
return;
}
if (!e.Button.IsLeft())
{
return;
}
var mousePos = new Vector2D(e.X, e.Y);
// This is a check to see if the mouse is moving while pressed.
// Without this, the DragDrop is fired directly when the control is clicked, now you have to drag a few pixels first.
if (VMath.Dist(mousePos, DragStartPosition) > MinDragPixel)
{
DoDragDrop(this, DragDropEffects.All);
IsDragging = true;
}
}
private double calculateTorque(IJoint joint, Vector3D axis)
{
Vector3D targetPosLocal = VMath.Inverse(joint.CombinedTransform) * targetPosW;
Vector3D endPosLocal = VMath.Inverse(joint.CombinedTransform) * endPosW;
if (double.IsInfinity(endPosLocal.x) || double.IsNaN(endPosLocal.x))
{
return(0);
}
Vector3D f = targetPosLocal - endPosLocal;
Vector3D a = axis;
Vector3D b = endPosLocal;
Vector3D r = new Vector3D(0);
r.x = a.y * b.z - a.z * b.y;
r.y = a.z * b.x - a.x * b.z;
r.z = a.x * b.y - a.y * b.x;
double torque = VMath.Dist(new Vector3D(0), f) * Math.Sin(vectorAngle(a, f)) * Math.Sin(vectorAngle(b, f)) * Math.Sign(Math.Cos(vectorAngle(r, f))) * 0.03;
return(torque);
}
private IJoint getNearestBone(Vector3D pos, IJoint currJoint, out double nearestDistance)
{
double distBone = 0.0;
Vector3D origin = new Vector3D(0);
Matrix4x4 t = currJoint.CombinedTransform;
Vector3D p1 = t * origin;
distBone = VMath.Dist(pos, p1);
for (int i = 0; i < currJoint.Children.Count; i++)
{
bool outside = false;
t = currJoint.Children[i].CombinedTransform;
Vector3D p2 = t * origin;
Vector3D v = p2 - p1;
Vector3D w = pos - p1;
double angle = (v.x * w.x + v.y * w.y + v.z * w.z) / (Math.Sqrt(v.x * v.x + v.y * v.y + v.z * v.z) * Math.Sqrt(w.x * w.x + w.y * w.y + w.z * w.z));
angle = Math.Acos(angle);
if (Math.Abs(angle) > 3.14 / 2)
{
distBone = VMath.Min(distBone, VMath.Dist(pos, p1));
outside = true;
}
Vector3D v_ = p1 - p2;
Vector3D w_ = pos - p2;
angle = (v_.x * w_.x + v_.y * w_.y + v_.z * w_.z) / (Math.Sqrt(v_.x * v_.x + v_.y * v_.y + v_.z * v_.z) * Math.Sqrt(w_.x * w_.x + w_.y * w_.y + w_.z * w_.z));
angle = Math.Acos(angle);
if (Math.Abs(angle) > 3.14 / 2)
{
distBone = VMath.Min(distBone, VMath.Dist(pos, p2));
outside = true;
}
if (!outside)
{
Vector3D vxw = new Vector3D();
vxw.x = v.y * w.z - v.z * w.y;
vxw.y = v.z * w.x - v.x * w.z;
vxw.z = v.x * w.y - v.y * w.x;
distBone = VMath.Min(distBone, System.Math.Sqrt(vxw.x * vxw.x + vxw.y * vxw.y + vxw.z * vxw.z) / System.Math.Sqrt(v.x * v.x + v.y * v.y + v.z * v.z));
}
}
nearestDistance = 1000000;
IJoint nearestChild = currJoint;
for (int i = 0; i < currJoint.Children.Count; i++)
{
double childDistance;
IJoint candidate = getNearestBone(pos, (IJoint)currJoint.Children[i], out childDistance);
if (childDistance < nearestDistance)
{
nearestChild = candidate;
nearestDistance = childDistance;
}
}
if (nearestDistance <= distBone)
{
return(nearestChild);
}
else
{
nearestDistance = distBone;
return(currJoint);
}
}
//called when data for any output pin is requested
public void Evaluate(int SpreadMax)
{
if (FPoints.SliceCount == 0 || FStep.SliceCount == 0 || FMinStep.SliceCount == 0)
{
FResampled.SliceCount = 0;
return;
}
FResampled.SliceCount = FPoints.SliceCount;
for (int i = 0; i < FPoints.SliceCount; i++)
{
if (FPoints[i].SliceCount == 0)
{
FResampled[i].SliceCount = 1;
FResampled[i][0] = new Vector2D();
break;
}
// security mesure to not allow stepsize of zero
double px = FMinStep[i];
if (FStep[i] != 0)
{
px = FStep[i];
}
IList <Vector2D> srcPts = new List <Vector2D>(FPoints[i]); // copy source points so we can insert into them
IList <Vector2D> dstPts = new List <Vector2D>(); // create the destination points that we return
dstPts.Add(srcPts[0]); // add the first source point as the first destination point
double D = 0.0; // accumulated distance
for (int j = 1; j < srcPts.Count; j++)
{
Vector2D pt1 = srcPts[j - 1];
Vector2D pt2 = srcPts[j];
double d = VMath.Dist(pt1, pt2); // distance in space
if ((D + d) >= px) // has enough space been traversed in the last step?
{
double qx = pt1.x + ((px - D) / d) * (pt2.x - pt1.x); // interpolate position
double qy = pt1.y + ((px - D) / d) * (pt2.y - pt1.y); // interpolate position
Vector2D q = new Vector2D(qx, qy);
dstPts.Add(q); // append new point 'q'
srcPts.Insert(j, q); // insert 'q' at position j in points s.t. 'q' will be the next i
D = 0.0;
}
else
{
D += d; // accumulator
}
}
// unless px divides evenly into the path length (not likely), we will have some accumulation
// left over, so just add the last point as the last point, which will not be the full interval.
if (D > 0.000001)
{
dstPts.Add(srcPts[srcPts.Count - 1]);
}
FResampled[i].AssignFrom(dstPts);
//FResampled.Insert(0, dstPts.ToSpread());
}
}
override protected double Distance(Vector3D t1, Vector3D t2)
{
return(VMath.Dist(t1, t2));
}
public static double Distance(this IAgent agent, IAgent otherAgent)
{
return(VMath.Dist((Vector3D)agent["Position"].First, (Vector3D)otherAgent["Position"].First));
}
private IEnumerable <EtherDreamPoint> GetFrame()
{
var colIndex = 0;
var shapeIndex = 0;
var result = Enumerable.Empty <EtherDreamPoint>();
foreach (var shape in FPointsInput)
{
var isClosed = FClosedShapeInput[shapeIndex];
Vector2D start;
Vector2D end;
if (isClosed)
{
start = shape[shape.SliceCount - 1];
end = start;
}
else
{
start = shape[0];
end = shape[shape.SliceCount - 1];
}
//start blanks
result = result.Concat(Enumerable.Repeat(CreateEtherDreamPoint(start, VColor.Black), FStartBlanksInput[shapeIndex]));
var lastPoint = Vector2D.Zero;
var doInterpolate = false;
foreach (var p in shape)
{
var col = FColorsInput[colIndex++];
//interpolate from last point
if (doInterpolate)
{
var count = (int)Math.Floor(VMath.Dist(lastPoint, p) / Math.Max(FPointInterpolationDistanceInput[shapeIndex], 0.0001));
var factor = 1.0 / (count + 1);
//points in between, need to be caculated directly since linq lazyness would access only the last value in lastPoint
result = result.Concat(Enumerable.Range(1, count).Select(index => CreateEtherDreamPoint(VMath.Lerp(lastPoint, p, index * factor), col)).ToArray());
}
else if (isClosed) // first iteration
{
var count = (int)Math.Floor(VMath.Dist(end, p) / Math.Max(FPointInterpolationDistanceInput[shapeIndex], 0.0001));
var factor = 1.0 / (count + 1);
//points in between, need to be caculated directly since linq lazyness would access only the last value in lastPoint
result = result.Concat(Enumerable.Range(1, count).Select(index => CreateEtherDreamPoint(VMath.Lerp(end, p, index * factor), col)).ToArray());
}
//actual point
result = result.Concat(Enumerable.Repeat(CreateEtherDreamPoint(p, col), FPointRepeatInput[shapeIndex]));
lastPoint = p;
doInterpolate = true;
}
//end blanks
result = result.Concat(Enumerable.Repeat(CreateEtherDreamPoint(end, VColor.Black), FEndBlanksInput[shapeIndex++]));
}
return(result);
}
//here we go, thats the method called by vvvv each frame
//all data handling should be in here
public void Evaluate(int SpreadMax)
{
//if any of the inputs has changed
//recompute the outputs
bool recalculate = false;
bool chainRangeChanged = false;
bool recalculateOrientation = false;
if (FChainStart.PinIsChanged)
{
FChainStart.GetString(0, out chainStart);
recalculate = true;
chainRangeChanged = true;
}
if (FChainEnd.PinIsChanged)
{
FChainEnd.GetString(0, out chainEnd);
recalculate = true;
chainRangeChanged = true;
}
object currInterface;
if (FPoseInput.PinIsChanged || chainRangeChanged)
{
if (FPoseInput.IsConnected)
{
FPoseInput.GetUpstreamInterface(out currInterface);
Skeleton s = (Skeleton)currInterface;
if (outputSkeleton == null || !s.Uid.Equals(outputSkeleton.Uid))
{
outputSkeleton = (Skeleton)((Skeleton)currInterface).DeepCopy();
outputSkeleton.BuildJointTable();
workingSkeleton = (Skeleton)outputSkeleton.DeepCopy();
workingSkeleton.BuildJointTable();
chainRangeChanged = true;
}
else
{
foreach (KeyValuePair <string, IJoint> pair in s.JointTable)
{
if (!jointChain.Exists(delegate(IJoint j) { return(j.Name == pair.Key); }))
{
outputSkeleton.JointTable[pair.Key].BaseTransform = pair.Value.BaseTransform;
outputSkeleton.JointTable[pair.Key].AnimationTransform = pair.Value.AnimationTransform;
workingSkeleton.JointTable[pair.Key].BaseTransform = pair.Value.BaseTransform;
workingSkeleton.JointTable[pair.Key].AnimationTransform = pair.Value.AnimationTransform;
}
outputSkeleton.JointTable[pair.Key].Constraints = pair.Value.Constraints;
workingSkeleton.JointTable[pair.Key].Constraints = pair.Value.Constraints;
}
}
workingSkeleton.CalculateCombinedTransforms();
recalculate = true;
}
else
{
outputSkeleton = null;
}
}
if (FVelocityInput.PinIsChanged)
{
double x;
FVelocityInput.GetValue(0, out x);
iterationsPerFrame = (int)(x * 10);
}
if (iterationsPerFrame > 0)
{
if (FTargetInput.PinIsChanged)
{
targetPosW = new Vector3D();
FTargetInput.GetValue3D(0, out targetPosW.x, out targetPosW.y, out targetPosW.z);
recalculate = true;
}
if (FEpsilonInput.PinIsChanged)
{
FEpsilonInput.GetValue(0, out epsilon);
recalculate = true;
}
if (FPoleTargetInput.PinIsChanged || FEnablePoleTargetInput.PinIsChanged)
{
double x;
FEnablePoleTargetInput.GetValue(0, out x);
enablePoleTarget = x > 0.0;
poleTargetW = new Vector3D();
FPoleTargetInput.GetValue3D(0, out poleTargetW.x, out poleTargetW.y, out poleTargetW.z);
recalculateOrientation = true;
}
if (chainRangeChanged && outputSkeleton != null)
{
initRotations();
}
double delta = VMath.Dist(endPosW, targetPosW);
if ((delta > epsilon || recalculate) && outputSkeleton != null && !string.IsNullOrEmpty(chainStart) && !string.IsNullOrEmpty(chainEnd))
{
List <Vector2D> constraints = new List <Vector2D>();
for (int i = 0; i < iterationsPerFrame; i++)
{
for (int j = 0; j < 3; j++)
{
IJoint currJoint = workingSkeleton.JointTable[chainEnd];
endPosW = currJoint.CombinedTransform * new Vector3D(0);
while (currJoint.Name != chainStart)
{
currJoint = currJoint.Parent;
Vector3D rotationAxis = new Vector3D(0, 0, 0);
rotationAxis[j] = 1;
double torque = calculateTorque(currJoint, rotationAxis);
Vector3D rot = rotations[currJoint.Name];
if ((rot[j] + torque) < currJoint.Constraints[j].x * 2 * Math.PI || (rot[j] + torque) > currJoint.Constraints[j].y * 2 * Math.PI)
{
torque = 0;
}
Matrix4x4 newTransform = VMath.Rotate(torque * rotationAxis.x, torque * rotationAxis.y, torque * rotationAxis.z) * currJoint.AnimationTransform;
Vector3D testVec = newTransform * new Vector3D(0);
if (!Double.IsInfinity(testVec.x) && !Double.IsNaN(testVec.x)) // an evil bug fix, to avoid n.def. values in animation transform matrix. the actual reason, why this would happen has not been found yet.
{
rot[j] += torque;
rotations[currJoint.Name] = rot;
currJoint.AnimationTransform = newTransform;
outputSkeleton.JointTable[currJoint.Name].AnimationTransform = currJoint.AnimationTransform;
}
}
}
try
{
Matrix4x4 pre;
if (workingSkeleton.JointTable[chainStart].Parent != null)
{
pre = workingSkeleton.JointTable[chainStart].Parent.CombinedTransform;
}
else
{
pre = VMath.IdentityMatrix;
}
((JointInfo)workingSkeleton.JointTable[chainStart]).CalculateCombinedTransforms(pre);
}
catch (Exception)
{
workingSkeleton.CalculateCombinedTransforms();
}
}
FPoseOutput.MarkPinAsChanged();
}
if ((recalculate || recalculateOrientation) && enablePoleTarget && outputSkeleton != null && !string.IsNullOrEmpty(chainStart) && !string.IsNullOrEmpty(chainEnd))
{
endPosW = workingSkeleton.JointTable[chainEnd].CombinedTransform * new Vector3D(0);
Vector3D poleTargetLocal = VMath.Inverse(workingSkeleton.JointTable[chainStart].CombinedTransform) * poleTargetW;
Vector3D t = VMath.Inverse(workingSkeleton.JointTable[chainStart].CombinedTransform) * endPosW; // endpoint in local coords
Vector3D a = VMath.Inverse(workingSkeleton.JointTable[chainStart].CombinedTransform) * (workingSkeleton.JointTable[chainStart].Children[0].CombinedTransform * new Vector3D(0)); // next child in local coords
Vector3D x = t * ((a.x * t.x + a.y * t.y + a.z * t.z) / Math.Pow(VMath.Dist(new Vector3D(0), t), 2));
Vector3D y = t * ((poleTargetLocal.x * t.x + poleTargetLocal.y * t.y + poleTargetLocal.z * t.z) / Math.Pow(VMath.Dist(new Vector3D(0), t), 2));
Vector3D c = poleTargetLocal - y;
Vector3D b = a - x;
double angle = vectorAngle(b, c);
Vector3D n = new Vector3D();
n.x = c.y * b.z - c.z * b.y;
n.y = c.z * b.x - c.x * b.z;
n.z = c.x * b.y - c.y * b.x;
n = n / VMath.Dist(new Vector3D(0), n);
FDebugOutput.SetValue(0, angle);
chainRotation = RotateAroundAxis(angle, n);
FPoseOutput.MarkPinAsChanged();
}
if (!enablePoleTarget)
{
chainRotation = VMath.IdentityMatrix;
}
outputSkeleton.JointTable[chainStart].AnimationTransform = chainRotation * outputSkeleton.JointTable[chainStart].AnimationTransform;
}
FPoseOutput.SetInterface(outputSkeleton);
}
