/** * 四元数の積を求める * 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; }