<commit_before>/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001-2003 <NAME>.       *
 * All rights reserved.  Email: <EMAIL>   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * This library is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/

// OPCODE TriMesh/TriMesh collision code
// Written at 2006-10-28 by <NAME> (http://gimpact.sourceforge.net)

#ifdef _MSC_VER
#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
#endif

#include <ode/collision.h>
#include <ode/matrix.h>
#include <ode/rotation.h>
#include <ode/odemath.h>
#include "config.h"

// New Implementation
#if dTRIMESH_OPCODE_USE_NEW_TRIMESH_TRIMESH_COLLIDER

#if dTRIMESH_ENABLED

#include "collision_util.h"
#include "collision_trimesh_internal.h"


#if !dTLS_ENABLED
// Have collider cache instance unconditionally of OPCODE or GIMPACT selection
/*extern */TrimeshCollidersCache g_ccTrimeshCollidersCache;
#endif


#if dTRIMESH_OPCODE

#define SMALL_ELT           REAL(2.5e-4)
#define EXPANDED_ELT_THRESH REAL(1.0e-3)
#define DISTANCE_EPSILON    REAL(1.0e-8)
#define VELOCITY_EPSILON    REAL(1.0e-5)
#define TINY_PENETRATION    REAL(5.0e-6)

struct LineContactSet
{
	enum
	{
		MAX_POINTS = 8
	};

    dVector3 Points[MAX_POINTS];
    int      Count;
};


// static void GetTriangleGeometryCallback(udword, VertexPointers&, udword); -- not used
inline void dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B);
//static void dInvertMatrix4( dMatrix4& B, dMatrix4& Binv );
//static int IntersectLineSegmentRay(dVector3, dVector3, dVector3, dVector3,  dVector3);
static void ClipConvexPolygonAgainstPlane( const dVector3, dReal, LineContactSet& );


///returns the penetration depth
static dReal MostDeepPoints(
							LineContactSet & points,
							const dVector3 plane_normal,
							dReal plane_dist,
							LineContactSet & deep_points);

static bool TriTriContacts(const dVector3 tr1[3],
							 const dVector3 tr2[3],
							 dxGeom* g1, dxGeom* g2, int Flags,
							 CONTACT_KEY_HASH_TABLE &hashcontactset,
							 dContactGeom* Contacts, int Stride,
							 int &contactcount);


/* some math macros */
#define CROSS(dest,v1,v2) { dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
	dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
	dest[2]=v1[0]*v2[1]-v1[1]*v2[0]; }

#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])

#define SUB(dest,v1,v2) { dest[0]=v1[0]-v2[0]; dest[1]=v1[1]-v2[1]; dest[2]=v1[2]-v2[2]; }

#define ADD(dest,v1,v2) { dest[0]=v1[0]+v2[0]; dest[1]=v1[1]+v2[1]; dest[2]=v1[2]+v2[2]; }

#define MULT(dest,v,factor) { dest[0]=factor*v[0]; dest[1]=factor*v[1]; dest[2]=factor*v[2]; }

#define SET(dest,src) { dest[0]=src[0]; dest[1]=src[1]; dest[2]=src[2]; }

#define SMULT(p,q,s) { p[0]=q[0]*s; p[1]=q[1]*s; p[2]=q[2]*s; }

#define COMBO(combo,p,t,q) { combo[0]=p[0]+t*q[0]; combo[1]=p[1]+t*q[1]; combo[2]=p[2]+t*q[2]; }

#define LENGTH(x)  ((dReal) 1.0f/InvSqrt(dDOT(x, x)))

#define DEPTH(d, p, q, n) d = (p[0] - q[0])*n[0] +  (p[1] - q[1])*n[1] +  (p[2] - q[2])*n[2];

inline const dReal dMin(const dReal x, const dReal y)
{
    return x < y ? x : y;
}


inline void
SwapNormals(dVector3 *&pen_v, dVector3 *&col_v, dVector3* v1, dVector3* v2,
            dVector3 *&pen_elt, dVector3 *elt_f1, dVector3 *elt_f2,
            dVector3 n, dVector3 n1, dVector3 n2)
{
    if (pen_v == v1) {
        pen_v = v2;
        pen_elt = elt_f2;
        col_v = v1;
        SET(n, n1);
    }
    else {
        pen_v = v1;
        pen_elt = elt_f1;
        col_v = v2;
        SET(n, n2);
    }
}

///////////////////////MECHANISM FOR AVOID CONTACT REDUNDANCE///////////////////////////////
////* Written by <NAME> (http://gimpact.sourceforge.net) *///
#define CONTACT_DIFF_EPSILON REAL(0.00001)
#if defined(dDOUBLE)
#define CONTACT_NORMAL_ZERO REAL(0.0000001)
#else // if defined(dSINGLE)
#define CONTACT_NORMAL_ZERO REAL(0.00001)
#endif
#define CONTACT_POS_HASH_QUOTIENT REAL(10000.0)
#define dSQRT3	REAL(1.7320508075688773)

