public void GetXAxis(ref UVector xAxis)
{
xAxis.m_vecX = this.matrix[0, 0];
xAxis.m_vecY = this.matrix[1, 0];
xAxis.m_vecZ = this.matrix[2, 0];
xAxis.Norm();
}
public void TransformToZAxis(Point3d pt1, Vector3d axis1)
{
UVector pt = new UVector();
pt.X = pt1.X;
pt.Y = pt1.Y;
pt.Z = pt1.Z;
UVector axis = new UVector();
axis.X = axis1.X;
axis.Y = axis1.Y;
axis.Z = axis1.Z;
UVector vx = GetPendiAxis(axis);
UVector vy = axis ^ vx;
Matrix4 mat2 = new Matrix4();
mat2.SetRow(0, vx);
mat2.SetRow(1, vy);
mat2.SetRow(2, axis);
mat2.SetRow(3, pt);
/*mat2.SetCol(0, vx);
* mat2.SetCol(1, vy);
* mat2.SetCol(2, axis);
* mat2.SetRow(3, pt);*/
PostMultiply(mat2.GetOrthInvers());
}
public void GetYAxis(ref UVector yAxis)
{
yAxis.m_vecX = this.matrix[0, 1];
yAxis.m_vecY = this.matrix[1, 1];
yAxis.m_vecZ = this.matrix[2, 1];
yAxis.Norm();
}
public UVector GetPendiAxis(UVector vec)
{
double dx = Math.Abs(vec.m_vecX);
double dy = Math.Abs(vec.m_vecY);
double dz = Math.Abs(vec.m_vecZ);
if (dz < dx)
{
if (dz < dy)
{
return(new UVector(-vec.m_vecY, vec.m_vecX, 0.0));
}
else
{
return(new UVector(vec.m_vecZ, 0.0, -vec.m_vecX));
}
}
else
{
if (dx < dy)
{
return(new UVector(0.0, -vec.m_vecZ, vec.m_vecY));
}
else
{
return(new UVector(vec.m_vecZ, 0.0, -vec.m_vecX));
}
}
}
public void TransformToZAxis(Point3d pt1, Vector3d vx1, Vector3d vy1)
{
UVector pt = new UVector();
pt.X = pt1.X;
pt.Y = pt1.Y;
pt.Z = pt1.Z;
UVector vx = new UVector();
vx.X = vx1.X;
vx.Y = vx1.Y;
vx.Z = vx1.Z;
UVector vy = new UVector();
vy.X = vy1.X;
vy.Y = vy1.Y;
vy.Z = vy1.Z;
Matrix4 mat2 = new Matrix4();
mat2.SetRow(0, vx);
mat2.SetRow(1, vy);
mat2.SetRow(2, vx ^ vy);
mat2.SetRow(3, pt);
/*mat2.SetCol(0, vx);
* mat2.SetCol(1, vy);
* mat2.SetCol(2, axis);
* mat2.SetRow(3, pt);*/
PostMultiply(mat2.GetOrthInvers());
}
public void GetZAxis(ref UVector zAxis)
{
zAxis.m_vecX = this.matrix[0, 0];
zAxis.m_vecY = this.matrix[1, 0];
zAxis.m_vecZ = this.matrix[2, 0];
zAxis.Norm();
}
public static UVector ToUVector(Vector3d vec)
{
UVector uvec = new UVector();
uvec.X = vec.X;
uvec.Y = vec.Y;
uvec.Z = vec.Z;
return(uvec);
}
public static UVector ToUVector(Point3d pos)
{
UVector uvec = new UVector();
uvec.X = pos.X;
uvec.Y = pos.Y;
uvec.Z = pos.Z;
return(uvec);
}
public void ApplyVec(ref UVector vec)
{
double x = matrix[0, 0] * vec.m_vecX + matrix[1, 0] * vec.m_vecY + matrix[2, 0] * vec.m_vecZ;
double y = matrix[0, 1] * vec.m_vecX + matrix[1, 1] * vec.m_vecY + matrix[2, 1] * vec.m_vecZ;
double z = matrix[0, 2] * vec.m_vecX + matrix[1, 2] * vec.m_vecY + matrix[2, 2] * vec.m_vecZ;
vec.m_vecX = x;
vec.m_vecY = y;
vec.m_vecZ = z;
}
public void ApplyAbsPos(ref UVector pt)
{
double x = matrix[0, 0] * pt.m_vecX + matrix[1, 0] * pt.m_vecY + matrix[2, 0] * pt.m_vecZ + matrix[3, 0];
double y = matrix[0, 1] * pt.m_vecX + matrix[1, 1] * pt.m_vecY + matrix[2, 1] * pt.m_vecZ + matrix[3, 1];
double z = matrix[0, 2] * pt.m_vecX + matrix[1, 2] * pt.m_vecY + matrix[2, 2] * pt.m_vecZ + matrix[3, 2];
pt.m_vecX = Math.Abs(x);
pt.m_vecY = Math.Abs(y);
pt.m_vecZ = Math.Abs(z);
}
public void ApplyPos(ref UVector pt)
{
double x = matrix[0, 0] * pt.m_vecX + matrix[1, 0] * pt.m_vecY + matrix[2, 0] * pt.m_vecZ + matrix[3, 0];
double y = matrix[0, 1] * pt.m_vecX + matrix[1, 1] * pt.m_vecY + matrix[2, 1] * pt.m_vecZ + matrix[3, 1];
double z = matrix[0, 2] * pt.m_vecX + matrix[1, 2] * pt.m_vecY + matrix[2, 2] * pt.m_vecZ + matrix[3, 2];
pt.m_vecX = x;
pt.m_vecY = y;
pt.m_vecZ = z;
}
public void ApplyAbsVec(ref UVector vec)
{
double x = Math.Abs(matrix[0, 0] * vec.m_vecX) + Math.Abs(matrix[1, 0] * vec.m_vecY) + Math.Abs(matrix[2, 0] * vec.m_vecZ);
double y = Math.Abs(matrix[0, 1] * vec.m_vecX) + Math.Abs(matrix[1, 1] * vec.m_vecY) + Math.Abs(matrix[2, 1] * vec.m_vecZ);
double z = Math.Abs(matrix[0, 2] * vec.m_vecX) + Math.Abs(matrix[1, 2] * vec.m_vecY) + Math.Abs(matrix[2, 2] * vec.m_vecZ);
vec.m_vecX = x;
vec.m_vecY = y;
vec.m_vecZ = z;
}
public void TransformToZAxis(UVector pt, UVector vx, UVector vy)
{
Matrix4 mat2 = new Matrix4();
mat2.SetRow(0, vx);
mat2.SetRow(1, vy);
mat2.SetRow(2, vx ^ vy);
mat2.SetRow(3, pt);
/*mat2.SetCol(0, vx);
* mat2.SetCol(1, vy);
* mat2.SetCol(2, axis);
* mat2.SetRow(3, pt);*/
PostMultiply(mat2.GetOrthInvers());
}
public void TransformToZAxis(UVector pt, UVector axis)
{
UVector vx = GetPendiAxis(axis);
UVector vy = axis ^ vx;
Matrix4 mat2 = new Matrix4();
mat2.SetRow(0, vx);
mat2.SetRow(1, vy);
mat2.SetRow(2, axis);
mat2.SetRow(3, pt);
/*mat2.SetCol(0, vx);
* mat2.SetCol(1, vy);
* mat2.SetCol(2, axis);
* mat2.SetRow(3, pt);*/
PostMultiply(mat2.GetOrthInvers());
}
public static Matrix4 CalRelativeMatrix(UVector vx1, UVector vy1, UVector vz1, UVector origin1,
UVector vx2, UVector vy2, UVector vz2, UVector origin2)
{
Matrix4 mat1 = new Matrix4();
mat1.SetRow(0, vx1);
mat1.SetRow(1, vy1);
mat1.SetRow(2, vz1);
mat1.SetRow(3, origin1);
Matrix4 mat2 = new Matrix4();
mat2.SetRow(0, vx2);
mat2.SetRow(1, vy2);
mat2.SetRow(2, vz2);
mat2.SetRow(3, origin2);
return(mat1 * mat2.GetOrthInvers());
}
public UVector PreProduct(UVector vec)
{
return(new UVector(vec.m_vecY * m_vecZ - vec.m_vecZ * m_vecY, vec.m_vecZ * m_vecX - vec.m_vecX * m_vecZ, vec.m_vecX * m_vecY - vec.m_vecY * m_vecX));
}
public UVector PreSubstract(UVector vec)
{
return(new UVector(vec.m_vecX - m_vecX, vec.m_vecY - m_vecY, vec.m_vecZ - m_vecZ));
}
public UVector PostSubstract(UVector vec)
{
return(new UVector(m_vecX - vec.m_vecX, m_vecY - vec.m_vecY, m_vecZ - vec.m_vecZ));
}
public UVector Add(UVector vec)
{
return(new UVector(m_vecX + vec.m_vecX, m_vecY + vec.m_vecY, m_vecZ + vec.m_vecZ));
}
public static double Angle(UVector vec1, UVector vec2)
{
return(Math.Acos(vec1 * vec2 / (Math.Sqrt(vec1 * vec1) * Math.Sqrt(vec2 * vec2))));
}
public UVector(UVector vec)
{
m_vecX = vec.m_vecX;
m_vecY = vec.m_vecY;
m_vecZ = vec.m_vecZ;
}
public UVector PostProduct(UVector vec)
{
return(new UVector(m_vecY * vec.m_vecZ - m_vecZ * vec.m_vecY, m_vecZ * vec.m_vecX - m_vecX * vec.m_vecZ, m_vecX * vec.m_vecY - m_vecY * vec.m_vecX));
}
public static double Dis(UVector v1, UVector v2)
{
return(Math.Sqrt((v2.m_vecX - v1.m_vecX) * (v2.m_vecX - v1.m_vecX) + (v2.m_vecY - v1.m_vecY) * (v2.m_vecY - v1.m_vecY) + (v2.m_vecZ - v1.m_vecZ) * (v2.m_vecZ - v1.m_vecZ)));
}
public void SetRow(int index, UVector vec)
{
matrix[index, 0] = vec.m_vecX; matrix[index, 1] = vec.m_vecY; matrix[index, 2] = vec.m_vecZ;
}
public double Dot(UVector vec)
{
return(vec.m_vecX * m_vecX + vec.m_vecY * m_vecY + vec.m_vecZ * m_vecZ);
}
public void SetCol(int index, UVector vec)
{
matrix[0, index] = vec.m_vecX; matrix[1, index] = vec.m_vecY; matrix[2, index] = vec.m_vecZ;
}
public static double Space(UVector vec1, UVector vec2)
{
return(Math.Sqrt((vec1 - vec2) * (vec1 - vec2)));
}
public void Translate(UVector pos)
{
matrix[3, 0] += pos.m_vecX;
matrix[3, 1] += pos.m_vecY;
matrix[3, 2] += pos.m_vecZ;
}