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