enum Error DepositAction::doAction(void) {
io << "DO DEPOSIT\n";
if((positionManager().getValue() - controlPoint()).norm() > 100) {
io << "not at a good position\n";
return IMPOSSIBLE;
}
s32 x = ((side == RED) ? 750 : -750);
// So we first get a bit closer to the basket
trajectoryManager().setMode(TrajectoryManager::FORWARD);
trajectoryManager().gotoPosition(Vect<2, s32>(x, 450));
while(!trajectoryManager().isEnded()) {
if (robot().getValue()) {
return SKATING;
}
if (check_for_collision(60)) {
return IMPOSSIBLE;
}
}
// We stick to the basket
asserv_speed_slow();
for(int i = 0; i < 3; i++) {
trajectoryManager().gotoDistance(3000);
while(!robot().getValue()) {
}
trajectoryManager().reset();
}
positionManager().setY(1050 - 300 - 125);
robot().unlock();
// Actually deposit the fruits now
basket_servo.setValue(BASKET_SERVO_DOWN_CMD);
_delay_ms(1000);
basket_servo.setValue(BASKET_SERVO_UP_CMD);
_fruit = 0;
// Go far from the basket
asserv_speed_normal();
trajectoryManager().gotoDistance(-50);
while(!trajectoryManager().isEnded()) {
robot().unlock();
}
// And finally we return to the control point
trajectoryManager().setMode(TrajectoryManager::FASTER);
trajectoryManager().gotoPosition(Vect<2, s32>(x, 450));
while(!trajectoryManager().isEnded()) {
if (robot().getValue()) {
return SKATING;
}
if (check_for_collision(60)) {
return IMPOSSIBLE;
}
}
//done();
return SUCCESS;
}
inline int getPErepresentingNodeContainingPE(int pe){
#if 1
return pe;
#else
#if USE_CONTROL_POINTS
std::vector<int> v;
v.push_back(1);
if(CkNumPes() >= 2)
v.push_back(2);
if(CkNumPes() >= 4)
v.push_back(4);
if(CkNumPes() >= 8)
v.push_back(8);
int pes_per_node = controlPoint("Number of PEs per Node", v);
#else
int pes_per_node = 1;
#endif
if(getenv("PE_PER_NODES") != NULL)
pes_per_node = CkNumPes()/atoi(getenv("PE_PER_NODES"));
if( pes_per_node > 1 && pes_per_node <= CkNumPes() ){
ComlibPrintf("NODE AWARE Sending a message to a representative of the node instead of its real owner\n");
int newpe = pe - (pe % pes_per_node);
return newpe;
} else {
return pe;
}
#endif
}
/*
BezierPatchMesh::BezierPatchSample BezierPatchMesh::sample(double u, double v) const
{
BezierPatchSample ret;
ret.uv = Vec2d(u,v);
Vec3d x (0,0,0);
// Programming TASK 2: implement this method
// You need to compute ret.position and ret.normal!
// This data will be used within the initialize() function for the
// triangle mesh construction.
//Tensorprodukt
//std::cout << "::::::::::::::::::::::::::::::::::::::::::::::::::" << std::endl;
//std::cout << "claculating for u:" << u << " v: " << v << std::endl;
for (int i = 0; i < mM ; ++i)
{
for (int j = 0; j < mN ; ++j)
{
double Bernsteinpolynom1;
double Bernsteinpolynom2;
double Binomialkoefizient1 = 1;
double Binomialkoefizient2 = 1;
if (i <= mM)
{
Binomialkoefizient1 = factorial(mM) / (factorial(mM - i) * factorial(i));
}
else Binomialkoefizient1 = 0;
if (j <= mN)
{
Binomialkoefizient2 = factorial(mN) / (factorial(mN - j) * factorial(j));
}
else Binomialkoefizient2 = 0;
//if (mM - 1 <= Math::safetyEps() && mM - 1 >= -Math::safetyEps()) Bernsteinpolynom1 = Binomialkoefizient1 * pow(u, i) * 1;
Bernsteinpolynom1 = Binomialkoefizient1 * pow(u, i) * pow((1 - u), (mM - i));
//if (mN - 1 <= Math::safetyEps() && mN - 1 >= -Math::safetyEps()) Bernsteinpolynom2 = Binomialkoefizient2 * pow(v, j) * 1;
Bernsteinpolynom2 = Binomialkoefizient2 * pow(v, j) * pow((1 - v), (mN - j));
x += (controlPoint(i, j) * Bernsteinpolynom1 * Bernsteinpolynom2);
//std::cout << "i: " << i << "j: " << j << ", Bp1: " << Bernsteinpolynom1 << ", Bp2: " << Bernsteinpolynom2 << ", cP: " << controlPoint(i, j) << std::endl;
}
}
ret.position = x;
ret.normal = Vec3d();
return ret;
}
*/
Vec3d BezierPatchMesh::deCasteljau(Vec3d b, double t, size_t r, size_t i) const
{
if (r == 0) {
return controlPoint(r, i);
}
else {
return ((1 - t) * (deCasteljau(b, t, (r - 1), i) + (t * deCasteljau(b, t, (r - 1), (i + 1)))));
}
}
void CGUIControl::UnfocusFromPoint(const CPoint &point)
{
if (HasFocus())
{
CPoint controlPoint(point);
m_transform.InverseTransformPosition(controlPoint.x, controlPoint.y);
if (!HitTest(controlPoint))
SetFocus(false);
}
}
s16 HuntAction::priority(void) {
if(_static_priority == 0) {
return 0;
}
if(_static_priority < 10) _static_priority++;
s16 dist = (controlPoint() - positionManager().getValue()).norm();
if(dist != 0) {
return _static_priority * (10000 / dist);
}
return 10000;
}
BezierPatchMesh::BezierPatchSample BezierPatchMesh::sample(real u, real v) const
{
// samples a Bezier curve and curve tangent at parameter t using
// DeCasteljau algorithm
BezierPatchSample ret;
ret.normal = Vec3();
ret.uv = Vec2(u,v);
//perform DeCasteljau algorithm in both directions
std::vector<Vec3> uControlPoints(mM), vControlPoints(mN);
Vec3 utangent, vtangent;
//compute v tangent
{
for(size_t j=0; j<mN; ++j)
{
//fill u curve
for(size_t i=0; i<mM; ++i)
uControlPoints[i] = controlPoint(i,j);
//sample u curve
vControlPoints[j] = this->deCasteljau(uControlPoints, u).first;
}
//sample v tangent
vtangent = this->deCasteljau(vControlPoints, v).second;
}
//compute u tangent and surface point
{
for(size_t i=0; i<mM; ++i)
{
//fill v curve
for(size_t j=0; j<mN; ++j)
vControlPoints[j] = this->controlPoint(i,j);
//sample u curve
uControlPoints[i] = this->deCasteljau(vControlPoints, v).first;
}
//sample u tangent and surface point
std::pair<Vec3,Vec3> point_tangent = this->deCasteljau(uControlPoints, u);
ret.position = point_tangent.first;
utangent = point_tangent.second;
}
//compute normal vector from surface tangents
ret.normal = (utangent % vtangent).normalize();
return ret;
}
BezierPatchMesh::BezierPatchSample BezierPatchMesh::sample(double u, double v) const
{
BezierPatchSample ret;
ret.uv = Vec2d(u, v);
//std::vector<Vec3d> P = mControlPoints;
size_t m = mM;
size_t n = mN;
//controlPoints(i,j)
std::vector<Vec3d> q;
Vec3d p;
for (int i = 0; i < m; ++i) {
for (size_t r = 1; r <= n; ++r) {
for (size_t j = 0; j <= (n - r); ++j) { // 3 - 1 = 2, 3 - 2 = 1, 3 - 3 = 0
q[i] = deCasteljau(controlPoint(j, 0), v, i + 1, j);
}
}
for (int j = 0; j < n; ++j) {
}
deCasteljauQ(q[i], u, , , q)
}
for (size_t r = 1; r <= n; ++r) {
for (size_t i = 0; i <= (n + r); ++i) {
p = deCasteljauQ(temp[i], u, r, i, temp);
}
}
// Programming TASK 2: implement this method
// You need to compute ret.position and ret.normal!
// This data will be used within the initialize() function for the
// triangle mesh construction.
ret.position = p;
ret.normal = Vec3d();
return ret;
}
void SplinePath::AddControlPoint(Node* point, unsigned index)
{
if (!point)
return;
WeakPtr<Node> controlPoint(point);
point->AddListener(this);
controlPoints_.Insert(index, controlPoint);
spline_.AddKnot(point->GetWorldPosition(), index);
UpdateNodeIds();
CalculateLength();
}
s16 DepositAction::priority(void) {
if(_fruit == 0) {
return 0;
}
if(_static_priority == 0) {
return 0;
}
_static_priority+= 3;
s16 dist = (controlPoint() - positionManager().getValue()).norm();
if(dist != 0) {
return _static_priority * (_fruit * 7000 / dist);
}
return _static_priority * (_fruit * 7000);
}
void SplinePath::ApplyAttributes()
{
if (!dirty_)
return;
// Remove all old instance nodes before searching for new. Can not call RemoveAllInstances() as that would modify
// the ID list on its own
for (unsigned i = 0; i < controlPoints_.Size(); ++i)
{
Node* node = controlPoints_[i];
if (node)
node->RemoveListener(this);
}
controlPoints_.Clear();
spline_.Clear();
Scene* scene = GetScene();
if (scene)
{
// The first index stores the number of IDs redundantly. This is for editing
for (unsigned i = 1; i < controlPointIdsAttr_.Size(); ++i)
{
Node* node = scene->GetNode(controlPointIdsAttr_[i].GetUInt());
if (node)
{
WeakPtr<Node> controlPoint(node);
node->AddListener(this);
controlPoints_.Push(controlPoint);
spline_.AddKnot(node->GetWorldPosition());
}
}
Node* node = scene->GetNode(controlledIdAttr_);
if (node)
{
WeakPtr<Node> controlled(node);
controlledNode_ = controlled;
}
}
CalculateLength();
dirty_ = false;
}
void SplinePath::OnMarkedDirty(Node* point)
{
if (!point)
return;
WeakPtr<Node> controlPoint(point);
for (unsigned i = 0; i < controlPoints_.Size(); ++i)
{
if (controlPoints_[i] == controlPoint)
{
spline_.SetKnot(point->GetWorldPosition(), i);
break;
}
}
CalculateLength();
}
void CGUIControl::UnfocusFromPoint(const CPoint &point)
{
if (HasFocus())
{
CPoint controlPoint(point);
m_transform.InverseTransformPosition(controlPoint.x, controlPoint.y);
if (!HitTest(controlPoint))
{
SetFocus(false);
// and tell our parent so it can unfocus
if (m_parentControl)
{
CGUIMessage msgLostFocus(GUI_MSG_LOSTFOCUS, GetID(), GetID());
m_parentControl->OnMessage(msgLostFocus);
}
}
}
}
void SplinePath::OnNodeSetEnabled(Node* point)
{
if (!point)
return;
WeakPtr<Node> controlPoint(point);
for (unsigned i = 0; i < controlPoints_.Size(); ++i)
{
if (controlPoints_[i] == controlPoint)
{
if (point->IsEnabled())
spline_.AddKnot(point->GetWorldPosition(), i);
else
spline_.RemoveKnot(i);
break;
}
}
CalculateLength();
}
void SplinePath::RemoveControlPoint(Node* point)
{
if (!point)
return;
WeakPtr<Node> controlPoint(point);
point->RemoveListener(this);
for (unsigned i = 0; i < controlPoints_.Size(); ++i)
{
if (controlPoints_[i] == controlPoint)
{
controlPoints_.Erase(i);
spline_.RemoveKnot(i);
break;
}
}
UpdateNodeIds();
CalculateLength();
}
FTContour::FTContour( FT_Vector* contour, char* pointTags, unsigned int numberOfPoints)
{
for( unsigned int pointIndex = 0; pointIndex < numberOfPoints; ++ pointIndex)
{
char pointTag = pointTags[pointIndex];
if( pointTag == FT_Curve_Tag_On || numberOfPoints < 2)
{
AddPoint( contour[pointIndex].x, contour[pointIndex].y);
continue;
}
FTPoint controlPoint( contour[pointIndex]);
FTPoint previousPoint = ( 0 == pointIndex)
? FTPoint( contour[numberOfPoints - 1])
: pointList[pointList.size() - 1];
FTPoint nextPoint = ( pointIndex == numberOfPoints - 1)
? pointList[0]
: FTPoint( contour[pointIndex + 1]);
if( pointTag == FT_Curve_Tag_Conic)
{
char nextPointTag = ( pointIndex == numberOfPoints - 1)
? pointTags[0]
: pointTags[pointIndex + 1];
while( nextPointTag == FT_Curve_Tag_Conic)
{
nextPoint = ( controlPoint + nextPoint) * 0.5f;
controlPoints[0][0] = previousPoint.X(); controlPoints[0][1] = previousPoint.Y();
controlPoints[1][0] = controlPoint.X(); controlPoints[1][1] = controlPoint.Y();
controlPoints[2][0] = nextPoint.X(); controlPoints[2][1] = nextPoint.Y();
evaluateQuadraticCurve();
++pointIndex;
previousPoint = nextPoint;
controlPoint = FTPoint( contour[pointIndex]);
nextPoint = ( pointIndex == numberOfPoints - 1)
? pointList[0]
: FTPoint( contour[pointIndex + 1]);
nextPointTag = ( pointIndex == numberOfPoints - 1)
? pointTags[0]
: pointTags[pointIndex + 1];
}
controlPoints[0][0] = previousPoint.X(); controlPoints[0][1] = previousPoint.Y();
controlPoints[1][0] = controlPoint.X(); controlPoints[1][1] = controlPoint.Y();
controlPoints[2][0] = nextPoint.X(); controlPoints[2][1] = nextPoint.Y();
evaluateQuadraticCurve();
continue;
}
if( pointTag == FT_Curve_Tag_Cubic)
{
FTPoint controlPoint2 = nextPoint;
FTPoint nextPoint = ( pointIndex == numberOfPoints - 2)
? pointList[0]
: FTPoint( contour[pointIndex + 2]);
controlPoints[0][0] = previousPoint.X(); controlPoints[0][1] = previousPoint.Y();
controlPoints[1][0] = controlPoint.X(); controlPoints[1][1] = controlPoint.Y();
controlPoints[2][0] = controlPoint2.X(); controlPoints[2][1] = controlPoint2.Y();
controlPoints[3][0] = nextPoint.X(); controlPoints[3][1] = nextPoint.Y();
evaluateCubicCurve();
++pointIndex;
continue;
}
}
}