1 files changed, 132 insertions, 0 deletions
diff --git a/libraries/ode-0.9/GIMPACT/src/gim_contact.cpp b/libraries/ode-0.9/GIMPACT/src/gim_contact.cpp
new file mode 100644
index 0000000..762af06
--- /dev/null
+++ b/libraries/ode-0.9/GIMPACT/src/gim_contact.cpp
@@ -0,0 +1,132 @@
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+For the latest info, see http://gimpact.sourceforge.net/
+Copyright (c) 2006 Francisco Leon. C.C. 80087371.
+email: projectileman@yahoo.com
+ 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 GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-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
+ GIMPACT-LICENSE-LGPL.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+-----------------------------------------------------------------------------
+*/
+#include "GIMPACT/gim_contact.h"
+void gim_merge_contacts(GDYNAMIC_ARRAY * source_contacts,
+                                        GDYNAMIC_ARRAY * dest_contacts)
+{
+    dest_contacts->m_size = 0;
+        GUINT source_count = source_contacts->m_size;
+        GIM_CONTACT * psource_contacts  = GIM_DYNARRAY_POINTER(GIM_CONTACT,(*source_contacts));
+        //create keys
+        GIM_RSORT_TOKEN * keycontacts = (GIM_RSORT_TOKEN * )gim_alloc(sizeof(GIM_RSORT_TOKEN)*source_count);
+    GUINT i;
+        for(i=0;i<source_count;i++)
+        {
+                keycontacts[i].m_value = i;
+                GIM_CALC_KEY_CONTACT(psource_contacts[i].m_point,keycontacts[i].m_key);
+        }
+        //sort keys
+        GIM_QUICK_SORT_ARRAY(GIM_RSORT_TOKEN , keycontacts, source_count, RSORT_TOKEN_COMPARATOR,GIM_DEF_EXCHANGE_MACRO);
+        // Merge contacts
+        GIM_CONTACT * pcontact = 0;
+        GIM_CONTACT * scontact = 0;
+        GUINT key,last_key=0;
+        for(i=0;i<source_contacts->m_size;i++)
+        {
+            key = keycontacts[i].m_key;
+                scontact = &psource_contacts[keycontacts[i].m_value];
+                if(i>0 && last_key ==  key)
+                {
+                        //merge contact
+                        if(pcontact->m_depth > scontact->m_depth + CONTACT_DIFF_EPSILON)
+                        {
+                            GIM_COPY_CONTACTS(pcontact, scontact);
+                        }
+                }
+                else
+                {//add new contact
+                    GIM_DYNARRAY_PUSH_EMPTY(GIM_CONTACT,(*dest_contacts));
+            pcontact = GIM_DYNARRAY_POINTER_LAST(GIM_CONTACT,(*dest_contacts));
+                    GIM_COPY_CONTACTS(pcontact, scontact);
+        }
+                last_key = key;
+        }
+        gim_free(keycontacts,0);
+}
+void gim_merge_contacts_unique(GDYNAMIC_ARRAY * source_contacts,
+                                        GDYNAMIC_ARRAY * dest_contacts)
+{
+    dest_contacts->m_size = 0;
+    //Traverse the source contacts
+        GUINT source_count = source_contacts->m_size;
+        if(source_count==0) return;
+        GIM_CONTACT * psource_contacts  = GIM_DYNARRAY_POINTER(GIM_CONTACT,(*source_contacts));
+        //add the unique contact
+        GIM_CONTACT * pcontact = 0;
+    GIM_DYNARRAY_PUSH_EMPTY(GIM_CONTACT,(*dest_contacts));
+    pcontact = GIM_DYNARRAY_POINTER_LAST(GIM_CONTACT,(*dest_contacts));
+    //set the first contact
+    GIM_COPY_CONTACTS(pcontact, psource_contacts);
+    if(source_count==1) return;
+    //scale the first contact
+    VEC_SCALE(pcontact->m_normal,pcontact->m_depth,pcontact->m_normal);
+    psource_contacts++;
+        //Average the contacts
+    GUINT i;
+        for(i=1;i<source_count;i++)
+        {
+            VEC_SUM(pcontact->m_point,pcontact->m_point,psource_contacts->m_point);
+            VEC_ACCUM(pcontact->m_normal,psource_contacts->m_depth,psource_contacts->m_normal);
+            psource_contacts++;
+        }
+        GREAL divide_average = 1.0f/((GREAL)source_count);
+        VEC_SCALE(pcontact->m_point,divide_average,pcontact->m_point);
+        pcontact->m_depth = VEC_DOT(pcontact->m_normal,pcontact->m_normal)*divide_average;
+        GIM_SQRT(pcontact->m_depth,pcontact->m_depth);
+        VEC_NORMALIZE(pcontact->m_normal);
+        /*GREAL normal_len;
+    VEC_INV_LENGTH(pcontact->m_normal,normal_len);
+        VEC_SCALE(pcontact->m_normal,normal_len,pcontact->m_normal);
+    //Deep = LEN(normal)/SQRT(source_count)
+    GIM_SQRT(divide_average,divide_average);
+        pcontact->m_depth = divide_average/normal_len;
+        */
+}