void UpdateContactKey(CONTACT_KEY & key, dContactGeom * contact)
{
	key.m_contact = contact;

	unsigned int hash=0;

	int i = 0;

	while (true)
	{
		dReal coord = contact->pos[i];
		coord = dFloor(coord * CONTACT_POS_HASH_QUOTIENT);

        const int sz = sizeof(coord) / sizeof(unsigned);
        unsigned hash_v[ sz ];
        memcpy(hash_v, &coord, sizeof(coord));
        for (int i=1; i<sz; ++i)
            hash_v[0] ^= hash_v[i];
        unsigned hash_input = hash_v[0];

		hash = (( hash << 4 ) + (hash_input >> 24)) ^ ( hash >> 28 );
		hash = (( hash << 4 ) + ((hash_input >> 16) & 0xFF)) ^ ( hash >> 28 );
		hash = (( hash << 4 ) + ((hash_input >> 8) & 0xFF)) ^ ( hash >> 28 );
		hash = (( hash << 4 ) + (hash_input & 0xFF)) ^ ( hash >> 28 );

		if (++i == 3)
		{
			break;
		}

		hash = (hash << 11) | (hash >> 21);
	}

	key.m_key = hash;
}


static inline unsigned int MakeContactIndex(unsigned int key)
{
	dIASSERT(CONTACTS_HASHSIZE == 256);

	unsigned int index = key ^ (key >> 16);
	index = (index ^ (index >> 8)) & 0xFF;
	return index;
}

dContactGeom *AddContactToNode(const CONTACT_KEY * contactkey,CONTACT_KEY_HASH_NODE * node)
{
	for(int i=0;i<node->m_keycount;i++)
	{
		if(node->m_keyarray[i].m_key == contactkey->m_key)
		{
			dContactGeom *contactfound = node->m_keyarray[i].m_contact;
			if (dDISTANCE(contactfound->pos, contactkey->m_contact->pos) < REAL(1.00001) /*for comp. errors*/ * dSQRT3 / CONTACT_POS_HASH_QUOTIENT /*cube diagonal*/)
			{
				return contactfound;
			}
		}
	}

	if (node->m_keycount < MAXCONTACT_X_NODE)
	{
		node->m_keyarray[node->m_keycount].m_contact = contactkey->m_contact;<commit_msg>Changed: Minor code improvements<commit_after>/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001-2003 <NAME>.       *
 * All rights reserved.  Email: <EMAIL>   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * This library is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/

// OPCODE TriMesh/TriMesh collision code
// Written at 2006-10-28 by <NAME> (http://gimpact.sourceforge.net)

#ifdef _MSC_VER
#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
#endif

#include <ode/collision.h>
#include <ode/matrix.h>
#include <ode/rotation.h>
#include <ode/odemath.h>
#include "config.h"

// New Implementation
#if dTRIMESH_OPCODE_USE_NEW_TRIMESH_TRIMESH_COLLIDER

#if dTRIMESH_ENABLED

#include "collision_util.h"
#include "collision_trimesh_internal.h"


#if !dTLS_ENABLED
// Have collider cache instance unconditionally of OPCODE or GIMPACT selection
/*extern */TrimeshCollidersCache g_ccTrimeshCollidersCache;
#endif


#if dTRIMESH_OPCODE

#define SMALL_ELT           REAL(2.5e-4)
#define EXPANDED_ELT_THRESH REAL(1.0e-3)
#define DISTANCE_EPSILON    REAL(1.0e-8)
#define VELOCITY_EPSILON    REAL(1.0e-5)
#define TINY_PENETRATION    REAL(5.0e-6)

struct LineContactSet
{
	enum
	{
		MAX_POINTS = 8
	};

    dVector3 Points[MAX_POINTS];
    int      Count;
};


// static void GetTriangleGeometryCallback(udword, VertexPointers&, udword); -- not used
inline void dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B);
//static void dInvertMatrix4( dMatrix4& B, dMatrix4& Binv );
//static int IntersectLineSegmentRay(dVector3, dVector3, dVector3, dVector3,  dVector3);
static void ClipConvexPolygonAgainstPlane( const dVector3, dReal, LineContactSet& );


///returns the penetration depth
static dReal MostDeepPoints(
							LineContactSet & points,
							const dVector3 plane_normal,
							dReal plane_dist,
							LineContactSet & deep_points);

static bool TriTriContacts(const dVector3 tr1[3],
							 const dVector3 tr2[3],
							 dxGeom* g1, dxGeom* g2, int Flags,
							 CONTACT_KEY_HASH_TABLE &hashcontactset,
							 dContactGeom* Contacts, int Stride,
							 int &contactcount);


/* some math macros */
#define CROSS(dest,v1,v2) { dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
	dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
	dest[2]=v1[0]*v2[1]-v1[1]*v2[0]; }

#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])

#define SUB(dest,v1,v2) { dest[0]=v1[0]-v2[0]; dest[1]=v1[1]-v2[1]; dest[2]=v1[2]-v2[2]; }

