public void RotateYPR(double yaw, double pitch, double roll)
{
var x = new PVector(1, 0, 0);
var y = new PVector(0, 1, 0);
var z = new PVector(0, 0, 1);
Rotate3D(z, yaw);
x.Rotate3D(z, yaw);
y.Rotate3D(z, yaw);
Rotate3D(x, pitch);
y.Rotate3D(x, pitch);
Rotate3D(y, roll);
}
public static PVector Rotate3D(PVector vector, PVector axis, double theta)
{
var vec = axis.CopyVector();
vec.Normalize();
double scalar_1 = (1 - Math.Cos(theta)) * (Dot(vector, vec));
double scalar_2 = Math.Cos(theta);
double scalar_3 = Math.Sin(theta);
PVector norm = vec.CopyVector();
PVector tv = vector.CopyVector();
PVector crs = Cross(norm, tv);
norm.Mult(scalar_1);
tv.Mult(scalar_2);
crs.Mult(scalar_3);
var retVal = norm.CopyVector();
retVal.Add(tv);
retVal.Add(crs);
return(retVal);
}
public static PCurve GetNodeCurve(PVector pt1, PVector pt2)
{
var p1 = pt1.CopyVector();
p1.Z = 0;
var p2 = pt2.CopyVector();
p2.Z = 0;
var wdif = Math.Abs(p2.X - p1.X) / 2;
if (wdif < 50)
{
wdif = 50;
}
PVector p1A = new PVector(p1.X + wdif, p1.Y);
PVector p2A = new PVector(p2.X - wdif, p2.Y);
var ls = new List <PVector>()
{
p1, p1A, p2A, p2
};
return(new PCurve(ls));
}
public double Dot(PVector v)
{
return(X * v.X + Y * v.Y + Z * v.Z);
}
public void Sub(PVector v2)
{
X -= v2.X;
Y -= v2.Y;
Z -= v2.Z;
}
public void Add(PVector v2)
{
X += v2.X;
Y += v2.Y;
Z += v2.Z;
}
public static PVector operator -(PVector v1, PVector v2)
{
return(PVector.Sub(v1, v2));
}
public static PVector operator +(PVector v1, PVector v2)
{
return(PVector.Add(v1, v2));
}
public static double Dot(PVector a, PVector b)
{
return(a.Dot(b));
}
public static PVector Cross(PVector a, PVector b)
{
return(a.Cross(b));
}
