float PolyLine2::GetPointParam( const Vec2& pt, bool bClosed ) const
{
int minEdge = -1;
float minDist = FLT_MAX;
float minLen = 0.0f;
int nPt = m_Points.size();
if (!bClosed)
{
nPt--;
}
float len = 0.0f;
for (int i = 0; i < nPt; i++)
{
const Vec2& a = GetPoint( i );
const Vec2& b = GetPoint( i + 1 );
float d = pt.dist_seg( a, b );
if (d < minDist)
{
minDist = d;
minEdge = i;
minLen = len;
}
len += (a - b).norm();
}
const Vec2& a = GetPoint( minEdge );
const Vec2& b = GetPoint( minEdge + 1 );
Vec2 dir = b - a;
dir.normalize();
float dlen = minLen + dir.dot( pt - a );
return dlen/GetLength( bClosed );
}
Vec2& Vec2::reflect(const Vec2& normal)
{
Vec2 I = *this;
*this = ((2.0f * -I.dot(normal)) * normal) + I;
return *this;
}
bool StaticHelpers::angleIsCloserToB(float a, float val, float b)
{
Vec2 vectA = degreesToVec2(a);
Vec2 vectV = degreesToVec2(val);
Vec2 vectB = degreesToVec2(b);
float diffA = acosf(vectV.dot(vectA)/(vectV.length() * vectA.length())) * 180 / M_PI;
float diffB = acosf(vectV.dot(vectB)/(vectV.length() * vectB.length())) * 180 / M_PI;
return diffA > diffB;
}
//
// `Line::intersectsAt()` is based on
// https://www.codeproject.com/Tips/862988/Find-the-Intersection-Point-of-Two-Line-Segments
//
// Licenced with the Code Project Open Licence (CPOL)
// http://www.codeproject.com/info/cpol10.aspx
//
Optional<Line::position_type> Line::intersectsAt(const Line& line) const
{
const Vec2 r = end - begin;
const Vec2 s = line.end - line.begin;
const Vec2 qp = line.begin - begin;
const double rxs = r.x * s.y - r.y * s.x;
const double qpxr = qp.x * r.y - qp.y * r.x;
const double qpxs = qp.x * s.y - qp.y * s.x;
const bool rxsIsZero = detail::IsZero(rxs);
if (rxsIsZero && detail::IsZero(qpxr))
{
const double qpr = qp.dot(r);
const double pqs = (begin - line.begin).dot(s);
if ((0 <= qpr && qpr <= r.dot(r)) || (0 <= pqs && pqs <= s.dot(s)))
{
// Two lines are overlapping
return Line::position_type(Math::QNaN, Math::QNaN);
}
// Two lines are collinear but disjoint.
return none;
}
if (rxsIsZero && !detail::IsZero(qpxr))
{
// Two lines are parallel and non-intersecting.
return none;
}
const double t = qpxs / rxs;
const double u = qpxr / rxs;
if (!rxsIsZero && (0.0 <= t && t <= 1.0) && (0.0 <= u && u <= 1.0))
{
// An intersection was found
return begin + t * r;
}
// Two line segments are not parallel but do not intersect
return none;
}
Vec2 SteeringForce::Pursuit(const Vehicle *evader)
{
//如果逃避者在前面，而且面对着智能体
//那么我们可以正好靠近逃避者
Vec2 ToEvader = evader->getPosition() - m_pVehicle->getPosition();
double RelativeHeading = m_pVehicle->getHeading().dot(evader->getHeading());
if (ToEvader.dot(m_pVehicle->getHeading())>0&&RelativeHeading<-0.95) {
return Seek(evader->getPosition());
}
//预测逃避者的位置
double LookAheadTime = ToEvader.length()/(m_pVehicle->getMaxSpeed()+evader->getVeloctity().length());
LookAheadTime +=TurnaroundTime(m_pVehicle,evader->getPosition());
return Seek(evader->getPosition()+evader->getVeloctity()*LookAheadTime);
}
Vec2 calculateCosASinAOfVec1ToVec2(const Vec2 &vec1,const Vec2 &vec2)
//return {cos(vec1,vec2),sin(vec1,vec2)}
{
float cosA= vec1.dot(vec2) / (vec1.getLength() * vec2.getLength());
float signalOfSinA;
{
float _vec1[3]={vec1.x,vec1.y,0};
float _vec2[3]={vec2.x,vec2.y,0};
float _rs[3];
__cross(_vec1, _vec2, _rs);
if (_rs[2] == 0) {
signalOfSinA = 0;
}else if(_rs[2] > 0){
signalOfSinA = 1;
}else{
signalOfSinA = -1;
}
}
float sinA = signalOfSinA*sqrtf(MAX(0,1-cosA*cosA));
return Vec2(cosA,sinA);
}
void PiecesLayer::dealCollision( float dur ){
for (b2ContactEdge *c = monsterBody->GetContactList();c;c = c->next){
b2Contact *contact = c->contact;
b2Body *bodyA = contact->GetFixtureA()->GetBody();
b2Body *bodyB = contact->GetFixtureB()->GetBody();
b2Body *collidedBody = (bodyA == monsterBody)?bodyB:bodyA;
Sprite* sp = (Sprite*)collidedBody->GetUserData();
if (sp && sp->getName() == "paddle"&& contact->IsTouching() == true)
{
Vec2 vec1 = paddleNode->getPosition();
Vec2 vec2 = paddleNode->convertToWorldSpace(sp->getPosition());
drawNode->drawSegment(vec1, vec2, 3.0f, Color4F(1, 1, 1, 1));
Vec2 normalVec = vec1 - vec2;
Vec2 monsterToCentreVec = paddleNode->getPosition() - monster->getPosition();
//if the normalVec is pointing to the centre
dirVec = normalVec.dot(monsterToCentreVec) > 0? normalVec:normalVec*(-1);
dirVec.normalize();
float m_velocity = sqrtf(monsterBody->GetLinearVelocity().x * monsterBody->GetLinearVelocity().x +
monsterBody->GetLinearVelocity().y * monsterBody->GetLinearVelocity().y);
forceVec.x = dirVec.x * 2 * monsterBody->GetMass() * m_velocity;
forceVec.y = dirVec.y * 2 * monsterBody->GetMass() * m_velocity;
//forceVec.Normalize();
monsterBody->ApplyLinearImpulse(forceVec, monsterBody->GetWorldCenter(), true);
oldVec = b2Vec2(monsterBody->GetLinearVelocity().x, monsterBody->GetLinearVelocity().y);
oldVec.Normalize();
monsterBody->SetLinearVelocity(b2Vec2(oldVec.x * 10, oldVec.y * 10));
//sp->setName("deleting paddle");
//Paddles[sp->getTag()]->scheduleOnce(schedule_selector(Paddle::dealPaddleDeletion),0.05f);
Paddles[sp->getTag()]->beginDeletion();
return;
}
if (sp && (sp->getName() == "brick" || sp->getName() == "wall") && contact->IsTouching() == true) {
brickForceVec.x =monsterBody->GetPosition().x - collidedBody->GetPosition().x;
brickForceVec.y =monsterBody->GetPosition().y - collidedBody->GetPosition().y;
brickForceVec.Normalize();
monsterBody->ApplyLinearImpulse(brickForceVec, monsterBody->GetWorldCenter(), true);
oldVec = b2Vec2(monsterBody->GetLinearVelocity().x, monsterBody->GetLinearVelocity().y);
oldVec.Normalize();
monsterBody->SetLinearVelocity(b2Vec2(oldVec.x * 10, oldVec.y * 10));
if (sp->getName() == "brick")
{
for (int i = 0; collidedBodies[i] == NULL; i++)
{
collidedBodies[i] = collidedBody;
break;
}
this->scheduleOnce(schedule_selector(PiecesLayer::dealDeletion),0.05f);
}
}
}
}
void BlacknWhiteLayer::dealCollision(float dur){
for (b2ContactEdge *c = monsterBody->GetContactList();c;c = c->next){
b2Contact *contact = c->contact;
b2Body *bodyA = contact->GetFixtureA()->GetBody();
b2Body *bodyB = contact->GetFixtureB()->GetBody();
b2Body *collidedBody = (bodyA == monsterBody)?bodyB:bodyA;
if ((collidedBody == paddleBody || collidedBody == paddleBody2) && contact->IsTouching() == true)
{
if ((collidedBody == paddleBody && monster->getName() == "blackBall") || (collidedBody == paddleBody2 && monster->getName() == "whiteBall")) {
//records the direction from the center point to the monster
Vec2 vec1;
Vec2 vec2;
if (collidedBody == paddleBody) {
vec1 = paddleNode->convertToWorldSpace( Vec2(paddle->getBoundingBox().getMinX(),paddle->getBoundingBox().getMaxY()) );
vec2 = paddleNode->convertToWorldSpace( Vec2(paddle->getBoundingBox().getMaxX(),paddle->getBoundingBox().getMaxY()) );
}
if (collidedBody == paddleBody2) {
vec1 = paddleNode->convertToWorldSpace( Vec2(paddle2->getBoundingBox().getMinX(),paddle2->getBoundingBox().getMaxY()) );
vec2 = paddleNode->convertToWorldSpace( Vec2(paddle2->getBoundingBox().getMaxX(),paddle2->getBoundingBox().getMaxY()) );
}
Vec2 normalVec = vec1 - vec2;
Vec2 monsterToCentreVec = paddleNode->getPosition() - monster->getPosition();
//if the normalVec is pointing to the centre
dirVec = normalVec.dot(monsterToCentreVec) > 0? normalVec:normalVec*(-1);
dirVec.normalize();
float m_velocity = sqrtf(monsterBody->GetLinearVelocity().x * monsterBody->GetLinearVelocity().x +
monsterBody->GetLinearVelocity().y * monsterBody->GetLinearVelocity().y);
forceVec.x = dirVec.x * 2 * monsterBody->GetMass() * m_velocity;
forceVec.y = dirVec.y * 2 * monsterBody->GetMass() * m_velocity;
//forceVec.Normalize();
monsterBody->ApplyLinearImpulse(forceVec, monsterBody->GetWorldCenter(), true);
oldVec = b2Vec2(monsterBody->GetLinearVelocity().x, monsterBody->GetLinearVelocity().y);
oldVec.Normalize();
monsterBody->SetLinearVelocity(b2Vec2(oldVec.x * MONSTER_SPEED, oldVec.y * MONSTER_SPEED));
return;
}else{
monsterBody->SetTransform(b2Vec2(460 / WORLDSCALE, 280 / WORLDSCALE), 0);
initMonsterBody();
}
}
/*Sprite* sp = (Sprite*)collidedBody->GetUserData();
if (sp && sp->getName() == "brick" && contact->IsTouching() == true) {
for (int i = 0; collidedBodies[i] == NULL; i++) {
collidedBodies[i] = collidedBody;
break;
}
brickForceVec.x =monsterBody->GetPosition().x - collidedBody->GetPosition().x;
brickForceVec.y =monsterBody->GetPosition().y - collidedBody->GetPosition().y;
brickForceVec.Normalize();
monsterBody->ApplyLinearImpulse(brickForceVec, monsterBody->GetWorldCenter(), true);
oldVec = b2Vec2(monsterBody->GetLinearVelocity().x, monsterBody->GetLinearVelocity().y);
oldVec.Normalize();
monsterBody->SetLinearVelocity(b2Vec2(oldVec.x * MONSTER_SPEED, oldVec.y * MONSTER_SPEED));
this->scheduleOnce(schedule_selector(BlacknWhiteLayer::dealDeletion),0.05f);
}*/
}
}