# -*- coding: utf-8 -*-
import numpy as np
import sympy as sp
import math
"""
Created on Wed Jun  2 14:48:50 2021

@author: Lupin
命名规范 ：
模块 ：全小写
类名 ： 驼峰命名， 首字母大写， 私有类可用一个下划线开头
    1. class Car()
    2. class _PrivateCar
函数，变量名 : 一律小写， 如果有多个单词， 用下划线分开
常量 : 大写， 使用下划线分割单词
"""

def Momentum_of_Inertia(longth, width, M):
    return 1 / 12 * M * (longth ** 2 + width ** 2)

def angles_relative(longth, width):
    """
    为了在质心坐标中的角度
    :param longth:
    :param width:
    :return:
    """
    theta = np.arctan(width/longth)
    outputs = np.zeros(4)
    outputs[0] = -theta
    outputs[1] = theta
    outputs[2] = np.pi - theta
    outputs[3] = np.pi + theta
    return outputs


def angle2vector(angle):
    output = np.array([np.cos(angle), np.sin(angle)])
    return output

def wedge(A,B):
    """
    手写向量叉乘, 竟然没找到库函数
    :param A:
    :param B:
    :return:
    """
    x1 = A[0]
    y1 = A[1]
    z1 = A[2]
    x2 = B[0]
    y2 = B[1]
    z2 = B[2]
    outputs = np.array([(y1*z2 - y2*z1),(x1*z2 - x2*z1),(x1*y2 - x2*y1)])
    return outputs

def euler2mat(eulers):
    """欧拉角转旋转矩阵,ZXZ顺规, 弧度制"""
    phi1 = eulers[0]
    PHI = eulers[1]
    phi2 = eulers[2]
    matrix_z1 = np.array([[math.cos(phi1), -math.sin(phi1), 0],
                    [math.sin(phi1), math.cos(phi1), 0],
                    [0, 0, 1]])
    matrix_x = np.array([[1, 0, 0],
                         [0, math.cos(PHI), -math.sin(PHI)],
                         [0, math.sin(PHI), math.cos(PHI)],
                         ])
    matrix_z2 = np.array([[math.cos(phi2), -math.sin(phi2), 0],
                         [math.sin(phi2), math.cos(phi2), 0],
                         [0, 0, 1]])
    R = np.dot(matrix_z2,np.dot(matrix_x,matrix_z1))  # 忘记是主动还是被动了, 反正能用
    return R
def euler2mat_active(eulers):
    """欧拉角转旋转矩阵,ZXZ顺规, 弧度制"""
    phi1 = eulers[0]
    PHI = eulers[1]
    phi2 = eulers[2]
    matrix_z1 = np.array([[math.cos(phi1), -math.sin(phi1), 0],
                    [math.sin(phi1), math.cos(phi1), 0],
                    [0, 0, 1]])
    matrix_x = np.array([[1, 0, 0],
                         [0, math.cos(PHI), -math.sin(PHI)],
                         [0, math.sin(PHI), math.cos(PHI)],
                         ])
    matrix_z2 = np.array([[math.cos(phi2), -math.sin(phi2), 0],
                         [math.sin(phi2), math.cos(phi2), 0],
                         [0, 0, 1]])
    R = np.dot(matrix_z2,np.dot(matrix_x,matrix_z1))  # 忘记是主动还是被动了, 反正能用
    return R

def euler2mat_passive(eulers):
    """欧拉角转旋转矩阵,ZXZ顺规, 弧度制"""
    R = euler2mat_active(eulers).T
    return R

def collision(v2x=0,v2y=50,omi2z=0,ksi1=0,ksi2=0,theta2=0):
    """

    :param v2x:
    :param v2y:
    :param omi2z:
    :param ksi1:
    :param ksi2:
    :param theta2:
    :return: 碰撞之后的 V1x, V2x, V1y, V2y, Omi1z, Omi2z
    """
    "前车"
    v1x = 0
    v1y = 0
    omi1z = 0
    theta1 = 0

    X = sp.Matrix([v1x, v2x, v1y, v2y, omi1z, omi2z])

    # 两个质量
    m1 = 2500
    m2 = 2500

    # 碰撞系数
    e = 1
    miu = 0.00

    # 汽车的转动惯量
    Izz1 = 5000
    Izz2 = 5000

    Gamma = ksi1

    d1 = 2
    d2 = 2

    #
    da = d2 * np.sin(theta2 + ksi2)
    db = d2 * np.cos(theta2 + ksi2)
    dc = d1 * np.sin(theta1 + ksi1)
    dd = d1 * np.cos(theta1 + ksi1)


    C1 = sp.Matrix(
    [[m1, m2, 0, 0, 0, 0,],
    [0, 0, m1, m2, 0, 0,],
    [0, m2*dc, m1*dd, 0, Izz1, 0,],
    [0, m2*da, m1*db, 0, 0, Izz2,],
    [np.cos(Gamma), -np.cos(Gamma), np.sin(Gamma), -np.sin(Gamma), dc*np.cos(Gamma)-dd*np.sin(Gamma), da*np.cos(Gamma)-db*np.sin(Gamma),],
    [0, m2*(np.sin(Gamma)+miu*np.cos(Gamma)), m1*(np.cos(Gamma) - miu*np.sin(Gamma)), 0, 0, 0,]])

    C2 = sp.Matrix(
    [[m1, m2, 0, 0, 0, 0, ],
     [0, 0, m1, m2, 0, 0, ],
     [0, m2 * dc, m1 * dd, 0, Izz1, 0, ],
     [0, m2 * da, m1 * db, 0, 0, Izz2, ],
     [-e*np.cos(Gamma), e*np.cos(Gamma), -e*np.sin(Gamma), e*np.sin(Gamma), -e*(dc*np.cos(Gamma)-dd*np.sin(Gamma)),
      -e*(da*np.cos(Gamma)-db*np.sin(Gamma)), ],
     [0, m2*(np.sin(Gamma)+miu*np.cos(Gamma)), m1*(np.cos(Gamma)-miu*np.sin(Gamma)), 0, 0, 0, ]])
    outputs = (C1.inv()*(C2*X))
    return outputs

if __name__ == '__main__':
    collision(v2x=0,v2y=50,omi2z=0,ksi1=0,ksi2=0,theta2=0)