#define ADD(dest,v1,v2) { dest[0]=v1[0]+v2[0]; dest[1]=v1[1]+v2[1]; dest[2]=v1[2]+v2[2]; }

#define MULT(dest,v,factor) { dest[0]=factor*v[0]; dest[1]=factor*v[1]; dest[2]=factor*v[2]; }

#define SET(dest,src) { dest[0]=src[0]; dest[1]=src[1]; dest[2]=src[2]; }

#define SMULT(p,q,s) { p[0]=q[0]*s; p[1]=q[1]*s; p[2]=q[2]*s; }

#define COMBO(combo,p,t,q) { combo[0]=p[0]+t*q[0]; combo[1]=p[1]+t*q[1]; combo[2]=p[2]+t*q[2]; }

#define LENGTH(x)  ((dReal) 1.0f/InvSqrt(dDOT(x, x)))

#define DEPTH(d, p, q, n) d = (p[0] - q[0])*n[0] +  (p[1] - q[1])*n[1] +  (p[2] - q[2])*n[2];

inline const dReal dMin(const dReal x, const dReal y)
{
    return x < y ? x : y;
}


inline void
SwapNormals(dVector3 *&pen_v, dVector3 *&col_v, dVector3* v1, dVector3* v2,
            dVector3 *&pen_elt, dVector3 *elt_f1, dVector3 *elt_f2,
            dVector3 n, dVector3 n1, dVector3 n2)
{
    if (pen_v == v1) {
        pen_v = v2;
        pen_elt = elt_f2;
        col_v = v1;
        SET(n, n1);
    }
    else {
        pen_v = v1;
        pen_elt = elt_f1;
        col_v = v2;
        SET(n, n2);
    }
}

///////////////////////MECHANISM FOR AVOID CONTACT REDUNDANCE///////////////////////////////
////* Written by <NAME> (http://gimpact.sourceforge.net) *///
#define CONTACT_DIFF_EPSILON REAL(0.00001)
#if defined(dDOUBLE)
#define CONTACT_NORMAL_ZERO REAL(0.0000001)
#else // if defined(dSINGLE)
#define CONTACT_NORMAL_ZERO REAL(0.00001)
#endif
#define CONTACT_POS_HASH_QUOTIENT REAL(10000.0)
#define dSQRT3	REAL(1.7320508075688773)

void UpdateContactKey(CONTACT_KEY & key, dContactGeom * contact)
{
	key.m_contact = contact;

	unsigned int hash=0;

	int i = 0;

	while (true)
	{
		dReal coord = contact->pos[i];
		coord = dFloor(coord * CONTACT_POS_HASH_QUOTIENT);

        const int sz = sizeof(coord) / sizeof(unsigned);
		dIASSERT(sizeof(coord) % sizeof(unsigned) == 0);

        unsigned hash_v[ sz ];
		memcpy(hash_v, &coord, sizeof(coord));

		unsigned int hash_input = hash_v[0];
        for (int i=1; i<sz; ++i)
            hash_input ^= hash_v[i];

		hash = (( hash << 4 ) + (hash_input >> 24)) ^ ( hash >> 28 );
		hash = (( hash << 4 ) + ((hash_input >> 16) & 0xFF)) ^ ( hash >> 28 );
		hash = (( hash << 4 ) + ((hash_input >> 8) & 0xFF)) ^ ( hash >> 28 );
		hash = (( hash << 4 ) + (hash_input & 0xFF)) ^ ( hash >> 28 );

		if (++i == 3)
		{
			break;
		}

		hash = (hash << 11) | (hash >> 21);
	}

	key.m_key = hash;
}


static inline unsigned int MakeContactIndex(unsigned int key)
{
	dIASSERT(CONTACTS_HASHSIZE == 256);

	unsigned int index = key ^ (key >> 16);
	index = (index ^ (index >> 8)) & 0xFF;

	return index;
}

dContactGeom *AddContactToNode(const CONTACT_KEY * contactkey,CONTACT_KEY_HASH_NODE * node)
{
	for(int i=0;i<node->m_keycount;i++)
	{
		if(node->m_keyarray[i].m_key == contactkey->m_key)
		{
			dContactGeom *contactfound = node->m_keyarray[i].m_contact;
			if (dDISTANCE(contactfound->pos, contactkey->m_contact->pos) < REAL(1.00001) /*for comp. errors*/ * dSQRT3 / CONTACT_POS_HASH_QUOTIENT /*cube diagonal*/)
			{
				return contactfound;
			}
		}
	}

	if (node->m_keycount < MAXCONTACT_X_NODE)
	{
		node->m_keyarray[node->m_keycount].m_contact = contactkey->m_contact;
{'fraction_non_alphanumeric': 0.16916318594267607, 'fraction_numerical': 0.04292092755293101, 'mean_word_length': 2.7421180274858528, 'pattern_counts': {'":': 0, '<': 49, '<?xml version=': 0, '>': 83, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 30, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2260592', 'n_tokens_mistral': 5544, 'n_tokens_neox': 5079, 'n_words': 1142}