1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55

import numpy as np
from scipy.sparse.linalg import expm
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D


def get_sita(p):
    sita = np.arccos(np.dot(np.array((1, 0, 0)), p) / np.linalg.norm(p))
    if (p[1] < 0):
        sita = np.pi * 2 - sita
    return sita


def get_arc_points(center, p1, p2, step=0.01):
    center = np.array(center)
    p1 = np.array(p1)
    p2 = np.array(p2)
    
    R = np.sqrt(np.sum(np.power(p1 - center, 2)))
    
    cp = np.cross(center - p1, p2 - p1)
    a, b, c = cp
    d = np.dot(cp, center)
    
    cs = np.arccos(np.dot(p1 - center, p2 - center) / np.linalg.norm(p1 - center) / np.linalg.norm(p2 - center))
    roteAxis = np.cross(cp, [0, 0, 1])
    sita = np.arccos(np.dot(cp, [0, 0, 1]) / np.linalg.norm(cp))
    if (get_sita(cp - np.array((0, 0, 1)))) > 0:
        sita = -sita
    roteMatrix = expm(np.cross(np.eye(3), roteAxis / np.linalg.norm(roteAxis) * sita))
    roteBackMatrix = expm(np.cross(np.eye(3), roteAxis / np.linalg.norm(roteAxis) * (-sita)))
    P = np.vstack((center, p1, p2))
    RP = np.dot(P, roteMatrix)
    sp1 = get_sita(RP[1, :] - RP[0, :])
    sp2 = get_sita(RP[2, :] - RP[0, :])
    if np.abs(sp1 - sp2) > np.pi:
        st = np.hstack((np.arange(sp1, 2 * np.pi, step), np.arange(0, sp2, step))) if sp1 > sp2 else np.hstack(
            (np.arange(sp2, 2 * np.pi, step), np.arange(0, sp1, step)))
    else:
        st = np.arange(sp1, sp2, step) if sp2 > sp1 else np.arange(sp2, sp1, step)
    arc = np.array((R * np.cos(st) + RP[0, 0], R * np.sin(st) + RP[0, 1], st * 0 + RP[0, 2]))
    for i in np.arange(0, arc.shape[1]):
        arc[:, i] = np.dot(arc[:, i], roteBackMatrix)
    return arc