<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.                     *
*                                                                       *
*************************************************************************/

/*
 *	Cylinder-trimesh collider by <NAME>
 *   Ported to ODE by <NAME>
 */


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

#define TRIMESH_INTERNAL
#include "collision_trimesh_internal.h"

#define MAX_REAL	dInfinity
static const int	nCYLINDER_AXIS				= 2;
static const int    nCYLINDER_CIRCLE_SEGMENTS	= 8;
static const int    nMAX_CYLINDER_TRIANGLE_CLIP_POINTS	= 12;

#define OPTIMIZE_CONTACTS 1

// Local contacts data
typedef struct _sLocalContactData
{
	dVector3	vPos;
	dVector3	vNormal;
	dReal		fDepth;
	int			triIndex;
	int			nFlags; // 0 = filtered out, 1 = OK
}sLocalContactData;

typedef struct _sCylinderTrimeshColliderData
{
	// cylinder data
	dMatrix3	mCylinderRot;
	dQuaternion	qCylinderRot;
	dQuaternion	qInvCylinderRot;
	dVector3	vCylinderPos;
	dVector3	vCylinderAxis;
	dReal		fCylinderRadius;
	dReal		fCylinderSize;
	dVector3	avCylinderNormals[nCYLINDER_CIRCLE_SEGMENTS];

	// mesh data
	dQuaternion	qTrimeshRot;
	dQuaternion	qInvTrimeshRot;
	dMatrix3	mTrimeshRot;
	dVector3	vTrimeshPos;

	// global collider data
	dVector3	vBestPoint;
	dReal		fBestDepth;
	dReal		fBestCenter;
	dReal		fBestrt;
	int			iBestAxis;
	dVector3	vContactNormal;
	dVector3	vNormal;
	dVector3	vE0;
	dVector3	vE1;
	dVector3	vE2;

	// ODE stuff
	dGeomID				gCylinder;
	dxTriMesh*			gTrimesh;
	dContactGeom*		gContact;
	int					iFlags;
	int					iSkip;
	int					nContacts;// = 0;
	sLocalContactData*	gLocalContacts;
} sCylinderTrimeshColliderData;

// Short type name
typedef  sCylinderTrimeshColliderData sData;

// Use to classify contacts to be "near" in position
static const dReal fSameContactPositionEpsilon = REAL(0.0001); // 1e-4
// Use to classify contacts to be "near" in normal direction
static const dReal fSameContactNormalEpsilon = REAL(0.0001); // 1e-4

// If this two contact can be classified as "near"
inline int _IsNearContacts(sLocalContactData& c1,sLocalContactData& c2)
{
	int bPosNear = 0;
	int bSameDir = 0;
	dVector3	vDiff;

	// First check if they are "near" in position
	dVector3Subtract(c1.vPos,c2.vPos,vDiff);
	if (  (dFabs(vDiff[0]) < fSameContactPositionEpsilon)
		&&(dFabs(vDiff[1]) < fSameContactPositionEpsilon)
		&&(dFabs(vDiff[2]) < fSameContactPositionEpsilon))
	{
		bPosNear = 1;
	}

	// Second check if they are "near" in normal direction
	dVector3Subtract(c1.vNormal,c2.vNormal,vDiff);
	if (  (dFabs(vDiff[0]) < fSameContactNormalEpsilon)
		&&(dFabs(vDiff[1]) < fSameContactNormalEpsilon)
		&&(dFabs(vDiff[2]) < fSameContactNormalEpsilon) )
	{
		bSameDir = 1;
	}

	// Will be "near" if position and normal direction are "near"
	return (bPosNear && bSameDir);
}

inline int _IsBetter(sLocalContactData& c1,sLocalContactData& c2)
{
	// The not better will be throw away
	// You can change the selection criteria here
	return (c1.fDepth > c2.fDepth);
}

