main.py

import argparse
import pybullet as p
import numpy as np
from classes.video_source import WebcamSource
from classes.body_vector import BodyVector
from classes.pybullet_model import PyBullet3DModel
from classes.refresh_counter import RefreshCounter
from classes.ralphapp import RalphApp
from initialization import mp_pose, mp_drawing, drawing_spec, init_pybullet
from constants import POSE_MODEL_COMPLEXITY





def vecteur_directeur_plan(points):
    """
    Calcule le vecteur normal du plan ajusté aux 4 points 3D
    par régression orthogonale (PCA).

    Paramètres :
        points : liste ou array de 4 points, chacun étant un triplet (x, y, z)

    Retour :
        normal : np.array de taille (3,), vecteur directeur unitaire orthogonal au plan
    """
    points = np.array(points)
    if points.shape != (4, 3):
        raise ValueError("Il faut fournir exactement 4 points en 3D (forme (4, 3)).")

    # Centrage des points
    centre = points.mean(axis=0)
    points_centres = points - centre

    # Matrice de covariance
    covariance = points_centres.T @ points_centres

    # Décomposition en valeurs propres
    valeurs_propres, vecteurs_propres = np.linalg.eigh(covariance)

    # Le vecteur propre associé à la plus petite valeur propre
    # correspond au vecteur normal au plan
    normal = vecteurs_propres[:, np.argmin(valeurs_propres)]

    # Normalisation
    normal = normal / np.linalg.norm(normal)

    return normal



def orienter_vecteur(normal, reference):
    """
    Oriente le vecteur `normal` pour qu’il pointe dans le même demi-espace que `reference`.
    """
    normal = normal / np.linalg.norm(normal)
    reference = reference / np.linalg.norm(reference)
    if np.dot(normal, reference) < 0:
        return -normal
    return normal





def vecteur_perpendiculaire_proj_3d(points3d):
    """
    Calcule un vecteur 3D unitaire dans le plan XY (z = 0),
    perpendiculaire au vecteur directeur formé par deux points 3D projetés.

    Paramètres :
        points3d : liste de 2 points 3D [(x1, y1, z1), (x2, y2, z2)]

    Retour :
        vecteur_3d_perp : np.array de forme (3,), unitaire, avec z = 0
    """
    points3d = np.array(points3d)
    if points3d.shape != (2, 3):
        raise ValueError("Il faut fournir exactement 2 points 3D (forme (2, 3)).")

    # Projection dans le plan XY
    p1_xy = points3d[0][:2]
    p2_xy = points3d[1][:2]
    v_xy = p2_xy - p1_xy

    norm = np.linalg.norm(v_xy)
    if norm == 0:
        raise ValueError("Les deux points projetés sont confondus dans le plan XY.")

    # Vecteur perpendiculaire dans le plan XY (rotation de +90°)
    v_perp_xy = np.array([-v_xy[1], v_xy[0]])

    # Normalisation
    v_perp_xy = v_perp_xy / np.linalg.norm(v_perp_xy)

    # Remise en 3D avec z = 0
    return np.array([v_perp_xy[0], v_perp_xy[1], 0.0])


