void Start() { distances = new float[joints.Length - 1]; copy = new Vector3[joints.Length]; //diferencia = Vector3.Distance(target.transform.position, terra.transform.position); diferencia = MyVector3.Distance(target.transform.position, terra.transform.position); stop = false; oldPosition = target.transform.position; }
void Update() { // Copy the joints positions to work with for (int i = 0; i < joints.Length; i++) { copy[i] = new MyVector3(joints[i].position.x, joints[i].position.y, joints[i].position.z); //Copy the joints if (i < joints.Length - 1) { distances[i] = MyVector3.Distance(joints[i + 1].position, joints[i].position); //Calculate the distances } } done = (copy[copy.Length - 1] - new MyVector3(target.position.x, target.position.y, target.position.z)).magnitude < treshold_condition; if (!done) { float targetRootDist = MyVector3.Distance(copy[0], new MyVector3(target.position.x, target.position.y, target.position.z)); // Update joint positions if (targetRootDist > distances.Sum()) { // The target is unreachable for (int i = 0; i < copy.Length - 1; i++) { float r = (new MyVector3(target.position.x, target.position.y, target.position.z) - copy[i]).magnitude; float lambda = distances[i] / r; copy[i + 1] = copy[i] * (1 - lambda) + new MyVector3(target.position.x, target.position.y, target.position.z) * lambda; } } else { MyVector3 b = copy[0]; float difA = (copy[copy.Length - 1] - new MyVector3(target.position.x, target.position.y, target.position.z)).magnitude; // The target is reachable while (difA > treshold_condition) { // STAGE 1: FORWARD REACHING copy[copy.Length - 1] = new MyVector3(target.position.x, target.position.y, target.position.z); for (int i = copy.Length - 2; i > 0; i--) { float r = (copy[i + 1] - copy[i]).magnitude; float lambda = distances[i] / r; copy[i] = copy[i + 1] * (1 - lambda) + copy[i] * lambda; } // STAGE 2: BACKWARD REACHING copy[0] = b; for (int i = 0; i < copy.Length - 1; i++) { float r = (copy[i + 1] - copy[i]).magnitude; float lambda = distances[i] / r; copy[i + 1] = copy[i] * (1 - lambda) + copy[i + 1] * lambda; } difA = (copy[copy.Length - 1] - new MyVector3(target.position.x, target.position.y, target.position.z)).magnitude; } } // Update original joint rotations for (int i = 0; i <= joints.Length - 2; i++) { MyQuaternion parentQuat = new MyQuaternion(joints[i + 1].rotation); MyQuaternion myQuat = new MyQuaternion(joints[i].rotation); MyVector3 A = new MyVector3(joints[i + 1].position - joints[i].position); MyVector3 B = copy[i + 1] - copy[i]; float cosa = MyVector3.Dot(MyVector3.Normalize(A), MyVector3.Normalize(B)); float sina = MyVector3.Cross(MyVector3.Normalize(A), MyVector3.Normalize(B)).magnitude; float alpha = Mathf.Atan2(sina, cosa) * Mathf.Rad2Deg; MyVector3 myAxis = MyVector3.Normalize(MyVector3.Cross(A, B)); //Vector3 axis = new Vector3(myAxis.x, myAxis.y, myAxis.z); myQuat = MyQuaternion.AngleAxis(alpha, ref myAxis); Quaternion quat = new Quaternion(myQuat.x, myQuat.y, myQuat.z, myQuat.w); joints[i].rotation = quat * joints[i].rotation; //joints[i].rotation = Quaternion.AngleAxis(alpha, axis) * joints[i].rotation; myQuat = new MyQuaternion(joints[i].rotation); float localAngle = MyQuaternion.Angle(parentQuat, myQuat); if (Mathf.Abs(localAngle) > maxRotation) { joints[i + 1].rotation = joints[i].rotation; } joints[i + 1].position = new Vector3(copy[i + 1].x, copy[i + 1].y, copy[i + 1].z); } } }
private void RayTracein3DPlane(List <MyVector3> points, MyVector3 curp, MyVector3 curdire, MyVector3 sectionPlaneNormal, out int norInsec, out int notNorInsec) { // Param double insecPs_Dist_theshold = 0.01; double insecP_DistBetweenRay_theshold = 20; MyVector3 cutNormal = sectionPlaneNormal.Cross(curdire).Normalize(); ray = new Line3(curp, cutNormal); norInsec = -1; // Normal side notNorInsec = -1; // Not Normal side double dist_left = double.MaxValue; double dist_right = double.MaxValue; for (int i = 0; i < points.Count; i++) { double dist_temp = ray.DistanceToLine(points[i]); if ((points[i] - curp).Dot(cutNormal) > 0) { // Normal side if (dist_left > dist_temp) { dist_left = dist_temp; norInsec = i; } } else { // Not Normal side if (dist_right > dist_temp) { dist_right = dist_temp; notNorInsec = i; } } } if (norInsec == -1) { norInsec = notNorInsec; System.Console.WriteLine("Warining: norInsec == -1"); return; } else if (notNorInsec == -1) { notNorInsec = norInsec; System.Console.WriteLine("Warining: notNorInsec == -1"); return; } else if (norInsec == -1 && notNorInsec == -1) { System.Console.WriteLine("Error: Ray Tracein3DPlane, no intersection points"); return; } if (MyVector3.Distance(points[norInsec], points[notNorInsec]) < insecPs_Dist_theshold) { // this two intersection is too close, so let them become same one.s System.Console.WriteLine("Warining: two intersection is too close"); norInsec = notNorInsec; return; } if (ray.DistanceToLine(points[norInsec]) > insecP_DistBetweenRay_theshold || ray.DistanceToLine(points[notNorInsec]) > insecP_DistBetweenRay_theshold) { System.Console.WriteLine("Warining: two intersection is too far"); // this two intersection is too far, so let them become same one.s norInsec = notNorInsec; return; } }
public void CylinderSnapping() { // Get Boundary2 if (boundaryPoints_2d == null) { boundaryPoints_2d = GetBoundaryPoints(mark); } List <MyVector2> boundary2 = ExtractOutline(edgeImage, boundaryPoints_2d); // Project 2D edge points //topCircle = new MyCircle(topCircle.Center, topCircle.Radius, -topCircle.Normal); MyVector3 normal = topCircle.Normal.Cross(this.camera.target).Cross(topCircle.Normal); MyPlane sectionPlane = new MyPlane(topCircle.Center, normal); boundary3 = Proj2dToPlane(sectionPlane, boundary2); topCircle = CiriFixTopCircle(topCircle, boundary3); // UpdateCircleNormal // foreach (var pbondary3 in pbondary3) // { // } // if (topCircle.Center) //{ //} // Algorithm Init Params double offset = topCircle.Radius / 50; cur_p = topCircle.Center - offset * topCircle.Normal; cur_dire = 1.0 * topCircle.Normal; MyVector3 cur_dire_new = new MyVector3(cur_dire); MyVector3 cur_p_new = new MyVector3(-1 * cur_p); Insection1 = new MyVector3(1, 1, 1); Insection2 = new MyVector3(0, 0, 0); int norInsec = -1; int notNorInsec = -1; MyVector3 tangential1 = new MyVector3(1, 1, 1); MyVector3 tangential2 = new MyVector3(1, 1, 1); List <MyCircle> CircleLists = new List <MyCircle>(); CircleLists.Add(topCircle); // Fix first circle int iter = 0; double r = double.MaxValue; System.Console.WriteLine(Insection1.Dot(tangential2)); System.Console.WriteLine(Math.Cos(2.0 / 3.0 * Math.PI)); int MaxInter = 1000; GeneratedCenters = new List <MyVector3>(); List <double> radius = new List <double>(); List <double> weights = new List <double>(); List <MyVector3> dires = new List <MyVector3>(); while (--MaxInter > 0) // { if (Insection1 == Insection2) // 交点一直保持相同 { System.Console.WriteLine("Warning: Insection is same!"); // 半径过小 break; } if (cur_dire.Dot(cur_dire_new) < 0) // 移动方向反向 { System.Console.WriteLine("Warning: Move Direction!"); break; } if (cur_p + offset * cur_dire == cur_p_new) // 中心点没有移动 { System.Console.WriteLine("Warning: Center not move!"); break; } RayTracein3DPlane(boundary3, cur_p_new, cur_dire_new.Cross(sectionPlane.Normal()), sectionPlane.Normal(), out norInsec, out notNorInsec); System.Console.WriteLine("{0} , {1}", MyVector3.Distance(boundary3[norInsec], cur_p_new), MyVector3.Distance(boundary3[notNorInsec], cur_p_new)); test1 = new Line3(boundary3[norInsec], cur_p_new - boundary3[norInsec]); test2 = new Line3(boundary3[notNorInsec], cur_p_new - boundary3[notNorInsec]); if (MyVector3.Distance(boundary3[norInsec], cur_p_new) < topCircle.Radius / 20 || // close to bottom MyVector3.Distance(boundary3[notNorInsec], cur_p_new) < topCircle.Radius / 20) { System.Console.WriteLine("Warning: Close to bottom!"); break; } if (tangential1.Dot(tangential2) < Math.Cos(2.0 / 3.0 * Math.PI)) //切线相向 { System.Console.WriteLine("Warning: tangential get oppsite direction!"); break; } if (r < 0.0001) { System.Console.WriteLine("Warning: Radius is too small!"); // 半径过小 break; } //if (MyVector3.Distance(cur_p, cur_p_new) ) //{ // System.Console.WriteLine("Warning: Radius is too small!"); // 半径过小 // break; //} if (iter != 0) { //offset = 1 / MyVector3.Distance(cur_p, cur_p_new) * 0.000001 + 0.5 * offset; offset = topCircle.Radius / 20; //System.Console.WriteLine("{0}", offset); cur_dire = cur_dire_new; cur_p = cur_p_new + offset * cur_dire; CircleLists.Add(new MyCircle(cur_p, r, cur_dire)); // Get Data for Fit double weight = Math.Abs(cur_dire_new.Dot(cur_dire)); GeneratedCenters.Add(cur_p_new); weights.Add(weight); radius.Add(r); dires.Add(cur_dire); } // Step1: Get IntersectionPoitn RayTracein3DPlane(boundary3, cur_p, cur_dire, sectionPlane.Normal(), out norInsec, out notNorInsec); // Step2 : Get Two Local Tangential Insection1 = boundary3[norInsec]; Insection2 = boundary3[notNorInsec]; tangential1 = GetLocalTangential(norInsec, boundary3, cur_dire); tangential2 = GetLocalTangential(notNorInsec, boundary3, cur_dire); // Visualization setdirecLine = new Line3(cur_p, cur_dire); setLine1 = new Line3(Insection1, tangential1); setLine2 = new Line3(Insection2, tangential2); // Step3 : Get New Cur Direction and Cur Point cur_dire_new = (tangential1 + tangential2) / 2; RayTracein3DPlane(boundary3, cur_p, cur_dire_new, sectionPlane.Normal(), out norInsec, out notNorInsec); cur_p_new = (boundary3[norInsec] + boundary3[notNorInsec]) / 2; r = 0.5 * MyVector3.Distance(boundary3[norInsec], boundary3[notNorInsec]); iter++; this.view.Refresh(); } // Fit centers and radius; GeneratedCenters = FittingCentersCurve(GeneratedCenters, weights); int inter = 1; while (inter-- > 0) { radius = FittRadius(radius); } // ReBuild Object CircleLists.Clear(); CircleLists.Add(topCircle); // Fix first circle for (int i = 0; i < GeneratedCenters.Count; i++) { CircleLists.Add(new MyCircle(GeneratedCenters[i], radius[i], dires[i])); } CurveCyliner = new SweepMesh(CircleLists); }
void Update() { if (target.position.y > 1.25f) { for (int i = 0; i < joints.Length; i++) { jointsPositions[i] = new MyVector3(joints[i].position.x, joints[i].position.y, joints[i].position.z); } targetPosition = new MyVector3(target.position.x, target.position.y, target.position.z); // Copy the joints positions to work with // and calculate all the distances for (int i = 0; i < copy.Length; i++) { copy[i] = jointsPositions[i]; if (i < distances.Length) { distances[i] = (jointsPositions[i + 1] - jointsPositions[i]).magnitude(); } } done = (targetPosition - jointsPositions[jointsPositions.Length - 1]).magnitude() < threshold_distance; if (!done) { float targetRootDist = MyVector3.Distance(copy[0], targetPosition); // Update joint positions if (targetRootDist > distances.Sum()) { // The target is unreachable for (int i = 0; i < copy.Length - 1; i++) { float dist = (targetPosition - copy[i]).magnitude(); float lam = distances[i] / dist; copy[i + 1] = (1 - lam) * copy[i] + lam * targetPosition; } } else { // The target is reachable iter = 0; while (!done /*|| iter < maxIter*/) { // STAGE 1: FORWARD REACHING copy[copy.Length - 1] = targetPosition; for (int i = copy.Length - 1; i > 0; i--) { MyVector3 temp = (copy[i - 1] - copy[i]).normalized(); temp = temp * distances[i - 1]; copy[i - 1] = temp + copy[i]; } // STAGE 2: BACKWARD REACHING copy[0] = jointsPositions[0]; for (int i = 0; i < copy.Length - 2; i++) { MyVector3 temp = (copy[i + 1] - copy[i]).normalized(); temp = temp * distances[i]; copy[i + 1] = temp + copy[i]; } done = (targetPosition - copy[copy.Length - 1]).magnitude() < threshold_distance; iter++; } } // Update original joint rotations for (int i = 0; i <= joints.Length - 2; i++) { MyVector3 a = jointsPositions[i + 1] - jointsPositions[i]; MyVector3 b = copy[i + 1] - copy[i]; MyVector3 axis = MyVector3.Cross(a, b).normalized(); float cosa = MyVector3.Dot(a, b) / (a.magnitude() * b.magnitude()); float sina = MyVector3.Cross(a.normalized(), b.normalized()).magnitude(); float angle = Mathf.Atan2(sina, cosa); MyQuaternion q = new MyQuaternion(Mathf.Cos(angle / 2), axis.x * Mathf.Sin(angle / 2), axis.y * Mathf.Sin(angle / 2), axis.z * Mathf.Sin(angle / 2)); MyQuaternion actualRot = new MyQuaternion(joints[i].rotation.w, joints[i].rotation.x, joints[i].rotation.y, joints[i].rotation.z); MyQuaternion newRot = MyQuaternion.multiply(q, actualRot); jointsPositions[i] = copy[i]; joints[i].position = new Vector3(jointsPositions[i].x, jointsPositions[i].y, jointsPositions[i].z); joints[i].rotation = new Quaternion(newRot.x, newRot.y, newRot.z, newRot.w); for (int j = i; j < joints.Length; j++) //Actualizar posiciones de los hijos despues de rotar { jointsPositions[j] = new MyVector3(joints[j].position.x, joints[j].position.y, joints[j].position.z); } } } } }
void Update() { // 1.Copy the joints positions to work with. //Calculate also the distances between joints //TODO1 //Debug.Log(joints[3].transform.position.x); newPosition = target.transform.position; //novaDiferencia = Vector3.Distance(target.transform.position, terra.transform.position); novaDiferencia = MyVector3.Distance(target.transform.position, terra.transform.position); if (novaDiferencia > diferencia) { novaDiferencia = diferencia; stop = true; } else { stop = false; } copy[0] = joints[0].position; for (int i = 0; i < copy.Length - 1; i++) { copy[i + 1] = joints[i + 1].position; //distances[i] = Vector3.Distance(copy[i + 1], copy[i]); distances[i] = MyVector3.Distance(copy[i + 1], copy[i]); } //done = TODO2 done = (copy[copy.Length - 1] - target.position).magnitude < treshold_condition; if (!done) { //float targetRootDist = Vector3.Distance(copy[0], target.position); float targetRootDist = MyVector3.Distance(copy[0], target.position); // Update joint positions if (targetRootDist > distances.Sum()) { // The target is unreachable //TODO3 for (int i = 0; i < copy.Length - 1; i++) { //float r = Vector3.Distance(target.position, copy[i]); float r = MyVector3.Distance(target.position, copy[i]); float lambda = distances[i] / r; copy[i + 1] = (1 - lambda) * copy[i] + (lambda * target.position); } } else { //float comvulguis = Vector3.Distance(target.position, copy[copy.Length - 1]); float comvulguis = MyVector3.Distance(target.position, copy[copy.Length - 1]); //Vector3 b = copy[0]; MyVector3 b = copy[0]; int iter = 0; // The target is reachable while (comvulguis > treshold_condition && iter < maxIterations) { iter++; // STAGE 1: FORWARD REACHING //TODO //Debug.Log("FABRIK iteration:" + iter); copy[copy.Length - 1] = target.position; for (int i = copy.Length - 2; i > 0; i--) { //float r = Vector3.Distance(copy[i + 1], copy[i]); float r = MyVector3.Distance(copy[i + 1], copy[i]); float lambda = distances[i] / r; copy[i] = (1 - lambda) * copy[i + 1] + (lambda * copy[i]); } // STAGE 2: BACKWARD REACHING //TODO copy[0] = b; for (int i = 0; i < copy.Length - 1; i++) { //float r = Vector3.Distance(copy[i + 1], copy[i]); float r = MyVector3.Distance(copy[i + 1], copy[i]); float lambda = distances[i] / r; copy[i + 1] = (1 - lambda) * copy[i] + (lambda * copy[i + 1]); } //comvulguis = Vector3.Distance(copy[copy.Length - 1], target.position); comvulguis = MyVector3.Distance(copy[copy.Length - 1], target.position); } } // Update original joint rotations for (int i = 0; i <= joints.Length - 2; i++) { //TODO4 //without rotations of the different pieces: //joints[i + 1].position = copy[i + 1]; //with rotations of the different pieces: //Vector3 init = joints[i + 1].position - joints[i].position; //Vector3 now = copy[i + 1] - copy[i]; MyVector3 init = joints[i + 1].position - joints[i].position; MyVector3 now = copy[i + 1] - copy[i]; init.Normalize(); now.Normalize(); //float angle = Mathf.Acos(Vector3.Dot(init.normalized, now.normalized)) * Mathf.Rad2Deg; float cosa = Vector3.Dot(init, now); float sina = Vector3.Cross(init, now).magnitude; //init.Normalize(); //now.Normalize(); //float cosa = MyVector3.DotProduct(init, now); //float sina = MyVector3.CrossProduct(init, now).SquareMagnitude; float angle = Mathf.Atan2(sina, cosa) * Mathf.Rad2Deg; MyVector3 axis = Vector3.Cross(init, now).normalized; joints[i].rotation = Quaternion.AngleAxis(angle, axis) * joints[i].rotation; joints[i + 1].position = copy[i + 1]; } } }