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); }