<commit_before>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2021 projectchrono.org
// All rights reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Authors: <NAME>, <NAME>
// =============================================================================
//
// Description: Utility functions for GJK and MPR narrowphase.
//
// =============================================================================

#pragma once

#include "chrono/collision/ChCollisionModel.h"
#include "chrono/collision/chrono/ChConvexShape.h"
#include "chrono/multicore_math/utility.h"

namespace chrono {
namespace collision {

/// @addtogroup collision_mc
/// @{

/// Support point for a sphere (for GJK and MPR).
inline real3 GetSupportPoint_Sphere(const real& radius, const real3& n) {
    return radius * n;
}

/// Support point for a triangle (for GJK and MPR).
inline real3 GetSupportPoint_Triangle(const real3* t, const real3& n) {
    real dist = Dot(t[0], n);
    real3 point = t[0];

    if (Dot(t[1], n) > dist) {
        dist = Dot(t[1], n);
        point = t[1];
    }

    if (Dot(t[2], n) > dist) {
        dist = Dot(t[2], n);
        point = t[2];
    }

    return point;
}

/// Support point for a box (for GJK and MPR).
inline real3 GetSupportPoint_Box(const real3& B, const real3& n) {
    real3 result = real3(0);
    result.x = Sign(n.x) * B.x;
    result.y = Sign(n.y) * B.y;
    result.z = Sign(n.z) * B.z;
    return result;
}

/// Support point for an ellipsoid (for GJK and MPR).
inline real3 GetSupportPoint_Ellipsoid(const real3& B, const real3& n) {
    real3 normal = n;
    real3 result = B * B * normal / Length(B * normal);
    return result;
}

/// Support point for a cylinder (for GJK and MPR).
inline real3 GetSupportPoint_Cylinder(const real3& B, const real3& n) {
    real s = Sqrt(n.x * n.x + n.z * n.z);
    real3 tmp;
    if (s != 0) {
        tmp.x = n.x * B.x / s;
        tmp.y = n.y < 0.0 ? -B.y : B.y;
        tmp.z = n.z * B.x / s;
    } else {
        tmp.x = B.x;
        tmp.y = n.y < 0.0 ? -B.y : B.y;
        tmp.z = 0;
    }
    return tmp;
}

/// Support point for a plane (for GJK and MPR).
inline real3 GetSupportPoint_Plane(const real3& B, const real3& n) {
    real3 result = B;

    if (n.x < 0)
        result.x = -result.x;

    if (n.y < 0)
        result.y = -result.y;

    return result;
}

/// Support point for a cone (for GJK and MPR).
inline real3 GetSupportPoint_Cone(const real3& B, const real3& n) {
    real radius = B.x;
    real height = B.y;

    real m_sinAngle = (radius / Sqrt(radius * radius + height * height));

    if (n.y > Length(n) * m_sinAngle) {
        return real3(0, height / 2.0, 0);
    } else {
        real s = Sqrt(n.x * n.x + n.z * n.z);
        if (s > 1e-9) {
            real3 tmp;
            tmp.x = n.x * B.x / s;
            tmp.y = -height / 2.0;
            tmp.z = n.z * B.z / s;
            return tmp;
        } else {
            real3 tmp;
            tmp.x = 0;
            tmp.y = -height / 2.0;
            tmp.z = 0;
            return tmp;
        }
    }
}

/// Support point for a line segment (for GJK and MPR).
inline real3 GetSupportPoint_Seg(const real B, const real3& n) {
    real3 result = real3(0);
    result.x = Sign(n.x) * B;

    return result;
}

/// Support point for a capsule (for GJK and MPR).
inline real3 GetSupportPoint_Capsule(const real2& B, const real3& n) {
    return GetSupportPoint_Seg(B.x, n) + GetSupportPoint_Sphere(B.y, n);
}

/// Support point for a disk (for GJK and MPR).
inline real3 GetSupportPoint_Disk(const real& B, const real3& n) {
    real3 n2 = real3(n.x, n.y, 0);
    n2 = Normalize(n2);

    real3 result = B * n2;

    return result;
}

/// Support point for a rectangle (for GJK and MPR).
inline real3 GetSupportPoint_Rect(const real3& B, const real3& n) {
    real3 result = real3(0);
    result.x = Sign(n.x) * B.x;
    result.z = Sign(n.z) * B.z;
    return result;
}

/// Support point for a rounded box, i.e. a sphere-swept box (for GJK and MPR).
inline real3 GetSupportPoint_RoundedBox(const real4& B, const real3& n) {
    return GetSupportPoint_Box(real3(B.x, B.y, B.z), n) + GetSupportPoint_Sphere(B.w, n);
}

/// Support point for a rounded cylinder, i.e. a sphere-swept cylinder (for GJK and MPR).
inline real3 GetSupportPoint_RoundedCylinder(const real4& B, const real3& n) {
    return GetSupportPoint_Cylinder(real3(B.x, B.y, B.z), n) + GetSupportPoint_Sphere(B.w, n);
}

/// Support point for a cylindrical shell (for GJK and MPR).<commit_msg>Fix incorrect support function for capsule shape<commit_after>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2021 projectchrono.org
// All rights reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Authors: <NAME>, <NAME>
// =============================================================================
//
// Description: Utility functions for GJK and MPR narrowphase.
//
// =============================================================================

#pragma once

#include "chrono/collision/ChCollisionModel.h"
#include "chrono/collision/chrono/ChConvexShape.h"
#include "chrono/multicore_math/utility.h"

namespace chrono {
namespace collision {

/// @addtogroup collision_mc
/// @{

/// Support point for a sphere (for GJK and MPR).
inline real3 GetSupportPoint_Sphere(const real& radius, const real3& n) {
    return radius * n;
}

/// Support point for a triangle (for GJK and MPR).
inline real3 GetSupportPoint_Triangle(const real3* t, const real3& n) {
    real dist = Dot(t[0], n);
    real3 point = t[0];

    if (Dot(t[1], n) > dist) {
        dist = Dot(t[1], n);
        point = t[1];
    }

    if (Dot(t[2], n) > dist) {
        dist = Dot(t[2], n);
        point = t[2];
    }

    return point;
}

/// Support point for a box (for GJK and MPR).
inline real3 GetSupportPoint_Box(const real3& B, const real3& n) {
    real3 result = real3(0);
    result.x = Sign(n.x) * B.x;
    result.y = Sign(n.y) * B.y;
    result.z = Sign(n.z) * B.z;
    return result;
}

/// Support point for an ellipsoid (for GJK and MPR).
inline real3 GetSupportPoint_Ellipsoid(const real3& B, const real3& n) {
    real3 normal = n;
    real3 result = B * B * normal / Length(B * normal);
    return result;
}

/// Support point for a cylinder (for GJK and MPR).
inline real3 GetSupportPoint_Cylinder(const real3& B, const real3& n) {
    real s = Sqrt(n.x * n.x + n.z * n.z);
    real3 tmp;
    if (s != 0) {
        tmp.x = n.x * B.x / s;
        tmp.y = n.y < 0.0 ? -B.y : B.y;
        tmp.z = n.z * B.x / s;
    } else {
        tmp.x = B.x;
        tmp.y = n.y < 0.0 ? -B.y : B.y;
        tmp.z = 0;
    }
    return tmp;
}

/// Support point for a plane (for GJK and MPR).
inline real3 GetSupportPoint_Plane(const real3& B, const real3& n) {
    real3 result = B;

    if (n.x < 0)
        result.x = -result.x;

    if (n.y < 0)
        result.y = -result.y;

    return result;
}

/// Support point for a cone (for GJK and MPR).
inline real3 GetSupportPoint_Cone(const real3& B, const real3& n) {
    real radius = B.x;
    real height = B.y;

    real m_sinAngle = (radius / Sqrt(radius * radius + height * height));

    if (n.y > Length(n) * m_sinAngle) {
        return real3(0, height / 2.0, 0);
    } else {
        real s = Sqrt(n.x * n.x + n.z * n.z);
        if (s > 1e-9) {
            real3 tmp;
            tmp.x = n.x * B.x / s;
            tmp.y = -height / 2.0;
            tmp.z = n.z * B.z / s;
            return tmp;
        } else {
            real3 tmp;
            tmp.x = 0;
            tmp.y = -height / 2.0;
            tmp.z = 0;
            return tmp;
        }
    }
}

/// Support point for a line segment (for GJK and MPR).
inline real3 GetSupportPoint_Seg(const real B, const real3& n) {
    real3 result = real3(0);
    result.x = Sign(n.x) * B;

    return result;
}

/// Support point for a capsule (for GJK and MPR).
inline real3 GetSupportPoint_Capsule(const real2& B, const real3& n) {
    return GetSupportPoint_Seg(B.y, n) + GetSupportPoint_Sphere(B.x, n);
}

/// Support point for a disk (for GJK and MPR).
inline real3 GetSupportPoint_Disk(const real& B, const real3& n) {
    real3 n2 = real3(n.x, n.y, 0);
    n2 = Normalize(n2);

    real3 result = B * n2;

    return result;
}

/// Support point for a rectangle (for GJK and MPR).
inline real3 GetSupportPoint_Rect(const real3& B, const real3& n) {
    real3 result = real3(0);
    result.x = Sign(n.x) * B.x;
    result.z = Sign(n.z) * B.z;
    return result;
}

/// Support point for a rounded box, i.e. a sphere-swept box (for GJK and MPR).
inline real3 GetSupportPoint_RoundedBox(const real4& B, const real3& n) {
    return GetSupportPoint_Box(real3(B.x, B.y, B.z), n) + GetSupportPoint_Sphere(B.w, n);
}

/// Support point for a rounded cylinder, i.e. a sphere-swept cylinder (for GJK and MPR).
inline real3 GetSupportPoint_RoundedCylinder(const real4& B, const real3& n) {
    return GetSupportPoint_Cylinder(real3(B.x, B.y, B.z), n) + GetSupportPoint_Sphere(B.w, n);
}

/// Support point for a cylindrical shell (for GJK and MPR).
{'fraction_non_alphanumeric': 0.19434447300771207, 'fraction_numerical': 0.01953727506426735, 'mean_word_length': 2.5023406553835073, 'pattern_counts': {'":': 0, '<': 15, '<?xml version=': 0, '>': 15, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '973993', 'n_tokens_mistral': 3554, 'n_tokens_neox': 3394, 'n_words': 1053}