summaryrefslogtreecommitdiff
path: root/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'examples/SharedMemory/PhysicsServerCommandProcessor.cpp')
-rw-r--r--examples/SharedMemory/PhysicsServerCommandProcessor.cpp121
1 files changed, 87 insertions, 34 deletions
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 0d826af41..e949f3e1f 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -8073,21 +8073,26 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
{
return false;
}
- int numSoftbodyContact = 0;
- for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
- {
- numSoftbodyContact += deformWorld->getSoftBodyArray()[i]->m_faceRigidContacts.size();
- }
- int num_contact_points = m_data->m_cachedContactPoints.size();
- m_data->m_cachedContactPoints.reserve(num_contact_points + numSoftbodyContact);
for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
{
btSoftBody* psb = deformWorld->getSoftBodyArray()[i];
+ btAlignedObjectArray<b3ContactPointData> distinctContactPoints;
+ btAlignedObjectArray<btSoftBody::Node*> nodesInContact;
for (int c = 0; c < psb->m_faceRigidContacts.size(); c++)
{
- const btSoftBody::DeformableFaceRigidContact* contact = &psb->m_faceRigidContacts[i];
- //convert rigidbody contact
+ const btSoftBody::DeformableFaceRigidContact* contact = &psb->m_faceRigidContacts[c];
+ // calculate normal and tangent impulse
+ btVector3 impulse = contact->m_cti.m_impulse;
+ btVector3 impulseNormal = impulse.dot(contact->m_cti.m_normal) * contact->m_cti.m_normal;
+ btVector3 impulseTangent = impulse - impulseNormal;
+ // get node in contact
+ int contactNodeIdx = contact->m_bary.maxAxis();
+ btSoftBody::Node* node = contact->m_face->m_n[contactNodeIdx];
+ // check if node is already in the list
+ int idx = nodesInContact.findLinearSearch2(node);
+
+ //apply the filter, if the user provides it
int linkIndexA = -1;
int linkIndexB = -1;
int objectIndexA = psb->getUserIndex2();
@@ -8104,8 +8109,6 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
linkIndexB = mblB->m_link;
objectIndexB = mblB->m_multiBody->getUserIndex2();
}
-
- //apply the filter, if the user provides it
bool swap = false;
if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0)
{
@@ -8151,37 +8154,87 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
{
continue;
}
- b3ContactPointData pt;
- pt.m_bodyUniqueIdA = objectIndexA;
- pt.m_bodyUniqueIdB = objectIndexB;
- pt.m_contactDistance = contact->m_cti.m_offset;
- pt.m_contactFlags = 0;
- pt.m_linkIndexA = linkIndexA;
- pt.m_linkIndexB = linkIndexB;
- for (int j = 0; j < 3; j++)
+
+ if (idx < 0)
{
- if (swap)
+ // add new node and contact point
+ nodesInContact.push_back(node);
+ b3ContactPointData pt;
+ pt.m_bodyUniqueIdA = objectIndexA;
+ pt.m_bodyUniqueIdB = objectIndexB;
+ pt.m_contactDistance = -contact->m_cti.m_offset;
+ pt.m_contactFlags = 0;
+ pt.m_linkIndexA = linkIndexA;
+ pt.m_linkIndexB = linkIndexB;
+ for (int j = 0; j < 3; j++)
{
- pt.m_contactNormalOnBInWS[j] = -contact->m_cti.m_normal[j];
- pt.m_positionOnAInWS[j] = contact->m_cti.m_normal[j];
- pt.m_positionOnBInWS[j] = -contact->m_cti.m_normal[j];
+ if (swap)
+ {
+ pt.m_contactNormalOnBInWS[j] = -contact->m_cti.m_normal[j];
+ pt.m_positionOnAInWS[j] = node->m_x[j] - pt.m_contactDistance * pt.m_contactNormalOnBInWS[j]; // not really precise because of margins in btSoftBody.cpp:line 2912
+ // node is force application point, therefore node position is contact point (not contact->m_contactPoint, because not equal to node)
+ pt.m_positionOnBInWS[j] = node->m_x[j];
+ }
+ else
+ {
+ pt.m_contactNormalOnBInWS[j] = contact->m_cti.m_normal[j];
+ // node is force application point, therefore node position is contact point (not contact->m_contactPoint, because not equal to node)
+ pt.m_positionOnAInWS[j] = node->m_x[j];
+ pt.m_positionOnBInWS[j] = node->m_x[j] - pt.m_contactDistance * pt.m_contactNormalOnBInWS[j]; // not really precise because of margins in btSoftBody.cpp:line 2912
+ }
}
- else
+ pt.m_normalForce = (impulseNormal / m_data->m_physicsDeltaTime).norm();
+ pt.m_linearFrictionForce1 = (impulseTangent.dot(contact->t1) * contact->t1 / m_data->m_physicsDeltaTime).norm();
+ pt.m_linearFrictionForce2 = (impulseTangent.dot(contact->t2) * contact->t2 / m_data->m_physicsDeltaTime).norm();
+ for (int j = 0; j < 3; j++)
{
- pt.m_contactNormalOnBInWS[j] = contact->m_cti.m_normal[j];
- pt.m_positionOnAInWS[j] = -contact->m_cti.m_normal[j];
- pt.m_positionOnBInWS[j] = contact->m_cti.m_normal[j];
+ pt.m_linearFrictionDirection1[j] = contact->t1[j];
+ pt.m_linearFrictionDirection2[j] = contact->t2[j];
}
+ distinctContactPoints.push_back(pt);
}
- pt.m_normalForce = 1;
- pt.m_linearFrictionForce1 = 0;
- pt.m_linearFrictionForce2 = 0;
- for (int j = 0; j < 3; j++)
+ else
{
- pt.m_linearFrictionDirection1[j] = 0;
- pt.m_linearFrictionDirection2[j] = 0;
+ // add values to existing contact point
+ b3ContactPointData* pt = &distinctContactPoints[idx];
+ // current normal force of node
+ btVector3 normalForce = btVector3(btScalar(pt->m_contactNormalOnBInWS[0]),
+ btScalar(pt->m_contactNormalOnBInWS[1]),
+ btScalar(pt->m_contactNormalOnBInWS[2])) * pt->m_normalForce;
+ // add normal force of additional node contact
+ btScalar swapFactor = swap ? -1.0 : 1.0;
+ normalForce += swapFactor * contact->m_cti.m_normal * (impulseNormal / m_data->m_physicsDeltaTime).norm();
+ // get magnitude of normal force
+ pt->m_normalForce = normalForce.norm();
+ // get direction of normal force
+ if (!normalForce.fuzzyZero())
+ {
+ // normalize for unit vectors if above numerical threshold
+ normalForce.normalize();
+ for (int j = 0; j < 3; j++)
+ {
+ pt->m_contactNormalOnBInWS[j] = normalForce[j];
+ }
+ }
+
+ // add magnitudes of tangential forces in existing directions
+ btVector3 linearFrictionDirection1 = btVector3(btScalar(pt->m_linearFrictionDirection1[0]),
+ btScalar(pt->m_linearFrictionDirection1[1]),
+ btScalar(pt->m_linearFrictionDirection1[2]));
+ btVector3 linearFrictionDirection2 = btVector3(btScalar(pt->m_linearFrictionDirection2[0]),
+ btScalar(pt->m_linearFrictionDirection2[1]),
+ btScalar(pt->m_linearFrictionDirection2[2]));
+ pt->m_linearFrictionForce1 = (impulseTangent.dot(linearFrictionDirection1) * linearFrictionDirection1 / m_data->m_physicsDeltaTime).norm();
+ pt->m_linearFrictionForce2 = (impulseTangent.dot(linearFrictionDirection2) * linearFrictionDirection2 / m_data->m_physicsDeltaTime).norm();
}
- m_data->m_cachedContactPoints.push_back(pt);
+ }
+
+ int num_contact_points = m_data->m_cachedContactPoints.size() + distinctContactPoints.size();
+ m_data->m_cachedContactPoints.reserve(num_contact_points);
+ // add points to contact points cache
+ for (int p = 0; p < distinctContactPoints.size(); p++)
+ {
+ m_data->m_cachedContactPoints.push_back(distinctContactPoints[p]);
}
}
#endif
View Lorry Controller Status