[llvm-commits] [test-suite] r91782 [3/5] - in /test-suite/trunk/MultiSource/Benchmarks: ./ Bullet/ Bullet/include/ Bullet/include/BulletCollision/ Bullet/include/BulletCollision/BroadphaseCollision/ Bullet/include/BulletCollision/CollisionDispatch/ Bullet/include/BulletCollision/CollisionShapes/ Bullet/include/BulletCollision/Gimpact/ Bullet/include/BulletCollision/NarrowPhaseCollision/ Bullet/include/BulletDynamics/ Bullet/include/BulletDynamics/Character/ Bullet/include/BulletDynamics/ConstraintSolver/ Bullet/include...
Anton Korobeynikov
asl at math.spbu.ru
Sat Dec 19 12:06:02 PST 2009
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_hash_table.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_hash_table.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_hash_table.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_hash_table.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,902 @@
+#ifndef GIM_HASH_TABLE_H_INCLUDED
+#define GIM_HASH_TABLE_H_INCLUDED
+/*! \file gim_trimesh_data.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at 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.
+ (3) The zlib/libpng license that is included with this library in
+ the file GIMPACT-LICENSE-ZLIB.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, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "gim_radixsort.h"
+
+
+#define GIM_INVALID_HASH 0xffffffff //!< A very very high value
+#define GIM_DEFAULT_HASH_TABLE_SIZE 380
+#define GIM_DEFAULT_HASH_TABLE_NODE_SIZE 4
+#define GIM_HASH_TABLE_GROW_FACTOR 2
+
+#define GIM_MIN_RADIX_SORT_SIZE 860 //!< calibrated on a PIII
+
+template<typename T>
+struct GIM_HASH_TABLE_NODE
+{
+ GUINT m_key;
+ T m_data;
+ GIM_HASH_TABLE_NODE()
+ {
+ }
+
+ GIM_HASH_TABLE_NODE(const GIM_HASH_TABLE_NODE & value)
+ {
+ m_key = value.m_key;
+ m_data = value.m_data;
+ }
+
+ GIM_HASH_TABLE_NODE(GUINT key, const T & data)
+ {
+ m_key = key;
+ m_data = data;
+ }
+
+ bool operator <(const GIM_HASH_TABLE_NODE<T> & other) const
+ {
+ ///inverse order, further objects are first
+ if(m_key < other.m_key) return true;
+ return false;
+ }
+
+ bool operator >(const GIM_HASH_TABLE_NODE<T> & other) const
+ {
+ ///inverse order, further objects are first
+ if(m_key > other.m_key) return true;
+ return false;
+ }
+
+ bool operator ==(const GIM_HASH_TABLE_NODE<T> & other) const
+ {
+ ///inverse order, further objects are first
+ if(m_key == other.m_key) return true;
+ return false;
+ }
+};
+
+///Macro for getting the key
+class GIM_HASH_NODE_GET_KEY
+{
+public:
+ template<class T>
+ inline GUINT operator()( const T& a)
+ {
+ return a.m_key;
+ }
+};
+
+
+
+///Macro for comparing the key and the element
+class GIM_HASH_NODE_CMP_KEY_MACRO
+{
+public:
+ template<class T>
+ inline int operator() ( const T& a, GUINT key)
+ {
+ return ((int)(a.m_key - key));
+ }
+};
+
+///Macro for comparing Hash nodes
+class GIM_HASH_NODE_CMP_MACRO
+{
+public:
+ template<class T>
+ inline int operator() ( const T& a, const T& b )
+ {
+ return ((int)(a.m_key - b.m_key));
+ }
+};
+
+
+
+
+
+//! Sorting for hash table
+/*!
+switch automatically between quicksort and radixsort
+*/
+template<typename T>
+void gim_sort_hash_node_array(T * array, GUINT array_count)
+{
+ if(array_count<GIM_MIN_RADIX_SORT_SIZE)
+ {
+ gim_heap_sort(array,array_count,GIM_HASH_NODE_CMP_MACRO());
+ }
+ else
+ {
+ memcopy_elements_func cmpfunc;
+ gim_radix_sort(array,array_count,GIM_HASH_NODE_GET_KEY(),cmpfunc);
+ }
+}
+
+
+
+
+
+
+// Note: assumes long is at least 32 bits.
+#define GIM_NUM_PRIME 28
+
+static const GUINT gim_prime_list[GIM_NUM_PRIME] =
+{
+ 53ul, 97ul, 193ul, 389ul, 769ul,
+ 1543ul, 3079ul, 6151ul, 12289ul, 24593ul,
+ 49157ul, 98317ul, 196613ul, 393241ul, 786433ul,
+ 1572869ul, 3145739ul, 6291469ul, 12582917ul, 25165843ul,
+ 50331653ul, 100663319ul, 201326611ul, 402653189ul, 805306457ul,
+ 1610612741ul, 3221225473ul, 4294967291ul
+};
+
+inline GUINT gim_next_prime(GUINT number)
+{
+ //Find nearest upper prime
+ GUINT result_ind = 0;
+ gim_binary_search(gim_prime_list,0,(GIM_NUM_PRIME-2),number,result_ind);
+
+ // inv: result_ind < 28
+ return gim_prime_list[result_ind];
+}
+
+
+
+//! A compact hash table implementation
+/*!
+A memory aligned compact hash table that coud be treated as an array.
+It could be a simple sorted array without the overhead of the hash key bucked, or could
+be a formely hash table with an array of keys.
+You can use switch_to_hashtable() and switch_to_sorted_array for saving space or increase speed.
+</br>
+
+<ul>
+<li> if node_size = 0, then this container becomes a simple sorted array allocator. reserve_size is used for reserve memory in m_nodes.
+When the array size reaches the size equivalent to 'min_hash_table_size', then it becomes a hash table by calling check_for_switching_to_hashtable.
+<li> If node_size != 0, then this container becomes a hash table for ever
+</ul>
+
+*/
+template<class T>
+class gim_hash_table
+{
+protected:
+ typedef GIM_HASH_TABLE_NODE<T> _node_type;
+
+ //!The nodes
+ //array< _node_type, SuperAllocator<_node_type> > m_nodes;
+ gim_array< _node_type > m_nodes;
+ //SuperBufferedArray< _node_type > m_nodes;
+ bool m_sorted;
+
+ ///Hash table data management. The hash table has the indices to the corresponding m_nodes array
+ GUINT * m_hash_table;//!<
+ GUINT m_table_size;//!<
+ GUINT m_node_size;//!<
+ GUINT m_min_hash_table_size;
+
+
+
+ //! Returns the cell index
+ inline GUINT _find_cell(GUINT hashkey)
+ {
+ _node_type * nodesptr = m_nodes.pointer();
+ GUINT start_index = (hashkey%m_table_size)*m_node_size;
+ GUINT end_index = start_index + m_node_size;
+
+ while(start_index<end_index)
+ {
+ GUINT value = m_hash_table[start_index];
+ if(value != GIM_INVALID_HASH)
+ {
+ if(nodesptr[value].m_key == hashkey) return start_index;
+ }
+ start_index++;
+ }
+ return GIM_INVALID_HASH;
+ }
+
+ //! Find the avaliable cell for the hashkey, and return an existing cell if it has the same hash key
+ inline GUINT _find_avaliable_cell(GUINT hashkey)
+ {
+ _node_type * nodesptr = m_nodes.pointer();
+ GUINT avaliable_index = GIM_INVALID_HASH;
+ GUINT start_index = (hashkey%m_table_size)*m_node_size;
+ GUINT end_index = start_index + m_node_size;
+
+ while(start_index<end_index)
+ {
+ GUINT value = m_hash_table[start_index];
+ if(value == GIM_INVALID_HASH)
+ {
+ if(avaliable_index==GIM_INVALID_HASH)
+ {
+ avaliable_index = start_index;
+ }
+ }
+ else if(nodesptr[value].m_key == hashkey)
+ {
+ return start_index;
+ }
+ start_index++;
+ }
+ return avaliable_index;
+ }
+
+
+
+ //! reserves the memory for the hash table.
+ /*!
+ \pre hash table must be empty
+ \post reserves the memory for the hash table, an initializes all elements to GIM_INVALID_HASH.
+ */
+ inline void _reserve_table_memory(GUINT newtablesize)
+ {
+ if(newtablesize==0) return;
+ if(m_node_size==0) return;
+
+ //Get a Prime size
+
+ m_table_size = gim_next_prime(newtablesize);
+
+ GUINT datasize = m_table_size*m_node_size;
+ //Alloc the data buffer
+ m_hash_table = (GUINT *)gim_alloc(datasize*sizeof(GUINT));
+ }
+
+ inline void _invalidate_keys()
+ {
+ GUINT datasize = m_table_size*m_node_size;
+ for(GUINT i=0;i<datasize;i++)
+ {
+ m_hash_table[i] = GIM_INVALID_HASH;// invalidate keys
+ }
+ }
+
+ //! Clear all memory for the hash table
+ inline void _clear_table_memory()
+ {
+ if(m_hash_table==NULL) return;
+ gim_free(m_hash_table);
+ m_hash_table = NULL;
+ m_table_size = 0;
+ }
+
+ //! Invalidates the keys (Assigning GIM_INVALID_HASH to all) Reorders the hash keys
+ inline void _rehash()
+ {
+ _invalidate_keys();
+
+ _node_type * nodesptr = m_nodes.pointer();
+ for(GUINT i=0;i<(GUINT)m_nodes.size();i++)
+ {
+ GUINT nodekey = nodesptr[i].m_key;
+ if(nodekey != GIM_INVALID_HASH)
+ {
+ //Search for the avaliable cell in buffer
+ GUINT index = _find_avaliable_cell(nodekey);
+
+
+ if(m_hash_table[index]!=GIM_INVALID_HASH)
+ {//The new index is alreade used... discard this new incomming object, repeated key
+ btAssert(m_hash_table[index]==nodekey);
+ nodesptr[i].m_key = GIM_INVALID_HASH;
+ }
+ else
+ {
+ //;
+ //Assign the value for alloc
+ m_hash_table[index] = i;
+ }
+ }
+ }
+ }
+
+ //! Resize hash table indices
+ inline void _resize_table(GUINT newsize)
+ {
+ //Clear memory
+ _clear_table_memory();
+ //Alloc the data
+ _reserve_table_memory(newsize);
+ //Invalidate keys and rehash
+ _rehash();
+ }
+
+ //! Destroy hash table memory
+ inline void _destroy()
+ {
+ if(m_hash_table==NULL) return;
+ _clear_table_memory();
+ }
+
+ //! Finds an avaliable hash table cell, and resizes the table if there isn't space
+ inline GUINT _assign_hash_table_cell(GUINT hashkey)
+ {
+ GUINT cell_index = _find_avaliable_cell(hashkey);
+
+ if(cell_index==GIM_INVALID_HASH)
+ {
+ //rehashing
+ _resize_table(m_table_size+1);
+ GUINT cell_index = _find_avaliable_cell(hashkey);
+ btAssert(cell_index!=GIM_INVALID_HASH);
+ }
+ return cell_index;
+ }
+
+ //! erase by index in hash table
+ inline bool _erase_by_index_hash_table(GUINT index)
+ {
+ if(index >= m_nodes.size()) return false;
+ if(m_nodes[index].m_key != GIM_INVALID_HASH)
+ {
+ //Search for the avaliable cell in buffer
+ GUINT cell_index = _find_cell(m_nodes[index].m_key);
+
+ btAssert(cell_index!=GIM_INVALID_HASH);
+ btAssert(m_hash_table[cell_index]==index);
+
+ m_hash_table[cell_index] = GIM_INVALID_HASH;
+ }
+
+ return this->_erase_unsorted(index);
+ }
+
+ //! erase by key in hash table
+ inline bool _erase_hash_table(GUINT hashkey)
+ {
+ if(hashkey == GIM_INVALID_HASH) return false;
+
+ //Search for the avaliable cell in buffer
+ GUINT cell_index = _find_cell(hashkey);
+ if(cell_index ==GIM_INVALID_HASH) return false;
+
+ GUINT index = m_hash_table[cell_index];
+ m_hash_table[cell_index] = GIM_INVALID_HASH;
+
+ return this->_erase_unsorted(index);
+ }
+
+
+
+ //! insert an element in hash table
+ /*!
+ If the element exists, this won't insert the element
+ \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted
+ If so, the element has been inserted at the last position of the array.
+ */
+ inline GUINT _insert_hash_table(GUINT hashkey, const T & value)
+ {
+ if(hashkey==GIM_INVALID_HASH)
+ {
+ //Insert anyway
+ _insert_unsorted(hashkey,value);
+ return GIM_INVALID_HASH;
+ }
+
+ GUINT cell_index = _assign_hash_table_cell(hashkey);
+
+ GUINT value_key = m_hash_table[cell_index];
+
+ if(value_key!= GIM_INVALID_HASH) return value_key;// Not overrited
+
+ m_hash_table[cell_index] = m_nodes.size();
+
+ _insert_unsorted(hashkey,value);
+ return GIM_INVALID_HASH;
+ }
+
+ //! insert an element in hash table.
+ /*!
+ If the element exists, this replaces the element.
+ \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted
+ If so, the element has been inserted at the last position of the array.
+ */
+ inline GUINT _insert_hash_table_replace(GUINT hashkey, const T & value)
+ {
+ if(hashkey==GIM_INVALID_HASH)
+ {
+ //Insert anyway
+ _insert_unsorted(hashkey,value);
+ return GIM_INVALID_HASH;
+ }
+
+ GUINT cell_index = _assign_hash_table_cell(hashkey);
+
+ GUINT value_key = m_hash_table[cell_index];
+
+ if(value_key!= GIM_INVALID_HASH)
+ {//replaces the existing
+ m_nodes[value_key] = _node_type(hashkey,value);
+ return value_key;// index of the replaced element
+ }
+
+ m_hash_table[cell_index] = m_nodes.size();
+
+ _insert_unsorted(hashkey,value);
+ return GIM_INVALID_HASH;
+
+ }
+
+
+ ///Sorted array data management. The hash table has the indices to the corresponding m_nodes array
+ inline bool _erase_sorted(GUINT index)
+ {
+ if(index>=(GUINT)m_nodes.size()) return false;
+ m_nodes.erase_sorted(index);
+ if(m_nodes.size()<2) m_sorted = false;
+ return true;
+ }
+
+ //! faster, but unsorted
+ inline bool _erase_unsorted(GUINT index)
+ {
+ if(index>=m_nodes.size()) return false;
+
+ GUINT lastindex = m_nodes.size()-1;
+ if(index<lastindex && m_hash_table!=0)
+ {
+ GUINT hashkey = m_nodes[lastindex].m_key;
+ if(hashkey!=GIM_INVALID_HASH)
+ {
+ //update the new position of the last element
+ GUINT cell_index = _find_cell(hashkey);
+ btAssert(cell_index!=GIM_INVALID_HASH);
+ //new position of the last element which will be swaped
+ m_hash_table[cell_index] = index;
+ }
+ }
+ m_nodes.erase(index);
+ m_sorted = false;
+ return true;
+ }
+
+ //! Insert in position ordered
+ /*!
+ Also checks if it is needed to transform this container to a hash table, by calling check_for_switching_to_hashtable
+ */
+ inline void _insert_in_pos(GUINT hashkey, const T & value, GUINT pos)
+ {
+ m_nodes.insert(_node_type(hashkey,value),pos);
+ this->check_for_switching_to_hashtable();
+ }
+
+ //! Insert an element in an ordered array
+ inline GUINT _insert_sorted(GUINT hashkey, const T & value)
+ {
+ if(hashkey==GIM_INVALID_HASH || size()==0)
+ {
+ m_nodes.push_back(_node_type(hashkey,value));
+ return GIM_INVALID_HASH;
+ }
+ //Insert at last position
+ //Sort element
+
+
+ GUINT result_ind=0;
+ GUINT last_index = m_nodes.size()-1;
+ _node_type * ptr = m_nodes.pointer();
+
+ bool found = gim_binary_search_ex(
+ ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO());
+
+
+ //Insert before found index
+ if(found)
+ {
+ return result_ind;
+ }
+ else
+ {
+ _insert_in_pos(hashkey, value, result_ind);
+ }
+ return GIM_INVALID_HASH;
+ }
+
+ inline GUINT _insert_sorted_replace(GUINT hashkey, const T & value)
+ {
+ if(hashkey==GIM_INVALID_HASH || size()==0)
+ {
+ m_nodes.push_back(_node_type(hashkey,value));
+ return GIM_INVALID_HASH;
+ }
+ //Insert at last position
+ //Sort element
+ GUINT result_ind;
+ GUINT last_index = m_nodes.size()-1;
+ _node_type * ptr = m_nodes.pointer();
+
+ bool found = gim_binary_search_ex(
+ ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO());
+
+ //Insert before found index
+ if(found)
+ {
+ m_nodes[result_ind] = _node_type(hashkey,value);
+ }
+ else
+ {
+ _insert_in_pos(hashkey, value, result_ind);
+ }
+ return result_ind;
+ }
+
+ //! Fast insertion in m_nodes array
+ inline GUINT _insert_unsorted(GUINT hashkey, const T & value)
+ {
+ m_nodes.push_back(_node_type(hashkey,value));
+ m_sorted = false;
+ return GIM_INVALID_HASH;
+ }
+
+
+
+public:
+
+ /*!
+ <li> if node_size = 0, then this container becomes a simple sorted array allocator. reserve_size is used for reserve memory in m_nodes.
+ When the array size reaches the size equivalent to 'min_hash_table_size', then it becomes a hash table by calling check_for_switching_to_hashtable.
+ <li> If node_size != 0, then this container becomes a hash table for ever
+ </ul>
+ */
+ gim_hash_table(GUINT reserve_size = GIM_DEFAULT_HASH_TABLE_SIZE,
+ GUINT node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE,
+ GUINT min_hash_table_size = GIM_INVALID_HASH)
+ {
+ m_hash_table = NULL;
+ m_table_size = 0;
+ m_sorted = false;
+ m_node_size = node_size;
+ m_min_hash_table_size = min_hash_table_size;
+
+ if(m_node_size!=0)
+ {
+ if(reserve_size!=0)
+ {
+ m_nodes.reserve(reserve_size);
+ _reserve_table_memory(reserve_size);
+ _invalidate_keys();
+ }
+ else
+ {
+ m_nodes.reserve(GIM_DEFAULT_HASH_TABLE_SIZE);
+ _reserve_table_memory(GIM_DEFAULT_HASH_TABLE_SIZE);
+ _invalidate_keys();
+ }
+ }
+ else if(reserve_size!=0)
+ {
+ m_nodes.reserve(reserve_size);
+ }
+
+ }
+
+ ~gim_hash_table()
+ {
+ _destroy();
+ }
+
+ inline bool is_hash_table()
+ {
+ if(m_hash_table) return true;
+ return false;
+ }
+
+ inline bool is_sorted()
+ {
+ if(size()<2) return true;
+ return m_sorted;
+ }
+
+ bool sort()
+ {
+ if(is_sorted()) return true;
+ if(m_nodes.size()<2) return false;
+
+
+ _node_type * ptr = m_nodes.pointer();
+ GUINT siz = m_nodes.size();
+ gim_sort_hash_node_array(ptr,siz);
+ m_sorted=true;
+
+
+
+ if(m_hash_table)
+ {
+ _rehash();
+ }
+ return true;
+ }
+
+ bool switch_to_hashtable()
+ {
+ if(m_hash_table) return false;
+ if(m_node_size==0) m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE;
+ if(m_nodes.size()<GIM_DEFAULT_HASH_TABLE_SIZE)
+ {
+ _resize_table(GIM_DEFAULT_HASH_TABLE_SIZE);
+ }
+ else
+ {
+ _resize_table(m_nodes.size()+1);
+ }
+
+ return true;
+ }
+
+ bool switch_to_sorted_array()
+ {
+ if(m_hash_table==NULL) return true;
+ _clear_table_memory();
+ return sort();
+ }
+
+ //!If the container reaches the
+ bool check_for_switching_to_hashtable()
+ {
+ if(this->m_hash_table) return true;
+
+ if(!(m_nodes.size()< m_min_hash_table_size))
+ {
+ if(m_node_size == 0)
+ {
+ m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE;
+ }
+
+ _resize_table(m_nodes.size()+1);
+ return true;
+ }
+ return false;
+ }
+
+ inline void set_sorted(bool value)
+ {
+ m_sorted = value;
+ }
+
+ //! Retrieves the amount of keys.
+ inline GUINT size() const
+ {
+ return m_nodes.size();
+ }
+
+ //! Retrieves the hash key.
+ inline GUINT get_key(GUINT index) const
+ {
+ return m_nodes[index].m_key;
+ }
+
+ //! Retrieves the value by index
+ /*!
+ */
+ inline T * get_value_by_index(GUINT index)
+ {
+ return &m_nodes[index].m_data;
+ }
+
+ inline const T& operator[](GUINT index) const
+ {
+ return m_nodes[index].m_data;
+ }
+
+ inline T& operator[](GUINT index)
+ {
+ return m_nodes[index].m_data;
+ }
+
+ //! Finds the index of the element with the key
+ /*!
+ \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted
+ If so, the element has been inserted at the last position of the array.
+ */
+ inline GUINT find(GUINT hashkey)
+ {
+ if(m_hash_table)
+ {
+ GUINT cell_index = _find_cell(hashkey);
+ if(cell_index==GIM_INVALID_HASH) return GIM_INVALID_HASH;
+ return m_hash_table[cell_index];
+ }
+ GUINT last_index = m_nodes.size();
+ if(last_index<2)
+ {
+ if(last_index==0) return GIM_INVALID_HASH;
+ if(m_nodes[0].m_key == hashkey) return 0;
+ return GIM_INVALID_HASH;
+ }
+ else if(m_sorted)
+ {
+ //Binary search
+ GUINT result_ind = 0;
+ last_index--;
+ _node_type * ptr = m_nodes.pointer();
+
+ bool found = gim_binary_search_ex(ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO());
+
+
+ if(found) return result_ind;
+ }
+ return GIM_INVALID_HASH;
+ }
+
+ //! Retrieves the value associated with the index
+ /*!
+ \return the found element, or null
+ */
+ inline T * get_value(GUINT hashkey)
+ {
+ GUINT index = find(hashkey);
+ if(index == GIM_INVALID_HASH) return NULL;
+ return &m_nodes[index].m_data;
+ }
+
+
+ /*!
+ */
+ inline bool erase_by_index(GUINT index)
+ {
+ if(index > m_nodes.size()) return false;
+
+ if(m_hash_table == NULL)
+ {
+ if(is_sorted())
+ {
+ return this->_erase_sorted(index);
+ }
+ else
+ {
+ return this->_erase_unsorted(index);
+ }
+ }
+ else
+ {
+ return this->_erase_by_index_hash_table(index);
+ }
+ return false;
+ }
+
+
+
+ inline bool erase_by_index_unsorted(GUINT index)
+ {
+ if(index > m_nodes.size()) return false;
+
+ if(m_hash_table == NULL)
+ {
+ return this->_erase_unsorted(index);
+ }
+ else
+ {
+ return this->_erase_by_index_hash_table(index);
+ }
+ return false;
+ }
+
+
+
+ /*!
+
+ */
+ inline bool erase_by_key(GUINT hashkey)
+ {
+ if(size()==0) return false;
+
+ if(m_hash_table)
+ {
+ return this->_erase_hash_table(hashkey);
+ }
+ //Binary search
+
+ if(is_sorted()==false) return false;
+
+ GUINT result_ind = find(hashkey);
+ if(result_ind!= GIM_INVALID_HASH)
+ {
+ return this->_erase_sorted(result_ind);
+ }
+ return false;
+ }
+
+ void clear()
+ {
+ m_nodes.clear();
+
+ if(m_hash_table==NULL) return;
+ GUINT datasize = m_table_size*m_node_size;
+ //Initialize the hashkeys.
+ GUINT i;
+ for(i=0;i<datasize;i++)
+ {
+ m_hash_table[i] = GIM_INVALID_HASH;// invalidate keys
+ }
+ m_sorted = false;
+ }
+
+ //! Insert an element into the hash
+ /*!
+ \return If GIM_INVALID_HASH, the object has been inserted succesfully. Else it returns the position
+ of the existing element.
+ */
+ inline GUINT insert(GUINT hashkey, const T & element)
+ {
+ if(m_hash_table)
+ {
+ return this->_insert_hash_table(hashkey,element);
+ }
+ if(this->is_sorted())
+ {
+ return this->_insert_sorted(hashkey,element);
+ }
+ return this->_insert_unsorted(hashkey,element);
+ }
+
+ //! Insert an element into the hash, and could overrite an existing object with the same hash.
+ /*!
+ \return If GIM_INVALID_HASH, the object has been inserted succesfully. Else it returns the position
+ of the replaced element.
+ */
+ inline GUINT insert_override(GUINT hashkey, const T & element)
+ {
+ if(m_hash_table)
+ {
+ return this->_insert_hash_table_replace(hashkey,element);
+ }
+ if(this->is_sorted())
+ {
+ return this->_insert_sorted_replace(hashkey,element);
+ }
+ this->_insert_unsorted(hashkey,element);
+ return m_nodes.size();
+ }
+
+
+
+ //! Insert an element into the hash,But if this container is a sorted array, this inserts it unsorted
+ /*!
+ */
+ inline GUINT insert_unsorted(GUINT hashkey,const T & element)
+ {
+ if(m_hash_table)
+ {
+ return this->_insert_hash_table(hashkey,element);
+ }
+ return this->_insert_unsorted(hashkey,element);
+ }
+
+
+};
+
+
+
+#endif // GIM_CONTAINERS_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_linear_math.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_linear_math.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_linear_math.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_linear_math.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,1573 @@
+#ifndef GIM_LINEAR_H_INCLUDED
+#define GIM_LINEAR_H_INCLUDED
+
+/*! \file gim_linear_math.h
+*\author Francisco Len Nßjera
+Type Independant Vector and matrix operations.
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at 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.
+ (3) The zlib/libpng license that is included with this library in
+ the file GIMPACT-LICENSE-ZLIB.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, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "gim_math.h"
+#include "gim_geom_types.h"
+
+
+
+
+//! Zero out a 2D vector
+#define VEC_ZERO_2(a) \
+{ \
+ (a)[0] = (a)[1] = 0.0f; \
+}\
+
+
+//! Zero out a 3D vector
+#define VEC_ZERO(a) \
+{ \
+ (a)[0] = (a)[1] = (a)[2] = 0.0f; \
+}\
+
+
+/// Zero out a 4D vector
+#define VEC_ZERO_4(a) \
+{ \
+ (a)[0] = (a)[1] = (a)[2] = (a)[3] = 0.0f; \
+}\
+
+
+/// Vector copy
+#define VEC_COPY_2(b,a) \
+{ \
+ (b)[0] = (a)[0]; \
+ (b)[1] = (a)[1]; \
+}\
+
+
+/// Copy 3D vector
+#define VEC_COPY(b,a) \
+{ \
+ (b)[0] = (a)[0]; \
+ (b)[1] = (a)[1]; \
+ (b)[2] = (a)[2]; \
+}\
+
+
+/// Copy 4D vector
+#define VEC_COPY_4(b,a) \
+{ \
+ (b)[0] = (a)[0]; \
+ (b)[1] = (a)[1]; \
+ (b)[2] = (a)[2]; \
+ (b)[3] = (a)[3]; \
+}\
+
+/// VECTOR SWAP
+#define VEC_SWAP(b,a) \
+{ \
+ GIM_SWAP_NUMBERS((b)[0],(a)[0]);\
+ GIM_SWAP_NUMBERS((b)[1],(a)[1]);\
+ GIM_SWAP_NUMBERS((b)[2],(a)[2]);\
+}\
+
+/// Vector difference
+#define VEC_DIFF_2(v21,v2,v1) \
+{ \
+ (v21)[0] = (v2)[0] - (v1)[0]; \
+ (v21)[1] = (v2)[1] - (v1)[1]; \
+}\
+
+
+/// Vector difference
+#define VEC_DIFF(v21,v2,v1) \
+{ \
+ (v21)[0] = (v2)[0] - (v1)[0]; \
+ (v21)[1] = (v2)[1] - (v1)[1]; \
+ (v21)[2] = (v2)[2] - (v1)[2]; \
+}\
+
+
+/// Vector difference
+#define VEC_DIFF_4(v21,v2,v1) \
+{ \
+ (v21)[0] = (v2)[0] - (v1)[0]; \
+ (v21)[1] = (v2)[1] - (v1)[1]; \
+ (v21)[2] = (v2)[2] - (v1)[2]; \
+ (v21)[3] = (v2)[3] - (v1)[3]; \
+}\
+
+
+/// Vector sum
+#define VEC_SUM_2(v21,v2,v1) \
+{ \
+ (v21)[0] = (v2)[0] + (v1)[0]; \
+ (v21)[1] = (v2)[1] + (v1)[1]; \
+}\
+
+
+/// Vector sum
+#define VEC_SUM(v21,v2,v1) \
+{ \
+ (v21)[0] = (v2)[0] + (v1)[0]; \
+ (v21)[1] = (v2)[1] + (v1)[1]; \
+ (v21)[2] = (v2)[2] + (v1)[2]; \
+}\
+
+
+/// Vector sum
+#define VEC_SUM_4(v21,v2,v1) \
+{ \
+ (v21)[0] = (v2)[0] + (v1)[0]; \
+ (v21)[1] = (v2)[1] + (v1)[1]; \
+ (v21)[2] = (v2)[2] + (v1)[2]; \
+ (v21)[3] = (v2)[3] + (v1)[3]; \
+}\
+
+
+/// scalar times vector
+#define VEC_SCALE_2(c,a,b) \
+{ \
+ (c)[0] = (a)*(b)[0]; \
+ (c)[1] = (a)*(b)[1]; \
+}\
+
+
+/// scalar times vector
+#define VEC_SCALE(c,a,b) \
+{ \
+ (c)[0] = (a)*(b)[0]; \
+ (c)[1] = (a)*(b)[1]; \
+ (c)[2] = (a)*(b)[2]; \
+}\
+
+
+/// scalar times vector
+#define VEC_SCALE_4(c,a,b) \
+{ \
+ (c)[0] = (a)*(b)[0]; \
+ (c)[1] = (a)*(b)[1]; \
+ (c)[2] = (a)*(b)[2]; \
+ (c)[3] = (a)*(b)[3]; \
+}\
+
+
+/// accumulate scaled vector
+#define VEC_ACCUM_2(c,a,b) \
+{ \
+ (c)[0] += (a)*(b)[0]; \
+ (c)[1] += (a)*(b)[1]; \
+}\
+
+
+/// accumulate scaled vector
+#define VEC_ACCUM(c,a,b) \
+{ \
+ (c)[0] += (a)*(b)[0]; \
+ (c)[1] += (a)*(b)[1]; \
+ (c)[2] += (a)*(b)[2]; \
+}\
+
+
+/// accumulate scaled vector
+#define VEC_ACCUM_4(c,a,b) \
+{ \
+ (c)[0] += (a)*(b)[0]; \
+ (c)[1] += (a)*(b)[1]; \
+ (c)[2] += (a)*(b)[2]; \
+ (c)[3] += (a)*(b)[3]; \
+}\
+
+
+/// Vector dot product
+#define VEC_DOT_2(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1])
+
+
+/// Vector dot product
+#define VEC_DOT(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2])
+
+/// Vector dot product
+#define VEC_DOT_4(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2] + (a)[3]*(b)[3])
+
+/// vector impact parameter (squared)
+#define VEC_IMPACT_SQ(bsq,direction,position) {\
+ GREAL _llel_ = VEC_DOT(direction, position);\
+ bsq = VEC_DOT(position, position) - _llel_*_llel_;\
+}\
+
+
+/// vector impact parameter
+#define VEC_IMPACT(bsq,direction,position) {\
+ VEC_IMPACT_SQ(bsq,direction,position); \
+ GIM_SQRT(bsq,bsq); \
+}\
+
+/// Vector length
+#define VEC_LENGTH_2(a,l)\
+{\
+ GREAL _pp = VEC_DOT_2(a,a);\
+ GIM_SQRT(_pp,l);\
+}\
+
+
+/// Vector length
+#define VEC_LENGTH(a,l)\
+{\
+ GREAL _pp = VEC_DOT(a,a);\
+ GIM_SQRT(_pp,l);\
+}\
+
+
+/// Vector length
+#define VEC_LENGTH_4(a,l)\
+{\
+ GREAL _pp = VEC_DOT_4(a,a);\
+ GIM_SQRT(_pp,l);\
+}\
+
+/// Vector inv length
+#define VEC_INV_LENGTH_2(a,l)\
+{\
+ GREAL _pp = VEC_DOT_2(a,a);\
+ GIM_INV_SQRT(_pp,l);\
+}\
+
+
+/// Vector inv length
+#define VEC_INV_LENGTH(a,l)\
+{\
+ GREAL _pp = VEC_DOT(a,a);\
+ GIM_INV_SQRT(_pp,l);\
+}\
+
+
+/// Vector inv length
+#define VEC_INV_LENGTH_4(a,l)\
+{\
+ GREAL _pp = VEC_DOT_4(a,a);\
+ GIM_INV_SQRT(_pp,l);\
+}\
+
+
+
+/// distance between two points
+#define VEC_DISTANCE(_len,_va,_vb) {\
+ vec3f _tmp_; \
+ VEC_DIFF(_tmp_, _vb, _va); \
+ VEC_LENGTH(_tmp_,_len); \
+}\
+
+
+/// Vector length
+#define VEC_CONJUGATE_LENGTH(a,l)\
+{\
+ GREAL _pp = 1.0 - a[0]*a[0] - a[1]*a[1] - a[2]*a[2];\
+ GIM_SQRT(_pp,l);\
+}\
+
+
+/// Vector length
+#define VEC_NORMALIZE(a) { \
+ GREAL len;\
+ VEC_INV_LENGTH(a,len); \
+ if(len<G_REAL_INFINITY)\
+ {\
+ a[0] *= len; \
+ a[1] *= len; \
+ a[2] *= len; \
+ } \
+}\
+
+/// Set Vector size
+#define VEC_RENORMALIZE(a,newlen) { \
+ GREAL len;\
+ VEC_INV_LENGTH(a,len); \
+ if(len<G_REAL_INFINITY)\
+ {\
+ len *= newlen;\
+ a[0] *= len; \
+ a[1] *= len; \
+ a[2] *= len; \
+ } \
+}\
+
+/// Vector cross
+#define VEC_CROSS(c,a,b) \
+{ \
+ c[0] = (a)[1] * (b)[2] - (a)[2] * (b)[1]; \
+ c[1] = (a)[2] * (b)[0] - (a)[0] * (b)[2]; \
+ c[2] = (a)[0] * (b)[1] - (a)[1] * (b)[0]; \
+}\
+
+
+/*! Vector perp -- assumes that n is of unit length
+ * accepts vector v, subtracts out any component parallel to n */
+#define VEC_PERPENDICULAR(vp,v,n) \
+{ \
+ GREAL dot = VEC_DOT(v, n); \
+ vp[0] = (v)[0] - dot*(n)[0]; \
+ vp[1] = (v)[1] - dot*(n)[1]; \
+ vp[2] = (v)[2] - dot*(n)[2]; \
+}\
+
+
+/*! Vector parallel -- assumes that n is of unit length */
+#define VEC_PARALLEL(vp,v,n) \
+{ \
+ GREAL dot = VEC_DOT(v, n); \
+ vp[0] = (dot) * (n)[0]; \
+ vp[1] = (dot) * (n)[1]; \
+ vp[2] = (dot) * (n)[2]; \
+}\
+
+/*! Same as Vector parallel -- n can have any length
+ * accepts vector v, subtracts out any component perpendicular to n */
+#define VEC_PROJECT(vp,v,n) \
+{ \
+ GREAL scalar = VEC_DOT(v, n); \
+ scalar/= VEC_DOT(n, n); \
+ vp[0] = (scalar) * (n)[0]; \
+ vp[1] = (scalar) * (n)[1]; \
+ vp[2] = (scalar) * (n)[2]; \
+}\
+
+
+/*! accepts vector v*/
+#define VEC_UNPROJECT(vp,v,n) \
+{ \
+ GREAL scalar = VEC_DOT(v, n); \
+ scalar = VEC_DOT(n, n)/scalar; \
+ vp[0] = (scalar) * (n)[0]; \
+ vp[1] = (scalar) * (n)[1]; \
+ vp[2] = (scalar) * (n)[2]; \
+}\
+
+
+/*! Vector reflection -- assumes n is of unit length
+ Takes vector v, reflects it against reflector n, and returns vr */
+#define VEC_REFLECT(vr,v,n) \
+{ \
+ GREAL dot = VEC_DOT(v, n); \
+ vr[0] = (v)[0] - 2.0 * (dot) * (n)[0]; \
+ vr[1] = (v)[1] - 2.0 * (dot) * (n)[1]; \
+ vr[2] = (v)[2] - 2.0 * (dot) * (n)[2]; \
+}\
+
+
+/*! Vector blending
+Takes two vectors a, b, blends them together with two scalars */
+#define VEC_BLEND_AB(vr,sa,a,sb,b) \
+{ \
+ vr[0] = (sa) * (a)[0] + (sb) * (b)[0]; \
+ vr[1] = (sa) * (a)[1] + (sb) * (b)[1]; \
+ vr[2] = (sa) * (a)[2] + (sb) * (b)[2]; \
+}\
+
+/*! Vector blending
+Takes two vectors a, b, blends them together with s <=1 */
+#define VEC_BLEND(vr,a,b,s) VEC_BLEND_AB(vr,(1-s),a,s,b)
+
+#define VEC_SET3(a,b,op,c) a[0]=b[0] op c[0]; a[1]=b[1] op c[1]; a[2]=b[2] op c[2];
+
+//! Finds the bigger cartesian coordinate from a vector
+#define VEC_MAYOR_COORD(vec, maxc)\
+{\
+ GREAL A[] = {fabs(vec[0]),fabs(vec[1]),fabs(vec[2])};\
+ maxc = A[0]>A[1]?(A[0]>A[2]?0:2):(A[1]>A[2]?1:2);\
+}\
+
+//! Finds the 2 smallest cartesian coordinates from a vector
+#define VEC_MINOR_AXES(vec, i0, i1)\
+{\
+ VEC_MAYOR_COORD(vec,i0);\
+ i0 = (i0+1)%3;\
+ i1 = (i0+1)%3;\
+}\
+
+
+
+
+#define VEC_EQUAL(v1,v2) (v1[0]==v2[0]&&v1[1]==v2[1]&&v1[2]==v2[2])
+
+#define VEC_NEAR_EQUAL(v1,v2) (GIM_NEAR_EQUAL(v1[0],v2[0])&&GIM_NEAR_EQUAL(v1[1],v2[1])&&GIM_NEAR_EQUAL(v1[2],v2[2]))
+
+
+/// Vector cross
+#define X_AXIS_CROSS_VEC(dst,src)\
+{ \
+ dst[0] = 0.0f; \
+ dst[1] = -src[2]; \
+ dst[2] = src[1]; \
+}\
+
+#define Y_AXIS_CROSS_VEC(dst,src)\
+{ \
+ dst[0] = src[2]; \
+ dst[1] = 0.0f; \
+ dst[2] = -src[0]; \
+}\
+
+#define Z_AXIS_CROSS_VEC(dst,src)\
+{ \
+ dst[0] = -src[1]; \
+ dst[1] = src[0]; \
+ dst[2] = 0.0f; \
+}\
+
+
+
+
+
+
+/// initialize matrix
+#define IDENTIFY_MATRIX_3X3(m) \
+{ \
+ m[0][0] = 1.0; \
+ m[0][1] = 0.0; \
+ m[0][2] = 0.0; \
+ \
+ m[1][0] = 0.0; \
+ m[1][1] = 1.0; \
+ m[1][2] = 0.0; \
+ \
+ m[2][0] = 0.0; \
+ m[2][1] = 0.0; \
+ m[2][2] = 1.0; \
+}\
+
+/*! initialize matrix */
+#define IDENTIFY_MATRIX_4X4(m) \
+{ \
+ m[0][0] = 1.0; \
+ m[0][1] = 0.0; \
+ m[0][2] = 0.0; \
+ m[0][3] = 0.0; \
+ \
+ m[1][0] = 0.0; \
+ m[1][1] = 1.0; \
+ m[1][2] = 0.0; \
+ m[1][3] = 0.0; \
+ \
+ m[2][0] = 0.0; \
+ m[2][1] = 0.0; \
+ m[2][2] = 1.0; \
+ m[2][3] = 0.0; \
+ \
+ m[3][0] = 0.0; \
+ m[3][1] = 0.0; \
+ m[3][2] = 0.0; \
+ m[3][3] = 1.0; \
+}\
+
+/*! initialize matrix */
+#define ZERO_MATRIX_4X4(m) \
+{ \
+ m[0][0] = 0.0; \
+ m[0][1] = 0.0; \
+ m[0][2] = 0.0; \
+ m[0][3] = 0.0; \
+ \
+ m[1][0] = 0.0; \
+ m[1][1] = 0.0; \
+ m[1][2] = 0.0; \
+ m[1][3] = 0.0; \
+ \
+ m[2][0] = 0.0; \
+ m[2][1] = 0.0; \
+ m[2][2] = 0.0; \
+ m[2][3] = 0.0; \
+ \
+ m[3][0] = 0.0; \
+ m[3][1] = 0.0; \
+ m[3][2] = 0.0; \
+ m[3][3] = 0.0; \
+}\
+
+/*! matrix rotation X */
+#define ROTX_CS(m,cosine,sine) \
+{ \
+ /* rotation about the x-axis */ \
+ \
+ m[0][0] = 1.0; \
+ m[0][1] = 0.0; \
+ m[0][2] = 0.0; \
+ m[0][3] = 0.0; \
+ \
+ m[1][0] = 0.0; \
+ m[1][1] = (cosine); \
+ m[1][2] = (sine); \
+ m[1][3] = 0.0; \
+ \
+ m[2][0] = 0.0; \
+ m[2][1] = -(sine); \
+ m[2][2] = (cosine); \
+ m[2][3] = 0.0; \
+ \
+ m[3][0] = 0.0; \
+ m[3][1] = 0.0; \
+ m[3][2] = 0.0; \
+ m[3][3] = 1.0; \
+}\
+
+/*! matrix rotation Y */
+#define ROTY_CS(m,cosine,sine) \
+{ \
+ /* rotation about the y-axis */ \
+ \
+ m[0][0] = (cosine); \
+ m[0][1] = 0.0; \
+ m[0][2] = -(sine); \
+ m[0][3] = 0.0; \
+ \
+ m[1][0] = 0.0; \
+ m[1][1] = 1.0; \
+ m[1][2] = 0.0; \
+ m[1][3] = 0.0; \
+ \
+ m[2][0] = (sine); \
+ m[2][1] = 0.0; \
+ m[2][2] = (cosine); \
+ m[2][3] = 0.0; \
+ \
+ m[3][0] = 0.0; \
+ m[3][1] = 0.0; \
+ m[3][2] = 0.0; \
+ m[3][3] = 1.0; \
+}\
+
+/*! matrix rotation Z */
+#define ROTZ_CS(m,cosine,sine) \
+{ \
+ /* rotation about the z-axis */ \
+ \
+ m[0][0] = (cosine); \
+ m[0][1] = (sine); \
+ m[0][2] = 0.0; \
+ m[0][3] = 0.0; \
+ \
+ m[1][0] = -(sine); \
+ m[1][1] = (cosine); \
+ m[1][2] = 0.0; \
+ m[1][3] = 0.0; \
+ \
+ m[2][0] = 0.0; \
+ m[2][1] = 0.0; \
+ m[2][2] = 1.0; \
+ m[2][3] = 0.0; \
+ \
+ m[3][0] = 0.0; \
+ m[3][1] = 0.0; \
+ m[3][2] = 0.0; \
+ m[3][3] = 1.0; \
+}\
+
+/*! matrix copy */
+#define COPY_MATRIX_2X2(b,a) \
+{ \
+ b[0][0] = a[0][0]; \
+ b[0][1] = a[0][1]; \
+ \
+ b[1][0] = a[1][0]; \
+ b[1][1] = a[1][1]; \
+ \
+}\
+
+
+/*! matrix copy */
+#define COPY_MATRIX_2X3(b,a) \
+{ \
+ b[0][0] = a[0][0]; \
+ b[0][1] = a[0][1]; \
+ b[0][2] = a[0][2]; \
+ \
+ b[1][0] = a[1][0]; \
+ b[1][1] = a[1][1]; \
+ b[1][2] = a[1][2]; \
+}\
+
+
+/*! matrix copy */
+#define COPY_MATRIX_3X3(b,a) \
+{ \
+ b[0][0] = a[0][0]; \
+ b[0][1] = a[0][1]; \
+ b[0][2] = a[0][2]; \
+ \
+ b[1][0] = a[1][0]; \
+ b[1][1] = a[1][1]; \
+ b[1][2] = a[1][2]; \
+ \
+ b[2][0] = a[2][0]; \
+ b[2][1] = a[2][1]; \
+ b[2][2] = a[2][2]; \
+}\
+
+
+/*! matrix copy */
+#define COPY_MATRIX_4X4(b,a) \
+{ \
+ b[0][0] = a[0][0]; \
+ b[0][1] = a[0][1]; \
+ b[0][2] = a[0][2]; \
+ b[0][3] = a[0][3]; \
+ \
+ b[1][0] = a[1][0]; \
+ b[1][1] = a[1][1]; \
+ b[1][2] = a[1][2]; \
+ b[1][3] = a[1][3]; \
+ \
+ b[2][0] = a[2][0]; \
+ b[2][1] = a[2][1]; \
+ b[2][2] = a[2][2]; \
+ b[2][3] = a[2][3]; \
+ \
+ b[3][0] = a[3][0]; \
+ b[3][1] = a[3][1]; \
+ b[3][2] = a[3][2]; \
+ b[3][3] = a[3][3]; \
+}\
+
+
+/*! matrix transpose */
+#define TRANSPOSE_MATRIX_2X2(b,a) \
+{ \
+ b[0][0] = a[0][0]; \
+ b[0][1] = a[1][0]; \
+ \
+ b[1][0] = a[0][1]; \
+ b[1][1] = a[1][1]; \
+}\
+
+
+/*! matrix transpose */
+#define TRANSPOSE_MATRIX_3X3(b,a) \
+{ \
+ b[0][0] = a[0][0]; \
+ b[0][1] = a[1][0]; \
+ b[0][2] = a[2][0]; \
+ \
+ b[1][0] = a[0][1]; \
+ b[1][1] = a[1][1]; \
+ b[1][2] = a[2][1]; \
+ \
+ b[2][0] = a[0][2]; \
+ b[2][1] = a[1][2]; \
+ b[2][2] = a[2][2]; \
+}\
+
+
+/*! matrix transpose */
+#define TRANSPOSE_MATRIX_4X4(b,a) \
+{ \
+ b[0][0] = a[0][0]; \
+ b[0][1] = a[1][0]; \
+ b[0][2] = a[2][0]; \
+ b[0][3] = a[3][0]; \
+ \
+ b[1][0] = a[0][1]; \
+ b[1][1] = a[1][1]; \
+ b[1][2] = a[2][1]; \
+ b[1][3] = a[3][1]; \
+ \
+ b[2][0] = a[0][2]; \
+ b[2][1] = a[1][2]; \
+ b[2][2] = a[2][2]; \
+ b[2][3] = a[3][2]; \
+ \
+ b[3][0] = a[0][3]; \
+ b[3][1] = a[1][3]; \
+ b[3][2] = a[2][3]; \
+ b[3][3] = a[3][3]; \
+}\
+
+
+/*! multiply matrix by scalar */
+#define SCALE_MATRIX_2X2(b,s,a) \
+{ \
+ b[0][0] = (s) * a[0][0]; \
+ b[0][1] = (s) * a[0][1]; \
+ \
+ b[1][0] = (s) * a[1][0]; \
+ b[1][1] = (s) * a[1][1]; \
+}\
+
+
+/*! multiply matrix by scalar */
+#define SCALE_MATRIX_3X3(b,s,a) \
+{ \
+ b[0][0] = (s) * a[0][0]; \
+ b[0][1] = (s) * a[0][1]; \
+ b[0][2] = (s) * a[0][2]; \
+ \
+ b[1][0] = (s) * a[1][0]; \
+ b[1][1] = (s) * a[1][1]; \
+ b[1][2] = (s) * a[1][2]; \
+ \
+ b[2][0] = (s) * a[2][0]; \
+ b[2][1] = (s) * a[2][1]; \
+ b[2][2] = (s) * a[2][2]; \
+}\
+
+
+/*! multiply matrix by scalar */
+#define SCALE_MATRIX_4X4(b,s,a) \
+{ \
+ b[0][0] = (s) * a[0][0]; \
+ b[0][1] = (s) * a[0][1]; \
+ b[0][2] = (s) * a[0][2]; \
+ b[0][3] = (s) * a[0][3]; \
+ \
+ b[1][0] = (s) * a[1][0]; \
+ b[1][1] = (s) * a[1][1]; \
+ b[1][2] = (s) * a[1][2]; \
+ b[1][3] = (s) * a[1][3]; \
+ \
+ b[2][0] = (s) * a[2][0]; \
+ b[2][1] = (s) * a[2][1]; \
+ b[2][2] = (s) * a[2][2]; \
+ b[2][3] = (s) * a[2][3]; \
+ \
+ b[3][0] = s * a[3][0]; \
+ b[3][1] = s * a[3][1]; \
+ b[3][2] = s * a[3][2]; \
+ b[3][3] = s * a[3][3]; \
+}\
+
+
+/*! multiply matrix by scalar */
+#define SCALE_VEC_MATRIX_2X2(b,svec,a) \
+{ \
+ b[0][0] = svec[0] * a[0][0]; \
+ b[1][0] = svec[0] * a[1][0]; \
+ \
+ b[0][1] = svec[1] * a[0][1]; \
+ b[1][1] = svec[1] * a[1][1]; \
+}\
+
+
+/*! multiply matrix by scalar. Each columns is scaled by each scalar vector component */
+#define SCALE_VEC_MATRIX_3X3(b,svec,a) \
+{ \
+ b[0][0] = svec[0] * a[0][0]; \
+ b[1][0] = svec[0] * a[1][0]; \
+ b[2][0] = svec[0] * a[2][0]; \
+ \
+ b[0][1] = svec[1] * a[0][1]; \
+ b[1][1] = svec[1] * a[1][1]; \
+ b[2][1] = svec[1] * a[2][1]; \
+ \
+ b[0][2] = svec[2] * a[0][2]; \
+ b[1][2] = svec[2] * a[1][2]; \
+ b[2][2] = svec[2] * a[2][2]; \
+}\
+
+
+/*! multiply matrix by scalar */
+#define SCALE_VEC_MATRIX_4X4(b,svec,a) \
+{ \
+ b[0][0] = svec[0] * a[0][0]; \
+ b[1][0] = svec[0] * a[1][0]; \
+ b[2][0] = svec[0] * a[2][0]; \
+ b[3][0] = svec[0] * a[3][0]; \
+ \
+ b[0][1] = svec[1] * a[0][1]; \
+ b[1][1] = svec[1] * a[1][1]; \
+ b[2][1] = svec[1] * a[2][1]; \
+ b[3][1] = svec[1] * a[3][1]; \
+ \
+ b[0][2] = svec[2] * a[0][2]; \
+ b[1][2] = svec[2] * a[1][2]; \
+ b[2][2] = svec[2] * a[2][2]; \
+ b[3][2] = svec[2] * a[3][2]; \
+ \
+ b[0][3] = svec[3] * a[0][3]; \
+ b[1][3] = svec[3] * a[1][3]; \
+ b[2][3] = svec[3] * a[2][3]; \
+ b[3][3] = svec[3] * a[3][3]; \
+}\
+
+
+/*! multiply matrix by scalar */
+#define ACCUM_SCALE_MATRIX_2X2(b,s,a) \
+{ \
+ b[0][0] += (s) * a[0][0]; \
+ b[0][1] += (s) * a[0][1]; \
+ \
+ b[1][0] += (s) * a[1][0]; \
+ b[1][1] += (s) * a[1][1]; \
+}\
+
+
+/*! multiply matrix by scalar */
+#define ACCUM_SCALE_MATRIX_3X3(b,s,a) \
+{ \
+ b[0][0] += (s) * a[0][0]; \
+ b[0][1] += (s) * a[0][1]; \
+ b[0][2] += (s) * a[0][2]; \
+ \
+ b[1][0] += (s) * a[1][0]; \
+ b[1][1] += (s) * a[1][1]; \
+ b[1][2] += (s) * a[1][2]; \
+ \
+ b[2][0] += (s) * a[2][0]; \
+ b[2][1] += (s) * a[2][1]; \
+ b[2][2] += (s) * a[2][2]; \
+}\
+
+
+/*! multiply matrix by scalar */
+#define ACCUM_SCALE_MATRIX_4X4(b,s,a) \
+{ \
+ b[0][0] += (s) * a[0][0]; \
+ b[0][1] += (s) * a[0][1]; \
+ b[0][2] += (s) * a[0][2]; \
+ b[0][3] += (s) * a[0][3]; \
+ \
+ b[1][0] += (s) * a[1][0]; \
+ b[1][1] += (s) * a[1][1]; \
+ b[1][2] += (s) * a[1][2]; \
+ b[1][3] += (s) * a[1][3]; \
+ \
+ b[2][0] += (s) * a[2][0]; \
+ b[2][1] += (s) * a[2][1]; \
+ b[2][2] += (s) * a[2][2]; \
+ b[2][3] += (s) * a[2][3]; \
+ \
+ b[3][0] += (s) * a[3][0]; \
+ b[3][1] += (s) * a[3][1]; \
+ b[3][2] += (s) * a[3][2]; \
+ b[3][3] += (s) * a[3][3]; \
+}\
+
+/*! matrix product */
+/*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/
+#define MATRIX_PRODUCT_2X2(c,a,b) \
+{ \
+ c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]; \
+ c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]; \
+ \
+ c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]; \
+ c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]; \
+ \
+}\
+
+/*! matrix product */
+/*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/
+#define MATRIX_PRODUCT_3X3(c,a,b) \
+{ \
+ c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]; \
+ c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]; \
+ c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]; \
+ \
+ c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]; \
+ c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]; \
+ c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]; \
+ \
+ c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]; \
+ c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]; \
+ c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]; \
+}\
+
+
+/*! matrix product */
+/*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/
+#define MATRIX_PRODUCT_4X4(c,a,b) \
+{ \
+ c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]+a[0][3]*b[3][0];\
+ c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]+a[0][3]*b[3][1];\
+ c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]+a[0][3]*b[3][2];\
+ c[0][3] = a[0][0]*b[0][3]+a[0][1]*b[1][3]+a[0][2]*b[2][3]+a[0][3]*b[3][3];\
+ \
+ c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]+a[1][3]*b[3][0];\
+ c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]+a[1][3]*b[3][1];\
+ c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]+a[1][3]*b[3][2];\
+ c[1][3] = a[1][0]*b[0][3]+a[1][1]*b[1][3]+a[1][2]*b[2][3]+a[1][3]*b[3][3];\
+ \
+ c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]+a[2][3]*b[3][0];\
+ c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]+a[2][3]*b[3][1];\
+ c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]+a[2][3]*b[3][2];\
+ c[2][3] = a[2][0]*b[0][3]+a[2][1]*b[1][3]+a[2][2]*b[2][3]+a[2][3]*b[3][3];\
+ \
+ c[3][0] = a[3][0]*b[0][0]+a[3][1]*b[1][0]+a[3][2]*b[2][0]+a[3][3]*b[3][0];\
+ c[3][1] = a[3][0]*b[0][1]+a[3][1]*b[1][1]+a[3][2]*b[2][1]+a[3][3]*b[3][1];\
+ c[3][2] = a[3][0]*b[0][2]+a[3][1]*b[1][2]+a[3][2]*b[2][2]+a[3][3]*b[3][2];\
+ c[3][3] = a[3][0]*b[0][3]+a[3][1]*b[1][3]+a[3][2]*b[2][3]+a[3][3]*b[3][3];\
+}\
+
+
+/*! matrix times vector */
+#define MAT_DOT_VEC_2X2(p,m,v) \
+{ \
+ p[0] = m[0][0]*v[0] + m[0][1]*v[1]; \
+ p[1] = m[1][0]*v[0] + m[1][1]*v[1]; \
+}\
+
+
+/*! matrix times vector */
+#define MAT_DOT_VEC_3X3(p,m,v) \
+{ \
+ p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2]; \
+ p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2]; \
+ p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2]; \
+}\
+
+
+/*! matrix times vector
+v is a vec4f
+*/
+#define MAT_DOT_VEC_4X4(p,m,v) \
+{ \
+ p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]*v[3]; \
+ p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]*v[3]; \
+ p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]*v[3]; \
+ p[3] = m[3][0]*v[0] + m[3][1]*v[1] + m[3][2]*v[2] + m[3][3]*v[3]; \
+}\
+
+/*! matrix times vector
+v is a vec3f
+and m is a mat4f<br>
+Last column is added as the position
+*/
+#define MAT_DOT_VEC_3X4(p,m,v) \
+{ \
+ p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]; \
+ p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]; \
+ p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]; \
+}\
+
+
+/*! vector transpose times matrix */
+/*! p[j] = v[0]*m[0][j] + v[1]*m[1][j] + v[2]*m[2][j]; */
+#define VEC_DOT_MAT_3X3(p,v,m) \
+{ \
+ p[0] = v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0]; \
+ p[1] = v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1]; \
+ p[2] = v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2]; \
+}\
+
+
+/*! affine matrix times vector */
+/** The matrix is assumed to be an affine matrix, with last two
+ * entries representing a translation */
+#define MAT_DOT_VEC_2X3(p,m,v) \
+{ \
+ p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]; \
+ p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]; \
+}\
+
+//! Transform a plane
+#define MAT_TRANSFORM_PLANE_4X4(pout,m,plane)\
+{ \
+ pout[0] = m[0][0]*plane[0] + m[0][1]*plane[1] + m[0][2]*plane[2];\
+ pout[1] = m[1][0]*plane[0] + m[1][1]*plane[1] + m[1][2]*plane[2];\
+ pout[2] = m[2][0]*plane[0] + m[2][1]*plane[1] + m[2][2]*plane[2];\
+ pout[3] = m[0][3]*pout[0] + m[1][3]*pout[1] + m[2][3]*pout[2] + plane[3];\
+}\
+
+
+
+/** inverse transpose of matrix times vector
+ *
+ * This macro computes inverse transpose of matrix m,
+ * and multiplies vector v into it, to yeild vector p
+ *
+ * DANGER !!! Do Not use this on normal vectors!!!
+ * It will leave normals the wrong length !!!
+ * See macro below for use on normals.
+ */
+#define INV_TRANSP_MAT_DOT_VEC_2X2(p,m,v) \
+{ \
+ GREAL det; \
+ \
+ det = m[0][0]*m[1][1] - m[0][1]*m[1][0]; \
+ p[0] = m[1][1]*v[0] - m[1][0]*v[1]; \
+ p[1] = - m[0][1]*v[0] + m[0][0]*v[1]; \
+ \
+ /* if matrix not singular, and not orthonormal, then renormalize */ \
+ if ((det!=1.0f) && (det != 0.0f)) { \
+ det = 1.0f / det; \
+ p[0] *= det; \
+ p[1] *= det; \
+ } \
+}\
+
+
+/** transform normal vector by inverse transpose of matrix
+ * and then renormalize the vector
+ *
+ * This macro computes inverse transpose of matrix m,
+ * and multiplies vector v into it, to yeild vector p
+ * Vector p is then normalized.
+ */
+#define NORM_XFORM_2X2(p,m,v) \
+{ \
+ GREAL len; \
+ \
+ /* do nothing if off-diagonals are zero and diagonals are \
+ * equal */ \
+ if ((m[0][1] != 0.0) || (m[1][0] != 0.0) || (m[0][0] != m[1][1])) { \
+ p[0] = m[1][1]*v[0] - m[1][0]*v[1]; \
+ p[1] = - m[0][1]*v[0] + m[0][0]*v[1]; \
+ \
+ len = p[0]*p[0] + p[1]*p[1]; \
+ GIM_INV_SQRT(len,len); \
+ p[0] *= len; \
+ p[1] *= len; \
+ } else { \
+ VEC_COPY_2 (p, v); \
+ } \
+}\
+
+
+/** outer product of vector times vector transpose
+ *
+ * The outer product of vector v and vector transpose t yeilds
+ * dyadic matrix m.
+ */
+#define OUTER_PRODUCT_2X2(m,v,t) \
+{ \
+ m[0][0] = v[0] * t[0]; \
+ m[0][1] = v[0] * t[1]; \
+ \
+ m[1][0] = v[1] * t[0]; \
+ m[1][1] = v[1] * t[1]; \
+}\
+
+
+/** outer product of vector times vector transpose
+ *
+ * The outer product of vector v and vector transpose t yeilds
+ * dyadic matrix m.
+ */
+#define OUTER_PRODUCT_3X3(m,v,t) \
+{ \
+ m[0][0] = v[0] * t[0]; \
+ m[0][1] = v[0] * t[1]; \
+ m[0][2] = v[0] * t[2]; \
+ \
+ m[1][0] = v[1] * t[0]; \
+ m[1][1] = v[1] * t[1]; \
+ m[1][2] = v[1] * t[2]; \
+ \
+ m[2][0] = v[2] * t[0]; \
+ m[2][1] = v[2] * t[1]; \
+ m[2][2] = v[2] * t[2]; \
+}\
+
+
+/** outer product of vector times vector transpose
+ *
+ * The outer product of vector v and vector transpose t yeilds
+ * dyadic matrix m.
+ */
+#define OUTER_PRODUCT_4X4(m,v,t) \
+{ \
+ m[0][0] = v[0] * t[0]; \
+ m[0][1] = v[0] * t[1]; \
+ m[0][2] = v[0] * t[2]; \
+ m[0][3] = v[0] * t[3]; \
+ \
+ m[1][0] = v[1] * t[0]; \
+ m[1][1] = v[1] * t[1]; \
+ m[1][2] = v[1] * t[2]; \
+ m[1][3] = v[1] * t[3]; \
+ \
+ m[2][0] = v[2] * t[0]; \
+ m[2][1] = v[2] * t[1]; \
+ m[2][2] = v[2] * t[2]; \
+ m[2][3] = v[2] * t[3]; \
+ \
+ m[3][0] = v[3] * t[0]; \
+ m[3][1] = v[3] * t[1]; \
+ m[3][2] = v[3] * t[2]; \
+ m[3][3] = v[3] * t[3]; \
+}\
+
+
+/** outer product of vector times vector transpose
+ *
+ * The outer product of vector v and vector transpose t yeilds
+ * dyadic matrix m.
+ */
+#define ACCUM_OUTER_PRODUCT_2X2(m,v,t) \
+{ \
+ m[0][0] += v[0] * t[0]; \
+ m[0][1] += v[0] * t[1]; \
+ \
+ m[1][0] += v[1] * t[0]; \
+ m[1][1] += v[1] * t[1]; \
+}\
+
+
+/** outer product of vector times vector transpose
+ *
+ * The outer product of vector v and vector transpose t yeilds
+ * dyadic matrix m.
+ */
+#define ACCUM_OUTER_PRODUCT_3X3(m,v,t) \
+{ \
+ m[0][0] += v[0] * t[0]; \
+ m[0][1] += v[0] * t[1]; \
+ m[0][2] += v[0] * t[2]; \
+ \
+ m[1][0] += v[1] * t[0]; \
+ m[1][1] += v[1] * t[1]; \
+ m[1][2] += v[1] * t[2]; \
+ \
+ m[2][0] += v[2] * t[0]; \
+ m[2][1] += v[2] * t[1]; \
+ m[2][2] += v[2] * t[2]; \
+}\
+
+
+/** outer product of vector times vector transpose
+ *
+ * The outer product of vector v and vector transpose t yeilds
+ * dyadic matrix m.
+ */
+#define ACCUM_OUTER_PRODUCT_4X4(m,v,t) \
+{ \
+ m[0][0] += v[0] * t[0]; \
+ m[0][1] += v[0] * t[1]; \
+ m[0][2] += v[0] * t[2]; \
+ m[0][3] += v[0] * t[3]; \
+ \
+ m[1][0] += v[1] * t[0]; \
+ m[1][1] += v[1] * t[1]; \
+ m[1][2] += v[1] * t[2]; \
+ m[1][3] += v[1] * t[3]; \
+ \
+ m[2][0] += v[2] * t[0]; \
+ m[2][1] += v[2] * t[1]; \
+ m[2][2] += v[2] * t[2]; \
+ m[2][3] += v[2] * t[3]; \
+ \
+ m[3][0] += v[3] * t[0]; \
+ m[3][1] += v[3] * t[1]; \
+ m[3][2] += v[3] * t[2]; \
+ m[3][3] += v[3] * t[3]; \
+}\
+
+
+/** determinant of matrix
+ *
+ * Computes determinant of matrix m, returning d
+ */
+#define DETERMINANT_2X2(d,m) \
+{ \
+ d = m[0][0] * m[1][1] - m[0][1] * m[1][0]; \
+}\
+
+
+/** determinant of matrix
+ *
+ * Computes determinant of matrix m, returning d
+ */
+#define DETERMINANT_3X3(d,m) \
+{ \
+ d = m[0][0] * (m[1][1]*m[2][2] - m[1][2] * m[2][1]); \
+ d -= m[0][1] * (m[1][0]*m[2][2] - m[1][2] * m[2][0]); \
+ d += m[0][2] * (m[1][0]*m[2][1] - m[1][1] * m[2][0]); \
+}\
+
+
+/** i,j,th cofactor of a 4x4 matrix
+ *
+ */
+#define COFACTOR_4X4_IJ(fac,m,i,j) \
+{ \
+ GUINT __ii[4], __jj[4], __k; \
+ \
+ for (__k=0; __k<i; __k++) __ii[__k] = __k; \
+ for (__k=i; __k<3; __k++) __ii[__k] = __k+1; \
+ for (__k=0; __k<j; __k++) __jj[__k] = __k; \
+ for (__k=j; __k<3; __k++) __jj[__k] = __k+1; \
+ \
+ (fac) = m[__ii[0]][__jj[0]] * (m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[2]] \
+ - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[1]]); \
+ (fac) -= m[__ii[0]][__jj[1]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[2]] \
+ - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[0]]);\
+ (fac) += m[__ii[0]][__jj[2]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[1]] \
+ - m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[0]]);\
+ \
+ __k = i+j; \
+ if ( __k != (__k/2)*2) { \
+ (fac) = -(fac); \
+ } \
+}\
+
+
+/** determinant of matrix
+ *
+ * Computes determinant of matrix m, returning d
+ */
+#define DETERMINANT_4X4(d,m) \
+{ \
+ GREAL cofac; \
+ COFACTOR_4X4_IJ (cofac, m, 0, 0); \
+ d = m[0][0] * cofac; \
+ COFACTOR_4X4_IJ (cofac, m, 0, 1); \
+ d += m[0][1] * cofac; \
+ COFACTOR_4X4_IJ (cofac, m, 0, 2); \
+ d += m[0][2] * cofac; \
+ COFACTOR_4X4_IJ (cofac, m, 0, 3); \
+ d += m[0][3] * cofac; \
+}\
+
+
+/** cofactor of matrix
+ *
+ * Computes cofactor of matrix m, returning a
+ */
+#define COFACTOR_2X2(a,m) \
+{ \
+ a[0][0] = (m)[1][1]; \
+ a[0][1] = - (m)[1][0]; \
+ a[1][0] = - (m)[0][1]; \
+ a[1][1] = (m)[0][0]; \
+}\
+
+
+/** cofactor of matrix
+ *
+ * Computes cofactor of matrix m, returning a
+ */
+#define COFACTOR_3X3(a,m) \
+{ \
+ a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1]; \
+ a[0][1] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]); \
+ a[0][2] = m[1][0]*m[2][1] - m[1][1]*m[2][0]; \
+ a[1][0] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]); \
+ a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0]; \
+ a[1][2] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]); \
+ a[2][0] = m[0][1]*m[1][2] - m[0][2]*m[1][1]; \
+ a[2][1] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]); \
+ a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]); \
+}\
+
+
+/** cofactor of matrix
+ *
+ * Computes cofactor of matrix m, returning a
+ */
+#define COFACTOR_4X4(a,m) \
+{ \
+ int i,j; \
+ \
+ for (i=0; i<4; i++) { \
+ for (j=0; j<4; j++) { \
+ COFACTOR_4X4_IJ (a[i][j], m, i, j); \
+ } \
+ } \
+}\
+
+
+/** adjoint of matrix
+ *
+ * Computes adjoint of matrix m, returning a
+ * (Note that adjoint is just the transpose of the cofactor matrix)
+ */
+#define ADJOINT_2X2(a,m) \
+{ \
+ a[0][0] = (m)[1][1]; \
+ a[1][0] = - (m)[1][0]; \
+ a[0][1] = - (m)[0][1]; \
+ a[1][1] = (m)[0][0]; \
+}\
+
+
+/** adjoint of matrix
+ *
+ * Computes adjoint of matrix m, returning a
+ * (Note that adjoint is just the transpose of the cofactor matrix)
+ */
+#define ADJOINT_3X3(a,m) \
+{ \
+ a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1]; \
+ a[1][0] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]); \
+ a[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0]; \
+ a[0][1] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]); \
+ a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0]; \
+ a[2][1] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]); \
+ a[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1]; \
+ a[1][2] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]); \
+ a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]); \
+}\
+
+
+/** adjoint of matrix
+ *
+ * Computes adjoint of matrix m, returning a
+ * (Note that adjoint is just the transpose of the cofactor matrix)
+ */
+#define ADJOINT_4X4(a,m) \
+{ \
+ char _i_,_j_; \
+ \
+ for (_i_=0; _i_<4; _i_++) { \
+ for (_j_=0; _j_<4; _j_++) { \
+ COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_); \
+ } \
+ } \
+}\
+
+
+/** compute adjoint of matrix and scale
+ *
+ * Computes adjoint of matrix m, scales it by s, returning a
+ */
+#define SCALE_ADJOINT_2X2(a,s,m) \
+{ \
+ a[0][0] = (s) * m[1][1]; \
+ a[1][0] = - (s) * m[1][0]; \
+ a[0][1] = - (s) * m[0][1]; \
+ a[1][1] = (s) * m[0][0]; \
+}\
+
+
+/** compute adjoint of matrix and scale
+ *
+ * Computes adjoint of matrix m, scales it by s, returning a
+ */
+#define SCALE_ADJOINT_3X3(a,s,m) \
+{ \
+ a[0][0] = (s) * (m[1][1] * m[2][2] - m[1][2] * m[2][1]); \
+ a[1][0] = (s) * (m[1][2] * m[2][0] - m[1][0] * m[2][2]); \
+ a[2][0] = (s) * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); \
+ \
+ a[0][1] = (s) * (m[0][2] * m[2][1] - m[0][1] * m[2][2]); \
+ a[1][1] = (s) * (m[0][0] * m[2][2] - m[0][2] * m[2][0]); \
+ a[2][1] = (s) * (m[0][1] * m[2][0] - m[0][0] * m[2][1]); \
+ \
+ a[0][2] = (s) * (m[0][1] * m[1][2] - m[0][2] * m[1][1]); \
+ a[1][2] = (s) * (m[0][2] * m[1][0] - m[0][0] * m[1][2]); \
+ a[2][2] = (s) * (m[0][0] * m[1][1] - m[0][1] * m[1][0]); \
+}\
+
+
+/** compute adjoint of matrix and scale
+ *
+ * Computes adjoint of matrix m, scales it by s, returning a
+ */
+#define SCALE_ADJOINT_4X4(a,s,m) \
+{ \
+ char _i_,_j_; \
+ for (_i_=0; _i_<4; _i_++) { \
+ for (_j_=0; _j_<4; _j_++) { \
+ COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_); \
+ a[_j_][_i_] *= s; \
+ } \
+ } \
+}\
+
+/** inverse of matrix
+ *
+ * Compute inverse of matrix a, returning determinant m and
+ * inverse b
+ */
+#define INVERT_2X2(b,det,a) \
+{ \
+ GREAL _tmp_; \
+ DETERMINANT_2X2 (det, a); \
+ _tmp_ = 1.0 / (det); \
+ SCALE_ADJOINT_2X2 (b, _tmp_, a); \
+}\
+
+
+/** inverse of matrix
+ *
+ * Compute inverse of matrix a, returning determinant m and
+ * inverse b
+ */
+#define INVERT_3X3(b,det,a) \
+{ \
+ GREAL _tmp_; \
+ DETERMINANT_3X3 (det, a); \
+ _tmp_ = 1.0 / (det); \
+ SCALE_ADJOINT_3X3 (b, _tmp_, a); \
+}\
+
+
+/** inverse of matrix
+ *
+ * Compute inverse of matrix a, returning determinant m and
+ * inverse b
+ */
+#define INVERT_4X4(b,det,a) \
+{ \
+ GREAL _tmp_; \
+ DETERMINANT_4X4 (det, a); \
+ _tmp_ = 1.0 / (det); \
+ SCALE_ADJOINT_4X4 (b, _tmp_, a); \
+}\
+
+//! Get the triple(3) row of a transform matrix
+#define MAT_GET_ROW(mat,vec3,rowindex)\
+{\
+ vec3[0] = mat[rowindex][0];\
+ vec3[1] = mat[rowindex][1];\
+ vec3[2] = mat[rowindex][2]; \
+}\
+
+//! Get the tuple(2) row of a transform matrix
+#define MAT_GET_ROW2(mat,vec2,rowindex)\
+{\
+ vec2[0] = mat[rowindex][0];\
+ vec2[1] = mat[rowindex][1];\
+}\
+
+
+//! Get the quad (4) row of a transform matrix
+#define MAT_GET_ROW4(mat,vec4,rowindex)\
+{\
+ vec4[0] = mat[rowindex][0];\
+ vec4[1] = mat[rowindex][1];\
+ vec4[2] = mat[rowindex][2];\
+ vec4[3] = mat[rowindex][3];\
+}\
+
+//! Get the triple(3) col of a transform matrix
+#define MAT_GET_COL(mat,vec3,colindex)\
+{\
+ vec3[0] = mat[0][colindex];\
+ vec3[1] = mat[1][colindex];\
+ vec3[2] = mat[2][colindex]; \
+}\
+
+//! Get the tuple(2) col of a transform matrix
+#define MAT_GET_COL2(mat,vec2,colindex)\
+{\
+ vec2[0] = mat[0][colindex];\
+ vec2[1] = mat[1][colindex];\
+}\
+
+
+//! Get the quad (4) col of a transform matrix
+#define MAT_GET_COL4(mat,vec4,colindex)\
+{\
+ vec4[0] = mat[0][colindex];\
+ vec4[1] = mat[1][colindex];\
+ vec4[2] = mat[2][colindex];\
+ vec4[3] = mat[3][colindex];\
+}\
+
+//! Get the triple(3) col of a transform matrix
+#define MAT_GET_X(mat,vec3)\
+{\
+ MAT_GET_COL(mat,vec3,0);\
+}\
+
+//! Get the triple(3) col of a transform matrix
+#define MAT_GET_Y(mat,vec3)\
+{\
+ MAT_GET_COL(mat,vec3,1);\
+}\
+
+//! Get the triple(3) col of a transform matrix
+#define MAT_GET_Z(mat,vec3)\
+{\
+ MAT_GET_COL(mat,vec3,2);\
+}\
+
+
+//! Get the triple(3) col of a transform matrix
+#define MAT_SET_X(mat,vec3)\
+{\
+ mat[0][0] = vec3[0];\
+ mat[1][0] = vec3[1];\
+ mat[2][0] = vec3[2];\
+}\
+
+//! Get the triple(3) col of a transform matrix
+#define MAT_SET_Y(mat,vec3)\
+{\
+ mat[0][1] = vec3[0];\
+ mat[1][1] = vec3[1];\
+ mat[2][1] = vec3[2];\
+}\
+
+//! Get the triple(3) col of a transform matrix
+#define MAT_SET_Z(mat,vec3)\
+{\
+ mat[0][2] = vec3[0];\
+ mat[1][2] = vec3[1];\
+ mat[2][2] = vec3[2];\
+}\
+
+
+//! Get the triple(3) col of a transform matrix
+#define MAT_GET_TRANSLATION(mat,vec3)\
+{\
+ vec3[0] = mat[0][3];\
+ vec3[1] = mat[1][3];\
+ vec3[2] = mat[2][3]; \
+}\
+
+//! Set the triple(3) col of a transform matrix
+#define MAT_SET_TRANSLATION(mat,vec3)\
+{\
+ mat[0][3] = vec3[0];\
+ mat[1][3] = vec3[1];\
+ mat[2][3] = vec3[2]; \
+}\
+
+
+
+//! Returns the dot product between a vec3f and the row of a matrix
+#define MAT_DOT_ROW(mat,vec3,rowindex) (vec3[0]*mat[rowindex][0] + vec3[1]*mat[rowindex][1] + vec3[2]*mat[rowindex][2])
+
+//! Returns the dot product between a vec2f and the row of a matrix
+#define MAT_DOT_ROW2(mat,vec2,rowindex) (vec2[0]*mat[rowindex][0] + vec2[1]*mat[rowindex][1])
+
+//! Returns the dot product between a vec4f and the row of a matrix
+#define MAT_DOT_ROW4(mat,vec4,rowindex) (vec4[0]*mat[rowindex][0] + vec4[1]*mat[rowindex][1] + vec4[2]*mat[rowindex][2] + vec4[3]*mat[rowindex][3])
+
+
+//! Returns the dot product between a vec3f and the col of a matrix
+#define MAT_DOT_COL(mat,vec3,colindex) (vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex])
+
+//! Returns the dot product between a vec2f and the col of a matrix
+#define MAT_DOT_COL2(mat,vec2,colindex) (vec2[0]*mat[0][colindex] + vec2[1]*mat[1][colindex])
+
+//! Returns the dot product between a vec4f and the col of a matrix
+#define MAT_DOT_COL4(mat,vec4,colindex) (vec4[0]*mat[0][colindex] + vec4[1]*mat[1][colindex] + vec4[2]*mat[2][colindex] + vec4[3]*mat[3][colindex])
+
+/*!Transpose matrix times vector
+v is a vec3f
+and m is a mat4f<br>
+*/
+#define INV_MAT_DOT_VEC_3X3(p,m,v) \
+{ \
+ p[0] = MAT_DOT_COL(m,v,0); \
+ p[1] = MAT_DOT_COL(m,v,1); \
+ p[2] = MAT_DOT_COL(m,v,2); \
+}\
+
+
+
+#endif // GIM_VECTOR_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_math.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_math.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_math.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_math.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,157 @@
+#ifndef GIM_MATH_H_INCLUDED
+#define GIM_MATH_H_INCLUDED
+/*! \file gim_math.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at 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.
+ (3) The zlib/libpng license that is included with this library in
+ the file GIMPACT-LICENSE-ZLIB.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, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "LinearMath/btScalar.h"
+
+
+
+#define GREAL btScalar
+#define GREAL2 double
+#define GINT int
+#define GUINT unsigned int
+#define GSHORT short
+#define GUSHORT unsigned short
+#define GINT64 long long
+#define GUINT64 unsigned long long
+
+
+
+#define G_PI 3.14159265358979f
+#define G_HALF_PI 1.5707963f
+//267948966
+#define G_TWO_PI 6.28318530f
+//71795864
+#define G_ROOT3 1.73205f
+#define G_ROOT2 1.41421f
+#define G_UINT_INFINITY 0xffffffff //!< A very very high value
+#define G_REAL_INFINITY FLT_MAX
+#define G_SIGN_BITMASK 0x80000000
+#define G_EPSILON SIMD_EPSILON
+
+
+
+enum GIM_SCALAR_TYPES
+{
+ G_STYPE_REAL =0,
+ G_STYPE_REAL2,
+ G_STYPE_SHORT,
+ G_STYPE_USHORT,
+ G_STYPE_INT,
+ G_STYPE_UINT,
+ G_STYPE_INT64,
+ G_STYPE_UINT64
+};
+
+
+
+#define G_DEGTORAD(X) ((X)*3.1415926f/180.0f)
+#define G_RADTODEG(X) ((X)*180.0f/3.1415926f)
+
+//! Integer representation of a floating-point value.
+#define GIM_IR(x) ((GUINT&)(x))
+
+//! Signed integer representation of a floating-point value.
+#define GIM_SIR(x) ((GINT&)(x))
+
+//! Absolute integer representation of a floating-point value
+#define GIM_AIR(x) (GIM_IR(x)&0x7fffffff)
+
+//! Floating-point representation of an integer value.
+#define GIM_FR(x) ((GREAL&)(x))
+
+#define GIM_MAX(a,b) (a<b?b:a)
+#define GIM_MIN(a,b) (a>b?b:a)
+
+#define GIM_MAX3(a,b,c) GIM_MAX(a,GIM_MAX(b,c))
+#define GIM_MIN3(a,b,c) GIM_MIN(a,GIM_MIN(b,c))
+
+#define GIM_IS_ZERO(value) (value < G_EPSILON && value > -G_EPSILON)
+
+#define GIM_IS_NEGATIVE(value) (value <= -G_EPSILON)
+
+#define GIM_IS_POSISITVE(value) (value >= G_EPSILON)
+
+#define GIM_NEAR_EQUAL(v1,v2) GIM_IS_ZERO((v1-v2))
+
+///returns a clamped number
+#define GIM_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number))
+
+#define GIM_GREATER(x, y) btFabs(x) > (y)
+
+///Swap numbers
+#define GIM_SWAP_NUMBERS(a,b){ \
+ a = a+b; \
+ b = a-b; \
+ a = a-b; \
+}\
+
+#define GIM_INV_SQRT(va,isva)\
+{\
+ if(va<=0.0000001f)\
+ {\
+ isva = G_REAL_INFINITY;\
+ }\
+ else\
+ {\
+ GREAL _x = va * 0.5f;\
+ GUINT _y = 0x5f3759df - ( GIM_IR(va) >> 1);\
+ isva = GIM_FR(_y);\
+ isva = isva * ( 1.5f - ( _x * isva * isva ) );\
+ }\
+}\
+
+#define GIM_SQRT(va,sva)\
+{\
+ GIM_INV_SQRT(va,sva);\
+ sva = 1.0f/sva;\
+}\
+
+//! Computes 1.0f / sqrtf(x). Comes from Quake3. See http://www.magic-software.com/3DGEDInvSqrt.html
+inline GREAL gim_inv_sqrt(GREAL f)
+{
+ GREAL r;
+ GIM_INV_SQRT(f,r);
+ return r;
+}
+
+inline GREAL gim_sqrt(GREAL f)
+{
+ GREAL r;
+ GIM_SQRT(f,r);
+ return r;
+}
+
+
+
+#endif // GIM_MATH_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_memory.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_memory.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_memory.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_memory.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,190 @@
+#ifndef GIM_MEMORY_H_INCLUDED
+#define GIM_MEMORY_H_INCLUDED
+/*! \file gim_memory.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at 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.
+ (3) The zlib/libpng license that is included with this library in
+ the file GIMPACT-LICENSE-ZLIB.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, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "gim_math.h"
+#include <string.h>
+
+#ifdef PREFETCH
+#include <xmmintrin.h> // for prefetch
+#define pfval 64
+#define pfval2 128
+//! Prefetch 64
+#define pf(_x,_i) _mm_prefetch((void *)(_x + _i + pfval), 0)
+//! Prefetch 128
+#define pf2(_x,_i) _mm_prefetch((void *)(_x + _i + pfval2), 0)
+#else
+//! Prefetch 64
+#define pf(_x,_i)
+//! Prefetch 128
+#define pf2(_x,_i)
+#endif
+
+
+///Functions for manip packed arrays of numbers
+#define GIM_COPY_ARRAYS(dest_array,source_array,element_count)\
+{\
+ for (GUINT _i_=0;_i_<element_count ;++_i_)\
+ {\
+ dest_array[_i_] = source_array[_i_];\
+ }\
+}\
+
+#define GIM_COPY_ARRAYS_1(dest_array,source_array,element_count,copy_macro)\
+{\
+ for (GUINT _i_=0;_i_<element_count ;++_i_)\
+ {\
+ copy_macro(dest_array[_i_],source_array[_i_]);\
+ }\
+}\
+
+
+#define GIM_ZERO_ARRAY(array,element_count)\
+{\
+ for (GUINT _i_=0;_i_<element_count ;++_i_)\
+ {\
+ array[_i_] = 0;\
+ }\
+}\
+
+#define GIM_CONSTANT_ARRAY(array,element_count,constant)\
+{\
+ for (GUINT _i_=0;_i_<element_count ;++_i_)\
+ {\
+ array[_i_] = constant;\
+ }\
+}\
+
+
+///Function prototypes to allocate and free memory.
+typedef void * gim_alloc_function (size_t size);
+typedef void * gim_alloca_function (size_t size);//Allocs on the heap
+typedef void * gim_realloc_function (void *ptr, size_t oldsize, size_t newsize);
+typedef void gim_free_function (void *ptr);
+
+
+///Memory Function Handlers
+///set new memory management functions. if fn is 0, the default handlers are used.
+void gim_set_alloc_handler (gim_alloc_function *fn);
+void gim_set_alloca_handler (gim_alloca_function *fn);
+void gim_set_realloc_handler (gim_realloc_function *fn);
+void gim_set_free_handler (gim_free_function *fn);
+
+
+///get current memory management functions.
+gim_alloc_function *gim_get_alloc_handler (void);
+gim_alloca_function *gim_get_alloca_handler(void);
+gim_realloc_function *gim_get_realloc_handler (void);
+gim_free_function *gim_get_free_handler (void);
+
+
+///Standar Memory functions
+void * gim_alloc(size_t size);
+void * gim_alloca(size_t size);
+void * gim_realloc(void *ptr, size_t oldsize, size_t newsize);
+void gim_free(void *ptr);
+
+
+
+#if defined (WIN32) && !defined(__MINGW32__) && !defined(__CYGWIN__)
+ #define GIM_SIMD_MEMORY 1
+#endif
+
+//! SIMD POINTER INTEGER
+#define SIMD_T GUINT64
+//! SIMD INTEGER SIZE
+#define SIMD_T_SIZE sizeof(SIMD_T)
+
+
+inline void gim_simd_memcpy(void * dst, const void * src, size_t copysize)
+{
+#ifdef GIM_SIMD_MEMORY
+/*
+//'long long int' is incompatible with visual studio 6...
+ //copy words
+ SIMD_T * ui_src_ptr = (SIMD_T *)src;
+ SIMD_T * ui_dst_ptr = (SIMD_T *)dst;
+ while(copysize>=SIMD_T_SIZE)
+ {
+ *(ui_dst_ptr++) = *(ui_src_ptr++);
+ copysize-=SIMD_T_SIZE;
+ }
+ if(copysize==0) return;
+*/
+
+ char * c_src_ptr = (char *)src;
+ char * c_dst_ptr = (char *)dst;
+ while(copysize>0)
+ {
+ *(c_dst_ptr++) = *(c_src_ptr++);
+ copysize--;
+ }
+ return;
+#else
+ memcpy(dst,src,copysize);
+#endif
+}
+
+
+
+template<class T>
+inline void gim_swap_elements(T* _array,size_t _i,size_t _j)
+{
+ T _e_tmp_ = _array[_i];
+ _array[_i] = _array[_j];
+ _array[_j] = _e_tmp_;
+}
+
+
+template<class T>
+inline void gim_swap_elements_memcpy(T* _array,size_t _i,size_t _j)
+{
+ char _e_tmp_[sizeof(T)];
+ gim_simd_memcpy(_e_tmp_,&_array[_i],sizeof(T));
+ gim_simd_memcpy(&_array[_i],&_array[_j],sizeof(T));
+ gim_simd_memcpy(&_array[_j],_e_tmp_,sizeof(T));
+}
+
+template <int SIZE>
+inline void gim_swap_elements_ptr(char * _array,size_t _i,size_t _j)
+{
+ char _e_tmp_[SIZE];
+ _i*=SIZE;
+ _j*=SIZE;
+ gim_simd_memcpy(_e_tmp_,_array+_i,SIZE);
+ gim_simd_memcpy(_array+_i,_array+_j,SIZE);
+ gim_simd_memcpy(_array+_j,_e_tmp_,SIZE);
+}
+
+#endif // GIM_MEMORY_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_radixsort.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_radixsort.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_radixsort.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_radixsort.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,406 @@
+#ifndef GIM_RADIXSORT_H_INCLUDED
+#define GIM_RADIXSORT_H_INCLUDED
+/*! \file gim_radixsort.h
+\author Francisco Len Nßjera.
+Based on the work of Michael Herf : "fast floating-point radix sort"
+Avaliable on http://www.stereopsis.com/radix.html
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at 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.
+ (3) The zlib/libpng license that is included with this library in
+ the file GIMPACT-LICENSE-ZLIB.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, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "gim_memory.h"
+
+///Macros for sorting.
+//! Prototype for comparators
+class less_comparator
+{
+ public:
+
+ template<class T,class Z>
+ inline int operator() ( const T& a, const Z& b )
+ {
+ return ( a<b?-1:(a>b?1:0));
+ }
+};
+
+//! Prototype for comparators
+class integer_comparator
+{
+ public:
+
+ template<class T>
+ inline int operator() ( const T& a, const T& b )
+ {
+ return (int)(a-b);
+ }
+};
+
+//!Prototype for getting the integer representation of an object
+class uint_key_func
+{
+public:
+ template<class T>
+ inline GUINT operator()( const T& a)
+ {
+ return (GUINT)a;
+ }
+};
+
+
+//!Prototype for copying elements
+class copy_elements_func
+{
+public:
+ template<class T>
+ inline void operator()(T& a,T& b)
+ {
+ a = b;
+ }
+};
+
+//!Prototype for copying elements
+class memcopy_elements_func
+{
+public:
+ template<class T>
+ inline void operator()(T& a,T& b)
+ {
+ gim_simd_memcpy(&a,&b,sizeof(T));
+ }
+};
+
+
+//! @{
+struct GIM_RSORT_TOKEN
+{
+ GUINT m_key;
+ GUINT m_value;
+ GIM_RSORT_TOKEN()
+ {
+ }
+ GIM_RSORT_TOKEN(const GIM_RSORT_TOKEN& rtoken)
+ {
+ m_key = rtoken.m_key;
+ m_value = rtoken.m_value;
+ }
+
+ inline bool operator <(const GIM_RSORT_TOKEN& other) const
+ {
+ return (m_key < other.m_key);
+ }
+
+ inline bool operator >(const GIM_RSORT_TOKEN& other) const
+ {
+ return (m_key > other.m_key);
+ }
+};
+
+//! Prototype for comparators
+class GIM_RSORT_TOKEN_COMPARATOR
+{
+ public:
+
+ inline int operator()( const GIM_RSORT_TOKEN& a, const GIM_RSORT_TOKEN& b )
+ {
+ return (int)((a.m_key) - (b.m_key));
+ }
+};
+
+
+
+#define kHist 2048
+// ---- utils for accessing 11-bit quantities
+#define D11_0(x) (x & 0x7FF)
+#define D11_1(x) (x >> 11 & 0x7FF)
+#define D11_2(x) (x >> 22 )
+
+
+
+///Radix sort for unsigned integer keys
+inline void gim_radix_sort_rtokens(
+ GIM_RSORT_TOKEN * array,
+ GIM_RSORT_TOKEN * sorted, GUINT element_count)
+{
+ GUINT i;
+ GUINT b0[kHist * 3];
+ GUINT *b1 = b0 + kHist;
+ GUINT *b2 = b1 + kHist;
+ for (i = 0; i < kHist * 3; ++i)
+ {
+ b0[i] = 0;
+ }
+ GUINT fi;
+ GUINT pos;
+ for (i = 0; i < element_count; ++i)
+ {
+ fi = array[i].m_key;
+ b0[D11_0(fi)] ++;
+ b1[D11_1(fi)] ++;
+ b2[D11_2(fi)] ++;
+ }
+ {
+ GUINT sum0 = 0, sum1 = 0, sum2 = 0;
+ GUINT tsum;
+ for (i = 0; i < kHist; ++i)
+ {
+ tsum = b0[i] + sum0;
+ b0[i] = sum0 - 1;
+ sum0 = tsum;
+ tsum = b1[i] + sum1;
+ b1[i] = sum1 - 1;
+ sum1 = tsum;
+ tsum = b2[i] + sum2;
+ b2[i] = sum2 - 1;
+ sum2 = tsum;
+ }
+ }
+ for (i = 0; i < element_count; ++i)
+ {
+ fi = array[i].m_key;
+ pos = D11_0(fi);
+ pos = ++b0[pos];
+ sorted[pos].m_key = array[i].m_key;
+ sorted[pos].m_value = array[i].m_value;
+ }
+ for (i = 0; i < element_count; ++i)
+ {
+ fi = sorted[i].m_key;
+ pos = D11_1(fi);
+ pos = ++b1[pos];
+ array[pos].m_key = sorted[i].m_key;
+ array[pos].m_value = sorted[i].m_value;
+ }
+ for (i = 0; i < element_count; ++i)
+ {
+ fi = array[i].m_key;
+ pos = D11_2(fi);
+ pos = ++b2[pos];
+ sorted[pos].m_key = array[i].m_key;
+ sorted[pos].m_value = array[i].m_value;
+ }
+}
+
+
+
+
+/// Get the sorted tokens from an array. For generic use. Tokens are IRR_RSORT_TOKEN
+/*!
+*\param array Array of elements to sort
+*\param sorted_tokens Tokens of sorted elements
+*\param element_count element count
+*\param uintkey_macro Functor which retrieves the integer representation of an array element
+*/
+template<typename T, class GETKEY_CLASS>
+void gim_radix_sort_array_tokens(
+ T* array ,
+ GIM_RSORT_TOKEN * sorted_tokens,
+ GUINT element_count,GETKEY_CLASS uintkey_macro)
+{
+ GIM_RSORT_TOKEN * _unsorted = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count);
+ for (GUINT _i=0;_i<element_count;++_i)
+ {
+ _unsorted[_i].m_key = uintkey_macro(array[_i]);
+ _unsorted[_i].m_value = _i;
+ }
+ gim_radix_sort_rtokens(_unsorted,sorted_tokens,element_count);
+ gim_free(_unsorted);
+ gim_free(_unsorted);
+}
+
+/// Sorts array in place. For generic use
+/*!
+\param type Type of the array
+\param array
+\param element_count
+\param get_uintkey_macro Macro for extract the Integer value of the element. Similar to SIMPLE_GET_UINTKEY
+\param copy_elements_macro Macro for copy elements, similar to SIMPLE_COPY_ELEMENTS
+*/
+template<typename T, class GETKEY_CLASS, class COPY_CLASS>
+void gim_radix_sort(
+ T * array, GUINT element_count,
+ GETKEY_CLASS get_uintkey_macro, COPY_CLASS copy_elements_macro)
+{
+ GIM_RSORT_TOKEN * _sorted = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count);
+ gim_radix_sort_array_tokens(array,_sorted,element_count,get_uintkey_macro);
+ T * _original_array = (T *) gim_alloc(sizeof(T)*element_count);
+ gim_simd_memcpy(_original_array,array,sizeof(T)*element_count);
+ for (GUINT _i=0;_i<element_count;++_i)
+ {
+ copy_elements_macro(array[_i],_original_array[_sorted[_i].m_value]);
+ }
+ gim_free(_original_array);
+ gim_free(_sorted);
+}
+
+//! Failsafe Iterative binary search,
+/*!
+If the element is not found, it returns the nearest upper element position, may be the further position after the last element.
+\param _array
+\param _start_i the beginning of the array
+\param _end_i the ending index of the array
+\param _search_key Value to find
+\param _comp_macro macro for comparing elements
+\param _found If true the value has found. Boolean
+\param _result_index the index of the found element, or if not found then it will get the index of the closest bigger value
+*/
+template<class T, typename KEYCLASS, typename COMP_CLASS>
+bool gim_binary_search_ex(
+ const T* _array, GUINT _start_i,
+ GUINT _end_i,GUINT & _result_index,
+ const KEYCLASS & _search_key,
+ COMP_CLASS _comp_macro)
+{
+ GUINT _k;
+ int _comp_result;
+ GUINT _i = _start_i;
+ GUINT _j = _end_i+1;
+ while (_i < _j)
+ {
+ _k = (_j+_i-1)/2;
+ _comp_result = _comp_macro(_array[_k], _search_key);
+ if (_comp_result == 0)
+ {
+ _result_index = _k;
+ return true;
+ }
+ else if (_comp_result < 0)
+ {
+ _i = _k+1;
+ }
+ else
+ {
+ _j = _k;
+ }
+ }
+ _result_index = _i;
+ return false;
+}
+
+
+
+//! Failsafe Iterative binary search,Template version
+/*!
+If the element is not found, it returns the nearest upper element position, may be the further position after the last element.
+\param _array
+\param _start_i the beginning of the array
+\param _end_i the ending index of the array
+\param _search_key Value to find
+\param _result_index the index of the found element, or if not found then it will get the index of the closest bigger value
+\return true if found, else false
+*/
+template<class T>
+bool gim_binary_search(
+ const T*_array,GUINT _start_i,
+ GUINT _end_i,const T & _search_key,
+ GUINT & _result_index)
+{
+ GUINT _i = _start_i;
+ GUINT _j = _end_i+1;
+ GUINT _k;
+ while(_i < _j)
+ {
+ _k = (_j+_i-1)/2;
+ if(_array[_k]==_search_key)
+ {
+ _result_index = _k;
+ return true;
+ }
+ else if (_array[_k]<_search_key)
+ {
+ _i = _k+1;
+ }
+ else
+ {
+ _j = _k;
+ }
+ }
+ _result_index = _i;
+ return false;
+}
+
+
+
+///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
+template <typename T, typename COMP_CLASS>
+void gim_down_heap(T *pArr, GUINT k, GUINT n,COMP_CLASS CompareFunc)
+{
+ /* PRE: a[k+1..N] is a heap */
+ /* POST: a[k..N] is a heap */
+
+ T temp = pArr[k - 1];
+ /* k has child(s) */
+ while (k <= n/2)
+ {
+ int child = 2*k;
+
+ if ((child < (int)n) && CompareFunc(pArr[child - 1] , pArr[child])<0)
+ {
+ child++;
+ }
+ /* pick larger child */
+ if (CompareFunc(temp , pArr[child - 1])<0)
+ {
+ /* move child up */
+ pArr[k - 1] = pArr[child - 1];
+ k = child;
+ }
+ else
+ {
+ break;
+ }
+ }
+ pArr[k - 1] = temp;
+} /*downHeap*/
+
+
+template <typename T, typename COMP_CLASS>
+void gim_heap_sort(T *pArr, GUINT element_count, COMP_CLASS CompareFunc)
+{
+ /* sort a[0..N-1], N.B. 0 to N-1 */
+ GUINT k;
+ GUINT n = element_count;
+ for (k = n/2; k > 0; k--)
+ {
+ gim_down_heap(pArr, k, n, CompareFunc);
+ }
+
+ /* a[1..N] is now a heap */
+ while ( n>=2 )
+ {
+ gim_swap_elements(pArr,0,n-1); /* largest of a[0..n-1] */
+ --n;
+ /* restore a[1..i-1] heap */
+ gim_down_heap(pArr, 1, n, CompareFunc);
+ }
+}
+
+
+
+
+#endif // GIM_RADIXSORT_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_tri_collision.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_tri_collision.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_tri_collision.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_tri_collision.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,379 @@
+#ifndef GIM_TRI_COLLISION_H_INCLUDED
+#define GIM_TRI_COLLISION_H_INCLUDED
+
+/*! \file gim_tri_collision.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at 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.
+ (3) The zlib/libpng license that is included with this library in
+ the file GIMPACT-LICENSE-ZLIB.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, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "gim_box_collision.h"
+#include "gim_clip_polygon.h"
+
+
+
+
+#define MAX_TRI_CLIPPING 16
+
+//! Structure for collision
+struct GIM_TRIANGLE_CONTACT_DATA
+{
+ GREAL m_penetration_depth;
+ GUINT m_point_count;
+ btVector4 m_separating_normal;
+ btVector3 m_points[MAX_TRI_CLIPPING];
+
+ SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT_DATA& other)
+ {
+ m_penetration_depth = other.m_penetration_depth;
+ m_separating_normal = other.m_separating_normal;
+ m_point_count = other.m_point_count;
+ GUINT i = m_point_count;
+ while(i--)
+ {
+ m_points[i] = other.m_points[i];
+ }
+ }
+
+ GIM_TRIANGLE_CONTACT_DATA()
+ {
+ }
+
+ GIM_TRIANGLE_CONTACT_DATA(const GIM_TRIANGLE_CONTACT_DATA& other)
+ {
+ copy_from(other);
+ }
+
+
+
+
+ //! classify points that are closer
+ template<typename DISTANCE_FUNC,typename CLASS_PLANE>
+ SIMD_FORCE_INLINE void mergepoints_generic(const CLASS_PLANE & plane,
+ GREAL margin, const btVector3 * points, GUINT point_count, DISTANCE_FUNC distance_func)
+ {
+ m_point_count = 0;
+ m_penetration_depth= -1000.0f;
+
+ GUINT point_indices[MAX_TRI_CLIPPING];
+
+ GUINT _k;
+
+ for(_k=0;_k<point_count;_k++)
+ {
+ GREAL _dist = -distance_func(plane,points[_k]) + margin;
+
+ if(_dist>=0.0f)
+ {
+ if(_dist>m_penetration_depth)
+ {
+ m_penetration_depth = _dist;
+ point_indices[0] = _k;
+ m_point_count=1;
+ }
+ else if((_dist+G_EPSILON)>=m_penetration_depth)
+ {
+ point_indices[m_point_count] = _k;
+ m_point_count++;
+ }
+ }
+ }
+
+ for( _k=0;_k<m_point_count;_k++)
+ {
+ m_points[_k] = points[point_indices[_k]];
+ }
+ }
+
+ //! classify points that are closer
+ SIMD_FORCE_INLINE void merge_points(const btVector4 & plane, GREAL margin,
+ const btVector3 * points, GUINT point_count)
+ {
+ m_separating_normal = plane;
+ mergepoints_generic(plane, margin, points, point_count, DISTANCE_PLANE_3D_FUNC());
+ }
+};
+
+
+//! Class for colliding triangles
+class GIM_TRIANGLE
+{
+public:
+ btScalar m_margin;
+ btVector3 m_vertices[3];
+
+ GIM_TRIANGLE():m_margin(0.1f)
+ {
+ }
+
+ SIMD_FORCE_INLINE GIM_AABB get_box() const
+ {
+ return GIM_AABB(m_vertices[0],m_vertices[1],m_vertices[2],m_margin);
+ }
+
+ SIMD_FORCE_INLINE void get_normal(btVector3 &normal) const
+ {
+ TRIANGLE_NORMAL(m_vertices[0],m_vertices[1],m_vertices[2],normal);
+ }
+
+ SIMD_FORCE_INLINE void get_plane(btVector4 &plane) const
+ {
+ TRIANGLE_PLANE(m_vertices[0],m_vertices[1],m_vertices[2],plane);;
+ }
+
+ SIMD_FORCE_INLINE void apply_transform(const btTransform & trans)
+ {
+ m_vertices[0] = trans(m_vertices[0]);
+ m_vertices[1] = trans(m_vertices[1]);
+ m_vertices[2] = trans(m_vertices[2]);
+ }
+
+ SIMD_FORCE_INLINE void get_edge_plane(GUINT edge_index,const btVector3 &triangle_normal,btVector4 &plane) const
+ {
+ const btVector3 & e0 = m_vertices[edge_index];
+ const btVector3 & e1 = m_vertices[(edge_index+1)%3];
+ EDGE_PLANE(e0,e1,triangle_normal,plane);
+ }
+
+ //! Gets the relative transformation of this triangle
+ /*!
+ The transformation is oriented to the triangle normal , and aligned to the 1st edge of this triangle. The position corresponds to vertice 0:
+ - triangle normal corresponds to Z axis.
+ - 1st normalized edge corresponds to X axis,
+
+ */
+ SIMD_FORCE_INLINE void get_triangle_transform(btTransform & triangle_transform) const
+ {
+ btMatrix3x3 & matrix = triangle_transform.getBasis();
+
+ btVector3 zaxis;
+ get_normal(zaxis);
+ MAT_SET_Z(matrix,zaxis);
+
+ btVector3 xaxis = m_vertices[1] - m_vertices[0];
+ VEC_NORMALIZE(xaxis);
+ MAT_SET_X(matrix,xaxis);
+
+ //y axis
+ xaxis = zaxis.cross(xaxis);
+ MAT_SET_Y(matrix,xaxis);
+
+ triangle_transform.setOrigin(m_vertices[0]);
+ }
+
+
+ //! Test triangles by finding separating axis
+ /*!
+ \param other Triangle for collide
+ \param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle
+ */
+ bool collide_triangle_hard_test(
+ const GIM_TRIANGLE & other,
+ GIM_TRIANGLE_CONTACT_DATA & contact_data) const;
+
+ //! Test boxes before doing hard test
+ /*!
+ \param other Triangle for collide
+ \param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle
+ \
+ */
+ SIMD_FORCE_INLINE bool collide_triangle(
+ const GIM_TRIANGLE & other,
+ GIM_TRIANGLE_CONTACT_DATA & contact_data) const
+ {
+ //test box collisioin
+ GIM_AABB boxu(m_vertices[0],m_vertices[1],m_vertices[2],m_margin);
+ GIM_AABB boxv(other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin);
+ if(!boxu.has_collision(boxv)) return false;
+
+ //do hard test
+ return collide_triangle_hard_test(other,contact_data);
+ }
+
+ /*!
+
+ Solve the System for u,v parameters:
+
+ u*axe1[i1] + v*axe2[i1] = vecproj[i1]
+ u*axe1[i2] + v*axe2[i2] = vecproj[i2]
+
+ sustitute:
+ v = (vecproj[i2] - u*axe1[i2])/axe2[i2]
+
+ then the first equation in terms of 'u':
+
+ --> u*axe1[i1] + ((vecproj[i2] - u*axe1[i2])/axe2[i2])*axe2[i1] = vecproj[i1]
+
+ --> u*axe1[i1] + vecproj[i2]*axe2[i1]/axe2[i2] - u*axe1[i2]*axe2[i1]/axe2[i2] = vecproj[i1]
+
+ --> u*(axe1[i1] - axe1[i2]*axe2[i1]/axe2[i2]) = vecproj[i1] - vecproj[i2]*axe2[i1]/axe2[i2]
+
+ --> u*((axe1[i1]*axe2[i2] - axe1[i2]*axe2[i1])/axe2[i2]) = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1])/axe2[i2]
+
+ --> u*(axe1[i1]*axe2[i2] - axe1[i2]*axe2[i1]) = vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1]
+
+ --> u = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1]) /(axe1[i1]*axe2[i2] - axe1[i2]*axe2[i1])
+
+if 0.0<= u+v <=1.0 then they are inside of triangle
+
+ \return false if the point is outside of triangle.This function doesn't take the margin
+ */
+ SIMD_FORCE_INLINE bool get_uv_parameters(
+ const btVector3 & point,
+ const btVector3 & tri_plane,
+ GREAL & u, GREAL & v) const
+ {
+ btVector3 _axe1 = m_vertices[1]-m_vertices[0];
+ btVector3 _axe2 = m_vertices[2]-m_vertices[0];
+ btVector3 _vecproj = point - m_vertices[0];
+ GUINT _i1 = (tri_plane.closestAxis()+1)%3;
+ GUINT _i2 = (_i1+1)%3;
+ if(btFabs(_axe2[_i2])<G_EPSILON)
+ {
+ u = (_vecproj[_i2]*_axe2[_i1] - _vecproj[_i1]*_axe2[_i2]) /(_axe1[_i2]*_axe2[_i1] - _axe1[_i1]*_axe2[_i2]);
+ v = (_vecproj[_i1] - u*_axe1[_i1])/_axe2[_i1];
+ }
+ else
+ {
+ u = (_vecproj[_i1]*_axe2[_i2] - _vecproj[_i2]*_axe2[_i1]) /(_axe1[_i1]*_axe2[_i2] - _axe1[_i2]*_axe2[_i1]);
+ v = (_vecproj[_i2] - u*_axe1[_i2])/_axe2[_i2];
+ }
+
+ if(u<-G_EPSILON)
+ {
+ return false;
+ }
+ else if(v<-G_EPSILON)
+ {
+ return false;
+ }
+ else
+ {
+ btScalar sumuv;
+ sumuv = u+v;
+ if(sumuv<-G_EPSILON)
+ {
+ return false;
+ }
+ else if(sumuv-1.0f>G_EPSILON)
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ //! is point in triangle beam?
+ /*!
+ Test if point is in triangle, with m_margin tolerance
+ */
+ SIMD_FORCE_INLINE bool is_point_inside(const btVector3 & point, const btVector3 & tri_normal) const
+ {
+ //Test with edge 0
+ btVector4 edge_plane;
+ this->get_edge_plane(0,tri_normal,edge_plane);
+ GREAL dist = DISTANCE_PLANE_POINT(edge_plane,point);
+ if(dist-m_margin>0.0f) return false; // outside plane
+
+ this->get_edge_plane(1,tri_normal,edge_plane);
+ dist = DISTANCE_PLANE_POINT(edge_plane,point);
+ if(dist-m_margin>0.0f) return false; // outside plane
+
+ this->get_edge_plane(2,tri_normal,edge_plane);
+ dist = DISTANCE_PLANE_POINT(edge_plane,point);
+ if(dist-m_margin>0.0f) return false; // outside plane
+ return true;
+ }
+
+
+ //! Bidireccional ray collision
+ SIMD_FORCE_INLINE bool ray_collision(
+ const btVector3 & vPoint,
+ const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal,
+ GREAL & tparam, GREAL tmax = G_REAL_INFINITY)
+ {
+ btVector4 faceplane;
+ {
+ btVector3 dif1 = m_vertices[1] - m_vertices[0];
+ btVector3 dif2 = m_vertices[2] - m_vertices[0];
+ VEC_CROSS(faceplane,dif1,dif2);
+ faceplane[3] = m_vertices[0].dot(faceplane);
+ }
+
+ GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax);
+ if(res == 0) return false;
+ if(! is_point_inside(pout,faceplane)) return false;
+
+ if(res==2) //invert normal
+ {
+ triangle_normal.setValue(-faceplane[0],-faceplane[1],-faceplane[2]);
+ }
+ else
+ {
+ triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]);
+ }
+
+ VEC_NORMALIZE(triangle_normal);
+
+ return true;
+ }
+
+
+ //! one direccion ray collision
+ SIMD_FORCE_INLINE bool ray_collision_front_side(
+ const btVector3 & vPoint,
+ const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal,
+ GREAL & tparam, GREAL tmax = G_REAL_INFINITY)
+ {
+ btVector4 faceplane;
+ {
+ btVector3 dif1 = m_vertices[1] - m_vertices[0];
+ btVector3 dif2 = m_vertices[2] - m_vertices[0];
+ VEC_CROSS(faceplane,dif1,dif2);
+ faceplane[3] = m_vertices[0].dot(faceplane);
+ }
+
+ GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax);
+ if(res != 1) return false;
+
+ if(!is_point_inside(pout,faceplane)) return false;
+
+ triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]);
+
+ VEC_NORMALIZE(triangle_normal);
+
+ return true;
+ }
+
+};
+
+
+
+
+#endif // GIM_TRI_COLLISION_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,52 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef CONTINUOUS_COLLISION_CONVEX_CAST_H
+#define CONTINUOUS_COLLISION_CONVEX_CAST_H
+
+#include "btConvexCast.h"
+#include "btSimplexSolverInterface.h"
+class btConvexPenetrationDepthSolver;
+class btConvexShape;
+
+/// btContinuousConvexCollision implements angular and linear time of impact for convex objects.
+/// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis).
+/// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent.
+/// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops
+class btContinuousConvexCollision : public btConvexCast
+{
+ btSimplexSolverInterface* m_simplexSolver;
+ btConvexPenetrationDepthSolver* m_penetrationDepthSolver;
+ const btConvexShape* m_convexA;
+ const btConvexShape* m_convexB;
+
+
+public:
+
+ btContinuousConvexCollision (const btConvexShape* shapeA,const btConvexShape* shapeB ,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver);
+
+ virtual bool calcTimeOfImpact(
+ const btTransform& fromA,
+ const btTransform& toA,
+ const btTransform& fromB,
+ const btTransform& toB,
+ CastResult& result);
+
+
+};
+
+#endif //CONTINUOUS_COLLISION_CONVEX_CAST_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btConvexCast.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btConvexCast.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btConvexCast.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btConvexCast.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,73 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef CONVEX_CAST_H
+#define CONVEX_CAST_H
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btScalar.h"
+class btMinkowskiSumShape;
+#include "LinearMath/btIDebugDraw.h"
+
+/// btConvexCast is an interface for Casting
+class btConvexCast
+{
+public:
+
+
+ virtual ~btConvexCast();
+
+ ///RayResult stores the closest result
+ /// alternatively, add a callback method to decide about closest/all results
+ struct CastResult
+ {
+ //virtual bool addRayResult(const btVector3& normal,btScalar fraction) = 0;
+
+ virtual void DebugDraw(btScalar fraction) {(void)fraction;}
+ virtual void drawCoordSystem(const btTransform& trans) {(void)trans;}
+
+ CastResult()
+ :m_fraction(btScalar(BT_LARGE_FLOAT)),
+ m_debugDrawer(0),
+ m_allowedPenetration(btScalar(0))
+ {
+ }
+
+
+ virtual ~CastResult() {};
+
+ btTransform m_hitTransformA;
+ btTransform m_hitTransformB;
+ btVector3 m_normal;
+ btVector3 m_hitPoint;
+ btScalar m_fraction; //input and output
+ btIDebugDraw* m_debugDrawer;
+ btScalar m_allowedPenetration;
+
+ };
+
+
+ /// cast a convex against another convex object
+ virtual bool calcTimeOfImpact(
+ const btTransform& fromA,
+ const btTransform& toA,
+ const btTransform& fromB,
+ const btTransform& toB,
+ CastResult& result) = 0;
+};
+
+#endif //CONVEX_CAST_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,42 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef __CONVEX_PENETRATION_DEPTH_H
+#define __CONVEX_PENETRATION_DEPTH_H
+
+class btStackAlloc;
+class btVector3;
+#include "btSimplexSolverInterface.h"
+class btConvexShape;
+class btTransform;
+
+///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
+class btConvexPenetrationDepthSolver
+{
+public:
+
+ virtual ~btConvexPenetrationDepthSolver() {};
+ virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver,
+ const btConvexShape* convexA,const btConvexShape* convexB,
+ const btTransform& transA,const btTransform& transB,
+ btVector3& v, btVector3& pa, btVector3& pb,
+ class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
+ ) = 0;
+
+
+};
+#endif //CONVEX_PENETRATION_DEPTH_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,89 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
+#define DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+class btStackAlloc;
+
+/// This interface is made to be used by an iterative approach to do TimeOfImpact calculations
+/// This interface allows to query for closest points and penetration depth between two (convex) objects
+/// the closest point is on the second object (B), and the normal points from the surface on B towards A.
+/// distance is between closest points on B and closest point on A. So you can calculate closest point on A
+/// by taking closestPointInA = closestPointInB + m_distance * m_normalOnSurfaceB
+struct btDiscreteCollisionDetectorInterface
+{
+
+ struct Result
+ {
+
+ virtual ~Result(){}
+
+ ///setShapeIdentifiersA/B provides experimental support for per-triangle material / custom material combiner
+ virtual void setShapeIdentifiersA(int partId0,int index0)=0;
+ virtual void setShapeIdentifiersB(int partId1,int index1)=0;
+ virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)=0;
+ };
+
+ struct ClosestPointInput
+ {
+ ClosestPointInput()
+ :m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT)),
+ m_stackAlloc(0)
+ {
+ }
+
+ btTransform m_transformA;
+ btTransform m_transformB;
+ btScalar m_maximumDistanceSquared;
+ btStackAlloc* m_stackAlloc;
+ };
+
+ virtual ~btDiscreteCollisionDetectorInterface() {};
+
+ //
+ // give either closest points (distance > 0) or penetration (distance)
+ // the normal always points from B towards A
+ //
+ virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false) = 0;
+
+};
+
+struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result
+{
+ btVector3 m_normalOnSurfaceB;
+ btVector3 m_closestPointInB;
+ btScalar m_distance; //negative means penetration !
+
+ btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT))
+ {
+
+ }
+ virtual ~btStorageResult() {};
+
+ virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+ {
+ if (depth < m_distance)
+ {
+ m_normalOnSurfaceB = normalOnBInWorld;
+ m_closestPointInB = pointInWorld;
+ m_distance = depth;
+ }
+ }
+};
+
+#endif //DISCRETE_COLLISION_DETECTOR_INTERFACE1_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,50 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef GJK_CONVEX_CAST_H
+#define GJK_CONVEX_CAST_H
+
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+
+#include "LinearMath/btVector3.h"
+#include "btConvexCast.h"
+class btConvexShape;
+class btMinkowskiSumShape;
+#include "btSimplexSolverInterface.h"
+
+///GjkConvexCast performs a raycast on a convex object using support mapping.
+class btGjkConvexCast : public btConvexCast
+{
+ btSimplexSolverInterface* m_simplexSolver;
+ const btConvexShape* m_convexA;
+ const btConvexShape* m_convexB;
+
+public:
+
+ btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver);
+
+ /// cast a convex against another convex object
+ virtual bool calcTimeOfImpact(
+ const btTransform& fromA,
+ const btTransform& toA,
+ const btTransform& fromB,
+ const btTransform& toB,
+ CastResult& result);
+
+};
+
+#endif //GJK_CONVEX_CAST_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,73 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the
+use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it
+freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not
+claim that you wrote the original software. If you use this software in a
+product, an acknowledgment in the product documentation would be appreciated
+but is not required.
+2. Altered source versions must be plainly marked as such, and must not be
+misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+GJK-EPA collision solver by Nathanael Presson, 2008
+*/
+#ifndef _68DA1F85_90B7_4bb0_A705_83B4040A75C6_
+#define _68DA1F85_90B7_4bb0_A705_83B4040A75C6_
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+
+///btGjkEpaSolver contributed under zlib by Nathanael Presson
+struct btGjkEpaSolver2
+{
+struct sResults
+ {
+ enum eStatus
+ {
+ Separated, /* Shapes doesnt penetrate */
+ Penetrating, /* Shapes are penetrating */
+ GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */
+ EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */
+ } status;
+ btVector3 witnesses[2];
+ btVector3 normal;
+ btScalar distance;
+ };
+
+static int StackSizeRequirement();
+
+static bool Distance( const btConvexShape* shape0,const btTransform& wtrs0,
+ const btConvexShape* shape1,const btTransform& wtrs1,
+ const btVector3& guess,
+ sResults& results);
+
+static bool Penetration(const btConvexShape* shape0,const btTransform& wtrs0,
+ const btConvexShape* shape1,const btTransform& wtrs1,
+ const btVector3& guess,
+ sResults& results,
+ bool usemargins=true);
+#ifndef __SPU__
+static btScalar SignedDistance( const btVector3& position,
+ btScalar margin,
+ const btConvexShape* shape,
+ const btTransform& wtrs,
+ sResults& results);
+
+static bool SignedDistance( const btConvexShape* shape0,const btTransform& wtrs0,
+ const btConvexShape* shape1,const btTransform& wtrs1,
+ const btVector3& guess,
+ sResults& results);
+#endif //__SPU__
+
+};
+
+#endif
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,43 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#ifndef BT_GJP_EPA_PENETRATION_DEPTH_H
+#define BT_GJP_EPA_PENETRATION_DEPTH_H
+
+#include "btConvexPenetrationDepthSolver.h"
+
+///EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to
+///calculate the penetration depth between two convex shapes.
+class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver
+{
+ public :
+
+ btGjkEpaPenetrationDepthSolver()
+ {
+ }
+
+ bool calcPenDepth( btSimplexSolverInterface& simplexSolver,
+ const btConvexShape* pConvexA, const btConvexShape* pConvexB,
+ const btTransform& transformA, const btTransform& transformB,
+ btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
+ class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc );
+
+ private :
+
+};
+
+#endif // BT_GJP_EPA_PENETRATION_DEPTH_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,103 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+
+#ifndef GJK_PAIR_DETECTOR_H
+#define GJK_PAIR_DETECTOR_H
+
+#include "btDiscreteCollisionDetectorInterface.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+
+class btConvexShape;
+#include "btSimplexSolverInterface.h"
+class btConvexPenetrationDepthSolver;
+
+/// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
+class btGjkPairDetector : public btDiscreteCollisionDetectorInterface
+{
+
+
+ btVector3 m_cachedSeparatingAxis;
+ btConvexPenetrationDepthSolver* m_penetrationDepthSolver;
+ btSimplexSolverInterface* m_simplexSolver;
+ const btConvexShape* m_minkowskiA;
+ const btConvexShape* m_minkowskiB;
+ int m_shapeTypeA;
+ int m_shapeTypeB;
+ btScalar m_marginA;
+ btScalar m_marginB;
+
+ bool m_ignoreMargin;
+ btScalar m_cachedSeparatingDistance;
+
+
+public:
+
+ //some debugging to fix degeneracy problems
+ int m_lastUsedMethod;
+ int m_curIter;
+ int m_degenerateSimplex;
+ int m_catchDegeneracies;
+
+
+ btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver);
+ btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver);
+ virtual ~btGjkPairDetector() {};
+
+ virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
+
+ void getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw);
+
+
+ void setMinkowskiA(btConvexShape* minkA)
+ {
+ m_minkowskiA = minkA;
+ }
+
+ void setMinkowskiB(btConvexShape* minkB)
+ {
+ m_minkowskiB = minkB;
+ }
+ void setCachedSeperatingAxis(const btVector3& seperatingAxis)
+ {
+ m_cachedSeparatingAxis = seperatingAxis;
+ }
+
+ const btVector3& getCachedSeparatingAxis() const
+ {
+ return m_cachedSeparatingAxis;
+ }
+ btScalar getCachedSeparatingDistance() const
+ {
+ return m_cachedSeparatingDistance;
+ }
+
+ void setPenetrationDepthSolver(btConvexPenetrationDepthSolver* penetrationDepthSolver)
+ {
+ m_penetrationDepthSolver = penetrationDepthSolver;
+ }
+
+ ///don't use setIgnoreMargin, it's for Bullet's internal use
+ void setIgnoreMargin(bool ignoreMargin)
+ {
+ m_ignoreMargin = ignoreMargin;
+ }
+
+
+};
+
+#endif //GJK_PAIR_DETECTOR_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,125 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef MANIFOLD_CONTACT_POINT_H
+#define MANIFOLD_CONTACT_POINT_H
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransformUtil.h"
+
+
+
+
+
+/// ManifoldContactPoint collects and maintains persistent contactpoints.
+/// used to improve stability and performance of rigidbody dynamics response.
+class btManifoldPoint
+ {
+ public:
+ btManifoldPoint()
+ :m_userPersistentData(0),
+ m_appliedImpulse(0.f),
+ m_lateralFrictionInitialized(false),
+ m_appliedImpulseLateral1(0.f),
+ m_appliedImpulseLateral2(0.f),
+ m_lifeTime(0)
+ {
+ }
+
+ btManifoldPoint( const btVector3 &pointA, const btVector3 &pointB,
+ const btVector3 &normal,
+ btScalar distance ) :
+ m_localPointA( pointA ),
+ m_localPointB( pointB ),
+ m_normalWorldOnB( normal ),
+ m_distance1( distance ),
+ m_combinedFriction(btScalar(0.)),
+ m_combinedRestitution(btScalar(0.)),
+ m_userPersistentData(0),
+ m_appliedImpulse(0.f),
+ m_lateralFrictionInitialized(false),
+ m_appliedImpulseLateral1(0.f),
+ m_appliedImpulseLateral2(0.f),
+ m_lifeTime(0)
+ {
+
+
+ }
+
+
+
+ btVector3 m_localPointA;
+ btVector3 m_localPointB;
+ btVector3 m_positionWorldOnB;
+ ///m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity
+ btVector3 m_positionWorldOnA;
+ btVector3 m_normalWorldOnB;
+
+ btScalar m_distance1;
+ btScalar m_combinedFriction;
+ btScalar m_combinedRestitution;
+
+ //BP mod, store contact triangles.
+ int m_partId0;
+ int m_partId1;
+ int m_index0;
+ int m_index1;
+
+ mutable void* m_userPersistentData;
+ btScalar m_appliedImpulse;
+
+ bool m_lateralFrictionInitialized;
+ btScalar m_appliedImpulseLateral1;
+ btScalar m_appliedImpulseLateral2;
+ int m_lifeTime;//lifetime of the contactpoint in frames
+
+ btVector3 m_lateralFrictionDir1;
+ btVector3 m_lateralFrictionDir2;
+
+ btScalar getDistance() const
+ {
+ return m_distance1;
+ }
+ int getLifeTime() const
+ {
+ return m_lifeTime;
+ }
+
+ const btVector3& getPositionWorldOnA() const {
+ return m_positionWorldOnA;
+// return m_positionWorldOnB + m_normalWorldOnB * m_distance1;
+ }
+
+ const btVector3& getPositionWorldOnB() const
+ {
+ return m_positionWorldOnB;
+ }
+
+ void setDistance(btScalar dist)
+ {
+ m_distance1 = dist;
+ }
+
+ ///this returns the most recent applied impulse, to satisfy contact constraints by the constraint solver
+ btScalar getAppliedImpulse() const
+ {
+ return m_appliedImpulse;
+ }
+
+
+
+ };
+
+#endif //MANIFOLD_CONTACT_POINT_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,36 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+#define MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+
+#include "btConvexPenetrationDepthSolver.h"
+
+///MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation.
+///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points.
+class btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver
+{
+public:
+
+ virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver,
+ const btConvexShape* convexA,const btConvexShape* convexB,
+ const btTransform& transA,const btTransform& transB,
+ btVector3& v, btVector3& pa, btVector3& pb,
+ class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
+ );
+};
+
+#endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,207 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef PERSISTENT_MANIFOLD_H
+#define PERSISTENT_MANIFOLD_H
+
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "btManifoldPoint.h"
+#include "LinearMath/btAlignedAllocator.h"
+
+struct btCollisionResult;
+
+///maximum contact breaking and merging threshold
+extern btScalar gContactBreakingThreshold;
+
+typedef bool (*ContactDestroyedCallback)(void* userPersistentData);
+typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp,void* body0,void* body1);
+extern ContactDestroyedCallback gContactDestroyedCallback;
+
+
+enum btContactManifoldTypes
+{
+ BT_PERSISTENT_MANIFOLD_TYPE = 1,
+ MAX_CONTACT_MANIFOLD_TYPE
+};
+
+#define MANIFOLD_CACHE_SIZE 4
+
+///btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping in the broadphase.
+///Those contact points are created by the collision narrow phase.
+///The cache can be empty, or hold 1,2,3 or 4 points. Some collision algorithms (GJK) might only add one point at a time.
+///updates/refreshes old contact points, and throw them away if necessary (distance becomes too large)
+///reduces the cache to 4 points, when more then 4 points are added, using following rules:
+///the contact point with deepest penetration is always kept, and it tries to maximuze the area covered by the points
+///note that some pairs of objects might have more then one contact manifold.
+ATTRIBUTE_ALIGNED16( class) btPersistentManifold : public btTypedObject
+{
+
+ btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE];
+
+ /// this two body pointers can point to the physics rigidbody class.
+ /// void* will allow any rigidbody class
+ void* m_body0;
+ void* m_body1;
+ int m_cachedPoints;
+
+ btScalar m_contactBreakingThreshold;
+ btScalar m_contactProcessingThreshold;
+
+
+ /// sort cached points so most isolated points come first
+ int sortCachedPoints(const btManifoldPoint& pt);
+
+ int findContactPoint(const btManifoldPoint* unUsed, int numUnused,const btManifoldPoint& pt);
+
+public:
+
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ int m_index1a;
+
+ btPersistentManifold();
+
+ btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold,btScalar contactProcessingThreshold)
+ : btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
+ m_body0(body0),m_body1(body1),m_cachedPoints(0),
+ m_contactBreakingThreshold(contactBreakingThreshold),
+ m_contactProcessingThreshold(contactProcessingThreshold)
+ {
+ }
+
+ SIMD_FORCE_INLINE void* getBody0() { return m_body0;}
+ SIMD_FORCE_INLINE void* getBody1() { return m_body1;}
+
+ SIMD_FORCE_INLINE const void* getBody0() const { return m_body0;}
+ SIMD_FORCE_INLINE const void* getBody1() const { return m_body1;}
+
+ void setBodies(void* body0,void* body1)
+ {
+ m_body0 = body0;
+ m_body1 = body1;
+ }
+
+ void clearUserCache(btManifoldPoint& pt);
+
+#ifdef DEBUG_PERSISTENCY
+ void DebugPersistency();
+#endif //
+
+ SIMD_FORCE_INLINE int getNumContacts() const { return m_cachedPoints;}
+
+ SIMD_FORCE_INLINE const btManifoldPoint& getContactPoint(int index) const
+ {
+ btAssert(index < m_cachedPoints);
+ return m_pointCache[index];
+ }
+
+ SIMD_FORCE_INLINE btManifoldPoint& getContactPoint(int index)
+ {
+ btAssert(index < m_cachedPoints);
+ return m_pointCache[index];
+ }
+
+ ///@todo: get this margin from the current physics / collision environment
+ btScalar getContactBreakingThreshold() const;
+
+ btScalar getContactProcessingThreshold() const
+ {
+ return m_contactProcessingThreshold;
+ }
+
+ int getCacheEntry(const btManifoldPoint& newPoint) const;
+
+ int addManifoldPoint( const btManifoldPoint& newPoint);
+
+ void removeContactPoint (int index)
+ {
+ clearUserCache(m_pointCache[index]);
+
+ int lastUsedIndex = getNumContacts() - 1;
+// m_pointCache[index] = m_pointCache[lastUsedIndex];
+ if(index != lastUsedIndex)
+ {
+ m_pointCache[index] = m_pointCache[lastUsedIndex];
+ //get rid of duplicated userPersistentData pointer
+ m_pointCache[lastUsedIndex].m_userPersistentData = 0;
+ m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f;
+ m_pointCache[lastUsedIndex].m_lateralFrictionInitialized = false;
+ m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f;
+ m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f;
+ m_pointCache[lastUsedIndex].m_lifeTime = 0;
+ }
+
+ btAssert(m_pointCache[lastUsedIndex].m_userPersistentData==0);
+ m_cachedPoints--;
+ }
+ void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex)
+ {
+ btAssert(validContactDistance(newPoint));
+
+#define MAINTAIN_PERSISTENCY 1
+#ifdef MAINTAIN_PERSISTENCY
+ int lifeTime = m_pointCache[insertIndex].getLifeTime();
+ btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
+ btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
+ btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
+
+ btAssert(lifeTime>=0);
+ void* cache = m_pointCache[insertIndex].m_userPersistentData;
+
+ m_pointCache[insertIndex] = newPoint;
+
+ m_pointCache[insertIndex].m_userPersistentData = cache;
+ m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
+ m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
+ m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
+
+ m_pointCache[insertIndex].m_lifeTime = lifeTime;
+#else
+ clearUserCache(m_pointCache[insertIndex]);
+ m_pointCache[insertIndex] = newPoint;
+
+#endif
+ }
+
+ bool validContactDistance(const btManifoldPoint& pt) const
+ {
+ return pt.m_distance1 <= getContactBreakingThreshold();
+ }
+ /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
+ void refreshContactPoints( const btTransform& trA,const btTransform& trB);
+
+
+ SIMD_FORCE_INLINE void clearManifold()
+ {
+ int i;
+ for (i=0;i<m_cachedPoints;i++)
+ {
+ clearUserCache(m_pointCache[i]);
+ }
+ m_cachedPoints = 0;
+ }
+
+
+
+}
+;
+
+
+
+
+
+#endif //PERSISTENT_MANIFOLD_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btPointCollector.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btPointCollector.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btPointCollector.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btPointCollector.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,64 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef POINT_COLLECTOR_H
+#define POINT_COLLECTOR_H
+
+#include "btDiscreteCollisionDetectorInterface.h"
+
+
+
+struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result
+{
+
+
+ btVector3 m_normalOnBInWorld;
+ btVector3 m_pointInWorld;
+ btScalar m_distance;//negative means penetration
+
+ bool m_hasResult;
+
+ btPointCollector ()
+ : m_distance(btScalar(BT_LARGE_FLOAT)),m_hasResult(false)
+ {
+ }
+
+ virtual void setShapeIdentifiersA(int partId0,int index0)
+ {
+ (void)partId0;
+ (void)index0;
+
+ }
+ virtual void setShapeIdentifiersB(int partId1,int index1)
+ {
+ (void)partId1;
+ (void)index1;
+ }
+
+ virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+ {
+ if (depth< m_distance)
+ {
+ m_hasResult = true;
+ m_normalOnBInWorld = normalOnBInWorld;
+ m_pointInWorld = pointInWorld;
+ //negative means penetration
+ m_distance = depth;
+ }
+ }
+};
+
+#endif //POINT_COLLECTOR_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,71 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef RAYCAST_TRI_CALLBACK_H
+#define RAYCAST_TRI_CALLBACK_H
+
+#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+#include "LinearMath/btTransform.h"
+struct btBroadphaseProxy;
+class btConvexShape;
+
+class btTriangleRaycastCallback: public btTriangleCallback
+{
+public:
+
+ //input
+ btVector3 m_from;
+ btVector3 m_to;
+
+ //@BP Mod - allow backface filtering and unflipped normals
+ enum EFlags
+ {
+ kF_None = 0,
+ kF_FilterBackfaces = 1 << 0,
+ kF_KeepUnflippedNormal = 1 << 1, // Prevents returned face normal getting flipped when a ray hits a back-facing triangle
+
+ kF_Terminator = 0xFFFFFFFF
+ };
+ unsigned int m_flags;
+
+ btScalar m_hitFraction;
+
+ btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags=0);
+
+ virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
+
+ virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) = 0;
+
+};
+
+class btTriangleConvexcastCallback : public btTriangleCallback
+{
+public:
+ const btConvexShape* m_convexShape;
+ btTransform m_convexShapeFrom;
+ btTransform m_convexShapeTo;
+ btTransform m_triangleToWorld;
+ btScalar m_hitFraction;
+ btScalar m_triangleCollisionMargin;
+
+ btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin);
+
+ virtual void processTriangle (btVector3* triangle, int partId, int triangleIndex);
+
+ virtual btScalar reportHit (const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0;
+};
+
+#endif //RAYCAST_TRI_CALLBACK_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,63 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef SIMPLEX_SOLVER_INTERFACE_H
+#define SIMPLEX_SOLVER_INTERFACE_H
+
+#include "LinearMath/btVector3.h"
+
+#define NO_VIRTUAL_INTERFACE 1
+#ifdef NO_VIRTUAL_INTERFACE
+#include "btVoronoiSimplexSolver.h"
+#define btSimplexSolverInterface btVoronoiSimplexSolver
+#else
+
+/// btSimplexSolverInterface can incrementally calculate distance between origin and up to 4 vertices
+/// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on
+/// voronoi regions or barycentric coordinates
+class btSimplexSolverInterface
+{
+ public:
+ virtual ~btSimplexSolverInterface() {};
+
+ virtual void reset() = 0;
+
+ virtual void addVertex(const btVector3& w, const btVector3& p, const btVector3& q) = 0;
+
+ virtual bool closest(btVector3& v) = 0;
+
+ virtual btScalar maxVertex() = 0;
+
+ virtual bool fullSimplex() const = 0;
+
+ virtual int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const = 0;
+
+ virtual bool inSimplex(const btVector3& w) = 0;
+
+ virtual void backup_closest(btVector3& v) = 0;
+
+ virtual bool emptySimplex() const = 0;
+
+ virtual void compute_points(btVector3& p1, btVector3& p2) = 0;
+
+ virtual int numVertices() const =0;
+
+
+};
+#endif
+#endif //SIMPLEX_SOLVER_INTERFACE_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,50 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef SUBSIMPLEX_CONVEX_CAST_H
+#define SUBSIMPLEX_CONVEX_CAST_H
+
+#include "btConvexCast.h"
+#include "btSimplexSolverInterface.h"
+class btConvexShape;
+
+/// btSubsimplexConvexCast implements Gino van den Bergens' paper
+///"Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection"
+/// GJK based Ray Cast, optimized version
+/// Objects should not start in overlap, otherwise results are not defined.
+class btSubsimplexConvexCast : public btConvexCast
+{
+ btSimplexSolverInterface* m_simplexSolver;
+ const btConvexShape* m_convexA;
+ const btConvexShape* m_convexB;
+
+public:
+
+ btSubsimplexConvexCast (const btConvexShape* shapeA,const btConvexShape* shapeB,btSimplexSolverInterface* simplexSolver);
+
+ //virtual ~btSubsimplexConvexCast();
+ ///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
+ ///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector.
+ virtual bool calcTimeOfImpact(
+ const btTransform& fromA,
+ const btTransform& toA,
+ const btTransform& fromB,
+ const btTransform& toB,
+ CastResult& result);
+
+};
+
+#endif //SUBSIMPLEX_CONVEX_CAST_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,157 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef btVoronoiSimplexSolver_H
+#define btVoronoiSimplexSolver_H
+
+#include "btSimplexSolverInterface.h"
+
+
+
+#define VORONOI_SIMPLEX_MAX_VERTS 5
+
+struct btUsageBitfield{
+ btUsageBitfield()
+ {
+ reset();
+ }
+
+ void reset()
+ {
+ usedVertexA = false;
+ usedVertexB = false;
+ usedVertexC = false;
+ usedVertexD = false;
+ }
+ unsigned short usedVertexA : 1;
+ unsigned short usedVertexB : 1;
+ unsigned short usedVertexC : 1;
+ unsigned short usedVertexD : 1;
+ unsigned short unused1 : 1;
+ unsigned short unused2 : 1;
+ unsigned short unused3 : 1;
+ unsigned short unused4 : 1;
+};
+
+
+struct btSubSimplexClosestResult
+{
+ btVector3 m_closestPointOnSimplex;
+ //MASK for m_usedVertices
+ //stores the simplex vertex-usage, using the MASK,
+ // if m_usedVertices & MASK then the related vertex is used
+ btUsageBitfield m_usedVertices;
+ btScalar m_barycentricCoords[4];
+ bool m_degenerate;
+
+ void reset()
+ {
+ m_degenerate = false;
+ setBarycentricCoordinates();
+ m_usedVertices.reset();
+ }
+ bool isValid()
+ {
+ bool valid = (m_barycentricCoords[0] >= btScalar(0.)) &&
+ (m_barycentricCoords[1] >= btScalar(0.)) &&
+ (m_barycentricCoords[2] >= btScalar(0.)) &&
+ (m_barycentricCoords[3] >= btScalar(0.));
+
+
+ return valid;
+ }
+ void setBarycentricCoordinates(btScalar a=btScalar(0.),btScalar b=btScalar(0.),btScalar c=btScalar(0.),btScalar d=btScalar(0.))
+ {
+ m_barycentricCoords[0] = a;
+ m_barycentricCoords[1] = b;
+ m_barycentricCoords[2] = c;
+ m_barycentricCoords[3] = d;
+ }
+
+};
+
+/// btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin.
+/// Can be used with GJK, as an alternative to Johnson distance algorithm.
+#ifdef NO_VIRTUAL_INTERFACE
+class btVoronoiSimplexSolver
+#else
+class btVoronoiSimplexSolver : public btSimplexSolverInterface
+#endif
+{
+public:
+
+ int m_numVertices;
+
+ btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS];
+ btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];
+ btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];
+
+
+
+ btVector3 m_cachedP1;
+ btVector3 m_cachedP2;
+ btVector3 m_cachedV;
+ btVector3 m_lastW;
+ bool m_cachedValidClosest;
+
+ btSubSimplexClosestResult m_cachedBC;
+
+ bool m_needsUpdate;
+
+ void removeVertex(int index);
+ void reduceVertices (const btUsageBitfield& usedVerts);
+ bool updateClosestVectorAndPoints();
+
+ bool closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult);
+ int pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d);
+ bool closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result);
+
+public:
+
+ void reset();
+
+ void addVertex(const btVector3& w, const btVector3& p, const btVector3& q);
+
+
+ bool closest(btVector3& v);
+
+ btScalar maxVertex();
+
+ bool fullSimplex() const
+ {
+ return (m_numVertices == 4);
+ }
+
+ int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const;
+
+ bool inSimplex(const btVector3& w);
+
+ void backup_closest(btVector3& v) ;
+
+ bool emptySimplex() const ;
+
+ void compute_points(btVector3& p1, btVector3& p2) ;
+
+ int numVertices() const
+ {
+ return m_numVertices;
+ }
+
+
+};
+
+#endif //VoronoiSimplexSolver
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Character/btCharacterControllerInterface.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Character/btCharacterControllerInterface.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Character/btCharacterControllerInterface.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Character/btCharacterControllerInterface.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,45 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CHARACTER_CONTROLLER_INTERFACE_H
+#define CHARACTER_CONTROLLER_INTERFACE_H
+
+#include "LinearMath/btVector3.h"
+#include "BulletDynamics/Dynamics/btActionInterface.h"
+
+class btCollisionShape;
+class btRigidBody;
+class btCollisionWorld;
+
+class btCharacterControllerInterface : public btActionInterface
+{
+public:
+ btCharacterControllerInterface () {};
+ virtual ~btCharacterControllerInterface () {};
+
+ virtual void setWalkDirection(const btVector3& walkDirection) = 0;
+ virtual void setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) = 0;
+ virtual void reset () = 0;
+ virtual void warp (const btVector3& origin) = 0;
+
+ virtual void preStep ( btCollisionWorld* collisionWorld) = 0;
+ virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0;
+ virtual bool canJump () const = 0;
+ virtual void jump () = 0;
+
+ virtual bool onGround () const = 0;
+};
+
+#endif
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Character/btKinematicCharacterController.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Character/btKinematicCharacterController.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Character/btKinematicCharacterController.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Character/btKinematicCharacterController.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,142 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef KINEMATIC_CHARACTER_CONTROLLER_H
+#define KINEMATIC_CHARACTER_CONTROLLER_H
+
+#include "LinearMath/btVector3.h"
+
+#include "btCharacterControllerInterface.h"
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+
+
+class btCollisionShape;
+class btRigidBody;
+class btCollisionWorld;
+class btCollisionDispatcher;
+class btPairCachingGhostObject;
+
+///btKinematicCharacterController is an object that supports a sliding motion in a world.
+///It uses a ghost object and convex sweep test to test for upcoming collisions. This is combined with discrete collision detection to recover from penetrations.
+///Interaction between btKinematicCharacterController and dynamic rigid bodies needs to be explicity implemented by the user.
+class btKinematicCharacterController : public btCharacterControllerInterface
+{
+protected:
+ btScalar m_halfHeight;
+
+ btPairCachingGhostObject* m_ghostObject;
+ btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
+
+ btScalar m_fallSpeed;
+ btScalar m_jumpSpeed;
+ btScalar m_maxJumpHeight;
+
+ btScalar m_turnAngle;
+
+ btScalar m_stepHeight;
+
+ btScalar m_addedMargin;//@todo: remove this and fix the code
+
+ ///this is the desired walk direction, set by the user
+ btVector3 m_walkDirection;
+ btVector3 m_normalizedDirection;
+
+ //some internal variables
+ btVector3 m_currentPosition;
+ btScalar m_currentStepOffset;
+ btVector3 m_targetPosition;
+
+ ///keep track of the contact manifolds
+ btManifoldArray m_manifoldArray;
+
+ bool m_touchingContact;
+ btVector3 m_touchingNormal;
+
+ bool m_useGhostObjectSweepTest;
+ bool m_useWalkDirection;
+ float m_velocityTimeInterval;
+ int m_upAxis;
+
+ btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
+ btVector3 parallelComponent (const btVector3& direction, const btVector3& normal);
+ btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal);
+
+ bool recoverFromPenetration ( btCollisionWorld* collisionWorld);
+ void stepUp (btCollisionWorld* collisionWorld);
+ void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
+ void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove);
+ void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
+public:
+ btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
+ ~btKinematicCharacterController ();
+
+
+ ///btActionInterface interface
+ virtual void updateAction( btCollisionWorld* collisionWorld,btScalar deltaTime)
+ {
+ preStep ( collisionWorld);
+ playerStep (collisionWorld, deltaTime);
+ }
+
+ ///btActionInterface interface
+ void debugDraw(btIDebugDraw* debugDrawer);
+
+ void setUpAxis (int axis)
+ {
+ if (axis < 0)
+ axis = 0;
+ if (axis > 2)
+ axis = 2;
+ m_upAxis = axis;
+ }
+
+ /// This should probably be called setPositionIncrementPerSimulatorStep.
+ /// This is neither a direction nor a velocity, but the amount to
+ /// increment the position each simulation iteration, regardless
+ /// of dt.
+ /// This call will reset any velocity set by setVelocityForTimeInterval().
+ virtual void setWalkDirection(const btVector3& walkDirection);
+
+ /// Caller provides a velocity with which the character should move for
+ /// the given time period. After the time period, velocity is reset
+ /// to zero.
+ /// This call will reset any walk direction set by setWalkDirection().
+ /// Negative time intervals will result in no motion.
+ virtual void setVelocityForTimeInterval(const btVector3& velocity,
+ btScalar timeInterval);
+
+ void reset ();
+ void warp (const btVector3& origin);
+
+ void preStep ( btCollisionWorld* collisionWorld);
+ void playerStep ( btCollisionWorld* collisionWorld, btScalar dt);
+
+ void setFallSpeed (btScalar fallSpeed);
+ void setJumpSpeed (btScalar jumpSpeed);
+ void setMaxJumpHeight (btScalar maxJumpHeight);
+ bool canJump () const;
+ void jump ();
+
+ btPairCachingGhostObject* getGhostObject();
+ void setUseGhostSweepTest(bool useGhostObjectSweepTest)
+ {
+ m_useGhostObjectSweepTest = useGhostObjectSweepTest;
+ }
+
+ bool onGround () const;
+};
+
+#endif // KINEMATIC_CHARACTER_CONTROLLER_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,259 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+Written by: Marcus Hennix
+*/
+
+
+
+/*
+Overview:
+
+btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc).
+It is a fixed translation, 3 degree-of-freedom (DOF) rotational "joint".
+It divides the 3 rotational DOFs into swing (movement within a cone) and twist.
+Swing is divided into swing1 and swing2 which can have different limits, giving an elliptical shape.
+(Note: the cone's base isn't flat, so this ellipse is "embedded" on the surface of a sphere.)
+
+In the contraint's frame of reference:
+twist is along the x-axis,
+and swing 1 and 2 are along the z and y axes respectively.
+*/
+
+
+
+#ifndef CONETWISTCONSTRAINT_H
+#define CONETWISTCONSTRAINT_H
+
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+class btRigidBody;
+
+
+///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
+class btConeTwistConstraint : public btTypedConstraint
+{
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
+ btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
+
+ btTransform m_rbAFrame;
+ btTransform m_rbBFrame;
+
+ btScalar m_limitSoftness;
+ btScalar m_biasFactor;
+ btScalar m_relaxationFactor;
+
+ btScalar m_damping;
+
+ btScalar m_swingSpan1;
+ btScalar m_swingSpan2;
+ btScalar m_twistSpan;
+
+ btScalar m_fixThresh;
+
+ btVector3 m_swingAxis;
+ btVector3 m_twistAxis;
+
+ btScalar m_kSwing;
+ btScalar m_kTwist;
+
+ btScalar m_twistLimitSign;
+ btScalar m_swingCorrection;
+ btScalar m_twistCorrection;
+
+ btScalar m_twistAngle;
+
+ btScalar m_accSwingLimitImpulse;
+ btScalar m_accTwistLimitImpulse;
+
+ bool m_angularOnly;
+ bool m_solveTwistLimit;
+ bool m_solveSwingLimit;
+
+ bool m_useSolveConstraintObsolete;
+
+ // not yet used...
+ btScalar m_swingLimitRatio;
+ btScalar m_twistLimitRatio;
+ btVector3 m_twistAxisA;
+
+ // motor
+ bool m_bMotorEnabled;
+ bool m_bNormalizedMotorStrength;
+ btQuaternion m_qTarget;
+ btScalar m_maxMotorImpulse;
+ btVector3 m_accMotorImpulse;
+
+public:
+
+ btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame);
+
+ btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
+
+ btConeTwistConstraint();
+
+ virtual void buildJacobian();
+
+ virtual void getInfo1 (btConstraintInfo1* info);
+
+ void getInfo1NonVirtual(btConstraintInfo1* info);
+
+ virtual void getInfo2 (btConstraintInfo2* info);
+
+ void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
+
+ virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
+
+ void updateRHS(btScalar timeStep);
+
+ const btRigidBody& getRigidBodyA() const
+ {
+ return m_rbA;
+ }
+ const btRigidBody& getRigidBodyB() const
+ {
+ return m_rbB;
+ }
+
+ void setAngularOnly(bool angularOnly)
+ {
+ m_angularOnly = angularOnly;
+ }
+
+ void setLimit(int limitIndex,btScalar limitValue)
+ {
+ switch (limitIndex)
+ {
+ case 3:
+ {
+ m_twistSpan = limitValue;
+ break;
+ }
+ case 4:
+ {
+ m_swingSpan2 = limitValue;
+ break;
+ }
+ case 5:
+ {
+ m_swingSpan1 = limitValue;
+ break;
+ }
+ default:
+ {
+ }
+ };
+ }
+
+ // setLimit(), a few notes:
+ // _softness:
+ // 0->1, recommend ~0.8->1.
+ // describes % of limits where movement is free.
+ // beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached.
+ // _biasFactor:
+ // 0->1?, recommend 0.3 +/-0.3 or so.
+ // strength with which constraint resists zeroth order (angular, not angular velocity) limit violation.
+ // __relaxationFactor:
+ // 0->1, recommend to stay near 1.
+ // the lower the value, the less the constraint will fight velocities which violate the angular limits.
+ void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+ {
+ m_swingSpan1 = _swingSpan1;
+ m_swingSpan2 = _swingSpan2;
+ m_twistSpan = _twistSpan;
+
+ m_limitSoftness = _softness;
+ m_biasFactor = _biasFactor;
+ m_relaxationFactor = _relaxationFactor;
+ }
+
+ const btTransform& getAFrame() { return m_rbAFrame; };
+ const btTransform& getBFrame() { return m_rbBFrame; };
+
+ inline int getSolveTwistLimit()
+ {
+ return m_solveTwistLimit;
+ }
+
+ inline int getSolveSwingLimit()
+ {
+ return m_solveTwistLimit;
+ }
+
+ inline btScalar getTwistLimitSign()
+ {
+ return m_twistLimitSign;
+ }
+
+ void calcAngleInfo();
+ void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
+
+ inline btScalar getSwingSpan1()
+ {
+ return m_swingSpan1;
+ }
+ inline btScalar getSwingSpan2()
+ {
+ return m_swingSpan2;
+ }
+ inline btScalar getTwistSpan()
+ {
+ return m_twistSpan;
+ }
+ inline btScalar getTwistAngle()
+ {
+ return m_twistAngle;
+ }
+ bool isPastSwingLimit() { return m_solveSwingLimit; }
+
+
+ void setDamping(btScalar damping) { m_damping = damping; }
+
+ void enableMotor(bool b) { m_bMotorEnabled = b; }
+ void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
+ void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
+
+ btScalar getFixThresh() { return m_fixThresh; }
+ void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
+
+ // setMotorTarget:
+ // q: the desired rotation of bodyA wrt bodyB.
+ // note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
+ // note: don't forget to enableMotor()
+ void setMotorTarget(const btQuaternion &q);
+
+ // same as above, but q is the desired rotation of frameA wrt frameB in constraint space
+ void setMotorTargetInConstraintSpace(const btQuaternion &q);
+
+ btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
+
+
+
+protected:
+ void init();
+
+ void computeConeLimitInfo(const btQuaternion& qCone, // in
+ btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
+
+ void computeTwistLimitInfo(const btQuaternion& qTwist, // in
+ btScalar& twistAngle, btVector3& vTwistAxis); // all outs
+
+ void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
+};
+
+#endif //CONETWISTCONSTRAINT_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btConstraintSolver.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btConstraintSolver.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btConstraintSolver.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btConstraintSolver.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,52 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONSTRAINT_SOLVER_H
+#define CONSTRAINT_SOLVER_H
+
+#include "LinearMath/btScalar.h"
+
+class btPersistentManifold;
+class btRigidBody;
+class btCollisionObject;
+class btTypedConstraint;
+struct btContactSolverInfo;
+struct btBroadphaseProxy;
+class btIDebugDraw;
+class btStackAlloc;
+class btDispatcher;
+/// btConstraintSolver provides solver interface
+class btConstraintSolver
+{
+
+public:
+
+ virtual ~btConstraintSolver() {}
+
+ virtual void prepareSolve (int /* numBodies */, int /* numManifolds */) {;}
+
+ ///solve a group of constraints
+ virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher) = 0;
+
+ virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */, btStackAlloc* /* stackAlloc */) {;}
+
+ ///clear internal cached data and reset random seed
+ virtual void reset() = 0;
+};
+
+
+
+
+#endif //CONSTRAINT_SOLVER_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btContactConstraint.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btContactConstraint.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btContactConstraint.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btContactConstraint.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,71 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONTACT_CONSTRAINT_H
+#define CONTACT_CONSTRAINT_H
+
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+
+///btContactConstraint can be automatically created to solve contact constraints using the unified btTypedConstraint interface
+ATTRIBUTE_ALIGNED16(class) btContactConstraint : public btTypedConstraint
+{
+protected:
+
+ btPersistentManifold m_contactManifold;
+
+public:
+
+ btContactConstraint();
+
+ btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB);
+
+ void setContactManifold(btPersistentManifold* contactManifold);
+
+ btPersistentManifold* getContactManifold()
+ {
+ return &m_contactManifold;
+ }
+
+ const btPersistentManifold* getContactManifold() const
+ {
+ return &m_contactManifold;
+ }
+
+ virtual ~btContactConstraint();
+
+ virtual void getInfo1 (btConstraintInfo1* info);
+
+ virtual void getInfo2 (btConstraintInfo2* info);
+
+ ///obsolete methods
+ virtual void buildJacobian();
+
+ ///obsolete methods
+ virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
+
+};
+
+
+///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects
+void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
+ btRigidBody& body2, const btVector3& pos2,
+ btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep);
+
+
+
+#endif //CONTACT_CONSTRAINT_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btContactSolverInfo.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btContactSolverInfo.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btContactSolverInfo.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,85 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONTACT_SOLVER_INFO
+#define CONTACT_SOLVER_INFO
+
+enum btSolverMode
+{
+ SOLVER_RANDMIZE_ORDER = 1,
+ SOLVER_FRICTION_SEPARATE = 2,
+ SOLVER_USE_WARMSTARTING = 4,
+ SOLVER_USE_FRICTION_WARMSTARTING = 8,
+ SOLVER_USE_2_FRICTION_DIRECTIONS = 16,
+ SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
+ SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64,
+ SOLVER_CACHE_FRIENDLY = 128,
+ SOLVER_SIMD = 256, //enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version
+ SOLVER_CUDA = 512 //will be open sourced during Game Developers Conference 2009. Much faster.
+};
+
+struct btContactSolverInfoData
+{
+
+
+ btScalar m_tau;
+ btScalar m_damping;
+ btScalar m_friction;
+ btScalar m_timeStep;
+ btScalar m_restitution;
+ int m_numIterations;
+ btScalar m_maxErrorReduction;
+ btScalar m_sor;
+ btScalar m_erp;//used as Baumgarte factor
+ btScalar m_erp2;//used in Split Impulse
+ btScalar m_globalCfm;//constraint force mixing
+ int m_splitImpulse;
+ btScalar m_splitImpulsePenetrationThreshold;
+ btScalar m_linearSlop;
+ btScalar m_warmstartingFactor;
+
+ int m_solverMode;
+ int m_restingContactRestitutionThreshold;
+
+
+};
+
+struct btContactSolverInfo : public btContactSolverInfoData
+{
+
+
+
+ inline btContactSolverInfo()
+ {
+ m_tau = btScalar(0.6);
+ m_damping = btScalar(1.0);
+ m_friction = btScalar(0.3);
+ m_restitution = btScalar(0.);
+ m_maxErrorReduction = btScalar(20.);
+ m_numIterations = 10;
+ m_erp = btScalar(0.2);
+ m_erp2 = btScalar(0.1);
+ m_globalCfm = btScalar(0.);
+ m_sor = btScalar(1.);
+ m_splitImpulse = false;
+ m_splitImpulsePenetrationThreshold = -0.02f;
+ m_linearSlop = btScalar(0.0);
+ m_warmstartingFactor=btScalar(0.85);
+ m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
+ m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
+ }
+};
+
+#endif //CONTACT_SOLVER_INFO
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,492 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev
+/// Added support for generic constraint solver through getInfo1/getInfo2 methods
+
+/*
+2007-09-09
+btGeneric6DofConstraint Refactored by Francisco Le?n
+email: projectileman at yahoo.com
+http://gimpact.sf.net
+*/
+
+
+#ifndef GENERIC_6DOF_CONSTRAINT_H
+#define GENERIC_6DOF_CONSTRAINT_H
+
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+class btRigidBody;
+
+
+
+
+//! Rotation Limit structure for generic joints
+class btRotationalLimitMotor
+{
+public:
+ //! limit_parameters
+ //!@{
+ btScalar m_loLimit;//!< joint limit
+ btScalar m_hiLimit;//!< joint limit
+ btScalar m_targetVelocity;//!< target motor velocity
+ btScalar m_maxMotorForce;//!< max force on motor
+ btScalar m_maxLimitForce;//!< max force on limit
+ btScalar m_damping;//!< Damping.
+ btScalar m_limitSoftness;//! Relaxation factor
+ btScalar m_ERP;//!< Error tolerance factor when joint is at limit
+ btScalar m_bounce;//!< restitution factor
+ bool m_enableMotor;
+
+ //!@}
+
+ //! temp_variables
+ //!@{
+ btScalar m_currentLimitError;//! How much is violated this limit
+ btScalar m_currentPosition; //! current value of angle
+ int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
+ btScalar m_accumulatedImpulse;
+ //!@}
+
+ btRotationalLimitMotor()
+ {
+ m_accumulatedImpulse = 0.f;
+ m_targetVelocity = 0;
+ m_maxMotorForce = 0.1f;
+ m_maxLimitForce = 300.0f;
+ m_loLimit = 1.0f;
+ m_hiLimit = -1.0f;
+ m_ERP = 0.5f;
+ m_bounce = 0.0f;
+ m_damping = 1.0f;
+ m_limitSoftness = 0.5f;
+ m_currentLimit = 0;
+ m_currentLimitError = 0;
+ m_enableMotor = false;
+ }
+
+ btRotationalLimitMotor(const btRotationalLimitMotor & limot)
+ {
+ m_targetVelocity = limot.m_targetVelocity;
+ m_maxMotorForce = limot.m_maxMotorForce;
+ m_limitSoftness = limot.m_limitSoftness;
+ m_loLimit = limot.m_loLimit;
+ m_hiLimit = limot.m_hiLimit;
+ m_ERP = limot.m_ERP;
+ m_bounce = limot.m_bounce;
+ m_currentLimit = limot.m_currentLimit;
+ m_currentLimitError = limot.m_currentLimitError;
+ m_enableMotor = limot.m_enableMotor;
+ }
+
+
+
+ //! Is limited
+ bool isLimited()
+ {
+ if(m_loLimit > m_hiLimit) return false;
+ return true;
+ }
+
+ //! Need apply correction
+ bool needApplyTorques()
+ {
+ if(m_currentLimit == 0 && m_enableMotor == false) return false;
+ return true;
+ }
+
+ //! calculates error
+ /*!
+ calculates m_currentLimit and m_currentLimitError.
+ */
+ int testLimitValue(btScalar test_value);
+
+ //! apply the correction impulses for two bodies
+ btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB);
+
+};
+
+
+
+class btTranslationalLimitMotor
+{
+public:
+ btVector3 m_lowerLimit;//!< the constraint lower limits
+ btVector3 m_upperLimit;//!< the constraint upper limits
+ btVector3 m_accumulatedImpulse;
+ //! Linear_Limit_parameters
+ //!@{
+ btScalar m_limitSoftness;//!< Softness for linear limit
+ btScalar m_damping;//!< Damping for linear limit
+ btScalar m_restitution;//! Bounce parameter for linear limit
+ //!@}
+ bool m_enableMotor[3];
+ btVector3 m_targetVelocity;//!< target motor velocity
+ btVector3 m_maxMotorForce;//!< max force on motor
+ btVector3 m_currentLimitError;//! How much is violated this limit
+ btVector3 m_currentLinearDiff;//! Current relative offset of constraint frames
+ int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
+
+ btTranslationalLimitMotor()
+ {
+ m_lowerLimit.setValue(0.f,0.f,0.f);
+ m_upperLimit.setValue(0.f,0.f,0.f);
+ m_accumulatedImpulse.setValue(0.f,0.f,0.f);
+
+ m_limitSoftness = 0.7f;
+ m_damping = btScalar(1.0f);
+ m_restitution = btScalar(0.5f);
+ for(int i=0; i < 3; i++)
+ {
+ m_enableMotor[i] = false;
+ m_targetVelocity[i] = btScalar(0.f);
+ m_maxMotorForce[i] = btScalar(0.f);
+ }
+ }
+
+ btTranslationalLimitMotor(const btTranslationalLimitMotor & other )
+ {
+ m_lowerLimit = other.m_lowerLimit;
+ m_upperLimit = other.m_upperLimit;
+ m_accumulatedImpulse = other.m_accumulatedImpulse;
+
+ m_limitSoftness = other.m_limitSoftness ;
+ m_damping = other.m_damping;
+ m_restitution = other.m_restitution;
+ for(int i=0; i < 3; i++)
+ {
+ m_enableMotor[i] = other.m_enableMotor[i];
+ m_targetVelocity[i] = other.m_targetVelocity[i];
+ m_maxMotorForce[i] = other.m_maxMotorForce[i];
+ }
+ }
+
+ //! Test limit
+ /*!
+ - free means upper < lower,
+ - locked means upper == lower
+ - limited means upper > lower
+ - limitIndex: first 3 are linear, next 3 are angular
+ */
+ inline bool isLimited(int limitIndex)
+ {
+ return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
+ }
+ inline bool needApplyForce(int limitIndex)
+ {
+ if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
+ return true;
+ }
+ int testLimitValue(int limitIndex, btScalar test_value);
+
+
+ btScalar solveLinearAxis(
+ btScalar timeStep,
+ btScalar jacDiagABInv,
+ btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
+ btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
+ int limit_index,
+ const btVector3 & axis_normal_on_a,
+ const btVector3 & anchorPos);
+
+
+};
+
+/// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
+/*!
+btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
+currently this limit supports rotational motors<br>
+<ul>
+<li> For Linear limits, use btGeneric6DofConstraint.setLinearUpperLimit, btGeneric6DofConstraint.setLinearLowerLimit. You can set the parameters with the btTranslationalLimitMotor structure accsesible through the btGeneric6DofConstraint.getTranslationalLimitMotor method.
+At this moment translational motors are not supported. May be in the future. </li>
+
+<li> For Angular limits, use the btRotationalLimitMotor structure for configuring the limit.
+This is accessible through btGeneric6DofConstraint.getLimitMotor method,
+This brings support for limit parameters and motors. </li>
+
+<li> Angulars limits have these possible ranges:
+<table border=1 >
+<tr
+
+ <td><b>AXIS</b></td>
+ <td><b>MIN ANGLE</b></td>
+ <td><b>MAX ANGLE</b></td>
+ <td>X</td>
+ <td>-PI</td>
+ <td>PI</td>
+ <td>Y</td>
+ <td>-PI/2</td>
+ <td>PI/2</td>
+ <td>Z</td>
+ <td>-PI/2</td>
+ <td>PI/2</td>
+</tr>
+</table>
+</li>
+</ul>
+
+*/
+class btGeneric6DofConstraint : public btTypedConstraint
+{
+protected:
+
+ //! relative_frames
+ //!@{
+ btTransform m_frameInA;//!< the constraint space w.r.t body A
+ btTransform m_frameInB;//!< the constraint space w.r.t body B
+ //!@}
+
+ //! Jacobians
+ //!@{
+ btJacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints
+ btJacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints
+ //!@}
+
+ //! Linear_Limit_parameters
+ //!@{
+ btTranslationalLimitMotor m_linearLimits;
+ //!@}
+
+
+ //! hinge_parameters
+ //!@{
+ btRotationalLimitMotor m_angularLimits[3];
+ //!@}
+
+
+protected:
+ //! temporal variables
+ //!@{
+ btScalar m_timeStep;
+ btTransform m_calculatedTransformA;
+ btTransform m_calculatedTransformB;
+ btVector3 m_calculatedAxisAngleDiff;
+ btVector3 m_calculatedAxis[3];
+ btVector3 m_calculatedLinearDiff;
+
+ btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
+
+ bool m_useLinearReferenceFrameA;
+
+ //!@}
+
+ btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other)
+ {
+ btAssert(0);
+ (void) other;
+ return *this;
+ }
+
+
+ int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+
+ int setLinearLimits(btConstraintInfo2 *info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+
+ void buildLinearJacobian(
+ btJacobianEntry & jacLinear,const btVector3 & normalWorld,
+ const btVector3 & pivotAInW,const btVector3 & pivotBInW);
+
+ void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
+
+ // tests linear limits
+ void calculateLinearInfo();
+
+ //! calcs the euler angles between the two bodies.
+ void calculateAngleInfo();
+
+
+
+public:
+
+ ///for backwards compatibility during the transition to 'getInfo/getInfo2'
+ bool m_useSolveConstraintObsolete;
+
+ btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
+
+ btGeneric6DofConstraint();
+
+ //! Calcs global transform of the offsets
+ /*!
+ Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
+ \sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
+ */
+ void calculateTransforms(const btTransform& transA,const btTransform& transB);
+
+ void calculateTransforms();
+
+ //! Gets the global transform of the offset for body A
+ /*!
+ \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
+ */
+ const btTransform & getCalculatedTransformA() const
+ {
+ return m_calculatedTransformA;
+ }
+
+ //! Gets the global transform of the offset for body B
+ /*!
+ \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
+ */
+ const btTransform & getCalculatedTransformB() const
+ {
+ return m_calculatedTransformB;
+ }
+
+ const btTransform & getFrameOffsetA() const
+ {
+ return m_frameInA;
+ }
+
+ const btTransform & getFrameOffsetB() const
+ {
+ return m_frameInB;
+ }
+
+
+ btTransform & getFrameOffsetA()
+ {
+ return m_frameInA;
+ }
+
+ btTransform & getFrameOffsetB()
+ {
+ return m_frameInB;
+ }
+
+
+ //! performs Jacobian calculation, and also calculates angle differences and axis
+ virtual void buildJacobian();
+
+ virtual void getInfo1 (btConstraintInfo1* info);
+
+ void getInfo1NonVirtual (btConstraintInfo1* info);
+
+ virtual void getInfo2 (btConstraintInfo2* info);
+
+ void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+
+
+ virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
+
+ void updateRHS(btScalar timeStep);
+
+ //! Get the rotation axis in global coordinates
+ /*!
+ \pre btGeneric6DofConstraint.buildJacobian must be called previously.
+ */
+ btVector3 getAxis(int axis_index) const;
+
+ //! Get the relative Euler angle
+ /*!
+ \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
+ */
+ btScalar getAngle(int axis_index) const;
+
+ //! Get the relative position of the constraint pivot
+ /*!
+ \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
+ */
+ btScalar getRelativePivotPosition(int axis_index) const;
+
+
+ //! Test angular limit.
+ /*!
+ Calculates angular correction and returns true if limit needs to be corrected.
+ \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
+ */
+ bool testAngularLimitMotor(int axis_index);
+
+ void setLinearLowerLimit(const btVector3& linearLower)
+ {
+ m_linearLimits.m_lowerLimit = linearLower;
+ }
+
+ void setLinearUpperLimit(const btVector3& linearUpper)
+ {
+ m_linearLimits.m_upperLimit = linearUpper;
+ }
+
+ void setAngularLowerLimit(const btVector3& angularLower)
+ {
+ for(int i = 0; i < 3; i++)
+ m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
+ }
+
+ void setAngularUpperLimit(const btVector3& angularUpper)
+ {
+ for(int i = 0; i < 3; i++)
+ m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
+ }
+
+ //! Retrieves the angular limit informacion
+ btRotationalLimitMotor * getRotationalLimitMotor(int index)
+ {
+ return &m_angularLimits[index];
+ }
+
+ //! Retrieves the limit informacion
+ btTranslationalLimitMotor * getTranslationalLimitMotor()
+ {
+ return &m_linearLimits;
+ }
+
+ //first 3 are linear, next 3 are angular
+ void setLimit(int axis, btScalar lo, btScalar hi)
+ {
+ if(axis<3)
+ {
+ m_linearLimits.m_lowerLimit[axis] = lo;
+ m_linearLimits.m_upperLimit[axis] = hi;
+ }
+ else
+ {
+ lo = btNormalizeAngle(lo);
+ hi = btNormalizeAngle(hi);
+ m_angularLimits[axis-3].m_loLimit = lo;
+ m_angularLimits[axis-3].m_hiLimit = hi;
+ }
+ }
+
+ //! Test limit
+ /*!
+ - free means upper < lower,
+ - locked means upper == lower
+ - limited means upper > lower
+ - limitIndex: first 3 are linear, next 3 are angular
+ */
+ bool isLimited(int limitIndex)
+ {
+ if(limitIndex<3)
+ {
+ return m_linearLimits.isLimited(limitIndex);
+
+ }
+ return m_angularLimits[limitIndex-3].isLimited();
+ }
+
+ virtual void calcAnchorPos(void); // overridable
+
+ int get_limit_motor_info2( btRotationalLimitMotor * limot,
+ const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
+ btConstraintInfo2 *info, int row, btVector3& ax1, int rotational);
+
+
+};
+
+
+#endif //GENERIC_6DOF_CONSTRAINT_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,54 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
+Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef GENERIC_6DOF_SPRING_CONSTRAINT_H
+#define GENERIC_6DOF_SPRING_CONSTRAINT_H
+
+
+#include "LinearMath/btVector3.h"
+#include "btTypedConstraint.h"
+#include "btGeneric6DofConstraint.h"
+
+
+/// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF
+
+/// DOF index used in enableSpring() and setStiffness() means:
+/// 0 : translation X
+/// 1 : translation Y
+/// 2 : translation Z
+/// 3 : rotation X (3rd Euler rotational around new position of X axis, range [-PI+epsilon, PI-epsilon] )
+/// 4 : rotation Y (2nd Euler rotational around new position of Y axis, range [-PI/2+epsilon, PI/2-epsilon] )
+/// 5 : rotation Z (1st Euler rotational around Z axis, range [-PI+epsilon, PI-epsilon] )
+
+class btGeneric6DofSpringConstraint : public btGeneric6DofConstraint
+{
+protected:
+ bool m_springEnabled[6];
+ btScalar m_equilibriumPoint[6];
+ btScalar m_springStiffness[6];
+ btScalar m_springDamping[6]; // between 0 and 1 (1 == no damping)
+ void internalUpdateSprings(btConstraintInfo2* info);
+public:
+ btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
+ void enableSpring(int index, bool onOff);
+ void setStiffness(int index, btScalar stiffness);
+ void setDamping(int index, btScalar damping);
+ void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
+ void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF
+ virtual void getInfo2 (btConstraintInfo2* info);
+};
+
+#endif // GENERIC_6DOF_SPRING_CONSTRAINT_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btHinge2Constraint.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btHinge2Constraint.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btHinge2Constraint.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btHinge2Constraint.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,58 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
+Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef HINGE2_CONSTRAINT_H
+#define HINGE2_CONSTRAINT_H
+
+
+
+#include "LinearMath/btVector3.h"
+#include "btTypedConstraint.h"
+#include "btGeneric6DofSpringConstraint.h"
+
+
+
+// Constraint similar to ODE Hinge2 Joint
+// has 3 degrees of frredom:
+// 2 rotational degrees of freedom, similar to Euler rotations around Z (axis 1) and X (axis 2)
+// 1 translational (along axis Z) with suspension spring
+
+class btHinge2Constraint : public btGeneric6DofSpringConstraint
+{
+protected:
+ btVector3 m_anchor;
+ btVector3 m_axis1;
+ btVector3 m_axis2;
+public:
+ // constructor
+ // anchor, axis1 and axis2 are in world coordinate system
+ // axis1 must be orthogonal to axis2
+ btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2);
+ // access
+ const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); }
+ const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); }
+ const btVector3& getAxis1() { return m_axis1; }
+ const btVector3& getAxis2() { return m_axis2; }
+ btScalar getAngle1() { return getAngle(2); }
+ btScalar getAngle2() { return getAngle(0); }
+ // limits
+ void setUpperLimit(btScalar ang1max) { setAngularUpperLimit(btVector3(-1.f, 0.f, ang1max)); }
+ void setLowerLimit(btScalar ang1min) { setAngularLowerLimit(btVector3( 1.f, 0.f, ang1min)); }
+};
+
+
+
+#endif // HINGE2_CONSTRAINT_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btHingeConstraint.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btHingeConstraint.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btHingeConstraint.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btHingeConstraint.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,223 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */
+
+#ifndef HINGECONSTRAINT_H
+#define HINGECONSTRAINT_H
+
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+class btRigidBody;
+
+/// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
+/// axis defines the orientation of the hinge axis
+ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint
+{
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
+ btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
+ btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
+
+ btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+ btTransform m_rbBFrame;
+
+ btScalar m_motorTargetVelocity;
+ btScalar m_maxMotorImpulse;
+
+ btScalar m_limitSoftness;
+ btScalar m_biasFactor;
+ btScalar m_relaxationFactor;
+
+ btScalar m_lowerLimit;
+ btScalar m_upperLimit;
+
+ btScalar m_kHinge;
+
+ btScalar m_limitSign;
+ btScalar m_correction;
+
+ btScalar m_accLimitImpulse;
+ btScalar m_hingeAngle;
+ btScalar m_referenceSign;
+
+ bool m_angularOnly;
+ bool m_enableAngularMotor;
+ bool m_solveLimit;
+ bool m_useSolveConstraintObsolete;
+ bool m_useReferenceFrameA;
+
+ btScalar m_accMotorImpulse;
+
+
+public:
+
+ btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA = false);
+
+ btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA = false);
+
+ btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
+
+ btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
+
+ btHingeConstraint();
+
+ virtual void buildJacobian();
+
+ virtual void getInfo1 (btConstraintInfo1* info);
+
+ void getInfo1NonVirtual(btConstraintInfo1* info);
+
+ virtual void getInfo2 (btConstraintInfo2* info);
+
+ void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
+
+ void getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
+
+ virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
+
+ void updateRHS(btScalar timeStep);
+
+ const btRigidBody& getRigidBodyA() const
+ {
+ return m_rbA;
+ }
+ const btRigidBody& getRigidBodyB() const
+ {
+ return m_rbB;
+ }
+
+ btRigidBody& getRigidBodyA()
+ {
+ return m_rbA;
+ }
+
+ btRigidBody& getRigidBodyB()
+ {
+ return m_rbB;
+ }
+
+ void setAngularOnly(bool angularOnly)
+ {
+ m_angularOnly = angularOnly;
+ }
+
+ void enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse)
+ {
+ m_enableAngularMotor = enableMotor;
+ m_motorTargetVelocity = targetVelocity;
+ m_maxMotorImpulse = maxMotorImpulse;
+ }
+
+ // extra motor API, including ability to set a target rotation (as opposed to angular velocity)
+ // note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to
+ // maintain a given angular target.
+ void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; }
+ void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
+ void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
+ void setMotorTarget(btScalar targetAngle, btScalar dt);
+
+
+ void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+ {
+ m_lowerLimit = btNormalizeAngle(low);
+ m_upperLimit = btNormalizeAngle(high);
+
+ m_limitSoftness = _softness;
+ m_biasFactor = _biasFactor;
+ m_relaxationFactor = _relaxationFactor;
+
+ }
+
+ void setAxis(btVector3& axisInA)
+ {
+ btVector3 rbAxisA1, rbAxisA2;
+ btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
+ btVector3 pivotInA = m_rbAFrame.getOrigin();
+// m_rbAFrame.getOrigin() = pivotInA;
+ m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
+ rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
+ rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
+
+ btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA;
+
+ btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
+ btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
+ btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
+
+
+ m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(pivotInA);
+ m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
+ rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
+ rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
+ }
+
+ btScalar getLowerLimit() const
+ {
+ return m_lowerLimit;
+ }
+
+ btScalar getUpperLimit() const
+ {
+ return m_upperLimit;
+ }
+
+
+ btScalar getHingeAngle();
+
+ btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
+
+ void testLimit(const btTransform& transA,const btTransform& transB);
+
+
+ const btTransform& getAFrame() const { return m_rbAFrame; };
+ const btTransform& getBFrame() const { return m_rbBFrame; };
+
+ btTransform& getAFrame() { return m_rbAFrame; };
+ btTransform& getBFrame() { return m_rbBFrame; };
+
+ inline int getSolveLimit()
+ {
+ return m_solveLimit;
+ }
+
+ inline btScalar getLimitSign()
+ {
+ return m_limitSign;
+ }
+
+ inline bool getAngularOnly()
+ {
+ return m_angularOnly;
+ }
+ inline bool getEnableAngularMotor()
+ {
+ return m_enableAngularMotor;
+ }
+ inline btScalar getMotorTargetVelosity()
+ {
+ return m_motorTargetVelocity;
+ }
+ inline btScalar getMaxMotorImpulse()
+ {
+ return m_maxMotorImpulse;
+ }
+
+};
+
+#endif //HINGECONSTRAINT_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btJacobianEntry.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btJacobianEntry.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btJacobianEntry.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btJacobianEntry.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,156 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef JACOBIAN_ENTRY_H
+#define JACOBIAN_ENTRY_H
+
+#include "LinearMath/btVector3.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+
+
+//notes:
+// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
+// which makes the btJacobianEntry memory layout 16 bytes
+// if you only are interested in angular part, just feed massInvA and massInvB zero
+
+/// Jacobian entry is an abstraction that allows to describe constraints
+/// it can be used in combination with a constraint solver
+/// Can be used to relate the effect of an impulse to the constraint error
+ATTRIBUTE_ALIGNED16(class) btJacobianEntry
+{
+public:
+ btJacobianEntry() {};
+ //constraint between two different rigidbodies
+ btJacobianEntry(
+ const btMatrix3x3& world2A,
+ const btMatrix3x3& world2B,
+ const btVector3& rel_pos1,const btVector3& rel_pos2,
+ const btVector3& jointAxis,
+ const btVector3& inertiaInvA,
+ const btScalar massInvA,
+ const btVector3& inertiaInvB,
+ const btScalar massInvB)
+ :m_linearJointAxis(jointAxis)
+ {
+ m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
+ m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
+ m_0MinvJt = inertiaInvA * m_aJ;
+ m_1MinvJt = inertiaInvB * m_bJ;
+ m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
+
+ btAssert(m_Adiag > btScalar(0.0));
+ }
+
+ //angular constraint between two different rigidbodies
+ btJacobianEntry(const btVector3& jointAxis,
+ const btMatrix3x3& world2A,
+ const btMatrix3x3& world2B,
+ const btVector3& inertiaInvA,
+ const btVector3& inertiaInvB)
+ :m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
+ {
+ m_aJ= world2A*jointAxis;
+ m_bJ = world2B*-jointAxis;
+ m_0MinvJt = inertiaInvA * m_aJ;
+ m_1MinvJt = inertiaInvB * m_bJ;
+ m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+
+ btAssert(m_Adiag > btScalar(0.0));
+ }
+
+ //angular constraint between two different rigidbodies
+ btJacobianEntry(const btVector3& axisInA,
+ const btVector3& axisInB,
+ const btVector3& inertiaInvA,
+ const btVector3& inertiaInvB)
+ : m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
+ , m_aJ(axisInA)
+ , m_bJ(-axisInB)
+ {
+ m_0MinvJt = inertiaInvA * m_aJ;
+ m_1MinvJt = inertiaInvB * m_bJ;
+ m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+
+ btAssert(m_Adiag > btScalar(0.0));
+ }
+
+ //constraint on one rigidbody
+ btJacobianEntry(
+ const btMatrix3x3& world2A,
+ const btVector3& rel_pos1,const btVector3& rel_pos2,
+ const btVector3& jointAxis,
+ const btVector3& inertiaInvA,
+ const btScalar massInvA)
+ :m_linearJointAxis(jointAxis)
+ {
+ m_aJ= world2A*(rel_pos1.cross(jointAxis));
+ m_bJ = world2A*(rel_pos2.cross(-jointAxis));
+ m_0MinvJt = inertiaInvA * m_aJ;
+ m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.));
+ m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
+
+ btAssert(m_Adiag > btScalar(0.0));
+ }
+
+ btScalar getDiagonal() const { return m_Adiag; }
+
+ // for two constraints on the same rigidbody (for example vehicle friction)
+ btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const
+ {
+ const btJacobianEntry& jacA = *this;
+ btScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
+ btScalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
+ return lin + ang;
+ }
+
+
+
+ // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
+ btScalar getNonDiagonal(const btJacobianEntry& jacB,const btScalar massInvA,const btScalar massInvB) const
+ {
+ const btJacobianEntry& jacA = *this;
+ btVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
+ btVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
+ btVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
+ btVector3 lin0 = massInvA * lin ;
+ btVector3 lin1 = massInvB * lin;
+ btVector3 sum = ang0+ang1+lin0+lin1;
+ return sum[0]+sum[1]+sum[2];
+ }
+
+ btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB)
+ {
+ btVector3 linrel = linvelA - linvelB;
+ btVector3 angvela = angvelA * m_aJ;
+ btVector3 angvelb = angvelB * m_bJ;
+ linrel *= m_linearJointAxis;
+ angvela += angvelb;
+ angvela += linrel;
+ btScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
+ return rel_vel2 + SIMD_EPSILON;
+ }
+//private:
+
+ btVector3 m_linearJointAxis;
+ btVector3 m_aJ;
+ btVector3 m_bJ;
+ btVector3 m_0MinvJt;
+ btVector3 m_1MinvJt;
+ //Optimization: can be stored in the w/last component of one of the vectors
+ btScalar m_Adiag;
+
+};
+
+#endif //JACOBIAN_ENTRY_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,101 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef POINT2POINTCONSTRAINT_H
+#define POINT2POINTCONSTRAINT_H
+
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+class btRigidBody;
+
+struct btConstraintSetting
+{
+ btConstraintSetting() :
+ m_tau(btScalar(0.3)),
+ m_damping(btScalar(1.)),
+ m_impulseClamp(btScalar(0.))
+ {
+ }
+ btScalar m_tau;
+ btScalar m_damping;
+ btScalar m_impulseClamp;
+};
+
+/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
+ATTRIBUTE_ALIGNED16(class) btPoint2PointConstraint : public btTypedConstraint
+{
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
+ btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
+
+ btVector3 m_pivotInA;
+ btVector3 m_pivotInB;
+
+
+
+public:
+
+ ///for backwards compatibility during the transition to 'getInfo/getInfo2'
+ bool m_useSolveConstraintObsolete;
+
+ btConstraintSetting m_setting;
+
+ btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB);
+
+ btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA);
+
+ btPoint2PointConstraint();
+
+ virtual void buildJacobian();
+
+ virtual void getInfo1 (btConstraintInfo1* info);
+
+ void getInfo1NonVirtual (btConstraintInfo1* info);
+
+ virtual void getInfo2 (btConstraintInfo2* info);
+
+ void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans);
+
+ virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
+
+ void updateRHS(btScalar timeStep);
+
+ void setPivotA(const btVector3& pivotA)
+ {
+ m_pivotInA = pivotA;
+ }
+
+ void setPivotB(const btVector3& pivotB)
+ {
+ m_pivotInB = pivotB;
+ }
+
+ const btVector3& getPivotInA() const
+ {
+ return m_pivotInA;
+ }
+
+ const btVector3& getPivotInB() const
+ {
+ return m_pivotInB;
+ }
+
+
+};
+
+#endif //POINT2POINTCONSTRAINT_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,107 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
+#define SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
+
+#include "btConstraintSolver.h"
+class btIDebugDraw;
+#include "btContactConstraint.h"
+#include "btSolverBody.h"
+#include "btSolverConstraint.h"
+#include "btTypedConstraint.h"
+#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
+
+///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
+class btSequentialImpulseConstraintSolver : public btConstraintSolver
+{
+protected:
+
+ btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
+ btConstraintArray m_tmpSolverContactConstraintPool;
+ btConstraintArray m_tmpSolverNonContactConstraintPool;
+ btConstraintArray m_tmpSolverContactFrictionConstraintPool;
+ btAlignedObjectArray<int> m_orderTmpConstraintPool;
+ btAlignedObjectArray<int> m_orderFrictionConstraintPool;
+ btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
+
+ btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation);
+
+ ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
+ unsigned long m_btSeed2;
+
+ void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
+ btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
+
+ void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
+
+
+ void resolveSplitPenetrationSIMD(
+ btSolverBody& body1,
+ btSolverBody& body2,
+ const btSolverConstraint& contactConstraint);
+
+ void resolveSplitPenetrationImpulseCacheFriendly(
+ btSolverBody& body1,
+ btSolverBody& body2,
+ const btSolverConstraint& contactConstraint);
+
+ //internal method
+ int getOrInitSolverBody(btCollisionObject& body);
+
+ void resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+
+ void resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+
+ void resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+
+ void resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+
+public:
+
+
+ btSequentialImpulseConstraintSolver();
+ virtual ~btSequentialImpulseConstraintSolver();
+
+ virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
+
+ btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+ btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+
+ ///clear internal cached data and reset random seed
+ virtual void reset();
+
+ unsigned long btRand2();
+
+ int btRandInt2 (int n);
+
+ void setRandSeed(unsigned long seed)
+ {
+ m_btSeed2 = seed;
+ }
+ unsigned long getRandSeed() const
+ {
+ return m_btSeed2;
+ }
+
+};
+
+#ifndef BT_PREFER_SIMD
+typedef btSequentialImpulseConstraintSolver btSequentialImpulseConstraintSolverPrefered;
+#endif
+
+
+#endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSliderConstraint.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSliderConstraint.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSliderConstraint.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSliderConstraint.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,234 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+Added by Roman Ponomarev (rponom at gmail.com)
+April 04, 2008
+
+TODO:
+ - add clamping od accumulated impulse to improve stability
+ - add conversion for ODE constraint solver
+*/
+
+#ifndef SLIDER_CONSTRAINT_H
+#define SLIDER_CONSTRAINT_H
+
+
+
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+
+
+class btRigidBody;
+
+
+
+#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0))
+#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0))
+#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7))
+
+
+
+class btSliderConstraint : public btTypedConstraint
+{
+protected:
+ ///for backwards compatibility during the transition to 'getInfo/getInfo2'
+ bool m_useSolveConstraintObsolete;
+ btTransform m_frameInA;
+ btTransform m_frameInB;
+ // use frameA fo define limits, if true
+ bool m_useLinearReferenceFrameA;
+ // linear limits
+ btScalar m_lowerLinLimit;
+ btScalar m_upperLinLimit;
+ // angular limits
+ btScalar m_lowerAngLimit;
+ btScalar m_upperAngLimit;
+ // softness, restitution and damping for different cases
+ // DirLin - moving inside linear limits
+ // LimLin - hitting linear limit
+ // DirAng - moving inside angular limits
+ // LimAng - hitting angular limit
+ // OrthoLin, OrthoAng - against constraint axis
+ btScalar m_softnessDirLin;
+ btScalar m_restitutionDirLin;
+ btScalar m_dampingDirLin;
+ btScalar m_softnessDirAng;
+ btScalar m_restitutionDirAng;
+ btScalar m_dampingDirAng;
+ btScalar m_softnessLimLin;
+ btScalar m_restitutionLimLin;
+ btScalar m_dampingLimLin;
+ btScalar m_softnessLimAng;
+ btScalar m_restitutionLimAng;
+ btScalar m_dampingLimAng;
+ btScalar m_softnessOrthoLin;
+ btScalar m_restitutionOrthoLin;
+ btScalar m_dampingOrthoLin;
+ btScalar m_softnessOrthoAng;
+ btScalar m_restitutionOrthoAng;
+ btScalar m_dampingOrthoAng;
+
+ // for interlal use
+ bool m_solveLinLim;
+ bool m_solveAngLim;
+
+ btJacobianEntry m_jacLin[3];
+ btScalar m_jacLinDiagABInv[3];
+
+ btJacobianEntry m_jacAng[3];
+
+ btScalar m_timeStep;
+ btTransform m_calculatedTransformA;
+ btTransform m_calculatedTransformB;
+
+ btVector3 m_sliderAxis;
+ btVector3 m_realPivotAInW;
+ btVector3 m_realPivotBInW;
+ btVector3 m_projPivotInW;
+ btVector3 m_delta;
+ btVector3 m_depth;
+ btVector3 m_relPosA;
+ btVector3 m_relPosB;
+
+ btScalar m_linPos;
+ btScalar m_angPos;
+
+ btScalar m_angDepth;
+ btScalar m_kAngle;
+
+ bool m_poweredLinMotor;
+ btScalar m_targetLinMotorVelocity;
+ btScalar m_maxLinMotorForce;
+ btScalar m_accumulatedLinMotorImpulse;
+
+ bool m_poweredAngMotor;
+ btScalar m_targetAngMotorVelocity;
+ btScalar m_maxAngMotorForce;
+ btScalar m_accumulatedAngMotorImpulse;
+
+ //------------------------
+ void initParams();
+public:
+ // constructors
+ btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
+ btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
+ btSliderConstraint();
+ // overrides
+ virtual void buildJacobian();
+ virtual void getInfo1 (btConstraintInfo1* info);
+
+ void getInfo1NonVirtual(btConstraintInfo1* info);
+
+ virtual void getInfo2 (btConstraintInfo2* info);
+
+ void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass);
+
+ virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
+
+
+ // access
+ const btRigidBody& getRigidBodyA() const { return m_rbA; }
+ const btRigidBody& getRigidBodyB() const { return m_rbB; }
+ const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
+ const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; }
+ const btTransform & getFrameOffsetA() const { return m_frameInA; }
+ const btTransform & getFrameOffsetB() const { return m_frameInB; }
+ btTransform & getFrameOffsetA() { return m_frameInA; }
+ btTransform & getFrameOffsetB() { return m_frameInB; }
+ btScalar getLowerLinLimit() { return m_lowerLinLimit; }
+ void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; }
+ btScalar getUpperLinLimit() { return m_upperLinLimit; }
+ void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
+ btScalar getLowerAngLimit() { return m_lowerAngLimit; }
+ void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); }
+ btScalar getUpperAngLimit() { return m_upperAngLimit; }
+ void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); }
+ bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; }
+ btScalar getSoftnessDirLin() { return m_softnessDirLin; }
+ btScalar getRestitutionDirLin() { return m_restitutionDirLin; }
+ btScalar getDampingDirLin() { return m_dampingDirLin ; }
+ btScalar getSoftnessDirAng() { return m_softnessDirAng; }
+ btScalar getRestitutionDirAng() { return m_restitutionDirAng; }
+ btScalar getDampingDirAng() { return m_dampingDirAng; }
+ btScalar getSoftnessLimLin() { return m_softnessLimLin; }
+ btScalar getRestitutionLimLin() { return m_restitutionLimLin; }
+ btScalar getDampingLimLin() { return m_dampingLimLin; }
+ btScalar getSoftnessLimAng() { return m_softnessLimAng; }
+ btScalar getRestitutionLimAng() { return m_restitutionLimAng; }
+ btScalar getDampingLimAng() { return m_dampingLimAng; }
+ btScalar getSoftnessOrthoLin() { return m_softnessOrthoLin; }
+ btScalar getRestitutionOrthoLin() { return m_restitutionOrthoLin; }
+ btScalar getDampingOrthoLin() { return m_dampingOrthoLin; }
+ btScalar getSoftnessOrthoAng() { return m_softnessOrthoAng; }
+ btScalar getRestitutionOrthoAng() { return m_restitutionOrthoAng; }
+ btScalar getDampingOrthoAng() { return m_dampingOrthoAng; }
+ void setSoftnessDirLin(btScalar softnessDirLin) { m_softnessDirLin = softnessDirLin; }
+ void setRestitutionDirLin(btScalar restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; }
+ void setDampingDirLin(btScalar dampingDirLin) { m_dampingDirLin = dampingDirLin; }
+ void setSoftnessDirAng(btScalar softnessDirAng) { m_softnessDirAng = softnessDirAng; }
+ void setRestitutionDirAng(btScalar restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; }
+ void setDampingDirAng(btScalar dampingDirAng) { m_dampingDirAng = dampingDirAng; }
+ void setSoftnessLimLin(btScalar softnessLimLin) { m_softnessLimLin = softnessLimLin; }
+ void setRestitutionLimLin(btScalar restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; }
+ void setDampingLimLin(btScalar dampingLimLin) { m_dampingLimLin = dampingLimLin; }
+ void setSoftnessLimAng(btScalar softnessLimAng) { m_softnessLimAng = softnessLimAng; }
+ void setRestitutionLimAng(btScalar restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; }
+ void setDampingLimAng(btScalar dampingLimAng) { m_dampingLimAng = dampingLimAng; }
+ void setSoftnessOrthoLin(btScalar softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; }
+ void setRestitutionOrthoLin(btScalar restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; }
+ void setDampingOrthoLin(btScalar dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; }
+ void setSoftnessOrthoAng(btScalar softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; }
+ void setRestitutionOrthoAng(btScalar restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; }
+ void setDampingOrthoAng(btScalar dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; }
+ void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; }
+ bool getPoweredLinMotor() { return m_poweredLinMotor; }
+ void setTargetLinMotorVelocity(btScalar targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; }
+ btScalar getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; }
+ void setMaxLinMotorForce(btScalar maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; }
+ btScalar getMaxLinMotorForce() { return m_maxLinMotorForce; }
+ void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; }
+ bool getPoweredAngMotor() { return m_poweredAngMotor; }
+ void setTargetAngMotorVelocity(btScalar targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; }
+ btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; }
+ void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; }
+ btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
+ btScalar getLinearPos() { return m_linPos; }
+
+
+ // access for ODE solver
+ bool getSolveLinLimit() { return m_solveLinLim; }
+ btScalar getLinDepth() { return m_depth[0]; }
+ bool getSolveAngLimit() { return m_solveAngLim; }
+ btScalar getAngDepth() { return m_angDepth; }
+ // internal
+ void buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
+ void solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB);
+ // shared code used by ODE solver
+ void calculateTransforms(const btTransform& transA,const btTransform& transB);
+ void testLinLimits();
+ void testLinLimits2(btConstraintInfo2* info);
+ void testAngLimits();
+ // access for PE Solver
+ btVector3 getAncorInA();
+ btVector3 getAncorInB();
+};
+
+
+
+#endif //SLIDER_CONSTRAINT_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,107 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SOLVE_2LINEAR_CONSTRAINT_H
+#define SOLVE_2LINEAR_CONSTRAINT_H
+
+#include "LinearMath/btMatrix3x3.h"
+#include "LinearMath/btVector3.h"
+
+
+class btRigidBody;
+
+
+
+/// constraint class used for lateral tyre friction.
+class btSolve2LinearConstraint
+{
+ btScalar m_tau;
+ btScalar m_damping;
+
+public:
+
+ btSolve2LinearConstraint(btScalar tau,btScalar damping)
+ {
+ m_tau = tau;
+ m_damping = damping;
+ }
+ //
+ // solve unilateral constraint (equality, direct method)
+ //
+ void resolveUnilateralPairConstraint(
+ btRigidBody* body0,
+ btRigidBody* body1,
+
+ const btMatrix3x3& world2A,
+ const btMatrix3x3& world2B,
+
+ const btVector3& invInertiaADiag,
+ const btScalar invMassA,
+ const btVector3& linvelA,const btVector3& angvelA,
+ const btVector3& rel_posA1,
+ const btVector3& invInertiaBDiag,
+ const btScalar invMassB,
+ const btVector3& linvelB,const btVector3& angvelB,
+ const btVector3& rel_posA2,
+
+ btScalar depthA, const btVector3& normalA,
+ const btVector3& rel_posB1,const btVector3& rel_posB2,
+ btScalar depthB, const btVector3& normalB,
+ btScalar& imp0,btScalar& imp1);
+
+
+ //
+ // solving 2x2 lcp problem (inequality, direct solution )
+ //
+ void resolveBilateralPairConstraint(
+ btRigidBody* body0,
+ btRigidBody* body1,
+ const btMatrix3x3& world2A,
+ const btMatrix3x3& world2B,
+
+ const btVector3& invInertiaADiag,
+ const btScalar invMassA,
+ const btVector3& linvelA,const btVector3& angvelA,
+ const btVector3& rel_posA1,
+ const btVector3& invInertiaBDiag,
+ const btScalar invMassB,
+ const btVector3& linvelB,const btVector3& angvelB,
+ const btVector3& rel_posA2,
+
+ btScalar depthA, const btVector3& normalA,
+ const btVector3& rel_posB1,const btVector3& rel_posB2,
+ btScalar depthB, const btVector3& normalB,
+ btScalar& imp0,btScalar& imp1);
+
+/*
+ void resolveAngularConstraint( const btMatrix3x3& invInertiaAWS,
+ const btScalar invMassA,
+ const btVector3& linvelA,const btVector3& angvelA,
+ const btVector3& rel_posA1,
+ const btMatrix3x3& invInertiaBWS,
+ const btScalar invMassB,
+ const btVector3& linvelB,const btVector3& angvelB,
+ const btVector3& rel_posA2,
+
+ btScalar depthA, const btVector3& normalA,
+ const btVector3& rel_posB1,const btVector3& rel_posB2,
+ btScalar depthB, const btVector3& normalB,
+ btScalar& imp0,btScalar& imp1);
+
+*/
+
+};
+
+#endif //SOLVE_2LINEAR_CONSTRAINT_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSolverBody.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSolverBody.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSolverBody.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSolverBody.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,191 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_SOLVER_BODY_H
+#define BT_SOLVER_BODY_H
+
+class btRigidBody;
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btAlignedAllocator.h"
+#include "LinearMath/btTransformUtil.h"
+
+///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
+#ifdef BT_USE_SSE
+#define USE_SIMD 1
+#endif //
+
+
+#ifdef USE_SIMD
+
+struct btSimdScalar
+{
+ SIMD_FORCE_INLINE btSimdScalar()
+ {
+
+ }
+
+ SIMD_FORCE_INLINE btSimdScalar(float fl)
+ :m_vec128 (_mm_set1_ps(fl))
+ {
+ }
+
+ SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
+ :m_vec128(v128)
+ {
+ }
+ union
+ {
+ __m128 m_vec128;
+ float m_floats[4];
+ int m_ints[4];
+ btScalar m_unusedPadding;
+ };
+ SIMD_FORCE_INLINE __m128 get128()
+ {
+ return m_vec128;
+ }
+
+ SIMD_FORCE_INLINE const __m128 get128() const
+ {
+ return m_vec128;
+ }
+
+ SIMD_FORCE_INLINE void set128(__m128 v128)
+ {
+ m_vec128 = v128;
+ }
+
+ SIMD_FORCE_INLINE operator __m128()
+ {
+ return m_vec128;
+ }
+ SIMD_FORCE_INLINE operator const __m128() const
+ {
+ return m_vec128;
+ }
+
+ SIMD_FORCE_INLINE operator float() const
+ {
+ return m_floats[0];
+ }
+
+};
+
+///@brief Return the elementwise product of two btSimdScalar
+SIMD_FORCE_INLINE btSimdScalar
+operator*(const btSimdScalar& v1, const btSimdScalar& v2)
+{
+ return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
+}
+
+///@brief Return the elementwise product of two btSimdScalar
+SIMD_FORCE_INLINE btSimdScalar
+operator+(const btSimdScalar& v1, const btSimdScalar& v2)
+{
+ return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
+}
+
+
+#else
+#define btSimdScalar btScalar
+#endif
+
+///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
+ATTRIBUTE_ALIGNED16 (struct) btSolverBody
+{
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+ btVector3 m_deltaLinearVelocity;
+ btVector3 m_deltaAngularVelocity;
+ btVector3 m_angularFactor;
+ btVector3 m_invMass;
+ btScalar m_friction;
+ btRigidBody* m_originalBody;
+ btVector3 m_pushVelocity;
+ btVector3 m_turnVelocity;
+
+
+ SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
+ {
+ if (m_originalBody)
+ velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
+ else
+ velocity.setValue(0,0,0);
+ }
+
+ SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const
+ {
+ if (m_originalBody)
+ angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity;
+ else
+ angVel.setValue(0,0,0);
+ }
+
+
+ //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+ SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
+ {
+ //if (m_invMass)
+ {
+ m_deltaLinearVelocity += linearComponent*impulseMagnitude;
+ m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+ }
+ }
+
+ SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+ {
+ if (m_originalBody)
+ {
+ m_pushVelocity += linearComponent*impulseMagnitude;
+ m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+ }
+ }
+
+ void writebackVelocity()
+ {
+ if (m_originalBody)
+ {
+ m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
+ m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
+
+ //m_originalBody->setCompanionId(-1);
+ }
+ }
+
+
+ void writebackVelocity(btScalar timeStep)
+ {
+ if (m_originalBody)
+ {
+ m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
+ m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
+
+ //correct the position/orientation based on push/turn recovery
+ btTransform newTransform;
+ btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
+ m_originalBody->setWorldTransform(newTransform);
+
+ //m_originalBody->setCompanionId(-1);
+ }
+ }
+
+
+
+};
+
+#endif //BT_SOLVER_BODY_H
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSolverConstraint.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSolverConstraint.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSolverConstraint.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSolverConstraint.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,96 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_SOLVER_CONSTRAINT_H
+#define BT_SOLVER_CONSTRAINT_H
+
+class btRigidBody;
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "btJacobianEntry.h"
+
+//#define NO_FRICTION_TANGENTIALS 1
+#include "btSolverBody.h"
+
+
+///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
+ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
+{
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ btVector3 m_relpos1CrossNormal;
+ btVector3 m_contactNormal;
+
+ btVector3 m_relpos2CrossNormal;
+ //btVector3 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
+
+ btVector3 m_angularComponentA;
+ btVector3 m_angularComponentB;
+
+ mutable btSimdScalar m_appliedPushImpulse;
+ mutable btSimdScalar m_appliedImpulse;
+
+
+ btScalar m_friction;
+ btScalar m_jacDiagABInv;
+ union
+ {
+ int m_numConsecutiveRowsPerKernel;
+ btScalar m_unusedPadding0;
+ };
+
+ union
+ {
+ int m_frictionIndex;
+ btScalar m_unusedPadding1;
+ };
+ union
+ {
+ int m_solverBodyIdA;
+ btScalar m_unusedPadding2;
+ };
+ union
+ {
+ int m_solverBodyIdB;
+ btScalar m_unusedPadding3;
+ };
+
+ union
+ {
+ void* m_originalContactPoint;
+ btScalar m_unusedPadding4;
+ };
+
+ btScalar m_rhs;
+ btScalar m_cfm;
+ btScalar m_lowerLimit;
+ btScalar m_upperLimit;
+
+ btScalar m_rhsPenetration;
+
+ enum btSolverConstraintType
+ {
+ BT_SOLVER_CONTACT_1D = 0,
+ BT_SOLVER_FRICTION_1D
+ };
+};
+
+typedef btAlignedObjectArray<btSolverConstraint> btConstraintArray;
+
+
+#endif //BT_SOLVER_CONSTRAINT_H
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btTypedConstraint.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btTypedConstraint.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btTypedConstraint.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btTypedConstraint.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,271 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef TYPED_CONSTRAINT_H
+#define TYPED_CONSTRAINT_H
+
+class btRigidBody;
+#include "LinearMath/btScalar.h"
+#include "btSolverConstraint.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+struct btSolverBody;
+
+enum btTypedConstraintType
+{
+ POINT2POINT_CONSTRAINT_TYPE=MAX_CONTACT_MANIFOLD_TYPE+1,
+ HINGE_CONSTRAINT_TYPE,
+ CONETWIST_CONSTRAINT_TYPE,
+ D6_CONSTRAINT_TYPE,
+ SLIDER_CONSTRAINT_TYPE,
+ CONTACT_CONSTRAINT_TYPE
+};
+
+///TypedConstraint is the baseclass for Bullet constraints and vehicles
+class btTypedConstraint : public btTypedObject
+{
+ int m_userConstraintType;
+ int m_userConstraintId;
+ bool m_needsFeedback;
+
+ btTypedConstraint& operator=(btTypedConstraint& other)
+ {
+ btAssert(0);
+ (void) other;
+ return *this;
+ }
+
+protected:
+ btRigidBody& m_rbA;
+ btRigidBody& m_rbB;
+ btScalar m_appliedImpulse;
+ btScalar m_dbgDrawSize;
+
+ btVector3 m_appliedLinearImpulse;
+ btVector3 m_appliedAngularImpulseA;
+ btVector3 m_appliedAngularImpulseB;
+
+public:
+
+ btTypedConstraint(btTypedConstraintType type);
+ virtual ~btTypedConstraint() {};
+ btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
+ btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
+
+ struct btConstraintInfo1 {
+ int m_numConstraintRows,nub;
+ };
+
+ struct btConstraintInfo2 {
+ // integrator parameters: frames per second (1/stepsize), default error
+ // reduction parameter (0..1).
+ btScalar fps,erp;
+
+ // for the first and second body, pointers to two (linear and angular)
+ // n*3 jacobian sub matrices, stored by rows. these matrices will have
+ // been initialized to 0 on entry. if the second body is zero then the
+ // J2xx pointers may be 0.
+ btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
+
+ // elements to jump from one row to the next in J's
+ int rowskip;
+
+ // right hand sides of the equation J*v = c + cfm * lambda. cfm is the
+ // "constraint force mixing" vector. c is set to zero on entry, cfm is
+ // set to a constant value (typically very small or zero) value on entry.
+ btScalar *m_constraintError,*cfm;
+
+ // lo and hi limits for variables (set to -/+ infinity on entry).
+ btScalar *m_lowerLimit,*m_upperLimit;
+
+ // findex vector for variables. see the LCP solver interface for a
+ // description of what this does. this is set to -1 on entry.
+ // note that the returned indexes are relative to the first index of
+ // the constraint.
+ int *findex;
+ // number of solver iterations
+ int m_numIterations;
+ };
+
+ ///internal method used by the constraint solver, don't use them directly
+ virtual void buildJacobian() = 0;
+
+ ///internal method used by the constraint solver, don't use them directly
+ virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
+ {
+ }
+
+ ///internal method used by the constraint solver, don't use them directly
+ virtual void getInfo1 (btConstraintInfo1* info)=0;
+
+ ///internal method used by the constraint solver, don't use them directly
+ virtual void getInfo2 (btConstraintInfo2* info)=0;
+
+ ///internal method used by the constraint solver, don't use them directly
+ void internalSetAppliedImpulse(btScalar appliedImpulse)
+ {
+ m_appliedImpulse = appliedImpulse;
+ }
+
+ ///internal method used by the constraint solver, don't use them directly
+ virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) = 0;
+
+ ///internal method used by the constraint solver, don't use them directly
+ btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
+
+ const btRigidBody& getRigidBodyA() const
+ {
+ return m_rbA;
+ }
+ const btRigidBody& getRigidBodyB() const
+ {
+ return m_rbB;
+ }
+
+ btRigidBody& getRigidBodyA()
+ {
+ return m_rbA;
+ }
+ btRigidBody& getRigidBodyB()
+ {
+ return m_rbB;
+ }
+
+ int getUserConstraintType() const
+ {
+ return m_userConstraintType ;
+ }
+
+ void setUserConstraintType(int userConstraintType)
+ {
+ m_userConstraintType = userConstraintType;
+ };
+
+ void setUserConstraintId(int uid)
+ {
+ m_userConstraintId = uid;
+ }
+
+ int getUserConstraintId() const
+ {
+ return m_userConstraintId;
+ }
+
+ int getUid() const
+ {
+ return m_userConstraintId;
+ }
+
+ bool needsFeedback() const
+ {
+ return m_needsFeedback;
+ }
+
+ ///enableFeedback will allow to read the applied linear and angular impulse
+ ///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information
+ void enableFeedback(bool needsFeedback)
+ {
+ m_needsFeedback = needsFeedback;
+ }
+
+ ///getAppliedImpulse is an estimated total applied impulse.
+ ///This feedback could be used to determine breaking constraints or playing sounds.
+ btScalar getAppliedImpulse() const
+ {
+ btAssert(m_needsFeedback);
+ return m_appliedImpulse;
+ }
+
+ const btVector3& getAppliedLinearImpulse() const
+ {
+ btAssert(m_needsFeedback);
+ return m_appliedLinearImpulse;
+ }
+
+ btVector3& getAppliedLinearImpulse()
+ {
+ btAssert(m_needsFeedback);
+ return m_appliedLinearImpulse;
+ }
+
+ const btVector3& getAppliedAngularImpulseA() const
+ {
+ btAssert(m_needsFeedback);
+ return m_appliedAngularImpulseA;
+ }
+
+ btVector3& getAppliedAngularImpulseA()
+ {
+ btAssert(m_needsFeedback);
+ return m_appliedAngularImpulseA;
+ }
+
+ const btVector3& getAppliedAngularImpulseB() const
+ {
+ btAssert(m_needsFeedback);
+ return m_appliedAngularImpulseB;
+ }
+
+ btVector3& getAppliedAngularImpulseB()
+ {
+ btAssert(m_needsFeedback);
+ return m_appliedAngularImpulseB;
+ }
+
+
+
+ btTypedConstraintType getConstraintType () const
+ {
+ return btTypedConstraintType(m_objectType);
+ }
+
+ void setDbgDrawSize(btScalar dbgDrawSize)
+ {
+ m_dbgDrawSize = dbgDrawSize;
+ }
+ btScalar getDbgDrawSize()
+ {
+ return m_dbgDrawSize;
+ }
+
+};
+
+// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits
+// all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI])
+SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
+{
+ if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
+ {
+ return angleInRadians;
+ }
+ else if(angleInRadians < angleLowerLimitInRadians)
+ {
+ btScalar diffLo = btNormalizeAngle(angleLowerLimitInRadians - angleInRadians); // this is positive
+ btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
+ return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
+ }
+ else if(angleInRadians > angleUpperLimitInRadians)
+ {
+ btScalar diffHi = btNormalizeAngle(angleInRadians - angleUpperLimitInRadians); // this is positive
+ btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
+ return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians;
+ }
+ else
+ {
+ return angleInRadians;
+ }
+}
+
+
+#endif //TYPED_CONSTRAINT_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btUniversalConstraint.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btUniversalConstraint.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btUniversalConstraint.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btUniversalConstraint.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,60 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
+Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef UNIVERSAL_CONSTRAINT_H
+#define UNIVERSAL_CONSTRAINT_H
+
+
+
+#include "LinearMath/btVector3.h"
+#include "btTypedConstraint.h"
+#include "btGeneric6DofConstraint.h"
+
+
+
+/// Constraint similar to ODE Universal Joint
+/// has 2 rotatioonal degrees of freedom, similar to Euler rotations around Z (axis 1)
+/// and Y (axis 2)
+/// Description from ODE manual :
+/// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular.
+/// In other words, rotation of the two bodies about the direction perpendicular to the two axes will be equal."
+
+class btUniversalConstraint : public btGeneric6DofConstraint
+{
+protected:
+ btVector3 m_anchor;
+ btVector3 m_axis1;
+ btVector3 m_axis2;
+public:
+ // constructor
+ // anchor, axis1 and axis2 are in world coordinate system
+ // axis1 must be orthogonal to axis2
+ btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2);
+ // access
+ const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); }
+ const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); }
+ const btVector3& getAxis1() { return m_axis1; }
+ const btVector3& getAxis2() { return m_axis2; }
+ btScalar getAngle1() { return getAngle(2); }
+ btScalar getAngle2() { return getAngle(1); }
+ // limits
+ void setUpperLimit(btScalar ang1max, btScalar ang2max) { setAngularUpperLimit(btVector3(0.f, ang1max, ang2max)); }
+ void setLowerLimit(btScalar ang1min, btScalar ang2min) { setAngularLowerLimit(btVector3(0.f, ang1min, ang2min)); }
+};
+
+
+
+#endif // UNIVERSAL_CONSTRAINT_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btActionInterface.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btActionInterface.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btActionInterface.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btActionInterface.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,40 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef _BT_ACTION_INTERFACE_H
+#define _BT_ACTION_INTERFACE_H
+
+class btIDebugDraw;
+class btCollisionWorld;
+
+#include "LinearMath/btScalar.h"
+
+///Basic interface to allow actions such as vehicles and characters to be updated inside a btDynamicsWorld
+class btActionInterface
+{
+ public:
+
+ virtual ~btActionInterface()
+ {
+ }
+
+ virtual void updateAction( btCollisionWorld* collisionWorld, btScalar deltaTimeStep)=0;
+
+ virtual void debugDraw(btIDebugDraw* debugDrawer) = 0;
+
+};
+
+#endif //_BT_ACTION_INTERFACE_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,46 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_CONTINUOUS_DYNAMICS_WORLD_H
+#define BT_CONTINUOUS_DYNAMICS_WORLD_H
+
+#include "btDiscreteDynamicsWorld.h"
+
+///btContinuousDynamicsWorld adds optional (per object) continuous collision detection for fast moving objects to the btDiscreteDynamicsWorld.
+///This copes with fast moving objects that otherwise would tunnel/miss collisions.
+///Under construction, don't use yet! Please use btDiscreteDynamicsWorld instead.
+class btContinuousDynamicsWorld : public btDiscreteDynamicsWorld
+{
+
+ void updateTemporalAabbs(btScalar timeStep);
+
+ public:
+
+ btContinuousDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+ virtual ~btContinuousDynamicsWorld();
+
+ ///time stepping with calculation of time of impact for selected fast moving objects
+ virtual void internalSingleStepSimulation( btScalar timeStep);
+
+ virtual void calculateTimeOfImpacts(btScalar timeStep);
+
+ virtual btDynamicsWorldType getWorldType() const
+ {
+ return BT_CONTINUOUS_DYNAMICS_WORLD;
+ }
+
+};
+
+#endif //BT_CONTINUOUS_DYNAMICS_WORLD_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,197 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef BT_DISCRETE_DYNAMICS_WORLD_H
+#define BT_DISCRETE_DYNAMICS_WORLD_H
+
+#include "btDynamicsWorld.h"
+
+class btDispatcher;
+class btOverlappingPairCache;
+class btConstraintSolver;
+class btSimulationIslandManager;
+class btTypedConstraint;
+class btActionInterface;
+
+class btIDebugDraw;
+#include "LinearMath/btAlignedObjectArray.h"
+
+
+///btDiscreteDynamicsWorld provides discrete rigid body simulation
+///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController
+class btDiscreteDynamicsWorld : public btDynamicsWorld
+{
+protected:
+
+ btConstraintSolver* m_constraintSolver;
+
+ btSimulationIslandManager* m_islandManager;
+
+ btAlignedObjectArray<btTypedConstraint*> m_constraints;
+
+ btAlignedObjectArray<btRigidBody*> m_nonStaticRigidBodies;
+
+ btVector3 m_gravity;
+
+ //for variable timesteps
+ btScalar m_localTime;
+ //for variable timesteps
+
+ bool m_ownsIslandManager;
+ bool m_ownsConstraintSolver;
+ bool m_synchronizeAllMotionStates;
+
+ btAlignedObjectArray<btActionInterface*> m_actions;
+
+ int m_profileTimings;
+
+ virtual void predictUnconstraintMotion(btScalar timeStep);
+
+ virtual void integrateTransforms(btScalar timeStep);
+
+ virtual void calculateSimulationIslands();
+
+ virtual void solveConstraints(btContactSolverInfo& solverInfo);
+
+ void updateActivationState(btScalar timeStep);
+
+ void updateActions(btScalar timeStep);
+
+ void startProfiling(btScalar timeStep);
+
+ virtual void internalSingleStepSimulation( btScalar timeStep);
+
+
+ virtual void saveKinematicState(btScalar timeStep);
+
+ void debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color);
+
+
+public:
+
+
+ ///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those
+ btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+
+ virtual ~btDiscreteDynamicsWorld();
+
+ ///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
+ virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
+
+
+ virtual void synchronizeMotionStates();
+
+ ///this can be useful to synchronize a single rigid body -> graphics object
+ void synchronizeSingleMotionState(btRigidBody* body);
+
+ virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false);
+
+ virtual void removeConstraint(btTypedConstraint* constraint);
+
+ virtual void addAction(btActionInterface*);
+
+ virtual void removeAction(btActionInterface*);
+
+ btSimulationIslandManager* getSimulationIslandManager()
+ {
+ return m_islandManager;
+ }
+
+ const btSimulationIslandManager* getSimulationIslandManager() const
+ {
+ return m_islandManager;
+ }
+
+ btCollisionWorld* getCollisionWorld()
+ {
+ return this;
+ }
+
+ virtual void setGravity(const btVector3& gravity);
+
+ virtual btVector3 getGravity () const;
+
+ virtual void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::StaticFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+
+ virtual void addRigidBody(btRigidBody* body);
+
+ virtual void addRigidBody(btRigidBody* body, short group, short mask);
+
+ virtual void removeRigidBody(btRigidBody* body);
+
+ ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
+ virtual void removeCollisionObject(btCollisionObject* collisionObject);
+
+ void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
+
+ void debugDrawConstraint(btTypedConstraint* constraint);
+
+ virtual void debugDrawWorld();
+
+ virtual void setConstraintSolver(btConstraintSolver* solver);
+
+ virtual btConstraintSolver* getConstraintSolver();
+
+ virtual int getNumConstraints() const;
+
+ virtual btTypedConstraint* getConstraint(int index) ;
+
+ virtual const btTypedConstraint* getConstraint(int index) const;
+
+
+ virtual btDynamicsWorldType getWorldType() const
+ {
+ return BT_DISCRETE_DYNAMICS_WORLD;
+ }
+
+ ///the forces on each rigidbody is accumulating together with gravity. clear this after each timestep.
+ virtual void clearForces();
+
+ ///apply gravity, call this once per timestep
+ virtual void applyGravity();
+
+ virtual void setNumTasks(int numTasks)
+ {
+ (void) numTasks;
+ }
+
+ ///obsolete, use updateActions instead
+ virtual void updateVehicles(btScalar timeStep)
+ {
+ updateActions(timeStep);
+ }
+
+ ///obsolete, use addAction instead
+ virtual void addVehicle(btActionInterface* vehicle);
+ ///obsolete, use removeAction instead
+ virtual void removeVehicle(btActionInterface* vehicle);
+ ///obsolete, use addAction instead
+ virtual void addCharacter(btActionInterface* character);
+ ///obsolete, use removeAction instead
+ virtual void removeCharacter(btActionInterface* character);
+
+ void setSynchronizeAllMotionStates(bool synchronizeAll)
+ {
+ m_synchronizeAllMotionStates = synchronizeAll;
+ }
+ bool getSynchronizeAllMotionStates() const
+ {
+ return m_synchronizeAllMotionStates;
+ }
+
+};
+
+#endif //BT_DISCRETE_DYNAMICS_WORLD_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btDynamicsWorld.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btDynamicsWorld.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btDynamicsWorld.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btDynamicsWorld.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,148 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_DYNAMICS_WORLD_H
+#define BT_DYNAMICS_WORLD_H
+
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+
+class btTypedConstraint;
+class btActionInterface;
+class btConstraintSolver;
+class btDynamicsWorld;
+
+
+/// Type for the callback for each tick
+typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep);
+
+enum btDynamicsWorldType
+{
+ BT_SIMPLE_DYNAMICS_WORLD=1,
+ BT_DISCRETE_DYNAMICS_WORLD=2,
+ BT_CONTINUOUS_DYNAMICS_WORLD=3
+};
+
+///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.
+class btDynamicsWorld : public btCollisionWorld
+{
+
+protected:
+ btInternalTickCallback m_internalTickCallback;
+ btInternalTickCallback m_internalPreTickCallback;
+ void* m_worldUserInfo;
+
+ btContactSolverInfo m_solverInfo;
+
+public:
+
+
+ btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration)
+ :btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0),m_internalPreTickCallback(0), m_worldUserInfo(0)
+ {
+ }
+
+ virtual ~btDynamicsWorld()
+ {
+ }
+
+ ///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds.
+ ///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'.
+ ///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'.
+ ///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant.
+ virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0;
+
+ virtual void debugDrawWorld() = 0;
+
+ virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false)
+ {
+ (void)constraint; (void)disableCollisionsBetweenLinkedBodies;
+ }
+
+ virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;}
+
+ virtual void addAction(btActionInterface* action) = 0;
+
+ virtual void removeAction(btActionInterface* action) = 0;
+
+ //once a rigidbody is added to the dynamics world, it will get this gravity assigned
+ //existing rigidbodies in the world get gravity assigned too, during this method
+ virtual void setGravity(const btVector3& gravity) = 0;
+ virtual btVector3 getGravity () const = 0;
+
+ virtual void synchronizeMotionStates() = 0;
+
+ virtual void addRigidBody(btRigidBody* body) = 0;
+
+ virtual void removeRigidBody(btRigidBody* body) = 0;
+
+ virtual void setConstraintSolver(btConstraintSolver* solver) = 0;
+
+ virtual btConstraintSolver* getConstraintSolver() = 0;
+
+ virtual int getNumConstraints() const { return 0; }
+
+ virtual btTypedConstraint* getConstraint(int index) { (void)index; return 0; }
+
+ virtual const btTypedConstraint* getConstraint(int index) const { (void)index; return 0; }
+
+ virtual btDynamicsWorldType getWorldType() const=0;
+
+ virtual void clearForces() = 0;
+
+ /// Set the callback for when an internal tick (simulation substep) happens, optional user info
+ void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0,bool isPreTick=false)
+ {
+ if (isPreTick)
+ {
+ m_internalPreTickCallback = cb;
+ } else
+ {
+ m_internalTickCallback = cb;
+ }
+ m_worldUserInfo = worldUserInfo;
+ }
+
+ void setWorldUserInfo(void* worldUserInfo)
+ {
+ m_worldUserInfo = worldUserInfo;
+ }
+
+ void* getWorldUserInfo() const
+ {
+ return m_worldUserInfo;
+ }
+
+ btContactSolverInfo& getSolverInfo()
+ {
+ return m_solverInfo;
+ }
+
+
+ ///obsolete, use addAction instead.
+ virtual void addVehicle(btActionInterface* vehicle) {(void)vehicle;}
+ ///obsolete, use removeAction instead
+ virtual void removeVehicle(btActionInterface* vehicle) {(void)vehicle;}
+ ///obsolete, use addAction instead.
+ virtual void addCharacter(btActionInterface* character) {(void)character;}
+ ///obsolete, use removeAction instead
+ virtual void removeCharacter(btActionInterface* character) {(void)character;}
+
+
+};
+
+#endif //BT_DYNAMICS_WORLD_H
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btRigidBody.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btRigidBody.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btRigidBody.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btRigidBody.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,503 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef RIGIDBODY_H
+#define RIGIDBODY_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btTransform.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+class btCollisionShape;
+class btMotionState;
+class btTypedConstraint;
+
+
+extern btScalar gDeactivationTime;
+extern bool gDisableDeactivation;
+
+
+///The btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape.
+///It is recommended for performance and memory use to share btCollisionShape objects whenever possible.
+///There are 3 types of rigid bodies:
+///- A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics.
+///- B) Fixed objects with zero mass. They are not moving (basically collision objects)
+///- C) Kinematic objects, which are objects without mass, but the user can move them. There is on-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform.
+///Bullet automatically deactivates dynamic rigid bodies, when the velocity is below a threshold for a given time.
+///Deactivated (sleeping) rigid bodies don't take any processing time, except a minor broadphase collision detection impact (to allow active objects to activate/wake up sleeping objects)
+class btRigidBody : public btCollisionObject
+{
+
+ btMatrix3x3 m_invInertiaTensorWorld;
+ btVector3 m_linearVelocity;
+ btVector3 m_angularVelocity;
+ btScalar m_inverseMass;
+ btVector3 m_angularFactor;
+ btVector3 m_linearFactor;
+
+ btVector3 m_gravity;
+ btVector3 m_gravity_acceleration;
+ btVector3 m_invInertiaLocal;
+ btVector3 m_totalForce;
+ btVector3 m_totalTorque;
+
+ btScalar m_linearDamping;
+ btScalar m_angularDamping;
+
+ bool m_additionalDamping;
+ btScalar m_additionalDampingFactor;
+ btScalar m_additionalLinearDampingThresholdSqr;
+ btScalar m_additionalAngularDampingThresholdSqr;
+ btScalar m_additionalAngularDampingFactor;
+
+
+ btScalar m_linearSleepingThreshold;
+ btScalar m_angularSleepingThreshold;
+
+ //m_optionalMotionState allows to automatic synchronize the world transform for active objects
+ btMotionState* m_optionalMotionState;
+
+ //keep track of typed constraints referencing this rigid body
+ btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;
+
+public:
+
+
+ ///The btRigidBodyConstructionInfo structure provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body.
+ ///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument)
+ ///You can use the motion state to synchronize the world transform between physics and graphics objects.
+ ///And if the motion state is provided, the rigid body will initialize its initial world transform from the motion state,
+ ///m_startWorldTransform is only used when you don't provide a motion state.
+ struct btRigidBodyConstructionInfo
+ {
+ btScalar m_mass;
+
+ ///When a motionState is provided, the rigid body will initialize its world transform from the motion state
+ ///In this case, m_startWorldTransform is ignored.
+ btMotionState* m_motionState;
+ btTransform m_startWorldTransform;
+
+ btCollisionShape* m_collisionShape;
+ btVector3 m_localInertia;
+ btScalar m_linearDamping;
+ btScalar m_angularDamping;
+
+ ///best simulation results when friction is non-zero
+ btScalar m_friction;
+ ///best simulation results using zero restitution.
+ btScalar m_restitution;
+
+ btScalar m_linearSleepingThreshold;
+ btScalar m_angularSleepingThreshold;
+
+ //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
+ //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
+ bool m_additionalDamping;
+ btScalar m_additionalDampingFactor;
+ btScalar m_additionalLinearDampingThresholdSqr;
+ btScalar m_additionalAngularDampingThresholdSqr;
+ btScalar m_additionalAngularDampingFactor;
+
+
+ btRigidBodyConstructionInfo( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
+ m_mass(mass),
+ m_motionState(motionState),
+ m_collisionShape(collisionShape),
+ m_localInertia(localInertia),
+ m_linearDamping(btScalar(0.)),
+ m_angularDamping(btScalar(0.)),
+ m_friction(btScalar(0.5)),
+ m_restitution(btScalar(0.)),
+ m_linearSleepingThreshold(btScalar(0.8)),
+ m_angularSleepingThreshold(btScalar(1.f)),
+ m_additionalDamping(false),
+ m_additionalDampingFactor(btScalar(0.005)),
+ m_additionalLinearDampingThresholdSqr(btScalar(0.01)),
+ m_additionalAngularDampingThresholdSqr(btScalar(0.01)),
+ m_additionalAngularDampingFactor(btScalar(0.01))
+ {
+ m_startWorldTransform.setIdentity();
+ }
+ };
+
+ ///btRigidBody constructor using construction info
+ btRigidBody( const btRigidBodyConstructionInfo& constructionInfo);
+
+ ///btRigidBody constructor for backwards compatibility.
+ ///To specify friction (etc) during rigid body construction, please use the other constructor (using btRigidBodyConstructionInfo)
+ btRigidBody( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0));
+
+
+ virtual ~btRigidBody()
+ {
+ //No constraints should point to this rigidbody
+ //Remove constraints from the dynamics world before you delete the related rigidbodies.
+ btAssert(m_constraintRefs.size()==0);
+ }
+
+protected:
+
+ ///setupRigidBody is only used internally by the constructor
+ void setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo);
+
+public:
+
+ void proceedToTransform(const btTransform& newTrans);
+
+ ///to keep collision detection and dynamics separate we don't store a rigidbody pointer
+ ///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
+ static const btRigidBody* upcast(const btCollisionObject* colObj)
+ {
+ if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
+ return (const btRigidBody*)colObj;
+ return 0;
+ }
+ static btRigidBody* upcast(btCollisionObject* colObj)
+ {
+ if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
+ return (btRigidBody*)colObj;
+ return 0;
+ }
+
+ /// continuous collision detection needs prediction
+ void predictIntegratedTransform(btScalar step, btTransform& predictedTransform) ;
+
+ void saveKinematicState(btScalar step);
+
+ void applyGravity();
+
+ void setGravity(const btVector3& acceleration);
+
+ const btVector3& getGravity() const
+ {
+ return m_gravity_acceleration;
+ }
+
+ void setDamping(btScalar lin_damping, btScalar ang_damping);
+
+ btScalar getLinearDamping() const
+ {
+ return m_linearDamping;
+ }
+
+ btScalar getAngularDamping() const
+ {
+ return m_angularDamping;
+ }
+
+ btScalar getLinearSleepingThreshold() const
+ {
+ return m_linearSleepingThreshold;
+ }
+
+ btScalar getAngularSleepingThreshold() const
+ {
+ return m_angularSleepingThreshold;
+ }
+
+ void applyDamping(btScalar timeStep);
+
+ SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const {
+ return m_collisionShape;
+ }
+
+ SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() {
+ return m_collisionShape;
+ }
+
+ void setMassProps(btScalar mass, const btVector3& inertia);
+
+ const btVector3& getLinearFactor() const
+ {
+ return m_linearFactor;
+ }
+ void setLinearFactor(const btVector3& linearFactor)
+ {
+ m_linearFactor = linearFactor;
+ }
+ btScalar getInvMass() const { return m_inverseMass; }
+ const btMatrix3x3& getInvInertiaTensorWorld() const {
+ return m_invInertiaTensorWorld;
+ }
+
+ void integrateVelocities(btScalar step);
+
+ void setCenterOfMassTransform(const btTransform& xform);
+
+ void applyCentralForce(const btVector3& force)
+ {
+ m_totalForce += force*m_linearFactor;
+ }
+
+ const btVector3& getTotalForce()
+ {
+ return m_totalForce;
+ };
+
+ const btVector3& getTotalTorque()
+ {
+ return m_totalTorque;
+ };
+
+ const btVector3& getInvInertiaDiagLocal() const
+ {
+ return m_invInertiaLocal;
+ };
+
+ void setInvInertiaDiagLocal(const btVector3& diagInvInertia)
+ {
+ m_invInertiaLocal = diagInvInertia;
+ }
+
+ void setSleepingThresholds(btScalar linear,btScalar angular)
+ {
+ m_linearSleepingThreshold = linear;
+ m_angularSleepingThreshold = angular;
+ }
+
+ void applyTorque(const btVector3& torque)
+ {
+ m_totalTorque += torque*m_angularFactor;
+ }
+
+ void applyForce(const btVector3& force, const btVector3& rel_pos)
+ {
+ applyCentralForce(force);
+ applyTorque(rel_pos.cross(force*m_linearFactor));
+ }
+
+ void applyCentralImpulse(const btVector3& impulse)
+ {
+ m_linearVelocity += impulse *m_linearFactor * m_inverseMass;
+ }
+
+ void applyTorqueImpulse(const btVector3& torque)
+ {
+ m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
+ }
+
+ void applyImpulse(const btVector3& impulse, const btVector3& rel_pos)
+ {
+ if (m_inverseMass != btScalar(0.))
+ {
+ applyCentralImpulse(impulse);
+ if (m_angularFactor)
+ {
+ applyTorqueImpulse(rel_pos.cross(impulse*m_linearFactor));
+ }
+ }
+ }
+
+ //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+ SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+ {
+ if (m_inverseMass != btScalar(0.))
+ {
+ m_linearVelocity += linearComponent*m_linearFactor*impulseMagnitude;
+ if (m_angularFactor)
+ {
+ m_angularVelocity += angularComponent*m_angularFactor*impulseMagnitude;
+ }
+ }
+ }
+
+ void clearForces()
+ {
+ m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+ m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+ }
+
+ void updateInertiaTensor();
+
+ const btVector3& getCenterOfMassPosition() const {
+ return m_worldTransform.getOrigin();
+ }
+ btQuaternion getOrientation() const;
+
+ const btTransform& getCenterOfMassTransform() const {
+ return m_worldTransform;
+ }
+ const btVector3& getLinearVelocity() const {
+ return m_linearVelocity;
+ }
+ const btVector3& getAngularVelocity() const {
+ return m_angularVelocity;
+ }
+
+
+ inline void setLinearVelocity(const btVector3& lin_vel)
+ {
+ m_linearVelocity = lin_vel;
+ }
+
+ inline void setAngularVelocity(const btVector3& ang_vel)
+ {
+ m_angularVelocity = ang_vel;
+ }
+
+ btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const
+ {
+ //we also calculate lin/ang velocity for kinematic objects
+ return m_linearVelocity + m_angularVelocity.cross(rel_pos);
+
+ //for kinematic objects, we could also use use:
+ // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
+ }
+
+ void translate(const btVector3& v)
+ {
+ m_worldTransform.getOrigin() += v;
+ }
+
+
+ void getAabb(btVector3& aabbMin,btVector3& aabbMax) const;
+
+
+
+
+
+ SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3& pos, const btVector3& normal) const
+ {
+ btVector3 r0 = pos - getCenterOfMassPosition();
+
+ btVector3 c0 = (r0).cross(normal);
+
+ btVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0);
+
+ return m_inverseMass + normal.dot(vec);
+
+ }
+
+ SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const
+ {
+ btVector3 vec = axis * getInvInertiaTensorWorld();
+ return axis.dot(vec);
+ }
+
+ SIMD_FORCE_INLINE void updateDeactivation(btScalar timeStep)
+ {
+ if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
+ return;
+
+ if ((getLinearVelocity().length2() < m_linearSleepingThreshold*m_linearSleepingThreshold) &&
+ (getAngularVelocity().length2() < m_angularSleepingThreshold*m_angularSleepingThreshold))
+ {
+ m_deactivationTime += timeStep;
+ } else
+ {
+ m_deactivationTime=btScalar(0.);
+ setActivationState(0);
+ }
+
+ }
+
+ SIMD_FORCE_INLINE bool wantsSleeping()
+ {
+
+ if (getActivationState() == DISABLE_DEACTIVATION)
+ return false;
+
+ //disable deactivation
+ if (gDisableDeactivation || (gDeactivationTime == btScalar(0.)))
+ return false;
+
+ if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
+ return true;
+
+ if (m_deactivationTime> gDeactivationTime)
+ {
+ return true;
+ }
+ return false;
+ }
+
+
+
+ const btBroadphaseProxy* getBroadphaseProxy() const
+ {
+ return m_broadphaseHandle;
+ }
+ btBroadphaseProxy* getBroadphaseProxy()
+ {
+ return m_broadphaseHandle;
+ }
+ void setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy)
+ {
+ m_broadphaseHandle = broadphaseProxy;
+ }
+
+ //btMotionState allows to automatic synchronize the world transform for active objects
+ btMotionState* getMotionState()
+ {
+ return m_optionalMotionState;
+ }
+ const btMotionState* getMotionState() const
+ {
+ return m_optionalMotionState;
+ }
+ void setMotionState(btMotionState* motionState)
+ {
+ m_optionalMotionState = motionState;
+ if (m_optionalMotionState)
+ motionState->getWorldTransform(m_worldTransform);
+ }
+
+ //for experimental overriding of friction/contact solver func
+ int m_contactSolverType;
+ int m_frictionSolverType;
+
+ void setAngularFactor(const btVector3& angFac)
+ {
+ m_angularFactor = angFac;
+ }
+
+ void setAngularFactor(btScalar angFac)
+ {
+ m_angularFactor.setValue(angFac,angFac,angFac);
+ }
+ const btVector3& getAngularFactor() const
+ {
+ return m_angularFactor;
+ }
+
+ //is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase?
+ bool isInWorld() const
+ {
+ return (getBroadphaseProxy() != 0);
+ }
+
+ virtual bool checkCollideWithOverride(btCollisionObject* co);
+
+ void addConstraintRef(btTypedConstraint* c);
+ void removeConstraintRef(btTypedConstraint* c);
+
+ btTypedConstraint* getConstraintRef(int index)
+ {
+ return m_constraintRefs[index];
+ }
+
+ int getNumConstraintRefs()
+ {
+ return m_constraintRefs.size();
+ }
+
+ int m_debugBodyId;
+};
+
+
+
+#endif
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,81 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_SIMPLE_DYNAMICS_WORLD_H
+#define BT_SIMPLE_DYNAMICS_WORLD_H
+
+#include "btDynamicsWorld.h"
+
+class btDispatcher;
+class btOverlappingPairCache;
+class btConstraintSolver;
+
+///The btSimpleDynamicsWorld serves as unit-test and to verify more complicated and optimized dynamics worlds.
+///Please use btDiscreteDynamicsWorld instead (or btContinuousDynamicsWorld once it is finished).
+class btSimpleDynamicsWorld : public btDynamicsWorld
+{
+protected:
+
+ btConstraintSolver* m_constraintSolver;
+
+ bool m_ownsConstraintSolver;
+
+ void predictUnconstraintMotion(btScalar timeStep);
+
+ void integrateTransforms(btScalar timeStep);
+
+ btVector3 m_gravity;
+
+public:
+
+
+
+ ///this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver
+ btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+
+ virtual ~btSimpleDynamicsWorld();
+
+ ///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead
+ virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
+
+ virtual void setGravity(const btVector3& gravity);
+
+ virtual btVector3 getGravity () const;
+
+ virtual void addRigidBody(btRigidBody* body);
+
+ virtual void removeRigidBody(btRigidBody* body);
+
+ ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
+ virtual void removeCollisionObject(btCollisionObject* collisionObject);
+
+ virtual void updateAabbs();
+
+ virtual void synchronizeMotionStates();
+
+ virtual void setConstraintSolver(btConstraintSolver* solver);
+
+ virtual btConstraintSolver* getConstraintSolver();
+
+ virtual btDynamicsWorldType getWorldType() const
+ {
+ return BT_SIMPLE_DYNAMICS_WORLD;
+ }
+
+ virtual void clearForces();
+
+};
+
+#endif //BT_SIMPLE_DYNAMICS_WORLD_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Vehicle/btRaycastVehicle.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Vehicle/btRaycastVehicle.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Vehicle/btRaycastVehicle.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Vehicle/btRaycastVehicle.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,209 @@
+/*
+ * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability
+ * of this software for any purpose.
+ * It is provided "as is" without express or implied warranty.
+*/
+#ifndef RAYCASTVEHICLE_H
+#define RAYCASTVEHICLE_H
+
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "btVehicleRaycaster.h"
+class btDynamicsWorld;
+#include "LinearMath/btAlignedObjectArray.h"
+#include "btWheelInfo.h"
+#include "BulletDynamics/Dynamics/btActionInterface.h"
+
+class btVehicleTuning;
+
+///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle.
+class btRaycastVehicle : public btActionInterface
+{
+
+ btAlignedObjectArray<btVector3> m_forwardWS;
+ btAlignedObjectArray<btVector3> m_axle;
+ btAlignedObjectArray<btScalar> m_forwardImpulse;
+ btAlignedObjectArray<btScalar> m_sideImpulse;
+
+public:
+ class btVehicleTuning
+ {
+ public:
+
+ btVehicleTuning()
+ :m_suspensionStiffness(btScalar(5.88)),
+ m_suspensionCompression(btScalar(0.83)),
+ m_suspensionDamping(btScalar(0.88)),
+ m_maxSuspensionTravelCm(btScalar(500.)),
+ m_frictionSlip(btScalar(10.5))
+ {
+ }
+ btScalar m_suspensionStiffness;
+ btScalar m_suspensionCompression;
+ btScalar m_suspensionDamping;
+ btScalar m_maxSuspensionTravelCm;
+ btScalar m_frictionSlip;
+
+ };
+private:
+
+ btScalar m_tau;
+ btScalar m_damping;
+ btVehicleRaycaster* m_vehicleRaycaster;
+ btScalar m_pitchControl;
+ btScalar m_steeringValue;
+ btScalar m_currentVehicleSpeedKmHour;
+
+ btRigidBody* m_chassisBody;
+
+ int m_indexRightAxis;
+ int m_indexUpAxis;
+ int m_indexForwardAxis;
+
+ void defaultInit(const btVehicleTuning& tuning);
+
+public:
+
+ //constructor to create a car from an existing rigidbody
+ btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster );
+
+ virtual ~btRaycastVehicle() ;
+
+
+ ///btActionInterface interface
+ virtual void updateAction( btCollisionWorld* collisionWorld, btScalar step)
+ {
+ updateVehicle(step);
+ }
+
+
+ ///btActionInterface interface
+ void debugDraw(btIDebugDraw* debugDrawer);
+
+ const btTransform& getChassisWorldTransform() const;
+
+ btScalar rayCast(btWheelInfo& wheel);
+
+ virtual void updateVehicle(btScalar step);
+
+
+ void resetSuspension();
+
+ btScalar getSteeringValue(int wheel) const;
+
+ void setSteeringValue(btScalar steering,int wheel);
+
+
+ void applyEngineForce(btScalar force, int wheel);
+
+ const btTransform& getWheelTransformWS( int wheelIndex ) const;
+
+ void updateWheelTransform( int wheelIndex, bool interpolatedTransform = true );
+
+ void setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth);
+
+ btWheelInfo& addWheel( const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS,btScalar suspensionRestLength,btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel);
+
+ inline int getNumWheels() const {
+ return int (m_wheelInfo.size());
+ }
+
+ btAlignedObjectArray<btWheelInfo> m_wheelInfo;
+
+
+ const btWheelInfo& getWheelInfo(int index) const;
+
+ btWheelInfo& getWheelInfo(int index);
+
+ void updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform = true);
+
+
+ void setBrake(btScalar brake,int wheelIndex);
+
+ void setPitchControl(btScalar pitch)
+ {
+ m_pitchControl = pitch;
+ }
+
+ void updateSuspension(btScalar deltaTime);
+
+ virtual void updateFriction(btScalar timeStep);
+
+
+
+ inline btRigidBody* getRigidBody()
+ {
+ return m_chassisBody;
+ }
+
+ const btRigidBody* getRigidBody() const
+ {
+ return m_chassisBody;
+ }
+
+ inline int getRightAxis() const
+ {
+ return m_indexRightAxis;
+ }
+ inline int getUpAxis() const
+ {
+ return m_indexUpAxis;
+ }
+
+ inline int getForwardAxis() const
+ {
+ return m_indexForwardAxis;
+ }
+
+
+ ///Worldspace forward vector
+ btVector3 getForwardVector() const
+ {
+ const btTransform& chassisTrans = getChassisWorldTransform();
+
+ btVector3 forwardW (
+ chassisTrans.getBasis()[0][m_indexForwardAxis],
+ chassisTrans.getBasis()[1][m_indexForwardAxis],
+ chassisTrans.getBasis()[2][m_indexForwardAxis]);
+
+ return forwardW;
+ }
+
+ ///Velocity of vehicle (positive if velocity vector has same direction as foward vector)
+ btScalar getCurrentSpeedKmHour() const
+ {
+ return m_currentVehicleSpeedKmHour;
+ }
+
+ virtual void setCoordinateSystem(int rightIndex,int upIndex,int forwardIndex)
+ {
+ m_indexRightAxis = rightIndex;
+ m_indexUpAxis = upIndex;
+ m_indexForwardAxis = forwardIndex;
+ }
+
+
+
+};
+
+class btDefaultVehicleRaycaster : public btVehicleRaycaster
+{
+ btDynamicsWorld* m_dynamicsWorld;
+public:
+ btDefaultVehicleRaycaster(btDynamicsWorld* world)
+ :m_dynamicsWorld(world)
+ {
+ }
+
+ virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result);
+
+};
+
+
+#endif //RAYCASTVEHICLE_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Vehicle/btVehicleRaycaster.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Vehicle/btVehicleRaycaster.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Vehicle/btVehicleRaycaster.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Vehicle/btVehicleRaycaster.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,35 @@
+/*
+ * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability
+ * of this software for any purpose.
+ * It is provided "as is" without express or implied warranty.
+*/
+#ifndef VEHICLE_RAYCASTER_H
+#define VEHICLE_RAYCASTER_H
+
+#include "LinearMath/btVector3.h"
+
+/// btVehicleRaycaster is provides interface for between vehicle simulation and raycasting
+struct btVehicleRaycaster
+{
+virtual ~btVehicleRaycaster()
+{
+}
+ struct btVehicleRaycasterResult
+ {
+ btVehicleRaycasterResult() :m_distFraction(btScalar(-1.)){};
+ btVector3 m_hitPointInWorld;
+ btVector3 m_hitNormalInWorld;
+ btScalar m_distFraction;
+ };
+
+ virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) = 0;
+
+};
+
+#endif //VEHICLE_RAYCASTER_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Vehicle/btWheelInfo.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Vehicle/btWheelInfo.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Vehicle/btWheelInfo.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Vehicle/btWheelInfo.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability
+ * of this software for any purpose.
+ * It is provided "as is" without express or implied warranty.
+*/
+#ifndef WHEEL_INFO_H
+#define WHEEL_INFO_H
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+
+class btRigidBody;
+
+struct btWheelInfoConstructionInfo
+{
+ btVector3 m_chassisConnectionCS;
+ btVector3 m_wheelDirectionCS;
+ btVector3 m_wheelAxleCS;
+ btScalar m_suspensionRestLength;
+ btScalar m_maxSuspensionTravelCm;
+ btScalar m_wheelRadius;
+
+ btScalar m_suspensionStiffness;
+ btScalar m_wheelsDampingCompression;
+ btScalar m_wheelsDampingRelaxation;
+ btScalar m_frictionSlip;
+ bool m_bIsFrontWheel;
+
+};
+
+/// btWheelInfo contains information per wheel about friction and suspension.
+struct btWheelInfo
+{
+ struct RaycastInfo
+ {
+ //set by raycaster
+ btVector3 m_contactNormalWS;//contactnormal
+ btVector3 m_contactPointWS;//raycast hitpoint
+ btScalar m_suspensionLength;
+ btVector3 m_hardPointWS;//raycast starting point
+ btVector3 m_wheelDirectionWS; //direction in worldspace
+ btVector3 m_wheelAxleWS; // axle in worldspace
+ bool m_isInContact;
+ void* m_groundObject; //could be general void* ptr
+ };
+
+ RaycastInfo m_raycastInfo;
+
+ btTransform m_worldTransform;
+
+ btVector3 m_chassisConnectionPointCS; //const
+ btVector3 m_wheelDirectionCS;//const
+ btVector3 m_wheelAxleCS; // const or modified by steering
+ btScalar m_suspensionRestLength1;//const
+ btScalar m_maxSuspensionTravelCm;
+ btScalar getSuspensionRestLength() const;
+ btScalar m_wheelsRadius;//const
+ btScalar m_suspensionStiffness;//const
+ btScalar m_wheelsDampingCompression;//const
+ btScalar m_wheelsDampingRelaxation;//const
+ btScalar m_frictionSlip;
+ btScalar m_steering;
+ btScalar m_rotation;
+ btScalar m_deltaRotation;
+ btScalar m_rollInfluence;
+
+ btScalar m_engineForce;
+
+ btScalar m_brake;
+
+ bool m_bIsFrontWheel;
+
+ void* m_clientInfo;//can be used to store pointer to sync transforms...
+
+ btWheelInfo(btWheelInfoConstructionInfo& ci)
+
+ {
+
+ m_suspensionRestLength1 = ci.m_suspensionRestLength;
+ m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm;
+
+ m_wheelsRadius = ci.m_wheelRadius;
+ m_suspensionStiffness = ci.m_suspensionStiffness;
+ m_wheelsDampingCompression = ci.m_wheelsDampingCompression;
+ m_wheelsDampingRelaxation = ci.m_wheelsDampingRelaxation;
+ m_chassisConnectionPointCS = ci.m_chassisConnectionCS;
+ m_wheelDirectionCS = ci.m_wheelDirectionCS;
+ m_wheelAxleCS = ci.m_wheelAxleCS;
+ m_frictionSlip = ci.m_frictionSlip;
+ m_steering = btScalar(0.);
+ m_engineForce = btScalar(0.);
+ m_rotation = btScalar(0.);
+ m_deltaRotation = btScalar(0.);
+ m_brake = btScalar(0.);
+ m_rollInfluence = btScalar(0.1);
+ m_bIsFrontWheel = ci.m_bIsFrontWheel;
+
+ }
+
+ void updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo);
+
+ btScalar m_clippedInvContactDotSuspension;
+ btScalar m_suspensionRelativeVelocity;
+ //calculated by suspension
+ btScalar m_wheelsSuspensionForce;
+ btScalar m_skidInfo;
+
+};
+
+#endif //WHEEL_INFO_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBody.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBody.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBody.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBody.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,891 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///btSoftBody implementation by Nathanael Presson
+
+#ifndef _BT_SOFT_BODY_H
+#define _BT_SOFT_BODY_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+
+#include "BulletCollision/CollisionShapes/btConcaveShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+#include "btSparseSDF.h"
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+
+class btBroadphaseInterface;
+class btDispatcher;
+
+/* btSoftBodyWorldInfo */
+struct btSoftBodyWorldInfo
+{
+ btScalar air_density;
+ btScalar water_density;
+ btScalar water_offset;
+ btVector3 water_normal;
+ btBroadphaseInterface* m_broadphase;
+ btDispatcher* m_dispatcher;
+ btVector3 m_gravity;
+ btSparseSdf<3> m_sparsesdf;
+};
+
+
+///The btSoftBody is an class to simulate cloth and volumetric soft bodies.
+///There is two-way interaction between btSoftBody and btRigidBody/btCollisionObject.
+class btSoftBody : public btCollisionObject
+{
+public:
+ btAlignedObjectArray<class btCollisionObject*> m_collisionDisabledObjects;
+
+ //
+ // Enumerations
+ //
+
+ ///eAeroModel
+ struct eAeroModel { enum _ {
+ V_Point, ///Vertex normals are oriented toward velocity
+ V_TwoSided, ///Vertex normals are fliped to match velocity
+ V_OneSided, ///Vertex normals are taken as it is
+ F_TwoSided, ///Face normals are fliped to match velocity
+ F_OneSided, ///Face normals are taken as it is
+ END
+ };};
+
+ ///eVSolver : velocities solvers
+ struct eVSolver { enum _ {
+ Linear, ///Linear solver
+ END
+ };};
+
+ ///ePSolver : positions solvers
+ struct ePSolver { enum _ {
+ Linear, ///Linear solver
+ Anchors, ///Anchor solver
+ RContacts, ///Rigid contacts solver
+ SContacts, ///Soft contacts solver
+ END
+ };};
+
+ ///eSolverPresets
+ struct eSolverPresets { enum _ {
+ Positions,
+ Velocities,
+ Default = Positions,
+ END
+ };};
+
+ ///eFeature
+ struct eFeature { enum _ {
+ None,
+ Node,
+ Link,
+ Face,
+ END
+ };};
+
+ typedef btAlignedObjectArray<eVSolver::_> tVSolverArray;
+ typedef btAlignedObjectArray<ePSolver::_> tPSolverArray;
+
+ //
+ // Flags
+ //
+
+ ///fCollision
+ struct fCollision { enum _ {
+ RVSmask = 0x000f, ///Rigid versus soft mask
+ SDF_RS = 0x0001, ///SDF based rigid vs soft
+ CL_RS = 0x0002, ///Cluster vs convex rigid vs soft
+
+ SVSmask = 0x0030, ///Rigid versus soft mask
+ VF_SS = 0x0010, ///Vertex vs face soft vs soft handling
+ CL_SS = 0x0020, ///Cluster vs cluster soft vs soft handling
+ CL_SELF = 0x0040, ///Cluster soft body self collision
+ /* presets */
+ Default = SDF_RS,
+ END
+ };};
+
+ ///fMaterial
+ struct fMaterial { enum _ {
+ DebugDraw = 0x0001, /// Enable debug draw
+ /* presets */
+ Default = DebugDraw,
+ END
+ };};
+
+ //
+ // API Types
+ //
+
+ /* sRayCast */
+ struct sRayCast
+ {
+ btSoftBody* body; /// soft body
+ eFeature::_ feature; /// feature type
+ int index; /// feature index
+ btScalar fraction; /// time of impact fraction (rayorg+(rayto-rayfrom)*fraction)
+ };
+
+ /* ImplicitFn */
+ struct ImplicitFn
+ {
+ virtual btScalar Eval(const btVector3& x)=0;
+ };
+
+ //
+ // Internal types
+ //
+
+ typedef btAlignedObjectArray<btScalar> tScalarArray;
+ typedef btAlignedObjectArray<btVector3> tVector3Array;
+
+ /* sCti is Softbody contact info */
+ struct sCti
+ {
+ btCollisionObject* m_colObj; /* Rigid body */
+ btVector3 m_normal; /* Outward normal */
+ btScalar m_offset; /* Offset from origin */
+ };
+
+ /* sMedium */
+ struct sMedium
+ {
+ btVector3 m_velocity; /* Velocity */
+ btScalar m_pressure; /* Pressure */
+ btScalar m_density; /* Density */
+ };
+
+ /* Base type */
+ struct Element
+ {
+ void* m_tag; // User data
+ Element() : m_tag(0) {}
+ };
+ /* Material */
+ struct Material : Element
+ {
+ btScalar m_kLST; // Linear stiffness coefficient [0,1]
+ btScalar m_kAST; // Area/Angular stiffness coefficient [0,1]
+ btScalar m_kVST; // Volume stiffness coefficient [0,1]
+ int m_flags; // Flags
+ };
+
+ /* Feature */
+ struct Feature : Element
+ {
+ Material* m_material; // Material
+ };
+ /* Node */
+ struct Node : Feature
+ {
+ btVector3 m_x; // Position
+ btVector3 m_q; // Previous step position
+ btVector3 m_v; // Velocity
+ btVector3 m_f; // Force accumulator
+ btVector3 m_n; // Normal
+ btScalar m_im; // 1/mass
+ btScalar m_area; // Area
+ btDbvtNode* m_leaf; // Leaf data
+ int m_battach:1; // Attached
+ };
+ /* Link */
+ struct Link : Feature
+ {
+ Node* m_n[2]; // Node pointers
+ btScalar m_rl; // Rest length
+ int m_bbending:1; // Bending link
+ btScalar m_c0; // (ima+imb)*kLST
+ btScalar m_c1; // rl^2
+ btScalar m_c2; // |gradient|^2/c0
+ btVector3 m_c3; // gradient
+ };
+ /* Face */
+ struct Face : Feature
+ {
+ Node* m_n[3]; // Node pointers
+ btVector3 m_normal; // Normal
+ btScalar m_ra; // Rest area
+ btDbvtNode* m_leaf; // Leaf data
+ };
+ /* Tetra */
+ struct Tetra : Feature
+ {
+ Node* m_n[4]; // Node pointers
+ btScalar m_rv; // Rest volume
+ btDbvtNode* m_leaf; // Leaf data
+ btVector3 m_c0[4]; // gradients
+ btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3)
+ btScalar m_c2; // m_c1/sum(|g0..3|^2)
+ };
+ /* RContact */
+ struct RContact
+ {
+ sCti m_cti; // Contact infos
+ Node* m_node; // Owner node
+ btMatrix3x3 m_c0; // Impulse matrix
+ btVector3 m_c1; // Relative anchor
+ btScalar m_c2; // ima*dt
+ btScalar m_c3; // Friction
+ btScalar m_c4; // Hardness
+ };
+ /* SContact */
+ struct SContact
+ {
+ Node* m_node; // Node
+ Face* m_face; // Face
+ btVector3 m_weights; // Weigths
+ btVector3 m_normal; // Normal
+ btScalar m_margin; // Margin
+ btScalar m_friction; // Friction
+ btScalar m_cfm[2]; // Constraint force mixing
+ };
+ /* Anchor */
+ struct Anchor
+ {
+ Node* m_node; // Node pointer
+ btVector3 m_local; // Anchor position in body space
+ btRigidBody* m_body; // Body
+ btMatrix3x3 m_c0; // Impulse matrix
+ btVector3 m_c1; // Relative anchor
+ btScalar m_c2; // ima*dt
+ };
+ /* Note */
+ struct Note : Element
+ {
+ const char* m_text; // Text
+ btVector3 m_offset; // Offset
+ int m_rank; // Rank
+ Node* m_nodes[4]; // Nodes
+ btScalar m_coords[4]; // Coordinates
+ };
+ /* Pose */
+ struct Pose
+ {
+ bool m_bvolume; // Is valid
+ bool m_bframe; // Is frame
+ btScalar m_volume; // Rest volume
+ tVector3Array m_pos; // Reference positions
+ tScalarArray m_wgh; // Weights
+ btVector3 m_com; // COM
+ btMatrix3x3 m_rot; // Rotation
+ btMatrix3x3 m_scl; // Scale
+ btMatrix3x3 m_aqq; // Base scaling
+ };
+ /* Cluster */
+ struct Cluster
+ {
+ btAlignedObjectArray<Node*> m_nodes;
+ tScalarArray m_masses;
+ tVector3Array m_framerefs;
+ btTransform m_framexform;
+ btScalar m_idmass;
+ btScalar m_imass;
+ btMatrix3x3 m_locii;
+ btMatrix3x3 m_invwi;
+ btVector3 m_com;
+ btVector3 m_vimpulses[2];
+ btVector3 m_dimpulses[2];
+ int m_nvimpulses;
+ int m_ndimpulses;
+ btVector3 m_lv;
+ btVector3 m_av;
+ btDbvtNode* m_leaf;
+ btScalar m_ndamping; /* Node damping */
+ btScalar m_ldamping; /* Linear damping */
+ btScalar m_adamping; /* Angular damping */
+ btScalar m_matching;
+ btScalar m_maxSelfCollisionImpulse;
+ btScalar m_selfCollisionImpulseFactor;
+ bool m_containsAnchor;
+ bool m_collide;
+ int m_clusterIndex;
+ Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0)
+ ,m_maxSelfCollisionImpulse(100.f),
+ m_selfCollisionImpulseFactor(0.01f),
+ m_containsAnchor(false)
+ {}
+ };
+ /* Impulse */
+ struct Impulse
+ {
+ btVector3 m_velocity;
+ btVector3 m_drift;
+ int m_asVelocity:1;
+ int m_asDrift:1;
+ Impulse() : m_velocity(0,0,0),m_drift(0,0,0),m_asVelocity(0),m_asDrift(0) {}
+ Impulse operator -() const
+ {
+ Impulse i=*this;
+ i.m_velocity=-i.m_velocity;
+ i.m_drift=-i.m_drift;
+ return(i);
+ }
+ Impulse operator*(btScalar x) const
+ {
+ Impulse i=*this;
+ i.m_velocity*=x;
+ i.m_drift*=x;
+ return(i);
+ }
+ };
+ /* Body */
+ struct Body
+ {
+ Cluster* m_soft;
+ btRigidBody* m_rigid;
+ btCollisionObject* m_collisionObject;
+
+ Body() : m_soft(0),m_rigid(0),m_collisionObject(0) {}
+ Body(Cluster* p) : m_soft(p),m_rigid(0),m_collisionObject(0) {}
+ Body(btCollisionObject* colObj) : m_soft(0),m_collisionObject(colObj)
+ {
+ m_rigid = btRigidBody::upcast(m_collisionObject);
+ }
+
+ void activate() const
+ {
+ if(m_rigid) m_rigid->activate();
+ }
+ const btMatrix3x3& invWorldInertia() const
+ {
+ static const btMatrix3x3 iwi(0,0,0,0,0,0,0,0,0);
+ if(m_rigid) return(m_rigid->getInvInertiaTensorWorld());
+ if(m_soft) return(m_soft->m_invwi);
+ return(iwi);
+ }
+ btScalar invMass() const
+ {
+ if(m_rigid) return(m_rigid->getInvMass());
+ if(m_soft) return(m_soft->m_imass);
+ return(0);
+ }
+ const btTransform& xform() const
+ {
+ static const btTransform identity=btTransform::getIdentity();
+ if(m_collisionObject) return(m_collisionObject->getInterpolationWorldTransform());
+ if(m_soft) return(m_soft->m_framexform);
+ return(identity);
+ }
+ btVector3 linearVelocity() const
+ {
+ if(m_rigid) return(m_rigid->getLinearVelocity());
+ if(m_soft) return(m_soft->m_lv);
+ return(btVector3(0,0,0));
+ }
+ btVector3 angularVelocity(const btVector3& rpos) const
+ {
+ if(m_rigid) return(btCross(m_rigid->getAngularVelocity(),rpos));
+ if(m_soft) return(btCross(m_soft->m_av,rpos));
+ return(btVector3(0,0,0));
+ }
+ btVector3 angularVelocity() const
+ {
+ if(m_rigid) return(m_rigid->getAngularVelocity());
+ if(m_soft) return(m_soft->m_av);
+ return(btVector3(0,0,0));
+ }
+ btVector3 velocity(const btVector3& rpos) const
+ {
+ return(linearVelocity()+angularVelocity(rpos));
+ }
+ void applyVImpulse(const btVector3& impulse,const btVector3& rpos) const
+ {
+ if(m_rigid) m_rigid->applyImpulse(impulse,rpos);
+ if(m_soft) btSoftBody::clusterVImpulse(m_soft,rpos,impulse);
+ }
+ void applyDImpulse(const btVector3& impulse,const btVector3& rpos) const
+ {
+ if(m_rigid) m_rigid->applyImpulse(impulse,rpos);
+ if(m_soft) btSoftBody::clusterDImpulse(m_soft,rpos,impulse);
+ }
+ void applyImpulse(const Impulse& impulse,const btVector3& rpos) const
+ {
+ if(impulse.m_asVelocity)
+ {
+// printf("impulse.m_velocity = %f,%f,%f\n",impulse.m_velocity.getX(),impulse.m_velocity.getY(),impulse.m_velocity.getZ());
+ applyVImpulse(impulse.m_velocity,rpos);
+ }
+ if(impulse.m_asDrift)
+ {
+// printf("impulse.m_drift = %f,%f,%f\n",impulse.m_drift.getX(),impulse.m_drift.getY(),impulse.m_drift.getZ());
+ applyDImpulse(impulse.m_drift,rpos);
+ }
+ }
+ void applyVAImpulse(const btVector3& impulse) const
+ {
+ if(m_rigid) m_rigid->applyTorqueImpulse(impulse);
+ if(m_soft) btSoftBody::clusterVAImpulse(m_soft,impulse);
+ }
+ void applyDAImpulse(const btVector3& impulse) const
+ {
+ if(m_rigid) m_rigid->applyTorqueImpulse(impulse);
+ if(m_soft) btSoftBody::clusterDAImpulse(m_soft,impulse);
+ }
+ void applyAImpulse(const Impulse& impulse) const
+ {
+ if(impulse.m_asVelocity) applyVAImpulse(impulse.m_velocity);
+ if(impulse.m_asDrift) applyDAImpulse(impulse.m_drift);
+ }
+ void applyDCImpulse(const btVector3& impulse) const
+ {
+ if(m_rigid) m_rigid->applyCentralImpulse(impulse);
+ if(m_soft) btSoftBody::clusterDCImpulse(m_soft,impulse);
+ }
+ };
+ /* Joint */
+ struct Joint
+ {
+ struct eType { enum _ {
+ Linear,
+ Angular,
+ Contact
+ };};
+ struct Specs
+ {
+ Specs() : erp(1),cfm(1),split(1) {}
+ btScalar erp;
+ btScalar cfm;
+ btScalar split;
+ };
+ Body m_bodies[2];
+ btVector3 m_refs[2];
+ btScalar m_cfm;
+ btScalar m_erp;
+ btScalar m_split;
+ btVector3 m_drift;
+ btVector3 m_sdrift;
+ btMatrix3x3 m_massmatrix;
+ bool m_delete;
+ virtual ~Joint() {}
+ Joint() : m_delete(false) {}
+ virtual void Prepare(btScalar dt,int iterations);
+ virtual void Solve(btScalar dt,btScalar sor)=0;
+ virtual void Terminate(btScalar dt)=0;
+ virtual eType::_ Type() const=0;
+ };
+ /* LJoint */
+ struct LJoint : Joint
+ {
+ struct Specs : Joint::Specs
+ {
+ btVector3 position;
+ };
+ btVector3 m_rpos[2];
+ void Prepare(btScalar dt,int iterations);
+ void Solve(btScalar dt,btScalar sor);
+ void Terminate(btScalar dt);
+ eType::_ Type() const { return(eType::Linear); }
+ };
+ /* AJoint */
+ struct AJoint : Joint
+ {
+ struct IControl
+ {
+ virtual void Prepare(AJoint*) {}
+ virtual btScalar Speed(AJoint*,btScalar current) { return(current); }
+ static IControl* Default() { static IControl def;return(&def); }
+ };
+ struct Specs : Joint::Specs
+ {
+ Specs() : icontrol(IControl::Default()) {}
+ btVector3 axis;
+ IControl* icontrol;
+ };
+ btVector3 m_axis[2];
+ IControl* m_icontrol;
+ void Prepare(btScalar dt,int iterations);
+ void Solve(btScalar dt,btScalar sor);
+ void Terminate(btScalar dt);
+ eType::_ Type() const { return(eType::Angular); }
+ };
+ /* CJoint */
+ struct CJoint : Joint
+ {
+ int m_life;
+ int m_maxlife;
+ btVector3 m_rpos[2];
+ btVector3 m_normal;
+ btScalar m_friction;
+ void Prepare(btScalar dt,int iterations);
+ void Solve(btScalar dt,btScalar sor);
+ void Terminate(btScalar dt);
+ eType::_ Type() const { return(eType::Contact); }
+ };
+ /* Config */
+ struct Config
+ {
+ eAeroModel::_ aeromodel; // Aerodynamic model (default: V_Point)
+ btScalar kVCF; // Velocities correction factor (Baumgarte)
+ btScalar kDP; // Damping coefficient [0,1]
+ btScalar kDG; // Drag coefficient [0,+inf]
+ btScalar kLF; // Lift coefficient [0,+inf]
+ btScalar kPR; // Pressure coefficient [-inf,+inf]
+ btScalar kVC; // Volume conversation coefficient [0,+inf]
+ btScalar kDF; // Dynamic friction coefficient [0,1]
+ btScalar kMT; // Pose matching coefficient [0,1]
+ btScalar kCHR; // Rigid contacts hardness [0,1]
+ btScalar kKHR; // Kinetic contacts hardness [0,1]
+ btScalar kSHR; // Soft contacts hardness [0,1]
+ btScalar kAHR; // Anchors hardness [0,1]
+ btScalar kSRHR_CL; // Soft vs rigid hardness [0,1] (cluster only)
+ btScalar kSKHR_CL; // Soft vs kinetic hardness [0,1] (cluster only)
+ btScalar kSSHR_CL; // Soft vs soft hardness [0,1] (cluster only)
+ btScalar kSR_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only)
+ btScalar kSK_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only)
+ btScalar kSS_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only)
+ btScalar maxvolume; // Maximum volume ratio for pose
+ btScalar timescale; // Time scale
+ int viterations; // Velocities solver iterations
+ int piterations; // Positions solver iterations
+ int diterations; // Drift solver iterations
+ int citerations; // Cluster solver iterations
+ int collisions; // Collisions flags
+ tVSolverArray m_vsequence; // Velocity solvers sequence
+ tPSolverArray m_psequence; // Position solvers sequence
+ tPSolverArray m_dsequence; // Drift solvers sequence
+ };
+ /* SolverState */
+ struct SolverState
+ {
+ btScalar sdt; // dt*timescale
+ btScalar isdt; // 1/sdt
+ btScalar velmrg; // velocity margin
+ btScalar radmrg; // radial margin
+ btScalar updmrg; // Update margin
+ };
+ /// RayFromToCaster takes a ray from, ray to (instead of direction!)
+ struct RayFromToCaster : btDbvt::ICollide
+ {
+ btVector3 m_rayFrom;
+ btVector3 m_rayTo;
+ btVector3 m_rayNormalizedDirection;
+ btScalar m_mint;
+ Face* m_face;
+ int m_tests;
+ RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt);
+ void Process(const btDbvtNode* leaf);
+
+ static inline btScalar rayFromToTriangle(const btVector3& rayFrom,
+ const btVector3& rayTo,
+ const btVector3& rayNormalizedDirection,
+ const btVector3& a,
+ const btVector3& b,
+ const btVector3& c,
+ btScalar maxt=SIMD_INFINITY);
+ };
+
+ //
+ // Typedef's
+ //
+
+ typedef void (*psolver_t)(btSoftBody*,btScalar,btScalar);
+ typedef void (*vsolver_t)(btSoftBody*,btScalar);
+ typedef btAlignedObjectArray<Cluster*> tClusterArray;
+ typedef btAlignedObjectArray<Note> tNoteArray;
+ typedef btAlignedObjectArray<Node> tNodeArray;
+ typedef btAlignedObjectArray<btDbvtNode*> tLeafArray;
+ typedef btAlignedObjectArray<Link> tLinkArray;
+ typedef btAlignedObjectArray<Face> tFaceArray;
+ typedef btAlignedObjectArray<Tetra> tTetraArray;
+ typedef btAlignedObjectArray<Anchor> tAnchorArray;
+ typedef btAlignedObjectArray<RContact> tRContactArray;
+ typedef btAlignedObjectArray<SContact> tSContactArray;
+ typedef btAlignedObjectArray<Material*> tMaterialArray;
+ typedef btAlignedObjectArray<Joint*> tJointArray;
+ typedef btAlignedObjectArray<btSoftBody*> tSoftBodyArray;
+
+ //
+ // Fields
+ //
+
+ Config m_cfg; // Configuration
+ SolverState m_sst; // Solver state
+ Pose m_pose; // Pose
+ void* m_tag; // User data
+ btSoftBodyWorldInfo* m_worldInfo; // World info
+ tNoteArray m_notes; // Notes
+ tNodeArray m_nodes; // Nodes
+ tLinkArray m_links; // Links
+ tFaceArray m_faces; // Faces
+ tTetraArray m_tetras; // Tetras
+ tAnchorArray m_anchors; // Anchors
+ tRContactArray m_rcontacts; // Rigid contacts
+ tSContactArray m_scontacts; // Soft contacts
+ tJointArray m_joints; // Joints
+ tMaterialArray m_materials; // Materials
+ btScalar m_timeacc; // Time accumulator
+ btVector3 m_bounds[2]; // Spatial bounds
+ bool m_bUpdateRtCst; // Update runtime constants
+ btDbvt m_ndbvt; // Nodes tree
+ btDbvt m_fdbvt; // Faces tree
+ btDbvt m_cdbvt; // Clusters tree
+ tClusterArray m_clusters; // Clusters
+
+ btAlignedObjectArray<bool>m_clusterConnectivity;//cluster connectivity, for self-collision
+
+ btTransform m_initialWorldTransform;
+
+ //
+ // Api
+ //
+
+ /* ctor */
+ btSoftBody( btSoftBodyWorldInfo* worldInfo,int node_count,
+ const btVector3* x,
+ const btScalar* m);
+ /* dtor */
+ virtual ~btSoftBody();
+ /* Check for existing link */
+
+ btAlignedObjectArray<int> m_userIndexMapping;
+
+ btSoftBodyWorldInfo* getWorldInfo()
+ {
+ return m_worldInfo;
+ }
+
+ ///@todo: avoid internal softbody shape hack and move collision code to collision library
+ virtual void setCollisionShape(btCollisionShape* collisionShape)
+ {
+
+ }
+
+ bool checkLink( int node0,
+ int node1) const;
+ bool checkLink( const Node* node0,
+ const Node* node1) const;
+ /* Check for existring face */
+ bool checkFace( int node0,
+ int node1,
+ int node2) const;
+ /* Append material */
+ Material* appendMaterial();
+ /* Append note */
+ void appendNote( const char* text,
+ const btVector3& o,
+ const btVector4& c=btVector4(1,0,0,0),
+ Node* n0=0,
+ Node* n1=0,
+ Node* n2=0,
+ Node* n3=0);
+ void appendNote( const char* text,
+ const btVector3& o,
+ Node* feature);
+ void appendNote( const char* text,
+ const btVector3& o,
+ Link* feature);
+ void appendNote( const char* text,
+ const btVector3& o,
+ Face* feature);
+ /* Append node */
+ void appendNode( const btVector3& x,btScalar m);
+ /* Append link */
+ void appendLink(int model=-1,Material* mat=0);
+ void appendLink( int node0,
+ int node1,
+ Material* mat=0,
+ bool bcheckexist=false);
+ void appendLink( Node* node0,
+ Node* node1,
+ Material* mat=0,
+ bool bcheckexist=false);
+ /* Append face */
+ void appendFace(int model=-1,Material* mat=0);
+ void appendFace( int node0,
+ int node1,
+ int node2,
+ Material* mat=0);
+ void appendTetra(int model,Material* mat);
+ //
+ void appendTetra(int node0,
+ int node1,
+ int node2,
+ int node3,
+ Material* mat=0);
+
+
+ /* Append anchor */
+ void appendAnchor( int node,
+ btRigidBody* body, bool disableCollisionBetweenLinkedBodies=false);
+ /* Append linear joint */
+ void appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1);
+ void appendLinearJoint(const LJoint::Specs& specs,Body body=Body());
+ void appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body);
+ /* Append linear joint */
+ void appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1);
+ void appendAngularJoint(const AJoint::Specs& specs,Body body=Body());
+ void appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body);
+ /* Add force (or gravity) to the entire body */
+ void addForce( const btVector3& force);
+ /* Add force (or gravity) to a node of the body */
+ void addForce( const btVector3& force,
+ int node);
+ /* Add velocity to the entire body */
+ void addVelocity( const btVector3& velocity);
+
+ /* Set velocity for the entire body */
+ void setVelocity( const btVector3& velocity);
+
+ /* Add velocity to a node of the body */
+ void addVelocity( const btVector3& velocity,
+ int node);
+ /* Set mass */
+ void setMass( int node,
+ btScalar mass);
+ /* Get mass */
+ btScalar getMass( int node) const;
+ /* Get total mass */
+ btScalar getTotalMass() const;
+ /* Set total mass (weighted by previous masses) */
+ void setTotalMass( btScalar mass,
+ bool fromfaces=false);
+ /* Set total density */
+ void setTotalDensity(btScalar density);
+ /* Set volume mass (using tetrahedrons) */
+ void setVolumeMass( btScalar mass);
+ /* Set volume density (using tetrahedrons) */
+ void setVolumeDensity( btScalar density);
+ /* Transform */
+ void transform( const btTransform& trs);
+ /* Translate */
+ void translate( const btVector3& trs);
+ /* Rotate */
+ void rotate( const btQuaternion& rot);
+ /* Scale */
+ void scale( const btVector3& scl);
+ /* Set current state as pose */
+ void setPose( bool bvolume,
+ bool bframe);
+ /* Return the volume */
+ btScalar getVolume() const;
+ /* Cluster count */
+ int clusterCount() const;
+ /* Cluster center of mass */
+ static btVector3 clusterCom(const Cluster* cluster);
+ btVector3 clusterCom(int cluster) const;
+ /* Cluster velocity at rpos */
+ static btVector3 clusterVelocity(const Cluster* cluster,const btVector3& rpos);
+ /* Cluster impulse */
+ static void clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse);
+ static void clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse);
+ static void clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse);
+ static void clusterVAImpulse(Cluster* cluster,const btVector3& impulse);
+ static void clusterDAImpulse(Cluster* cluster,const btVector3& impulse);
+ static void clusterAImpulse(Cluster* cluster,const Impulse& impulse);
+ static void clusterDCImpulse(Cluster* cluster,const btVector3& impulse);
+ /* Generate bending constraints based on distance in the adjency graph */
+ int generateBendingConstraints( int distance,
+ Material* mat=0);
+ /* Randomize constraints to reduce solver bias */
+ void randomizeConstraints();
+ /* Release clusters */
+ void releaseCluster(int index);
+ void releaseClusters();
+ /* Generate clusters (K-mean) */
+ ///generateClusters with k=0 will create a convex cluster for each tetrahedron or triangle
+ ///otherwise an approximation will be used (better performance)
+ int generateClusters(int k,int maxiterations=8192);
+ /* Refine */
+ void refine(ImplicitFn* ifn,btScalar accurary,bool cut);
+ /* CutLink */
+ bool cutLink(int node0,int node1,btScalar position);
+ bool cutLink(const Node* node0,const Node* node1,btScalar position);
+
+ ///Ray casting using rayFrom and rayTo in worldspace, (not direction!)
+ bool rayTest(const btVector3& rayFrom,
+ const btVector3& rayTo,
+ sRayCast& results);
+ /* Solver presets */
+ void setSolver(eSolverPresets::_ preset);
+ /* predictMotion */
+ void predictMotion(btScalar dt);
+ /* solveConstraints */
+ void solveConstraints();
+ /* staticSolve */
+ void staticSolve(int iterations);
+ /* solveCommonConstraints */
+ static void solveCommonConstraints(btSoftBody** bodies,int count,int iterations);
+ /* solveClusters */
+ static void solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies);
+ /* integrateMotion */
+ void integrateMotion();
+ /* defaultCollisionHandlers */
+ void defaultCollisionHandler(btCollisionObject* pco);
+ void defaultCollisionHandler(btSoftBody* psb);
+
+ //
+ // Cast
+ //
+
+ static const btSoftBody* upcast(const btCollisionObject* colObj)
+ {
+ if (colObj->getInternalType()==CO_SOFT_BODY)
+ return (const btSoftBody*)colObj;
+ return 0;
+ }
+ static btSoftBody* upcast(btCollisionObject* colObj)
+ {
+ if (colObj->getInternalType()==CO_SOFT_BODY)
+ return (btSoftBody*)colObj;
+ return 0;
+ }
+
+ //
+ // ::btCollisionObject
+ //
+
+ virtual void getAabb(btVector3& aabbMin,btVector3& aabbMax) const
+ {
+ aabbMin = m_bounds[0];
+ aabbMax = m_bounds[1];
+ }
+ //
+ // Private
+ //
+ void pointersToIndices();
+ void indicesToPointers(const int* map=0);
+
+ int rayTest(const btVector3& rayFrom,const btVector3& rayTo,
+ btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const;
+ void initializeFaceTree();
+ btVector3 evaluateCom() const;
+ bool checkContact(btCollisionObject* colObj,const btVector3& x,btScalar margin,btSoftBody::sCti& cti) const;
+ void updateNormals();
+ void updateBounds();
+ void updatePose();
+ void updateConstants();
+ void initializeClusters();
+ void updateClusters();
+ void cleanupClusters();
+ void prepareClusters(int iterations);
+ void solveClusters(btScalar sor);
+ void applyClusters(bool drift);
+ void dampClusters();
+ void applyForces();
+ static void PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti);
+ static void PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti);
+ static void PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti);
+ static void PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti);
+ static void VSolve_Links(btSoftBody* psb,btScalar kst);
+ static psolver_t getSolver(ePSolver::_ solver);
+ static vsolver_t getSolver(eVSolver::_ solver);
+
+};
+
+
+
+#endif //_BT_SOFT_BODY_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,153 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H
+#define SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btDispatcher;
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+class btSoftBody;
+class btCollisionShape;
+
+#include "LinearMath/btHashMap.h"
+
+#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h" //for definition of MAX_NUM_PARTS_IN_BITS
+
+struct btTriIndex
+{
+ int m_PartIdTriangleIndex;
+ class btCollisionShape* m_childShape;
+
+ btTriIndex(int partId,int triangleIndex,btCollisionShape* shape)
+ {
+ m_PartIdTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
+ m_childShape = shape;
+ }
+
+ int getTriangleIndex() const
+ {
+ // Get only the lower bits where the triangle index is stored
+ return (m_PartIdTriangleIndex&~((~0)<<(31-MAX_NUM_PARTS_IN_BITS)));
+ }
+ int getPartId() const
+ {
+ // Get only the highest bits where the part index is stored
+ return (m_PartIdTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS));
+ }
+ int getUid() const
+ {
+ return m_PartIdTriangleIndex;
+ }
+};
+
+
+///For each triangle in the concave mesh that overlaps with the AABB of a soft body (m_softBody), processTriangle is called.
+class btSoftBodyTriangleCallback : public btTriangleCallback
+{
+ btSoftBody* m_softBody;
+ btCollisionObject* m_triBody;
+
+ btVector3 m_aabbMin;
+ btVector3 m_aabbMax ;
+
+ btManifoldResult* m_resultOut;
+
+ btDispatcher* m_dispatcher;
+ const btDispatcherInfo* m_dispatchInfoPtr;
+ btScalar m_collisionMarginTriangle;
+
+ btHashMap<btHashKey<btTriIndex>,btTriIndex> m_shapeCache;
+
+public:
+ int m_triangleCount;
+
+ // btPersistentManifold* m_manifoldPtr;
+
+ btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
+
+ void setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual ~btSoftBodyTriangleCallback();
+
+ virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
+
+ void clearCache();
+
+ SIMD_FORCE_INLINE const btVector3& getAabbMin() const
+ {
+ return m_aabbMin;
+ }
+ SIMD_FORCE_INLINE const btVector3& getAabbMax() const
+ {
+ return m_aabbMax;
+ }
+
+};
+
+
+
+
+/// btSoftBodyConcaveCollisionAlgorithm supports collision between soft body shapes and (concave) trianges meshes.
+class btSoftBodyConcaveCollisionAlgorithm : public btCollisionAlgorithm
+{
+
+ bool m_isSwapped;
+
+ btSoftBodyTriangleCallback m_btSoftBodyTriangleCallback;
+
+public:
+
+ btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
+
+ virtual ~btSoftBodyConcaveCollisionAlgorithm();
+
+ virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
+ {
+ //we don't add any manifolds
+ }
+
+ void clearCache();
+
+ struct CreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+ void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftBodyConcaveCollisionAlgorithm));
+ return new(mem) btSoftBodyConcaveCollisionAlgorithm(ci,body0,body1,false);
+ }
+ };
+
+ struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+ void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftBodyConcaveCollisionAlgorithm));
+ return new(mem) btSoftBodyConcaveCollisionAlgorithm(ci,body0,body1,true);
+ }
+ };
+
+};
+
+#endif //SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyHelpers.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyHelpers.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyHelpers.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyHelpers.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,141 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SOFT_BODY_HELPERS_H
+#define SOFT_BODY_HELPERS_H
+
+#include "btSoftBody.h"
+
+//
+// Helpers
+//
+
+/* fDrawFlags */
+struct fDrawFlags { enum _ {
+ Nodes = 0x0001,
+ Links = 0x0002,
+ Faces = 0x0004,
+ Tetras = 0x0008,
+ Normals = 0x0010,
+ Contacts = 0x0020,
+ Anchors = 0x0040,
+ Notes = 0x0080,
+ Clusters = 0x0100,
+ NodeTree = 0x0200,
+ FaceTree = 0x0400,
+ ClusterTree = 0x0800,
+ Joints = 0x1000,
+ /* presets */
+ Std = Links+Faces+Tetras+Anchors+Notes+Joints,
+ StdTetra = Std-Faces+Tetras
+};};
+
+struct btSoftBodyHelpers
+{
+ /* Draw body */
+ static void Draw( btSoftBody* psb,
+ btIDebugDraw* idraw,
+ int drawflags=fDrawFlags::Std);
+ /* Draw body infos */
+ static void DrawInfos( btSoftBody* psb,
+ btIDebugDraw* idraw,
+ bool masses,
+ bool areas,
+ bool stress);
+ /* Draw node tree */
+ static void DrawNodeTree( btSoftBody* psb,
+ btIDebugDraw* idraw,
+ int mindepth=0,
+ int maxdepth=-1);
+ /* Draw face tree */
+ static void DrawFaceTree( btSoftBody* psb,
+ btIDebugDraw* idraw,
+ int mindepth=0,
+ int maxdepth=-1);
+ /* Draw cluster tree */
+ static void DrawClusterTree(btSoftBody* psb,
+ btIDebugDraw* idraw,
+ int mindepth=0,
+ int maxdepth=-1);
+ /* Draw rigid frame */
+ static void DrawFrame( btSoftBody* psb,
+ btIDebugDraw* idraw);
+ /* Create a rope */
+ static btSoftBody* CreateRope( btSoftBodyWorldInfo& worldInfo,
+ const btVector3& from,
+ const btVector3& to,
+ int res,
+ int fixeds);
+ /* Create a patch */
+ static btSoftBody* CreatePatch(btSoftBodyWorldInfo& worldInfo,
+ const btVector3& corner00,
+ const btVector3& corner10,
+ const btVector3& corner01,
+ const btVector3& corner11,
+ int resx,
+ int resy,
+ int fixeds,
+ bool gendiags);
+ /* Create a patch with UV Texture Coordinates */
+ static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
+ const btVector3& corner00,
+ const btVector3& corner10,
+ const btVector3& corner01,
+ const btVector3& corner11,
+ int resx,
+ int resy,
+ int fixeds,
+ bool gendiags,
+ float* tex_coords=0);
+ static float CalculateUV(int resx,int resy,int ix,int iy,int id);
+ /* Create an ellipsoid */
+ static btSoftBody* CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,
+ const btVector3& center,
+ const btVector3& radius,
+ int res);
+ /* Create from trimesh */
+ static btSoftBody* CreateFromTriMesh( btSoftBodyWorldInfo& worldInfo,
+ const btScalar* vertices,
+ const int* triangles,
+ int ntriangles);
+ /* Create from convex-hull */
+ static btSoftBody* CreateFromConvexHull( btSoftBodyWorldInfo& worldInfo,
+ const btVector3* vertices,
+ int nvertices);
+
+
+ /* Export TetGen compatible .smesh file */
+ static void ExportAsSMeshFile( btSoftBody* psb,
+ const char* filename);
+ /* Create from TetGen .ele, .face, .node files */
+ static btSoftBody* CreateFromTetGenFile( btSoftBodyWorldInfo& worldInfo,
+ const char* ele,
+ const char* face,
+ const char* node,
+ bool bfacelinks,
+ bool btetralinks,
+ bool bfacesfromtetras);
+ /* Create from TetGen .ele, .face, .node data */
+ static btSoftBody* CreateFromTetGenData( btSoftBodyWorldInfo& worldInfo,
+ const char* ele,
+ const char* face,
+ const char* node,
+ bool bfacelinks,
+ bool btetralinks,
+ bool bfacesfromtetras);
+
+};
+
+#endif //SOFT_BODY_HELPERS_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyInternals.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyInternals.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyInternals.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyInternals.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,931 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///btSoftBody implementation by Nathanael Presson
+
+#ifndef _BT_SOFT_BODY_INTERNALS_H
+#define _BT_SOFT_BODY_INTERNALS_H
+
+#include "btSoftBody.h"
+
+
+#include "LinearMath/btQuickprof.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
+
+//
+// btSymMatrix
+//
+template <typename T>
+struct btSymMatrix
+{
+ btSymMatrix() : dim(0) {}
+ btSymMatrix(int n,const T& init=T()) { resize(n,init); }
+ void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); }
+ int index(int c,int r) const { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); }
+ T& operator()(int c,int r) { return(store[index(c,r)]); }
+ const T& operator()(int c,int r) const { return(store[index(c,r)]); }
+ btAlignedObjectArray<T> store;
+ int dim;
+};
+
+//
+// btSoftBodyCollisionShape
+//
+class btSoftBodyCollisionShape : public btConcaveShape
+{
+public:
+ btSoftBody* m_body;
+
+ btSoftBodyCollisionShape(btSoftBody* backptr)
+ {
+ m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
+ m_body=backptr;
+ }
+
+ virtual ~btSoftBodyCollisionShape()
+ {
+
+ }
+
+ void processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const
+ {
+ //not yet
+ btAssert(0);
+ }
+
+ ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
+ virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+ {
+ /* t should be identity, but better be safe than...fast? */
+ const btVector3 mins=m_body->m_bounds[0];
+ const btVector3 maxs=m_body->m_bounds[1];
+ const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()),
+ t*btVector3(maxs.x(),mins.y(),mins.z()),
+ t*btVector3(maxs.x(),maxs.y(),mins.z()),
+ t*btVector3(mins.x(),maxs.y(),mins.z()),
+ t*btVector3(mins.x(),mins.y(),maxs.z()),
+ t*btVector3(maxs.x(),mins.y(),maxs.z()),
+ t*btVector3(maxs.x(),maxs.y(),maxs.z()),
+ t*btVector3(mins.x(),maxs.y(),maxs.z())};
+ aabbMin=aabbMax=crns[0];
+ for(int i=1;i<8;++i)
+ {
+ aabbMin.setMin(crns[i]);
+ aabbMax.setMax(crns[i]);
+ }
+ }
+
+
+ virtual void setLocalScaling(const btVector3& /*scaling*/)
+ {
+ ///na
+ }
+ virtual const btVector3& getLocalScaling() const
+ {
+ static const btVector3 dummy(1,1,1);
+ return dummy;
+ }
+ virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const
+ {
+ ///not yet
+ btAssert(0);
+ }
+ virtual const char* getName()const
+ {
+ return "SoftBody";
+ }
+
+};
+
+//
+// btSoftClusterCollisionShape
+//
+class btSoftClusterCollisionShape : public btConvexInternalShape
+{
+public:
+ const btSoftBody::Cluster* m_cluster;
+
+ btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
+
+
+ virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
+ {
+ btSoftBody::Node* const * n=&m_cluster->m_nodes[0];
+ btScalar d=btDot(vec,n[0]->m_x);
+ int j=0;
+ for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i)
+ {
+ const btScalar k=btDot(vec,n[i]->m_x);
+ if(k>d) { d=k;j=i; }
+ }
+ return(n[j]->m_x);
+ }
+ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+ {
+ return(localGetSupportingVertex(vec));
+ }
+ //notice that the vectors should be unit length
+ virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+ {}
+
+
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const
+ {}
+
+ virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+ {}
+
+ virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
+
+ //debugging
+ virtual const char* getName()const {return "SOFTCLUSTER";}
+
+ virtual void setMargin(btScalar margin)
+ {
+ btConvexInternalShape::setMargin(margin);
+ }
+ virtual btScalar getMargin() const
+ {
+ return getMargin();
+ }
+};
+
+//
+// Inline's
+//
+
+//
+template <typename T>
+static inline void ZeroInitialize(T& value)
+{
+ static const T zerodummy;
+ value=zerodummy;
+}
+//
+template <typename T>
+static inline bool CompLess(const T& a,const T& b)
+{ return(a<b); }
+//
+template <typename T>
+static inline bool CompGreater(const T& a,const T& b)
+{ return(a>b); }
+//
+template <typename T>
+static inline T Lerp(const T& a,const T& b,btScalar t)
+{ return(a+(b-a)*t); }
+//
+template <typename T>
+static inline T InvLerp(const T& a,const T& b,btScalar t)
+{ return((b+a*t-b*t)/(a*b)); }
+//
+static inline btMatrix3x3 Lerp( const btMatrix3x3& a,
+ const btMatrix3x3& b,
+ btScalar t)
+{
+ btMatrix3x3 r;
+ r[0]=Lerp(a[0],b[0],t);
+ r[1]=Lerp(a[1],b[1],t);
+ r[2]=Lerp(a[2],b[2],t);
+ return(r);
+}
+//
+static inline btVector3 Clamp(const btVector3& v,btScalar maxlength)
+{
+ const btScalar sql=v.length2();
+ if(sql>(maxlength*maxlength))
+ return((v*maxlength)/btSqrt(sql));
+ else
+ return(v);
+}
+//
+template <typename T>
+static inline T Clamp(const T& x,const T& l,const T& h)
+{ return(x<l?l:x>h?h:x); }
+//
+template <typename T>
+static inline T Sq(const T& x)
+{ return(x*x); }
+//
+template <typename T>
+static inline T Cube(const T& x)
+{ return(x*x*x); }
+//
+template <typename T>
+static inline T Sign(const T& x)
+{ return((T)(x<0?-1:+1)); }
+//
+template <typename T>
+static inline bool SameSign(const T& x,const T& y)
+{ return((x*y)>0); }
+//
+static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y)
+{
+ const btVector3 d=x-y;
+ return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2]));
+}
+//
+static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s)
+{
+ const btScalar xx=a.x()*a.x();
+ const btScalar yy=a.y()*a.y();
+ const btScalar zz=a.z()*a.z();
+ const btScalar xy=a.x()*a.y();
+ const btScalar yz=a.y()*a.z();
+ const btScalar zx=a.z()*a.x();
+ btMatrix3x3 m;
+ m[0]=btVector3(1-xx+xx*s,xy*s-xy,zx*s-zx);
+ m[1]=btVector3(xy*s-xy,1-yy+yy*s,yz*s-yz);
+ m[2]=btVector3(zx*s-zx,yz*s-yz,1-zz+zz*s);
+ return(m);
+}
+//
+static inline btMatrix3x3 Cross(const btVector3& v)
+{
+ btMatrix3x3 m;
+ m[0]=btVector3(0,-v.z(),+v.y());
+ m[1]=btVector3(+v.z(),0,-v.x());
+ m[2]=btVector3(-v.y(),+v.x(),0);
+ return(m);
+}
+//
+static inline btMatrix3x3 Diagonal(btScalar x)
+{
+ btMatrix3x3 m;
+ m[0]=btVector3(x,0,0);
+ m[1]=btVector3(0,x,0);
+ m[2]=btVector3(0,0,x);
+ return(m);
+}
+//
+static inline btMatrix3x3 Add(const btMatrix3x3& a,
+ const btMatrix3x3& b)
+{
+ btMatrix3x3 r;
+ for(int i=0;i<3;++i) r[i]=a[i]+b[i];
+ return(r);
+}
+//
+static inline btMatrix3x3 Sub(const btMatrix3x3& a,
+ const btMatrix3x3& b)
+{
+ btMatrix3x3 r;
+ for(int i=0;i<3;++i) r[i]=a[i]-b[i];
+ return(r);
+}
+//
+static inline btMatrix3x3 Mul(const btMatrix3x3& a,
+ btScalar b)
+{
+ btMatrix3x3 r;
+ for(int i=0;i<3;++i) r[i]=a[i]*b;
+ return(r);
+}
+//
+static inline void Orthogonalize(btMatrix3x3& m)
+{
+ m[2]=btCross(m[0],m[1]).normalized();
+ m[1]=btCross(m[2],m[0]).normalized();
+ m[0]=btCross(m[1],m[2]).normalized();
+}
+//
+static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r)
+{
+ const btMatrix3x3 cr=Cross(r);
+ return(Sub(Diagonal(im),cr*iwi*cr));
+}
+
+//
+static inline btMatrix3x3 ImpulseMatrix( btScalar dt,
+ btScalar ima,
+ btScalar imb,
+ const btMatrix3x3& iwi,
+ const btVector3& r)
+{
+ return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse());
+}
+
+//
+static inline btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra,
+ btScalar imb,const btMatrix3x3& iib,const btVector3& rb)
+{
+ return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse());
+}
+
+//
+static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia,
+ const btMatrix3x3& iib)
+{
+ return(Add(iia,iib).inverse());
+}
+
+//
+static inline btVector3 ProjectOnAxis( const btVector3& v,
+ const btVector3& a)
+{
+ return(a*btDot(v,a));
+}
+//
+static inline btVector3 ProjectOnPlane( const btVector3& v,
+ const btVector3& a)
+{
+ return(v-ProjectOnAxis(v,a));
+}
+
+//
+static inline void ProjectOrigin( const btVector3& a,
+ const btVector3& b,
+ btVector3& prj,
+ btScalar& sqd)
+{
+ const btVector3 d=b-a;
+ const btScalar m2=d.length2();
+ if(m2>SIMD_EPSILON)
+ {
+ const btScalar t=Clamp<btScalar>(-btDot(a,d)/m2,0,1);
+ const btVector3 p=a+d*t;
+ const btScalar l2=p.length2();
+ if(l2<sqd)
+ {
+ prj=p;
+ sqd=l2;
+ }
+ }
+}
+//
+static inline void ProjectOrigin( const btVector3& a,
+ const btVector3& b,
+ const btVector3& c,
+ btVector3& prj,
+ btScalar& sqd)
+{
+ const btVector3& q=btCross(b-a,c-a);
+ const btScalar m2=q.length2();
+ if(m2>SIMD_EPSILON)
+ {
+ const btVector3 n=q/btSqrt(m2);
+ const btScalar k=btDot(a,n);
+ const btScalar k2=k*k;
+ if(k2<sqd)
+ {
+ const btVector3 p=n*k;
+ if( (btDot(btCross(a-p,b-p),q)>0)&&
+ (btDot(btCross(b-p,c-p),q)>0)&&
+ (btDot(btCross(c-p,a-p),q)>0))
+ {
+ prj=p;
+ sqd=k2;
+ }
+ else
+ {
+ ProjectOrigin(a,b,prj,sqd);
+ ProjectOrigin(b,c,prj,sqd);
+ ProjectOrigin(c,a,prj,sqd);
+ }
+ }
+ }
+}
+
+//
+template <typename T>
+static inline T BaryEval( const T& a,
+ const T& b,
+ const T& c,
+ const btVector3& coord)
+{
+ return(a*coord.x()+b*coord.y()+c*coord.z());
+}
+//
+static inline btVector3 BaryCoord( const btVector3& a,
+ const btVector3& b,
+ const btVector3& c,
+ const btVector3& p)
+{
+ const btScalar w[]={ btCross(a-p,b-p).length(),
+ btCross(b-p,c-p).length(),
+ btCross(c-p,a-p).length()};
+ const btScalar isum=1/(w[0]+w[1]+w[2]);
+ return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum));
+}
+
+//
+static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn,
+ const btVector3& a,
+ const btVector3& b,
+ const btScalar accuracy,
+ const int maxiterations=256)
+{
+ btScalar span[2]={0,1};
+ btScalar values[2]={fn->Eval(a),fn->Eval(b)};
+ if(values[0]>values[1])
+ {
+ btSwap(span[0],span[1]);
+ btSwap(values[0],values[1]);
+ }
+ if(values[0]>-accuracy) return(-1);
+ if(values[1]<+accuracy) return(-1);
+ for(int i=0;i<maxiterations;++i)
+ {
+ const btScalar t=Lerp(span[0],span[1],values[0]/(values[0]-values[1]));
+ const btScalar v=fn->Eval(Lerp(a,b,t));
+ if((t<=0)||(t>=1)) break;
+ if(btFabs(v)<accuracy) return(t);
+ if(v<0)
+ { span[0]=t;values[0]=v; }
+ else
+ { span[1]=t;values[1]=v; }
+ }
+ return(-1);
+}
+
+//
+static inline btVector3 NormalizeAny(const btVector3& v)
+{
+ const btScalar l=v.length();
+ if(l>SIMD_EPSILON)
+ return(v/l);
+ else
+ return(btVector3(0,0,0));
+}
+
+//
+static inline btDbvtVolume VolumeOf( const btSoftBody::Face& f,
+ btScalar margin)
+{
+ const btVector3* pts[]={ &f.m_n[0]->m_x,
+ &f.m_n[1]->m_x,
+ &f.m_n[2]->m_x};
+ btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3);
+ vol.Expand(btVector3(margin,margin,margin));
+ return(vol);
+}
+
+//
+static inline btVector3 CenterOf( const btSoftBody::Face& f)
+{
+ return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3);
+}
+
+//
+static inline btScalar AreaOf( const btVector3& x0,
+ const btVector3& x1,
+ const btVector3& x2)
+{
+ const btVector3 a=x1-x0;
+ const btVector3 b=x2-x0;
+ const btVector3 cr=btCross(a,b);
+ const btScalar area=cr.length();
+ return(area);
+}
+
+//
+static inline btScalar VolumeOf( const btVector3& x0,
+ const btVector3& x1,
+ const btVector3& x2,
+ const btVector3& x3)
+{
+ const btVector3 a=x1-x0;
+ const btVector3 b=x2-x0;
+ const btVector3 c=x3-x0;
+ return(btDot(a,btCross(b,c)));
+}
+
+//
+static void EvaluateMedium( const btSoftBodyWorldInfo* wfi,
+ const btVector3& x,
+ btSoftBody::sMedium& medium)
+{
+ medium.m_velocity = btVector3(0,0,0);
+ medium.m_pressure = 0;
+ medium.m_density = wfi->air_density;
+ if(wfi->water_density>0)
+ {
+ const btScalar depth=-(btDot(x,wfi->water_normal)+wfi->water_offset);
+ if(depth>0)
+ {
+ medium.m_density = wfi->water_density;
+ medium.m_pressure = depth*wfi->water_density*wfi->m_gravity.length();
+ }
+ }
+}
+
+//
+static inline void ApplyClampedForce( btSoftBody::Node& n,
+ const btVector3& f,
+ btScalar dt)
+{
+ const btScalar dtim=dt*n.m_im;
+ if((f*dtim).length2()>n.m_v.length2())
+ {/* Clamp */
+ n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim;
+ }
+ else
+ {/* Apply */
+ n.m_f+=f;
+ }
+}
+
+//
+static inline int MatchEdge( const btSoftBody::Node* a,
+ const btSoftBody::Node* b,
+ const btSoftBody::Node* ma,
+ const btSoftBody::Node* mb)
+{
+ if((a==ma)&&(b==mb)) return(0);
+ if((a==mb)&&(b==ma)) return(1);
+ return(-1);
+}
+
+//
+// btEigen : Extract eigen system,
+// straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
+// outputs are NOT sorted.
+//
+struct btEigen
+{
+ static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0)
+ {
+ static const int maxiterations=16;
+ static const btScalar accuracy=(btScalar)0.0001;
+ btMatrix3x3& v=*vectors;
+ int iterations=0;
+ vectors->setIdentity();
+ do {
+ int p=0,q=1;
+ if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; }
+ if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; }
+ if(btFabs(a[p][q])>accuracy)
+ {
+ const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]);
+ const btScalar z=btFabs(w);
+ const btScalar t=w/(z*(btSqrt(1+w*w)+z));
+ if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */
+ {
+ const btScalar c=1/btSqrt(t*t+1);
+ const btScalar s=c*t;
+ mulPQ(a,c,s,p,q);
+ mulTPQ(a,c,s,p,q);
+ mulPQ(v,c,s,p,q);
+ } else break;
+ } else break;
+ } while((++iterations)<maxiterations);
+ if(values)
+ {
+ *values=btVector3(a[0][0],a[1][1],a[2][2]);
+ }
+ return(iterations);
+ }
+private:
+ static inline void mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
+ {
+ const btScalar m[2][3]={ {a[p][0],a[p][1],a[p][2]},
+ {a[q][0],a[q][1],a[q][2]}};
+ int i;
+
+ for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i];
+ for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i];
+ }
+ static inline void mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
+ {
+ const btScalar m[2][3]={ {a[0][p],a[1][p],a[2][p]},
+ {a[0][q],a[1][q],a[2][q]}};
+ int i;
+
+ for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i];
+ for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i];
+ }
+};
+
+//
+// Polar decomposition,
+// "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
+//
+static inline int PolarDecompose( const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s)
+{
+ static const btScalar half=(btScalar)0.5;
+ static const btScalar accuracy=(btScalar)0.0001;
+ static const int maxiterations=16;
+ int i=0;
+ btScalar det=0;
+ q = Mul(m,1/btVector3(m[0][0],m[1][1],m[2][2]).length());
+ det = q.determinant();
+ if(!btFuzzyZero(det))
+ {
+ for(;i<maxiterations;++i)
+ {
+ q=Mul(Add(q,Mul(q.adjoint(),1/det).transpose()),half);
+ const btScalar ndet=q.determinant();
+ if(Sq(ndet-det)>accuracy) det=ndet; else break;
+ }
+ /* Final orthogonalization */
+ Orthogonalize(q);
+ /* Compute 'S' */
+ s=q.transpose()*m;
+ }
+ else
+ {
+ q.setIdentity();
+ s.setIdentity();
+ }
+ return(i);
+}
+
+//
+// btSoftColliders
+//
+struct btSoftColliders
+{
+ //
+ // ClusterBase
+ //
+ struct ClusterBase : btDbvt::ICollide
+ {
+ btScalar erp;
+ btScalar idt;
+ btScalar m_margin;
+ btScalar friction;
+ btScalar threshold;
+ ClusterBase()
+ {
+ erp =(btScalar)1;
+ idt =0;
+ m_margin =0;
+ friction =0;
+ threshold =(btScalar)0;
+ }
+ bool SolveContact( const btGjkEpaSolver2::sResults& res,
+ btSoftBody::Body ba,btSoftBody::Body bb,
+ btSoftBody::CJoint& joint)
+ {
+ if(res.distance<m_margin)
+ {
+ btVector3 norm = res.normal;
+ norm.normalize();//is it necessary?
+
+ const btVector3 ra=res.witnesses[0]-ba.xform().getOrigin();
+ const btVector3 rb=res.witnesses[1]-bb.xform().getOrigin();
+ const btVector3 va=ba.velocity(ra);
+ const btVector3 vb=bb.velocity(rb);
+ const btVector3 vrel=va-vb;
+ const btScalar rvac=btDot(vrel,norm);
+ btScalar depth=res.distance-m_margin;
+
+// printf("depth=%f\n",depth);
+ const btVector3 iv=norm*rvac;
+ const btVector3 fv=vrel-iv;
+ joint.m_bodies[0] = ba;
+ joint.m_bodies[1] = bb;
+ joint.m_refs[0] = ra*ba.xform().getBasis();
+ joint.m_refs[1] = rb*bb.xform().getBasis();
+ joint.m_rpos[0] = ra;
+ joint.m_rpos[1] = rb;
+ joint.m_cfm = 1;
+ joint.m_erp = 1;
+ joint.m_life = 0;
+ joint.m_maxlife = 0;
+ joint.m_split = 1;
+
+ joint.m_drift = depth*norm;
+
+ joint.m_normal = norm;
+// printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ());
+ joint.m_delete = false;
+ joint.m_friction = fv.length2()<(-rvac*friction)?1:friction;
+ joint.m_massmatrix = ImpulseMatrix( ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0],
+ bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]);
+
+ return(true);
+ }
+ return(false);
+ }
+ };
+ //
+ // CollideCL_RS
+ //
+ struct CollideCL_RS : ClusterBase
+ {
+ btSoftBody* psb;
+
+ btCollisionObject* m_colObj;
+ void Process(const btDbvtNode* leaf)
+ {
+ btSoftBody::Cluster* cluster=(btSoftBody::Cluster*)leaf->data;
+ btSoftClusterCollisionShape cshape(cluster);
+
+ const btConvexShape* rshape=(const btConvexShape*)m_colObj->getCollisionShape();
+
+ ///don't collide an anchored cluster with a static/kinematic object
+ if(m_colObj->isStaticOrKinematicObject() && cluster->m_containsAnchor)
+ return;
+
+ btGjkEpaSolver2::sResults res;
+ if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(),
+ rshape,m_colObj->getInterpolationWorldTransform(),
+ btVector3(1,0,0),res))
+ {
+ btSoftBody::CJoint joint;
+ if(SolveContact(res,cluster,m_colObj,joint))//prb,joint))
+ {
+ btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
+ *pj=joint;psb->m_joints.push_back(pj);
+ if(m_colObj->isStaticOrKinematicObject())
+ {
+ pj->m_erp *= psb->m_cfg.kSKHR_CL;
+ pj->m_split *= psb->m_cfg.kSK_SPLT_CL;
+ }
+ else
+ {
+ pj->m_erp *= psb->m_cfg.kSRHR_CL;
+ pj->m_split *= psb->m_cfg.kSR_SPLT_CL;
+ }
+ }
+ }
+ }
+ void Process(btSoftBody* ps,btCollisionObject* colOb)
+ {
+ psb = ps;
+ m_colObj = colOb;
+ idt = ps->m_sst.isdt;
+ m_margin = m_colObj->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin();
+ ///Bullet rigid body uses multiply instead of minimum to determine combined friction. Some customization would be useful.
+ friction = btMin(psb->m_cfg.kDF,m_colObj->getFriction());
+ btVector3 mins;
+ btVector3 maxs;
+
+ ATTRIBUTE_ALIGNED16(btDbvtVolume) volume;
+ colOb->getCollisionShape()->getAabb(colOb->getInterpolationWorldTransform(),mins,maxs);
+ volume=btDbvtVolume::FromMM(mins,maxs);
+ volume.Expand(btVector3(1,1,1)*m_margin);
+ ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root,volume,*this);
+ }
+ };
+ //
+ // CollideCL_SS
+ //
+ struct CollideCL_SS : ClusterBase
+ {
+ btSoftBody* bodies[2];
+ void Process(const btDbvtNode* la,const btDbvtNode* lb)
+ {
+ btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data;
+ btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data;
+
+
+ bool connected=false;
+ if ((bodies[0]==bodies[1])&&(bodies[0]->m_clusterConnectivity.size()))
+ {
+ connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex+bodies[0]->m_clusters.size()*clb->m_clusterIndex];
+ }
+
+ if (!connected)
+ {
+ btSoftClusterCollisionShape csa(cla);
+ btSoftClusterCollisionShape csb(clb);
+ btGjkEpaSolver2::sResults res;
+ if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(),
+ &csb,btTransform::getIdentity(),
+ cla->m_com-clb->m_com,res))
+ {
+ btSoftBody::CJoint joint;
+ if(SolveContact(res,cla,clb,joint))
+ {
+ btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
+ *pj=joint;bodies[0]->m_joints.push_back(pj);
+ pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL);
+ pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2;
+ }
+ }
+ } else
+ {
+ static int count=0;
+ count++;
+ //printf("count=%d\n",count);
+
+ }
+ }
+ void Process(btSoftBody* psa,btSoftBody* psb)
+ {
+ idt = psa->m_sst.isdt;
+ //m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
+ m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin());
+ friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF);
+ bodies[0] = psa;
+ bodies[1] = psb;
+ psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
+ }
+ };
+ //
+ // CollideSDF_RS
+ //
+ struct CollideSDF_RS : btDbvt::ICollide
+ {
+ void Process(const btDbvtNode* leaf)
+ {
+ btSoftBody::Node* node=(btSoftBody::Node*)leaf->data;
+ DoNode(*node);
+ }
+ void DoNode(btSoftBody::Node& n) const
+ {
+ const btScalar m=n.m_im>0?dynmargin:stamargin;
+ btSoftBody::RContact c;
+ if( (!n.m_battach)&&
+ psb->checkContact(m_colObj1,n.m_x,m,c.m_cti))
+ {
+ const btScalar ima=n.m_im;
+ const btScalar imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f;
+ const btScalar ms=ima+imb;
+ if(ms>0)
+ {
+ const btTransform& wtr=m_rigidBody?m_rigidBody->getInterpolationWorldTransform() : m_colObj1->getWorldTransform();
+ static const btMatrix3x3 iwiStatic(0,0,0,0,0,0,0,0,0);
+ const btMatrix3x3& iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
+ const btVector3 ra=n.m_x-wtr.getOrigin();
+ const btVector3 va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt : btVector3(0,0,0);
+ const btVector3 vb=n.m_x-n.m_q;
+ const btVector3 vr=vb-va;
+ const btScalar dn=btDot(vr,c.m_cti.m_normal);
+ const btVector3 fv=vr-c.m_cti.m_normal*dn;
+ const btScalar fc=psb->m_cfg.kDF*m_colObj1->getFriction();
+ c.m_node = &n;
+ c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra);
+ c.m_c1 = ra;
+ c.m_c2 = ima*psb->m_sst.sdt;
+ c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc;
+ c.m_c4 = m_colObj1->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
+ psb->m_rcontacts.push_back(c);
+ if (m_rigidBody)
+ m_rigidBody->activate();
+ }
+ }
+ }
+ btSoftBody* psb;
+ btCollisionObject* m_colObj1;
+ btRigidBody* m_rigidBody;
+ btScalar dynmargin;
+ btScalar stamargin;
+ };
+ //
+ // CollideVF_SS
+ //
+ struct CollideVF_SS : btDbvt::ICollide
+ {
+ void Process(const btDbvtNode* lnode,
+ const btDbvtNode* lface)
+ {
+ btSoftBody::Node* node=(btSoftBody::Node*)lnode->data;
+ btSoftBody::Face* face=(btSoftBody::Face*)lface->data;
+ btVector3 o=node->m_x;
+ btVector3 p;
+ btScalar d=SIMD_INFINITY;
+ ProjectOrigin( face->m_n[0]->m_x-o,
+ face->m_n[1]->m_x-o,
+ face->m_n[2]->m_x-o,
+ p,d);
+ const btScalar m=mrg+(o-node->m_q).length()*2;
+ if(d<(m*m))
+ {
+ const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]};
+ const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o);
+ const btScalar ma=node->m_im;
+ btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w);
+ if( (n[0]->m_im<=0)||
+ (n[1]->m_im<=0)||
+ (n[2]->m_im<=0))
+ {
+ mb=0;
+ }
+ const btScalar ms=ma+mb;
+ if(ms>0)
+ {
+ btSoftBody::SContact c;
+ c.m_normal = p/-btSqrt(d);
+ c.m_margin = m;
+ c.m_node = node;
+ c.m_face = face;
+ c.m_weights = w;
+ c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF);
+ c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR;
+ c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR;
+ psb[0]->m_scontacts.push_back(c);
+ }
+ }
+ }
+ btSoftBody* psb[2];
+ btScalar mrg;
+ };
+};
+
+#endif //_BT_SOFT_BODY_INTERNALS_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,48 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION
+#define BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION
+
+#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h"
+
+class btVoronoiSimplexSolver;
+class btGjkEpaPenetrationDepthSolver;
+
+
+///btSoftBodyRigidBodyCollisionConfiguration add softbody interaction on top of btDefaultCollisionConfiguration
+class btSoftBodyRigidBodyCollisionConfiguration : public btDefaultCollisionConfiguration
+{
+
+ //default CreationFunctions, filling the m_doubleDispatch table
+ btCollisionAlgorithmCreateFunc* m_softSoftCreateFunc;
+ btCollisionAlgorithmCreateFunc* m_softRigidConvexCreateFunc;
+ btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConvexCreateFunc;
+ btCollisionAlgorithmCreateFunc* m_softRigidConcaveCreateFunc;
+ btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConcaveCreateFunc;
+
+public:
+
+ btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo());
+
+ virtual ~btSoftBodyRigidBodyCollisionConfiguration();
+
+ ///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation
+ virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);
+
+};
+
+#endif //BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftRigidCollisionAlgorithm.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftRigidCollisionAlgorithm.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftRigidCollisionAlgorithm.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftRigidCollisionAlgorithm.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,75 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SOFT_RIGID_COLLISION_ALGORITHM_H
+#define SOFT_RIGID_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+class btPersistentManifold;
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+
+#include "LinearMath/btVector3.h"
+class btSoftBody;
+
+/// btSoftRigidCollisionAlgorithm provides collision detection between btSoftBody and btRigidBody
+class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm
+{
+ // bool m_ownManifold;
+ // btPersistentManifold* m_manifoldPtr;
+
+ btSoftBody* m_softBody;
+ btCollisionObject* m_rigidCollisionObject;
+
+ ///for rigid versus soft (instead of soft versus rigid), we use this swapped boolean
+ bool m_isSwapped;
+
+public:
+
+ btSoftRigidCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped);
+
+ virtual ~btSoftRigidCollisionAlgorithm();
+
+ virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
+ {
+ //we don't add any manifolds
+ }
+
+
+ struct CreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+ void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftRigidCollisionAlgorithm));
+ if (!m_swapped)
+ {
+ return new(mem) btSoftRigidCollisionAlgorithm(0,ci,body0,body1,false);
+ } else
+ {
+ return new(mem) btSoftRigidCollisionAlgorithm(0,ci,body0,body1,true);
+ }
+ }
+ };
+
+};
+
+#endif //SOFT_RIGID_COLLISION_ALGORITHM_H
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftRigidDynamicsWorld.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftRigidDynamicsWorld.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftRigidDynamicsWorld.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftRigidDynamicsWorld.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,85 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_SOFT_RIGID_DYNAMICS_WORLD_H
+#define BT_SOFT_RIGID_DYNAMICS_WORLD_H
+
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+#include "btSoftBody.h"
+
+typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
+
+class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld
+{
+
+ btSoftBodyArray m_softBodies;
+ int m_drawFlags;
+ bool m_drawNodeTree;
+ bool m_drawFaceTree;
+ bool m_drawClusterTree;
+ btSoftBodyWorldInfo m_sbi;
+
+protected:
+
+ virtual void predictUnconstraintMotion(btScalar timeStep);
+
+ virtual void internalSingleStepSimulation( btScalar timeStep);
+
+ void updateSoftBodies();
+
+ void solveSoftBodiesConstraints();
+
+
+public:
+
+ btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+
+ virtual ~btSoftRigidDynamicsWorld();
+
+ virtual void debugDrawWorld();
+
+ void addSoftBody(btSoftBody* body,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter);
+
+ void removeSoftBody(btSoftBody* body);
+
+ ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btDiscreteDynamicsWorld::removeCollisionObject
+ virtual void removeCollisionObject(btCollisionObject* collisionObject);
+
+ int getDrawFlags() const { return(m_drawFlags); }
+ void setDrawFlags(int f) { m_drawFlags=f; }
+
+ btSoftBodyWorldInfo& getWorldInfo()
+ {
+ return m_sbi;
+ }
+ const btSoftBodyWorldInfo& getWorldInfo() const
+ {
+ return m_sbi;
+ }
+
+
+ btSoftBodyArray& getSoftBodyArray()
+ {
+ return m_softBodies;
+ }
+
+ const btSoftBodyArray& getSoftBodyArray() const
+ {
+ return m_softBodies;
+ }
+
+};
+
+#endif //BT_SOFT_RIGID_DYNAMICS_WORLD_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftSoftCollisionAlgorithm.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftSoftCollisionAlgorithm.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftSoftCollisionAlgorithm.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftSoftCollisionAlgorithm.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,69 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SOFT_SOFT_COLLISION_ALGORITHM_H
+#define SOFT_SOFT_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+
+class btPersistentManifold;
+class btSoftBody;
+
+///collision detection between two btSoftBody shapes
+class btSoftSoftCollisionAlgorithm : public btCollisionAlgorithm
+{
+ bool m_ownManifold;
+ btPersistentManifold* m_manifoldPtr;
+
+ btSoftBody* m_softBody0;
+ btSoftBody* m_softBody1;
+
+
+public:
+ btSoftSoftCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+ : btCollisionAlgorithm(ci) {}
+
+ virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
+ {
+ if (m_manifoldPtr && m_ownManifold)
+ manifoldArray.push_back(m_manifoldPtr);
+ }
+
+ btSoftSoftCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
+
+ virtual ~btSoftSoftCollisionAlgorithm();
+
+ struct CreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+ int bbsize = sizeof(btSoftSoftCollisionAlgorithm);
+ void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize);
+ return new(ptr) btSoftSoftCollisionAlgorithm(0,ci,body0,body1);
+ }
+ };
+
+};
+
+#endif //SOFT_SOFT_COLLISION_ALGORITHM_H
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSparseSDF.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSparseSDF.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSparseSDF.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSparseSDF.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,306 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///btSparseSdf implementation by Nathanael Presson
+
+#ifndef _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_
+#define _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_
+
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
+
+// Modified Paul Hsieh hash
+template <const int DWORDLEN>
+unsigned int HsiehHash(const void* pdata)
+{
+ const unsigned short* data=(const unsigned short*)pdata;
+ unsigned hash=DWORDLEN<<2,tmp;
+ for(int i=0;i<DWORDLEN;++i)
+ {
+ hash += data[0];
+ tmp = (data[1]<<11)^hash;
+ hash = (hash<<16)^tmp;
+ data += 2;
+ hash += hash>>11;
+ }
+ hash^=hash<<3;hash+=hash>>5;
+ hash^=hash<<4;hash+=hash>>17;
+ hash^=hash<<25;hash+=hash>>6;
+ return(hash);
+}
+
+template <const int CELLSIZE>
+struct btSparseSdf
+{
+ //
+ // Inner types
+ //
+ struct IntFrac
+ {
+ int b;
+ int i;
+ btScalar f;
+ };
+ struct Cell
+ {
+ btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1];
+ int c[3];
+ int puid;
+ unsigned hash;
+ btCollisionShape* pclient;
+ Cell* next;
+ };
+ //
+ // Fields
+ //
+
+ btAlignedObjectArray<Cell*> cells;
+ btScalar voxelsz;
+ int puid;
+ int ncells;
+ int nprobes;
+ int nqueries;
+
+ //
+ // Methods
+ //
+
+ //
+ void Initialize(int hashsize=2383)
+ {
+ cells.resize(hashsize,0);
+ Reset();
+ }
+ //
+ void Reset()
+ {
+ for(int i=0,ni=cells.size();i<ni;++i)
+ {
+ Cell* pc=cells[i];
+ cells[i]=0;
+ while(pc)
+ {
+ Cell* pn=pc->next;
+ delete pc;
+ pc=pn;
+ }
+ }
+ voxelsz =0.25;
+ puid =0;
+ ncells =0;
+ nprobes =1;
+ nqueries =1;
+ }
+ //
+ void GarbageCollect(int lifetime=256)
+ {
+ const int life=puid-lifetime;
+ for(int i=0;i<cells.size();++i)
+ {
+ Cell*& root=cells[i];
+ Cell* pp=0;
+ Cell* pc=root;
+ while(pc)
+ {
+ Cell* pn=pc->next;
+ if(pc->puid<life)
+ {
+ if(pp) pp->next=pn; else root=pn;
+ delete pc;pc=pp;--ncells;
+ }
+ pp=pc;pc=pn;
+ }
+ }
+ //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries);
+ nqueries=1;
+ nprobes=1;
+ ++puid; ///@todo: Reset puid's when int range limit is reached */
+ /* else setup a priority list... */
+ }
+ //
+ int RemoveReferences(btCollisionShape* pcs)
+ {
+ int refcount=0;
+ for(int i=0;i<cells.size();++i)
+ {
+ Cell*& root=cells[i];
+ Cell* pp=0;
+ Cell* pc=root;
+ while(pc)
+ {
+ Cell* pn=pc->next;
+ if(pc->pclient==pcs)
+ {
+ if(pp) pp->next=pn; else root=pn;
+ delete pc;pc=pp;++refcount;
+ }
+ pp=pc;pc=pn;
+ }
+ }
+ return(refcount);
+ }
+ //
+ btScalar Evaluate( const btVector3& x,
+ btCollisionShape* shape,
+ btVector3& normal,
+ btScalar margin)
+ {
+ /* Lookup cell */
+ const btVector3 scx=x/voxelsz;
+ const IntFrac ix=Decompose(scx.x());
+ const IntFrac iy=Decompose(scx.y());
+ const IntFrac iz=Decompose(scx.z());
+ const unsigned h=Hash(ix.b,iy.b,iz.b,shape);
+ Cell*& root=cells[static_cast<int>(h%cells.size())];
+ Cell* c=root;
+ ++nqueries;
+ while(c)
+ {
+ ++nprobes;
+ if( (c->hash==h) &&
+ (c->c[0]==ix.b) &&
+ (c->c[1]==iy.b) &&
+ (c->c[2]==iz.b) &&
+ (c->pclient==shape))
+ { break; }
+ else
+ { c=c->next; }
+ }
+ if(!c)
+ {
+ ++nprobes;
+ ++ncells;
+ c=new Cell();
+ c->next=root;root=c;
+ c->pclient=shape;
+ c->hash=h;
+ c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b;
+ BuildCell(*c);
+ }
+ c->puid=puid;
+ /* Extract infos */
+ const int o[]={ ix.i,iy.i,iz.i};
+ const btScalar d[]={ c->d[o[0]+0][o[1]+0][o[2]+0],
+ c->d[o[0]+1][o[1]+0][o[2]+0],
+ c->d[o[0]+1][o[1]+1][o[2]+0],
+ c->d[o[0]+0][o[1]+1][o[2]+0],
+ c->d[o[0]+0][o[1]+0][o[2]+1],
+ c->d[o[0]+1][o[1]+0][o[2]+1],
+ c->d[o[0]+1][o[1]+1][o[2]+1],
+ c->d[o[0]+0][o[1]+1][o[2]+1]};
+ /* Normal */
+#if 1
+ const btScalar gx[]={ d[1]-d[0],d[2]-d[3],
+ d[5]-d[4],d[6]-d[7]};
+ const btScalar gy[]={ d[3]-d[0],d[2]-d[1],
+ d[7]-d[4],d[6]-d[5]};
+ const btScalar gz[]={ d[4]-d[0],d[5]-d[1],
+ d[7]-d[3],d[6]-d[2]};
+ normal.setX(Lerp( Lerp(gx[0],gx[1],iy.f),
+ Lerp(gx[2],gx[3],iy.f),iz.f));
+ normal.setY(Lerp( Lerp(gy[0],gy[1],ix.f),
+ Lerp(gy[2],gy[3],ix.f),iz.f));
+ normal.setZ(Lerp( Lerp(gz[0],gz[1],ix.f),
+ Lerp(gz[2],gz[3],ix.f),iy.f));
+ normal = normal.normalized();
+#else
+ normal = btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized();
+#endif
+ /* Distance */
+ const btScalar d0=Lerp(Lerp(d[0],d[1],ix.f),
+ Lerp(d[3],d[2],ix.f),iy.f);
+ const btScalar d1=Lerp(Lerp(d[4],d[5],ix.f),
+ Lerp(d[7],d[6],ix.f),iy.f);
+ return(Lerp(d0,d1,iz.f)-margin);
+ }
+ //
+ void BuildCell(Cell& c)
+ {
+ const btVector3 org=btVector3( (btScalar)c.c[0],
+ (btScalar)c.c[1],
+ (btScalar)c.c[2]) *
+ CELLSIZE*voxelsz;
+ for(int k=0;k<=CELLSIZE;++k)
+ {
+ const btScalar z=voxelsz*k+org.z();
+ for(int j=0;j<=CELLSIZE;++j)
+ {
+ const btScalar y=voxelsz*j+org.y();
+ for(int i=0;i<=CELLSIZE;++i)
+ {
+ const btScalar x=voxelsz*i+org.x();
+ c.d[i][j][k]=DistanceToShape( btVector3(x,y,z),
+ c.pclient);
+ }
+ }
+ }
+ }
+ //
+ static inline btScalar DistanceToShape(const btVector3& x,
+ btCollisionShape* shape)
+ {
+ btTransform unit;
+ unit.setIdentity();
+ if(shape->isConvex())
+ {
+ btGjkEpaSolver2::sResults res;
+ btConvexShape* csh=static_cast<btConvexShape*>(shape);
+ return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res));
+ }
+ return(0);
+ }
+ //
+ static inline IntFrac Decompose(btScalar x)
+ {
+ /* That one need a lot of improvements... */
+ /* Remove test, faster floor... */
+ IntFrac r;
+ x/=CELLSIZE;
+ const int o=x<0?(int)(-x+1):0;
+ x+=o;r.b=(int)x;
+ const btScalar k=(x-r.b)*CELLSIZE;
+ r.i=(int)k;r.f=k-r.i;r.b-=o;
+ return(r);
+ }
+ //
+ static inline btScalar Lerp(btScalar a,btScalar b,btScalar t)
+ {
+ return(a+(b-a)*t);
+ }
+
+
+
+ //
+ static inline unsigned int Hash(int x,int y,int z,btCollisionShape* shape)
+ {
+ struct btS
+ {
+ int x,y,z;
+ void* p;
+ };
+
+ btS myset;
+
+ myset.x=x;myset.y=y;myset.z=z;myset.p=shape;
+ const void* ptr = &myset;
+
+ unsigned int result = HsiehHash<sizeof(btS)/4> (ptr);
+
+
+ return result;
+ }
+};
+
+
+#endif
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btAabbUtil2.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btAabbUtil2.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btAabbUtil2.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btAabbUtil2.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,236 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef AABB_UTIL2
+#define AABB_UTIL2
+
+#include "btTransform.h"
+#include "btVector3.h"
+#include "btMinMax.h"
+
+
+
+SIMD_FORCE_INLINE void AabbExpand (btVector3& aabbMin,
+ btVector3& aabbMax,
+ const btVector3& expansionMin,
+ const btVector3& expansionMax)
+{
+ aabbMin = aabbMin + expansionMin;
+ aabbMax = aabbMax + expansionMax;
+}
+
+/// conservative test for overlap between two aabbs
+SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
+ const btVector3 &point)
+{
+ bool overlap = true;
+ overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap;
+ overlap = (aabbMin1.getZ() > point.getZ() || aabbMax1.getZ() < point.getZ()) ? false : overlap;
+ overlap = (aabbMin1.getY() > point.getY() || aabbMax1.getY() < point.getY()) ? false : overlap;
+ return overlap;
+}
+
+
+/// conservative test for overlap between two aabbs
+SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
+ const btVector3 &aabbMin2, const btVector3 &aabbMax2)
+{
+ bool overlap = true;
+ overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap;
+ overlap = (aabbMin1.getZ() > aabbMax2.getZ() || aabbMax1.getZ() < aabbMin2.getZ()) ? false : overlap;
+ overlap = (aabbMin1.getY() > aabbMax2.getY() || aabbMax1.getY() < aabbMin2.getY()) ? false : overlap;
+ return overlap;
+}
+
+/// conservative test for overlap between triangle and aabb
+SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3 *vertices,
+ const btVector3 &aabbMin, const btVector3 &aabbMax)
+{
+ const btVector3 &p1 = vertices[0];
+ const btVector3 &p2 = vertices[1];
+ const btVector3 &p3 = vertices[2];
+
+ if (btMin(btMin(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
+ if (btMax(btMax(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
+
+ if (btMin(btMin(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
+ if (btMax(btMax(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
+
+ if (btMin(btMin(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
+ if (btMax(btMax(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
+ return true;
+}
+
+
+SIMD_FORCE_INLINE int btOutcode(const btVector3& p,const btVector3& halfExtent)
+{
+ return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) |
+ (p.getX() > halfExtent.getX() ? 0x08 : 0x0) |
+ (p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |
+ (p.getY() > halfExtent.getY() ? 0x10 : 0x0) |
+ (p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |
+ (p.getZ() > halfExtent.getZ() ? 0x20 : 0x0);
+}
+
+
+
+SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom,
+ const btVector3& rayInvDirection,
+ const unsigned int raySign[3],
+ const btVector3 bounds[2],
+ btScalar& tmin,
+ btScalar lambda_min,
+ btScalar lambda_max)
+{
+ btScalar tmax, tymin, tymax, tzmin, tzmax;
+ tmin = (bounds[raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
+ tmax = (bounds[1-raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
+ tymin = (bounds[raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
+ tymax = (bounds[1-raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
+
+ if ( (tmin > tymax) || (tymin > tmax) )
+ return false;
+
+ if (tymin > tmin)
+ tmin = tymin;
+
+ if (tymax < tmax)
+ tmax = tymax;
+
+ tzmin = (bounds[raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
+ tzmax = (bounds[1-raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
+
+ if ( (tmin > tzmax) || (tzmin > tmax) )
+ return false;
+ if (tzmin > tmin)
+ tmin = tzmin;
+ if (tzmax < tmax)
+ tmax = tzmax;
+ return ( (tmin < lambda_max) && (tmax > lambda_min) );
+}
+
+SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom,
+ const btVector3& rayTo,
+ const btVector3& aabbMin,
+ const btVector3& aabbMax,
+ btScalar& param, btVector3& normal)
+{
+ btVector3 aabbHalfExtent = (aabbMax-aabbMin)* btScalar(0.5);
+ btVector3 aabbCenter = (aabbMax+aabbMin)* btScalar(0.5);
+ btVector3 source = rayFrom - aabbCenter;
+ btVector3 target = rayTo - aabbCenter;
+ int sourceOutcode = btOutcode(source,aabbHalfExtent);
+ int targetOutcode = btOutcode(target,aabbHalfExtent);
+ if ((sourceOutcode & targetOutcode) == 0x0)
+ {
+ btScalar lambda_enter = btScalar(0.0);
+ btScalar lambda_exit = param;
+ btVector3 r = target - source;
+ int i;
+ btScalar normSign = 1;
+ btVector3 hitNormal(0,0,0);
+ int bit=1;
+
+ for (int j=0;j<2;j++)
+ {
+ for (i = 0; i != 3; ++i)
+ {
+ if (sourceOutcode & bit)
+ {
+ btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
+ if (lambda_enter <= lambda)
+ {
+ lambda_enter = lambda;
+ hitNormal.setValue(0,0,0);
+ hitNormal[i] = normSign;
+ }
+ }
+ else if (targetOutcode & bit)
+ {
+ btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
+ btSetMin(lambda_exit, lambda);
+ }
+ bit<<=1;
+ }
+ normSign = btScalar(-1.);
+ }
+ if (lambda_enter <= lambda_exit)
+ {
+ param = lambda_enter;
+ normal = hitNormal;
+ return true;
+ }
+ }
+ return false;
+}
+
+
+
+SIMD_FORCE_INLINE void btTransformAabb(const btVector3& halfExtents, btScalar margin,const btTransform& t,btVector3& aabbMinOut,btVector3& aabbMaxOut)
+{
+ btVector3 halfExtentsWithMargin = halfExtents+btVector3(margin,margin,margin);
+ btMatrix3x3 abs_b = t.getBasis().absolute();
+ btVector3 center = t.getOrigin();
+ btVector3 extent = btVector3(abs_b[0].dot(halfExtentsWithMargin),
+ abs_b[1].dot(halfExtentsWithMargin),
+ abs_b[2].dot(halfExtentsWithMargin));
+ aabbMinOut = center - extent;
+ aabbMaxOut = center + extent;
+}
+
+
+SIMD_FORCE_INLINE void btTransformAabb(const btVector3& localAabbMin,const btVector3& localAabbMax, btScalar margin,const btTransform& trans,btVector3& aabbMinOut,btVector3& aabbMaxOut)
+{
+ btAssert(localAabbMin.getX() <= localAabbMax.getX());
+ btAssert(localAabbMin.getY() <= localAabbMax.getY());
+ btAssert(localAabbMin.getZ() <= localAabbMax.getZ());
+ btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin);
+ localHalfExtents+=btVector3(margin,margin,margin);
+
+ btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin);
+ btMatrix3x3 abs_b = trans.getBasis().absolute();
+ btVector3 center = trans(localCenter);
+ btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
+ abs_b[1].dot(localHalfExtents),
+ abs_b[2].dot(localHalfExtents));
+ aabbMinOut = center-extent;
+ aabbMaxOut = center+extent;
+}
+
+#define USE_BANCHLESS 1
+#ifdef USE_BANCHLESS
+ //This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
+ SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
+ {
+ return static_cast<unsigned int>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
+ & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
+ & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
+ 1, 0));
+ }
+#else
+ SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
+ {
+ bool overlap = true;
+ overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
+ overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
+ overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
+ return overlap;
+ }
+#endif //USE_BANCHLESS
+
+#endif
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btAlignedAllocator.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btAlignedAllocator.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btAlignedAllocator.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btAlignedAllocator.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,107 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_ALIGNED_ALLOCATOR
+#define BT_ALIGNED_ALLOCATOR
+
+///we probably replace this with our own aligned memory allocator
+///so we replace _aligned_malloc and _aligned_free with our own
+///that is better portable and more predictable
+
+#include "btScalar.h"
+//#define BT_DEBUG_MEMORY_ALLOCATIONS 1
+#ifdef BT_DEBUG_MEMORY_ALLOCATIONS
+
+#define btAlignedAlloc(a,b) \
+ btAlignedAllocInternal(a,b,__LINE__,__FILE__)
+
+#define btAlignedFree(ptr) \
+ btAlignedFreeInternal(ptr,__LINE__,__FILE__)
+
+void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filename);
+
+void btAlignedFreeInternal (void* ptr,int line,char* filename);
+
+#else
+ void* btAlignedAllocInternal (size_t size, int alignment);
+ void btAlignedFreeInternal (void* ptr);
+
+ #define btAlignedAlloc(size,alignment) btAlignedAllocInternal(size,alignment)
+ #define btAlignedFree(ptr) btAlignedFreeInternal(ptr)
+
+#endif
+typedef int size_type;
+
+typedef void *(btAlignedAllocFunc)(size_t size, int alignment);
+typedef void (btAlignedFreeFunc)(void *memblock);
+typedef void *(btAllocFunc)(size_t size);
+typedef void (btFreeFunc)(void *memblock);
+
+///The developer can let all Bullet memory allocations go through a custom memory allocator, using btAlignedAllocSetCustom
+void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc);
+///If the developer has already an custom aligned allocator, then btAlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it.
+void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc);
+
+
+///The btAlignedAllocator is a portable class for aligned memory allocations.
+///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using btAlignedAllocSetCustom and btAlignedAllocSetCustomAligned.
+template < typename T , unsigned Alignment >
+class btAlignedAllocator {
+
+ typedef btAlignedAllocator< T , Alignment > self_type;
+
+public:
+
+ //just going down a list:
+ btAlignedAllocator() {}
+ /*
+ btAlignedAllocator( const self_type & ) {}
+ */
+
+ template < typename Other >
+ btAlignedAllocator( const btAlignedAllocator< Other , Alignment > & ) {}
+
+ typedef const T* const_pointer;
+ typedef const T& const_reference;
+ typedef T* pointer;
+ typedef T& reference;
+ typedef T value_type;
+
+ pointer address ( reference ref ) const { return &ref; }
+ const_pointer address ( const_reference ref ) const { return &ref; }
+ pointer allocate ( size_type n , const_pointer * hint = 0 ) {
+ (void)hint;
+ return reinterpret_cast< pointer >(btAlignedAlloc( sizeof(value_type) * n , Alignment ));
+ }
+ void construct ( pointer ptr , const value_type & value ) { new (ptr) value_type( value ); }
+ void deallocate( pointer ptr ) {
+ btAlignedFree( reinterpret_cast< void * >( ptr ) );
+ }
+ void destroy ( pointer ptr ) { ptr->~value_type(); }
+
+
+ template < typename O > struct rebind {
+ typedef btAlignedAllocator< O , Alignment > other;
+ };
+ template < typename O >
+ self_type & operator=( const btAlignedAllocator< O , Alignment > & ) { return *this; }
+
+ friend bool operator==( const self_type & , const self_type & ) { return true; }
+};
+
+
+
+#endif //BT_ALIGNED_ALLOCATOR
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btAlignedObjectArray.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btAlignedObjectArray.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btAlignedObjectArray.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btAlignedObjectArray.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,442 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef BT_OBJECT_ARRAY__
+#define BT_OBJECT_ARRAY__
+
+#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE
+#include "btAlignedAllocator.h"
+
+///If the platform doesn't support placement new, you can disable BT_USE_PLACEMENT_NEW
+///then the btAlignedObjectArray doesn't support objects with virtual methods, and non-trivial constructors/destructors
+///You can enable BT_USE_MEMCPY, then swapping elements in the array will use memcpy instead of operator=
+///see discussion here: http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1231 and
+///http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1240
+
+#define BT_USE_PLACEMENT_NEW 1
+//#define BT_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in <memory.h> or <string.h> or otherwise...
+
+#ifdef BT_USE_MEMCPY
+#include <memory.h>
+#include <string.h>
+#endif //BT_USE_MEMCPY
+
+#ifdef BT_USE_PLACEMENT_NEW
+#include <new> //for placement new
+#endif //BT_USE_PLACEMENT_NEW
+
+
+///The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods
+///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data
+template <typename T>
+//template <class T>
+class btAlignedObjectArray
+{
+ btAlignedAllocator<T , 16> m_allocator;
+
+ int m_size;
+ int m_capacity;
+ T* m_data;
+ //PCK: added this line
+ bool m_ownsMemory;
+
+ protected:
+ SIMD_FORCE_INLINE int allocSize(int size)
+ {
+ return (size ? size*2 : 1);
+ }
+ SIMD_FORCE_INLINE void copy(int start,int end, T* dest) const
+ {
+ int i;
+ for (i=start;i<end;++i)
+#ifdef BT_USE_PLACEMENT_NEW
+ new (&dest[i]) T(m_data[i]);
+#else
+ dest[i] = m_data[i];
+#endif //BT_USE_PLACEMENT_NEW
+ }
+
+ SIMD_FORCE_INLINE void init()
+ {
+ //PCK: added this line
+ m_ownsMemory = true;
+ m_data = 0;
+ m_size = 0;
+ m_capacity = 0;
+ }
+ SIMD_FORCE_INLINE void destroy(int first,int last)
+ {
+ int i;
+ for (i=first; i<last;i++)
+ {
+ m_data[i].~T();
+ }
+ }
+
+ SIMD_FORCE_INLINE void* allocate(int size)
+ {
+ if (size)
+ return m_allocator.allocate(size);
+ return 0;
+ }
+
+ SIMD_FORCE_INLINE void deallocate()
+ {
+ if(m_data) {
+ //PCK: enclosed the deallocation in this block
+ if (m_ownsMemory)
+ {
+ m_allocator.deallocate(m_data);
+ }
+ m_data = 0;
+ }
+ }
+
+
+
+
+ public:
+
+ btAlignedObjectArray()
+ {
+ init();
+ }
+
+ ~btAlignedObjectArray()
+ {
+ clear();
+ }
+
+ ///Generally it is best to avoid using the copy constructor of an btAlignedObjectArray, and use a (const) reference to the array instead.
+ btAlignedObjectArray(const btAlignedObjectArray& otherArray)
+ {
+ init();
+
+ int otherSize = otherArray.size();
+ resize (otherSize);
+ otherArray.copy(0, otherSize, m_data);
+ }
+
+
+
+ /// return the number of elements in the array
+ SIMD_FORCE_INLINE int size() const
+ {
+ return m_size;
+ }
+
+ SIMD_FORCE_INLINE const T& operator[](int n) const
+ {
+ return m_data[n];
+ }
+
+ SIMD_FORCE_INLINE T& operator[](int n)
+ {
+ return m_data[n];
+ }
+
+
+ ///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
+ SIMD_FORCE_INLINE void clear()
+ {
+ destroy(0,size());
+
+ deallocate();
+
+ init();
+ }
+
+ SIMD_FORCE_INLINE void pop_back()
+ {
+ m_size--;
+ m_data[m_size].~T();
+ }
+
+ ///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
+ ///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
+ SIMD_FORCE_INLINE void resize(int newsize, const T& fillData=T())
+ {
+ int curSize = size();
+
+ if (newsize < curSize)
+ {
+ for(int i = newsize; i < curSize; i++)
+ {
+ m_data[i].~T();
+ }
+ } else
+ {
+ if (newsize > size())
+ {
+ reserve(newsize);
+ }
+#ifdef BT_USE_PLACEMENT_NEW
+ for (int i=curSize;i<newsize;i++)
+ {
+ new ( &m_data[i]) T(fillData);
+ }
+#endif //BT_USE_PLACEMENT_NEW
+
+ }
+
+ m_size = newsize;
+ }
+
+
+ SIMD_FORCE_INLINE T& expand( const T& fillValue=T())
+ {
+ int sz = size();
+ if( sz == capacity() )
+ {
+ reserve( allocSize(size()) );
+ }
+ m_size++;
+#ifdef BT_USE_PLACEMENT_NEW
+ new (&m_data[sz]) T(fillValue); //use the in-place new (not really allocating heap memory)
+#endif
+
+ return m_data[sz];
+ }
+
+
+ SIMD_FORCE_INLINE void push_back(const T& _Val)
+ {
+ int sz = size();
+ if( sz == capacity() )
+ {
+ reserve( allocSize(size()) );
+ }
+
+#ifdef BT_USE_PLACEMENT_NEW
+ new ( &m_data[m_size] ) T(_Val);
+#else
+ m_data[size()] = _Val;
+#endif //BT_USE_PLACEMENT_NEW
+
+ m_size++;
+ }
+
+
+ /// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
+ SIMD_FORCE_INLINE int capacity() const
+ {
+ return m_capacity;
+ }
+
+ SIMD_FORCE_INLINE void reserve(int _Count)
+ { // determine new minimum length of allocated storage
+ if (capacity() < _Count)
+ { // not enough room, reallocate
+ T* s = (T*)allocate(_Count);
+
+ copy(0, size(), s);
+
+ destroy(0,size());
+
+ deallocate();
+
+ //PCK: added this line
+ m_ownsMemory = true;
+
+ m_data = s;
+
+ m_capacity = _Count;
+
+ }
+ }
+
+
+ class less
+ {
+ public:
+
+ bool operator() ( const T& a, const T& b )
+ {
+ return ( a < b );
+ }
+ };
+
+ template <typename L>
+ void quickSortInternal(L CompareFunc,int lo, int hi)
+ {
+ // lo is the lower index, hi is the upper index
+ // of the region of array a that is to be sorted
+ int i=lo, j=hi;
+ T x=m_data[(lo+hi)/2];
+
+ // partition
+ do
+ {
+ while (CompareFunc(m_data[i],x))
+ i++;
+ while (CompareFunc(x,m_data[j]))
+ j--;
+ if (i<=j)
+ {
+ swap(i,j);
+ i++; j--;
+ }
+ } while (i<=j);
+
+ // recursion
+ if (lo<j)
+ quickSortInternal( CompareFunc, lo, j);
+ if (i<hi)
+ quickSortInternal( CompareFunc, i, hi);
+ }
+
+
+ template <typename L>
+ void quickSort(L CompareFunc)
+ {
+ //don't sort 0 or 1 elements
+ if (size()>1)
+ {
+ quickSortInternal(CompareFunc,0,size()-1);
+ }
+ }
+
+
+ ///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
+ template <typename L>
+ void downHeap(T *pArr, int k, int n,L CompareFunc)
+ {
+ /* PRE: a[k+1..N] is a heap */
+ /* POST: a[k..N] is a heap */
+
+ T temp = pArr[k - 1];
+ /* k has child(s) */
+ while (k <= n/2)
+ {
+ int child = 2*k;
+
+ if ((child < n) && CompareFunc(pArr[child - 1] , pArr[child]))
+ {
+ child++;
+ }
+ /* pick larger child */
+ if (CompareFunc(temp , pArr[child - 1]))
+ {
+ /* move child up */
+ pArr[k - 1] = pArr[child - 1];
+ k = child;
+ }
+ else
+ {
+ break;
+ }
+ }
+ pArr[k - 1] = temp;
+ } /*downHeap*/
+
+ void swap(int index0,int index1)
+ {
+#ifdef BT_USE_MEMCPY
+ char temp[sizeof(T)];
+ memcpy(temp,&m_data[index0],sizeof(T));
+ memcpy(&m_data[index0],&m_data[index1],sizeof(T));
+ memcpy(&m_data[index1],temp,sizeof(T));
+#else
+ T temp = m_data[index0];
+ m_data[index0] = m_data[index1];
+ m_data[index1] = temp;
+#endif //BT_USE_PLACEMENT_NEW
+
+ }
+
+ template <typename L>
+ void heapSort(L CompareFunc)
+ {
+ /* sort a[0..N-1], N.B. 0 to N-1 */
+ int k;
+ int n = m_size;
+ for (k = n/2; k > 0; k--)
+ {
+ downHeap(m_data, k, n, CompareFunc);
+ }
+
+ /* a[1..N] is now a heap */
+ while ( n>=1 )
+ {
+ swap(0,n-1); /* largest of a[0..n-1] */
+
+
+ n = n - 1;
+ /* restore a[1..i-1] heap */
+ downHeap(m_data, 1, n, CompareFunc);
+ }
+ }
+
+ ///non-recursive binary search, assumes sorted array
+ int findBinarySearch(const T& key) const
+ {
+ int first = 0;
+ int last = size();
+
+ //assume sorted array
+ while (first <= last) {
+ int mid = (first + last) / 2; // compute mid point.
+ if (key > m_data[mid])
+ first = mid + 1; // repeat search in top half.
+ else if (key < m_data[mid])
+ last = mid - 1; // repeat search in bottom half.
+ else
+ return mid; // found it. return position /////
+ }
+ return size(); // failed to find key
+ }
+
+
+ int findLinearSearch(const T& key) const
+ {
+ int index=size();
+ int i;
+
+ for (i=0;i<size();i++)
+ {
+ if (m_data[i] == key)
+ {
+ index = i;
+ break;
+ }
+ }
+ return index;
+ }
+
+ void remove(const T& key)
+ {
+
+ int findIndex = findLinearSearch(key);
+ if (findIndex<size())
+ {
+ swap( findIndex,size()-1);
+ pop_back();
+ }
+ }
+
+ //PCK: whole function
+ void initializeFromBuffer(void *buffer, int size, int capacity)
+ {
+ clear();
+ m_ownsMemory = false;
+ m_data = (T*)buffer;
+ m_size = size;
+ m_capacity = capacity;
+ }
+
+};
+
+#endif //BT_OBJECT_ARRAY__
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btConvexHull.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btConvexHull.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btConvexHull.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btConvexHull.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,241 @@
+
+/*
+Stan Melax Convex Hull Computation
+Copyright (c) 2008 Stan Melax http://www.melax.com/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///includes modifications/improvements by John Ratcliff, see BringOutYourDead below.
+
+#ifndef CD_HULL_H
+#define CD_HULL_H
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+typedef btAlignedObjectArray<unsigned int> TUIntArray;
+
+class HullResult
+{
+public:
+ HullResult(void)
+ {
+ mPolygons = true;
+ mNumOutputVertices = 0;
+ mNumFaces = 0;
+ mNumIndices = 0;
+ }
+ bool mPolygons; // true if indices represents polygons, false indices are triangles
+ unsigned int mNumOutputVertices; // number of vertices in the output hull
+ btAlignedObjectArray<btVector3> m_OutputVertices; // array of vertices
+ unsigned int mNumFaces; // the number of faces produced
+ unsigned int mNumIndices; // the total number of indices
+ btAlignedObjectArray<unsigned int> m_Indices; // pointer to indices.
+
+// If triangles, then indices are array indexes into the vertex list.
+// If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc..
+};
+
+enum HullFlag
+{
+ QF_TRIANGLES = (1<<0), // report results as triangles, not polygons.
+ QF_REVERSE_ORDER = (1<<1), // reverse order of the triangle indices.
+ QF_DEFAULT = QF_TRIANGLES
+};
+
+
+class HullDesc
+{
+public:
+ HullDesc(void)
+ {
+ mFlags = QF_DEFAULT;
+ mVcount = 0;
+ mVertices = 0;
+ mVertexStride = sizeof(btVector3);
+ mNormalEpsilon = 0.001f;
+ mMaxVertices = 4096; // maximum number of points to be considered for a convex hull.
+ mMaxFaces = 4096;
+ };
+
+ HullDesc(HullFlag flag,
+ unsigned int vcount,
+ const btVector3 *vertices,
+ unsigned int stride = sizeof(btVector3))
+ {
+ mFlags = flag;
+ mVcount = vcount;
+ mVertices = vertices;
+ mVertexStride = stride;
+ mNormalEpsilon = btScalar(0.001);
+ mMaxVertices = 4096;
+ }
+
+ bool HasHullFlag(HullFlag flag) const
+ {
+ if ( mFlags & flag ) return true;
+ return false;
+ }
+
+ void SetHullFlag(HullFlag flag)
+ {
+ mFlags|=flag;
+ }
+
+ void ClearHullFlag(HullFlag flag)
+ {
+ mFlags&=~flag;
+ }
+
+ unsigned int mFlags; // flags to use when generating the convex hull.
+ unsigned int mVcount; // number of vertices in the input point cloud
+ const btVector3 *mVertices; // the array of vertices.
+ unsigned int mVertexStride; // the stride of each vertex, in bytes.
+ btScalar mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on.
+ unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull!
+ unsigned int mMaxFaces;
+};
+
+enum HullError
+{
+ QE_OK, // success!
+ QE_FAIL // failed.
+};
+
+class btPlane
+{
+ public:
+ btVector3 normal;
+ btScalar dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0
+ btPlane(const btVector3 &n,btScalar d):normal(n),dist(d){}
+ btPlane():normal(),dist(0){}
+
+};
+
+
+
+class ConvexH
+{
+ public:
+ class HalfEdge
+ {
+ public:
+ short ea; // the other half of the edge (index into edges list)
+ unsigned char v; // the vertex at the start of this edge (index into vertices list)
+ unsigned char p; // the facet on which this edge lies (index into facets list)
+ HalfEdge(){}
+ HalfEdge(short _ea,unsigned char _v, unsigned char _p):ea(_ea),v(_v),p(_p){}
+ };
+ ConvexH()
+ {
+ }
+ ~ConvexH()
+ {
+ }
+ btAlignedObjectArray<btVector3> vertices;
+ btAlignedObjectArray<HalfEdge> edges;
+ btAlignedObjectArray<btPlane> facets;
+ ConvexH(int vertices_size,int edges_size,int facets_size);
+};
+
+
+class int4
+{
+public:
+ int x,y,z,w;
+ int4(){};
+ int4(int _x,int _y, int _z,int _w){x=_x;y=_y;z=_z;w=_w;}
+ const int& operator[](int i) const {return (&x)[i];}
+ int& operator[](int i) {return (&x)[i];}
+};
+
+class PHullResult
+{
+public:
+
+ PHullResult(void)
+ {
+ mVcount = 0;
+ mIndexCount = 0;
+ mFaceCount = 0;
+ mVertices = 0;
+ }
+
+ unsigned int mVcount;
+ unsigned int mIndexCount;
+ unsigned int mFaceCount;
+ btVector3* mVertices;
+ TUIntArray m_Indices;
+};
+
+
+
+///The HullLibrary class can create a convex hull from a collection of vertices, using the ComputeHull method.
+///The btShapeHull class uses this HullLibrary to create a approximate convex mesh given a general (non-polyhedral) convex shape.
+class HullLibrary
+{
+
+ btAlignedObjectArray<class btHullTriangle*> m_tris;
+
+public:
+
+ btAlignedObjectArray<int> m_vertexIndexMapping;
+
+
+ HullError CreateConvexHull(const HullDesc& desc, // describes the input request
+ HullResult& result); // contains the resulst
+ HullError ReleaseResult(HullResult &result); // release memory allocated for this result, we are done with it.
+
+private:
+
+ bool ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit);
+
+ class btHullTriangle* allocateTriangle(int a,int b,int c);
+ void deAllocateTriangle(btHullTriangle*);
+ void b2bfix(btHullTriangle* s,btHullTriangle*t);
+
+ void removeb2b(btHullTriangle* s,btHullTriangle*t);
+
+ void checkit(btHullTriangle *t);
+
+ btHullTriangle* extrudable(btScalar epsilon);
+
+ int calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit);
+
+ int calchullgen(btVector3 *verts,int verts_count, int vlimit);
+
+ int4 FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow);
+
+ class ConvexH* ConvexHCrop(ConvexH& convex,const btPlane& slice);
+
+ void extrude(class btHullTriangle* t0,int v);
+
+ ConvexH* test_cube();
+
+ //BringOutYourDead (John Ratcliff): When you create a convex hull you hand it a large input set of vertices forming a 'point cloud'.
+ //After the hull is generated it give you back a set of polygon faces which index the *original* point cloud.
+ //The thing is, often times, there are many 'dead vertices' in the point cloud that are on longer referenced by the hull.
+ //The routine 'BringOutYourDead' find only the referenced vertices, copies them to an new buffer, and re-indexes the hull so that it is a minimal representation.
+ void BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int* indices,unsigned indexcount);
+
+ bool CleanupVertices(unsigned int svcount,
+ const btVector3* svertices,
+ unsigned int stride,
+ unsigned int &vcount, // output number of vertices
+ btVector3* vertices, // location to store the results.
+ btScalar normalepsilon,
+ btVector3& scale);
+};
+
+
+#endif
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btDefaultMotionState.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btDefaultMotionState.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btDefaultMotionState.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btDefaultMotionState.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,38 @@
+#ifndef DEFAULT_MOTION_STATE_H
+#define DEFAULT_MOTION_STATE_H
+
+///The btDefaultMotionState provides a common implementation to synchronize world transforms with offsets.
+struct btDefaultMotionState : public btMotionState
+{
+ btTransform m_graphicsWorldTrans;
+ btTransform m_centerOfMassOffset;
+ btTransform m_startWorldTrans;
+ void* m_userPointer;
+
+ btDefaultMotionState(const btTransform& startTrans = btTransform::getIdentity(),const btTransform& centerOfMassOffset = btTransform::getIdentity())
+ : m_graphicsWorldTrans(startTrans),
+ m_centerOfMassOffset(centerOfMassOffset),
+ m_startWorldTrans(startTrans),
+ m_userPointer(0)
+
+ {
+ }
+
+ ///synchronizes world transform from user to physics
+ virtual void getWorldTransform(btTransform& centerOfMassWorldTrans ) const
+ {
+ centerOfMassWorldTrans = m_centerOfMassOffset.inverse() * m_graphicsWorldTrans ;
+ }
+
+ ///synchronizes world transform from physics to user
+ ///Bullet only calls the update of worldtransform for active objects
+ virtual void setWorldTransform(const btTransform& centerOfMassWorldTrans)
+ {
+ m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset ;
+ }
+
+
+
+};
+
+#endif //DEFAULT_MOTION_STATE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btGeometryUtil.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btGeometryUtil.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btGeometryUtil.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btGeometryUtil.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,42 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef BT_GEOMETRY_UTIL_H
+#define BT_GEOMETRY_UTIL_H
+
+#include "btVector3.h"
+#include "btAlignedObjectArray.h"
+
+///The btGeometryUtil helper class provides a few methods to convert between plane equations and vertices.
+class btGeometryUtil
+{
+ public:
+
+
+ static void getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut );
+
+ static void getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut );
+
+ static bool isInside(const btAlignedObjectArray<btVector3>& vertices, const btVector3& planeNormal, btScalar margin);
+
+ static bool isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar margin);
+
+ static bool areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar margin);
+
+};
+
+
+#endif //BT_GEOMETRY_UTIL_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btHashMap.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btHashMap.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btHashMap.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btHashMap.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,369 @@
+#ifndef BT_HASH_MAP_H
+#define BT_HASH_MAP_H
+
+#include "btAlignedObjectArray.h"
+
+///very basic hashable string implementation, compatible with btHashMap
+struct btHashString
+{
+ const char* m_string;
+ unsigned int m_hash;
+
+ SIMD_FORCE_INLINE unsigned int getHash()const
+ {
+ return m_hash;
+ }
+
+ btHashString(const char* name)
+ :m_string(name)
+ {
+ /* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */
+ static const unsigned int InitialFNV = 2166136261u;
+ static const unsigned int FNVMultiple = 16777619u;
+
+ /* Fowler / Noll / Vo (FNV) Hash */
+ unsigned int hash = InitialFNV;
+
+ for(int i = 0; m_string[i]; i++)
+ {
+ hash = hash ^ (m_string[i]); /* xor the low 8 bits */
+ hash = hash * FNVMultiple; /* multiply by the magic number */
+ }
+ m_hash = hash;
+ }
+
+ int portableStringCompare(const char* src, const char* dst) const
+ {
+ int ret = 0 ;
+
+ while( ! (ret = *(unsigned char *)src - *(unsigned char *)dst) && *dst)
+ ++src, ++dst;
+
+ if ( ret < 0 )
+ ret = -1 ;
+ else if ( ret > 0 )
+ ret = 1 ;
+
+ return( ret );
+ }
+
+ const bool equals(const btHashString& other) const
+ {
+ return (m_string == other.m_string) ||
+ (0==portableStringCompare(m_string,other.m_string));
+
+ }
+
+};
+
+const int BT_HASH_NULL=0xffffffff;
+
+template <class Value>
+class btHashKey
+{
+ int m_uid;
+public:
+
+ btHashKey(int uid)
+ :m_uid(uid)
+ {
+ }
+
+ int getUid1() const
+ {
+ return m_uid;
+ }
+
+ bool equals(const btHashKey<Value>& other) const
+ {
+ return getUid1() == other.getUid1();
+ }
+ //to our success
+ SIMD_FORCE_INLINE unsigned int getHash()const
+ {
+ int key = m_uid;
+ // Thomas Wang's hash
+ key += ~(key << 15);
+ key ^= (key >> 10);
+ key += (key << 3);
+ key ^= (key >> 6);
+ key += ~(key << 11);
+ key ^= (key >> 16);
+ return key;
+ }
+
+
+};
+
+
+template <class Value>
+class btHashKeyPtr
+{
+ int m_uid;
+public:
+
+ btHashKeyPtr(int uid)
+ :m_uid(uid)
+ {
+ }
+
+ int getUid1() const
+ {
+ return m_uid;
+ }
+
+ bool equals(const btHashKeyPtr<Value>& other) const
+ {
+ return getUid1() == other.getUid1();
+ }
+
+ //to our success
+ SIMD_FORCE_INLINE unsigned int getHash()const
+ {
+ int key = m_uid;
+ // Thomas Wang's hash
+ key += ~(key << 15);
+ key ^= (key >> 10);
+ key += (key << 3);
+ key ^= (key >> 6);
+ key += ~(key << 11);
+ key ^= (key >> 16);
+ return key;
+ }
+
+
+};
+
+///The btHashMap template class implements a generic and lightweight hashmap.
+///A basic sample of how to use btHashMap is located in Demos\BasicDemo\main.cpp
+template <class Key, class Value>
+class btHashMap
+{
+
+ btAlignedObjectArray<int> m_hashTable;
+ btAlignedObjectArray<int> m_next;
+
+ btAlignedObjectArray<Value> m_valueArray;
+ btAlignedObjectArray<Key> m_keyArray;
+
+ void growTables(const Key& key)
+ {
+ int newCapacity = m_valueArray.capacity();
+
+ if (m_hashTable.size() < newCapacity)
+ {
+ //grow hashtable and next table
+ int curHashtableSize = m_hashTable.size();
+
+ m_hashTable.resize(newCapacity);
+ m_next.resize(newCapacity);
+
+ int i;
+
+ for (i= 0; i < newCapacity; ++i)
+ {
+ m_hashTable[i] = BT_HASH_NULL;
+ }
+ for (i = 0; i < newCapacity; ++i)
+ {
+ m_next[i] = BT_HASH_NULL;
+ }
+
+ for(i=0;i<curHashtableSize;i++)
+ {
+ //const Value& value = m_valueArray[i];
+ //const Key& key = m_keyArray[i];
+
+ int hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity()-1); // New hash value with new mask
+ m_next[i] = m_hashTable[hashValue];
+ m_hashTable[hashValue] = i;
+ }
+
+
+ }
+ }
+
+ public:
+
+ void insert(const Key& key, const Value& value) {
+ int hash = key.getHash() & (m_valueArray.capacity()-1);
+
+ //replace value if the key is already there
+ int index = findIndex(key);
+ if (index != BT_HASH_NULL)
+ {
+ m_valueArray[index]=value;
+ return;
+ }
+
+ int count = m_valueArray.size();
+ int oldCapacity = m_valueArray.capacity();
+ m_valueArray.push_back(value);
+ m_keyArray.push_back(key);
+
+ int newCapacity = m_valueArray.capacity();
+ if (oldCapacity < newCapacity)
+ {
+ growTables(key);
+ //hash with new capacity
+ hash = key.getHash() & (m_valueArray.capacity()-1);
+ }
+ m_next[count] = m_hashTable[hash];
+ m_hashTable[hash] = count;
+ }
+
+ void remove(const Key& key) {
+
+ int hash = key.getHash() & (m_valueArray.capacity()-1);
+
+ int pairIndex = findIndex(key);
+
+ if (pairIndex ==BT_HASH_NULL)
+ {
+ return;
+ }
+
+ // Remove the pair from the hash table.
+ int index = m_hashTable[hash];
+ btAssert(index != BT_HASH_NULL);
+
+ int previous = BT_HASH_NULL;
+ while (index != pairIndex)
+ {
+ previous = index;
+ index = m_next[index];
+ }
+
+ if (previous != BT_HASH_NULL)
+ {
+ btAssert(m_next[previous] == pairIndex);
+ m_next[previous] = m_next[pairIndex];
+ }
+ else
+ {
+ m_hashTable[hash] = m_next[pairIndex];
+ }
+
+ // We now move the last pair into spot of the
+ // pair being removed. We need to fix the hash
+ // table indices to support the move.
+
+ int lastPairIndex = m_valueArray.size() - 1;
+
+ // If the removed pair is the last pair, we are done.
+ if (lastPairIndex == pairIndex)
+ {
+ m_valueArray.pop_back();
+ m_keyArray.pop_back();
+ return;
+ }
+
+ // Remove the last pair from the hash table.
+ int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity()-1);
+
+ index = m_hashTable[lastHash];
+ btAssert(index != BT_HASH_NULL);
+
+ previous = BT_HASH_NULL;
+ while (index != lastPairIndex)
+ {
+ previous = index;
+ index = m_next[index];
+ }
+
+ if (previous != BT_HASH_NULL)
+ {
+ btAssert(m_next[previous] == lastPairIndex);
+ m_next[previous] = m_next[lastPairIndex];
+ }
+ else
+ {
+ m_hashTable[lastHash] = m_next[lastPairIndex];
+ }
+
+ // Copy the last pair into the remove pair's spot.
+ m_valueArray[pairIndex] = m_valueArray[lastPairIndex];
+ m_keyArray[pairIndex] = m_keyArray[lastPairIndex];
+
+ // Insert the last pair into the hash table
+ m_next[pairIndex] = m_hashTable[lastHash];
+ m_hashTable[lastHash] = pairIndex;
+
+ m_valueArray.pop_back();
+ m_keyArray.pop_back();
+
+ }
+
+
+ int size() const
+ {
+ return m_valueArray.size();
+ }
+
+ const Value* getAtIndex(int index) const
+ {
+ btAssert(index < m_valueArray.size());
+
+ return &m_valueArray[index];
+ }
+
+ Value* getAtIndex(int index)
+ {
+ btAssert(index < m_valueArray.size());
+
+ return &m_valueArray[index];
+ }
+
+ Value* operator[](const Key& key) {
+ return find(key);
+ }
+
+ const Value* find(const Key& key) const
+ {
+ int index = findIndex(key);
+ if (index == BT_HASH_NULL)
+ {
+ return NULL;
+ }
+ return &m_valueArray[index];
+ }
+
+ Value* find(const Key& key)
+ {
+ int index = findIndex(key);
+ if (index == BT_HASH_NULL)
+ {
+ return NULL;
+ }
+ return &m_valueArray[index];
+ }
+
+
+ int findIndex(const Key& key) const
+ {
+ unsigned int hash = key.getHash() & (m_valueArray.capacity()-1);
+
+ if (hash >= (unsigned int)m_hashTable.size())
+ {
+ return BT_HASH_NULL;
+ }
+
+ int index = m_hashTable[hash];
+ while ((index != BT_HASH_NULL) && key.equals(m_keyArray[index]) == false)
+ {
+ index = m_next[index];
+ }
+ return index;
+ }
+
+ void clear()
+ {
+ m_hashTable.clear();
+ m_next.clear();
+ m_valueArray.clear();
+ m_keyArray.clear();
+ }
+
+};
+
+#endif //BT_HASH_MAP_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btIDebugDraw.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btIDebugDraw.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btIDebugDraw.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btIDebugDraw.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,287 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef IDEBUG_DRAW__H
+#define IDEBUG_DRAW__H
+
+#include "btVector3.h"
+#include "btTransform.h"
+
+
+///The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
+///Typical use case: create a debug drawer object, and assign it to a btCollisionWorld or btDynamicsWorld using setDebugDrawer and call debugDrawWorld.
+///A class that implements the btIDebugDraw interface has to implement the drawLine method at a minimum.
+class btIDebugDraw
+{
+ public:
+
+ enum DebugDrawModes
+ {
+ DBG_NoDebug=0,
+ DBG_DrawWireframe = 1,
+ DBG_DrawAabb=2,
+ DBG_DrawFeaturesText=4,
+ DBG_DrawContactPoints=8,
+ DBG_NoDeactivation=16,
+ DBG_NoHelpText = 32,
+ DBG_DrawText=64,
+ DBG_ProfileTimings = 128,
+ DBG_EnableSatComparison = 256,
+ DBG_DisableBulletLCP = 512,
+ DBG_EnableCCD = 1024,
+ DBG_DrawConstraints = (1 << 11),
+ DBG_DrawConstraintLimits = (1 << 12),
+ DBG_FastWireframe = (1<<13),
+ DBG_MAX_DEBUG_DRAW_MODE
+ };
+
+ virtual ~btIDebugDraw() {};
+
+ virtual void drawLine(const btVector3& from,const btVector3& to, const btVector3& fromColor, const btVector3& toColor)
+ {
+ drawLine (from, to, fromColor);
+ }
+
+ virtual void drawBox (const btVector3& boxMin, const btVector3& boxMax, const btVector3& color, btScalar alpha)
+ {
+ }
+
+ virtual void drawSphere (const btVector3& p, btScalar radius, const btVector3& color)
+ {
+ }
+
+ virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0;
+
+ virtual void drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& /*n0*/,const btVector3& /*n1*/,const btVector3& /*n2*/,const btVector3& color, btScalar alpha)
+ {
+ drawTriangle(v0,v1,v2,color,alpha);
+ }
+ virtual void drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& color, btScalar /*alpha*/)
+ {
+ drawLine(v0,v1,color);
+ drawLine(v1,v2,color);
+ drawLine(v2,v0,color);
+ }
+
+ virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)=0;
+
+ virtual void reportErrorWarning(const char* warningString) = 0;
+
+ virtual void draw3dText(const btVector3& location,const char* textString) = 0;
+
+ virtual void setDebugMode(int debugMode) =0;
+
+ virtual int getDebugMode() const = 0;
+
+ inline void drawAabb(const btVector3& from,const btVector3& to,const btVector3& color)
+ {
+
+ btVector3 halfExtents = (to-from)* 0.5f;
+ btVector3 center = (to+from) *0.5f;
+ int i,j;
+
+ btVector3 edgecoord(1.f,1.f,1.f),pa,pb;
+ for (i=0;i<4;i++)
+ {
+ for (j=0;j<3;j++)
+ {
+ pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],
+ edgecoord[2]*halfExtents[2]);
+ pa+=center;
+
+ int othercoord = j%3;
+ edgecoord[othercoord]*=-1.f;
+ pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],
+ edgecoord[2]*halfExtents[2]);
+ pb+=center;
+
+ drawLine(pa,pb,color);
+ }
+ edgecoord = btVector3(-1.f,-1.f,-1.f);
+ if (i<3)
+ edgecoord[i]*=-1.f;
+ }
+ }
+ void drawTransform(const btTransform& transform, btScalar orthoLen)
+ {
+ btVector3 start = transform.getOrigin();
+ drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(0.7f,0,0));
+ drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0,0.7f,0));
+ drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0,0,0.7f));
+ }
+
+ void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle,
+ const btVector3& color, bool drawSect, btScalar stepDegrees = btScalar(10.f))
+ {
+ const btVector3& vx = axis;
+ btVector3 vy = normal.cross(axis);
+ btScalar step = stepDegrees * SIMD_RADS_PER_DEG;
+ int nSteps = (int)((maxAngle - minAngle) / step);
+ if(!nSteps) nSteps = 1;
+ btVector3 prev = center + radiusA * vx * btCos(minAngle) + radiusB * vy * btSin(minAngle);
+ if(drawSect)
+ {
+ drawLine(center, prev, color);
+ }
+ for(int i = 1; i <= nSteps; i++)
+ {
+ btScalar angle = minAngle + (maxAngle - minAngle) * btScalar(i) / btScalar(nSteps);
+ btVector3 next = center + radiusA * vx * btCos(angle) + radiusB * vy * btSin(angle);
+ drawLine(prev, next, color);
+ prev = next;
+ }
+ if(drawSect)
+ {
+ drawLine(center, prev, color);
+ }
+ }
+ void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius,
+ btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f))
+ {
+ btVector3 vA[74];
+ btVector3 vB[74];
+ btVector3 *pvA = vA, *pvB = vB, *pT;
+ btVector3 npole = center + up * radius;
+ btVector3 spole = center - up * radius;
+ btVector3 arcStart;
+ btScalar step = stepDegrees * SIMD_RADS_PER_DEG;
+ const btVector3& kv = up;
+ const btVector3& iv = axis;
+ btVector3 jv = kv.cross(iv);
+ bool drawN = false;
+ bool drawS = false;
+ if(minTh <= -SIMD_HALF_PI)
+ {
+ minTh = -SIMD_HALF_PI + step;
+ drawN = true;
+ }
+ if(maxTh >= SIMD_HALF_PI)
+ {
+ maxTh = SIMD_HALF_PI - step;
+ drawS = true;
+ }
+ if(minTh > maxTh)
+ {
+ minTh = -SIMD_HALF_PI + step;
+ maxTh = SIMD_HALF_PI - step;
+ drawN = drawS = true;
+ }
+ int n_hor = (int)((maxTh - minTh) / step) + 1;
+ if(n_hor < 2) n_hor = 2;
+ btScalar step_h = (maxTh - minTh) / btScalar(n_hor - 1);
+ bool isClosed = false;
+ if(minPs > maxPs)
+ {
+ minPs = -SIMD_PI + step;
+ maxPs = SIMD_PI;
+ isClosed = true;
+ }
+ else if((maxPs - minPs) >= SIMD_PI * btScalar(2.f))
+ {
+ isClosed = true;
+ }
+ else
+ {
+ isClosed = false;
+ }
+ int n_vert = (int)((maxPs - minPs) / step) + 1;
+ if(n_vert < 2) n_vert = 2;
+ btScalar step_v = (maxPs - minPs) / btScalar(n_vert - 1);
+ for(int i = 0; i < n_hor; i++)
+ {
+ btScalar th = minTh + btScalar(i) * step_h;
+ btScalar sth = radius * btSin(th);
+ btScalar cth = radius * btCos(th);
+ for(int j = 0; j < n_vert; j++)
+ {
+ btScalar psi = minPs + btScalar(j) * step_v;
+ btScalar sps = btSin(psi);
+ btScalar cps = btCos(psi);
+ pvB[j] = center + cth * cps * iv + cth * sps * jv + sth * kv;
+ if(i)
+ {
+ drawLine(pvA[j], pvB[j], color);
+ }
+ else if(drawS)
+ {
+ drawLine(spole, pvB[j], color);
+ }
+ if(j)
+ {
+ drawLine(pvB[j-1], pvB[j], color);
+ }
+ else
+ {
+ arcStart = pvB[j];
+ }
+ if((i == (n_hor - 1)) && drawN)
+ {
+ drawLine(npole, pvB[j], color);
+ }
+ if(isClosed)
+ {
+ if(j == (n_vert-1))
+ {
+ drawLine(arcStart, pvB[j], color);
+ }
+ }
+ else
+ {
+ if(((!i) || (i == (n_hor-1))) && ((!j) || (j == (n_vert-1))))
+ {
+ drawLine(center, pvB[j], color);
+ }
+ }
+ }
+ pT = pvA; pvA = pvB; pvB = pT;
+ }
+ }
+
+ void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btVector3& color)
+ {
+ drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
+ drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMin[2]), color);
+ drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMin[2]), color);
+ drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMin[2]), color);
+ drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
+ drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
+ drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
+ drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
+ drawLine(btVector3(bbMin[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
+ drawLine(btVector3(bbMax[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
+ drawLine(btVector3(bbMax[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
+ drawLine(btVector3(bbMin[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
+ }
+ void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btTransform& trans, const btVector3& color)
+ {
+ drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
+ drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), color);
+ drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), color);
+ drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), color);
+ drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
+ drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
+ drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
+ drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
+ drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
+ drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
+ drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
+ drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
+ }
+};
+
+
+#endif //IDEBUG_DRAW__H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btList.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btList.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btList.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btList.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,73 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef GEN_LIST_H
+#define GEN_LIST_H
+
+class btGEN_Link {
+public:
+ btGEN_Link() : m_next(0), m_prev(0) {}
+ btGEN_Link(btGEN_Link *next, btGEN_Link *prev) : m_next(next), m_prev(prev) {}
+
+ btGEN_Link *getNext() const { return m_next; }
+ btGEN_Link *getPrev() const { return m_prev; }
+
+ bool isHead() const { return m_prev == 0; }
+ bool isTail() const { return m_next == 0; }
+
+ void insertBefore(btGEN_Link *link) {
+ m_next = link;
+ m_prev = link->m_prev;
+ m_next->m_prev = this;
+ m_prev->m_next = this;
+ }
+
+ void insertAfter(btGEN_Link *link) {
+ m_next = link->m_next;
+ m_prev = link;
+ m_next->m_prev = this;
+ m_prev->m_next = this;
+ }
+
+ void remove() {
+ m_next->m_prev = m_prev;
+ m_prev->m_next = m_next;
+ }
+
+private:
+ btGEN_Link *m_next;
+ btGEN_Link *m_prev;
+};
+
+class btGEN_List {
+public:
+ btGEN_List() : m_head(&m_tail, 0), m_tail(0, &m_head) {}
+
+ btGEN_Link *getHead() const { return m_head.getNext(); }
+ btGEN_Link *getTail() const { return m_tail.getPrev(); }
+
+ void addHead(btGEN_Link *link) { link->insertAfter(&m_head); }
+ void addTail(btGEN_Link *link) { link->insertBefore(&m_tail); }
+
+private:
+ btGEN_Link m_head;
+ btGEN_Link m_tail;
+};
+
+#endif
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btMatrix3x3.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btMatrix3x3.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btMatrix3x3.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btMatrix3x3.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,618 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef btMatrix3x3_H
+#define btMatrix3x3_H
+
+#include "btScalar.h"
+
+#include "btVector3.h"
+#include "btQuaternion.h"
+
+
+
+/**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3.
+ * Make sure to only include a pure orthogonal matrix without scaling. */
+class btMatrix3x3 {
+ public:
+ /** @brief No initializaion constructor */
+ btMatrix3x3 () {}
+
+// explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
+
+ /**@brief Constructor from Quaternion */
+ explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
+ /*
+ template <typename btScalar>
+ Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
+ {
+ setEulerYPR(yaw, pitch, roll);
+ }
+ */
+ /** @brief Constructor with row major formatting */
+ btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
+ const btScalar& yx, const btScalar& yy, const btScalar& yz,
+ const btScalar& zx, const btScalar& zy, const btScalar& zz)
+ {
+ setValue(xx, xy, xz,
+ yx, yy, yz,
+ zx, zy, zz);
+ }
+ /** @brief Copy constructor */
+ SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
+ {
+ m_el[0] = other.m_el[0];
+ m_el[1] = other.m_el[1];
+ m_el[2] = other.m_el[2];
+ }
+ /** @brief Assignment Operator */
+ SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
+ {
+ m_el[0] = other.m_el[0];
+ m_el[1] = other.m_el[1];
+ m_el[2] = other.m_el[2];
+ return *this;
+ }
+
+ /** @brief Get a column of the matrix as a vector
+ * @param i Column number 0 indexed */
+ SIMD_FORCE_INLINE btVector3 getColumn(int i) const
+ {
+ return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
+ }
+
+
+ /** @brief Get a row of the matrix as a vector
+ * @param i Row number 0 indexed */
+ SIMD_FORCE_INLINE const btVector3& getRow(int i) const
+ {
+ btFullAssert(0 <= i && i < 3);
+ return m_el[i];
+ }
+
+ /** @brief Get a mutable reference to a row of the matrix as a vector
+ * @param i Row number 0 indexed */
+ SIMD_FORCE_INLINE btVector3& operator[](int i)
+ {
+ btFullAssert(0 <= i && i < 3);
+ return m_el[i];
+ }
+
+ /** @brief Get a const reference to a row of the matrix as a vector
+ * @param i Row number 0 indexed */
+ SIMD_FORCE_INLINE const btVector3& operator[](int i) const
+ {
+ btFullAssert(0 <= i && i < 3);
+ return m_el[i];
+ }
+
+ /** @brief Multiply by the target matrix on the right
+ * @param m Rotation matrix to be applied
+ * Equivilant to this = this * m */
+ btMatrix3x3& operator*=(const btMatrix3x3& m);
+
+ /** @brief Set from a carray of btScalars
+ * @param m A pointer to the beginning of an array of 9 btScalars */
+ void setFromOpenGLSubMatrix(const btScalar *m)
+ {
+ m_el[0].setValue(m[0],m[4],m[8]);
+ m_el[1].setValue(m[1],m[5],m[9]);
+ m_el[2].setValue(m[2],m[6],m[10]);
+
+ }
+ /** @brief Set the values of the matrix explicitly (row major)
+ * @param xx Top left
+ * @param xy Top Middle
+ * @param xz Top Right
+ * @param yx Middle Left
+ * @param yy Middle Middle
+ * @param yz Middle Right
+ * @param zx Bottom Left
+ * @param zy Bottom Middle
+ * @param zz Bottom Right*/
+ void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz,
+ const btScalar& yx, const btScalar& yy, const btScalar& yz,
+ const btScalar& zx, const btScalar& zy, const btScalar& zz)
+ {
+ m_el[0].setValue(xx,xy,xz);
+ m_el[1].setValue(yx,yy,yz);
+ m_el[2].setValue(zx,zy,zz);
+ }
+
+ /** @brief Set the matrix from a quaternion
+ * @param q The Quaternion to match */
+ void setRotation(const btQuaternion& q)
+ {
+ btScalar d = q.length2();
+ btFullAssert(d != btScalar(0.0));
+ btScalar s = btScalar(2.0) / d;
+ btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
+ btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
+ btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
+ btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
+ setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
+ xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
+ xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
+ }
+
+
+ /** @brief Set the matrix from euler angles using YPR around YXZ respectively
+ * @param yaw Yaw about Y axis
+ * @param pitch Pitch about X axis
+ * @param roll Roll about Z axis
+ */
+ void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
+ {
+ setEulerZYX(roll, pitch, yaw);
+ }
+
+ /** @brief Set the matrix from euler angles YPR around ZYX axes
+ * @param eulerX Roll about X axis
+ * @param eulerY Pitch around Y axis
+ * @param eulerZ Yaw aboud Z axis
+ *
+ * These angles are used to produce a rotation matrix. The euler
+ * angles are applied in ZYX order. I.e a vector is first rotated
+ * about X then Y and then Z
+ **/
+ void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) {
+ ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code
+ btScalar ci ( btCos(eulerX));
+ btScalar cj ( btCos(eulerY));
+ btScalar ch ( btCos(eulerZ));
+ btScalar si ( btSin(eulerX));
+ btScalar sj ( btSin(eulerY));
+ btScalar sh ( btSin(eulerZ));
+ btScalar cc = ci * ch;
+ btScalar cs = ci * sh;
+ btScalar sc = si * ch;
+ btScalar ss = si * sh;
+
+ setValue(cj * ch, sj * sc - cs, sj * cc + ss,
+ cj * sh, sj * ss + cc, sj * cs - sc,
+ -sj, cj * si, cj * ci);
+ }
+
+ /**@brief Set the matrix to the identity */
+ void setIdentity()
+ {
+ setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0),
+ btScalar(0.0), btScalar(1.0), btScalar(0.0),
+ btScalar(0.0), btScalar(0.0), btScalar(1.0));
+ }
+
+ static const btMatrix3x3& getIdentity()
+ {
+ static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0),
+ btScalar(0.0), btScalar(1.0), btScalar(0.0),
+ btScalar(0.0), btScalar(0.0), btScalar(1.0));
+ return identityMatrix;
+ }
+
+ /**@brief Fill the values of the matrix into a 9 element array
+ * @param m The array to be filled */
+ void getOpenGLSubMatrix(btScalar *m) const
+ {
+ m[0] = btScalar(m_el[0].x());
+ m[1] = btScalar(m_el[1].x());
+ m[2] = btScalar(m_el[2].x());
+ m[3] = btScalar(0.0);
+ m[4] = btScalar(m_el[0].y());
+ m[5] = btScalar(m_el[1].y());
+ m[6] = btScalar(m_el[2].y());
+ m[7] = btScalar(0.0);
+ m[8] = btScalar(m_el[0].z());
+ m[9] = btScalar(m_el[1].z());
+ m[10] = btScalar(m_el[2].z());
+ m[11] = btScalar(0.0);
+ }
+
+ /**@brief Get the matrix represented as a quaternion
+ * @param q The quaternion which will be set */
+ void getRotation(btQuaternion& q) const
+ {
+ btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
+ btScalar temp[4];
+
+ if (trace > btScalar(0.0))
+ {
+ btScalar s = btSqrt(trace + btScalar(1.0));
+ temp[3]=(s * btScalar(0.5));
+ s = btScalar(0.5) / s;
+
+ temp[0]=((m_el[2].y() - m_el[1].z()) * s);
+ temp[1]=((m_el[0].z() - m_el[2].x()) * s);
+ temp[2]=((m_el[1].x() - m_el[0].y()) * s);
+ }
+ else
+ {
+ int i = m_el[0].x() < m_el[1].y() ?
+ (m_el[1].y() < m_el[2].z() ? 2 : 1) :
+ (m_el[0].x() < m_el[2].z() ? 2 : 0);
+ int j = (i + 1) % 3;
+ int k = (i + 2) % 3;
+
+ btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
+ temp[i] = s * btScalar(0.5);
+ s = btScalar(0.5) / s;
+
+ temp[3] = (m_el[k][j] - m_el[j][k]) * s;
+ temp[j] = (m_el[j][i] + m_el[i][j]) * s;
+ temp[k] = (m_el[k][i] + m_el[i][k]) * s;
+ }
+ q.setValue(temp[0],temp[1],temp[2],temp[3]);
+ }
+
+ /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
+ * @param yaw Yaw around Y axis
+ * @param pitch Pitch around X axis
+ * @param roll around Z axis */
+ void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const
+ {
+
+ // first use the normal calculus
+ yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
+ pitch = btScalar(btAsin(-m_el[2].x()));
+ roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
+
+ // on pitch = +/-HalfPI
+ if (btFabs(pitch)==SIMD_HALF_PI)
+ {
+ if (yaw>0)
+ yaw-=SIMD_PI;
+ else
+ yaw+=SIMD_PI;
+
+ if (roll>0)
+ roll-=SIMD_PI;
+ else
+ roll+=SIMD_PI;
+ }
+ };
+
+
+ /**@brief Get the matrix represented as euler angles around ZYX
+ * @param yaw Yaw around X axis
+ * @param pitch Pitch around Y axis
+ * @param roll around X axis
+ * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/
+ void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const
+ {
+ struct Euler{btScalar yaw, pitch, roll;};
+ Euler euler_out;
+ Euler euler_out2; //second solution
+ //get the pointer to the raw data
+
+ // Check that pitch is not at a singularity
+ if (btFabs(m_el[2].x()) >= 1)
+ {
+ euler_out.yaw = 0;
+ euler_out2.yaw = 0;
+
+ // From difference of angles formula
+ btScalar delta = btAtan2(m_el[0].x(),m_el[0].z());
+ if (m_el[2].x() > 0) //gimbal locked up
+ {
+ euler_out.pitch = SIMD_PI / btScalar(2.0);
+ euler_out2.pitch = SIMD_PI / btScalar(2.0);
+ euler_out.roll = euler_out.pitch + delta;
+ euler_out2.roll = euler_out.pitch + delta;
+ }
+ else // gimbal locked down
+ {
+ euler_out.pitch = -SIMD_PI / btScalar(2.0);
+ euler_out2.pitch = -SIMD_PI / btScalar(2.0);
+ euler_out.roll = -euler_out.pitch + delta;
+ euler_out2.roll = -euler_out.pitch + delta;
+ }
+ }
+ else
+ {
+ euler_out.pitch = - btAsin(m_el[2].x());
+ euler_out2.pitch = SIMD_PI - euler_out.pitch;
+
+ euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch),
+ m_el[2].z()/btCos(euler_out.pitch));
+ euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch),
+ m_el[2].z()/btCos(euler_out2.pitch));
+
+ euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch),
+ m_el[0].x()/btCos(euler_out.pitch));
+ euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch),
+ m_el[0].x()/btCos(euler_out2.pitch));
+ }
+
+ if (solution_number == 1)
+ {
+ yaw = euler_out.yaw;
+ pitch = euler_out.pitch;
+ roll = euler_out.roll;
+ }
+ else
+ {
+ yaw = euler_out2.yaw;
+ pitch = euler_out2.pitch;
+ roll = euler_out2.roll;
+ }
+ }
+
+ /**@brief Create a scaled copy of the matrix
+ * @param s Scaling vector The elements of the vector will scale each column */
+
+ btMatrix3x3 scaled(const btVector3& s) const
+ {
+ return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
+ m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
+ m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
+ }
+
+ /**@brief Return the determinant of the matrix */
+ btScalar determinant() const;
+ /**@brief Return the adjoint of the matrix */
+ btMatrix3x3 adjoint() const;
+ /**@brief Return the matrix with all values non negative */
+ btMatrix3x3 absolute() const;
+ /**@brief Return the transpose of the matrix */
+ btMatrix3x3 transpose() const;
+ /**@brief Return the inverse of the matrix */
+ btMatrix3x3 inverse() const;
+
+ btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
+ btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
+
+ SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const
+ {
+ return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
+ }
+ SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const
+ {
+ return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
+ }
+ SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const
+ {
+ return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
+ }
+
+
+ /**@brief diagonalizes this matrix by the Jacobi method.
+ * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
+ * coordinate system, i.e., old_this = rot * new_this * rot^T.
+ * @param threshold See iteration
+ * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied
+ * by the sum of the absolute values of the diagonal, or when maxSteps have been executed.
+ *
+ * Note that this matrix is assumed to be symmetric.
+ */
+ void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
+ {
+ rot.setIdentity();
+ for (int step = maxSteps; step > 0; step--)
+ {
+ // find off-diagonal element [p][q] with largest magnitude
+ int p = 0;
+ int q = 1;
+ int r = 2;
+ btScalar max = btFabs(m_el[0][1]);
+ btScalar v = btFabs(m_el[0][2]);
+ if (v > max)
+ {
+ q = 2;
+ r = 1;
+ max = v;
+ }
+ v = btFabs(m_el[1][2]);
+ if (v > max)
+ {
+ p = 1;
+ q = 2;
+ r = 0;
+ max = v;
+ }
+
+ btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
+ if (max <= t)
+ {
+ if (max <= SIMD_EPSILON * t)
+ {
+ return;
+ }
+ step = 1;
+ }
+
+ // compute Jacobi rotation J which leads to a zero for element [p][q]
+ btScalar mpq = m_el[p][q];
+ btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
+ btScalar theta2 = theta * theta;
+ btScalar cos;
+ btScalar sin;
+ if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
+ {
+ t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
+ : 1 / (theta - btSqrt(1 + theta2));
+ cos = 1 / btSqrt(1 + t * t);
+ sin = cos * t;
+ }
+ else
+ {
+ // approximation for large theta-value, i.e., a nearly diagonal matrix
+ t = 1 / (theta * (2 + btScalar(0.5) / theta2));
+ cos = 1 - btScalar(0.5) * t * t;
+ sin = cos * t;
+ }
+
+ // apply rotation to matrix (this = J^T * this * J)
+ m_el[p][q] = m_el[q][p] = 0;
+ m_el[p][p] -= t * mpq;
+ m_el[q][q] += t * mpq;
+ btScalar mrp = m_el[r][p];
+ btScalar mrq = m_el[r][q];
+ m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
+ m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
+
+ // apply rotation to rot (rot = rot * J)
+ for (int i = 0; i < 3; i++)
+ {
+ btVector3& row = rot[i];
+ mrp = row[p];
+ mrq = row[q];
+ row[p] = cos * mrp - sin * mrq;
+ row[q] = cos * mrq + sin * mrp;
+ }
+ }
+ }
+
+
+
+ protected:
+ /**@brief Calculate the matrix cofactor
+ * @param r1 The first row to use for calculating the cofactor
+ * @param c1 The first column to use for calculating the cofactor
+ * @param r1 The second row to use for calculating the cofactor
+ * @param c1 The second column to use for calculating the cofactor
+ * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
+ */
+ btScalar cofac(int r1, int c1, int r2, int c2) const
+ {
+ return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
+ }
+ ///Data storage for the matrix, each vector is a row of the matrix
+ btVector3 m_el[3];
+ };
+
+ SIMD_FORCE_INLINE btMatrix3x3&
+ btMatrix3x3::operator*=(const btMatrix3x3& m)
+ {
+ setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
+ m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
+ m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
+ return *this;
+ }
+
+ SIMD_FORCE_INLINE btScalar
+ btMatrix3x3::determinant() const
+ {
+ return btTriple((*this)[0], (*this)[1], (*this)[2]);
+ }
+
+
+ SIMD_FORCE_INLINE btMatrix3x3
+ btMatrix3x3::absolute() const
+ {
+ return btMatrix3x3(
+ btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
+ btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
+ btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
+ }
+
+ SIMD_FORCE_INLINE btMatrix3x3
+ btMatrix3x3::transpose() const
+ {
+ return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
+ m_el[0].y(), m_el[1].y(), m_el[2].y(),
+ m_el[0].z(), m_el[1].z(), m_el[2].z());
+ }
+
+ SIMD_FORCE_INLINE btMatrix3x3
+ btMatrix3x3::adjoint() const
+ {
+ return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
+ cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
+ cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
+ }
+
+ SIMD_FORCE_INLINE btMatrix3x3
+ btMatrix3x3::inverse() const
+ {
+ btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
+ btScalar det = (*this)[0].dot(co);
+ btFullAssert(det != btScalar(0.0));
+ btScalar s = btScalar(1.0) / det;
+ return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
+ co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
+ co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
+ }
+
+ SIMD_FORCE_INLINE btMatrix3x3
+ btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
+ {
+ return btMatrix3x3(
+ m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
+ m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
+ m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
+ m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
+ m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
+ m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
+ m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
+ m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
+ m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
+ }
+
+ SIMD_FORCE_INLINE btMatrix3x3
+ btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
+ {
+ return btMatrix3x3(
+ m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
+ m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
+ m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
+
+ }
+
+ SIMD_FORCE_INLINE btVector3
+ operator*(const btMatrix3x3& m, const btVector3& v)
+ {
+ return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
+ }
+
+
+ SIMD_FORCE_INLINE btVector3
+ operator*(const btVector3& v, const btMatrix3x3& m)
+ {
+ return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
+ }
+
+ SIMD_FORCE_INLINE btMatrix3x3
+ operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
+ {
+ return btMatrix3x3(
+ m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
+ m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
+ m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
+ }
+
+/*
+ SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
+ return btMatrix3x3(
+ m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
+ m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
+ m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
+ m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
+ m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
+ m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
+ m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
+ m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
+ m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
+}
+*/
+
+/**@brief Equality operator between two matrices
+ * It will test all elements are equal. */
+SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
+{
+ return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
+ m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
+ m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
+}
+
+#endif
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btMinMax.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btMinMax.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btMinMax.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btMinMax.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,69 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef GEN_MINMAX_H
+#define GEN_MINMAX_H
+
+template <class T>
+SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b)
+{
+ return a < b ? a : b ;
+}
+
+template <class T>
+SIMD_FORCE_INLINE const T& btMax(const T& a, const T& b)
+{
+ return a > b ? a : b;
+}
+
+template <class T>
+SIMD_FORCE_INLINE const T& GEN_clamped(const T& a, const T& lb, const T& ub)
+{
+ return a < lb ? lb : (ub < a ? ub : a);
+}
+
+template <class T>
+SIMD_FORCE_INLINE void btSetMin(T& a, const T& b)
+{
+ if (b < a)
+ {
+ a = b;
+ }
+}
+
+template <class T>
+SIMD_FORCE_INLINE void btSetMax(T& a, const T& b)
+{
+ if (a < b)
+ {
+ a = b;
+ }
+}
+
+template <class T>
+SIMD_FORCE_INLINE void GEN_clamp(T& a, const T& lb, const T& ub)
+{
+ if (a < lb)
+ {
+ a = lb;
+ }
+ else if (ub < a)
+ {
+ a = ub;
+ }
+}
+
+#endif
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btMotionState.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btMotionState.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btMotionState.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btMotionState.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,40 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_MOTIONSTATE_H
+#define BT_MOTIONSTATE_H
+
+#include "btTransform.h"
+
+///The btMotionState interface class allows the dynamics world to synchronize and interpolate the updated world transforms with graphics
+///For optimizations, potentially only moving objects get synchronized (using setWorldPosition/setWorldOrientation)
+class btMotionState
+{
+ public:
+
+ virtual ~btMotionState()
+ {
+
+ }
+
+ virtual void getWorldTransform(btTransform& worldTrans ) const =0;
+
+ //Bullet only calls the update of worldtransform for active objects
+ virtual void setWorldTransform(const btTransform& worldTrans)=0;
+
+
+};
+
+#endif //BT_MOTIONSTATE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btPoolAllocator.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btPoolAllocator.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btPoolAllocator.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btPoolAllocator.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,102 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef _BT_POOL_ALLOCATOR_H
+#define _BT_POOL_ALLOCATOR_H
+
+#include "btScalar.h"
+#include "btAlignedAllocator.h"
+
+///The btPoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately.
+class btPoolAllocator
+{
+ int m_elemSize;
+ int m_maxElements;
+ int m_freeCount;
+ void* m_firstFree;
+ unsigned char* m_pool;
+
+public:
+
+ btPoolAllocator(int elemSize, int maxElements)
+ :m_elemSize(elemSize),
+ m_maxElements(maxElements)
+ {
+ m_pool = (unsigned char*) btAlignedAlloc( static_cast<unsigned int>(m_elemSize*m_maxElements),16);
+
+ unsigned char* p = m_pool;
+ m_firstFree = p;
+ m_freeCount = m_maxElements;
+ int count = m_maxElements;
+ while (--count) {
+ *(void**)p = (p + m_elemSize);
+ p += m_elemSize;
+ }
+ *(void**)p = 0;
+ }
+
+ ~btPoolAllocator()
+ {
+ btAlignedFree( m_pool);
+ }
+
+ int getFreeCount() const
+ {
+ return m_freeCount;
+ }
+
+ void* allocate(int size)
+ {
+ // release mode fix
+ (void)size;
+ btAssert(!size || size<=m_elemSize);
+ btAssert(m_freeCount>0);
+ void* result = m_firstFree;
+ m_firstFree = *(void**)m_firstFree;
+ --m_freeCount;
+ return result;
+ }
+
+ bool validPtr(void* ptr)
+ {
+ if (ptr) {
+ if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize))
+ {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ void freeMemory(void* ptr)
+ {
+ if (ptr) {
+ btAssert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
+
+ *(void**)ptr = m_firstFree;
+ m_firstFree = ptr;
+ ++m_freeCount;
+ }
+ }
+
+ int getElementSize() const
+ {
+ return m_elemSize;
+ }
+
+
+};
+
+#endif //_BT_POOL_ALLOCATOR_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btQuadWord.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btQuadWord.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btQuadWord.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btQuadWord.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,180 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef SIMD_QUADWORD_H
+#define SIMD_QUADWORD_H
+
+#include "btScalar.h"
+#include "btMinMax.h"
+
+
+#if defined (__CELLOS_LV2) && defined (__SPU__)
+#include <altivec.h>
+#endif
+
+/**@brief The btQuadWord class is base class for btVector3 and btQuaternion.
+ * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
+ */
+#ifndef USE_LIBSPE2
+ATTRIBUTE_ALIGNED16(class) btQuadWord
+#else
+class btQuadWord
+#endif
+{
+protected:
+
+#if defined (__SPU__) && defined (__CELLOS_LV2__)
+ union {
+ vec_float4 mVec128;
+ btScalar m_floats[4];
+ };
+public:
+ vec_float4 get128() const
+ {
+ return mVec128;
+ }
+protected:
+#else //__CELLOS_LV2__ __SPU__
+ btScalar m_floats[4];
+#endif //__CELLOS_LV2__ __SPU__
+
+ public:
+
+
+ /**@brief Return the x value */
+ SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
+ /**@brief Return the y value */
+ SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
+ /**@brief Return the z value */
+ SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
+ /**@brief Set the x value */
+ SIMD_FORCE_INLINE void setX(btScalar x) { m_floats[0] = x;};
+ /**@brief Set the y value */
+ SIMD_FORCE_INLINE void setY(btScalar y) { m_floats[1] = y;};
+ /**@brief Set the z value */
+ SIMD_FORCE_INLINE void setZ(btScalar z) { m_floats[2] = z;};
+ /**@brief Set the w value */
+ SIMD_FORCE_INLINE void setW(btScalar w) { m_floats[3] = w;};
+ /**@brief Return the x value */
+ SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
+ /**@brief Return the y value */
+ SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
+ /**@brief Return the z value */
+ SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
+ /**@brief Return the w value */
+ SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
+
+ //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; }
+ //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
+ ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
+ SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; }
+ SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; }
+
+ SIMD_FORCE_INLINE bool operator==(const btQuadWord& other) const
+ {
+ return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0]));
+ }
+
+ SIMD_FORCE_INLINE bool operator!=(const btQuadWord& other) const
+ {
+ return !(*this == other);
+ }
+
+ /**@brief Set x,y,z and zero w
+ * @param x Value of x
+ * @param y Value of y
+ * @param z Value of z
+ */
+ SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z)
+ {
+ m_floats[0]=x;
+ m_floats[1]=y;
+ m_floats[2]=z;
+ m_floats[3] = 0.f;
+ }
+
+/* void getValue(btScalar *m) const
+ {
+ m[0] = m_floats[0];
+ m[1] = m_floats[1];
+ m[2] = m_floats[2];
+ }
+*/
+/**@brief Set the values
+ * @param x Value of x
+ * @param y Value of y
+ * @param z Value of z
+ * @param w Value of w
+ */
+ SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
+ {
+ m_floats[0]=x;
+ m_floats[1]=y;
+ m_floats[2]=z;
+ m_floats[3]=w;
+ }
+ /**@brief No initialization constructor */
+ SIMD_FORCE_INLINE btQuadWord()
+ // :m_floats[0](btScalar(0.)),m_floats[1](btScalar(0.)),m_floats[2](btScalar(0.)),m_floats[3](btScalar(0.))
+ {
+ }
+
+ /**@brief Three argument constructor (zeros w)
+ * @param x Value of x
+ * @param y Value of y
+ * @param z Value of z
+ */
+ SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z)
+ {
+ m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = 0.0f;
+ }
+
+/**@brief Initializing constructor
+ * @param x Value of x
+ * @param y Value of y
+ * @param z Value of z
+ * @param w Value of w
+ */
+ SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
+ {
+ m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = w;
+ }
+
+ /**@brief Set each element to the max of the current values and the values of another btQuadWord
+ * @param other The other btQuadWord to compare with
+ */
+ SIMD_FORCE_INLINE void setMax(const btQuadWord& other)
+ {
+ btSetMax(m_floats[0], other.m_floats[0]);
+ btSetMax(m_floats[1], other.m_floats[1]);
+ btSetMax(m_floats[2], other.m_floats[2]);
+ btSetMax(m_floats[3], other.m_floats[3]);
+ }
+ /**@brief Set each element to the min of the current values and the values of another btQuadWord
+ * @param other The other btQuadWord to compare with
+ */
+ SIMD_FORCE_INLINE void setMin(const btQuadWord& other)
+ {
+ btSetMin(m_floats[0], other.m_floats[0]);
+ btSetMin(m_floats[1], other.m_floats[1]);
+ btSetMin(m_floats[2], other.m_floats[2]);
+ btSetMin(m_floats[3], other.m_floats[3]);
+ }
+
+
+
+};
+
+#endif //SIMD_QUADWORD_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btQuaternion.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btQuaternion.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btQuaternion.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btQuaternion.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,426 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef SIMD__QUATERNION_H_
+#define SIMD__QUATERNION_H_
+
+
+#include "btVector3.h"
+#include "btQuadWord.h"
+
+/**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */
+class btQuaternion : public btQuadWord {
+public:
+ /**@brief No initialization constructor */
+ btQuaternion() {}
+
+ // template <typename btScalar>
+ // explicit Quaternion(const btScalar *v) : Tuple4<btScalar>(v) {}
+ /**@brief Constructor from scalars */
+ btQuaternion(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w)
+ : btQuadWord(x, y, z, w)
+ {}
+ /**@brief Axis angle Constructor
+ * @param axis The axis which the rotation is around
+ * @param angle The magnitude of the rotation around the angle (Radians) */
+ btQuaternion(const btVector3& axis, const btScalar& angle)
+ {
+ setRotation(axis, angle);
+ }
+ /**@brief Constructor from Euler angles
+ * @param yaw Angle around Y unless BT_EULER_DEFAULT_ZYX defined then Z
+ * @param pitch Angle around X unless BT_EULER_DEFAULT_ZYX defined then Y
+ * @param roll Angle around Z unless BT_EULER_DEFAULT_ZYX defined then X */
+ btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
+ {
+#ifndef BT_EULER_DEFAULT_ZYX
+ setEuler(yaw, pitch, roll);
+#else
+ setEulerZYX(yaw, pitch, roll);
+#endif
+ }
+ /**@brief Set the rotation using axis angle notation
+ * @param axis The axis around which to rotate
+ * @param angle The magnitude of the rotation in Radians */
+ void setRotation(const btVector3& axis, const btScalar& angle)
+ {
+ btScalar d = axis.length();
+ btAssert(d != btScalar(0.0));
+ btScalar s = btSin(angle * btScalar(0.5)) / d;
+ setValue(axis.x() * s, axis.y() * s, axis.z() * s,
+ btCos(angle * btScalar(0.5)));
+ }
+ /**@brief Set the quaternion using Euler angles
+ * @param yaw Angle around Y
+ * @param pitch Angle around X
+ * @param roll Angle around Z */
+ void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
+ {
+ btScalar halfYaw = btScalar(yaw) * btScalar(0.5);
+ btScalar halfPitch = btScalar(pitch) * btScalar(0.5);
+ btScalar halfRoll = btScalar(roll) * btScalar(0.5);
+ btScalar cosYaw = btCos(halfYaw);
+ btScalar sinYaw = btSin(halfYaw);
+ btScalar cosPitch = btCos(halfPitch);
+ btScalar sinPitch = btSin(halfPitch);
+ btScalar cosRoll = btCos(halfRoll);
+ btScalar sinRoll = btSin(halfRoll);
+ setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
+ cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
+ sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
+ cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
+ }
+ /**@brief Set the quaternion using euler angles
+ * @param yaw Angle around Z
+ * @param pitch Angle around Y
+ * @param roll Angle around X */
+ void setEulerZYX(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
+ {
+ btScalar halfYaw = btScalar(yaw) * btScalar(0.5);
+ btScalar halfPitch = btScalar(pitch) * btScalar(0.5);
+ btScalar halfRoll = btScalar(roll) * btScalar(0.5);
+ btScalar cosYaw = btCos(halfYaw);
+ btScalar sinYaw = btSin(halfYaw);
+ btScalar cosPitch = btCos(halfPitch);
+ btScalar sinPitch = btSin(halfPitch);
+ btScalar cosRoll = btCos(halfRoll);
+ btScalar sinRoll = btSin(halfRoll);
+ setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
+ cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
+ cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
+ cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
+ }
+ /**@brief Add two quaternions
+ * @param q The quaternion to add to this one */
+ SIMD_FORCE_INLINE btQuaternion& operator+=(const btQuaternion& q)
+ {
+ m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3];
+ return *this;
+ }
+
+ /**@brief Subtract out a quaternion
+ * @param q The quaternion to subtract from this one */
+ btQuaternion& operator-=(const btQuaternion& q)
+ {
+ m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3];
+ return *this;
+ }
+
+ /**@brief Scale this quaternion
+ * @param s The scalar to scale by */
+ btQuaternion& operator*=(const btScalar& s)
+ {
+ m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s;
+ return *this;
+ }
+
+ /**@brief Multiply this quaternion by q on the right
+ * @param q The other quaternion
+ * Equivilant to this = this * q */
+ btQuaternion& operator*=(const btQuaternion& q)
+ {
+ setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
+ m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
+ m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
+ m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
+ return *this;
+ }
+ /**@brief Return the dot product between this quaternion and another
+ * @param q The other quaternion */
+ btScalar dot(const btQuaternion& q) const
+ {
+ return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3];
+ }
+
+ /**@brief Return the length squared of the quaternion */
+ btScalar length2() const
+ {
+ return dot(*this);
+ }
+
+ /**@brief Return the length of the quaternion */
+ btScalar length() const
+ {
+ return btSqrt(length2());
+ }
+
+ /**@brief Normalize the quaternion
+ * Such that x^2 + y^2 + z^2 +w^2 = 1 */
+ btQuaternion& normalize()
+ {
+ return *this /= length();
+ }
+
+ /**@brief Return a scaled version of this quaternion
+ * @param s The scale factor */
+ SIMD_FORCE_INLINE btQuaternion
+ operator*(const btScalar& s) const
+ {
+ return btQuaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
+ }
+
+
+ /**@brief Return an inversely scaled versionof this quaternion
+ * @param s The inverse scale factor */
+ btQuaternion operator/(const btScalar& s) const
+ {
+ btAssert(s != btScalar(0.0));
+ return *this * (btScalar(1.0) / s);
+ }
+
+ /**@brief Inversely scale this quaternion
+ * @param s The scale factor */
+ btQuaternion& operator/=(const btScalar& s)
+ {
+ btAssert(s != btScalar(0.0));
+ return *this *= btScalar(1.0) / s;
+ }
+
+ /**@brief Return a normalized version of this quaternion */
+ btQuaternion normalized() const
+ {
+ return *this / length();
+ }
+ /**@brief Return the angle between this quaternion and the other
+ * @param q The other quaternion */
+ btScalar angle(const btQuaternion& q) const
+ {
+ btScalar s = btSqrt(length2() * q.length2());
+ btAssert(s != btScalar(0.0));
+ return btAcos(dot(q) / s);
+ }
+ /**@brief Return the angle of rotation represented by this quaternion */
+ btScalar getAngle() const
+ {
+ btScalar s = btScalar(2.) * btAcos(m_floats[3]);
+ return s;
+ }
+
+ /**@brief Return the axis of the rotation represented by this quaternion */
+ btVector3 getAxis() const
+ {
+ btScalar s_squared = btScalar(1.) - btPow(m_floats[3], btScalar(2.));
+ if (s_squared < btScalar(10.) * SIMD_EPSILON) //Check for divide by zero
+ return btVector3(1.0, 0.0, 0.0); // Arbitrary
+ btScalar s = btSqrt(s_squared);
+ return btVector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s);
+ }
+
+ /**@brief Return the inverse of this quaternion */
+ btQuaternion inverse() const
+ {
+ return btQuaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
+ }
+
+ /**@brief Return the sum of this quaternion and the other
+ * @param q2 The other quaternion */
+ SIMD_FORCE_INLINE btQuaternion
+ operator+(const btQuaternion& q2) const
+ {
+ const btQuaternion& q1 = *this;
+ return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
+ }
+
+ /**@brief Return the difference between this quaternion and the other
+ * @param q2 The other quaternion */
+ SIMD_FORCE_INLINE btQuaternion
+ operator-(const btQuaternion& q2) const
+ {
+ const btQuaternion& q1 = *this;
+ return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
+ }
+
+ /**@brief Return the negative of this quaternion
+ * This simply negates each element */
+ SIMD_FORCE_INLINE btQuaternion operator-() const
+ {
+ const btQuaternion& q2 = *this;
+ return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]);
+ }
+ /**@todo document this and it's use */
+ SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const
+ {
+ btQuaternion diff,sum;
+ diff = *this - qd;
+ sum = *this + qd;
+ if( diff.dot(diff) > sum.dot(sum) )
+ return qd;
+ return (-qd);
+ }
+
+ /**@todo document this and it's use */
+ SIMD_FORCE_INLINE btQuaternion nearest( const btQuaternion& qd) const
+ {
+ btQuaternion diff,sum;
+ diff = *this - qd;
+ sum = *this + qd;
+ if( diff.dot(diff) < sum.dot(sum) )
+ return qd;
+ return (-qd);
+ }
+
+
+ /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
+ * @param q The other quaternion to interpolate with
+ * @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q.
+ * Slerp interpolates assuming constant velocity. */
+ btQuaternion slerp(const btQuaternion& q, const btScalar& t) const
+ {
+ btScalar theta = angle(q);
+ if (theta != btScalar(0.0))
+ {
+ btScalar d = btScalar(1.0) / btSin(theta);
+ btScalar s0 = btSin((btScalar(1.0) - t) * theta);
+ btScalar s1 = btSin(t * theta);
+ return btQuaternion((m_floats[0] * s0 + q.x() * s1) * d,
+ (m_floats[1] * s0 + q.y() * s1) * d,
+ (m_floats[2] * s0 + q.z() * s1) * d,
+ (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
+ }
+ else
+ {
+ return *this;
+ }
+ }
+
+ static const btQuaternion& getIdentity()
+ {
+ static const btQuaternion identityQuat(btScalar(0.),btScalar(0.),btScalar(0.),btScalar(1.));
+ return identityQuat;
+ }
+
+ SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; }
+
+
+};
+
+
+/**@brief Return the negative of a quaternion */
+SIMD_FORCE_INLINE btQuaternion
+operator-(const btQuaternion& q)
+{
+ return btQuaternion(-q.x(), -q.y(), -q.z(), -q.w());
+}
+
+
+
+/**@brief Return the product of two quaternions */
+SIMD_FORCE_INLINE btQuaternion
+operator*(const btQuaternion& q1, const btQuaternion& q2) {
+ return btQuaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
+ q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
+ q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
+ q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
+}
+
+SIMD_FORCE_INLINE btQuaternion
+operator*(const btQuaternion& q, const btVector3& w)
+{
+ return btQuaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
+ q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
+ q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
+ -q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
+}
+
+SIMD_FORCE_INLINE btQuaternion
+operator*(const btVector3& w, const btQuaternion& q)
+{
+ return btQuaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
+ w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
+ w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
+ -w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
+}
+
+/**@brief Calculate the dot product between two quaternions */
+SIMD_FORCE_INLINE btScalar
+dot(const btQuaternion& q1, const btQuaternion& q2)
+{
+ return q1.dot(q2);
+}
+
+
+/**@brief Return the length of a quaternion */
+SIMD_FORCE_INLINE btScalar
+length(const btQuaternion& q)
+{
+ return q.length();
+}
+
+/**@brief Return the angle between two quaternions*/
+SIMD_FORCE_INLINE btScalar
+angle(const btQuaternion& q1, const btQuaternion& q2)
+{
+ return q1.angle(q2);
+}
+
+/**@brief Return the inverse of a quaternion*/
+SIMD_FORCE_INLINE btQuaternion
+inverse(const btQuaternion& q)
+{
+ return q.inverse();
+}
+
+/**@brief Return the result of spherical linear interpolation betwen two quaternions
+ * @param q1 The first quaternion
+ * @param q2 The second quaternion
+ * @param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2
+ * Slerp assumes constant velocity between positions. */
+SIMD_FORCE_INLINE btQuaternion
+slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t)
+{
+ return q1.slerp(q2, t);
+}
+
+SIMD_FORCE_INLINE btVector3
+quatRotate(const btQuaternion& rotation, const btVector3& v)
+{
+ btQuaternion q = rotation * v;
+ q *= rotation.inverse();
+ return btVector3(q.getX(),q.getY(),q.getZ());
+}
+
+SIMD_FORCE_INLINE btQuaternion
+shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
+{
+ btVector3 c = v0.cross(v1);
+ btScalar d = v0.dot(v1);
+
+ if (d < -1.0 + SIMD_EPSILON)
+ {
+ btVector3 n,unused;
+ btPlaneSpace1(v0,n,unused);
+ return btQuaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
+ }
+
+ btScalar s = btSqrt((1.0f + d) * 2.0f);
+ btScalar rs = 1.0f / s;
+
+ return btQuaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
+}
+
+SIMD_FORCE_INLINE btQuaternion
+shortestArcQuatNormalize2(btVector3& v0,btVector3& v1)
+{
+ v0.normalize();
+ v1.normalize();
+ return shortestArcQuat(v0,v1);
+}
+
+#endif
+
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btQuickprof.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btQuickprof.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btQuickprof.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btQuickprof.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,370 @@
+
+/***************************************************************************************************
+**
+** Real-Time Hierarchical Profiling for Game Programming Gems 3
+**
+** by Greg Hjelstrom & Byon Garrabrant
+**
+***************************************************************************************************/
+
+// Credits: The Clock class was inspired by the Timer classes in
+// Ogre (www.ogre3d.org).
+
+
+
+#ifndef QUICK_PROF_H
+#define QUICK_PROF_H
+
+//To disable built-in profiling, please comment out next line
+//#define BT_NO_PROFILE 1
+#ifndef BT_NO_PROFILE
+
+#include "btScalar.h"
+#include "LinearMath/btAlignedAllocator.h"
+#include <new>
+
+
+
+
+//if you don't need btClock, you can comment next line
+#define USE_BT_CLOCK 1
+
+#ifdef USE_BT_CLOCK
+#ifdef __CELLOS_LV2__
+#include <sys/sys_time.h>
+#include <sys/time_util.h>
+#include <stdio.h>
+#endif
+
+#if defined (SUNOS) || defined (__SUNOS__)
+#include <stdio.h>
+#endif
+
+#if defined(WIN32) || defined(_WIN32)
+
+#define USE_WINDOWS_TIMERS
+#define WIN32_LEAN_AND_MEAN
+#define NOWINRES
+#define NOMCX
+#define NOIME
+#ifdef _XBOX
+#include <Xtl.h>
+#else
+#include <windows.h>
+#endif
+#include <time.h>
+
+#else
+#include <sys/time.h>
+#endif
+
+#define mymin(a,b) (a > b ? a : b)
+
+///The btClock is a portable basic clock that measures accurate time in seconds, use for profiling.
+class btClock
+{
+public:
+ btClock()
+ {
+#ifdef USE_WINDOWS_TIMERS
+ QueryPerformanceFrequency(&mClockFrequency);
+#endif
+ reset();
+ }
+
+ ~btClock()
+ {
+ }
+
+ /// Resets the initial reference time.
+ void reset()
+ {
+#ifdef USE_WINDOWS_TIMERS
+ QueryPerformanceCounter(&mStartTime);
+ mStartTick = GetTickCount();
+ mPrevElapsedTime = 0;
+#else
+#ifdef __CELLOS_LV2__
+
+ typedef uint64_t ClockSize;
+ ClockSize newTime;
+ //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+ SYS_TIMEBASE_GET( newTime );
+ mStartTime = newTime;
+#else
+ gettimeofday(&mStartTime, 0);
+#endif
+
+#endif
+ }
+
+ /// Returns the time in ms since the last call to reset or since
+ /// the btClock was created.
+ unsigned long int getTimeMilliseconds()
+ {
+#ifdef USE_WINDOWS_TIMERS
+ LARGE_INTEGER currentTime;
+ QueryPerformanceCounter(¤tTime);
+ LONGLONG elapsedTime = currentTime.QuadPart -
+ mStartTime.QuadPart;
+
+ // Compute the number of millisecond ticks elapsed.
+ unsigned long msecTicks = (unsigned long)(1000 * elapsedTime /
+ mClockFrequency.QuadPart);
+
+ // Check for unexpected leaps in the Win32 performance counter.
+ // (This is caused by unexpected data across the PCI to ISA
+ // bridge, aka south bridge. See Microsoft KB274323.)
+ unsigned long elapsedTicks = GetTickCount() - mStartTick;
+ signed long msecOff = (signed long)(msecTicks - elapsedTicks);
+ if (msecOff < -100 || msecOff > 100)
+ {
+ // Adjust the starting time forwards.
+ LONGLONG msecAdjustment = mymin(msecOff *
+ mClockFrequency.QuadPart / 1000, elapsedTime -
+ mPrevElapsedTime);
+ mStartTime.QuadPart += msecAdjustment;
+ elapsedTime -= msecAdjustment;
+
+ // Recompute the number of millisecond ticks elapsed.
+ msecTicks = (unsigned long)(1000 * elapsedTime /
+ mClockFrequency.QuadPart);
+ }
+
+ // Store the current elapsed time for adjustments next time.
+ mPrevElapsedTime = elapsedTime;
+
+ return msecTicks;
+#else
+
+#ifdef __CELLOS_LV2__
+ uint64_t freq=sys_time_get_timebase_frequency();
+ double dFreq=((double) freq) / 1000.0;
+ typedef uint64_t ClockSize;
+ ClockSize newTime;
+ SYS_TIMEBASE_GET( newTime );
+ //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+
+ return (unsigned long int)((double(newTime-mStartTime)) / dFreq);
+#else
+
+ struct timeval currentTime;
+ gettimeofday(¤tTime, 0);
+ return (currentTime.tv_sec - mStartTime.tv_sec) * 1000 +
+ (currentTime.tv_usec - mStartTime.tv_usec) / 1000;
+#endif //__CELLOS_LV2__
+#endif
+ }
+
+ /// Returns the time in us since the last call to reset or since
+ /// the Clock was created.
+ unsigned long int getTimeMicroseconds()
+ {
+#ifdef USE_WINDOWS_TIMERS
+ LARGE_INTEGER currentTime;
+ QueryPerformanceCounter(¤tTime);
+ LONGLONG elapsedTime = currentTime.QuadPart -
+ mStartTime.QuadPart;
+
+ // Compute the number of millisecond ticks elapsed.
+ unsigned long msecTicks = (unsigned long)(1000 * elapsedTime /
+ mClockFrequency.QuadPart);
+
+ // Check for unexpected leaps in the Win32 performance counter.
+ // (This is caused by unexpected data across the PCI to ISA
+ // bridge, aka south bridge. See Microsoft KB274323.)
+ unsigned long elapsedTicks = GetTickCount() - mStartTick;
+ signed long msecOff = (signed long)(msecTicks - elapsedTicks);
+ if (msecOff < -100 || msecOff > 100)
+ {
+ // Adjust the starting time forwards.
+ LONGLONG msecAdjustment = mymin(msecOff *
+ mClockFrequency.QuadPart / 1000, elapsedTime -
+ mPrevElapsedTime);
+ mStartTime.QuadPart += msecAdjustment;
+ elapsedTime -= msecAdjustment;
+ }
+
+ // Store the current elapsed time for adjustments next time.
+ mPrevElapsedTime = elapsedTime;
+
+ // Convert to microseconds.
+ unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime /
+ mClockFrequency.QuadPart);
+
+ return usecTicks;
+#else
+
+#ifdef __CELLOS_LV2__
+ uint64_t freq=sys_time_get_timebase_frequency();
+ double dFreq=((double) freq)/ 1000000.0;
+ typedef uint64_t ClockSize;
+ ClockSize newTime;
+ //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+ SYS_TIMEBASE_GET( newTime );
+
+ return (unsigned long int)((double(newTime-mStartTime)) / dFreq);
+#else
+
+ struct timeval currentTime;
+ gettimeofday(¤tTime, 0);
+ return (currentTime.tv_sec - mStartTime.tv_sec) * 1000000 +
+ (currentTime.tv_usec - mStartTime.tv_usec);
+#endif//__CELLOS_LV2__
+#endif
+ }
+
+private:
+#ifdef USE_WINDOWS_TIMERS
+ LARGE_INTEGER mClockFrequency;
+ DWORD mStartTick;
+ LONGLONG mPrevElapsedTime;
+ LARGE_INTEGER mStartTime;
+#else
+#ifdef __CELLOS_LV2__
+ uint64_t mStartTime;
+#else
+ struct timeval mStartTime;
+#endif
+#endif //__CELLOS_LV2__
+
+};
+
+#endif //USE_BT_CLOCK
+
+
+
+
+///A node in the Profile Hierarchy Tree
+class CProfileNode {
+
+public:
+ CProfileNode( const char * name, CProfileNode * parent );
+ ~CProfileNode( void );
+
+ CProfileNode * Get_Sub_Node( const char * name );
+
+ CProfileNode * Get_Parent( void ) { return Parent; }
+ CProfileNode * Get_Sibling( void ) { return Sibling; }
+ CProfileNode * Get_Child( void ) { return Child; }
+
+ void CleanupMemory();
+ void Reset( void );
+ void Call( void );
+ bool Return( void );
+
+ const char * Get_Name( void ) { return Name; }
+ int Get_Total_Calls( void ) { return TotalCalls; }
+ float Get_Total_Time( void ) { return TotalTime; }
+
+protected:
+
+ const char * Name;
+ int TotalCalls;
+ float TotalTime;
+ unsigned long int StartTime;
+ int RecursionCounter;
+
+ CProfileNode * Parent;
+ CProfileNode * Child;
+ CProfileNode * Sibling;
+};
+
+///An iterator to navigate through the tree
+class CProfileIterator
+{
+public:
+ // Access all the children of the current parent
+ void First(void);
+ void Next(void);
+ bool Is_Done(void);
+ bool Is_Root(void) { return (CurrentParent->Get_Parent() == 0); }
+
+ void Enter_Child( int index ); // Make the given child the new parent
+ void Enter_Largest_Child( void ); // Make the largest child the new parent
+ void Enter_Parent( void ); // Make the current parent's parent the new parent
+
+ // Access the current child
+ const char * Get_Current_Name( void ) { return CurrentChild->Get_Name(); }
+ int Get_Current_Total_Calls( void ) { return CurrentChild->Get_Total_Calls(); }
+ float Get_Current_Total_Time( void ) { return CurrentChild->Get_Total_Time(); }
+
+ // Access the current parent
+ const char * Get_Current_Parent_Name( void ) { return CurrentParent->Get_Name(); }
+ int Get_Current_Parent_Total_Calls( void ) { return CurrentParent->Get_Total_Calls(); }
+ float Get_Current_Parent_Total_Time( void ) { return CurrentParent->Get_Total_Time(); }
+
+protected:
+
+ CProfileNode * CurrentParent;
+ CProfileNode * CurrentChild;
+
+ CProfileIterator( CProfileNode * start );
+ friend class CProfileManager;
+};
+
+
+///The Manager for the Profile system
+class CProfileManager {
+public:
+ static void Start_Profile( const char * name );
+ static void Stop_Profile( void );
+
+ static void CleanupMemory(void)
+ {
+ Root.CleanupMemory();
+ }
+
+ static void Reset( void );
+ static void Increment_Frame_Counter( void );
+ static int Get_Frame_Count_Since_Reset( void ) { return FrameCounter; }
+ static float Get_Time_Since_Reset( void );
+
+ static CProfileIterator * Get_Iterator( void )
+ {
+
+ return new CProfileIterator( &Root );
+ }
+ static void Release_Iterator( CProfileIterator * iterator ) { delete ( iterator); }
+
+ static void dumpRecursive(CProfileIterator* profileIterator, int spacing);
+
+ static void dumpAll();
+
+private:
+ static CProfileNode Root;
+ static CProfileNode * CurrentNode;
+ static int FrameCounter;
+ static unsigned long int ResetTime;
+};
+
+
+///ProfileSampleClass is a simple way to profile a function's scope
+///Use the BT_PROFILE macro at the start of scope to time
+class CProfileSample {
+public:
+ CProfileSample( const char * name )
+ {
+ CProfileManager::Start_Profile( name );
+ }
+
+ ~CProfileSample( void )
+ {
+ CProfileManager::Stop_Profile();
+ }
+};
+
+
+#define BT_PROFILE( name ) CProfileSample __profile( name )
+
+#else
+
+#define BT_PROFILE( name )
+
+#endif //#ifndef BT_NO_PROFILE
+
+
+
+#endif //QUICK_PROF_H
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btRandom.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btRandom.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btRandom.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btRandom.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,42 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef GEN_RANDOM_H
+#define GEN_RANDOM_H
+
+#ifdef MT19937
+
+#include <limits.h>
+#include <mt19937.h>
+
+#define GEN_RAND_MAX UINT_MAX
+
+SIMD_FORCE_INLINE void GEN_srand(unsigned int seed) { init_genrand(seed); }
+SIMD_FORCE_INLINE unsigned int GEN_rand() { return genrand_int32(); }
+
+#else
+
+#include <stdlib.h>
+
+#define GEN_RAND_MAX RAND_MAX
+
+SIMD_FORCE_INLINE void GEN_srand(unsigned int seed) { srand(seed); }
+SIMD_FORCE_INLINE unsigned int GEN_rand() { return rand(); }
+
+#endif
+
+#endif
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btScalar.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btScalar.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btScalar.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btScalar.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,495 @@
+/*
+Copyright (c) 2003-2009 Erwin Coumans http://bullet.googlecode.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef SIMD___SCALAR_H
+#define SIMD___SCALAR_H
+
+#include <math.h>
+#include <stdlib.h>//size_t for MSVC 6.0
+#include <cstdlib>
+#include <cfloat>
+#include <float.h>
+
+/* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
+#define BT_BULLET_VERSION 275
+
+inline int btGetVersion()
+{
+ return BT_BULLET_VERSION;
+}
+
+#if defined(DEBUG) || defined (_DEBUG)
+#define BT_DEBUG
+#endif
+
+
+#ifdef WIN32
+
+ #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300)
+
+ #define SIMD_FORCE_INLINE inline
+ #define ATTRIBUTE_ALIGNED16(a) a
+ #define ATTRIBUTE_ALIGNED128(a) a
+ #else
+ //#define BT_HAS_ALIGNED_ALLOCATOR
+ #pragma warning(disable : 4324) // disable padding warning
+// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
+// #pragma warning(disable:4996) //Turn off warnings about deprecated C routines
+// #pragma warning(disable:4786) // Disable the "debug name too long" warning
+
+ #define SIMD_FORCE_INLINE __forceinline
+ #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
+ #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a
+ #ifdef _XBOX
+ #define BT_USE_VMX128
+
+ #include <ppcintrinsics.h>
+ #define BT_HAVE_NATIVE_FSEL
+ #define btFsel(a,b,c) __fsel((a),(b),(c))
+ #else
+
+#if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
+ #define BT_USE_SSE
+ #include <emmintrin.h>
+#endif
+
+ #endif//_XBOX
+
+ #endif //__MINGW32__
+
+ #include <assert.h>
+#ifdef BT_DEBUG
+ #define btAssert assert
+#else
+ #define btAssert(x)
+#endif
+ //btFullAssert is optional, slows down a lot
+ #define btFullAssert(x)
+
+ #define btLikely(_c) _c
+ #define btUnlikely(_c) _c
+
+#else
+
+#if defined (__CELLOS_LV2__)
+ #define SIMD_FORCE_INLINE inline
+ #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+ #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
+ #ifndef assert
+ #include <assert.h>
+ #endif
+#ifdef BT_DEBUG
+ #define btAssert assert
+#else
+ #define btAssert(x)
+#endif
+ //btFullAssert is optional, slows down a lot
+ #define btFullAssert(x)
+
+ #define btLikely(_c) _c
+ #define btUnlikely(_c) _c
+
+#else
+
+#ifdef USE_LIBSPE2
+
+ #define SIMD_FORCE_INLINE __inline
+ #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+ #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
+ #ifndef assert
+ #include <assert.h>
+ #endif
+#ifdef BT_DEBUG
+ #define btAssert assert
+#else
+ #define btAssert(x)
+#endif
+ //btFullAssert is optional, slows down a lot
+ #define btFullAssert(x)
+
+
+ #define btLikely(_c) __builtin_expect((_c), 1)
+ #define btUnlikely(_c) __builtin_expect((_c), 0)
+
+
+#else
+ //non-windows systems
+
+#if (defined (__APPLE__) && defined (__i386__) && (!defined (BT_USE_DOUBLE_PRECISION)))
+ #define BT_USE_SSE
+ #include <emmintrin.h>
+
+ #define SIMD_FORCE_INLINE inline
+///@todo: check out alignment methods for other platforms/compilers
+ #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+ #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
+ #ifndef assert
+ #include <assert.h>
+ #endif
+
+ #if defined(DEBUG) || defined (_DEBUG)
+ #define btAssert assert
+ #else
+ #define btAssert(x)
+ #endif
+
+ //btFullAssert is optional, slows down a lot
+ #define btFullAssert(x)
+ #define btLikely(_c) _c
+ #define btUnlikely(_c) _c
+
+#else
+
+ #define SIMD_FORCE_INLINE inline
+ ///@todo: check out alignment methods for other platforms/compilers
+ ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+ ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
+ #define ATTRIBUTE_ALIGNED16(a) a
+ #define ATTRIBUTE_ALIGNED128(a) a
+ #ifndef assert
+ #include <assert.h>
+ #endif
+
+#if defined(DEBUG) || defined (_DEBUG)
+ #define btAssert assert
+#else
+ #define btAssert(x)
+#endif
+
+ //btFullAssert is optional, slows down a lot
+ #define btFullAssert(x)
+ #define btLikely(_c) _c
+ #define btUnlikely(_c) _c
+#endif //__APPLE__
+
+#endif // LIBSPE2
+
+#endif //__CELLOS_LV2__
+#endif
+
+
+///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
+#if defined(BT_USE_DOUBLE_PRECISION)
+typedef double btScalar;
+//this number could be bigger in double precision
+#define BT_LARGE_FLOAT 1e30
+#else
+typedef float btScalar;
+//keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX
+#define BT_LARGE_FLOAT 1e18f
+#endif
+
+
+
+#define BT_DECLARE_ALIGNED_ALLOCATOR() \
+ SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \
+ SIMD_FORCE_INLINE void operator delete(void* ptr) { btAlignedFree(ptr); } \
+ SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \
+ SIMD_FORCE_INLINE void operator delete(void*, void*) { } \
+ SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \
+ SIMD_FORCE_INLINE void operator delete[](void* ptr) { btAlignedFree(ptr); } \
+ SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \
+ SIMD_FORCE_INLINE void operator delete[](void*, void*) { } \
+
+
+
+#if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS)
+
+SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); }
+SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); }
+SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); }
+SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); }
+SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); }
+SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acos(x); }
+SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asin(x); }
+SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); }
+SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); }
+SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); }
+SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); }
+SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); }
+SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); }
+
+#else
+
+SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
+{
+#ifdef USE_APPROXIMATION
+ double x, z, tempf;
+ unsigned long *tfptr = ((unsigned long *)&tempf) + 1;
+
+ tempf = y;
+ *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */
+ x = tempf;
+ z = y*btScalar(0.5); /* hoist out the /2 */
+ x = (btScalar(1.5)*x)-(x*x)*(x*z); /* iteration formula */
+ x = (btScalar(1.5)*x)-(x*x)*(x*z);
+ x = (btScalar(1.5)*x)-(x*x)*(x*z);
+ x = (btScalar(1.5)*x)-(x*x)*(x*z);
+ x = (btScalar(1.5)*x)-(x*x)*(x*z);
+ return x*y;
+#else
+ return sqrtf(y);
+#endif
+}
+SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); }
+SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); }
+SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); }
+SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
+SIMD_FORCE_INLINE btScalar btAcos(btScalar x) {
+ btAssert(x <= btScalar(1.));
+ return acosf(x);
+}
+SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asinf(x); }
+SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
+SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
+SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
+SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); }
+SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
+SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
+
+#endif
+
+#define SIMD_2_PI btScalar(6.283185307179586232)
+#define SIMD_PI (SIMD_2_PI * btScalar(0.5))
+#define SIMD_HALF_PI (SIMD_2_PI * btScalar(0.25))
+#define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0))
+#define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI)
+#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
+
+#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x)))) /* reciprocal square root */
+
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define SIMD_EPSILON DBL_EPSILON
+#define SIMD_INFINITY DBL_MAX
+#else
+#define SIMD_EPSILON FLT_EPSILON
+#define SIMD_INFINITY FLT_MAX
+#endif
+
+SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x)
+{
+ btScalar coeff_1 = SIMD_PI / 4.0f;
+ btScalar coeff_2 = 3.0f * coeff_1;
+ btScalar abs_y = btFabs(y);
+ btScalar angle;
+ if (x >= 0.0f) {
+ btScalar r = (x - abs_y) / (x + abs_y);
+ angle = coeff_1 - coeff_1 * r;
+ } else {
+ btScalar r = (x + abs_y) / (abs_y - x);
+ angle = coeff_2 - coeff_1 * r;
+ }
+ return (y < 0.0f) ? -angle : angle;
+}
+
+SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; }
+
+SIMD_FORCE_INLINE bool btEqual(btScalar a, btScalar eps) {
+ return (((a) <= eps) && !((a) < -eps));
+}
+SIMD_FORCE_INLINE bool btGreaterEqual (btScalar a, btScalar eps) {
+ return (!((a) <= eps));
+}
+
+
+SIMD_FORCE_INLINE int btIsNegative(btScalar x) {
+ return x < btScalar(0.0) ? 1 : 0;
+}
+
+SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; }
+SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; }
+
+#define BT_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
+
+#ifndef btFsel
+SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c)
+{
+ return a >= 0 ? b : c;
+}
+#endif
+#define btFsels(a,b,c) (btScalar)btFsel(a,b,c)
+
+
+SIMD_FORCE_INLINE bool btMachineIsLittleEndian()
+{
+ long int i = 1;
+ const char *p = (const char *) &i;
+ if (p[0] == 1) // Lowest address contains the least significant byte
+ return true;
+ else
+ return false;
+}
+
+
+
+///btSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360
+///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html
+SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero)
+{
+ // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
+ // Rely on positive value or'ed with its negative having sign bit on
+ // and zero value or'ed with its negative (which is still zero) having sign bit off
+ // Use arithmetic shift right, shifting the sign bit through all 32 bits
+ unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
+ unsigned testEqz = ~testNz;
+ return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
+}
+SIMD_FORCE_INLINE int btSelect(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
+{
+ unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
+ unsigned testEqz = ~testNz;
+ return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
+}
+SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
+{
+#ifdef BT_HAVE_NATIVE_FSEL
+ return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
+#else
+ return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero;
+#endif
+}
+
+template<typename T> SIMD_FORCE_INLINE void btSwap(T& a, T& b)
+{
+ T tmp = a;
+ a = b;
+ b = tmp;
+}
+
+
+//PCK: endian swapping functions
+SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val)
+{
+ return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24));
+}
+
+SIMD_FORCE_INLINE unsigned short btSwapEndian(unsigned short val)
+{
+ return static_cast<unsigned short>(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8));
+}
+
+SIMD_FORCE_INLINE unsigned btSwapEndian(int val)
+{
+ return btSwapEndian((unsigned)val);
+}
+
+SIMD_FORCE_INLINE unsigned short btSwapEndian(short val)
+{
+ return btSwapEndian((unsigned short) val);
+}
+
+///btSwapFloat uses using char pointers to swap the endianness
+////btSwapFloat/btSwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values
+///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754.
+///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception.
+///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you.
+///so instead of returning a float/double, we return integer/long long integer
+SIMD_FORCE_INLINE unsigned int btSwapEndianFloat(float d)
+{
+ unsigned int a = 0;
+ unsigned char *dst = (unsigned char *)&a;
+ unsigned char *src = (unsigned char *)&d;
+
+ dst[0] = src[3];
+ dst[1] = src[2];
+ dst[2] = src[1];
+ dst[3] = src[0];
+ return a;
+}
+
+// unswap using char pointers
+SIMD_FORCE_INLINE float btUnswapEndianFloat(unsigned int a)
+{
+ float d = 0.0f;
+ unsigned char *src = (unsigned char *)&a;
+ unsigned char *dst = (unsigned char *)&d;
+
+ dst[0] = src[3];
+ dst[1] = src[2];
+ dst[2] = src[1];
+ dst[3] = src[0];
+
+ return d;
+}
+
+
+// swap using char pointers
+SIMD_FORCE_INLINE void btSwapEndianDouble(double d, unsigned char* dst)
+{
+ unsigned char *src = (unsigned char *)&d;
+
+ dst[0] = src[7];
+ dst[1] = src[6];
+ dst[2] = src[5];
+ dst[3] = src[4];
+ dst[4] = src[3];
+ dst[5] = src[2];
+ dst[6] = src[1];
+ dst[7] = src[0];
+
+}
+
+// unswap using char pointers
+SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src)
+{
+ double d = 0.0;
+ unsigned char *dst = (unsigned char *)&d;
+
+ dst[0] = src[7];
+ dst[1] = src[6];
+ dst[2] = src[5];
+ dst[3] = src[4];
+ dst[4] = src[3];
+ dst[5] = src[2];
+ dst[6] = src[1];
+ dst[7] = src[0];
+
+ return d;
+}
+
+// returns normalized value in range [-SIMD_PI, SIMD_PI]
+SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians)
+{
+ angleInRadians = btFmod(angleInRadians, SIMD_2_PI);
+ if(angleInRadians < -SIMD_PI)
+ {
+ return angleInRadians + SIMD_2_PI;
+ }
+ else if(angleInRadians > SIMD_PI)
+ {
+ return angleInRadians - SIMD_2_PI;
+ }
+ else
+ {
+ return angleInRadians;
+ }
+}
+
+///rudimentary class to provide type info
+struct btTypedObject
+{
+ btTypedObject(int objectType)
+ :m_objectType(objectType)
+ {
+ }
+ int m_objectType;
+ inline int getObjectType() const
+ {
+ return m_objectType;
+ }
+};
+#endif //SIMD___SCALAR_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btStackAlloc.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btStackAlloc.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btStackAlloc.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btStackAlloc.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,116 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+StackAlloc extracted from GJK-EPA collision solver by Nathanael Presson
+Nov.2006
+*/
+
+#ifndef BT_STACK_ALLOC
+#define BT_STACK_ALLOC
+
+#include "btScalar.h" //for btAssert
+#include "btAlignedAllocator.h"
+
+///The btBlock class is an internal structure for the btStackAlloc memory allocator.
+struct btBlock
+{
+ btBlock* previous;
+ unsigned char* address;
+};
+
+///The StackAlloc class provides some fast stack-based memory allocator (LIFO last-in first-out)
+class btStackAlloc
+{
+public:
+
+ btStackAlloc(unsigned int size) { ctor();create(size); }
+ ~btStackAlloc() { destroy(); }
+
+ inline void create(unsigned int size)
+ {
+ destroy();
+ data = (unsigned char*) btAlignedAlloc(size,16);
+ totalsize = size;
+ }
+ inline void destroy()
+ {
+ btAssert(usedsize==0);
+ //Raise(L"StackAlloc is still in use");
+
+ if(usedsize==0)
+ {
+ if(!ischild && data)
+ btAlignedFree(data);
+
+ data = 0;
+ usedsize = 0;
+ }
+
+ }
+
+ int getAvailableMemory() const
+ {
+ return static_cast<int>(totalsize - usedsize);
+ }
+
+ unsigned char* allocate(unsigned int size)
+ {
+ const unsigned int nus(usedsize+size);
+ if(nus<totalsize)
+ {
+ usedsize=nus;
+ return(data+(usedsize-size));
+ }
+ btAssert(0);
+ //&& (L"Not enough memory"));
+
+ return(0);
+ }
+ SIMD_FORCE_INLINE btBlock* beginBlock()
+ {
+ btBlock* pb = (btBlock*)allocate(sizeof(btBlock));
+ pb->previous = current;
+ pb->address = data+usedsize;
+ current = pb;
+ return(pb);
+ }
+ SIMD_FORCE_INLINE void endBlock(btBlock* block)
+ {
+ btAssert(block==current);
+ //Raise(L"Unmatched blocks");
+ if(block==current)
+ {
+ current = block->previous;
+ usedsize = (unsigned int)((block->address-data)-sizeof(btBlock));
+ }
+ }
+
+private:
+ void ctor()
+ {
+ data = 0;
+ totalsize = 0;
+ usedsize = 0;
+ current = 0;
+ ischild = false;
+ }
+ unsigned char* data;
+ unsigned int totalsize;
+ unsigned int usedsize;
+ btBlock* current;
+ bool ischild;
+};
+
+#endif //BT_STACK_ALLOC
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btTransform.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btTransform.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btTransform.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btTransform.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,243 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef btTransform_H
+#define btTransform_H
+
+#include "btVector3.h"
+#include "btMatrix3x3.h"
+
+
+/**@brief The btTransform class supports rigid transforms with only translation and rotation and no scaling/shear.
+ *It can be used in combination with btVector3, btQuaternion and btMatrix3x3 linear algebra classes. */
+class btTransform {
+
+
+public:
+
+ /**@brief No initialization constructor */
+ btTransform() {}
+ /**@brief Constructor from btQuaternion (optional btVector3 )
+ * @param q Rotation from quaternion
+ * @param c Translation from Vector (default 0,0,0) */
+ explicit SIMD_FORCE_INLINE btTransform(const btQuaternion& q,
+ const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0)))
+ : m_basis(q),
+ m_origin(c)
+ {}
+
+ /**@brief Constructor from btMatrix3x3 (optional btVector3)
+ * @param b Rotation from Matrix
+ * @param c Translation from Vector default (0,0,0)*/
+ explicit SIMD_FORCE_INLINE btTransform(const btMatrix3x3& b,
+ const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0)))
+ : m_basis(b),
+ m_origin(c)
+ {}
+ /**@brief Copy constructor */
+ SIMD_FORCE_INLINE btTransform (const btTransform& other)
+ : m_basis(other.m_basis),
+ m_origin(other.m_origin)
+ {
+ }
+ /**@brief Assignment Operator */
+ SIMD_FORCE_INLINE btTransform& operator=(const btTransform& other)
+ {
+ m_basis = other.m_basis;
+ m_origin = other.m_origin;
+ return *this;
+ }
+
+
+ /**@brief Set the current transform as the value of the product of two transforms
+ * @param t1 Transform 1
+ * @param t2 Transform 2
+ * This = Transform1 * Transform2 */
+ SIMD_FORCE_INLINE void mult(const btTransform& t1, const btTransform& t2) {
+ m_basis = t1.m_basis * t2.m_basis;
+ m_origin = t1(t2.m_origin);
+ }
+
+/* void multInverseLeft(const btTransform& t1, const btTransform& t2) {
+ btVector3 v = t2.m_origin - t1.m_origin;
+ m_basis = btMultTransposeLeft(t1.m_basis, t2.m_basis);
+ m_origin = v * t1.m_basis;
+ }
+ */
+
+/**@brief Return the transform of the vector */
+ SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const
+ {
+ return btVector3(m_basis[0].dot(x) + m_origin.x(),
+ m_basis[1].dot(x) + m_origin.y(),
+ m_basis[2].dot(x) + m_origin.z());
+ }
+
+ /**@brief Return the transform of the vector */
+ SIMD_FORCE_INLINE btVector3 operator*(const btVector3& x) const
+ {
+ return (*this)(x);
+ }
+
+ /**@brief Return the transform of the btQuaternion */
+ SIMD_FORCE_INLINE btQuaternion operator*(const btQuaternion& q) const
+ {
+ return getRotation() * q;
+ }
+
+ /**@brief Return the basis matrix for the rotation */
+ SIMD_FORCE_INLINE btMatrix3x3& getBasis() { return m_basis; }
+ /**@brief Return the basis matrix for the rotation */
+ SIMD_FORCE_INLINE const btMatrix3x3& getBasis() const { return m_basis; }
+
+ /**@brief Return the origin vector translation */
+ SIMD_FORCE_INLINE btVector3& getOrigin() { return m_origin; }
+ /**@brief Return the origin vector translation */
+ SIMD_FORCE_INLINE const btVector3& getOrigin() const { return m_origin; }
+
+ /**@brief Return a quaternion representing the rotation */
+ btQuaternion getRotation() const {
+ btQuaternion q;
+ m_basis.getRotation(q);
+ return q;
+ }
+
+
+ /**@brief Set from an array
+ * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */
+ void setFromOpenGLMatrix(const btScalar *m)
+ {
+ m_basis.setFromOpenGLSubMatrix(m);
+ m_origin.setValue(m[12],m[13],m[14]);
+ }
+
+ /**@brief Fill an array representation
+ * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */
+ void getOpenGLMatrix(btScalar *m) const
+ {
+ m_basis.getOpenGLSubMatrix(m);
+ m[12] = m_origin.x();
+ m[13] = m_origin.y();
+ m[14] = m_origin.z();
+ m[15] = btScalar(1.0);
+ }
+
+ /**@brief Set the translational element
+ * @param origin The vector to set the translation to */
+ SIMD_FORCE_INLINE void setOrigin(const btVector3& origin)
+ {
+ m_origin = origin;
+ }
+
+ SIMD_FORCE_INLINE btVector3 invXform(const btVector3& inVec) const;
+
+
+ /**@brief Set the rotational element by btMatrix3x3 */
+ SIMD_FORCE_INLINE void setBasis(const btMatrix3x3& basis)
+ {
+ m_basis = basis;
+ }
+
+ /**@brief Set the rotational element by btQuaternion */
+ SIMD_FORCE_INLINE void setRotation(const btQuaternion& q)
+ {
+ m_basis.setRotation(q);
+ }
+
+
+ /**@brief Set this transformation to the identity */
+ void setIdentity()
+ {
+ m_basis.setIdentity();
+ m_origin.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+ }
+
+ /**@brief Multiply this Transform by another(this = this * another)
+ * @param t The other transform */
+ btTransform& operator*=(const btTransform& t)
+ {
+ m_origin += m_basis * t.m_origin;
+ m_basis *= t.m_basis;
+ return *this;
+ }
+
+ /**@brief Return the inverse of this transform */
+ btTransform inverse() const
+ {
+ btMatrix3x3 inv = m_basis.transpose();
+ return btTransform(inv, inv * -m_origin);
+ }
+
+ /**@brief Return the inverse of this transform times the other transform
+ * @param t The other transform
+ * return this.inverse() * the other */
+ btTransform inverseTimes(const btTransform& t) const;
+
+ /**@brief Return the product of this transform and the other */
+ btTransform operator*(const btTransform& t) const;
+
+ /**@brief Return an identity transform */
+ static const btTransform& getIdentity()
+ {
+ static const btTransform identityTransform(btMatrix3x3::getIdentity());
+ return identityTransform;
+ }
+
+private:
+ ///Storage for the rotation
+ btMatrix3x3 m_basis;
+ ///Storage for the translation
+ btVector3 m_origin;
+};
+
+
+SIMD_FORCE_INLINE btVector3
+btTransform::invXform(const btVector3& inVec) const
+{
+ btVector3 v = inVec - m_origin;
+ return (m_basis.transpose() * v);
+}
+
+SIMD_FORCE_INLINE btTransform
+btTransform::inverseTimes(const btTransform& t) const
+{
+ btVector3 v = t.getOrigin() - m_origin;
+ return btTransform(m_basis.transposeTimes(t.m_basis),
+ v * m_basis);
+}
+
+SIMD_FORCE_INLINE btTransform
+btTransform::operator*(const btTransform& t) const
+{
+ return btTransform(m_basis * t.m_basis,
+ (*this)(t.m_origin));
+}
+
+/**@brief Test if two transforms have all elements equal */
+SIMD_FORCE_INLINE bool operator==(const btTransform& t1, const btTransform& t2)
+{
+ return ( t1.getBasis() == t2.getBasis() &&
+ t1.getOrigin() == t2.getOrigin() );
+}
+
+
+#endif
+
+
+
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btTransformUtil.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btTransformUtil.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btTransformUtil.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btTransformUtil.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,230 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef SIMD_TRANSFORM_UTIL_H
+#define SIMD_TRANSFORM_UTIL_H
+
+#include "btTransform.h"
+#define ANGULAR_MOTION_THRESHOLD btScalar(0.5)*SIMD_HALF_PI
+
+
+
+
+SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir)
+{
+ return btVector3(supportDir.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(),
+ supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(),
+ supportDir.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z());
+}
+
+
+
+
+
+
+/// Utils related to temporal transforms
+class btTransformUtil
+{
+
+public:
+
+ static void integrateTransform(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep,btTransform& predictedTransform)
+ {
+ predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
+// #define QUATERNION_DERIVATIVE
+ #ifdef QUATERNION_DERIVATIVE
+ btQuaternion predictedOrn = curTrans.getRotation();
+ predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5));
+ predictedOrn.normalize();
+ #else
+ //Exponential map
+ //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
+
+ btVector3 axis;
+ btScalar fAngle = angvel.length();
+ //limit the angular motion
+ if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
+ {
+ fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
+ }
+
+ if ( fAngle < btScalar(0.001) )
+ {
+ // use Taylor's expansions of sync function
+ axis = angvel*( btScalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(btScalar(0.020833333333))*fAngle*fAngle );
+ }
+ else
+ {
+ // sync(fAngle) = sin(c*fAngle)/t
+ axis = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle );
+ }
+ btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*btScalar(0.5) ));
+ btQuaternion orn0 = curTrans.getRotation();
+
+ btQuaternion predictedOrn = dorn * orn0;
+ predictedOrn.normalize();
+ #endif
+ predictedTransform.setRotation(predictedOrn);
+ }
+
+ static void calculateVelocityQuaternion(const btVector3& pos0,const btVector3& pos1,const btQuaternion& orn0,const btQuaternion& orn1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
+ {
+ linVel = (pos1 - pos0) / timeStep;
+ btVector3 axis;
+ btScalar angle;
+ if (orn0 != orn1)
+ {
+ calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle);
+ angVel = axis * angle / timeStep;
+ } else
+ {
+ angVel.setValue(0,0,0);
+ }
+ }
+
+ static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle)
+ {
+ btQuaternion orn1 = orn0.nearest(orn1a);
+ btQuaternion dorn = orn1 * orn0.inverse();
+ ///floating point inaccuracy can lead to w component > 1..., which breaks
+ dorn.normalize();
+ angle = dorn.getAngle();
+ axis = btVector3(dorn.x(),dorn.y(),dorn.z());
+ axis[3] = btScalar(0.);
+ //check for axis length
+ btScalar len = axis.length2();
+ if (len < SIMD_EPSILON*SIMD_EPSILON)
+ axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
+ else
+ axis /= btSqrt(len);
+ }
+
+ static void calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
+ {
+ linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
+ btVector3 axis;
+ btScalar angle;
+ calculateDiffAxisAngle(transform0,transform1,axis,angle);
+ angVel = axis * angle / timeStep;
+ }
+
+ static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle)
+ {
+ btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
+ btQuaternion dorn;
+ dmat.getRotation(dorn);
+
+ ///floating point inaccuracy can lead to w component > 1..., which breaks
+ dorn.normalize();
+
+ angle = dorn.getAngle();
+ axis = btVector3(dorn.x(),dorn.y(),dorn.z());
+ axis[3] = btScalar(0.);
+ //check for axis length
+ btScalar len = axis.length2();
+ if (len < SIMD_EPSILON*SIMD_EPSILON)
+ axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
+ else
+ axis /= btSqrt(len);
+ }
+
+};
+
+
+///The btConvexSeparatingDistanceUtil can help speed up convex collision detection
+///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance
+class btConvexSeparatingDistanceUtil
+{
+ btQuaternion m_ornA;
+ btQuaternion m_ornB;
+ btVector3 m_posA;
+ btVector3 m_posB;
+
+ btVector3 m_separatingNormal;
+
+ btScalar m_boundingRadiusA;
+ btScalar m_boundingRadiusB;
+ btScalar m_separatingDistance;
+
+public:
+
+ btConvexSeparatingDistanceUtil(btScalar boundingRadiusA,btScalar boundingRadiusB)
+ :m_boundingRadiusA(boundingRadiusA),
+ m_boundingRadiusB(boundingRadiusB),
+ m_separatingDistance(0.f)
+ {
+ }
+
+ btScalar getConservativeSeparatingDistance()
+ {
+ return m_separatingDistance;
+ }
+
+ void updateSeparatingDistance(const btTransform& transA,const btTransform& transB)
+ {
+ const btVector3& toPosA = transA.getOrigin();
+ const btVector3& toPosB = transB.getOrigin();
+ btQuaternion toOrnA = transA.getRotation();
+ btQuaternion toOrnB = transB.getRotation();
+
+ if (m_separatingDistance>0.f)
+ {
+
+
+ btVector3 linVelA,angVelA,linVelB,angVelB;
+ btTransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,btScalar(1.),linVelA,angVelA);
+ btTransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,btScalar(1.),linVelB,angVelB);
+ btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
+ btVector3 relLinVel = (linVelB-linVelA);
+ btScalar relLinVelocLength = (linVelB-linVelA).dot(m_separatingNormal);
+ if (relLinVelocLength<0.f)
+ {
+ relLinVelocLength = 0.f;
+ }
+
+ btScalar projectedMotion = maxAngularProjectedVelocity +relLinVelocLength;
+ m_separatingDistance -= projectedMotion;
+ }
+
+ m_posA = toPosA;
+ m_posB = toPosB;
+ m_ornA = toOrnA;
+ m_ornB = toOrnB;
+ }
+
+ void initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB)
+ {
+ m_separatingDistance = separatingDistance;
+
+ if (m_separatingDistance>0.f)
+ {
+ m_separatingNormal = separatingVector;
+
+ const btVector3& toPosA = transA.getOrigin();
+ const btVector3& toPosB = transB.getOrigin();
+ btQuaternion toOrnA = transA.getRotation();
+ btQuaternion toOrnB = transB.getRotation();
+ m_posA = toPosA;
+ m_posB = toPosB;
+ m_ornA = toOrnA;
+ m_ornB = toOrnB;
+ }
+ }
+
+};
+
+
+#endif //SIMD_TRANSFORM_UTIL_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btVector3.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btVector3.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btVector3.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btVector3.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,660 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef SIMD__VECTOR3_H
+#define SIMD__VECTOR3_H
+
+
+#include "btScalar.h"
+#include "btScalar.h"
+#include "btMinMax.h"
+/**@brief btVector3 can be used to represent 3D points and vectors.
+ * It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
+ * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
+ */
+
+ATTRIBUTE_ALIGNED16(class) btVector3
+{
+public:
+
+#if defined (__SPU__) && defined (__CELLOS_LV2__)
+ btScalar m_floats[4];
+public:
+ SIMD_FORCE_INLINE const vec_float4& get128() const
+ {
+ return *((const vec_float4*)&m_floats[0]);
+ }
+public:
+#else //__CELLOS_LV2__ __SPU__
+#ifdef BT_USE_SSE // WIN32
+ union {
+ __m128 mVec128;
+ btScalar m_floats[4];
+ };
+ SIMD_FORCE_INLINE __m128 get128() const
+ {
+ return mVec128;
+ }
+ SIMD_FORCE_INLINE void set128(__m128 v128)
+ {
+ mVec128 = v128;
+ }
+#else
+ btScalar m_floats[4];
+#endif
+#endif //__CELLOS_LV2__ __SPU__
+
+ public:
+
+ /**@brief No initialization constructor */
+ SIMD_FORCE_INLINE btVector3() {}
+
+
+
+ /**@brief Constructor from scalars
+ * @param x X value
+ * @param y Y value
+ * @param z Z value
+ */
+ SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z)
+ {
+ m_floats[0] = x;
+ m_floats[1] = y;
+ m_floats[2] = z;
+ m_floats[3] = btScalar(0.);
+ }
+
+
+/**@brief Add a vector to this one
+ * @param The vector to add to this one */
+ SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v)
+ {
+
+ m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2];
+ return *this;
+ }
+
+
+ /**@brief Subtract a vector from this one
+ * @param The vector to subtract */
+ SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v)
+ {
+ m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2];
+ return *this;
+ }
+ /**@brief Scale the vector
+ * @param s Scale factor */
+ SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
+ {
+ m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s;
+ return *this;
+ }
+
+ /**@brief Inversely scale the vector
+ * @param s Scale factor to divide by */
+ SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s)
+ {
+ btFullAssert(s != btScalar(0.0));
+ return *this *= btScalar(1.0) / s;
+ }
+
+ /**@brief Return the dot product
+ * @param v The other vector in the dot product */
+ SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
+ {
+ return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2];
+ }
+
+ /**@brief Return the length of the vector squared */
+ SIMD_FORCE_INLINE btScalar length2() const
+ {
+ return dot(*this);
+ }
+
+ /**@brief Return the length of the vector */
+ SIMD_FORCE_INLINE btScalar length() const
+ {
+ return btSqrt(length2());
+ }
+
+ /**@brief Return the distance squared between the ends of this and another vector
+ * This is symantically treating the vector like a point */
+ SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;
+
+ /**@brief Return the distance between the ends of this and another vector
+ * This is symantically treating the vector like a point */
+ SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const;
+
+ /**@brief Normalize this vector
+ * x^2 + y^2 + z^2 = 1 */
+ SIMD_FORCE_INLINE btVector3& normalize()
+ {
+ return *this /= length();
+ }
+
+ /**@brief Return a normalized version of this vector */
+ SIMD_FORCE_INLINE btVector3 normalized() const;
+
+ /**@brief Rotate this vector
+ * @param wAxis The axis to rotate about
+ * @param angle The angle to rotate by */
+ SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle );
+
+ /**@brief Return the angle between this and another vector
+ * @param v The other vector */
+ SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const
+ {
+ btScalar s = btSqrt(length2() * v.length2());
+ btFullAssert(s != btScalar(0.0));
+ return btAcos(dot(v) / s);
+ }
+ /**@brief Return a vector will the absolute values of each element */
+ SIMD_FORCE_INLINE btVector3 absolute() const
+ {
+ return btVector3(
+ btFabs(m_floats[0]),
+ btFabs(m_floats[1]),
+ btFabs(m_floats[2]));
+ }
+ /**@brief Return the cross product between this and another vector
+ * @param v The other vector */
+ SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
+ {
+ return btVector3(
+ m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1],
+ m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
+ m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
+ }
+
+ SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
+ {
+ return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) +
+ m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) +
+ m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
+ }
+
+ /**@brief Return the axis with the smallest value
+ * Note return values are 0,1,2 for x, y, or z */
+ SIMD_FORCE_INLINE int minAxis() const
+ {
+ return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
+ }
+
+ /**@brief Return the axis with the largest value
+ * Note return values are 0,1,2 for x, y, or z */
+ SIMD_FORCE_INLINE int maxAxis() const
+ {
+ return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
+ }
+
+ SIMD_FORCE_INLINE int furthestAxis() const
+ {
+ return absolute().minAxis();
+ }
+
+ SIMD_FORCE_INLINE int closestAxis() const
+ {
+ return absolute().maxAxis();
+ }
+
+ SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
+ {
+ btScalar s = btScalar(1.0) - rt;
+ m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
+ m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
+ m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
+ //don't do the unused w component
+ // m_co[3] = s * v0[3] + rt * v1[3];
+ }
+
+ /**@brief Return the linear interpolation between this and another vector
+ * @param v The other vector
+ * @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
+ SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const
+ {
+ return btVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
+ m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
+ m_floats[2] + (v.m_floats[2] -m_floats[2]) * t);
+ }
+
+ /**@brief Elementwise multiply this vector by the other
+ * @param v The other vector */
+ SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
+ {
+ m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2];
+ return *this;
+ }
+
+ /**@brief Return the x value */
+ SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
+ /**@brief Return the y value */
+ SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
+ /**@brief Return the z value */
+ SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
+ /**@brief Set the x value */
+ SIMD_FORCE_INLINE void setX(btScalar x) { m_floats[0] = x;};
+ /**@brief Set the y value */
+ SIMD_FORCE_INLINE void setY(btScalar y) { m_floats[1] = y;};
+ /**@brief Set the z value */
+ SIMD_FORCE_INLINE void setZ(btScalar z) {m_floats[2] = z;};
+ /**@brief Set the w value */
+ SIMD_FORCE_INLINE void setW(btScalar w) { m_floats[3] = w;};
+ /**@brief Return the x value */
+ SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
+ /**@brief Return the y value */
+ SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
+ /**@brief Return the z value */
+ SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
+ /**@brief Return the w value */
+ SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
+
+ //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; }
+ //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
+ ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
+ SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; }
+ SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; }
+
+ SIMD_FORCE_INLINE bool operator==(const btVector3& other) const
+ {
+ return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0]));
+ }
+
+ SIMD_FORCE_INLINE bool operator!=(const btVector3& other) const
+ {
+ return !(*this == other);
+ }
+
+ /**@brief Set each element to the max of the current values and the values of another btVector3
+ * @param other The other btVector3 to compare with
+ */
+ SIMD_FORCE_INLINE void setMax(const btVector3& other)
+ {
+ btSetMax(m_floats[0], other.m_floats[0]);
+ btSetMax(m_floats[1], other.m_floats[1]);
+ btSetMax(m_floats[2], other.m_floats[2]);
+ btSetMax(m_floats[3], other.w());
+ }
+ /**@brief Set each element to the min of the current values and the values of another btVector3
+ * @param other The other btVector3 to compare with
+ */
+ SIMD_FORCE_INLINE void setMin(const btVector3& other)
+ {
+ btSetMin(m_floats[0], other.m_floats[0]);
+ btSetMin(m_floats[1], other.m_floats[1]);
+ btSetMin(m_floats[2], other.m_floats[2]);
+ btSetMin(m_floats[3], other.w());
+ }
+
+ SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z)
+ {
+ m_floats[0]=x;
+ m_floats[1]=y;
+ m_floats[2]=z;
+ m_floats[3] = btScalar(0.);
+ }
+
+ void getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const
+ {
+ v0->setValue(0. ,-z() ,y());
+ v1->setValue(z() ,0. ,-x());
+ v2->setValue(-y() ,x() ,0.);
+ }
+
+ void setZero()
+ {
+ setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+ }
+
+};
+
+/**@brief Return the sum of two vectors (Point symantics)*/
+SIMD_FORCE_INLINE btVector3
+operator+(const btVector3& v1, const btVector3& v2)
+{
+ return btVector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]);
+}
+
+/**@brief Return the elementwise product of two vectors */
+SIMD_FORCE_INLINE btVector3
+operator*(const btVector3& v1, const btVector3& v2)
+{
+ return btVector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]);
+}
+
+/**@brief Return the difference between two vectors */
+SIMD_FORCE_INLINE btVector3
+operator-(const btVector3& v1, const btVector3& v2)
+{
+ return btVector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]);
+}
+/**@brief Return the negative of the vector */
+SIMD_FORCE_INLINE btVector3
+operator-(const btVector3& v)
+{
+ return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
+}
+
+/**@brief Return the vector scaled by s */
+SIMD_FORCE_INLINE btVector3
+operator*(const btVector3& v, const btScalar& s)
+{
+ return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
+}
+
+/**@brief Return the vector scaled by s */
+SIMD_FORCE_INLINE btVector3
+operator*(const btScalar& s, const btVector3& v)
+{
+ return v * s;
+}
+
+/**@brief Return the vector inversely scaled by s */
+SIMD_FORCE_INLINE btVector3
+operator/(const btVector3& v, const btScalar& s)
+{
+ btFullAssert(s != btScalar(0.0));
+ return v * (btScalar(1.0) / s);
+}
+
+/**@brief Return the vector inversely scaled by s */
+SIMD_FORCE_INLINE btVector3
+operator/(const btVector3& v1, const btVector3& v2)
+{
+ return btVector3(v1.m_floats[0] / v2.m_floats[0],v1.m_floats[1] / v2.m_floats[1],v1.m_floats[2] / v2.m_floats[2]);
+}
+
+/**@brief Return the dot product between two vectors */
+SIMD_FORCE_INLINE btScalar
+btDot(const btVector3& v1, const btVector3& v2)
+{
+ return v1.dot(v2);
+}
+
+
+/**@brief Return the distance squared between two vectors */
+SIMD_FORCE_INLINE btScalar
+btDistance2(const btVector3& v1, const btVector3& v2)
+{
+ return v1.distance2(v2);
+}
+
+
+/**@brief Return the distance between two vectors */
+SIMD_FORCE_INLINE btScalar
+btDistance(const btVector3& v1, const btVector3& v2)
+{
+ return v1.distance(v2);
+}
+
+/**@brief Return the angle between two vectors */
+SIMD_FORCE_INLINE btScalar
+btAngle(const btVector3& v1, const btVector3& v2)
+{
+ return v1.angle(v2);
+}
+
+/**@brief Return the cross product of two vectors */
+SIMD_FORCE_INLINE btVector3
+btCross(const btVector3& v1, const btVector3& v2)
+{
+ return v1.cross(v2);
+}
+
+SIMD_FORCE_INLINE btScalar
+btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
+{
+ return v1.triple(v2, v3);
+}
+
+/**@brief Return the linear interpolation between two vectors
+ * @param v1 One vector
+ * @param v2 The other vector
+ * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */
+SIMD_FORCE_INLINE btVector3
+lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
+{
+ return v1.lerp(v2, t);
+}
+
+
+
+SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
+{
+ return (v - *this).length2();
+}
+
+SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
+{
+ return (v - *this).length();
+}
+
+SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
+{
+ return *this / length();
+}
+
+SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar angle )
+{
+ // wAxis must be a unit lenght vector
+
+ btVector3 o = wAxis * wAxis.dot( *this );
+ btVector3 x = *this - o;
+ btVector3 y;
+
+ y = wAxis.cross( *this );
+
+ return ( o + x * btCos( angle ) + y * btSin( angle ) );
+}
+
+class btVector4 : public btVector3
+{
+public:
+
+ SIMD_FORCE_INLINE btVector4() {}
+
+
+ SIMD_FORCE_INLINE btVector4(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
+ : btVector3(x,y,z)
+ {
+ m_floats[3] = w;
+ }
+
+
+ SIMD_FORCE_INLINE btVector4 absolute4() const
+ {
+ return btVector4(
+ btFabs(m_floats[0]),
+ btFabs(m_floats[1]),
+ btFabs(m_floats[2]),
+ btFabs(m_floats[3]));
+ }
+
+
+
+ btScalar getW() const { return m_floats[3];}
+
+
+ SIMD_FORCE_INLINE int maxAxis4() const
+ {
+ int maxIndex = -1;
+ btScalar maxVal = btScalar(-BT_LARGE_FLOAT);
+ if (m_floats[0] > maxVal)
+ {
+ maxIndex = 0;
+ maxVal = m_floats[0];
+ }
+ if (m_floats[1] > maxVal)
+ {
+ maxIndex = 1;
+ maxVal = m_floats[1];
+ }
+ if (m_floats[2] > maxVal)
+ {
+ maxIndex = 2;
+ maxVal =m_floats[2];
+ }
+ if (m_floats[3] > maxVal)
+ {
+ maxIndex = 3;
+ maxVal = m_floats[3];
+ }
+
+
+
+
+ return maxIndex;
+
+ }
+
+
+ SIMD_FORCE_INLINE int minAxis4() const
+ {
+ int minIndex = -1;
+ btScalar minVal = btScalar(BT_LARGE_FLOAT);
+ if (m_floats[0] < minVal)
+ {
+ minIndex = 0;
+ minVal = m_floats[0];
+ }
+ if (m_floats[1] < minVal)
+ {
+ minIndex = 1;
+ minVal = m_floats[1];
+ }
+ if (m_floats[2] < minVal)
+ {
+ minIndex = 2;
+ minVal =m_floats[2];
+ }
+ if (m_floats[3] < minVal)
+ {
+ minIndex = 3;
+ minVal = m_floats[3];
+ }
+
+ return minIndex;
+
+ }
+
+
+ SIMD_FORCE_INLINE int closestAxis4() const
+ {
+ return absolute4().maxAxis4();
+ }
+
+
+
+
+ /**@brief Set x,y,z and zero w
+ * @param x Value of x
+ * @param y Value of y
+ * @param z Value of z
+ */
+
+
+/* void getValue(btScalar *m) const
+ {
+ m[0] = m_floats[0];
+ m[1] = m_floats[1];
+ m[2] =m_floats[2];
+ }
+*/
+/**@brief Set the values
+ * @param x Value of x
+ * @param y Value of y
+ * @param z Value of z
+ * @param w Value of w
+ */
+ SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
+ {
+ m_floats[0]=x;
+ m_floats[1]=y;
+ m_floats[2]=z;
+ m_floats[3]=w;
+ }
+
+
+
+
+};
+
+
+///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
+SIMD_FORCE_INLINE void btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal)
+{
+ #ifdef BT_USE_DOUBLE_PRECISION
+ unsigned char* dest = (unsigned char*) &destVal;
+ unsigned char* src = (unsigned char*) &sourceVal;
+ dest[0] = src[7];
+ dest[1] = src[6];
+ dest[2] = src[5];
+ dest[3] = src[4];
+ dest[4] = src[3];
+ dest[5] = src[2];
+ dest[6] = src[1];
+ dest[7] = src[0];
+#else
+ unsigned char* dest = (unsigned char*) &destVal;
+ unsigned char* src = (unsigned char*) &sourceVal;
+ dest[0] = src[3];
+ dest[1] = src[2];
+ dest[2] = src[1];
+ dest[3] = src[0];
+#endif //BT_USE_DOUBLE_PRECISION
+}
+///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
+SIMD_FORCE_INLINE void btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec)
+{
+ for (int i=0;i<4;i++)
+ {
+ btSwapScalarEndian(sourceVec[i],destVec[i]);
+ }
+
+}
+
+///btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
+SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3& vector)
+{
+
+ btVector3 swappedVec;
+ for (int i=0;i<4;i++)
+ {
+ btSwapScalarEndian(vector[i],swappedVec[i]);
+ }
+ vector = swappedVec;
+}
+
+SIMD_FORCE_INLINE void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q)
+{
+ if (btFabs(n.z()) > SIMDSQRT12) {
+ // choose p in y-z plane
+ btScalar a = n[1]*n[1] + n[2]*n[2];
+ btScalar k = btRecipSqrt (a);
+ p.setValue(0,-n[2]*k,n[1]*k);
+ // set q = n x p
+ q.setValue(a*k,-n[0]*p[2],n[0]*p[1]);
+ }
+ else {
+ // choose p in x-y plane
+ btScalar a = n.x()*n.x() + n.y()*n.y();
+ btScalar k = btRecipSqrt (a);
+ p.setValue(-n.y()*k,n.x()*k,0);
+ // set q = n x p
+ q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k);
+ }
+}
+
+#endif //SIMD__VECTOR3_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/btBulletCollisionCommon.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/btBulletCollisionCommon.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/btBulletCollisionCommon.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/btBulletCollisionCommon.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,66 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BULLET_COLLISION_COMMON_H
+#define BULLET_COLLISION_COMMON_H
+
+///Common headerfile includes for Bullet Collision Detection
+
+///Bullet's btCollisionWorld and btCollisionObject definitions
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+///Collision Shapes
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+#include "BulletCollision/CollisionShapes/btConeShape.h"
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
+#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleMesh.h"
+#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/CollisionShapes/btTetrahedronShape.h"
+#include "BulletCollision/CollisionShapes/btEmptyShape.h"
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+#include "BulletCollision/CollisionShapes/btUniformScalingShape.h"
+
+///Narrowphase Collision Detector
+#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
+
+//btSphereBoxCollisionAlgorithm is broken, use gjk for now
+//#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h"
+
+///Dispatching and generation of collision pairs (broadphase)
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/BroadphaseCollision/btAxisSweep3.h"
+#include "BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h"
+#include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.h"
+
+///Math library & Utils
+#include "LinearMath/btQuaternion.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btDefaultMotionState.h"
+#include "LinearMath/btQuickprof.h"
+#include "LinearMath/btIDebugDraw.h"
+
+#endif //BULLET_COLLISION_COMMON_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/btBulletDynamicsCommon.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/btBulletDynamicsCommon.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/btBulletDynamicsCommon.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/btBulletDynamicsCommon.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,49 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BULLET_DYNAMICS_COMMON_H
+#define BULLET_DYNAMICS_COMMON_H
+
+///Common headerfile includes for Bullet Dynamics, including Collision Detection
+#include "btBulletCollisionCommon.h"
+
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+#include "BulletDynamics/Dynamics/btContinuousDynamicsWorld.h"
+
+#include "BulletDynamics/Dynamics/btSimpleDynamicsWorld.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+
+#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btUniversalConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btHinge2Constraint.h"
+
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+
+
+///Vehicle simulation, with wheel contact simulated by raycasts
+#include "BulletDynamics/Vehicle/btRaycastVehicle.h"
+
+
+
+
+
+
+#endif //BULLET_DYNAMICS_COMMON_H
+
More information about the llvm-commits
mailing list