diff --git a/libraries/ode-0.9/GIMPACT/src/gim_contact.cpp b/libraries/ode-0.9/GIMPACT/src/gim_contact.cpp new file mode 100644 index 0000000..762af06 --- /dev/null +++ b/libraries/ode-0.9/GIMPACT/src/gim_contact.cpp
@@ -0,0 +1,132 @@
	1
	2	/*
	3	-----------------------------------------------------------------------------
	4	This source file is part of GIMPACT Library.
	5
	6	For the latest info, see http://gimpact.sourceforge.net/
	7
	8	Copyright (c) 2006 Francisco Leon. C.C. 80087371.
	9	email: projectileman@yahoo.com
	10
	11	This library is free software; you can redistribute it and/or
	12	modify it under the terms of EITHER:
	13	(1) The GNU Lesser General Public License as published by the Free
	14	Software Foundation; either version 2.1 of the License, or (at
	15	your option) any later version. The text of the GNU Lesser
	16	General Public License is included with this library in the
	17	file GIMPACT-LICENSE-LGPL.TXT.
	18	(2) The BSD-style license that is included with this library in
	19	the file GIMPACT-LICENSE-BSD.TXT.
	20
	21	This library is distributed in the hope that it will be useful,
	22	but WITHOUT ANY WARRANTY; without even the implied warranty of
	23	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
	24	GIMPACT-LICENSE-LGPL.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
	25
	26	-----------------------------------------------------------------------------
	27	*/
	28
	29	#include "GIMPACT/gim_contact.h"
	30
	31	void gim_merge_contacts(GDYNAMIC_ARRAY * source_contacts,
	32	GDYNAMIC_ARRAY * dest_contacts)
	33	{
	34	dest_contacts->m_size = 0;
	35
	36	GUINT source_count = source_contacts->m_size;
	37	GIM_CONTACT * psource_contacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,(*source_contacts));
	38	//create keys
	39	GIM_RSORT_TOKEN * keycontacts = (GIM_RSORT_TOKEN * )gim_alloc(sizeof(GIM_RSORT_TOKEN)*source_count);
	40
	41	GUINT i;
	42	for(i=0;i<source_count;i++)
	43	{
	44	keycontacts[i].m_value = i;
	45	GIM_CALC_KEY_CONTACT(psource_contacts[i].m_point,keycontacts[i].m_key);
	46	}
	47
	48	//sort keys
	49	GIM_QUICK_SORT_ARRAY(GIM_RSORT_TOKEN , keycontacts, source_count, RSORT_TOKEN_COMPARATOR,GIM_DEF_EXCHANGE_MACRO);
	50
	51	// Merge contacts
	52	GIM_CONTACT * pcontact = 0;
	53	GIM_CONTACT * scontact = 0;
	54	GUINT key,last_key=0;
	55
	56	for(i=0;i<source_contacts->m_size;i++)
	57	{
	58	key = keycontacts[i].m_key;
	59	scontact = &psource_contacts[keycontacts[i].m_value];
	60
	61	if(i>0 && last_key == key)
	62	{
	63	//merge contact
	64	if(pcontact->m_depth > scontact->m_depth + CONTACT_DIFF_EPSILON)
	65	{
	66	GIM_COPY_CONTACTS(pcontact, scontact);
	67	}
	68	}
	69	else
	70	{//add new contact
	71	GIM_DYNARRAY_PUSH_EMPTY(GIM_CONTACT,(*dest_contacts));
	72	pcontact = GIM_DYNARRAY_POINTER_LAST(GIM_CONTACT,(*dest_contacts));
	73	GIM_COPY_CONTACTS(pcontact, scontact);
	74	}
	75	last_key = key;
	76	}
	77	gim_free(keycontacts,0);
	78	}
	79
	80	void gim_merge_contacts_unique(GDYNAMIC_ARRAY * source_contacts,
	81	GDYNAMIC_ARRAY * dest_contacts)
	82	{
	83	dest_contacts->m_size = 0;
	84	//Traverse the source contacts
	85	GUINT source_count = source_contacts->m_size;
	86	if(source_count==0) return;
	87
	88	GIM_CONTACT * psource_contacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,(*source_contacts));
	89
	90	//add the unique contact
	91	GIM_CONTACT * pcontact = 0;
	92	GIM_DYNARRAY_PUSH_EMPTY(GIM_CONTACT,(*dest_contacts));
	93	pcontact = GIM_DYNARRAY_POINTER_LAST(GIM_CONTACT,(*dest_contacts));
	94	//set the first contact
	95	GIM_COPY_CONTACTS(pcontact, psource_contacts);
	96
	97	if(source_count==1) return;
	98	//scale the first contact
	99	VEC_SCALE(pcontact->m_normal,pcontact->m_depth,pcontact->m_normal);
	100
	101	psource_contacts++;
	102
	103	//Average the contacts
	104	GUINT i;
	105	for(i=1;i<source_count;i++)
	106	{
	107	VEC_SUM(pcontact->m_point,pcontact->m_point,psource_contacts->m_point);
	108	VEC_ACCUM(pcontact->m_normal,psource_contacts->m_depth,psource_contacts->m_normal);
	109	psource_contacts++;
	110	}
	111
	112	GREAL divide_average = 1.0f/((GREAL)source_count);
	113
	114	VEC_SCALE(pcontact->m_point,divide_average,pcontact->m_point);
	115
	116	pcontact->m_depth = VEC_DOT(pcontact->m_normal,pcontact->m_normal)*divide_average;
	117	GIM_SQRT(pcontact->m_depth,pcontact->m_depth);
	118
	119	VEC_NORMALIZE(pcontact->m_normal);
	120
	121	/*GREAL normal_len;
	122	VEC_INV_LENGTH(pcontact->m_normal,normal_len);
	123	VEC_SCALE(pcontact->m_normal,normal_len,pcontact->m_normal);
	124
	125	//Deep = LEN(normal)/SQRT(source_count)
	126	GIM_SQRT(divide_average,divide_average);
	127	pcontact->m_depth = divide_average/normal_len;
	128	*/
	129	}
	130
	131
	132