def main(inp):
    init_pybullet()
    app = RalphApp()
    source = WebcamSource()
    body = BodyVector()
    body_3d_model = PyBullet3DModel()
    counter = RefreshCounter()

    with mp_pose.Pose(
        min_detection_confidence=0.5,
        min_tracking_confidence=0.5,
        static_image_mode=False,
        model_complexity=POSE_MODEL_COMPLEXITY
    ) as pose:
        for idx, (frame, frame_rgb) in enumerate(source):
            results = pose.process(frame_rgb)            

            if results.pose_landmarks:
                mp_drawing.draw_landmarks(
                    frame,
                    results.pose_landmarks,
                    mp_pose.POSE_CONNECTIONS,
                    landmark_drawing_spec=drawing_spec,
                    connection_drawing_spec=drawing_spec,
                )

                list_landmarks = results.pose_landmarks.landmark

                body.body_update(list_landmarks)
                
                # Convertir les coordonnées
                b2_3d_vector_list = body.change_of_geometric_reference_b1_to_b2_list(body.position_body_members)
 
                # Mettre à jour le modèle 3D
                body_3d_model.full_update(b2_3d_vector_list)

                # # Mettre à jour la taille de l'avatar
                # if counter.refresh():
                #     body_3d_model.create_skeleton()

                
                LeftShoulder_x, LeftShoulder_y, LeftShoulder_z = b2_3d_vector_list[12]
                LeftForeArm_x, LeftForeArm_y, LeftForeArm_z = b2_3d_vector_list[14]
                LeftHandle_x, LeftHandle_y, LeftHandle_z = b2_3d_vector_list[16]


                LeftArm = -(LeftForeArm_x - LeftShoulder_x), -(LeftForeArm_y - LeftShoulder_y), (LeftForeArm_z - LeftShoulder_z)
                LeftForeArm = -(LeftHandle_x - LeftForeArm_x),(LeftHandle_y - LeftForeArm_y), (LeftHandle_z - LeftForeArm_z)

                app.update_joint("LeftShoulder", *LeftArm)
                app.update_joint("LeftForeArm", *LeftForeArm)

                RightShoulder_x, RightShoulder_y, RightShoulder_z = b2_3d_vector_list[11]
                RightForeArm_x, RightForeArm_y, RightForeArm_z = b2_3d_vector_list[13]
                RightHandle_x, RightHandle_y, RightHandle_z = b2_3d_vector_list[15]

                RightArm = -(RightForeArm_x - RightShoulder_x), -(RightForeArm_y - RightShoulder_y), (RightForeArm_z - RightShoulder_z)
                RightForeArm = -(RightHandle_x - RightForeArm_x), (RightHandle_y - RightForeArm_y), (RightHandle_z - RightForeArm_z)
                app.update_joint("RightShoulder", *RightArm)
                app.update_joint("RightForeArm", *RightForeArm)

                LeftUpLeg_x, LeftUpLeg_y, LeftUpLeg_z = b2_3d_vector_list[24]
                LeftLeg_x, LeftLeg_y, LeftLeg_z = b2_3d_vector_list[26]
                LeftFoot_x, LeftFoot_y, LeftFoot_z = b2_3d_vector_list[28]

                LeftUpLeg = -(LeftLeg_x - LeftUpLeg_x), -(LeftLeg_y - LeftUpLeg_y), (LeftLeg_z - LeftUpLeg_z)
                LeftLeg = -(LeftFoot_x - LeftLeg_x), -(LeftFoot_y - LeftLeg_y), (LeftFoot_z - LeftLeg_z)

                app.update_joint("LeftUpLeg", *LeftUpLeg)
                app.update_joint("LeftLeg", *LeftLeg)

                RightUpLeg_x, RightUpLeg_y, RightUpLeg_z = b2_3d_vector_list[23]
                RightLeg_x, RightLeg_y, RightLeg_z = b2_3d_vector_list[25]
                RightFoot_x, RightFoot_y, RightFoot_z = b2_3d_vector_list[27]

                RightUpLeg = -(RightLeg_x - RightUpLeg_x), -(RightLeg_y - RightUpLeg_y), (RightLeg_z - RightUpLeg_z)
                RightLeg = -(RightFoot_x - RightLeg_x), -(RightFoot_y - RightLeg_y), (RightFoot_z - RightLeg_z)

                app.update_joint("RightUpLeg", *RightUpLeg)
                app.update_joint("RightLeg", *RightLeg)

                points = [
                    b2_3d_vector_list[11],
                    b2_3d_vector_list[12],
                    b2_3d_vector_list[23],
                    b2_3d_vector_list[24]
                ]

                n = vecteur_directeur_plan(points)

                reference = np.array([0, -1, 0])  # On veut que le vecteur pointe vers le haut
                normal_oriente = orienter_vecteur(n, reference)


                print("Vecteur normal du plan :", normal_oriente)
                x, y, z = normal_oriente
                app.update_joint("Hips", x, y, -z)


                pts = [
                    b2_3d_vector_list[23],
                    b2_3d_vector_list[24]
                ]

                v3d = vecteur_perpendiculaire_proj_3d(pts)
                print("Vecteur perpendiculaire projeté en 3D :", v3d)

                app.update_joint("Pelvis", *v3d)


                # Exemples d'appels unitaires
                #app.update_joint("Hips", 0, -1, 0 )
                #app.update_joint("LeftLeg", 0, 0, -1 )
                # Exécute une seule frame Panda3D
                app.step()

                p.stepSimulation()

            source.show(frame)

if __name__ == "__main__":
    parser = argparse.ArgumentParser(
        description="Choose video file otherwise webcam is used."
    )
    parser.add_argument(
        "-i", metavar="path-to-file", type=str, help="Path to video file"
    )
    args = parser.parse_args()
    main(args.i)