580 likes | 701 Views
Motor V. Ütközés detektálás és válasz. Sz écsi László. Let öltés. di ák: www .iit.bme.hu /~szecsi/GraphGame / l10-collision. ppt. Új osztály: FatCollider. class FatCollider { friend class RigidBody; friend class RigidModel; protected: double thickness; double density;
E N D
Motor V.Ütközésdetektálás és válasz Szécsi László
Letöltés • diák: • www.iit.bme.hu/~szecsi/GraphGame • /l10-collision.ppt
Új osztály: FatCollider • class FatCollider • { • friend class RigidBody; • friend class RigidModel; • protected: • double thickness; • double density; • double restitution; • double friction;
FatCollider folyt. • FatCollider(double density, double thickness, double restitution, double friction); • double getRestitution(); • double getFriction(); • virtual double getMass()=0; • virtual void • getAngularMass(D3DXMATRIX& • massMatrix)=0;
FatCollider folyt. • virtual void getNearestPointTo( • const D3DXVECTOR3& b, • D3DXVECTOR3& out)=0; • virtual void getReferencePoint(D3DXVECTOR3& out)=0; • virtual FatCollider* makeTransformedClone( • const D3DXMATRIX& transformationMatrix)=0; • virtual void translate(const D3DXVECTOR3& offset)=0; • };
FatCollider.cpp • FatCollider::FatCollider(double density, double thickness, double restitution, double friction){ • this->density = density; • this->thickness = thickness; • this->restitution = restitution; • this->friction = friction; • } • double FatCollider::getRestitution() • {return restitution;} • double FatCollider::getFriction() • { return friction;}
Új class: FatCylinder • class FatCylinder : • public FatCollider • { • double radius; • double height; • D3DXVECTOR3 position; • D3DXVECTOR3 discNormal;
FatCylinder folyt. • public: • FatCylinder(double density, double thickness, double restitution, double friction, double radius, double height, const D3DXVECTOR3& position, const D3DXVECTOR3& discNormal); • double getMass(); • void getAngularMass(D3DXMATRIX& massMatrix);
FatCylinder folyt. • void getNearestPointTo(const D3DXVECTOR3& b, D3DXVECTOR3& out); • void getReferencePoint(D3DXVECTOR3& out); • FatCollider* makeTransformedClone(const D3DXMATRIX& transformationMatrix); • void translate(const D3DXVECTOR3& offset); • };
FatCylinder.cpp • FatCylinder::FatCylinder(double density, double thickness, double restitution, double friction, double radius, double height, const D3DXVECTOR3& position, const D3DXVECTOR3& discNormal) • :FatCollider(density, thickness, restitution, friction) • { • this->radius = radius; • this->position = position; • this->discNormal = discNormal; • this->height = height; • }
FatCylinder.cpp • double FatCylinder::getMass() • { • return density * (radius + thickness) * (radius + thickness) * 3.14 * (height + thickness * 2.0); • }
FatCylinder.cpp • void FatCylinder::getAngularMass(D3DXMATRIX& massMatrix) • { • double mass = getMass(); • double cylinderRadius = radius + thickness; • double cylinderRadius2 = cylinderRadius * cylinderRadius; • double cylinderHeight2 = (height + thickness * 2.0) * (height + thickness * 2.0); • D3DXMatrixScaling(&massMatrix, • mass * (3.0 * cylinderRadius2 + cylinderHeight2) / 12.0, • mass * cylinderRadius2 * 0.5, • mass * (3.0 * cylinderRadius2 + cylinderHeight2) / 12.0); • D3DXVECTOR3 rotationAxis; • D3DXVec3Cross(&rotationAxis, &discNormal, &D3DXVECTOR3(0.0f, 1.0f, 0.0f)); • float rotationAngle = acos(discNormal.y); • D3DXMATRIX transformMatrix, transposedTransformMatrix; • D3DXMatrixRotationAxis(&transformMatrix, &rotationAxis, rotationAngle); • D3DXMatrixTranspose(&transposedTransformMatrix, &transformMatrix); • D3DXMATRIX translationMassMatrix; • float mrr = mass * D3DXVec3LengthSq(&position); • D3DXMatrixScaling(&translationMassMatrix, mrr, mrr, mrr); • D3DXMATRIX posProducts( • mass * position.x * position.x, mass * position.x * position.y, mass * position.x * position.z, 0.0f, • mass * position.y * position.x, mass * position.y * position.y, mass * position.y * position.z, 0.0f, • mass * position.z * position.x, mass * position.z * position.y, mass * position.z * position.z, 0.0f, • 0.0f, 0.0f, 0.0f, 1.0f • ); • translationMassMatrix -= posProducts; • massMatrix = transposedTransformMatrix * massMatrix * transformMatrix + translationMassMatrix; • }
FatCylinder.cpp • void FatCylinder::getNearestPointTo(const D3DXVECTOR3& b, D3DXVECTOR3& out) • { • out = b; • out -= position; • D3DXVECTOR3 onNormal = discNormal * D3DXVec3Dot(&out, &discNormal); • D3DXVECTOR3 onTangent = out - onNormal; • float ln = D3DXVec3Length(&onNormal); • if(ln > height * 0.5) • onNormal *= height * 0.5 / ln; • float ld = D3DXVec3Length(&onTangent); • if(ld > radius) • onTangent *= radius / ld; • out = onNormal + onTangent + position; • }
FatCylinder.cpp • void FatCylinder::getReferencePoint( • D3DXVECTOR3& out) • { • out = position; • } • void FatCylinder::translate(const D3DXVECTOR3& offset) • { • position += offset; • }
FatCylinder.cpp • FatCollider* FatCylinder::makeTransformedClone(const D3DXMATRIX& transformationMatrix) • { • D3DXVECTOR3 transformedPosition, transformedNormal; • D3DXVec3TransformCoord(&transformedPosition, &position, &transformationMatrix); • D3DXVec3TransformNormal(&transformedNormal, &discNormal, &transformationMatrix); • return new FatCylinder(density, thickness, restitution, friction, radius, height, transformedPosition, transformedNormal); • }
RigidModel kieg. • #include "FatCollider.h" • class RigidModel{ • D3DXVECTOR3 centreOfMass; • std::vector<FatCollider*> colliders; • void recomputeMass();
RigidModel • public: • void addFatCollider(FatCollider* collider); • void translateToCentreOfMass(); • const D3DXVECTOR3&getCentreOfMass();
RigidModel.cpp • void RigidModel::addFatCollider(FatCollider* collider) • { • colliders.push_back(collider); • recomputeMass(); • }
RigidModel.cpp • void RigidModel::recomputeMass(){ • double mass = 0;D3DXMATRIX angularMass; • D3DXMatrixScaling(&angularMass, 0.0f, 0.0f, 0.0f); • std::vector<FatCollider*>::iterator i = colliders.begin(); • while(i != colliders.end()){ • if( (*i)->density == 0.0){ • invMass = 0.0; D3DXMatrixScaling(&invAngularMass, 0, 0, 0); • return; } • mass += (*i)->getMass(); • D3DXMATRIX colliderAngularMass; • (*i)->getAngularMass(colliderAngularMass); • angularMass += colliderAngularMass; • i++; • } • invMass = 1.0 / mass; • angularMass._44 = 1.0f; • D3DXMatrixInverse(&invAngularMass, NULL, &angularMass); • }
RigidModel.cpp • void RigidModel::translateToCentreOfMass(){ • centreOfMass = D3DXVECTOR3(0, 0, 0); • std::vector<FatCollider*>::iterator i = colliders.begin(); • while(i != colliders.end()) { • if( (*i)->density == 0.0) • {centreOfMass == D3DXVECTOR3(0, 0, 0);return;} • double m = (*i)->getMass(); • D3DXVECTOR3 com; • (*i)->getReferencePoint(com); • centreOfMass += com * m; • i++; • } • centreOfMass *= invMass; • std::vector<FatCollider*>::iterator ii = colliders.begin(); • while(ii != colliders.end()) { • (*ii)->translate(-centreOfMass); ii++; } • recomputeMass(); • }
RigidModel.cpp • const D3DXVECTOR3& RigidModel::getCentreOfMass() • { • return centreOfMass; • }
EngineCore::loadRigidModels • loadFatCylinders(rigidModelNode, rigidModel); • rigidModel->translateToCentreOfMass(); • rigidModelDirectory[rigidModelName] = rigidModel;
EngineCore osztályba • void loadFatCylinders( • XMLNode& rigidModelNode, • RigidModel* rigidModel);
EngineCore.cpp • #include "FatCylinder.h" • void EngineCore::loadFatCylinders(XMLNode& rigidModelNode, RigidModel* rigidModel) • { • int iFatCylinder = 0; • XMLNode colliderNode; • while( !(colliderNode = rigidModelNode.getChildNode(L"FatCylinder", iFatCylinder)).isEmpty() ){ • double density = colliderNode.readDouble(L"density", 1.0); • double restitution = colliderNode.readDouble(L"restitution", 0.7); • double friction = colliderNode.readDouble(L"friction", 0.2); • double thickness = colliderNode.readDouble(L"thickness", 1.0); • double radius = colliderNode.readDouble(L"radius", 3.0); • double height = colliderNode.readDouble(L"height", 3.0); • D3DXVECTOR3 position = colliderNode.readVector(L"position"); • D3DXVECTOR3 axis = colliderNode.readVector(L"axis"); • rigidModel->addFatCollider(new FatCylinder(density, thickness, restitution, friction, radius, height, position, axis)); • iFatCylinder++; • } • }
XML • <RigidModel name="ship" invMass="1" invAngularMassX="1" invAngularMassY="1" invAngularMassZ="1" • drag.x="0.2" drag.y="1" drag.z="1" angularDrag.x="50" angularDrag.y="50" angularDrag.z="50"> • <FatCylinder density="0.01" thickness="0.5" radius="0.8" height="8" position.x="6.797987" position.y="0.232483" position.z="0" axis.x="-0.988273" axis.y="0.152695" axis.z="0" /> • <FatCylinder density="0.01" thickness="0.5" radius="2.5" height="0.2" position.x="7.699793" position.y="-0.646181" position.z="0.436795" axis.x="0.234549" axis.y="0.867554" axis.z="-0.438562" /> • <FatCylinder density="0.01" thickness="0.5" radius="2.5" height="0.2" position.x="7.758713" position.y="-0.776405" position.z="-0.472291" axis.x="0.120485" axis.y="0.905816" axis.z="0.406178" /> • <FatCylinder density="0.01" thickness="0.5" radius="0.8" height="10" position.x="-3.07312" position.y="0.323494" position.z="0" axis.x="0.990722" axis.y="0.135901" axis.z="0" /> • <FatCylinder density="0.01" thickness="0.5" radius="3.2" height="1.2" position.x="-5.431546" position.y="-0.17174" position.z="0" axis.x="-0.139047" axis.y="0.990286" axis.z="0" /> • <FatCylinder density="0.01" thickness="0.5" radius="1.2" height="0.2" position.x="-4.910843" position.y="-2.162253" position.z="-5.752216" axis.x="0" axis.y="0.423868" axis.z="-0.905724" /> • <FatCylinder density="0.01" thickness="0.5" radius="1.2" height="0.2" position.x="-4.883704" position.y="-2.303208" position.z="5.850989" axis.x="0" axis.y="0.423214" axis.z="0.90603" /> • <FatCylinder density="0.01" thickness="0.5" radius="1.2" height="0.2" position.x="-5.011202" position.y="1.466043" position.z="3.776621" axis.x="0" axis.y="0.130077" axis.z="0.991504" /> • <FatCylinder density="0.01" thickness="0.5" radius="1.2" height="0.2" position.x="-5.055292" position.y="1.6017" position.z="-3.677074" axis.x="0" axis.y="0.093695" axis.z="-0.995601" /> • </RigidModel>
Próba • Tömeg az ütköző geometria alapján számolva • Kevés az erő és a forgatónyomaték ehhez • Állítsuk nagyobbra a motorcontrolban • |force| = 35 • |torque| = 100
Entity kieg. • virtual RigidBody* asRigidBody(); • //cpp • RigidBody* Entity::asRigidBody() • { • return NULL; • }
RigidBody • RigidBody* asRigidBody(); • //cpp • RigidBody* RigidBody::asRigidBody() • { • return this; • }
RigidBody • class RigidBody { • D3DXVECTOR3 positionCorrection; • D3DXVECTOR3 impulse; • D3DXVECTOR3 angularImpulse; • public: • virtual void interact(Entity* target); • virtual void affect(Entity* affector); • D3DXVECTOR3 getPointVelocity(const D3DXVECTOR3& point);
RigidBody::animate • momentum += force * dt + impulse; • D3DXVECTOR3 velocity = momentum * rigidModel->invMass; • position += velocity * dt + positionCorrection; • angularMomentum += torque * dt + angularImpulse;
RigidBody::control • force = D3DXVECTOR3(0.0, 0.0, 0.0); • torque = D3DXVECTOR3(0.0, 0.0, 0.0); • impulse = D3DXVECTOR3(0.0, 0.0, 0.0); • angularImpulse = D3DXVECTOR3(0.0, 0.0, 0.0); • positionCorrection = D3DXVECTOR3(0.0, 0.0, 0.0);
RigidBody::control • if(controller) • controller->apply(this, context); • context.interactors->interact(this); • }
void RigidBody::interact(Entity* target) • { • if(target == this) • return; • target->affect(this); • }
RigidBody::getPointVelocity • D3DXVECTOR3 RigidBody::getPointVelocity( • const D3DXVECTOR3& point){ • D3DXVECTOR3 rp = point - position; • D3DXMATRIX worldSpaceInvMassMatrix; • getWorldInvMassMatrix(worldSpaceInvMassMatrix); • // compute angular velocity vector • D3DXVECTOR3 angularVelocity; • D3DXVec3TransformCoord(&angularVelocity, &angularMomentum, &worldSpaceInvMassMatrix); • // compute tangential velocity from angular velocity • D3DXVECTOR3 velo; • D3DXVec3Cross(&velo, &angularVelocity, &rp); • // add object velocity • velo += momentum * rigidModel->invMass; • return velo; • }
RigidBody::affect • Másik entitás RigidBody-e? • A másik fatCollidereit áttranszformáljuk a mi modellezési koordináta rendszerünkbe • model2->world->model1 • Minden collider párra • Iteratívan megkeressük a legközelebbi pontokat • Ha közelebb vannak, mint amilyen vastagok, akkor ütközés-válasz • Impulzus: képlet szerint
RigidBody::affect • void RigidBody::affect(Entity* affector) • { • RigidBody* antiBody = affector->asRigidBody(); • // rigid body collision detection • if(antiBody) • { • std::vector<FatCollider*>& antiColliders = antiBody->rigidModel->colliders; • // for each collider i in antiColliders • std::vector<FatCollider*>::iterator i = antiColliders.begin(); • while(i != antiColliders.end()) • { • // transform i to my model space • // other * o.rotationMatrix * o.translation / t.translationMatrix / t.rotationMatrix • D3DXMATRIX otherPosMinusMyPosMatrix; • D3DXMatrixTranslation(&otherPosMinusMyPosMatrix, • antiBody->position.x - position.x, • antiBody->position.y - position.y, • antiBody->position.z - position.z); • D3DXMATRIX rotateBackMatrix; • D3DXMatrixTranspose(&rotateBackMatrix, &rotationMatrix); • FatCollider* otherColliderInMyModelSpace = • (*i)->makeTransformedClone(antiBody->rotationMatrix * otherPosMinusMyPosMatrix * rotateBackMatrix); • // for each collider j in colliders • std::vector<FatCollider*>& myColliders = rigidModel->colliders; • std::vector<FatCollider*>::iterator j = myColliders.begin(); • while(j != myColliders.end()) • { • D3DXVECTOR3 a, b; • (*j)->getReferencePoint(a); • for(int c=0; c<10; c++) • { • otherColliderInMyModelSpace->getNearestPointTo(a, b); • (*j)->getNearestPointTo(b, a); • } • D3DXVECTOR3 collisionNormal = a - b; • float abDistance = D3DXVec3Length(&collisionNormal); • if(abDistance < 0.1) • { • D3DXVECTOR3 aCenter, bCenter; • (*j)->getReferencePoint(aCenter); • aCenter -= a; • D3DXVec3Normalize(&aCenter, &aCenter); • aCenter *= (*j)->thickness * 0.5; • a += aCenter; • (*i)->getReferencePoint(bCenter); • bCenter -= b; • D3DXVec3Normalize(&bCenter, &bCenter); • bCenter *= (*i)->thickness * 0.5; • b += bCenter; • } • float objectDistance = abDistance - (*i)->thickness - (*j)->thickness; • if(objectDistance < 0) • { • // to world collision point and normal • D3DXVec3Normalize(&collisionNormal, &collisionNormal); • D3DXVECTOR3 collisionPoint = (a - collisionNormal * (*j)->thickness + b + collisionNormal * (*i)->thickness) * 0.5; • D3DXVec3TransformNormal(&collisionNormal, &collisionNormal, &rotationMatrix); • D3DXVec3TransformNormal(&collisionPoint, &collisionPoint, &rotationMatrix); • collisionPoint += position; • // compute relative velocity of colliding points • D3DXVECTOR3 relativeVelocity = antiBody->getPointVelocity(collisionPoint) - getPointVelocity(collisionPoint); • float normalVelocityLength = D3DXVec3Dot( &relativeVelocity, &collisionNormal); • D3DXVECTOR3 normalVelocity = collisionNormal * normalVelocityLength; • D3DXVECTOR3 frictionVelocity = relativeVelocity - normalVelocity; • if( D3DXVec3Length(&frictionVelocity) > 1.0) • D3DXVec3Normalize(&frictionVelocity, &frictionVelocity); • // compute impulse • float invMassDiv = 0.0; • invMassDiv += rigidModel->invMass; • invMassDiv += antiBody->rigidModel->invMass; • D3DXVECTOR3 arma = collisionPoint - position; • D3DXVECTOR3 armb = collisionPoint - antiBody->position; • D3DXVECTOR3 kan, kbn, ikan, ikbn, ikank, ikbnk; • // compute inverse mass matrix • D3DXMATRIX myWorldInvMassMatrix, antiWorldInvMassMatrix; • this->getWorldInvMassMatrix(myWorldInvMassMatrix); • antiBody->getWorldInvMassMatrix(antiWorldInvMassMatrix); • D3DXVec3Cross(&kan, &arma, &collisionNormal); • D3DXVec3Cross(&kbn, &armb, &collisionNormal); • D3DXVec3TransformNormal(&ikan, &kan, &myWorldInvMassMatrix); • D3DXVec3TransformNormal(&ikbn, &kbn, &antiWorldInvMassMatrix); • D3DXVec3Cross(&ikank, &ikan, &arma); • D3DXVec3Cross(&ikbnk, &ikbn, &armb); • invMassDiv += D3DXVec3Dot(&collisionNormal, &ikank); • invMassDiv += D3DXVec3Dot(&collisionNormal, &ikbnk); • D3DXVECTOR3 impulse = • (1 + (*i)->getRestitution() * (*j)->getRestitution()) * (normalVelocity + frictionVelocity * (*i)->getFriction() * (*j)->getFriction()) • / invMassDiv; • // add collision response • this->impulse += impulse; • D3DXVECTOR3 angularImpulse; • D3DXVec3Cross(&angularImpulse, &arma, &impulse); • this->angularImpulse += angularImpulse; • this->positionCorrection -= collisionNormal * (objectDistance * 0.55f) * rigidModel->invMass / (rigidModel->invMass + antiBody->rigidModel->invMass); • } • j++; • } • delete otherColliderInMyModelSpace; • i++; • } • } • }
XML – másik RigidBody • <RigidBody name="flagShip" shadedMesh="steelPredator" rigidModel="ship" control="player"/> • <RigidBody name="nmiShip" pos.x="-30" shadedMesh="predator" rigidModel="ship"/>
Próba • nekitolatunk: elpördülnek • .fx-ben a lámpákat kivehetjük, hogy lássuk is mi történik. Pl. • float4 psBasic(TrafoOutput input) : COLOR0 • { • float3 lighting = abs(input.normal.y) * 2; • /* for(int il=0; il<nSpotlights; il++) • { • float3 lightDiff = spotlights[il].position - input.worldPos; • float3 lightDir = normalize(lightDiff); • lighting += spotlights[il].peakRadiance * • max(0, dot(input.normal, lightDir)) * • pow(max(0,dot(-lightDir, spotlights[il].direction)), spotlights[il].focus) • / dot(lightDiff, lightDiff); • }*/ • return float4(lighting * tex2D(kdMapSampler, input.tex) * kdColor, 1); • }
Pozíció nem stimmel • Ütközőket elmozgattuk, hogy az origóban legyen a tömegközéppont • A rajzolt modellt is mozgassuk el
RigidBody::render • D3DXMATRIX positionMatrix; • D3DXMatrixTranslation(&positionMatrix, position.x, position.y, position.z); • D3DXMATRIX massOffsetMatrix; • D3DXMatrixTranslation(&massOffsetMatrix, • -rigidModel->centreOfMass.x, • -rigidModel->centreOfMass.y, • -rigidModel->centreOfMass.z); • D3DXMATRIX bodyModelMatrix = massOffsetMatrix * rotationMatrix * positionMatrix;
Próba • Ütközés működik • Mozogjon ez is: • Vezérlés célokkal • Target ~ Motor • TargetControl ~ MotorControl
XML • <TargetControl name="cruiser" maxForce="40" maxTorque="200"> • <Target position.x="50" position.y="20" position.z="50" radius="10" /> • <Target position.x="-50" position.y="20" position.z="50" radius="10" /> • <Target position.x="-50" position.y="20" position.z="-50" radius="10" /> • <Target position.x="50" position.y="20" position.z="-50" radius="10" /> • </TargetControl>
Új class: Target • class Entity; • class Target • { • friend class TargetControl; • D3DXVECTOR3 position; • double radius; • Entity* mark; • public: • Target(const D3DXVECTOR3& position, double radius); • void setMark(Entity* mark); • };
Target.cpp • Target::Target(const D3DXVECTOR3& position, double radius) • { • mark = NULL; • this->radius = radius; • this->position = position; • } • void Target::setMark(Entity* mark) • { • this->mark = mark; • }
Új class: TargetControl • #include <vector> • class Target; • class TargetControl : • public RigidBodyControl{ • std::vector<Target*> targets; • std::vector<Target*>::iterator currentTarget; • double maxForce; • double maxTorque; • public: • TargetControl(double maxForce, double maxTorque); • ~TargetControl(void); • void addTarget(Target* target); • void apply(RigidBody* controlledBody, const ControlContext& context); • };
TargetControl.cpp • TargetControl::TargetControl(double maxForce, double maxTorque) • { • currentTarget = targets.end(); • this->maxForce = maxForce; • this->maxTorque = maxTorque; • }
TargetControl.cpp • void TargetControl::addTarget(Target* target) • { • targets.push_back(target); • currentTarget = targets.begin(); • } • TargetControl::~TargetControl(void) • { • std::vector<Target*>::iterator i = targets.begin(); • while(i != targets.end()) • { • delete *i; • i++; • } • }
TargetControl::apply • void TargetControl::apply(RigidBody* controlledBody, const ControlContext& context) • { • if(!targets.empty()) • { • Target* target = *currentTarget; • D3DXVECTOR3 markDifference = target->position - controlledBody->position; • if(target->mark) • markDifference += target->mark->getPosition(); • if(D3DXVec3Length(&markDifference) < target->radius) • { • currentTarget++; • if(currentTarget == targets.end()) • currentTarget = targets.begin(); • }
TargetControl::apply • D3DXVECTOR3 markDirection; • D3DXVec3Normalize(&markDirection, &markDifference); • D3DXMATRIX modelMatrix; • controlledBody->getModelMatrix(modelMatrix); • D3DXVECTOR3 ahead; • D3DXVec3TransformNormal(&ahead, &D3DXVECTOR3(1, 0, 0), &modelMatrix); • D3DXVECTOR3 turnAxis; • D3DXVec3Cross(&turnAxis, &ahead, &markDirection); • controlledBody->torque += turnAxis * maxTorque; • controlledBody->force += ahead * D3DXVec3Dot(&ahead, &markDirection) * maxForce; • } • }
RigidBody-hoz hozzáférés • class RigidBody : • public Entity • { • friend class TargetControl;