static void inspectJoints(void) { const dReal forcelimit = 2000.0; int i; for (i=0; i<SEGMCNT-1; i++) { if (dJointGetBody(hinges[i], 0)) { // This joint has not snapped already... inspect it. dReal l0 = dCalcVectorLength3(jfeedbacks[i].f1); dReal l1 = dCalcVectorLength3(jfeedbacks[i].f2); colours[i+0] = 0.95*colours[i+0] + 0.05 * l0/forcelimit; colours[i+1] = 0.95*colours[i+1] + 0.05 * l1/forcelimit; if (l0 > forcelimit || l1 > forcelimit) stress[i]++; else stress[i]=0; if (stress[i]>4) { // Low-pass filter the noisy feedback data. // Only after 4 consecutive timesteps with excessive load, snap. fprintf(stderr,"SNAP! (that was the sound of joint %d breaking)\n", i); dJointAttach (hinges[i], 0, 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; }
void dxJointTransmission::getInfo2( dReal worldFPS, dReal /*worldERP*/, const Info2Descr* info ) { dVector3 a[2], n[2], l[2], r[2], c[2], s, t, O, d, z, u, v; dReal theta, delta, nn, na_0, na_1, cosphi, sinphi, m; const dReal *p[2], *omega[2]; int i; // Transform all needed quantities to the global frame. for (i = 0 ; i < 2 ; i += 1) { dBodyGetRelPointPos(node[i].body, anchors[i][0], anchors[i][1], anchors[i][2], a[i]); dBodyVectorToWorld(node[i].body, axes[i][0], axes[i][1], axes[i][2], n[i]); p[i] = dBodyGetPosition(node[i].body); omega[i] = dBodyGetAngularVel(node[i].body); } if (update) { // Make sure both gear reference frames end up with the same // handedness. if (dCalcVectorDot3(n[0], n[1]) < 0) { dNegateVector3(axes[0]); dNegateVector3(n[0]); } } // Calculate the mesh geometry based on the current mode. switch (mode) { case dTransmissionParallelAxes: // Simply calculate the contact point as the point on the // baseline that will yield the correct ratio. dIASSERT (ratio > 0); dSubtractVectors3(d, a[1], a[0]); dAddScaledVectors3(c[0], a[0], d, 1, ratio / (1 + ratio)); dCopyVector3(c[1], c[0]); dNormalize3(d); for (i = 0 ; i < 2 ; i += 1) { dCalcVectorCross3(l[i], d, n[i]); } break; case dTransmissionIntersectingAxes: // Calculate the line of intersection between the planes of the // gears. dCalcVectorCross3(l[0], n[0], n[1]); dCopyVector3(l[1], l[0]); nn = dCalcVectorDot3(n[0], n[1]); dIASSERT(fabs(nn) != 1); na_0 = dCalcVectorDot3(n[0], a[0]); na_1 = dCalcVectorDot3(n[1], a[1]); dAddScaledVectors3(O, n[0], n[1], (na_0 - na_1 * nn) / (1 - nn * nn), (na_1 - na_0 * nn) / (1 - nn * nn)); // Find the contact point as: // // c = ((r_a - O) . l) l + O // // where r_a the anchor point of either gear and l, O the tangent // line direction and origin. for (i = 0 ; i < 2 ; i += 1) { dSubtractVectors3(d, a[i], O); m = dCalcVectorDot3(d, l[i]); dAddScaledVectors3(c[i], O, l[i], 1, m); } break; case dTransmissionChainDrive: dSubtractVectors3(d, a[0], a[1]); m = dCalcVectorLength3(d); dIASSERT(m > 0); // Caclulate the angle of the contact point relative to the // baseline. cosphi = clamp((radii[1] - radii[0]) / m, REAL(-1.0), REAL(1.0)); // Force into range to fix possible computation errors sinphi = dSqrt (REAL(1.0) - cosphi * cosphi); dNormalize3(d); for (i = 0 ; i < 2 ; i += 1) { // Calculate the contact radius in the local reference // frame of the chain. This has axis x pointing along the // baseline, axis y pointing along the sprocket axis and // the remaining axis normal to both. u[0] = radii[i] * cosphi; u[1] = 0; u[2] = radii[i] * sinphi; // Transform the contact radius into the global frame. dCalcVectorCross3(z, d, n[i]); v[0] = dCalcVectorDot3(d, u); v[1] = dCalcVectorDot3(n[i], u); v[2] = dCalcVectorDot3(z, u); // Finally calculate contact points and l. dAddVectors3(c[i], a[i], v); dCalcVectorCross3(l[i], v, n[i]); dNormalize3(l[i]); // printf ("%d: %f, %f, %f\n", // i, l[i][0], l[i][1], l[i][2]); } break; } if (update) { // We need to calculate an initial reference frame for each // wheel which we can measure the current phase against. This // frame will have the initial contact radius as the x axis, // the wheel axis as the z axis and their cross product as the // y axis. for (i = 0 ; i < 2 ; i += 1) { dSubtractVectors3 (r[i], c[i], a[i]); radii[i] = dCalcVectorLength3(r[i]); dIASSERT(radii[i] > 0); dBodyVectorFromWorld(node[i].body, r[i][0], r[i][1], r[i][2], reference[i]); dNormalize3(reference[i]); dCopyVector3(reference[i] + 8, axes[i]); dCalcVectorCross3(reference[i] + 4, reference[i] + 8, reference[i]); // printf ("%f\n", dDOT(r[i], n[i])); // printf ("(%f, %f, %f,\n %f, %f, %f,\n %f, %f, %f)\n", // reference[i][0],reference[i][1],reference[i][2], // reference[i][4],reference[i][5],reference[i][6], // reference[i][8],reference[i][9],reference[i][10]); radii[i] = radii[i]; phase[i] = 0; } ratio = radii[0] / radii[1]; update = 0; } for (i = 0 ; i < 2 ; i += 1) { dReal phase_hat; dSubtractVectors3 (r[i], c[i], a[i]); // Transform the (global) contact radius into the gear's // reference frame. dBodyVectorFromWorld (node[i].body, r[i][0], r[i][1], r[i][2], s); dMultiply0_331(t, reference[i], s); // Now simply calculate its angle on the plane relative to the // x-axis which is the initial contact radius. This will be // an angle between -pi and pi that is coterminal with the // actual phase of the wheel. To find the real phase we // estimate it by adding omega * dt to the old phase and then // find the closest angle to that, that is coterminal to // theta. theta = atan2(t[1], t[0]); phase_hat = phase[i] + dCalcVectorDot3(omega[i], n[i]) / worldFPS; if (phase_hat > M_PI_2) { if (theta < 0) { theta += (dReal)(2 * M_PI); } theta += (dReal)(floor(phase_hat / (2 * M_PI)) * (2 * M_PI)); } else if (phase_hat < -M_PI_2) { if (theta > 0) { theta -= (dReal)(2 * M_PI); } theta += (dReal)(ceil(phase_hat / (2 * M_PI)) * (2 * M_PI)); } if (phase_hat - theta > M_PI) { phase[i] = theta + (dReal)(2 * M_PI); } else if (phase_hat - theta < -M_PI) { phase[i] = theta - (dReal)(2 * M_PI); } else { phase[i] = theta; } dIASSERT(fabs(phase_hat - phase[i]) < M_PI); } // Calculate the phase error. Depending on the mode the condition // is that the distances traveled by each contact point must be // either equal (chain and sprockets) or opposite (gears). if (mode == dTransmissionChainDrive) { delta = (dCalcVectorLength3(r[0]) * phase[0] - dCalcVectorLength3(r[1]) * phase[1]); } else { delta = (dCalcVectorLength3(r[0]) * phase[0] + dCalcVectorLength3(r[1]) * phase[1]); } // When in chain mode a torque reversal, signified by the change // in sign of the wheel phase difference, has the added effect of // switching the active chain branch. We must therefore reflect // the contact points and tangents across the baseline. if (mode == dTransmissionChainDrive && delta < 0) { dVector3 d; dSubtractVectors3(d, a[0], a[1]); for (i = 0 ; i < 2 ; i += 1) { dVector3 nn; dReal a; dCalcVectorCross3(nn, n[i], d); a = dCalcVectorDot3(nn, nn); dIASSERT(a > 0); dAddScaledVectors3(c[i], c[i], nn, 1, -2 * dCalcVectorDot3(c[i], nn) / a); dAddScaledVectors3(l[i], l[i], nn, -1, 2 * dCalcVectorDot3(l[i], nn) / a); } } // Do not add the constraint if there's backlash and we're in the // backlash gap. if (backlash == 0 || fabs(delta) > backlash) { // The constraint is satisfied iff the absolute velocity of the // contact point projected onto the tangent of the wheels is equal // for both gears. This velocity can be calculated as: // // u = v + omega x r_c // // The constraint therefore becomes: // (v_1 + omega_1 x r_c1) . l = (v_2 + omega_2 x r_c2) . l <=> // (v_1 . l + (r_c1 x l) . omega_1 = v_2 . l + (r_c2 x l) . omega_2 for (i = 0 ; i < 2 ; i += 1) { dSubtractVectors3 (r[i], c[i], p[i]); } dCalcVectorCross3(info->J1a, r[0], l[0]); dCalcVectorCross3(info->J2a, l[1], r[1]); dCopyVector3(info->J1l, l[0]); dCopyNegatedVector3(info->J2l, l[1]); if (delta > 0) { if (backlash > 0) { info->lo[0] = -dInfinity; info->hi[0] = 0; } info->c[0] = -worldFPS * erp * (delta - backlash); } else { if (backlash > 0) { info->lo[0] = 0; info->hi[0] = dInfinity; } info->c[0] = -worldFPS * erp * (delta + backlash); } } info->cfm[0] = cfm; // printf ("%f, %f, %f, %f, %f\n", delta, phase[0], phase[1], -phase[1] / phase[0], ratio); // Cache the contact point (in world coordinates) to avoid // recalculation if requested by the user. dCopyVector3(contacts[0], c[0]); dCopyVector3(contacts[1], c[1]); }
void dxJointDBall::getInfo2( dxJoint::Info2 *info ) { info->erp = erp; info->cfm[0] = cfm; dVector3 globalA1, globalA2; dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], globalA1); if (node[1].body) dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], globalA2); else dCopyVector3(globalA2, anchor2); dVector3 q; dSubtractVectors3(q, globalA1, globalA2); #ifdef dSINGLE const dReal MIN_LENGTH = REAL(1e-7); #else const dReal MIN_LENGTH = REAL(1e-12); #endif if (dCalcVectorLength3(q) < MIN_LENGTH) { // too small, let's choose an arbitrary direction // heuristic: difference in velocities at anchors dVector3 v1, v2; dBodyGetPointVel(node[0].body, globalA1[0], globalA1[1], globalA1[2], v1); if (node[1].body) dBodyGetPointVel(node[1].body, globalA2[0], globalA2[1], globalA2[2], v2); else dSetZero(v2, 3); dSubtractVectors3(q, v1, v2); if (dCalcVectorLength3(q) < MIN_LENGTH) { // this direction is as good as any q[0] = 1; q[1] = 0; q[2] = 0; } } dNormalize3(q); info->J1l[0] = q[0]; info->J1l[1] = q[1]; info->J1l[2] = q[2]; dVector3 relA1; dBodyVectorToWorld(node[0].body, anchor1[0], anchor1[1], anchor1[2], relA1); dMatrix3 a1m; dSetZero(a1m, 12); dSetCrossMatrixMinus(a1m, relA1, 4); dMultiply1_331(info->J1a, a1m, q); if (node[1].body) { info->J2l[0] = -q[0]; info->J2l[1] = -q[1]; info->J2l[2] = -q[2]; dVector3 relA2; dBodyVectorToWorld(node[1].body, anchor2[0], anchor2[1], anchor2[2], relA2); dMatrix3 a2m; dSetZero(a2m, 12); dSetCrossMatrixPlus(a2m, relA2, 4); dMultiply1_331(info->J2a, a2m, q); } const dReal k = info->fps * info->erp; info->c[0] = k * (targetDistance - dCalcPointsDistance3(globalA1, globalA2)); }
int dCollideCylinderSphere(dxGeom* Cylinder, dxGeom* Sphere, int flags, dContactGeom *contact, int skip) { dIASSERT (skip >= (int)sizeof(dContactGeom)); dIASSERT (Cylinder->type == dCylinderClass); dIASSERT (Sphere->type == dSphereClass); dIASSERT ((flags & NUMC_MASK) >= 1); //unsigned char* pContactData = (unsigned char*)contact; int GeomCount = 0; // count of used contacts #ifdef dSINGLE const dReal toleranz = REAL(0.0001); #endif #ifdef dDOUBLE const dReal toleranz = REAL(0.0000001); #endif // get the data from the geoms dReal radius, length; dGeomCylinderGetParams(Cylinder, &radius, &length); dVector3 &cylpos = Cylinder->final_posr->pos; //const dReal* pfRot1 = dGeomGetRotation(Cylinder); dReal radius2; radius2 = dGeomSphereGetRadius(Sphere); const dReal* SpherePos = dGeomGetPosition(Sphere); // G1Pos1 is the middle of the first disc // G1Pos2 is the middle of the second disc // vDir1 is the unit direction of the cylinderaxis dVector3 G1Pos1, G1Pos2, vDir1; vDir1[0] = Cylinder->final_posr->R[2]; vDir1[1] = Cylinder->final_posr->R[6]; vDir1[2] = Cylinder->final_posr->R[10]; dReal s; s = length * REAL(0.5); // just a precomputed factor G1Pos2[0] = vDir1[0] * s + cylpos[0]; G1Pos2[1] = vDir1[1] * s + cylpos[1]; G1Pos2[2] = vDir1[2] * s + cylpos[2]; G1Pos1[0] = vDir1[0] * -s + cylpos[0]; G1Pos1[1] = vDir1[1] * -s + cylpos[1]; G1Pos1[2] = vDir1[2] * -s + cylpos[2]; dVector3 C; dReal t; // Step 1: compute the two distances 's' and 't' // 's' is the distance from the first disc (in vDir1-/Zylinderaxis-direction), the disc with G1Pos1 in the middle s = (SpherePos[0] - G1Pos1[0]) * vDir1[0] - (G1Pos1[1] - SpherePos[1]) * vDir1[1] - (G1Pos1[2] - SpherePos[2]) * vDir1[2]; if(s < (-radius2) || s > (length + radius2) ) { // Sphere is too far away from the discs // no collision return 0; } // C is the direction from Sphere-middle to the cylinder-axis (vDir1); C is orthogonal to the cylinder-axis C[0] = s * vDir1[0] + G1Pos1[0] - SpherePos[0]; C[1] = s * vDir1[1] + G1Pos1[1] - SpherePos[1]; C[2] = s * vDir1[2] + G1Pos1[2] - SpherePos[2]; // t is the distance from the Sphere-middle to the cylinder-axis! t = dCalcVectorLength3(C); if(t > (radius + radius2) ) { // Sphere is too far away from the cylinder axis! // no collision return 0; } // decide which kind of collision we have: if(t > radius && (s < 0 || s > length) ) { // 3. collision if(s <= 0) { contact->depth = radius2 - dSqrt( (s) * (s) + (t - radius) * (t - radius) ); if(contact->depth < 0) { // no collision! return 0; } contact->pos[0] = C[0] / t * -radius + G1Pos1[0]; contact->pos[1] = C[1] / t * -radius + G1Pos1[1]; contact->pos[2] = C[2] / t * -radius + G1Pos1[2]; contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth); contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth); contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth); contact->g1 = Cylinder; contact->g2 = Sphere; contact->side1 = -1; contact->side2 = -1; GeomCount++; return GeomCount; } else { // now s is bigger than length here! contact->depth = radius2 - dSqrt( (s - length) * (s - length) + (t - radius) * (t - radius) ); if(contact->depth < 0) { // no collision! return 0; } contact->pos[0] = C[0] / t * -radius + G1Pos2[0]; contact->pos[1] = C[1] / t * -radius + G1Pos2[1]; contact->pos[2] = C[2] / t * -radius + G1Pos2[2]; contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth); contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth); contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth); contact->g1 = Cylinder; contact->g2 = Sphere; contact->side1 = -1; contact->side2 = -1; GeomCount++; return GeomCount; } } else if( (radius - t) <= s && (radius - t) <= (length - s) ) { // 1. collsision if(t > (radius2 + toleranz)) { // cylinder-axis is outside the sphere contact->depth = (radius2 + radius) - t; if(contact->depth < 0) { // should never happen, but just for safeness return 0; } else { C[0] /= t; C[1] /= t; C[2] /= t; contact->pos[0] = C[0] * radius2 + SpherePos[0]; contact->pos[1] = C[1] * radius2 + SpherePos[1]; contact->pos[2] = C[2] * radius2 + SpherePos[2]; contact->normal[0] = C[0]; contact->normal[1] = C[1]; contact->normal[2] = C[2]; contact->g1 = Cylinder; contact->g2 = Sphere; contact->side1 = -1; contact->side2 = -1; GeomCount++; return GeomCount; } } else { // cylinder-axis is outside of the sphere contact->depth = (radius2 + radius) - t; if(contact->depth < 0) { // should never happen, but just for safeness return 0; } else { contact->pos[0] = C[0] + SpherePos[0]; contact->pos[1] = C[1] + SpherePos[1]; contact->pos[2] = C[2] + SpherePos[2]; contact->normal[0] = C[0] / t; contact->normal[1] = C[1] / t; contact->normal[2] = C[2] / t; contact->g1 = Cylinder; contact->g2 = Sphere; contact->side1 = -1; contact->side2 = -1; GeomCount++; return GeomCount; } } } else { // 2. collision if(s <= (length * REAL(0.5)) ) { // collsision with the first disc contact->depth = s + radius2; if(contact->depth < 0) { // should never happen, but just for safeness return 0; } contact->pos[0] = radius2 * vDir1[0] + SpherePos[0]; contact->pos[1] = radius2 * vDir1[1] + SpherePos[1]; contact->pos[2] = radius2 * vDir1[2] + SpherePos[2]; contact->normal[0] = vDir1[0]; contact->normal[1] = vDir1[1]; contact->normal[2] = vDir1[2]; contact->g1 = Cylinder; contact->g2 = Sphere; contact->side1 = -1; contact->side2 = -1; GeomCount++; return GeomCount; } else { // collsision with the second disc contact->depth = (radius2 + length - s); if(contact->depth < 0) { // should never happen, but just for safeness return 0; } contact->pos[0] = radius2 * -vDir1[0] + SpherePos[0]; contact->pos[1] = radius2 * -vDir1[1] + SpherePos[1]; contact->pos[2] = radius2 * -vDir1[2] + SpherePos[2]; contact->normal[0] = -vDir1[0]; contact->normal[1] = -vDir1[1]; contact->normal[2] = -vDir1[2]; contact->g1 = Cylinder; contact->g2 = Sphere; contact->side1 = -1; contact->side2 = -1; GeomCount++; return GeomCount; } } return GeomCount; }