void ParticleSystem::collide(ParticlePtr part, SpherePtr sphere) { Vector3<float> distance = part->getPosition() - sphere->getRigidBody()->getPosition(); float radiusSum = sphere->getRadius(); //test auf collision if (distance.length() <= radiusSum) // sonst nur radiusSum { //berechnung normalB und contactPoint distance.normalize(); const Vector3<float>& normalB = distance; //const Vector3<float>& contactPoint = part->getPosition(); //+ 0.01f * normalB; // folgender Abschnitt aus: // CollisionSystem::collisionResponse(part, sphere, contactPoint, normalB); //berechnung der relativen geschwindigkeit in richtung normalB const Vector3<float>& velocityA = part->getVelocity(); const Vector3<float>& velocityB = sphere->getRigidBody()->getVelocity(); Vector3<float> velA = part->getVelocity(); Vector3<float> forceA = part->getForce(); Vector3<float> velB = sphere->getRigidBody()->getVelocity(); Vector3<float> forceB = sphere->getRigidBody()->getForce(); const Vector3<float>& vR = velocityA - velocityB; float vN = dot(vR, normalB); //behandlung für colliding contact if (vN <= 0.5) { //Kraftberechnung für Resting Contact //Todo //--------------bisherige Behandlung---------------------------------------------- Vector3<float> normale = -1 * part->getPosition() + sphere->getRigidBody()->getPosition(); normale.normalize(); float vADoN = dot(velA, normale); Vector3<float> vS = normale * vADoN; float fADoN = dot(forceA, normale); Vector3<float> fS = normale * fADoN; float vBDoN = dot (velA, normale); Vector3<float> vS2 = normale * vBDoN; float fBDoN = dot (forceB, normale); Vector3<float> fS2 = normale * fBDoN; Vec3 nullvector = Vec3(0.0,0.0,0.0); Vector3<float> force = part->getForce(); force.normalize(); if (dot (force, normale) > 0 ){ part->setVelocity(part->getVelocity() - vS); part->setVelocity(part->getVelocity() * 0.9f); // Wert: 0.75 part->setVelocity(part->getVelocity() + sphere->getRigidBody()->getVelocity()); part->addForce((-1.f) * fS); part->setForce(part->getForce() * 0.9f); // Wert: 0.75 part->addForce(sphere->getRigidBody()->getForce()); if (!(sphere->getRigidBody()->getVelocity() == Vec3(0.0,0.0,0.0))){ sphere->getRigidBody()->addForce((-0.02f) * fS2); sphere->getRigidBody()-> setVelocity(sphere->getRigidBody()->getVelocity() - (vS2 * 0.02f)); } part->setIsColliding(true); }else if (dot (force, normale) < 0){ part->setVelocity(part->getVelocity() + vS); part->setVelocity(part->getVelocity() * 0.9f); // Wert: 0.75 part->setVelocity(part->getVelocity() + sphere->getRigidBody()->getVelocity()); part->addForce((1.0) * fS); part->setForce(part->getForce() * 0.9f); // Wert: 0.75 part->addForce(sphere->getRigidBody()->getForce()); if (!(sphere->getRigidBody()->getVelocity() == Vec3(0.0,0.0,0.0))){ sphere->getRigidBody()->addForce((-0.02f) * fS2); sphere->getRigidBody() ->setVelocity(sphere->getRigidBody()->getVelocity() - (vS2 * 0.02f)); } } //----------------alte Behandlung------------------------------------------------- //berechnung der impuls-kraft in richtung normalB /* float damping = 0.5f; //(0.6 + sphere->getBounciness()) / 2.0; float invMassA, invMassB = 0.0; Matrix<float> invTensorA(3,3); Matrix<float> invTensorB(3,3); Vec3 rA = part->getForce(); Vec3 rB = sphere->getRigidBody()->getForce(); //test ob dynamisch if(part->getIsDynamicFlag()) { invMassA = 1.0f / part->getMass(); } else invMassA = 0.0; if(sphere->getRigidBody()->getIsDynamicFlag()) { invMassB = 1.0f / sphere->getMass(); invTensorB = sphere->getRigidBody()->getInvWorldInertiaTensor(); } else invMassB = 0.0; const Vector3<float>& crossA = cross(rA,normalB); const Vector3<float>& crossB = cross(rB,normalB); const Vector3<float>& uA = invTensorA * crossA; const Vector3<float>& uB = invTensorB * crossB; float fNum = -(1+damping)*(dot(normalB,(velocityA - velocityB))); float fDenom = invMassA + invMassB + dot(crossA,uA)+dot(crossB,uB); float f = fNum / fDenom; Vector3<float> impulseForce = 9.5 * f * normalB; //jetzt: part->setVelocity(part->getVelocity() + impulseForce/part->getMass()); */ //----------neue Behandlung------------------------------------------------------- ////berechnung der impuls-kraft in richtung normalB //float damping = 0.1f; //float invMassA = part->getInvMass(); // float invMassB = sphere->getInvMass(); // // // calculate impulse //float fNum = -(/*1+*/damping)*(dot(normalB,(velocityB - velocityA))); //float fDenom = invMassA + invMassB; // //float impulseMagnitude = fNum / fDenom; //if (impulseMagnitude < 0){ //Vector3<float> impulseForce = impulseMagnitude * normalB; // //if (part->getInvMass()!=0) // part->setVelocity(part->getVelocity() - (impulseForce/invMassA)); // //if (sphere->getInvMass()!=0) // sphere->setVelocity(sphere->getVelocity() - (impulseForce * invMassB)); //} //-------------------------------------------------------------------------------- } } }
/** * \brief Collision Partikel mit einer Box. * \param box zu testende Box */ void ParticleSystem::collide(ParticlePtr part, BoxPtr box) { //sphere in das lokale koordinatensystem der box bringen Vector3<float> particlePos = part->getPosition() - box->getRigidBody()->getPosition(); Quaternion boxOri = box->getRigidBody()->getOrientation(); const Quaternion& boxInvOri = boxOri.inverse(); particlePos = qRotate(particlePos, boxInvOri); //test auf collision mit den 6 seiten der box unsigned int cnt = 0; float rad = 3.0f; float right = dot(particlePos, Vector3<float>(1,0,0)) + rad; float left = dot(particlePos, Vector3<float>(-1,0,0)) - rad; float front = dot(particlePos, Vector3<float>(0,0,1)) + rad; float back = dot(particlePos, Vector3<float>(0,0,-1)) - rad; float top = dot(particlePos, Vector3<float>(0,1,0)) + rad; float bottom = dot(particlePos, Vector3<float>(0,-1,0)) - rad; float dR = box->getWidth()/2 - right + rad; float dL = box->getWidth()/2 - left + rad; float dF = box->getDepth()/2 - front + rad; float dBa = box->getDepth()/2 - back + rad; float dT = box->getHeight()/2 - top + rad; float dBo = box->getHeight()/2 - bottom + rad; if (right >= 0 && dR >= 0) { cnt ++; if (right > box->getWidth()/2) particlePos[X] = box->getWidth()/2; else particlePos[X] = right ; } if (left > 0 && dL >= 0) { cnt ++; if(left > box->getWidth()/2) particlePos[X] = -box->getWidth()/2; else particlePos[X] = -left ; } if (front >= 0 && dF >= 0) { cnt ++; if (front > box->getDepth()/2) particlePos[Z] = box->getDepth()/2; else particlePos[Z] = front; } if (back > 0 && dBa >= 0) { cnt ++; if (back > box->getDepth()/2) particlePos[Z] = -box->getDepth()/2; else particlePos[Z] = -back; } if (top >= 0 && dT >= 0) { cnt ++; if (top > box->getHeight()/2) particlePos[Y] = box->getHeight()/2; else particlePos[Y] = top; } if (bottom > 0 && dBo >= 0) { cnt ++; if (bottom > box->getHeight()/2) particlePos[Y] = -box->getHeight()/2; else particlePos[Y] = -bottom ; } //test auf collision if (cnt >= 3) { //berechnung normalB und contactPoint const Vector3<float>& contactPoint = qRotate(particlePos, boxOri) + box->getRigidBody()->getPosition(); //cout << "C: " << contactPoint << endl; //cout << "P: " << part->getPosition() << endl; Vector3<float> normalB = part->getPosition() - contactPoint; //cout << normalB.length() <<"\n"; //if (normalB.length() != 0) normalB.normalize(); // folgender Abschnitt aus: // CollisionSystem::collisionResponse(part, sphere, contactPoint, normalB); //berechnung der relativen geschwindigkeit in richtung normalB const Vector3<float>& velocityA = part->getVelocity(); const Vector3<float>& velocityB = box->getRigidBody()->getVelocity(); Vector3<float> velA = part->getVelocity(); Vector3<float> forceA = part->getForce(); // const Vector3<float>& rA = contactPoint - part->getPosition(); // vorher: particlePos! // const Vector3<float>& rB = contactPoint - box->getPosition(); const Vector3<float>& vA = velocityA; const Vector3<float>& vB = velocityB; const Vector3<float>& vR = vA - vB; float vN = dot(vR, normalB); //------------------Behandlung für colliding contact--------------------------------------- if (vN <= 0.5) { //--------------------neue Behandlung aus Sphere kopiert------------------------- float vDoN = dot(velA, normalB); Vector3<float> vS = normalB * vDoN; float fDoN = dot(forceA, normalB); Vector3<float> fS = normalB * fDoN; Vector3<float> force = part->getForce(); force.normalize(); if (dot (force, normalB) > 0 ){ part->setVelocity(part->getVelocity() - vS); part->setVelocity(part->getVelocity() * 0.75); // Wert: 0.75 part->addForce((-1.0) * fS); part->setForce(part->getForce() * 0.75); // Wert: 0.75 part->setIsColliding(true); } //------------------------------------------------------------------------------ ////berechnung der impuls-kraft in richtung normalB ////float damping = (0.01f + box->getBounciness()) / 2.0f; //float damping = 0.12f; //float invMassA, invMassB = 0.0f; ////Matrix<float> invTensorA(3,3); ////Matrix<float> invTensorB = box->getInvWorldInertiaTensor(); ////test ob dynamisch //if(part->getIsDynamicFlag()) //{ // invMassA = part->getInvMass(); //} //else // invMassA = 0.0; // //if(box->getRigidBody()->getIsDynamicFlag()) //{ // invMassB = box->getInvMass(); // //invTensorB = box->getRigidBody()->getInvWorldInertiaTensor(); //} //else // invMassB = 0.0; //float fNum = -(/*1+*/damping)*(dot(normalB,(velocityA - velocityB))); //float fDenom = invMassA + invMassB;// + dot(crossA,uA)+dot(crossB,uB); //float impulseMagnitude = fNum / fDenom; //if (impulseMagnitude < 0){ //Vector3<float> impulseForce = impulseMagnitude * normalB; ///*impulseForce.normalize(); //float vDoI = dot(velA, impulseForce); //float fDoI = dot(forceA, impulseForce); //Vec3 vS = impulseForce * vDoI; //Vec3 fS = impulseForce * fDoI;*/ ////part->setVelocity(part->getVelocity()[X], 0.0f, part->getVelocity()[Z]); ////part->setVelocity(part->getVelocity() - vS); ////part->addForce(fS); ////setzen der linaren momente //if (part->getInvMass()!=0) //{ //part->setVelocity(part->getVelocity() - impulseForce/invMassA); //part->setIsColliding(true); //} ////part->setVelocity(part->getVelocity() + (impulseForce / invMassB)); // //} } } }
void ParticleSystem::collide(ParticlePtr part, CapsulePtr caps) { //part->setIsColliding(false); Vector3<float> unitV(0,1,0); Vector3<float> rV(qRotate(unitV, caps->getRigidBody()->getOrientation())); Vector3<float> A1 = caps->getRigidBody()->getPosition()+(rV*(caps->getHeight()/2)); Vector3<float> A2 = caps->getRigidBody()->getPosition()-(rV*(caps->getHeight()/2)); Vec3 nullVector = Vec3(0.0,0.0,0.0); // teile des codes Copyright 2001, softSurfer (www.softsurfer.com) // alles geschummelt. Kugelzentrum als infinitisimal kleines Segment interpretiert. Vector3<float> smallNumberVector(0.1f,0.1f,0.1f); Vector3<float> B1 = part->getPosition() + smallNumberVector; Vector3<float> B2 = part->getPosition() - smallNumberVector; Vector3<float> u = A2 - A1; Vector3<float> v = B2 - B1; Vector3<float> w = A1 - B1; float a = dot(u,u); // always >= 0 float b = dot(u,v); float c = dot(v,v); // always >= 0 float d = dot(u,w); float e = dot(v,w); float D = a*c - b*b; // always >= 0 float sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0 float tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0 // compute the line parameters of the two closest points if (D < 0.0000000001) { // the lines are almost parallel sN = 0.0; // force using point P0 on segment S1 sD = 1.0; // to prevent possible division by 0.0 later tN = e; tD = c; } else { // get the closest points on the infinite lines sN = (b*e - c*d); tN = (a*e - b*d); if (sN < 0.0) { // sc < 0 => the s=0 edge is visible sN = 0.0; tN = e; tD = c; } else if (sN > sD) { // sc > 1 => the s=1 edge is visible sN = sD; tN = e + b; tD = c; } } if (tN < 0.0) { // tc < 0 => the t=0 edge is visible tN = 0.0; // recompute sc for this edge if (-d < 0.0) sN = 0.0; else if (-d > a) sN = sD; else { sN = -d; sD = a; } } else if (tN > tD) { // tc > 1 => the t=1 edge is visible tN = tD; // recompute sc for this edge if ((-d + b) < 0.0) sN = 0; else if ((-d + b) > a) sN = sD; else { sN = (-d + b); sD = a; } } // finally do the division to get sc and tc if(fabs(sN) < 0.000000001) sc = 0.0; else sc = sN/sD; if(fabs(tN) < 0.0000000001) tc = 0.0; else tc = tN/tD; // get the difference of the two closest points Vector3<float> dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc) float dist = dP.length(); // return the closest distance float radParticle = 3.0f; // 3.0f für Partikelradius //collision-behandlung if (dist < (caps->getRadius()) + radParticle) { assert(dP.length() != 0.0); if (dP.length() != 0.0) dP.normalize(); Vector3<float> normalB = dP ; Vector3<float> contactPoint = part->getPosition() + (radParticle * dP); Vector3<float> posOldToContact = contactPoint - (part->getPosition()); //überprüfung der normalen, falls die capsule weiter als radius eingedrungen ist, umdrehen if (dot(posOldToContact, normalB)<0) { //cout << "Normale " << normalB << " von B=" << caps->getId() << " zeigt in die falsche Richtung !\n"; normalB = -normalB; } else { // cout << "Normale " << normalB << " von B=" << caps->getId() << " zeigt in die richtige Richtung !\n"; } const Vector3<float>& rA = contactPoint - caps->getRigidBody()->getPosition(); const Vector3<float>& aVelocityA = caps->getRigidBody()->getAngularVelocity(); Vector3<float> velA = part->getVelocity(); Vector3<float> forceA = part->getForce(); const Vector3<float>& velocityB = part->getVelocity(); const Vector3<float>& velocityA = caps->getRigidBody()->getVelocity() + cross(aVelocityA, rA); const Vector3<float>& vR = velocityA - velocityB; float vN = dot(vR, normalB); //test auf colliding contact if (vN <= 0.03) // Wert: 0.5 { //Vector3<float> normale = -1 * part->getPosition() + caps->getPosition(); //normalB.normalize(); //-----------Marke Eigenbau-------------------------------------------------------------------------------- //float vDoN = dot(velA, normalB); //Vector3<float> vS = normalB * vDoN; //float fDoN = dot(forceA, normalB); //Vector3<float> fS = normalB * fDoN; //Vector3<float> force = part->getForce(); ////force.normalize(); //if (dot (force, normalB) >= 0.0 ){ // part->setVelocity(part->getVelocity() - vS); // part->setVelocity(part->getVelocity() * 0.75); // Wert: 0.75 // part->addForce((-1) * fS); // part->setForce(part->getForce() * 0.75); // Wert: 0.75 // part->setIsColliding(true); // } // //part->setVelocity(part->getVelocity() * vDoN); //part->setForce(part->getForce() * fDoN); //------------Kopie aus Sphere-------------------------------------------------- float vDoN = dot(velA, normalB); Vector3<float> vS = normalB * vDoN; float fDoN = dot(forceA, normalB); Vector3<float> fS = normalB * fDoN; Vector3<float> force = part->getForce(); if (force.length() != 0.0) force.normalize(); if (dot (force, normalB) > 0 ){ part->setVelocity(part->getVelocity() - vS); part->setVelocity(part->getVelocity() * 0.9f); // Wert: 0.75 part->setVelocity(part->getVelocity() + caps->getRigidBody()->getVelocity()); part->addForce((-1.f) * fS); part->setForce(part->getForce() * 0.9f); // Wert: 0.75 //part->addForce(caps->getRigidBody()->getForce()); part->setIsColliding(true); part->setIsCollidingWhere(1); } else if (dot (force, normalB) < 0){ part->setVelocity(part->getVelocity() + vS); part->setVelocity(part->getVelocity() * 0.9f); // Wert: 0.75 part->setVelocity(part->getVelocity() + caps->getRigidBody()->getVelocity()); part->addForce((1.f) * fS); part->setForce(part->getForce() * 0.9f); // Wert: 0.75 //part->addForce(caps->getRigidBody()->getForce()); part->setIsColliding(true); part->setIsCollidingWhere(-1); } //--------------------------neue Kollisionsbehandlung------------------------------------------------------------- /* float damping = 1.0f; //float damping = (0.1f + caps->getBounciness()) / 2.0f; float invMassA = caps->getInvMass(); float invMassB = part->getInvMass();//1.0f / part->getMass(); Matrix<float> invTensorA = caps->getInvWorldInertiaTensor(); const Vector3<float>& crossA = cross(rA,normalB); const Vector3<float>& uA = invTensorA * crossA; // fNum = damping *( -(1+(-0.5f)) * .... float fNum = damping*(-(1+0.0f)*(dot(normalB,(velocityA - velocityB)) + dot(aVelocityA,crossA))); float fDenom = invMassA + invMassB + dot(crossA,uA); float impulseMagnitude = fNum/fDenom; // if (impulseMagnitude > 0) { Vector3<float> impulseForce = impulseMagnitude * normalB; //impulseMagnitude * 2.0 * normalB if (part->getInvMass()!=0) // Velocity setzen part->setVelocity(part->getVelocity() - (impulseForce * invMassB)); }*/ } //--------------------keine Ahnung was ist---------------------------------------------------- /* if (vN > -1.5 && vN < 1.5){ //Vector3<float> normale = -1 * part->getPosition() + caps->getPosition(); normalB.normalize(); float vDoN = dot(velA, normalB); Vector3<float> vS = normalB * vDoN; float fDoN = dot(forceA, normalB); Vector3<float> fS = normalB * fDoN; Vector3<float> force = part->getForce(); //force.normalize(); if (dot (force, normalB) >= -0.5 ){ part->setVelocity(part->getVelocity() - (vS * 1.2)); //part->setVelocity(Vector3<float>(0.0,0.0,0.0)); //part->setForce(Vector3<float>(0.0,0.01,0.0)); part->addForce((-1.2) * fS); }*/ //--------------------------------------------------------------------------------------------------------- /*vDoN *= 50; fDoN *= 50;*/ //part->setVelocity(part->getVelocity() * vDoN); //part->setForce(part->getForce() * fDoN); } }