// iterate through gLocalContacts and filtered out "near contact"
inline void	_OptimizeLocalContacts(sData& cData)
{
	int nContacts = cData.nContacts;

	for (int i = 0; i < nContacts-1; i++)
	{
		for (int j = i+1; j < nContacts; j++)
		{
			if (_IsNearContacts(cData.gLocalContacts[i],cData.gLocalContacts[j]))
			{
				// If they are seem to be the samed then filtered 
				// out the least penetrate one
				if (_IsBetter(cData.gLocalContacts[j],cData.gLocalContacts[i]))
				{
					cData.gLocalContacts[i].nFlags = 0; // filtered 1st contact
				}
				else
				{
					cData.gLocalContacts[j].nFlags = 0; // filtered 2nd contact
				}

				// NOTE<commit_msg>Cosmetic (typo correction in commentary)<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.                     *
*                                                                       *
*************************************************************************/

/*
 *	Cylinder-trimesh collider by <NAME>
 *   Ported to ODE by <NAME>
 */


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

#define TRIMESH_INTERNAL
#include "collision_trimesh_internal.h"

#define MAX_REAL	dInfinity
static const int	nCYLINDER_AXIS				= 2;
static const int    nCYLINDER_CIRCLE_SEGMENTS	= 8;
static const int    nMAX_CYLINDER_TRIANGLE_CLIP_POINTS	= 12;

#define OPTIMIZE_CONTACTS 1

// Local contacts data
typedef struct _sLocalContactData
{
	dVector3	vPos;
	dVector3	vNormal;
	dReal		fDepth;
	int			triIndex;
	int			nFlags; // 0 = filtered out, 1 = OK
}sLocalContactData;

typedef struct _sCylinderTrimeshColliderData
{
	// cylinder data
	dMatrix3	mCylinderRot;
	dQuaternion	qCylinderRot;
	dQuaternion	qInvCylinderRot;
	dVector3	vCylinderPos;
	dVector3	vCylinderAxis;
	dReal		fCylinderRadius;
	dReal		fCylinderSize;
	dVector3	avCylinderNormals[nCYLINDER_CIRCLE_SEGMENTS];

	// mesh data
	dQuaternion	qTrimeshRot;
	dQuaternion	qInvTrimeshRot;
	dMatrix3	mTrimeshRot;
	dVector3	vTrimeshPos;

	// global collider data
	dVector3	vBestPoint;
	dReal		fBestDepth;
	dReal		fBestCenter;
	dReal		fBestrt;
	int			iBestAxis;
	dVector3	vContactNormal;
	dVector3	vNormal;
	dVector3	vE0;
	dVector3	vE1;
	dVector3	vE2;

	// ODE stuff
	dGeomID				gCylinder;
	dxTriMesh*			gTrimesh;
	dContactGeom*		gContact;
	int					iFlags;
	int					iSkip;
	int					nContacts;// = 0;
	sLocalContactData*	gLocalContacts;
} sCylinderTrimeshColliderData;

// Short type name
typedef  sCylinderTrimeshColliderData sData;

// Use to classify contacts to be "near" in position
static const dReal fSameContactPositionEpsilon = REAL(0.0001); // 1e-4
// Use to classify contacts to be "near" in normal direction
static const dReal fSameContactNormalEpsilon = REAL(0.0001); // 1e-4

// If this two contact can be classified as "near"
inline int _IsNearContacts(sLocalContactData& c1,sLocalContactData& c2)
{
	int bPosNear = 0;
	int bSameDir = 0;
	dVector3	vDiff;

	// First check if they are "near" in position
	dVector3Subtract(c1.vPos,c2.vPos,vDiff);
	if (  (dFabs(vDiff[0]) < fSameContactPositionEpsilon)
		&&(dFabs(vDiff[1]) < fSameContactPositionEpsilon)
		&&(dFabs(vDiff[2]) < fSameContactPositionEpsilon))
	{
		bPosNear = 1;
	}

	// Second check if they are "near" in normal direction
	dVector3Subtract(c1.vNormal,c2.vNormal,vDiff);
	if (  (dFabs(vDiff[0]) < fSameContactNormalEpsilon)
		&&(dFabs(vDiff[1]) < fSameContactNormalEpsilon)
		&&(dFabs(vDiff[2]) < fSameContactNormalEpsilon) )
	{
		bSameDir = 1;
	}

	// Will be "near" if position and normal direction are "near"
	return (bPosNear && bSameDir);
}

inline int _IsBetter(sLocalContactData& c1,sLocalContactData& c2)
{
	// The not better will be throw away
	// You can change the selection criteria here
	return (c1.fDepth > c2.fDepth);
}

// iterate through gLocalContacts and filtered out "near contact"
inline void	_OptimizeLocalContacts(sData& cData)
{
	int nContacts = cData.nContacts;

	for (int i = 0; i < nContacts-1; i++)
	{
		for (int j = i+1; j < nContacts; j++)
		{
			if (_IsNearContacts(cData.gLocalContacts[i],cData.gLocalContacts[j]))
			{
				// If they are seem to be the same then filtered 
				// out the least penetrate one
				if (_IsBetter(cData.gLocalContacts[j],cData.gLocalContacts[i]))
				{
					cData.gLocalContacts[i].nFlags = 0; // filtered 1st contact
				}
				else
				{
					cData.gLocalContacts[j].nFlags = 0; // filtered 2nd contact
				}

				// NOTE
{'fraction_non_alphanumeric': 0.11329275476143727, 'fraction_numerical': 0.016296878067936383, 'mean_word_length': 2.6896052155016297, 'pattern_counts': {'":': 0, '<': 35, '<?xml version=': 0, '>': 21, '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': '4923968', 'n_tokens_mistral': 3692, 'n_tokens_neox': 3379, 'n_words': 919}