Function fixed_rotation_cost

cadquery/occ_impl/solver.py:529–546

(
    problem,
    m1: Type[None],
    T1_0,
    R1_0,
    T1,
    R1,
    val: Tuple[float, float, float],
    scale: float = 1,
)

def fixed_rotation_cost(
    problem,
    m1: Type[None],
    T1_0,
    R1_0,
    T1,
    R1,
    val: Tuple[float, float, float],
    scale: float = 1,
):

    q = gp_Quaternion()
    q.SetEulerAngles(gp_Extrinsic_XYZ, *val)
    q_dm = ca.DM((q.W(), q.X(), q.Y(), q.Z()))

    dummy = 1 - ca.dot(ca.vertcat(*Quaternion(R1_0 + R1)), q_dm) ** 2

    return dummy


# dictionary of individual constraint cost functions