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);
}
}
