public float maxStretch = 0.3f; //maximo de lo que se puede estirar un segmento
void Start()
{
//inicializar el line renderer
lineRenderer = GetComponent <LineRenderer>();
//recibir las posiciones de los 2 extremos de la cuerda
Vector3Class initPos = new Vector3Class(pos1.transform.position);
Vector3Class endPos = new Vector3Class(pos2.transform.position);
//generador de los puntos de la cuerda, en resumen hace un vector entre pos1,pos2
Vector3Class stringVector = endPos - initPos;
stringVector = stringVector.Normalize(stringVector);
List <Vector3Class> ropePositions = new List <Vector3Class>();
//determina el punto inicial de todos los segmentos con
//"largo segmento * posicion en lista * direccion normalizadapos1pos2 + pos1"
for (int i = 0; i < stringPartitions; i++)
{
Vector3Class partPos = initPos;
partPos += stringVector * i * stringPartLength;
ropePositions.Add(partPos);
}
//se añaden todos los segmentos con la posicion deseada
//se introducen los elementos al reves para facilitar algunos calculos
for (int i = ropePositions.Count - 1; i >= 0; i--)
{
ropeSegments.Add(new CilinderBody(ropePositions[i]));
}
//actualiza posicion del objeto bola
pos2.position = ropeSegments[0].pos.GetValues();
}
private void CalculateAccelerations()
{
//calculamos la fuerza de cada muelle y las guardamos en una lista para luego adjudicarla a los segmentos
List <Vector3Class> springForces = new List <Vector3Class>();
for (int i = 0; i < ropeSegments.Count - 1; i++)
{
Vector3Class p1p2 = ropeSegments[i + 1].pos - ropeSegments[i].pos;
//elasticity part
float part1 = kElasticity * (p1p2.Size() - stringPartLength);
//Damping part
float part2 = kDamping * ((p1p2.DotProduct(ropeSegments[i + 1].speed - ropeSegments[i].speed, p1p2.Normalize(p1p2))));
//calculamos total del muelle juntando las 2 partes
Vector3Class resultForce = p1p2.Normalize(p1p2);
resultForce *= (part1 + part2) * -1;
//al estar el orden de pos2 a pos1, este force es el negativo (f2)
resultForce *= -1;
springForces.Add(resultForce);
}
for (int i = 0; i < ropeSegments.Count - 1; i++)
{
Vector3Class springForce = new Vector3Class();
//spring que corresponde a F1 de la formula
if (i != 0)
{
springForce -= springForces[i - 1];
}
//spring que corresponde a F2 de la formula
springForce += springForces[i];
//masa del segmento de la cuerda, hacemos variable a parte para añadir el peso de la bola al ultimo
float springMass = massRopeSegment;
//segmento con la bola
if (i == 0)
{
springMass = massLastRopeSegment;
}
//creamos fuerza de gravedad con la masa
Vector3Class gravityForce = new Vector3Class(0f, gravity, 0f);
gravityForce *= springMass;
//sumamos ambas fuerzas calculadas
Vector3Class totalForce = springForce + gravityForce;
//finalmente, calculamos la aceleracion resultante de la fuerza
Vector3Class acceleration = totalForce / springMass;
CilinderBody temp = ropeSegments[i];
temp.acc = acceleration;
ropeSegments[i] = temp;
}
}
private void ImplementMaximumStretch()
{
//loop por todos los puntos para ajustarlos, de arriba a abajo
for (int i = ropeSegments.Count - 1; i > 0; i--)
{
//recibimos 2 segmentos para comparar
Vector3Class topSegment = ropeSegments[i].pos;
CilinderBody bottomSegment = ropeSegments[i - 1]; //este es CilinderBody porque modificaremos su pos si hace falta
//distancia entre ambos segmentos
float dist = (topSegment - bottomSegment.pos).Size();
//si esta distancia supera el maximo
if (dist > maxStretch)
{
//calculamos la diferencia entre la distancia actual y el maximo fijado
float diffenceLength = dist - maxStretch;
//vector normalizado para establecer la direccion del vector para ajustar el segmento
Vector3Class adjustDirection = topSegment.Normalize(topSegment - bottomSegment.pos);
//vector resultante para ajustar al segmento
Vector3Class adjustVec = adjustDirection * diffenceLength;
//sumamos vector resultante con posicion de bottomsegment y actualizamos pos del segmento
bottomSegment.pos += adjustVec;
ropeSegments[i - 1] = bottomSegment;
}
else if (dist < minStretch)
{
//calculamos la diferencia entre la distancia actual y el maximo fijado
float differenceLength = minStretch - dist;
//vector normalizado para establecer la direccion del vector para ajustar el segmento
Vector3Class adjustDirection = topSegment.Normalize(bottomSegment.pos - topSegment);
//vector resultante para ajustar al segmento
Vector3Class adjustVec = adjustDirection * differenceLength;
//sumamos vector resultante con posicion de bottomsegment y actualizamos pos del segmento
bottomSegment.pos += adjustVec;
ropeSegments[i - 1] = bottomSegment;
}
}
}
public override bool Draw(out IModelPoint mp, out IModel fmodel, out IModel smodel)
{
int index = -1;
IDrawPrimitive primitive = null;
IDoubleArray vArray = null;
IUInt16Array indexArray = null;
IFloatArray array3 = null;
IDoubleArray norms = null;
base.Draw(out mp, out fmodel, out smodel);
try
{
object renderInfo = (base._renderType == RenderType.Texture) ? ((object)base._tcNames) : ((object)base._colors);
int[] numArray = new int[2];
numArray[1] = 1;
if (!base.NewEmptyModel(numArray, base._renderType, renderInfo, out fmodel))
{
return(false);
}
//绘制底部
primitive = fmodel.GetGroup(0).GetPrimitive(0);
if (DrawGeometry.ConvertPolygon(this._vtx2, 0.0, 0, out vArray, out indexArray, out array3, out norms))
{
for (index = 0; index < vArray.Length; index++)
{
primitive.VertexArray.Append((float)vArray.Array[index]);
}
for (index = 0; index < indexArray.Length; index++)
{
primitive.IndexArray.Append(indexArray.Array[index]);
}
for (index = 0; index < (vArray.Length / 3); index++)
{
float num2 = (float)vArray.Array[index * 3];
primitive.TexcoordArray.Append(num2);
num2 = (float)vArray.Array[(index * 3) + 1];
primitive.TexcoordArray.Append(num2);
}
for (index = 0; index < norms.Length; index++)
{
primitive.NormalArray.Append((float)norms.Array[index]);
}
}
double num3 = 0.0;
int num4 = base._vtx.Length / 2;
List <ushort> list = new List <ushort>();
primitive = fmodel.GetGroup(0).GetPrimitive(1);
for (index = 0; index < num4; index++)
{
primitive.VertexArray.Append((float)base._vtx[index * 2]);
primitive.VertexArray.Append((float)base._vtx[(index * 2) + 1]);
primitive.VertexArray.Append((float)(this._terrainLine[index] - base._hBottom));
list.Add((ushort)((primitive.VertexArray.Length / 3) - 1));
primitive.VertexArray.Append((float)this._vtx2[index * 2]);
primitive.VertexArray.Append((float)this._vtx2[(index * 2) + 1]);
primitive.VertexArray.Append(0f);
list.Add((ushort)((primitive.VertexArray.Length / 3) - 1));
num3 = (index == 0) ? num3 : (num3 += this._segLenth[index - 1]);
primitive.TexcoordArray.Append((float)num3);
primitive.TexcoordArray.Append((float)(this._terrainLine[index] - base._hBottom));
primitive.TexcoordArray.Append((float)num3);
primitive.TexcoordArray.Append(0f);
IVector3 vector = new Vector3Class
{
X = base._vtx[index * 2],
Y = base._vtx[(index * 2) + 1],
Z = 0.0
};
vector.Normalize();
primitive.NormalArray.Append((float)vector.X);
primitive.NormalArray.Append((float)vector.Y);
primitive.NormalArray.Append((float)vector.Z);
//工程开挖挖洞效果贴图模型出错,添加法向量数组,使其与顶点数组数量一致
primitive.NormalArray.Append((float)vector.X);
primitive.NormalArray.Append((float)vector.Y);
primitive.NormalArray.Append((float)vector.Z);
}
for (index = 0; index < (num4 - 1); index++)
{
primitive.IndexArray.Append(list[index * 2]);
primitive.IndexArray.Append(list[(index * 2) + 1]);
primitive.IndexArray.Append(list[((index + 1) * 2) + 1]);
primitive.IndexArray.Append(list[index * 2]);
primitive.IndexArray.Append(list[((index + 1) * 2) + 1]);
primitive.IndexArray.Append(list[(index + 1) * 2]);
}
return(true);
}
catch (Exception exception)
{
return(false);
}
}
internal void CalcPolyNode(SupportProfile selectedMaterialProfile, AtumPrinter selectedPrinter, float sliceHeight, int objectIndex)
{
this.PolyTree = new PolyTree();
var polygonPoints = new List <IntPoint>();
foreach (var polyLinePart in this)
{
var tStart = (polyLinePart.P1 - new Vector3Class(selectedPrinter.ProjectorResolutionX / 2, selectedPrinter.ProjectorResolutionY / 2, 0)) / 10f;
tStart -= new Vector3Class(0.1f, 0.1f, 0);
DebugPoints.Add(tStart);
polygonPoints.Add(new IntPoint(tStart + new Vector3Class(0, 0, sliceHeight)));
}
//add the end of the line
var tEnd = (this.Last().P2 - new Vector3Class(selectedPrinter.ProjectorResolutionX / 2, selectedPrinter.ProjectorResolutionY / 2, 0)) / 10f;
tEnd -= new Vector3Class(0.1f, 0.1f, 0);
polygonPoints.Add(new IntPoint(tEnd));
//when not closed use normals to create offset points
if (!ClosedLine)
{
this.Reverse();
// this.Reverse();
var normalVector = new Vector3Class();
foreach (var polyLinePart in this)
{
normalVector += polyLinePart.Normal;
normalVector.Z = 0;
normalVector.Normalize();
normalVector *= selectedMaterialProfile.SupportOverhangDistance / 2;
//do point offset
var tPoint = normalVector + ((polyLinePart.P1 - new Vector3Class(selectedPrinter.ProjectorResolutionX / 2, selectedPrinter.ProjectorResolutionY / 2, 0)) / 10f);
tPoint -= new Vector3Class(0.1f, 0.1f, 0);
DebugPoints.Add(tPoint + new Vector3Class(0, 0, sliceHeight));
polygonPoints.Add(new IntPoint(tPoint));
normalVector = polyLinePart.Normal;
}
var polyTreeOffset = new PolyTree();
var polyNode = new PolyNode();
polyNode.m_polygon = polygonPoints;
polyTreeOffset._allPolys.Add(polyNode);
var c = new Clipper();
c.AddPath(polyTreeOffset._allPolys[0].Contour, PolyType.ptClip, true);
c.AddPath(new List <IntPoint>(), PolyType.ptSubject, true);
c.Execute(ClipType.ctXor, this.PolyTree, PolyFillType.pftEvenOdd, PolyFillType.pftEvenOdd);
//if (sliceHeight >=13 && sliceHeight <=15f)
// TriangleHelper.SavePolyNodesContourToPng(this.PolyTree._allPolys, sliceHeight.ToString("0.00") + "-" + objectIndex.ToString() + "-t2");
}
else
{
var polyTreeOffset = new PolyTree();
var polyNode = new PolyNode();
polyNode.m_polygon = polygonPoints;
polyTreeOffset._allPolys.Add(polyNode);
polyTreeOffset = MagsAIEngine.ClipperOffset(polyTreeOffset, selectedMaterialProfile.SupportOverhangDistance / 2);
if (polyTreeOffset._allPolys.Count > 0)
{
var c = new Clipper();
c.AddPath(polygonPoints, PolyType.ptClip, true);
c.AddPath(polyTreeOffset._allPolys[0].Contour, PolyType.ptClip, true);
c.AddPath(new List <IntPoint>(), PolyType.ptSubject, true);
c.Execute(ClipType.ctXor, this.PolyTree, PolyFillType.pftEvenOdd, PolyFillType.pftEvenOdd);
}
}
}
// Update is called once per frame
void Update()
{
target = new Vector3Class(Destination.transform.position);
if (this.tag == "offset")
{
//Algunos ajustes para que la mano se posicione relativamente del target con mas realismo
Vector3Class P = new Vector3Class(BaseJoint.transform.position); //Joints[0].transform.position ??
Vector3Class Q = new Vector3Class(Destination.transform.position);
//Normalizar distancia de Q(Pendulo) a P(Effector) y multiplicar por el radio de la bola del pendulo + offset adicional de ajuste para obtener el offset del target total:
Vector3Class offsetTarget = Q - P;
//Debug.Log("x" + offsetTarget.x + "y" + offsetTarget.y + "z" + offsetTarget.z);
float totalOffset = 0.3f; //<-Poner aqui un offset adicional... En realidad sería la mitad del grosor de la mano si el endefector HAND estuviese justo a la mitad.
offsetTarget = offsetTarget.Normalize(offsetTarget) * (offsetTarget.Size() - totalOffset);
offsetTarget += P; //Actualiza posicion del objetivo
offsetTarget.y = target.y;
target = offsetTarget;
}
//ApproachTarget(target);
//ForwardKinematics(Solution);
if (perfFollow && tag == "arm")
{
int counterWhile = 0;
while (counterWhile < 20)
{
counterWhile++;
if (ErrorFunction(target, Solution) > StopThreshold)
{
ApproachTarget(target);
for (int i = 0; i < Joints.Length; i++)
{
Joints[i].MoveArm(Solution[i]);
}
}
else
{
break;
}
}
}
else
{
if (ErrorFunction(target, Solution) > StopThreshold)
{
ApproachTarget(target);
for (int i = 0; i < Joints.Length; i++)
{
Joints[i].MoveArm(Solution[i]);
}
}
}
if (DebugDraw)
{
Debug.DrawLine(Effector.transform.position, target.GetValues(), Color.green);
Debug.DrawLine(Destination.transform.position, target.GetValues(), new Color(0, 0.5f, 0));
}
}
