/**
* 四元数の積を求める
* q * r
*
*/
public static CadQuaternion Product(CadQuaternion q, CadQuaternion r)
{
// A = (a; U)
// B = (b; V)
// AB = (ab - U・V; aV + bU + U×V)
CadQuaternion ans;
double d1, d2, d3, d4;
d1 = q.t * r.t;
d2 = q.x * r.x;
d3 = q.y * r.y;
d4 = q.z * r.z;
ans.t = d1 - d2 - d3 - d4;
d1 = q.t * r.x;
d2 = r.t * q.x;
d3 = q.y * r.z;
d4 = -q.z * r.y;
ans.x = d1 + d2 + d3 + d4;
d1 = q.t * r.y;
d2 = r.t * q.y;
d3 = q.z * r.x;
d4 = -q.x * r.z;
ans.y = d1 + d2 + d3 + d4;
d1 = q.t * r.z;
d2 = r.t * q.z;
d3 = q.x * r.y;
d4 = -q.y * r.x;
ans.z = d1 + d2 + d3 + d4;
return(ans);
}
/**
* Vector (vx, vy, vz)を回転軸としてradianだけ回転する四元数を作成
*
*/
public static CadQuaternion RotateQuaternion(double vx, double vy, double vz, double radian)
{
CadQuaternion ans = default(CadQuaternion);
double norm;
double c, s;
norm = vx * vx + vy * vy + vz * vz;
if (norm <= 0.0)
{
return(ans);
}
norm = 1.0 / Math.Sqrt(norm);
vx *= norm;
vy *= norm;
vz *= norm;
c = Math.Cos(0.5 * radian);
s = Math.Sin(0.5 * radian);
ans.t = c;
ans.x = s * vx;
ans.y = s * vy;
ans.z = s * vz;
return(ans);
}
public static void RotateFigure(CadFigure fig, Vector3d org, Vector3d axis, double t)
{
CadQuaternion q = CadQuaternion.RotateQuaternion(axis, t);
CadQuaternion r = q.Conjugate();;
fig.Rotate(org, q, r);
}
public void RotateEyePoint(Vector2 prev, Vector2 current)
{
Vector2 d = current - prev;
double ry = (d.X / 10.0) * (Math.PI / 20);
double rx = (d.Y / 10.0) * (Math.PI / 20);
CadQuaternion q;
CadQuaternion r;
CadQuaternion qp;
q = CadQuaternion.RotateQuaternion(Vector3d.UnitY, ry);
r = q.Conjugate();
qp = CadQuaternion.FromVector(mEye);
qp = r * qp;
qp = qp * q;
mEye = qp.ToVector3d();
qp = CadQuaternion.FromVector(mUpVector);
qp = r * qp;
qp = qp * q;
mUpVector = qp.ToVector3d();
Vector3d ev = mLookAt - mEye;
Vector3d a = new Vector3d(ev);
Vector3d b = new Vector3d(mUpVector);
Vector3d axis = CadMath.Normal(a, b);
if (!axis.IsZero())
{
q = CadQuaternion.RotateQuaternion(axis, rx);
r = q.Conjugate();
qp = CadQuaternion.FromVector(mEye);
qp = r * qp;
qp = qp * q;
mEye = qp.ToVector3d();
qp = CadQuaternion.FromVector(mUpVector);
qp = r * qp;
qp = qp * q;
mUpVector = qp.ToVector3d();
}
CalcViewMatrix();
CalcViewDir();
CalcProjectionZW();
}
/**
* 共役四元数を返す
*
*
*/
public CadQuaternion Conjugate()
{
CadQuaternion q = this;
q.t = t;
q.x = -x;
q.y = -y;
q.z = -z;
return(q);
}
public void PanCamera(DrawContext dc, Vector2 prev, Vector2 current)
{
Vector2 d = current - prev;
double rx = d.X * (Math.PI / 1000);
double ry = d.Y * (Math.PI / 1000);
CadQuaternion q;
CadQuaternion r;
CadQuaternion qp;
Vector3d lookv = dc.LookAt - dc.Eye;
Vector3d upv = dc.UpVector;
q = CadQuaternion.RotateQuaternion(upv, rx);
r = q.Conjugate();
qp = CadQuaternion.FromVector(lookv);
qp = r * qp;
qp = qp * q;
lookv = qp.ToVector3d();
Vector3d ev = dc.LookAt - dc.Eye;
Vector3d a = new Vector3d(ev);
Vector3d b = new Vector3d(upv);
Vector3d axis = CadMath.Normal(a, b);
if (!axis.IsZero())
{
q = CadQuaternion.RotateQuaternion(axis, ry);
r = q.Conjugate();
qp = CadQuaternion.FromVector(lookv);
qp = r * qp;
qp = qp * q;
lookv = qp.ToVector3d();
qp = CadQuaternion.FromVector(upv);
qp = r * qp;
qp = qp * q;
upv = qp.ToVector3d();
}
dc.SetCamera(dc.Eye, lookv + dc.Eye, upv);
}
public static void ForEachSegs(
CadVertex cp, CadVertex pa, CadVertex pb,
int splitCnt,
Action <CadVertex, CadVertex> action)
{
CadVertex va = pa - cp;
CadVertex vb = pb - cp;
if (va.Norm() < 0.01)
{
return;
}
double dt = (2.0 * Math.PI) / (double)splitCnt;
int div = splitCnt;
Vector3d normal = CadMath.Normal(va.vector, vb.vector);
CadQuaternion q = CadQuaternion.RotateQuaternion(normal, dt);
CadQuaternion r = q.Conjugate();
CadVertex p = va;
CadVertex tp1 = pa;
CadVertex tp2 = pa;
int i = 0;
for (; i < div - 1; i++)
{
CadQuaternion qp = CadQuaternion.FromPoint(p.vector);
qp = r * qp;
qp = qp * q;
p.vector = qp.ToPoint();
tp2 = p + cp;
action(tp1, tp2);
tp1 = tp2;
}
action(tp1, pa);
}
/**
* Vector (v.x, v.y, v.z)を回転軸としてradianだけ回転する四元数を作成
*
*/
public static CadQuaternion RotateQuaternion(Vector3d axis, double radian)
{
axis = axis.Normalized();
CadQuaternion ans = default;
double c, s;
c = Math.Cos(0.5 * radian);
s = Math.Sin(0.5 * radian);
ans.t = c;
ans.x = s * axis.X;
ans.y = s * axis.Y;
ans.z = s * axis.Z;
return(ans);
}
public static void Draw(
CadVertex cp, CadVertex pa, CadVertex pb,
int splitCnt,
DrawContext dc, DrawPen pen)
{
CadVertex va = pa - cp;
CadVertex vb = pb - cp;
if (va.Norm() < 0.01)
{
return;
}
double dt = (2.0 * Math.PI) / (double)splitCnt;
int div = splitCnt;
Vector3d normal = CadMath.Normal(va.vector, vb.vector);
CadQuaternion q = CadQuaternion.RotateQuaternion(normal, dt);
CadQuaternion r = q.Conjugate();
CadVertex p = va;
CadVertex tp1 = pa;
CadVertex tp2 = pa;
int i = 0;
for (; i < div - 1; i++)
{
CadQuaternion qp = CadQuaternion.FromPoint(p.vector);
qp = r * qp;
qp = qp * q;
p.vector = qp.ToPoint();
tp2 = p + cp;
dc.Drawing.DrawLine(pen, tp1.vector, tp2.vector);
tp1 = tp2;
}
dc.Drawing.DrawLine(pen, tp1.vector, pa.vector);
}
public static Matrix4d ToMatrix4d(CadQuaternion q)
{
Matrix4d m = default(Matrix4d);
double xx = q.x * q.x * 2.0;
double yy = q.y * q.y * 2.0;
double zz = q.z * q.z * 2.0;
double xy = q.x * q.y * 2.0;
double yz = q.y * q.z * 2.0;
double zx = q.z * q.x * 2.0;
double xw = q.x * q.t * 2.0;
double yw = q.y * q.t * 2.0;
double zw = q.z * q.t * 2.0;
// 1.0 - yy - zz, xy + zw, zx - yw, 0.0
// xy - zw, 1.0 - zz - xx, yz + xw, 0.0
// zx + yw, yz - xw, 1.0 - xx - yy, 0.0
// 0.0, 0.0, 0.0, 1.0
m.Row0[0] = 1.0 - yy - zz;
m.Row0[1] = xy + zw;
m.Row0[2] = zx - yw;
m.Row0[3] = 0.0;
m.Row1[0] = xy - zw;
m.Row1[1] = 1.0 - zz - xx;
m.Row1[2] = yz + xw;
m.Row1[3] = 0.0;
m.Row2[0] = zx + yw;
m.Row2[1] = yz - xw;
m.Row2[2] = 1.0 - xx - yy;
m.Row2[3] = 0.0;
m.Row3[0] = 0.0;
m.Row3[1] = 0.0;
m.Row3[2] = 0.0;
m.Row3[3] = 1.0;
return(m);
}
public void Rotate(CadQuaternion q, CadQuaternion r)
{
CadQuaternion qp;
qp = CadQuaternion.FromPoint(p0.vector);
qp = r * qp;
qp = qp * q;
p0.vector = CadQuaternion.ToPoint(qp);
qp = CadQuaternion.FromPoint(p1.vector);
qp = r * qp;
qp = qp * q;
p1.vector = CadQuaternion.ToPoint(qp);
qp = CadQuaternion.FromPoint(p2.vector);
qp = r * qp;
qp = qp * q;
p2.vector = CadQuaternion.ToPoint(qp);
qp = CadQuaternion.FromPoint(p3.vector);
qp = r * qp;
qp = qp * q;
p3.vector = CadQuaternion.ToPoint(qp);
qp = CadQuaternion.FromPoint(p4.vector);
qp = r * qp;
qp = qp * q;
p4.vector = CadQuaternion.ToPoint(qp);
}
public static void DrawArrow(
Action <DrawPen, Vector3d, Vector3d> DrawLine,
DrawPen pen,
Vector3d pt0,
Vector3d pt1,
ArrowTypes type,
ArrowPos pos,
double len,
double width)
{
DrawLine(pen, pt0, pt1);
Vector3d d = pt1 - pt0;
double dl = d.Length;
if (dl < 0.00001)
{
return;
}
Vector3d tmp = new Vector3d(dl, 0, 0);
double angle = Vector3d.CalculateAngle(tmp, d);
Vector3d normal = CadMath.CrossProduct(tmp, d); // 回転軸
if (normal.Length < 0.0001)
{
normal = new Vector3d(0, 0, 1);
}
else
{
normal = normal.UnitVector();
normal = CadMath.Normal(tmp, d);
}
CadQuaternion q = CadQuaternion.RotateQuaternion(normal, -angle);
CadQuaternion r = q.Conjugate();
ArrowHead a;
if (pos == ArrowPos.END || pos == ArrowPos.START_END)
{
a = ArrowHead.Create(type, ArrowPos.END, len, width);
a.Rotate(q, r);
a += pt1;
DrawLine(pen, a.p0.vector, a.p1.vector);
DrawLine(pen, a.p0.vector, a.p2.vector);
DrawLine(pen, a.p0.vector, a.p3.vector);
DrawLine(pen, a.p0.vector, a.p4.vector);
}
if (pos == ArrowPos.START || pos == ArrowPos.START_END)
{
a = ArrowHead.Create(type, ArrowPos.START, len, width);
a.Rotate(q, r);
a += pt0;
DrawLine(pen, a.p0.vector, a.p1.vector);
DrawLine(pen, a.p0.vector, a.p2.vector);
DrawLine(pen, a.p0.vector, a.p3.vector);
DrawLine(pen, a.p0.vector, a.p4.vector);
}
}
public static Vector3d ToVector3d(CadQuaternion q)
{
return(new Vector3d(q.x, q.y, q.z));
}
public override void MoveSelectedPointsFromStored(DrawContext dc, Vector3d delta)
{
CadVertex cp = StoreList[0];
if (cp.Selected)
{
mPointList[0] = cp + delta;
mPointList[1] = StoreList[1] + delta;
mPointList[2] = StoreList[2] + delta;
mPointList[3] = StoreList[3] + delta;
mPointList[4] = StoreList[4] + delta;
return;
}
StackArray <CadVertex> vt = default;
vt[0] = StoreList[1] - cp;
vt[1] = StoreList[2] - cp;
vt[2] = StoreList[3] - cp;
vt[3] = StoreList[4] - cp;
vt.Length = 4;
if (vt[0].Norm() < 0.01)
{
return;
}
int ai = -1;
for (int i = 0; i < 4; i++)
{
if (StoreList[i + 1].Selected)
{
ai = i;
break;
}
}
if (ai < 0)
{
return;
}
int bi = (ai + 1) % 4;
int ci = (ai + 2) % 4;
int di = (ai + 3) % 4;
Vector3d normal = CadMath.CrossProduct(vt[ai].vector, vt[bi].vector);
normal = normal.UnitVector();
vt[ai] += delta;
CadVertex uva = vt[ai].UnitVector();
CadVertex uvb = vt[bi].UnitVector();
if (!uva.EqualsThreshold(uvb))
{
normal = CadMath.CrossProduct(vt[ai].vector, vt[bi].vector);
if (normal.IsZero())
{
return;
}
normal = normal.UnitVector();
}
CadQuaternion q = CadQuaternion.RotateQuaternion(normal, Math.PI / 2.0);
CadQuaternion r = q.Conjugate();
CadQuaternion qp = CadQuaternion.FromPoint(vt[ai].vector);
qp = r * qp;
qp = qp * q;
vt[bi] = (CadVertex)qp.ToPoint();
vt[ci] = -vt[ai];
vt[di] = -vt[bi];
CadVertex tmp;
for (int i = 0; i < vt.Length; i++)
{
tmp = vt[i];
tmp.Selected = false;
vt[i] = tmp;
}
tmp = vt[ai];
tmp.Selected = true;
vt[ai] = tmp;
mPointList[1] = vt[0] + cp;
mPointList[2] = vt[1] + cp;
mPointList[3] = vt[2] + cp;
mPointList[4] = vt[3] + cp;
}
