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