std::vector< float > Saliency::distributionFilter( const std::vector< SuperpixelStatistic >& stat ) const { const int N = stat.size(); // Setup the data and features std::vector< Vec3f > features( stat.size() ); Mat_<float> data( stat.size(), 4 ); for( int i=0; i<N; i++ ) { //features[i] = stat[i].mean_color_ / settings_.sigma_c_; { Vec3f x = stat[i].mean_color_; x[0] = x[0]/settings_.sigma_c_; x[1] = x[1]/settings_.sigma_c_; x[2] = x[2]/settings_.sigma_c_; features[i] = x; } Vec2f p = stat[i].mean_position_; data(i,0) = 1; data(i,1) = p[0]; data(i,2) = p[1]; data(i,3) = p.dot(p); } // Filter Filter filter( (const float*)features.data(), N, 3 ); filter.filter( data.ptr<float>(), data.ptr<float>(), 4 ); // Compute the uniqueness std::vector< float > r( N ); for( int i=0; i<N; i++ ) r[i] = data(i,3) / data(i,0) - ( data(i,1) * data(i,1) + data(i,2) * data(i,2) ) / ( data(i,0) * data(i,0) ); normVec( r ); return r; }
std::vector< float > Saliency::distribution( const std::vector< SuperpixelStatistic >& stat ) const { const int N = stat.size(); std::vector< float > r( N ); const float sc = 0.5 / (settings_.sigma_c_*settings_.sigma_c_); for( int i=0; i<N; i++ ) { float u = 0, norm = 1e-10; Vec3f c = stat[i].mean_color_; Vec2f p(0.f, 0.f); // Find the mean position for( int j=0; j<N; j++ ) { Vec3f dc = stat[j].mean_color_ - c; float w = fast_exp( - sc * dc.dot(dc) ); p += w*stat[j].mean_position_; norm += w; } p *= 1.0 / norm; // Compute the variance for( int j=0; j<N; j++ ) { Vec3f dc = stat[j].mean_color_ - c; Vec2f dp = stat[j].mean_position_ - p; float w = fast_exp( - sc * dc.dot(dc) ); u += w*dp.dot(dp); } r[i] = u / norm; } normVec( r ); return r; }
std::vector< float > Saliency::uniqueness( const std::vector< SuperpixelStatistic >& stat ) const { const int N = stat.size(); std::vector< float > r( N ); const float sp = 0.5 / (settings_.sigma_p_ * settings_.sigma_p_); for( int i=0; i<N; i++ ) { float u = 0, norm = 1e-10; Vec3f c = stat[i].mean_color_; Vec2f p = stat[i].mean_position_; // Evaluate the score, for now without filtering for( int j=0; j<N; j++ ) { Vec3f dc = stat[j].mean_color_ - c; Vec2f dp = stat[j].mean_position_ - p; float w = fast_exp( - sp * dp.dot(dp) ); u += w*dc.dot(dc); norm += w; } // Let's not normalize here, must have been a typo in the paper // r[i] = u / norm; r[i] = u; } normVec( r ); return r; }
//---------------------------------------------------------------------- // returns true if the given goint lies in the left half plane of the // oriented line AB. // Author: afischle //---------------------------------------------------------------------- static bool isLeft(const Vec2f &a, const Vec2f &b, const Vec2f &point) { // compute the normal pointing into the left half plane of the oriented // line Vec2f n = computeEdgeNormal(a, b, false); // the dot product is >0 when two vectors point into the same direction return n.dot(point - a) > 0.f; }
//---------------------------------------------------------------------- // Returns a normal outline containining normal contours. These normal contours // contain GlyphVertexNormal structs providing the normals of the adjacent edges, // the mean normal and the angle enclosed by the edge normals. // // Author: afischle //---------------------------------------------------------------------- const TextVectorGlyph::Normals &TextVectorGlyph::getNormals(UInt32 level) const { // Try to find the normal outline of the specified detail level in the // cache map NormalMap::const_iterator it = _normalMap.find(level); if (it != _normalMap.end()) // We already have that level - return the corresponding outline return it->second; // We did not find that level, so we have to create it Normals &normals = _normalMap.insert(NormalMap::value_type(level, Normals())).first->second; // get the polygon outline of this glyph at the desired detail level // The contours of this outline are not closed! const PolygonOutline &outline = getLines(level); // compute the contour orientations when they are not available if (_contourOrientations.empty()) computeContourOrientations(); UInt32 start = 0, end, index = 0; vector<UInt32>::const_iterator iIt; vector<Orientation>::const_iterator oriIt = _contourOrientations.begin(); for (iIt = outline.contours.begin(); iIt != outline.contours.end(); ++iIt, ++oriIt) { end = *iIt; OSG_ASSERT(end - 1 < outline.coords.size()); OSG_ASSERT(start < outline.coords.size()); OSG_ASSERT(oriIt != _contourOrientations.end()); Vec2f prevEdgeNormal = computeEdgeNormal(outline.coords[end - 1], outline.coords[start], (*oriIt) == CW); while (index < end) { UInt32 nextIndex = index + 1; if (nextIndex >= end) nextIndex = start; OSG_ASSERT(index < outline.coords.size()); OSG_ASSERT(nextIndex < outline.coords.size()); Vec2f nextEdgeNormal = computeEdgeNormal(outline.coords[index], outline.coords[nextIndex], (*oriIt) == CW); Vec2f meanEdgeNormal = prevEdgeNormal + nextEdgeNormal; meanEdgeNormal.normalize(); Real32 edgeAngle = osgACos(osgAbs(prevEdgeNormal.dot(nextEdgeNormal))); normals.push_back(VertexNormal(nextEdgeNormal, meanEdgeNormal, edgeAngle)); //the outgoing edge of this vertex is the incoming of the next vertex prevEdgeNormal = nextEdgeNormal; ++index; } start = end; } return normals; }
static Vec3f makeBallPoint(const Vec2f &p) { float L2 = p.dot(p); Vec3f b = Vec3f(p, 0); if( L2<1 ) b.z = sqrtf(1-L2); else b = normalize(b); b.y = -b.y; return b; }
void Ball::collideWith(BallRef other) { // calculate minimal distance between balls static float minimal = 2 * RADIUS; // 1) we have already established that the two balls are colliding, // let's move back in time to the moment before collision mPosition -= mVelocity; other->mPosition -= other->mVelocity; // 2) convert to simple 1-dimensional collision // by projecting onto line through both centers Vec2f line = other->mPosition - mPosition; Vec2f unit = line.normalized(); float distance = line.dot(unit); float velocity_a = mVelocity.dot(unit); float velocity_b = other->mVelocity.dot(unit); if(velocity_a == velocity_b) return; // no collision will happen // 3) find time of collision float t = (minimal - distance) / (velocity_b - velocity_a); // 4) move to that moment in time mPosition += t * mVelocity; other->mPosition += t * other->mVelocity; // 5) exchange velocities mVelocity -= velocity_a * unit; mVelocity += velocity_b * unit; other->mVelocity -= velocity_b * unit; other->mVelocity += velocity_a * unit; // 6) move forward to current time mPosition += (1.0f - t) * mVelocity; other->mPosition += (1.0f - t) * other->mVelocity; // 7) make sure the balls stay within window collideWithWindow(); other->collideWithWindow(); }
bool PointTracker::find_correspondences(const vector<Vec2f>& points, float f) { if (init_phase) { // We do a simple freetrack-like sorting in the init phase... // sort points int point_d_order[PointModel::N_POINTS]; point_model->get_d_order(points, point_d_order); // set correspondences for (int i=0; i<PointModel::N_POINTS; ++i) { p[point_model->d_order[i]] = points[point_d_order[i]]; } } else { // ... otherwise we look at the distance to the projection of the expected model points // project model points under current pose p_exp[0] = project(Vec3f(0,0,0), f); p_exp[1] = project(point_model->M01, f); p_exp[2] = project(point_model->M02, f); // set correspondences by minimum distance to projected model point bool point_taken[PointModel::N_POINTS]; for (int i=0; i<PointModel::N_POINTS; ++i) point_taken[i] = false; float min_sdist = 0; int min_idx = 0; for (int i=0; i<PointModel::N_POINTS; ++i) { // find closest point to projected model point i for (int j=0; j<PointModel::N_POINTS; ++j) { Vec2f d = p_exp[i]-points[j]; float sdist = d.dot(d); if (sdist < min_sdist || j==0) { min_idx = j; min_sdist = sdist; } } // if one point is closest to more than one model point, abort if (point_taken[min_idx]) return false; point_taken[min_idx] = true; p[i] = points[min_idx]; } } return true; }
Tree::PortalBuilder::SuperPlane* Tree::PortalBuilder::createSuperPlane( Node* node , Vec2f const& max , Vec2f const& min ) { assert( !node->isLeaf() ); SuperPlane* splane = new SuperPlane; splane->node = node; splane->plane = mTree->getEdge( node->idxEdge ).plane; splane->tag = mSPlanes.size(); Portal portal; portal.con[0] = 0; portal.con[1] = 0; Vec2f v[4] = { max , Vec2f( min.x , max.y ) , min , Vec2f( max.x , min.y ) }; for( int i = 0 , prev = 3 ; i < 4 ; prev = i++ ) { Vec2f dir = v[ i ] - v[ prev ]; float dot = dir.dot( splane->plane.normal ); if ( abs( dot ) < FLOAT_EPSILON ) continue; float t = ( v[i].dot( splane->plane.normal ) + splane->plane.d ) / dot; Vec2f p = v[i] + t * dir; } Node* cur = node->parent; while( cur ) { cur = cur->parent; } return splane; }
void Branch::addArc( const Vec2f &p1 ) { if ( mPath.empty() ) { mPath.moveTo( p1 ); return; } size_t segmentNum = mPath.getNumSegments(); if ( segmentNum == 0 ) { mPath.lineTo( p1 ); return; } // last point Vec2f p0 = mPath.getSegmentPosition( segmentNum - 1, 1.f ); // same point as the last one -> skip if ( p0.distanceSquared( p1 ) < EPSILON ) return; // segment tangent - direction of the last segment Vec2f tangent = p0 - mPath.getSegmentPosition( segmentNum - 1, .98f ); Vec2f n( -tangent.y, tangent.x ); // segment normal Vec2f d( p1 - p0 ); // distance of the arc center and the last path point d *= .5f; Vec2f b( -d.y, d.x ); // arc bisector // parallel vectors, p1 lies on the tangent float dot = b.dot( n ); float sqrLength = b.lengthSquared() * n.lengthSquared(); // does not seem to be a very precise test, hence the .1f limit instead of EPSILON if ( math< float >::abs( dot * dot - sqrLength ) < .1f ) { mPath.lineTo( p1 ); return; } // the arc center is the intersection of the segment normal and the bisector float s; if ( n.x == 0.f ) { if ( b.x == 0.f ) // parallel { mPath.lineTo( p1 ); return; } s = -d.x / b.x; } else { float den = b.x * n.y / n.x - b.y; if ( den == 0.f ) // parallel { mPath.lineTo( p1 ); return; } s = ( d.y - d.x * n.y / n.x ) / ( b.x * n.y / n.x - b.y ); } Vec2f c( ( p0 + p1 ) *.5f + s * b ); // arc center Vec2f d0( p0 - c ); float a0 = math< float >::atan2( d0.y, d0.x ); Vec2f d1( p1 - c ); float a1 = math< float >::atan2( d1.y, d1.x ); // segment normal line and segment bisector distance tangent.normalize(); float C = tangent.dot( p0 ); float dist = tangent.dot( ( p0 + p1 ) * .5f ) - C; // arc direction depends on the quadrant, in which the new point resides bool forward = ( s > 0.f ) ^ ( dist < 0.f ); mPath.arc( c, d0.length(), a0, a1, forward ); }
// Execute function void BehDribbleBall::execute(ActuatorVars& AV, const ActuatorVars& lastAV, bool justActivated) { // // Gaze control // // Look for the ball GazeBehLookForBall::execute(AV, lastAV, justActivated); // // Walking control // // If we have no ball then stop where you are if(!SV.haveBall) { // Set our walking GCV AV.GCV.setZero(); AV.halt = false; AV.doKick = false; AV.rightKick = true; AV.doDive = DD_NONE; // Visualisation markers if(MM.willPublish()) { MM.SubStateText.setText("Dribble without Ball"); MM.SubStateText.updateAdd(); } // Return as we have nothing more to do return; } // Required gait control vector Vec3f GCV(0.0f, 0.0f, 0.0f); // x forwards, y left, z CCW // Save the relative offsets to the ball and target in local variables Vec2f ball = SV.ballDir; // Vector from robot to ball in body-fixed coordinates Vec2f target = GV.ballTargetDir; // Vector from robot to target in body-fixed coordinates if(!WBS.haveBallTarget) // If we don't have a ball target then take one that's in the direction we're facing in front of the ball target << ball.x() + config.minBallToTargetDist(), ball.y(); // Calculate the vector from the ball to the target in body-fixed coordinates Vec2f E = target - ball; // The E coordinate system is centred at the ball Vec2f unitEx = eigenNormalized(E); // x-axis of E: Unit vector from the ball towards the ball target Vec2f unitEy = eigenRotatedCCW90(unitEx); // y-axis of E: Unit vector 90 deg CCW from the x-axis of E // Transform the robot position and orientation into the E coordinate frame Vec2f robotE(-ball.dot(unitEx), -ball.dot(unitEy)); float robotAngle = -eigenAngleOf(unitEx); // Decide which foot we would prefer for dribbling based on the current robot position bool preferRightFoot = (robotE.y() >= WBS.reqBallDirMidY); // Decide whether we should reconsider the foot we're currently using for dribbling if(GV.suggestRightFoot()) // The right foot is being strongly suggested to us from above m_changeToRightFoot.vote(true, 2); else if(GV.suggestLeftFoot()) // The left foot is being strongly suggested to us from above m_changeToRightFoot.vote(false, 2); else m_changeToRightFoot.vote(preferRightFoot); bool reconsiderFoot = (m_changeToRightFoot.unanimous() && m_changeToRightFoot.decision() != m_useRightFoot); // Decide which foot to use for dribbling if(justActivated || reconsiderFoot) m_useRightFoot = m_changeToRightFoot.decision(); // Calculate the desired path angle for the dribble approach Vec2f robotToBehindBallE, robotToPathTargetE; float pathAngle = calcPathAngle(m_useRightFoot, robotE, robotToBehindBallE, robotToPathTargetE); float localPathAngle = picut(pathAngle - robotAngle); // Calculate the robot to path angle interpolation factor u (0.0 = Walk to robot angle, 1.0 = Walk to path angle) float u = calcPathInterpFactor(robotToBehindBallE, localPathAngle); // Calculate the desired XY walking velocity float walkAngle = calcWalkAngle(robotToBehindBallE, u, localPathAngle, pathAngle); Vec2f walkVecXY = WBS.calcGcvXY(config.dbAppGcvSpeedX(), config.dbAppGcvSpeedY(), walkAngle); // Calculate the final GCV by factoring in the turning Z velocity and XY reduction for angular misalignments float angleRatio = localPathAngle / config.dbAppAngleErrLimit(); float ratioXY = coerce<float>(1.0f - fabs(angleRatio), 0.0f, 1.0f); float signedRatioZ = coerceAbs(angleRatio, 1.0f); GCV.x() = ratioXY * walkVecXY.x(); GCV.y() = ratioXY * walkVecXY.y(); GCV.z() = signedRatioZ * config.dbAppGcvSpeedZ(); // Normalise the GCV to our desired maximum walking speed float gcvNorm = GCV.norm(); float speedLimit = fabs(config.dbAppGcvSpeedLimit()); if(gcvNorm > speedLimit) GCV *= speedLimit / gcvNorm; // Apply obstacle avoidance and set the walking target Vec2f walkingTarget = robotToPathTargetE.x() * unitEx + robotToPathTargetE.y() * unitEy; WBS.obstacleAvoidance(GCV, walkingTarget); WBS.setWalkingTarget(walkingTarget); // Set our walking GCV AV.GCV = GCV; AV.halt = false; AV.doKick = false; AV.rightKick = true; AV.doDive = DD_NONE; // Plotting if(config.plotData()) { PM.plotScalar(preferRightFoot * PMSCALE_FOOTSEL, PM_DBAPP_PREFERRIGHTFOOT); PM.plotScalar(reconsiderFoot * PMSCALE_FOOTOK, PM_DBAPP_RECONSIDERFOOT); PM.plotScalar(m_useRightFoot * PMSCALE_FOOTSEL, PM_DBAPP_USERIGHTFOOT); PM.plotScalar(robotE.x(), PM_DBAPP_ROBOTEX); PM.plotScalar(robotE.y(), PM_DBAPP_ROBOTEY); PM.plotScalar(localPathAngle, PM_DBAPP_LOCALPATHANGLE); PM.plotScalar(u, PM_DBAPP_UFACTOR); PM.plotScalar(ratioXY, PM_DBAPP_RATIOXY); } // Visualisation markers if(MM.willPublish()) { MM.SubStateText.setText(m_useRightFoot ? "Right Dribble" : "Left Dribble"); MM.SubStateText.updateAdd(); } }
/* * RETURNS numeric_limits<float>::max() IF CLOSEST POINT IS FARTHER THAN threshold DISTANCE * * REFERENCE: "Minimum Distance between a Point and a Line" BY Paul Bourke * http://paulbourke.net/geometry/pointlineplane */ float getShortestDistance(const Vec2f &point, const vector<Vec2f> &polygon, bool close, float threshold) { float min = threshold * threshold; // BECAUSE IT IS MORE EFFICIENT TO WORK WITH MAGNIFIED DISTANCES int end = polygon.size(); bool found = false; for (int i = 0; i < end; i++) { int i0, i1; if (i == end - 1) { if (close) { i0 = i; i1 = 0; } else { break; } } else { i0 = i; i1 = i + 1; } Vec2f p0 = polygon[i0]; Vec2f p1 = polygon[i1]; if (p0 != p1) { Vec2f delta = p1 - p0; float u = delta.dot(point - p0) / delta.lengthSquared(); if (u >= 0 && u <= 1) { Vec2f p = p0 + u * delta; float mag = (p - point).lengthSquared(); if (mag < min) { min = mag; found = true; } } else { float mag0 = (p0 - point).lengthSquared(); float mag1 = (p1 - point).lengthSquared(); if ((mag0 < min) && (mag0 < mag1)) { min = mag0; found = true; } else if ((mag1 < min) && (mag1 < mag0)) { min = mag1; found = true; } } } } return found ? ci::math<float>::sqrt(min) : numeric_limits<float>::max(); }
float MapperOp::angle(const Vec2f& v1, const Vec2f& v2) { return acos( v1.dot(v2) / (v1.length() * v2.length()) ); }
void labelCorners(_Polygon (&Polygons)[6], int (&orderOfPolygons)[6], Point2f (&Corners)[24]) { Vec2f DA = Vec2f(Polygons[orderOfPolygons[0]].Center.x - Polygons[orderOfPolygons[3]].Center.x, Polygons[orderOfPolygons[0]].Center.y - Polygons[orderOfPolygons[3]].Center.y); DA = normalize(DA); Vec3f DA3; DA3.val[0] = DA.val[0]; DA3.val[1] = DA.val[1]; DA3.val[2] = 0; _Diagonal Diagonal; float angle, dist, dist2; int startingPoint[6], cornerLabel = 1, cornerIndex; for (int polygonIndex = 0; polygonIndex < 6; polygonIndex++) { Diagonal.Diag = Vec2f(Polygons[polygonIndex].Corners[0].x - Polygons[polygonIndex].Corners[2].x, Polygons[polygonIndex].Corners[0].y - Polygons[polygonIndex].Corners[2].y); Diagonal.indexOfCorner1 = 0; Diagonal.indexOfCorner2 = 2; angle = DA.dot(normalize(Diagonal.Diag)); if (abs(angle) < 0.95) { Diagonal.Diag = Vec2f(Polygons[polygonIndex].Corners[1].x - Polygons[polygonIndex].Corners[3].x, Polygons[polygonIndex].Corners[1].y - Polygons[polygonIndex].Corners[3].y); Diagonal.indexOfCorner1 = 1; Diagonal.indexOfCorner2 = 3; } startingPoint[polygonIndex] = Diagonal.indexOfCorner1; // For square A if (polygonIndex == orderOfPolygons[0]) { // Select the corner which is farther from the center of D dist = calcDistanceBet2Points(Polygons[polygonIndex].Corners[Diagonal.indexOfCorner1], Polygons[orderOfPolygons[3]].Center); dist2 = calcDistanceBet2Points(Polygons[polygonIndex].Corners[Diagonal.indexOfCorner2], Polygons[orderOfPolygons[3]].Center); if (dist2 > dist) startingPoint[polygonIndex] = Diagonal.indexOfCorner2; } // For square D else if (polygonIndex == orderOfPolygons[3]) { // Select the corner which is closer to the center of A dist = calcDistanceBet2Points(Polygons[polygonIndex].Corners[Diagonal.indexOfCorner1], Polygons[orderOfPolygons[0]].Center); dist2 = calcDistanceBet2Points(Polygons[polygonIndex].Corners[Diagonal.indexOfCorner2], Polygons[orderOfPolygons[0]].Center); if (dist2 < dist) startingPoint[polygonIndex] = Diagonal.indexOfCorner2; } // For remaining squares else { // Consider vector DV as the vector joining D's center to the current polygon's center. // Select the corner which is on the same side of DV as that of A's center. Vec3f DV = Vec3f(Polygons[polygonIndex].Center.x - Polygons[orderOfPolygons[3]].Center.x, Polygons[polygonIndex].Center.y - Polygons[orderOfPolygons[3]].Center.y, 0); Vec3f cP1 = DV.cross(DA3); Vec3f DCorner = Vec3f(Polygons[polygonIndex].Corners[Diagonal.indexOfCorner1].x - Polygons[orderOfPolygons[3]].Center.x, Polygons[polygonIndex].Corners[Diagonal.indexOfCorner1].y - Polygons[orderOfPolygons[3]].Center.y, 0); Vec3f cP2 = DV.cross(DCorner); if (cP1.val[2]*cP2.val[2] < -1) startingPoint[polygonIndex] = Diagonal.indexOfCorner2; } } for (int polygonIndex = 0; polygonIndex < 6; polygonIndex++) { cornerIndex = startingPoint[orderOfPolygons[polygonIndex]]; for (int k=0; k<4; k++) { Corners[cornerLabel-1] = Polygons[orderOfPolygons[polygonIndex]].Corners[cornerIndex]; cornerLabel++; cornerIndex = (cornerIndex + 1) % 4; } } }
/*! The mouseMove is called by the viewer when the mouse is moved in the viewer and this handle is the active one. \param x the x-pos of the mouse (pixel) \param y the y-pos of the mouse (pixel) */ void Manipulator::mouseMove(const Int16 x, const Int16 y) { //SLOG << "Manipulator::mouseMove() enter\n" << std::flush; // get the beacon's core (must be ComponentTransform) and it's center if( getTarget() != NULL ) { // get transformation of beacon Transform *t = dynamic_cast<Transform *>(getTarget()->getCore()); if( t != NULL ) { UInt16 coord(0); // active coordinate: X=0, Y=1, Z=2 Int16 xDiff; // the mousepos x delta Int16 yDiff; // the mousepos y delta Vec3f centerV; // center of beacon Vec3f handleCenterV; // center of subhandle Vec2f mouseScreenV; // mouse move vector Vec2f handleScreenV; // handle vec in (cc) Real32 handleScreenVLen; // len of handle vec in (cc) Vec3f translation; // for matrix decomposition Quaternion rotation; Vec3f scaleFactor; Quaternion scaleOrientation; // TODO: das ist ja schon ein wenig haesslich static const Vec3f coordVector[3] = { Vec3f(1.0f, 0.0f, 0.0f), Vec3f(0.0f, 1.0f, 0.0f), Vec3f(0.0f, 0.0f, 1.0f) }; // check for the active handle if( getActiveSubHandle() == getHandleXNode()) { coord = 0; } else if(getActiveSubHandle() == getHandleYNode()) { coord = 1; } else if(getActiveSubHandle() == getHandleZNode()) { coord = 2; } // TODO: only for debugging, -> FDEBUG //SLOG << "moving " << ( coord == 0 ? "x\n" : // coord == 1 ? "y\n" : // "z\n" ) // << std::flush; // calculate diffs between current and last mouse position xDiff = x - Int16(getLastMousePos()[0]); yDiff = y - Int16(getLastMousePos()[1]); // set the vector resulting from user mouse movement and calc its length mouseScreenV.setValues(xDiff, -yDiff); t->getMatrix().getTransform(translation, rotation, scaleFactor, scaleOrientation); // calculate the camera coordinate of the center centerV = translation; Pnt2f centerPixPos = calcScreenProjection(centerV.addToZero(), getViewport()); // calculate the camera coordinate of the handle center handleCenterV = centerV + coordVector[coord]*getLength()[coord]; Pnt2f handleCenterPixPos = calcScreenProjection(handleCenterV.addToZero(), getViewport()); handleScreenV = handleCenterPixPos - centerPixPos; handleScreenVLen = handleScreenV.length(); Real32 s = handleScreenV.dot(mouseScreenV) / handleScreenVLen; doMovement(t, coord, s * getLength()[coord] * 0.01, translation, rotation, scaleFactor, scaleOrientation); } else { SWARNING << "handled object has no parent transform!\n"; } callExternalUpdateHandler(); } else { SWARNING << "Handle has no target.\n"; } setLastMousePos(Pnt2f(Real32(x), Real32(y))); updateHandleTransform(); //SLOG << "Manipulator::mouseMove() leave\n" << std::flush; }
// Handle obstacles in a generic way by post-adjusting a set of commanded game vars bool WAKGameShared::obstacleBallHandling(GameVars& GV) const { // Plot that obstacle ball handling is not active for the case that this function returns early if(config.plotData()) PM.plotScalar(false * PMSCALE_LEGAL, PM_OBH_ACTIVE); // Don't do anything if obstacle avoidance is disabled or we don't have a ball if(!config.sEnableObstacles() || !config.obhEnableObstBallHandling() || !SV.haveBall) return false; // Don't do anything if the closest obstacle is not valid if(!SV.obstClosest.valid()) return false; // Calculate the ball to target unit vector Vec2f ballToTargetVec = GV.ballTargetDir - SV.ballDir; float ballToTargetDist = ballToTargetVec.norm(); if(ballToTargetDist <= 0.0f) return false; Vec2f txhat = ballToTargetVec / ballToTargetDist; // Calculate the ball to obstacle unit vectors Vec2f ballToObstVec = SV.obstClosest.vec - SV.ballDir; float ballToObstDist = ballToObstVec.norm(); if(ballToObstDist <= 0.0f || ballToObstDist >= std::min(config.kickMaxDist(), ballToTargetDist + config.obhObstBeyondTargetBuf())) return false; Vec2f oxhat = ballToObstVec / ballToObstDist; Vec2f oyhat = eigenRotatedCCW90(oxhat); // Calculate the angle at the ball from the obstacle to the ball target float angleAtBall = atan2(txhat.dot(oyhat), txhat.dot(oxhat)); bool ballBehindObst = (fabs(angleAtBall) > M_PI_2); if(ballBehindObst && ballToObstDist > config.obhObstBeforeBallBuf()) return false; // Wrap the angle at the ball to (-pi/2, pi/2] by factors of pi and adjust for the sign float angleAtBallStd = angleAtBall; if(angleAtBallStd > M_PI_2) angleAtBallStd -= M_PI; if(angleAtBallStd <= -M_PI_2) angleAtBallStd += M_PI; int angleAtBallStdSign = sign(angleAtBallStd); angleAtBallStd = fabs(angleAtBallStd); // Calculate the high and low angles at the ball, and the appropriate difference float angleAtBallHigh = coerce<float>(asin(coerceAbs(config.obhObstClearanceHigh() / ballToObstDist, 1.0f)), 0.0f, config.obhClearanceAngleHighMax()); float angleAtBallLow = coerce<float>(asin(coerceAbs(config.obhObstClearanceLow() / ballToObstDist, 1.0f)), 0.0f, std::min(angleAtBallHigh, config.obhClearanceAngleLowMax())); // Calculate the angle to adjust the target angle by float angleAtBallDiff = angleAtBallHigh - angleAtBallLow; float targetAngleAdjust = 0.0f; if(angleAtBallStd < angleAtBallLow) targetAngleAdjust = angleAtBallStdSign * interpolateCoerced(0.0f, angleAtBallLow, 0.0f, angleAtBallDiff, angleAtBallStd); else if(angleAtBallStd < angleAtBallHigh) targetAngleAdjust = angleAtBallStdSign * interpolateCoerced(angleAtBallLow, angleAtBallHigh, angleAtBallDiff, 0.0f, angleAtBallStd); float targetAngleAdjustAbs = fabs(targetAngleAdjust); // Calculate the angle adjust limits for dribbling and foot selection float angleAdjustForDribble = coerceMin(0.5f * GV.ballTargetWedge * config.obhAngleAdjustWedgeRatio(), 0.0f); float angleAdjustForFootSel = config.obhAngleAdjustForFootSel(); // See whether the obstacle is blocking bool obstMightBlock = (angleAtBallDiff > angleAdjustForDribble); bool obstIsBlocking = (obstMightBlock && (angleAtBallStd < angleAtBallLow || targetAngleAdjustAbs > angleAdjustForDribble)); // Disable kick and suggest a foot to use if the obstacle is blocking bool kickIfPossible = !obstIsBlocking; GameVars::FootSelection suggestFoot = GameVars::FS_EITHER_FOOT; if(obstIsBlocking && ballToObstDist < config.obhFootSelObstBallDistMax()) { if(targetAngleAdjust > angleAdjustForFootSel) suggestFoot = (ballBehindObst ? GameVars::FS_LEFT_FOOT : GameVars::FS_RIGHT_FOOT); else if(targetAngleAdjust < -angleAdjustForFootSel) suggestFoot = (ballBehindObst ? GameVars::FS_RIGHT_FOOT : GameVars::FS_LEFT_FOOT); } // Decide whether the obstacle ball handling has to make an adjustment to the game variables bool adjustNotNeeded = (targetAngleAdjustAbs <= 0.0f && kickIfPossible && suggestFoot == GameVars::FS_EITHER_FOOT); // Plotting if(config.plotData()) { PM.plotScalar(!adjustNotNeeded * PMSCALE_LEGAL, PM_OBH_ACTIVE); PM.plotScalar(ballToObstDist, PM_OBH_BALLTOOBSTDIST); PM.plotScalar(angleAtBallLow, PM_OBH_ANGLEATBALLLOW); PM.plotScalar(angleAtBallHigh, PM_OBH_ANGLEATBALLHIGH); PM.plotScalar(angleAtBallStd, PM_OBH_ANGLEATBALLSTD); PM.plotScalar(targetAngleAdjust, PM_OBH_TARGETANGLEADJUST); PM.plotScalar(angleAdjustForDribble, PM_OBH_ANGLEADJUSTFORDRIBBLE); PM.plotScalar(angleAdjustForFootSel, PM_OBH_ANGLEADJUSTFORFOOTSEL); PM.plotScalar(kickIfPossible * PMSCALE_KICK, PM_OBH_KICKIFPOSSIBLE); PM.plotScalar(suggestFoot, PM_OBH_SUGGESTFOOT); } // If no adjustment is required then we are fine if(adjustNotNeeded) return false; // Calculate the new ball target as a rotation of the nominal ball target Vec2f ballToTargetVecDes = eigenRotatedCCW(ballToTargetVec, targetAngleAdjust); Vec2f ballTargetVec = SV.ballDir + ballToTargetVecDes; // Update the game variables GV.ballTargetDir = ballTargetVec; if(GV.kickIfPossible) GV.kickIfPossible = kickIfPossible; if(GV.suggestFoot == GameVars::FS_EITHER_FOOT) GV.suggestFoot = suggestFoot; // Return that the game variables were modified return true; }
void World::simulate(float dt) { mAllocator.clearFrame(); int vIterNum = 50; int pIterNum = 10; mColManager.preocss( dt ); for( RigidBodyList::iterator iter = mRigidBodies.begin() ,itEnd = mRigidBodies.end(); iter != itEnd ; ++iter ) { RigidBody* body = *iter; body->saveState(); if ( body->getMotionType() != BodyMotion::eStatic ) { if ( body->getMotionType() == BodyMotion::eDynamic ) body->addLinearImpulse( body->mMass * mGrivaty * dt ); body->applyImpulse(); body->mLinearVel *= 1.0 / ( 1 + body->mLinearDamping ); } } ContactManifold** sortedContact = new ( mAllocator ) ContactManifold* [ mColManager.mMainifolds.size() ]; int numMainfold = mColManager.mMainifolds.size(); int idxStatic = mColManager.mMainifolds.size() - 1; int idxNormal = 0; for( int i = 0 ; i < mColManager.mMainifolds.size() ; ++i ) { ContactManifold& cm = *mColManager.mMainifolds[i]; RigidBody* bodyA = static_cast< RigidBody* >( cm.mContect.object[0] ); RigidBody* bodyB = static_cast< RigidBody* >( cm.mContect.object[1] ); if ( bodyA->getMotionType() == BodyMotion::eStatic || bodyB->getMotionType() == BodyMotion::eStatic ) { sortedContact[idxStatic--] = &cm; } else { sortedContact[idxNormal++] = &cm; } } for( int i = 0 ; i < numMainfold ; ++i ) { ContactManifold& cm = *sortedContact[i]; Contact& c = cm.mContect; Vec2f cp = 0.5 * ( c.pos[0] + c.pos[1] ); RigidBody* bodyA = static_cast< RigidBody* >( c.object[0] ); RigidBody* bodyB = static_cast< RigidBody* >( c.object[1] ); Vec2f vA = bodyA->getVelFromWorldPos( cp ); Vec2f vB = bodyB->getVelFromWorldPos( cp ); float vrel = c.normal.dot( vB - vA ); float relectParam = 1.0f; cm.velParam = 0; if ( vrel < -1 ) { cm.velParam = -relectParam * vrel; } //cm.impulse = 0; ////warm start Vec2f rA = cp - bodyA->mPosCenter; Vec2f rB = cp - bodyB->mPosCenter; Vec2f dp = cm.impulse * c.normal; bodyA->mLinearVel -= dp * bodyA->mInvMass; //bodyA->mAngularVel -= rA.cross( dp ) * bodyA->mInvI; bodyB->mLinearVel += dp * bodyB->mInvMass; //bodyB->mAngularVel += rB.cross( dp ) * bodyB->mInvI; } if ( numMainfold != 0 ) jumpDebug(); //std::sort( sortedContact.begin() , sortedContact.end() , DepthSort() ); for( int nIter = 0 ; nIter < vIterNum ; ++nIter ) { for( int i = 0 ; i < numMainfold ; ++i ) { ContactManifold& cm = *sortedContact[i]; Contact& c = cm.mContect; RigidBody* bodyA = static_cast< RigidBody* >( c.object[0] ); RigidBody* bodyB = static_cast< RigidBody* >( c.object[1] ); Vec2f cp = 0.5 * ( c.pos[0] + c.pos[1] ); Vec2f vA = bodyA->getVelFromWorldPos( cp ); Vec2f vB = bodyB->getVelFromWorldPos( cp ); Vec2f rA = cp - bodyA->mPosCenter; Vec2f rB = cp - bodyB->mPosCenter; Vec2f vrel = vB - vA; float vn = vrel.dot( c.normal ); float nrA = rA.cross( c.normal ); float nrB = rB.cross( c.normal ); float invMass = 0; invMass += bodyA->mInvMass + bodyA->mInvI * nrA * nrA; invMass += bodyB->mInvMass + bodyB->mInvI * nrB * nrB; float impulse = -( vn - cm.velParam ) / invMass; float newImpulse = Math::Max( cm.impulse + impulse , 0.0f ); impulse = newImpulse - cm.impulse; bodyA->mLinearVel -= impulse * c.normal * bodyA->mInvMass; //bodyA->mAngularVel -= impulse * nrA * bodyA->mInvI; bodyB->mLinearVel += impulse * c.normal * bodyB->mInvMass; //bodyB->mAngularVel += impulse * nrB * bodyB->mInvI; cm.impulse = newImpulse; float fa = impulse * nrA * bodyA->mInvI; float fb = impulse * nrB * bodyB->mInvI; if ( fa != 0 || fb !=0 ) { int i = 1; } if ( numMainfold != 0 ) jumpDebug(); } } for( RigidBodyList::iterator iter = mRigidBodies.begin() ,itEnd = mRigidBodies.end(); iter != itEnd ; ++iter ) { RigidBody* body = *iter; body->intergedTramsform( dt ); //body->mAngularVel = 0; } for( int nIter = 0 ; nIter < pIterNum ; ++nIter ) { float const kValueB = 0.8f; float const kMaxDepth = 2.f; float const kSlopValue = 0.0001f; float maxDepth = 0.0; for( int i = 0 ; i < numMainfold ; ++i ) { ContactManifold& cm = *sortedContact[i]; Contact& c = cm.mContect; RigidBody* bodyA = static_cast< RigidBody* >( c.object[0] ); RigidBody* bodyB = static_cast< RigidBody* >( c.object[1] ); if ( bodyB->getMotionType() != BodyMotion::eDynamic && bodyB->getMotionType() != BodyMotion::eDynamic ) continue; Vec2f cpA = bodyA->mXForm.mul( c.posLocal[0] ); Vec2f cpB = bodyB->mXForm.mul( c.posLocal[1] ); //TODO: normal change need concerned Vec2f normal = c.normal; float depth = normal.dot( cpA - cpB ) + 0.001; if ( depth <= 0 ) continue; Vec2f cp = 0.5 * ( cpA + cpB ); Vec2f rA = cp - bodyA->mPosCenter; Vec2f rB = cp - bodyB->mPosCenter; float nrA = rA.cross( normal ); float nrB = rB.cross( normal ); float invMass = 0; invMass += bodyA->mInvMass + bodyA->mInvI * nrA * nrA; invMass += bodyB->mInvMass + bodyB->mInvI * nrB * nrB; float offDepth = Math::Clamp ( ( depth - kSlopValue ) , 0 , kMaxDepth ); float impulse = ( invMass > 0 ) ? kValueB * offDepth / invMass : 0; if ( impulse > 0 ) { bodyA->mXForm.translate( -impulse * normal * bodyA->mInvMass ); bodyA->mRotationAngle += -impulse * nrA * bodyA->mInvI; bodyA->synTransform(); bodyB->mXForm.translate( impulse * normal * bodyB->mInvMass ); bodyB->mRotationAngle += impulse * nrB * bodyB->mInvI; bodyB->synTransform(); } { Vec2f cpA = bodyA->mXForm.mul( c.posLocal[0] ); Vec2f cpB = bodyB->mXForm.mul( c.posLocal[1] ); //TODO: normal change need concerned float depth2 = normal.dot( cpA - cpB ); float dp = depth - depth2; ::Msg( "dp = %f " , dp ); int i = 1; } } if ( maxDepth < 3 * kMaxDepth ) break; } if ( numMainfold != 0 ) jumpDebug(); }