//************************************************************************
// Called to compare two entrys
//************************************************************************
bool CContactList::CompareEntries(CListEntry<CContactListEntry*,CContactListGroup*> *pLeft,CListEntry<CContactListEntry*,CContactListGroup*> *pRight)
{
CContactListEntry *pLeftEntry = GetContactData(pLeft);
CContactListEntry *pRightEntry = GetContactData(pRight);
if(pLeftEntry && pRightEntry)
{
int iLeftMessages = pLeftEntry->iMessages;
int iRightMessages = pRightEntry->iMessages;
if(pLeft->GetType() == CONTAINER)
iLeftMessages += ((CListContainer<CContactListEntry*,CContactListGroup*>*)pLeft)->GetGroupData()->iEvents;
if(pRight->GetType() == CONTAINER)
iRightMessages += ((CListContainer<CContactListEntry*,CContactListGroup*>*)pRight)->GetGroupData()->iEvents;
if (!iRightMessages && iLeftMessages)
return true;
else if (iRightMessages && !iLeftMessages)
return false;
else if (iLeftMessages && iRightMessages)
return (iLeftMessages > iRightMessages);
else if(pLeftEntry->iStatus != pRightEntry->iStatus)
return (aiStatusPriority[pLeftEntry->iStatus - ID_STATUS_OFFLINE] > aiStatusPriority[pRightEntry->iStatus - ID_STATUS_OFFLINE]);
else
return mir_tstrcmpi(pLeftEntry->strName.c_str(),pRightEntry->strName.c_str())<0;
}
else if(pLeft->GetType() == ITEM && pRight->GetType() == CONTAINER)
return false;
else if(pLeft->GetType() == CONTAINER && pRight->GetType() == ITEM)
return true;
else if(pLeft->GetType() == CONTAINER && pRight->GetType() == CONTAINER)
{
CContactListGroup *pGroup1 = ((CListContainer<CContactListEntry*,CContactListGroup*>*)pLeft)->GetGroupData();
CContactListGroup *pGroup2 = ((CListContainer<CContactListEntry*,CContactListGroup*>*)pRight)->GetGroupData();
if (!pGroup2->iEvents && pGroup1->iEvents)
return true;
else if (pGroup2->iEvents && !pGroup1->iEvents)
return false;
else if (pGroup1->iEvents && pGroup2->iEvents)
return (pGroup1->iEvents > pGroup2->iEvents);
else
return mir_tstrcmpi(pGroup1->strName.c_str(),pGroup2->strName.c_str())<0;
}
return false;
}
//************************************************************************
// removes a contact from the list
//************************************************************************
void CContactList::RemoveContact(HANDLE hContact) {
CListContainer<CContactListEntry*,CContactListGroup*> *pGroup = NULL;
///tstring strGroup = GetContactGroupPath(hContact);
CListEntry<CContactListEntry*,CContactListGroup*> *pContactEntry = FindContact(hContact);
if(!pContactEntry) {
return;
}
if( !CConfig::GetBoolSetting(CLIST_USEGROUPS)){
if(pContactEntry->GetType() == ITEM)
RemoveItem(((CListItem<CContactListEntry*,CContactListGroup*>*)pContactEntry)->GetItemData());
else
RemoveGroup(((CListContainer<CContactListEntry*,CContactListGroup*>*)pContactEntry)->GetGroupData());
} else {
pGroup = (CListContainer<CContactListEntry*,CContactListGroup*>*)pContactEntry->GetParent();
ASSERT(pGroup != NULL);
CContactListEntry *pEntry = GetContactData(pContactEntry);
if(!pEntry) {
return;
}
// Update the contacts group if it has one
if(pGroup->GetType() != ROOT)
{
if(!CAppletManager::IsSubContact(hContact) && pEntry->iStatus != ID_STATUS_OFFLINE)
ChangeGroupObjectCounters(pGroup->GetGroupData()->strPath,0,-1);
if(!CAppletManager::IsSubContact(hContact) && pEntry->iMessages > 0)
ChangeGroupObjectCounters(pGroup->GetGroupData()->strPath,0,0,-pEntry->iMessages);
}
if(pContactEntry->GetType() == ITEM)
pGroup->RemoveItem(((CListItem<CContactListEntry*,CContactListGroup*>*)pContactEntry)->GetItemData());
else {
pGroup->RemoveGroup(((CListContainer<CContactListEntry*,CContactListGroup*>*)pContactEntry)->GetGroupData());
// Reenumerate all subcontacts (maybe MetaContacts was disabled
int numContacts = CallService(MS_MC_GETNUMCONTACTS,(WPARAM)hContact,0);
HANDLE hSubContact = NULL;
for(int i=0;i<numContacts;i++) {
hSubContact = (HANDLE *) CallService(MS_MC_GETSUBCONTACT,(WPARAM)hContact, (LPARAM)i);
if(!FindContact(hSubContact)) {
AddContact(hSubContact);
}
}
}
CListContainer<CContactListEntry*,CContactListGroup*> *pParent = (CListContainer<CContactListEntry*,CContactListGroup*>*)pGroup->GetParent();
while(pParent != NULL && pGroup->IsEmpty() && !pGroup->GetGroupData()->hMetaContact)
{
pParent->RemoveGroup(pGroup->GetGroupData());
pGroup = pParent;
pParent = (CListContainer<CContactListEntry*,CContactListGroup*>*)pGroup->GetParent();
}
}
}
//************************************************************************
// updates the message count for the specified contact
//************************************************************************
void CContactList::UpdateMessageCounter(CListEntry<CContactListEntry*,CContactListGroup*> *pContactEntry)
{
CContactListEntry *pEntry = GetContactData(pContactEntry);
if(!pEntry) {
return;
}
int iOldMessages = pEntry->iMessages;
bool bSort = false;
HANDLE hEvent= NULL;
hEvent = db_event_firstUnread(pEntry->hHandle);
if(CAppletManager::IsMessageWindowOpen(pEntry->hHandle) || (hEvent == NULL && pEntry->iMessages > 0))
{
pEntry->iMessages = 0;
bSort = true;
}
else
{
pEntry->iMessages = 0;
HANDLE hLastEvent = db_event_last(pEntry->hHandle);
while(hLastEvent != NULL && hEvent != NULL)
{
pEntry->iMessages++;
if(hLastEvent == hEvent)
break;
hLastEvent = db_event_prev(hLastEvent);
}
}
if(pEntry->iMessages >= 100)
pEntry->strMessages = _T(">99");
else
{
char buffer[8];
buffer[0] = 0;
itoa(pEntry->iMessages,buffer,10);
pEntry->strMessages = toTstring(buffer);
}
CListContainer<CContactListEntry*,CContactListGroup*>* pContainer = ((CListContainer<CContactListEntry*,CContactListGroup*>*)pContactEntry->GetParent());
// Update the contacts group if it has one
if(pContainer->GetType() != ROOT)
{
// Update the groups event count
if(iOldMessages != 0 && pEntry->iMessages == 0)
ChangeGroupObjectCounters(pContainer->GetGroupData()->strPath,0,0,-1);
else if(iOldMessages == 0 && pEntry->iMessages != 0)
ChangeGroupObjectCounters(pContainer->GetGroupData()->strPath,0,0,1);
else
return;
// sort the groups parent
((CListContainer<CContactListEntry*,CContactListGroup*>*)pContainer->GetParent())->sort(CContactList::CompareEntries);
}
}
//************************************************************************
// returns the contact's status
//************************************************************************
int CContactList::GetContactStatus(MCONTACT hContact)
{
CListEntry<CContactListEntry *,CContactListGroup*> *pContactEntry = FindContact(hContact);
if(!pContactEntry)
return ID_STATUS_OFFLINE;
CContactListEntry *pEntry = GetContactData(pContactEntry);
if(!pEntry) {
return ID_STATUS_OFFLINE;
}
return pEntry->iStatus;
}
//************************************************************************
// called when a contacts hidden flag has changed
//************************************************************************
void CContactList::OnContactHiddenChanged(MCONTACT hContact, bool bHidden)
{
CListEntry<CContactListEntry*,CContactListGroup*> *pContactEntry = FindContact(hContact);
if(!pContactEntry && !bHidden)
{
AddContact(hContact);
return;
}
else if(!pContactEntry)
return;
if(!IsVisible(GetContactData(pContactEntry)))
RemoveContact(hContact);
}
//************************************************************************
// called when the contacts message count has changed
//************************************************************************
void CContactList::OnMessageCountChanged(MCONTACT hContact)
{
CListEntry<CContactListEntry *,CContactListGroup*> *pContactEntry = FindContact(hContact);
if(!pContactEntry)
{
AddContact(hContact);
return;
}
UpdateMessageCounter(pContactEntry);
if(!IsVisible(GetContactData(pContactEntry)))
RemoveContact(hContact);
((CListContainer<CContactListEntry*,CContactListGroup*>*)pContactEntry->GetParent())->sort(CContactList::CompareEntries);
}
//************************************************************************
// called when a contacts status has changed
//************************************************************************
void CContactList::OnStatusChange(MCONTACT hContact,int iStatus)
{
// find the entry in the list
CListEntry<CContactListEntry *,CContactListGroup*> *pContactEntry = FindContact(hContact);
if(!pContactEntry)
{
AddContact(hContact);
return;
}
CContactListEntry *pItemData = GetContactData(pContactEntry);
if(!pItemData) {
return;
}
// get the old status
int iOldStatus = pItemData->iStatus;
// Update the list entry
TCHAR *szStatus = pcli->pfnGetStatusModeDescription(iStatus, 0);
if(szStatus != NULL)
pItemData->strStatus =toTstring(szStatus);
pItemData->iStatus = iStatus;
// update the contacts group
CListContainer<CContactListEntry*,CContactListGroup*>* pGroup = ((CListContainer<CContactListEntry*,CContactListGroup*>*)pContactEntry->GetParent());
if(pGroup->GetType() != ROOT)
{
if(!db_mc_isSub(hContact) && iStatus == ID_STATUS_OFFLINE && iOldStatus != ID_STATUS_OFFLINE)
ChangeGroupObjectCounters(pGroup->GetGroupData()->strPath,0,-1);
else if(!db_mc_isSub(hContact) && iStatus != ID_STATUS_OFFLINE && iOldStatus == ID_STATUS_OFFLINE)
ChangeGroupObjectCounters(pGroup->GetGroupData()->strPath,0,1);
}
// check if the entry is still visible
if(!IsVisible(pItemData))
{
RemoveContact(hContact);
return;
}
// sort the list
pGroup->sort(CContactList::CompareEntries);
}
//************************************************************************
// called when a contacts status has changed
//************************************************************************
void CContactList::OnStatusChange(HANDLE hContact,int iStatus)
{
// find the entry in the list
CListEntry<CContactListEntry *,CContactListGroup*> *pContactEntry = FindContact(hContact);
if(!pContactEntry)
{
AddContact(hContact);
return;
}
CContactListEntry *pItemData = GetContactData(pContactEntry);
if(!pItemData) {
return;
}
// get the old status
int iOldStatus = pItemData->iStatus;
// Update the list entry
char *szStatus = (char *) CallService(MS_CLIST_GETSTATUSMODEDESCRIPTION, iStatus, 0);
if(szStatus != NULL)
pItemData->strStatus =toTstring(szStatus);
pItemData->iStatus = iStatus;
// update the contacts group
CListContainer<CContactListEntry*,CContactListGroup*>* pGroup = ((CListContainer<CContactListEntry*,CContactListGroup*>*)pContactEntry->GetParent());
if(pGroup->GetType() != ROOT)
{
if(!CAppletManager::IsSubContact(hContact) && iStatus == ID_STATUS_OFFLINE && iOldStatus != ID_STATUS_OFFLINE)
ChangeGroupObjectCounters(pGroup->GetGroupData()->strPath,0,-1);
else if(!CAppletManager::IsSubContact(hContact) && iStatus != ID_STATUS_OFFLINE && iOldStatus == ID_STATUS_OFFLINE)
ChangeGroupObjectCounters(pGroup->GetGroupData()->strPath,0,1);
}
// check if the entry is still visible
if(!IsVisible(pItemData))
{
RemoveContact(hContact);
return;
}
// sort the list
pGroup->sort(CContactList::CompareEntries);
}
//************************************************************************
// called when a contacts nickname has changed
//************************************************************************
void CContactList::OnContactNickChanged(MCONTACT hContact, tstring strNick)
{
CListEntry<CContactListEntry *,CContactListGroup*> *pContactEntry = FindContact(hContact);
if(!pContactEntry)
return;
if(pContactEntry->GetType() == CONTAINER)
{
CListContainer *pGroup = ((CListContainer<CContactListEntry*,CContactListGroup*>*)pContactEntry);
pGroup->GetGroupData()->strName = strNick;
tstring strPath = GetContactGroupPath(hContact);
pGroup->GetGroupData()->strPath = strPath + (strPath.empty()?_T(""):_T("\\")) + strNick;
}
CContactListEntry* pEntry = GetContactData(pContactEntry);
if(!pEntry) {
return;
}
pEntry->strName = strNick;
((CListContainer<CContactListEntry*,CContactListGroup*>*)pContactEntry->GetParent())->sort(CContactList::CompareEntries);
}
//************************************************************************
// refreshes the list
//************************************************************************
void CContactList::RefreshList()
{
if((db_get_b(NULL,"MetaContacts","Enabled",1) != 0) != m_bUseMetaContacts ||
CConfig::GetBoolSetting(CLIST_USEGROUPS) != m_bUseGroups)
{
InitializeGroupObjects();
Clear();
}
m_bUseGroups = CConfig::GetBoolSetting(CLIST_USEGROUPS);
m_bUseMetaContacts = db_get_b(NULL,"MetaContacts","Enabled",1) != 0;
CListEntry<CContactListEntry*,CContactListGroup*> *pContactEntry = NULL;
MCONTACT hContact = db_find_first();
while(hContact != NULL)
{
pContactEntry = FindContact(hContact);
if(!pContactEntry)
AddContact(hContact);
else if(pContactEntry && !IsVisible(GetContactData(pContactEntry)))
RemoveContact(hContact);
hContact = db_find_next(hContact);
}
}
int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
dIASSERT (Stride >= (int)sizeof(dContactGeom));
dIASSERT (g1->type == dTriMeshClass);
dIASSERT (SphereGeom->type == dSphereClass);
dIASSERT ((Flags & NUMC_MASK) >= 1);
dxTriMesh* TriMesh = (dxTriMesh*)g1;
// Init
const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache();
SphereCollider& Collider = pccColliderCache->_SphereCollider;
const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
dReal Radius = dGeomSphereGetRadius(SphereGeom);
// Sphere
Sphere Sphere;
Sphere.mCenter.x = Position[0];
Sphere.mCenter.y = Position[1];
Sphere.mCenter.z = Position[2];
Sphere.mRadius = Radius;
Matrix4x4 amatrix;
// TC results
if (TriMesh->doSphereTC) {
dxTriMesh::SphereTC* sphereTC = 0;
for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){
if (TriMesh->SphereTCCache[i].Geom == SphereGeom){
sphereTC = &TriMesh->SphereTCCache[i];
break;
}
}
if (!sphereTC){
TriMesh->SphereTCCache.push(dxTriMesh::SphereTC());
sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1];
sphereTC->Geom = SphereGeom;
}
// Intersect
Collider.SetTemporalCoherence(true);
Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
else {
Collider.SetTemporalCoherence(false);
Collider.Collide(pccColliderCache->defaultSphereCache, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
if (! Collider.GetContactStatus()) {
// no collision occurred
return 0;
}
// get results
int TriCount = Collider.GetNbTouchedPrimitives();
const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
if (TriCount != 0){
if (TriMesh->ArrayCallback != null){
TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount);
}
int OutTriCount = 0;
for (int i = 0; i < TriCount; i++){
if (OutTriCount == (Flags & NUMC_MASK)){
break;
}
const int TriIndex = Triangles[i];
dVector3 dv[3];
if (!Callback(TriMesh, SphereGeom, TriIndex))
continue;
FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
dVector3& v0 = dv[0];
dVector3& v1 = dv[1];
dVector3& v2 = dv[2];
dVector3 vu;
vu[0] = v1[0] - v0[0];
vu[1] = v1[1] - v0[1];
vu[2] = v1[2] - v0[2];
vu[3] = REAL(0.0);
dVector3 vv;
vv[0] = v2[0] - v0[0];
vv[1] = v2[1] - v0[1];
vv[2] = v2[2] - v0[2];
vv[3] = REAL(0.0);
// Get plane coefficients
dVector4 Plane;
dCROSS(Plane, =, vu, vv);
// Even though all triangles might be initially valid,
// a triangle may degenerate into a segment after applying
// space transformation.
if (!dSafeNormalize3(Plane)) {
continue;
}
/* If the center of the sphere is within the positive halfspace of the
* triangle's plane, allow a contact to be generated.
* If the center of the sphere made it into the positive halfspace of a
* back-facing triangle, then the physics update and/or velocity needs
* to be adjusted (penetration has occured anyway).
*/
dReal side = dDOT(Plane,Position) - dDOT(Plane, v0);
if(side < REAL(0.0)) {
continue;
}
dReal Depth;
dReal u, v;
if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){
continue; // Sphere doesn't hit triangle
}
if (Depth < REAL(0.0)){
continue; // Negative depth does not produce a contact
}
dVector3 ContactPos;
dReal w = REAL(1.0) - u - v;
ContactPos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v);
ContactPos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v);
ContactPos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v);
// Depth returned from GetContactData is depth along
// contact point - sphere center direction
// we'll project it to contact normal
dVector3 dir;
dir[0] = Position[0]-ContactPos[0];
dir[1] = Position[1]-ContactPos[1];
dir[2] = Position[2]-ContactPos[2];
dReal dirProj = dDOT(dir, Plane) / dSqrt(dDOT(dir, dir));
// Since Depth already had a requirement to be non-negative,
// negative direction projections should not be allowed as well,
// as otherwise the multiplication will result in negative contact depth.
if (dirProj < REAL(0.0)){
continue; // Zero contact depth could be ignored
}
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
Contact->pos[0] = ContactPos[0];
Contact->pos[1] = ContactPos[1];
Contact->pos[2] = ContactPos[2];
Contact->pos[3] = REAL(0.0);
// Using normal as plane (reversed)
Contact->normal[0] = -Plane[0];
Contact->normal[1] = -Plane[1];
Contact->normal[2] = -Plane[2];
Contact->normal[3] = REAL(0.0);
Contact->depth = Depth * dirProj;
//Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance
#if !defined MERGECONTACTS // Merge all contacts into 1
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
Contact->side2 = -1;
#endif // Otherwise assigned later
Contact->side1 = TriIndex;
OutTriCount++;
}
#if defined MERGECONTACTS // Merge all contacts into 1
if (OutTriCount > 0){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
Contact->side2 = -1;
if (OutTriCount > 1 && !(Flags & CONTACTS_UNIMPORTANT)){
dVector3 pos;
pos[0] = Contact->pos[0];
pos[1] = Contact->pos[1];
pos[2] = Contact->pos[2];
dVector3 normal;
normal[0] = Contact->normal[0] * Contact->depth;
normal[1] = Contact->normal[1] * Contact->depth;
normal[2] = Contact->normal[2] * Contact->depth;
int TriIndex = Contact->side1;
for (int i = 1; i < OutTriCount; i++){
dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
pos[0] += TempContact->pos[0];
pos[1] += TempContact->pos[1];
pos[2] += TempContact->pos[2];
normal[0] += TempContact->normal[0] * TempContact->depth;
normal[1] += TempContact->normal[1] * TempContact->depth;
normal[2] += TempContact->normal[2] * TempContact->depth;
TriIndex = (TriMesh->TriMergeCallback) ? TriMesh->TriMergeCallback(TriMesh, TriIndex, TempContact->side1) : -1;
}
Contact->side1 = TriIndex;
Contact->pos[0] = pos[0] / OutTriCount;
Contact->pos[1] = pos[1] / OutTriCount;
Contact->pos[2] = pos[2] / OutTriCount;
// Remember to divide in square space.
Contact->depth = dSqrt(dDOT(normal, normal) / OutTriCount);
if (Contact->depth > dEpsilon) { // otherwise the normal is too small
dVector3Copy(Contact->normal, normal);
dNormalize3(Contact->normal);
} // otherwise original Contact's normal would be used and it should be already normalized
}
return 1;
}
else return 0;
#elif defined MERGECONTACTNORMALS // Merge all normals, and distribute between all contacts
if (OutTriCount != 0){
if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){
dVector3 Normal;
dContactGeom* FirstContact = SAFECONTACT(Flags, Contacts, 0, Stride);
Normal[0] = FirstContact->normal[0] * FirstContact->depth;
Normal[1] = FirstContact->normal[1] * FirstContact->depth;
Normal[2] = FirstContact->normal[2] * FirstContact->depth;
Normal[3] = FirstContact->normal[3] * FirstContact->depth;
for (int i = 1; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
Normal[0] += Contact->normal[0] * Contact->depth;
Normal[1] += Contact->normal[1] * Contact->depth;
Normal[2] += Contact->normal[2] * Contact->depth;
Normal[3] += Contact->normal[3] * Contact->depth;
}
dNormalize3(Normal);
for (int i = 0; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
Contact->normal[0] = Normal[0];
Contact->normal[1] = Normal[1];
Contact->normal[2] = Normal[2];
Contact->normal[3] = Normal[3];
}
}
return OutTriCount;
}
else return 0;
#else // none of MERGECONTACTS and MERGECONTACTNORMALS // Just return
return OutTriCount;
#endif // MERGECONTACTS
}
else return 0;
int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
dIASSERT (Stride >= (int)sizeof(dContactGeom));
dIASSERT (g1->type == dTriMeshClass);
dIASSERT (SphereGeom->type == dSphereClass);
dIASSERT ((Flags & NUMC_MASK) >= 1);
dxTriMesh* TriMesh = (dxTriMesh*)g1;
// Init
const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
SphereCollider& Collider = TriMesh->_SphereCollider;
const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
dReal Radius = dGeomSphereGetRadius(SphereGeom);
// Sphere
Sphere Sphere;
Sphere.mCenter.x = Position[0];
Sphere.mCenter.y = Position[1];
Sphere.mCenter.z = Position[2];
Sphere.mRadius = Radius;
Matrix4x4 amatrix;
// TC results
if (TriMesh->doSphereTC) {
dxTriMesh::SphereTC* sphereTC = 0;
for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){
if (TriMesh->SphereTCCache[i].Geom == SphereGeom){
sphereTC = &TriMesh->SphereTCCache[i];
break;
}
}
if (!sphereTC){
TriMesh->SphereTCCache.push(dxTriMesh::SphereTC());
sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1];
sphereTC->Geom = SphereGeom;
}
// Intersect
Collider.SetTemporalCoherence(true);
Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
else {
Collider.SetTemporalCoherence(false);
Collider.Collide(dxTriMesh::defaultSphereCache, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
if (! Collider.GetContactStatus()) {
// no collision occurred
return 0;
}
// get results
int TriCount = Collider.GetNbTouchedPrimitives();
const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
if (TriCount != 0){
if (TriMesh->ArrayCallback != null){
TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount);
}
int OutTriCount = 0;
for (int i = 0; i < TriCount; i++){
if (OutTriCount == (Flags & NUMC_MASK)){
break;
}
const int TriIndex = Triangles[i];
dVector3 dv[3];
if (!Callback(TriMesh, SphereGeom, TriIndex))
continue;
FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
dVector3& v0 = dv[0];
dVector3& v1 = dv[1];
dVector3& v2 = dv[2];
dVector3 vu;
vu[0] = v1[0] - v0[0];
vu[1] = v1[1] - v0[1];
vu[2] = v1[2] - v0[2];
vu[3] = REAL(0.0);
dVector3 vv;
vv[0] = v2[0] - v0[0];
vv[1] = v2[1] - v0[1];
vv[2] = v2[2] - v0[2];
vv[3] = REAL(0.0);
// Get plane coefficients
dVector4 Plane;
dCROSS(Plane, =, vu, vv);
dReal Area = dSqrt(dDOT(Plane, Plane)); // We can use this later
Plane[0] /= Area;
Plane[1] /= Area;
Plane[2] /= Area;
Plane[3] = dDOT(Plane, v0);
/* If the center of the sphere is within the positive halfspace of the
* triangle's plane, allow a contact to be generated.
* If the center of the sphere made it into the positive halfspace of a
* back-facing triangle, then the physics update and/or velocity needs
* to be adjusted (penetration has occured anyway).
*/
dReal side = dDOT(Plane,Position) - Plane[3];
if(side < REAL(0.0)) {
continue;
}
dReal Depth;
dReal u, v;
if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){
continue; // Sphere doesn't hit triangle
}
if (Depth < REAL(0.0)){
Depth = REAL(0.0);
}
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
dReal w = REAL(1.0) - u - v;
Contact->pos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v);
Contact->pos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v);
Contact->pos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v);
Contact->pos[3] = REAL(0.0);
// Using normal as plane (reversed)
Contact->normal[0] = -Plane[0];
Contact->normal[1] = -Plane[1];
Contact->normal[2] = -Plane[2];
Contact->normal[3] = REAL(0.0);
// Depth returned from GetContactData is depth along
// contact point - sphere center direction
// we'll project it to contact normal
dVector3 dir;
dir[0] = Position[0]-Contact->pos[0];
dir[1] = Position[1]-Contact->pos[1];
dir[2] = Position[2]-Contact->pos[2];
dReal dirProj = dDOT(dir, Plane) / dSqrt(dDOT(dir, dir));
Contact->depth = Depth * dirProj;
//Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance
Contact->side1 = TriIndex;
//Contact->g1 = TriMesh;
//Contact->g2 = SphereGeom;
OutTriCount++;
}
#ifdef MERGECONTACTS // Merge all contacts into 1
if (OutTriCount != 0){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){
Contact->normal[0] *= Contact->depth;
Contact->normal[1] *= Contact->depth;
Contact->normal[2] *= Contact->depth;
Contact->normal[3] *= Contact->depth;
for (int i = 1; i < OutTriCount; i++){
dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
Contact->pos[0] += TempContact->pos[0];
Contact->pos[1] += TempContact->pos[1];
Contact->pos[2] += TempContact->pos[2];
Contact->pos[3] += TempContact->pos[3];
Contact->normal[0] += TempContact->normal[0] * TempContact->depth;
Contact->normal[1] += TempContact->normal[1] * TempContact->depth;
Contact->normal[2] += TempContact->normal[2] * TempContact->depth;
Contact->normal[3] += TempContact->normal[3] * TempContact->depth;
}
Contact->pos[0] /= OutTriCount;
Contact->pos[1] /= OutTriCount;
Contact->pos[2] /= OutTriCount;
Contact->pos[3] /= OutTriCount;
// Remember to divide in square space.
Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal) / OutTriCount);
dNormalize3(Contact->normal);
}
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
// TODO:
// Side1 now contains index of triangle that gave first hit
// Probably we should find index of triangle with deepest penetration
return 1;
}
else return 0;
#elif defined MERGECONTACTNORMALS // Merge all normals, and distribute between all contacts
if (OutTriCount != 0){
if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){
dVector3& Normal = SAFECONTACT(Flags, Contacts, 0, Stride)->normal;
Normal[0] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
Normal[1] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
Normal[2] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
Normal[3] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
for (int i = 1; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
Normal[0] += Contact->normal[0] * Contact->depth;
Normal[1] += Contact->normal[1] * Contact->depth;
Normal[2] += Contact->normal[2] * Contact->depth;
Normal[3] += Contact->normal[3] * Contact->depth;
}
dNormalize3(Normal);
for (int i = 1; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
Contact->normal[0] = Normal[0];
Contact->normal[1] = Normal[1];
Contact->normal[2] = Normal[2];
Contact->normal[3] = Normal[3];
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
}
}
else{
SAFECONTACT(Flags, Contacts, 0, Stride)->g1 = TriMesh;
SAFECONTACT(Flags, Contacts, 0, Stride)->g2 = SphereGeom;
}
return OutTriCount;
}
else return 0;
#else //MERGECONTACTNORMALS // Just gather penetration depths and return
for (int i = 0; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
//Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal));
/*Contact->normal[0] /= Contact->depth;
Contact->normal[1] /= Contact->depth;
Contact->normal[2] /= Contact->depth;
Contact->normal[3] /= Contact->depth;*/
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
}
return OutTriCount;
#endif // MERGECONTACTS
}
else return 0;
int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
dIASSERT (Stride >= (int)sizeof(dContactGeom));
dIASSERT (g1->type == dTriMeshClass);
dIASSERT (SphereGeom->type == dSphereClass);
dIASSERT ((Flags & NUMC_MASK) >= 1);
dxTriMesh* TriMesh = (dxTriMesh*)g1;
// Init
const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind();
dIASSERT(uiTLSKind == SphereGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
SphereCollider& Collider = pccColliderCache->_SphereCollider;
const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
dReal Radius = dGeomSphereGetRadius(SphereGeom);
// Sphere
Sphere Sphere;
dCopyVector3(Sphere.mCenter, Position);
Sphere.mRadius = Radius;
Matrix4x4 amatrix;
// TC results
if (TriMesh->doSphereTC) {
dxTriMesh::SphereTC* sphereTC = 0;
for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){
if (TriMesh->SphereTCCache[i].Geom == SphereGeom){
sphereTC = &TriMesh->SphereTCCache[i];
break;
}
}
if (!sphereTC){
TriMesh->SphereTCCache.push(dxTriMesh::SphereTC());
sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1];
sphereTC->Geom = SphereGeom;
}
// Intersect
Collider.SetTemporalCoherence(true);
Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
else {
Collider.SetTemporalCoherence(false);
Collider.Collide(pccColliderCache->defaultSphereCache, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
if (! Collider.GetContactStatus()) {
// no collision occurred
return 0;
}
// get results
int TriCount = Collider.GetNbTouchedPrimitives();
const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
if (TriCount != 0){
if (TriMesh->ArrayCallback != null){
TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount);
}
int OutTriCount = 0;
for (int i = 0; i < TriCount; i++){
if (OutTriCount == (Flags & NUMC_MASK)){
break;
}
const int TriIndex = Triangles[i];
dVector3 dv[3];
if (!Callback(TriMesh, SphereGeom, TriIndex))
continue;
FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
dVector3& v0 = dv[0];
dVector3& v1 = dv[1];
dVector3& v2 = dv[2];
dVector3 vu;
dSubtractVectors3r4(vu, v1, v0);
vu[3] = REAL(0.0);
dVector3 vv;
dSubtractVectors3r4(vv, v2, v0);
vv[3] = REAL(0.0);
// Get plane coefficients
dVector4 Plane;
dCalcVectorCross3(Plane, vu, vv);
// Even though all triangles might be initially valid,
// a triangle may degenerate into a segment after applying
// space transformation.
if (!dSafeNormalize3(Plane)) {
continue;
}
/* If the center of the sphere is within the positive halfspace of the
* triangle's plane, allow a contact to be generated.
* If the center of the sphere made it into the positive halfspace of a
* back-facing triangle, then the physics update and/or velocity needs
* to be adjusted (penetration has occured anyway).
*/
dReal side = dCalcVectorDot3(Plane, Position) - dCalcVectorDot3(Plane, v0);
if(side < REAL(0.0))
{
continue;
}
dReal Depth;
dReal u, v;
if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){
continue; // Sphere doesn't hit triangle
}
if (Depth < REAL(0.0)){
continue; // Negative depth does not produce a contact
}
dVector3 ContactPos;
dReal w = REAL(1.0) - u - v;
dAddScaledVectors3r4(ContactPos, v1, v2, u, v);
dAddScaledVector3r4(ContactPos, v0, w);
// Depth returned from GetContactData is depth along
// contact point - sphere center direction
// we'll project it to contact normal
dVector3 dir;
dSubtractVectors3r4(dir, Position, ContactPos);
dReal dirProj = dCalcVectorDot3(dir, Plane) / dCalcVectorLength3(dir);
// Since Depth already had a requirement to be non-negative,
// negative direction projections should not be allowed as well,
// as otherwise the multiplication will result in negative contact depth.
if (dirProj < REAL(0.0))
continue; // Zero contact depth could be ignored
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
dCopyVector3r4(Contact->pos, ContactPos);
// Using normal as plane (reversed)
dCopyNegatedVector3r4(Contact->normal, Plane);
Contact->depth = Depth * dirProj;
//Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance
// We need to set these unconditionally, as the merging may fail! - Bram
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
Contact->side2 = -1;
Contact->side1 = TriIndex;
OutTriCount++;
}
if (OutTriCount > 0)
{
if (TriMesh->SphereContactsMergeOption == MERGE_CONTACTS_FULLY)
{
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
Contact->side2 = -1;
if (OutTriCount > 1 && !(Flags & CONTACTS_UNIMPORTANT))
{
dVector3 pos;
dCopyVector3r4(pos, Contact->pos);
dVector3 normal;
dCopyScaledVector3r4(normal, Contact->normal, Contact->depth);
int TriIndex = Contact->side1;
for (int i = 1; i < OutTriCount; i++)
{
dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
dAddVector3r4(pos, TempContact->pos);
dAddScaledVector3r4(normal, TempContact->normal, TempContact->depth);
TriIndex = (TriMesh->TriMergeCallback) ? TriMesh->TriMergeCallback(TriMesh, TriIndex, TempContact->side1) : -1;
}
Contact->side1 = TriIndex;
dReal invOutTriCount = dRecip(OutTriCount);
dCopyScaledVector3r4(Contact->pos, pos, invOutTriCount);
if ( !dSafeNormalize3(normal) )
return OutTriCount; // Cannot merge in this pathological case
// Using a merged normal, means that for each intersection, this new normal will be less effective in solving the intersection.
// That is why we need to correct this by increasing the depth for each intersection.
// The maximum of the adjusted depths is our newly merged depth value - Bram.
dReal mergedDepth = REAL(0.0);
dReal minEffectiveness = REAL(0.5);
for ( int i = 0; i < OutTriCount; ++i )
{
dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
dReal effectiveness = dCalcVectorDot3(normal, TempContact->normal);
if ( effectiveness < dEpsilon )
return OutTriCount; // Cannot merge this pathological case
// Cap our adjustment for the new normal to a factor 2, meaning a 60 deg change in normal.
effectiveness = ( effectiveness < minEffectiveness ) ? minEffectiveness : effectiveness;
dReal adjusted = TempContact->depth / effectiveness;
mergedDepth = ( mergedDepth < adjusted ) ? adjusted : mergedDepth;
}
Contact->depth = mergedDepth;
dCopyVector3r4(Contact->normal, normal);
}
return 1;
}
else if (TriMesh->SphereContactsMergeOption == MERGE_CONTACT_NORMALS)
{
if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT))
{
dVector3 Normal;
dContactGeom* FirstContact = SAFECONTACT(Flags, Contacts, 0, Stride);
dCopyScaledVector3r4(Normal, FirstContact->normal, FirstContact->depth);
for (int i = 1; i < OutTriCount; i++)
{
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
dAddScaledVector3r4(Normal, Contact->normal, Contact->depth);
}
dNormalize3(Normal);
for (int i = 0; i < OutTriCount; i++)
{
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
dCopyVector3r4(Contact->normal, Normal);
}
}
return OutTriCount;
}
else
{
dIASSERT(TriMesh->SphereContactsMergeOption == DONT_MERGE_CONTACTS);
return OutTriCount;
}
}
else return 0;
}
else return 0;
}
//************************************************************************
// called when a contacts group has changed
//************************************************************************
void CContactList::OnContactGroupChanged(MCONTACT hContact,tstring strGroup)
{
bool bMetaContact = false;
strGroup = GetContactGroupPath(hContact);
// Decrease the membercount of the old group
CListEntry<CContactListEntry *,CContactListGroup*> *pContactEntry = FindContact(hContact);
CContactListGroup *pOldGroup = NULL;
// If the contactentry was not found, try adding the contact (metacontacts fix)
if(!pContactEntry) {
return;
}
if(pContactEntry->GetType() == CONTAINER)
bMetaContact = true;
CListContainer<CContactListEntry*,CContactListGroup*>* pContainer = ((CListContainer<CContactListEntry*,CContactListGroup*>*)pContactEntry->GetParent());
// Update the contacts group if it has one
if(pContainer->GetType() != ROOT)
{
pOldGroup = pContainer->GetGroupData();
if(!db_mc_isSub(hContact))
ChangeGroupObjectCounters(pOldGroup->strPath,-1);
}
// increase the membercount of the new group, and check if it needs to be created
if(!strGroup.empty())
{
CContactListGroup *pGroup = GetGroupObjectByPath(strGroup);
if(!pGroup)
pGroup = CreateGroupObjectByPath(strGroup);
if(!db_mc_isSub(hContact))
ChangeGroupObjectCounters(strGroup,1);
}
// move subcontacts
if(pContactEntry->GetType() == CONTAINER)
{
CListContainer<CContactListEntry*,CContactListGroup*> *pGroup = (CListContainer<CContactListEntry*,CContactListGroup*>*)pContactEntry;
CListContainer<CContactListEntry*,CContactListGroup*>::iterator iter = pGroup->begin();
while(!pGroup->empty())
{
iter = pGroup->begin();
if((*iter)->GetType() == ITEM)
OnContactGroupChanged(GetContactData(*iter)->hHandle,_T(""));
Sleep(1);
}
}
// update the list
RemoveContact(hContact);
AddContact(hContact);
if(bMetaContact)
{
tstring strName = CAppletManager::GetContactDisplayname(hContact);
tstring strPath = _T("");
if(pOldGroup)
strPath += pOldGroup->strPath;
strPath += (strPath.empty()?_T(""):_T("\\")) + strName;
DeleteGroupObjectByPath(strPath);
}
// check if the old group ( if it exists ) needs to be deleted
if(pOldGroup && !pOldGroup->hMetaContact && pOldGroup->iMembers <= 0 && pOldGroup->iGroups <= 0)
DeleteGroupObjectByPath(pOldGroup->strPath);
}
int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
dxTriMesh* TriMesh = (dxTriMesh*)g1;
// Init
const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
SphereCollider& Collider = TriMesh->_SphereCollider;
const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
dReal Radius = dGeomSphereGetRadius(SphereGeom);
// Sphere
Sphere Sphere;
Sphere.mCenter.x = Position[0];
Sphere.mCenter.y = Position[1];
Sphere.mCenter.z = Position[2];
Sphere.mRadius = Radius;
Matrix4x4 amatrix;
// TC results
if (TriMesh->doSphereTC) {
dxTriMesh::SphereTC* sphereTC = 0;
for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){
if (TriMesh->SphereTCCache[i].Geom == SphereGeom){
sphereTC = &TriMesh->SphereTCCache[i];
break;
}
}
if (!sphereTC){
TriMesh->SphereTCCache.push(dxTriMesh::SphereTC());
sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1];
sphereTC->Geom = SphereGeom;
}
// Intersect
Collider.SetTemporalCoherence(true);
Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
else {
Collider.SetTemporalCoherence(false);
Collider.Collide(dxTriMesh::defaultSphereCache, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
if (!Collider.GetContactStatus()) {
/* no collision occurred */
return 0;
}
// get results
int TriCount = Collider.GetNbTouchedPrimitives();
const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
if (TriCount != 0){
if (TriMesh->ArrayCallback != null){
TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount);
}
int OutTriCount = 0;
for (int i = 0; i < TriCount; i++){
if (OutTriCount == (Flags & 0xffff)){
break;
}
const int& TriIndex = Triangles[i];
dVector3 dv[3];
FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
dVector3& v0 = dv[0];
dVector3& v1 = dv[1];
dVector3& v2 = dv[2];
dVector3 vu;
vu[0] = v1[0] - v0[0];
vu[1] = v1[1] - v0[1];
vu[2] = v1[2] - v0[2];
vu[3] = REAL(0.0);
dVector3 vv;
vv[0] = v2[0] - v0[0];
vv[1] = v2[1] - v0[1];
vv[2] = v2[2] - v0[2];
vv[3] = REAL(0.0);
dReal Depth;
float u, v;
if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){
continue; // Sphere doesnt hit triangle
}
dReal w = REAL(1.0) - u - v;
if (Depth < REAL(0.0)){
Depth = REAL(0.0);
}
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
Contact->pos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v);
Contact->pos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v);
Contact->pos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v);
Contact->pos[3] = REAL(0.0);
dVector4 Plane;
dCROSS(Plane, =, vv, vu); // Reversed
Plane[3] = dDOT(Plane, v0); // Using normal as plane.
dReal Area = dSqrt(dDOT(Plane, Plane)); // We can use this later
Plane[0] /= Area;
Plane[1] /= Area;
Plane[2] /= Area;
Plane[3] /= Area;
Contact->normal[0] = Plane[0];
Contact->normal[1] = Plane[1];
Contact->normal[2] = Plane[2];
Contact->normal[3] = REAL(0.0);
Contact->depth = Depth;
//Contact->g1 = TriMesh;
//Contact->g2 = SphereGeom;
OutTriCount++;
}
#ifdef MERGECONTACTS // Merge all contacts into 1
if (OutTriCount != 0){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
if (OutTriCount != 1){
Contact->normal[0] *= Contact->depth;
Contact->normal[1] *= Contact->depth;
Contact->normal[2] *= Contact->depth;
Contact->normal[3] *= Contact->depth;
for (int i = 1; i < OutTriCount; i++){
dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
Contact->pos[0] += TempContact->pos[0];
Contact->pos[1] += TempContact->pos[1];
Contact->pos[2] += TempContact->pos[2];
Contact->pos[3] += TempContact->pos[3];
Contact->normal[0] += TempContact->normal[0] * TempContact->depth;
Contact->normal[1] += TempContact->normal[1] * TempContact->depth;
Contact->normal[2] += TempContact->normal[2] * TempContact->depth;
Contact->normal[3] += TempContact->normal[3] * TempContact->depth;
}
Contact->pos[0] /= OutTriCount;
Contact->pos[1] /= OutTriCount;
Contact->pos[2] /= OutTriCount;
Contact->pos[3] /= OutTriCount;
// Remember to divide in square space.
Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal) / OutTriCount);
dNormalize3(Contact->normal);
}
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
return 1;
}
else return 0;
#elif defined MERGECONTACTNORMALS // Merge all normals, and distribute between all contacts
if (OutTriCount != 0){
if (OutTriCount != 1){
dVector3& Normal = SAFECONTACT(Flags, Contacts, 0, Stride)->normal;
Normal[0] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
Normal[1] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
Normal[2] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
Normal[3] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
for (int i = 1; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
Normal[0] += Contact->normal[0] * Contact->depth;
Normal[1] += Contact->normal[1] * Contact->depth;
Normal[2] += Contact->normal[2] * Contact->depth;
Normal[3] += Contact->normal[3] * Contact->depth;
}
dNormalize3(Normal);
for (int i = 1; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
Contact->normal[0] = Normal[0];
Contact->normal[1] = Normal[1];
Contact->normal[2] = Normal[2];
Contact->normal[3] = Normal[3];
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
}
}
else{
SAFECONTACT(Flags, Contacts, 0, Stride)->g1 = TriMesh;
SAFECONTACT(Flags, Contacts, 0, Stride)->g2 = SphereGeom;
}
return OutTriCount;
}
else return 0;
#else //MERGECONTACTNORMALS // Just gather penetration depths and return
for (int i = 0; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
//Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal));
/*Contact->normal[0] /= Contact->depth;
Contact->normal[1] /= Contact->depth;
Contact->normal[2] /= Contact->depth;
Contact->normal[3] /= Contact->depth;*/
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
}
return OutTriCount;
#endif // MERGECONTACTS
}
else return 0;
