//----------------------------------------------------------------------- void Extruder::_extrudeBodyImpl(TriangleBuffer& buffer, const Shape* shapeToExtrude) const { assert(mExtrusionPath && shapeToExtrude && "Shape and Path must not be null!"); unsigned int numSegPath = mExtrusionPath->getSegCount(); unsigned int numSegShape = shapeToExtrude->getSegCount(); assert(numSegPath>0 && numSegShape>0 && "Shape and path must contain at least two points"); Real totalPathLength = mExtrusionPath->getTotalLength(); Real totalShapeLength = shapeToExtrude->getTotalLength(); // Merge shape and path with tracks Ogre::Real lineicPos=0.; Path path = *mExtrusionPath; if (mRotationTrack) path = path.mergeKeysWithTrack(*mRotationTrack); if (mScaleTrack) path = path.mergeKeysWithTrack(*mScaleTrack); if (mPathTextureTrack) path = path.mergeKeysWithTrack(*mPathTextureTrack); numSegPath = path.getSegCount(); Shape shape = *shapeToExtrude; if (mShapeTextureTrack) shape = shape.mergeKeysWithTrack(*mShapeTextureTrack); numSegShape = shape.getSegCount(); // Estimate vertex and index count buffer.rebaseOffset(); buffer.estimateIndexCount(numSegShape*numSegPath*6); buffer.estimateVertexCount((numSegShape+1)*(numSegPath+1)); Vector3 oldup; for (unsigned int i = 0; i <= numSegPath; ++i) { Vector3 v0 = path.getPoint(i); Vector3 direction = path.getAvgDirection(i); Quaternion q = Utils::_computeQuaternion(direction); Radian angle = (q*Vector3::UNIT_Y).angleBetween(oldup); if (i>0 && angle>(Radian)Math::HALF_PI/2.) { q = Utils::_computeQuaternion(direction, oldup); } oldup = q * Vector3::UNIT_Y; Real scale=1.; if (i>0) lineicPos += (v0-path.getPoint(i-1)).length(); // Get the values of angle and scale if (mRotationTrack) { Real angle; angle = mRotationTrack->getValue(lineicPos, lineicPos / totalPathLength, i); q = q*Quaternion((Radian)angle, Vector3::UNIT_Z); } if (mScaleTrack) { scale = mScaleTrack->getValue(lineicPos, lineicPos / totalPathLength, i); } Real uTexCoord; if (mPathTextureTrack) uTexCoord = mPathTextureTrack->getValue(lineicPos, lineicPos / totalPathLength, i); else uTexCoord = lineicPos / totalPathLength; Real lineicShapePos = 0.; // Insert new points for (unsigned int j =0; j <= numSegShape; ++j) { Vector2 vp2 = shapeToExtrude->getPoint(j); //Vector2 vp2direction = shapeToExtrude->getAvgDirection(j); Vector2 vp2normal = shapeToExtrude->getAvgNormal(j); Vector3 vp(vp2.x, vp2.y, 0); Vector3 normal(vp2normal.x, vp2normal.y, 0); buffer.rebaseOffset(); Vector3 newPoint = v0+q*(scale*vp); if (j>0) lineicShapePos += (vp2 - shape.getPoint(j-1)).length(); Real vTexCoord; if (mShapeTextureTrack) vTexCoord = mShapeTextureTrack->getValue(lineicShapePos, lineicShapePos / totalShapeLength, j); else vTexCoord = lineicShapePos / totalShapeLength; addPoint(buffer, newPoint, q*normal, Vector2(uTexCoord, vTexCoord)); if (j <numSegShape && i <numSegPath) { if (shapeToExtrude->getOutSide() == SIDE_LEFT) { buffer.triangle(numSegShape + 1, numSegShape + 2, 0); buffer.triangle(0, numSegShape + 2, 1); } else { buffer.triangle(numSegShape + 2, numSegShape + 1, 0); buffer.triangle(numSegShape + 2, 0, 1); } } } } }
bool Cylinder::intersectBody( const ray& r, isect& i ) const { double x0 = r.getPosition()[0]; double y0 = r.getPosition()[1]; double x1 = r.getDirection()[0]; double y1 = r.getDirection()[1]; double a = x1*x1+y1*y1; double b = 2.0*(x0*x1 + y0*y1); double c = x0*x0 + y0*y0 - 1.0; if( 0.0 == a ) { // This implies that x1 = 0.0 and y1 = 0.0, which further // implies that the ray is aligned with the body of the cylinder, // so no intersection. return false; } double discriminant = b*b - 4.0*a*c; if( discriminant < 0.0 ) { return false; } discriminant = sqrt( discriminant ); double t2 = (-b + discriminant) / (2.0 * a); if( t2 <= RAY_EPSILON ) { return false; } double t1 = (-b - discriminant) / (2.0 * a); if( t1 > RAY_EPSILON ) { // Two intersections. vec3f P = r.at( t1 ); double z = P[2]; if( z >= 0.0 && z <= 1.0 ) { // It's okay. i.t = t1; i.N = vec3f( P[0], P[1], 0.0 ).normalize(); return true; } } vec3f P = r.at( t2 ); double z = P[2]; if( z >= 0.0 && z <= 1.0 ) { i.t = t2; vec3f normal( P[0], P[1], 0.0 ); // In case we are _inside_ the _uncapped_ cone, we need to flip the normal. // Essentially, the cone in this case is a double-sided surface // and has _2_ normals if( !capped && normal.dot( r.getDirection() ) > 0 ) normal = -normal; i.N = normal.normalize(); return true; } return false; }
//----------------------------------------------------------------------------- void ManualObject::normal(const Vector3& norm) { normal(norm.x, norm.y, norm.z); }
void DoAnim() { static double time = 0.0; //Total time running. static double artTime = 0.0; //Total time with the current art. static DWORD lastTime = 0; //Time of last call. const double elapsed = double(GetTickCount() - lastTime) / 1000.0; if (lastTime) { lastTime = GetTickCount(); } else { lastTime = GetTickCount(); return; } time += elapsed; artTime += elapsed; //If we need new art, get it. static CKnot::AutoArt threads; static Arrays arrays; static FloatArray grid; if (!threads.get() || artTime > ResetTime) { CKnot::StrokeList sl = CreateSquareStrokes(); sl = RemoveStrokes(sl); threads = CKnot::CreateThread(sl); artTime = 0.0; grid.clear(); grid.reserve(sl.size() * 10); for (CKnot::StrokeList::const_iterator it = sl.begin(); it != sl.end(); ++it) { grid.push_back(it->a.x); grid.push_back(it->a.y); grid.push_back(it->type == CKnot::Cross ? 1.0 : 0.0); grid.push_back(it->type == CKnot::Glance ? 1.0 : 0.0); grid.push_back(it->type == CKnot::Bounce ? 1.0 : 0.0); grid.push_back(it->b.x); grid.push_back(it->b.y); grid.push_back(it->type == CKnot::Cross ? 1.0 : 0.0); grid.push_back(it->type == CKnot::Glance ? 1.0 : 0.0); grid.push_back(it->type == CKnot::Bounce ? 1.0 : 0.0); } for (size_t i = 0; i < arrays.size(); ++i) delete arrays[i]; arrays.clear(); const size_t threadCount = threads->GetThreadCount(); for (size_t i = 0; i < threadCount; ++i) { const CKnot::Art::Thread* thread = threads->GetThread(i); const CKnot::Art::Z* z = threads->GetZ(i); const size_t segsPerKnot = 25; const size_t kc = thread->GetKnotCount(); FloatArray* quads = new FloatArray; arrays.push_back(quads); const size_t target = kc * segsPerKnot; const size_t memSize = 12 * (target + 1); quads->reserve(memSize); const float scr = double(rand()) / RAND_MAX / 2; const float ecr = double(rand()) / RAND_MAX / 2 + .5; const float scg = double(rand()) / RAND_MAX / 2; const float ecg = double(rand()) / RAND_MAX / 2 + .5; const float scb = double(rand()) / RAND_MAX / 2; const float ecb = double(rand()) / RAND_MAX / 2 + .5; for (size_t i = 0; i <= target; ++i) { const double s = double(i) / double(target); const double t = s; const CKnot::vec2 cur = thread->Y(t); const CKnot::vec2 dcur = thread->Y(t + .00001); const CKnot::vec2 diff = dcur - cur; CKnot::vec2 normal(diff.y, -diff.x); normal = normal * (1.0 / normal.GetLength()); normal = normal * .01; const CKnot::vec2 flip(-normal.x, -normal.y); const CKnot::vec2 start = cur + normal; const CKnot::vec2 end = cur + flip; const bool over = z->Y(t) > 0.0; //Coords quads->push_back(start.x); quads->push_back(start.y); quads->push_back(over ? 0.01 : .1); //Colors quads->push_back(scr); quads->push_back(scg); quads->push_back(scb); quads->push_back(end.x); quads->push_back(end.y); quads->push_back(over ? 0.01 : .1); quads->push_back(ecr); quads->push_back(ecg); quads->push_back(ecb); } assert(quads->size() == memSize); } } //Clear the background some nice color. glClearColor( 0.125f + std::sin(time / 2.0) / 8.0, 0.125f + std::sin(time / 3.0) / 8.0, 0.125f + std::sin(time / 5.0) / 8.0, 0.0f); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glEnableClientState(GL_VERTEX_ARRAY); glEnableClientState(GL_COLOR_ARRAY); for (size_t i = 0; i < arrays.size(); ++i) { FloatArray& quads = *arrays[i]; glVertexPointer(3, GL_FLOAT, 24, &quads.front()); glColorPointer(3, GL_FLOAT, 24, &quads.front() + 3); const size_t count = quads.size() / 6; const size_t progress = size_t(std::min(artTime / DrawTime, 1.0) * count / 2); //From 0 to .5 of vertices. assert(progress >= 0); assert(progress <= count / 2); size_t start = (count / 2) - progress; start += start % 2; glDrawArrays(GL_QUAD_STRIP, start, progress * 2); } //Draw graph if (DrawGraph) { glVertexPointer(2, GL_FLOAT, 20, &grid.front()); glColorPointer(3, GL_FLOAT, 20, &grid.front() + 2); glDrawArrays(GL_LINES, 0, grid.size() / 5); } glDisableClientState(GL_VERTEX_ARRAY); glDisableClientState(GL_COLOR_ARRAY); glLoadIdentity(); }
int Colisao::colisaoJogadorDisco(int j) { static bool antcol[2]; // estado anterior da colisao (true/false) // Seja P o jogador e Q o disco. // Seja P1 a posicao anterior e P2 a posicao atual (o mesmo vale para Q) Vetor P1(jog[j]->getOldPos()), P2(jog[j]->getPos()); Vetor Q1(disco->getOldPos()), Q2(disco->getPos()); // Calcularemos agora as velocidades Vetor VP = P2 - P1; Vetor VQ = Q2 - Q1; // Definindo variaveis para otimizar o calculo Vetor A = P1 - Q1; Vetor B = VP - VQ; float A2 = A.prodEscalar(A); float B2 = B.prodEscalar(B); float AB = A.prodEscalar(B); float AB2 = AB * AB; // quadrado da distancia entre os centros float d2 = pow(jog[j]->getRaio() + disco->getRaio(), 2); float t; // tempo // se ainda nao esta colidindo if ((jog[j]->getPos() - disco->getPos()).norma() > jog[j]->getRaio() + disco->getRaio()) { //LD fixes float zero = 0.0f; // Teste para verificar a nao-colisao if (B == zero) // movimento relativo == 0 { antcol[j] = false; return 0; } // nao houve colisao // Teste para verificar a nao-colisao if (A2 - AB2 / B2 > d2) { antcol[j] = false; return 0; } // nao houve colisao // Teste para verificar a nao colisao float raiz = AB2 - B2 * (A2 - d2); if (raiz < 0) { antcol[j] = false; return 0; } // nao houve colisao // tempo da colisao t = (-(AB) - sqrt(raiz)) / B2; // Teste para verificar a nao-colisao if (!(t > 0 && t <= 1)) { antcol[j] = false; return 0; } // nao houve colisao // Verifica se ja houve colisao no instante anterior if (antcol[j]) return 0; // Atualiza posicoes para o instante de colisao jog[j]->setPos(P1 + t * VP); disco->setPos(Q1 + t * VQ); } else { // Atualiza a posicao do disco (para ficar _fora_ do jog[j]-> Vetor normal = (disco->getPos() - jog[j]->getPos()).versor() * (disco->getRaio() + jog[j]->getRaio()); disco->setPos(jog[j]->getPos() + normal); } // normal (do disco para o jogador) Vetor normal(jog[j]->getPos() - disco->getPos()); normal = (disco->getVel().prodEscalar(normal) / normal.prodEscalar(normal)) * normal; // projecao // vetor tangente Vetor tangente = disco->getVel() - normal; // velocidade resultante (da disco) disco->setVel(tangente - normal); // afasta um pouco o disco do jogador disco->setPos(disco->getPos() + disco->getVel().versor() * 3); // Aumenta velocidade do disco (armengue) Vetor v = disco->getVel().versor(); if (v.norma() == 0) v = jog[j]->getVel().versor() * 10.0; disco->setVel(disco->getVel() + v * 2.0); antcol[j] = true; return 1; }
static void ExtrudeFaces (void* userData, int vertexCount, const dFloat* faceVertec, int id) { dFloat OFFSET = 0.1f; dFloat face[32][10]; NewtonMesh* mesh = (NewtonMesh*) userData; // calculate the face normal dVector normal (0.0f); dVector p0 (faceVertec[0 * 3 + 0], faceVertec[0 * 3 + 1], faceVertec[0 * 3 + 2]); dVector p1 (faceVertec[1 * 3 + 0], faceVertec[1 * 3 + 1], faceVertec[1 * 3 + 2]); dVector e0 (p1 - p0); for (int i = 2; i < vertexCount; i ++) { dVector p2 (faceVertec[i * 3 + 0], faceVertec[i * 3 + 1], faceVertec[i * 3 + 2]); dVector e1 (p2 - p0); normal += e0 * e1; e0 = e1; } normal = normal.Scale (1.0f / dSqrt (normal % normal)); dVector displacemnet (normal.Scale (OFFSET)); // add the face displace by some offset for (int i = 0; i < vertexCount; i ++) { dVector p1 (faceVertec[i * 3 + 0], faceVertec[i * 3 + 1], faceVertec[i * 3 + 2]); p1 += displacemnet; face[i][0] = p1.m_x; face[i][1] = p1.m_y; face[i][2] = p1.m_z; face[i][3] = normal.m_x; face[i][4] = normal.m_y; face[i][5] = normal.m_z; face[i][6] = 0.0f; face[i][7] = 0.0f; face[i][8] = 0.0f; face[i][9] = 0.0f; } // add the face NewtonMeshAddFace (mesh, vertexCount, &face[0][0], 10 * sizeof (dFloat), id); // now add on face walk the perimeter and add a rivet face dVector q0 (faceVertec[(vertexCount - 1) * 3 + 0], faceVertec[(vertexCount - 1) * 3 + 1], faceVertec[(vertexCount - 1) * 3 + 2]); q0 += displacemnet; for (int i = 0; i < vertexCount; i ++) { dVector q1 (faceVertec[i * 3 + 0], faceVertec[i * 3 + 1], faceVertec[i * 3 + 2]); q1 += displacemnet; // calculate the river normal dVector edge (q1 - q0); dVector n (edge * normal); n = n.Scale (1.0f / sqrtf (n % n)); // build a quad to serve a the face between the two parellel faces face[0][0] = q0.m_x; face[0][1] = q0.m_y; face[0][2] = q0.m_z; face[0][3] = n.m_x; face[0][4] = n.m_y; face[0][5] = n.m_z; face[0][6] = 0.0f; face[0][7] = 0.0f; face[0][8] = 0.0f; face[0][9] = 0.0f; face[1][0] = q1.m_x; face[1][1] = q1.m_y; face[1][2] = q1.m_z; face[1][3] = n.m_x; face[1][4] = n.m_y; face[1][5] = n.m_z; face[1][6] = 0.0f; face[1][7] = 0.0f; face[1][8] = 0.0f; face[1][9] = 0.0f; face[2][0] = q1.m_x - displacemnet.m_x; face[2][1] = q1.m_y - displacemnet.m_y; face[2][2] = q1.m_z - displacemnet.m_z; face[2][3] = n.m_x; face[2][4] = n.m_y; face[2][5] = n.m_z; face[2][6] = 0.0f; face[2][7] = 0.0f; face[2][8] = 0.0f; face[2][9] = 0.0f; face[3][0] = q0.m_x - displacemnet.m_x; face[3][1] = q0.m_y - displacemnet.m_y; face[3][2] = q0.m_z - displacemnet.m_z; face[3][3] = n.m_x; face[3][4] = n.m_y; face[3][5] = n.m_z; face[3][6] = 0.0f; face[3][7] = 0.0f; face[3][8] = 0.0f; face[3][9] = 0.0f; // save the first point for the next rivet q0 = q1; // add this face to the mesh NewtonMeshAddFace (mesh, 4, &face[0][0], 10 * sizeof (dFloat), id); } }
const numeric fsolve(const ex& f_in, const symbol& x, const numeric& x1, const numeric& x2) { if (!x1.is_real() || !x2.is_real()) { throw std::runtime_error("fsolve(): interval not bounded by real numbers"); } if (x1==x2) { throw std::runtime_error("fsolve(): vanishing interval"); } // xx[0] == left interval limit, xx[1] == right interval limit. // fx[0] == f(xx[0]), fx[1] == f(xx[1]). // We keep the root bracketed: xx[0]<xx[1] and fx[0]*fx[1]<0. numeric xx[2] = { x1<x2 ? x1 : x2, x1<x2 ? x2 : x1 }; ex f; if (is_a<relational>(f_in)) { f = f_in.lhs()-f_in.rhs(); } else { f = f_in; } const ex fx_[2] = { f.subs(x==xx[0]).evalf(), f.subs(x==xx[1]).evalf() }; if (!is_a<numeric>(fx_[0]) || !is_a<numeric>(fx_[1])) { throw std::runtime_error("fsolve(): function does not evaluate numerically"); } numeric fx[2] = { ex_to<numeric>(fx_[0]), ex_to<numeric>(fx_[1]) }; if (!fx[0].is_real() || !fx[1].is_real()) { throw std::runtime_error("fsolve(): function evaluates to complex values at interval boundaries"); } if (fx[0]*fx[1]>=0) { throw std::runtime_error("fsolve(): function does not change sign at interval boundaries"); } // The Newton-Raphson method has quadratic convergence! Simply put, it // replaces x with x-f(x)/f'(x) at each step. -f/f' is the delta: const ex ff = normal(-f/f.diff(x)); int side = 0; // Start at left interval limit. numeric xxprev; numeric fxprev; do { xxprev = xx[side]; fxprev = fx[side]; ex dx_ = ff.subs(x == xx[side]).evalf(); if (!is_a<numeric>(dx_)) throw std::runtime_error("fsolve(): function derivative does not evaluate numerically"); xx[side] += ex_to<numeric>(dx_); // Now check if Newton-Raphson method shot out of the interval bool bad_shot = (side == 0 && xx[0] < xxprev) || (side == 1 && xx[1] > xxprev) || xx[0] > xx[1]; if (!bad_shot) { // Compute f(x) only if new x is inside the interval. // The function might be difficult to compute numerically // or even ill defined outside the interval. Also it's // a small optimization. ex f_x = f.subs(x == xx[side]).evalf(); if (!is_a<numeric>(f_x)) throw std::runtime_error("fsolve(): function does not evaluate numerically"); fx[side] = ex_to<numeric>(f_x); } if (bad_shot) { // Oops, Newton-Raphson method shot out of the interval. // Restore, and try again with the other side instead! xx[side] = xxprev; fx[side] = fxprev; side = !side; xxprev = xx[side]; fxprev = fx[side]; ex dx_ = ff.subs(x == xx[side]).evalf(); if (!is_a<numeric>(dx_)) throw std::runtime_error("fsolve(): function derivative does not evaluate numerically [2]"); xx[side] += ex_to<numeric>(dx_); ex f_x = f.subs(x==xx[side]).evalf(); if (!is_a<numeric>(f_x)) throw std::runtime_error("fsolve(): function does not evaluate numerically [2]"); fx[side] = ex_to<numeric>(f_x); } if ((fx[side]<0 && fx[!side]<0) || (fx[side]>0 && fx[!side]>0)) { // Oops, the root isn't bracketed any more. // Restore, and perform a bisection! xx[side] = xxprev; fx[side] = fxprev; // Ah, the bisection! Bisections converge linearly. Unfortunately, // they occur pretty often when Newton-Raphson arrives at an x too // close to the result on one side of the interval and // f(x-f(x)/f'(x)) turns out to have the same sign as f(x) due to // precision errors! Recall that this function does not have a // precision goal as one of its arguments but instead relies on // x converging to a fixed point. We speed up the (safe but slow) // bisection method by mixing in a dash of the (unsafer but faster) // secant method: Instead of splitting the interval at the // arithmetic mean (bisection), we split it nearer to the root as // determined by the secant between the values xx[0] and xx[1]. // Don't set the secant_weight to one because that could disturb // the convergence in some corner cases! constexpr double secant_weight = 0.984375; // == 63/64 < 1 numeric xxmid = (1-secant_weight)*0.5*(xx[0]+xx[1]) + secant_weight*(xx[0]+fx[0]*(xx[0]-xx[1])/(fx[1]-fx[0])); ex fxmid_ = f.subs(x == xxmid).evalf(); if (!is_a<numeric>(fxmid_)) throw std::runtime_error("fsolve(): function does not evaluate numerically [3]"); numeric fxmid = ex_to<numeric>(fxmid_); if (fxmid.is_zero()) { // Luck strikes... return xxmid; } if ((fxmid<0 && fx[side]>0) || (fxmid>0 && fx[side]<0)) { side = !side; } xxprev = xx[side]; fxprev = fx[side]; xx[side] = xxmid; fx[side] = fxmid; } } while (xxprev!=xx[side]); return xxprev; }
void HintedHandleEstimator::estimate( const sensor_msgs::PointCloud2::ConstPtr& cloud_msg, const geometry_msgs::PointStampedConstPtr &point_msg) { boost::mutex::scoped_lock lock(mutex_); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>); pcl::PassThrough<pcl::PointXYZ> pass; int K = 1; std::vector<int> pointIdxNKNSearch(K); std::vector<float> pointNKNSquaredDistance(K); pcl::search::KdTree<pcl::PointXYZ>::Ptr kd_tree(new pcl::search::KdTree<pcl::PointXYZ>); pcl::fromROSMsg(*cloud_msg, *cloud); geometry_msgs::PointStamped transed_point; ros::Time now = ros::Time::now(); try { listener_.waitForTransform(cloud->header.frame_id, point_msg->header.frame_id, now, ros::Duration(1.0)); listener_.transformPoint(cloud->header.frame_id, now, *point_msg, point_msg->header.frame_id, transed_point); } catch(tf::TransformException ex) { JSK_ROS_ERROR("%s", ex.what()); return; } pcl::PointXYZ searchPoint; searchPoint.x = transed_point.point.x; searchPoint.y = transed_point.point.y; searchPoint.z = transed_point.point.z; //remove too far cloud pass.setInputCloud(cloud); pass.setFilterFieldName("x"); pass.setFilterLimits(searchPoint.x - 3*handle.arm_w, searchPoint.x + 3*handle.arm_w); pass.filter(*cloud); pass.setInputCloud(cloud); pass.setFilterFieldName("y"); pass.setFilterLimits(searchPoint.y - 3*handle.arm_w, searchPoint.y + 3*handle.arm_w); pass.filter(*cloud); pass.setInputCloud(cloud); pass.setFilterFieldName("z"); pass.setFilterLimits(searchPoint.z - 3*handle.arm_w, searchPoint.z + 3*handle.arm_w); pass.filter(*cloud); if(cloud->points.size() < 10){ JSK_ROS_INFO("points are too small"); return; } if(1){ //estimate_normal pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne; ne.setInputCloud(cloud); ne.setSearchMethod(kd_tree); ne.setRadiusSearch(0.02); ne.setViewPoint(0, 0, 0); ne.compute(*cloud_normals); } else{ //use normal of msg } if(! (kd_tree->nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)){ JSK_ROS_INFO("kdtree failed"); return; } float x = cloud->points[pointIdxNKNSearch[0]].x; float y = cloud->points[pointIdxNKNSearch[0]].y; float z = cloud->points[pointIdxNKNSearch[0]].z; float v_x = cloud_normals->points[pointIdxNKNSearch[0]].normal_x; float v_y = cloud_normals->points[pointIdxNKNSearch[0]].normal_y; float v_z = cloud_normals->points[pointIdxNKNSearch[0]].normal_z; double theta = acos(v_x); // use normal for estimating handle direction tf::Quaternion normal(0, v_z/NORM(0, v_y, v_z) * cos(theta/2), -v_y/NORM(0, v_y, v_z) * cos(theta/2), sin(theta/2)); tf::Quaternion final_quaternion = normal; double min_theta_index = 0; double min_width = 100; tf::Quaternion min_qua(0, 0, 0, 1); visualization_msgs::Marker debug_hand_marker; debug_hand_marker.header = cloud_msg->header; debug_hand_marker.ns = string("debug_grasp"); debug_hand_marker.id = 0; debug_hand_marker.type = visualization_msgs::Marker::LINE_LIST; debug_hand_marker.pose.orientation.w = 1; debug_hand_marker.scale.x=0.003; tf::Matrix3x3 best_mat; //search 180 degree and calc the shortest direction for(double theta_=0; theta_<3.14/2; theta_+=3.14/2/30){ tf::Quaternion rotate_(sin(theta_), 0, 0, cos(theta_)); tf::Quaternion temp_qua = normal * rotate_; tf::Matrix3x3 temp_mat(temp_qua); geometry_msgs::Pose pose_respected_to_tf; pose_respected_to_tf.position.x = x; pose_respected_to_tf.position.y = y; pose_respected_to_tf.position.z = z; pose_respected_to_tf.orientation.x = temp_qua.getX(); pose_respected_to_tf.orientation.y = temp_qua.getY(); pose_respected_to_tf.orientation.z = temp_qua.getZ(); pose_respected_to_tf.orientation.w = temp_qua.getW(); Eigen::Affine3d box_pose_respected_to_cloud_eigend; tf::poseMsgToEigen(pose_respected_to_tf, box_pose_respected_to_cloud_eigend); Eigen::Affine3d box_pose_respected_to_cloud_eigend_inversed = box_pose_respected_to_cloud_eigend.inverse(); Eigen::Matrix4f box_pose_respected_to_cloud_eigen_inversed_matrixf; Eigen::Matrix4d box_pose_respected_to_cloud_eigen_inversed_matrixd = box_pose_respected_to_cloud_eigend_inversed.matrix(); jsk_pcl_ros::convertMatrix4<Eigen::Matrix4d, Eigen::Matrix4f>( box_pose_respected_to_cloud_eigen_inversed_matrixd, box_pose_respected_to_cloud_eigen_inversed_matrixf); Eigen::Affine3f offset = Eigen::Affine3f(box_pose_respected_to_cloud_eigen_inversed_matrixf); pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::transformPointCloud(*cloud, *output_cloud, offset); pcl::PassThrough<pcl::PointXYZ> pass; pcl::PointCloud<pcl::PointXYZ>::Ptr points_z(new pcl::PointCloud<pcl::PointXYZ>), points_yz(new pcl::PointCloud<pcl::PointXYZ>), points_xyz(new pcl::PointCloud<pcl::PointXYZ>); pass.setInputCloud(output_cloud); pass.setFilterFieldName("y"); pass.setFilterLimits(-handle.arm_w*2, handle.arm_w*2); pass.filter(*points_z); pass.setInputCloud(points_z); pass.setFilterFieldName("z"); pass.setFilterLimits(-handle.finger_d, handle.finger_d); pass.filter(*points_yz); pass.setInputCloud(points_yz); pass.setFilterFieldName("x"); pass.setFilterLimits(-(handle.arm_l-handle.finger_l), handle.finger_l); pass.filter(*points_xyz); pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; for(size_t index=0; index<points_xyz->size(); index++){ points_xyz->points[index].x = points_xyz->points[index].z = 0; } if(points_xyz->points.size() == 0){JSK_ROS_INFO("points are empty");return;} kdtree.setInputCloud(points_xyz); std::vector<int> pointIdxRadiusSearch; std::vector<float> pointRadiusSquaredDistance; pcl::PointXYZ search_point_tree; search_point_tree.x=search_point_tree.y=search_point_tree.z=0; if( kdtree.radiusSearch(search_point_tree, 10, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ){ double before_w=10, temp_w; for(size_t index = 0; index < pointIdxRadiusSearch.size(); ++index){ temp_w =sqrt(pointRadiusSquaredDistance[index]); if(temp_w - before_w > handle.finger_w*2){ break; // there are small space for finger } before_w=temp_w; } if(before_w < min_width){ min_theta_index = theta_; min_width = before_w; min_qua = temp_qua; best_mat = temp_mat; } //for debug view geometry_msgs::Point temp_point; std_msgs::ColorRGBA temp_color; temp_color.r=0; temp_color.g=0; temp_color.b=1; temp_color.a=1; temp_point.x=x-temp_mat.getColumn(1)[0] * before_w; temp_point.y=y-temp_mat.getColumn(1)[1] * before_w; temp_point.z=z-temp_mat.getColumn(1)[2] * before_w; debug_hand_marker.points.push_back(temp_point); debug_hand_marker.colors.push_back(temp_color); temp_point.x+=2*temp_mat.getColumn(1)[0] * before_w; temp_point.y+=2*temp_mat.getColumn(1)[1] * before_w; temp_point.z+=2*temp_mat.getColumn(1)[2] * before_w; debug_hand_marker.points.push_back(temp_point); debug_hand_marker.colors.push_back(temp_color); } } geometry_msgs::PoseStamped handle_pose_stamped; handle_pose_stamped.header = cloud_msg->header; handle_pose_stamped.pose.position.x = x; handle_pose_stamped.pose.position.y = y; handle_pose_stamped.pose.position.z = z; handle_pose_stamped.pose.orientation.x = min_qua.getX(); handle_pose_stamped.pose.orientation.y = min_qua.getY(); handle_pose_stamped.pose.orientation.z = min_qua.getZ(); handle_pose_stamped.pose.orientation.w = min_qua.getW(); std_msgs::Float64 min_width_msg; min_width_msg.data = min_width; pub_pose_.publish(handle_pose_stamped); pub_debug_marker_.publish(debug_hand_marker); pub_debug_marker_array_.publish(make_handle_array(handle_pose_stamped, handle)); jsk_recognition_msgs::SimpleHandle simple_handle; simple_handle.header = handle_pose_stamped.header; simple_handle.pose = handle_pose_stamped.pose; simple_handle.handle_width = min_width; pub_handle_.publish(simple_handle); }
osg::Vec3dArray* BoundaryUtil::findMeshBoundary( osg::Node* node, bool geocentric ) { // the normal defines the XY plane in which to search for a boundary osg::Vec3d normal(0,0,1); if ( geocentric ) { // define the XY plane based on the normal to the center of the dataset: osg::BoundingSphere bs = node->getBound(); normal = bs.center(); normal.normalize(); } osg::ref_ptr<osg::Vec3dArray> _result = new osg::Vec3dArray(); // first build a topology graph from the node. TopologyGraph topology; // set up a quat that will rotate geometry into our XY plane if ( normal != osg::Vec3(0,0,1) ) topology._rot.makeRotate( normal, osg::Vec3d(0,0,1) ); // build the topology BuildTopologyVisitor buildTopoVisitor(topology); node->accept( buildTopoVisitor ); // starting with the minimum-Y vertex (which is guaranteed to be in the boundary) // traverse the outside of the point set. Do this by sorting all the edges by // their angle relative to the vector to the previous point. The vector with the // smallest angle represents the edge connecting the current point to the next // boundary point. Walk the edge until we return to the beginning. Index vptr = topology._minY; Index vptr_prev = topology._verts.end(); while( true ) { // store this vertex in the result set: _result->push_back( *vptr ); // pull up the next 2D vertex (XY plane): osg::Vec2d vert( vptr->x(), vptr->y() ); // construct the "base" vector that points from the current point back // to the previous point; or to -X in the initial case osg::Vec2d base; if ( vptr_prev == topology._verts.end() ) base.set( -1, 0 ); else base = osg::Vec2d( vptr_prev->x(), vptr_prev->y() ) - vert; // pull up the edge set for this vertex: IndexSet& edges = topology._edgeMap[vptr]; // find the edge with the minimun delta angle to the base vector double minAngle = DBL_MAX; Index minEdge = topology._verts.end(); for( IndexSet::iterator e = edges.begin(); e != edges.end(); ++e ) { // don't go back from whence we just came if ( *e == vptr_prev ) continue; // calculate the angle between the base vector and the current edge: osg::Vec2d edgeVert( (*e)->x(), (*e)->y() ); osg::Vec2d edge = edgeVert - vert; double baseAngle = atan2(base.y(), base.x()); double edgeAngle = atan2(edge.y(), edge.x()); double outsideAngle = baseAngle - edgeAngle; // normalize it to [0..360) if ( outsideAngle < 0.0 ) outsideAngle += 2.0*osg::PI; // see it is qualifies as the new minimum angle if ( outsideAngle < minAngle ) { minAngle = outsideAngle; minEdge = *e; } } if ( minEdge == topology._verts.end() ) { // this will probably never happen osg::notify(osg::WARN) << "Illegal state - bailing" << std::endl; return 0L; } vptr_prev = vptr; // follow the chosen edge around the outside of the geometry: vptr = minEdge; // once we make it all the way around, we're done: if ( vptr == topology._minY ) break; } // un-rotate the results from the XY plane back to their original frame: osg::Quat invRot = topology._rot.inverse(); for( osg::Vec3dArray::iterator i = _result->begin(); i != _result->end(); ++i ) { (*i) = invRot * (*i); } return _result.release(); }
void main() { bool patternfound = false; bool reset = false; bool resetAuto = false; int nbImages = 0; double moyFinale = 0; char key = 0; bool detectionMire = false; bool detectionVisage = false; int cpt = 0, moyCpt = 0, i = 0; std::cout << "initialisation de Chehra..." << std::endl; Chehra chehra; std::cout << "done" << std::endl; cv::TermCriteria termcrit(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03); cv::Size winSize(31, 31); cv::Mat cameraMatrix, distCoeffs; cv::Mat imCalib; cv::Mat imCalibColor; cv::Mat imCalibNext; cv::Mat rvecs, tvecs; cv::Mat Rc, C = cv::Mat(3, 1, CV_64F), rotVecInv; std::vector<cv::Point2f> imagePoints; std::vector<cv::Point3f> objectPoints; std::vector<cv::Point3f> cubeObjectPoints; std::vector<cv::Point3f> dessinPointsVisage; std::vector<std::vector<cv::Point2f>> chessCornersInit(2); std::vector<std::vector<cv::Point2f>> pointsVisageInit(2); std::vector<cv::Point3f> chessCorners3D; std::vector<cv::Point3f> pointsVisage3D; std::vector<cv::Point3f> visage; std::vector<double> distances; double moyDistances; // Creation des coins de la mire for(int x = 0; x < COLCHESSBOARD; x++) for(int y = 0; y < ROWCHESSBOARD; y++) chessCorners3D.push_back(cv::Point3f(x * SIZEMIRE, y * SIZEMIRE, 0.0f)); // Creation des points a projeter for(int x = 0; x < COLCHESSBOARD; x++) for(int y = 0; y < ROWCHESSBOARD; y++) objectPoints.push_back(cv::Point3f(x * SIZEMIRE, y * SIZEMIRE, 0.0f)); cv::FileStorage fs("../rsc/intrinsicMatrix.yml", cv::FileStorage::READ); fs["cameraMatrix"] >> cameraMatrix; fs["distCoeffs"] >> distCoeffs; double f = (cameraMatrix.at<double>(0, 0) + cameraMatrix.at<double>(1, 1)) / 2; // NEAR = distance focale ; si pixels carrés, fx = fy -> np //mais est généralement différent de fy donc on prend (pour l'instant) par défaut la valeur médiane double g = 2000 * f; // je sais pas pourquoi. au pif. fs.release(); cv::VideoCapture vcap(0); if(!vcap.isOpened()){ std::cout << "FAIL!" << std::endl; return; } cv::Mat *frame = new cv::Mat(cv::Mat::zeros(vcap.get(CV_CAP_PROP_FRAME_HEIGHT), vcap.get(CV_CAP_PROP_FRAME_WIDTH), CV_8UC3)); do { vcap >> *frame; }while(frame->empty()); osg::ref_ptr<osg::Image> backgroundImage = new osg::Image; backgroundImage->setImage(frame->cols, frame->rows, 3, GL_RGB, GL_BGR, GL_UNSIGNED_BYTE, (uchar*)(frame->data), osg::Image::AllocationMode::NO_DELETE, 1); // read the scene from the list of file specified commandline args. osg::ref_ptr<osg::Group> group = new osg::Group; osg::ref_ptr<osg::Geode> cam = createHUD(backgroundImage, vcap.get(CV_CAP_PROP_FRAME_WIDTH), vcap.get(CV_CAP_PROP_FRAME_HEIGHT), cameraMatrix.at<double>(0, 2), cameraMatrix.at<double>(1, 2), f); std::cout << "initialisation de l'objet 3D..." << std::endl; osg::ref_ptr<osg::Node> objet3D = osgDB::readNodeFile("../rsc/objets3D/Creature.obj"); std::cout << "done" << std::endl; osg::StateSet* obectStateset = objet3D->getOrCreateStateSet(); obectStateset->setMode(GL_DEPTH_TEST,osg::StateAttribute::OFF); osg::ref_ptr<osg::MatrixTransform> mat = new osg::MatrixTransform(); osg::ref_ptr<osg::PositionAttitudeTransform> pat = new osg::PositionAttitudeTransform(); // construct the viewer. osgViewer::CompositeViewer compositeViewer; osgViewer::View* viewer = new osgViewer::View; osgViewer::View* viewer2 = new osgViewer::View; // add the HUD subgraph. group->addChild(cam); mat->addChild(objet3D); pat->addChild(mat); group->addChild(pat); pat->setScale(osg::Vec3d(3, 3, 3)); osg::Matrixd projectionMatrix; projectionMatrix.makeFrustum( -cameraMatrix.at<double>(0, 2), vcap.get(CV_CAP_PROP_FRAME_WIDTH) - cameraMatrix.at<double>(0, 2), -cameraMatrix.at<double>(1, 2), vcap.get(CV_CAP_PROP_FRAME_HEIGHT) - cameraMatrix.at<double>(1, 2), f, g); osg::Vec3d eye(0.0f, 0.0f, 0.0f), target(0.0f, g, 0.0f), normal(0.0f, 0.0f, 1.0f); // set the scene to render viewer->setSceneData(group.get()); viewer->setUpViewInWindow(0, 0, 1920 / 2, 1080 / 2); viewer->getCamera()->setProjectionMatrix(projectionMatrix); viewer->getCamera()->setViewMatrixAsLookAt(eye, target, normal); viewer2->setSceneData(group.get()); viewer2->setUpViewInWindow(1920 / 2, 0, 1920 / 2, 1080 / 2); viewer2->getCamera()->setProjectionMatrix(projectionMatrix); osg::Vec3d eye2(4 * f, 3 * f / 2, 0.0f), target2(0.0f, f, 0.0f), normal2(0.0f, 0.0f, 1.0f); viewer2->getCamera()->setViewMatrixAsLookAt(eye2, target2, normal2); compositeViewer.addView(viewer); compositeViewer.addView(viewer2); compositeViewer.realize(); // set up windows and associated threads. do { group->removeChild(pat); patternfound = false; resetAuto = false; detectionMire = false; detectionVisage = false; imagePoints.clear(); chessCornersInit[0].clear(); chessCornersInit[1].clear(); pointsVisageInit[0].clear(); pointsVisageInit[1].clear(); pointsVisage3D.clear(); dessinPointsVisage.clear(); visage.clear(); moyDistances = 0; distances.clear(); imCalibNext.release(); std::cout << "recherche de pattern" << std::endl; time_t start = clock(); double timer = 0; do { start = clock(); vcap >> *frame; backgroundImage->dirty(); //detectionMire = detecterMire(frame, &chessCornersInit[1], &imCalibNext); detectionVisage = detecterVisage(frame, &chehra, &pointsVisageInit[1], &visage, &pointsVisage3D, &imCalibNext); cpt++; double duree = (clock() - start)/(double) CLOCKS_PER_SEC; timer += duree; if(timer >= 1){ std::cout << cpt << " fps" << std::endl; moyCpt += cpt; timer = 0; duree = 0; i++; cpt = 0; start = clock(); } compositeViewer.frame(); }while(!detectionMire && !detectionVisage && !compositeViewer.done()); if(compositeViewer.done()) break; std::cout << "pattern detectee" << std::endl << std::endl; group->addChild(pat); do { start = clock(); vcap >> *frame; cv::Mat rotVec = trackingMire(frame, &imCalibNext, &pointsVisageInit, &pointsVisage3D, &cameraMatrix, &distCoeffs, &tvecs); //cv::Mat rotVec = trackingMire(frame, &imCalibNext, &chessCornersInit, &chessCorners3D, &cameraMatrix, &distCoeffs, &tvecs); //imagePoints = dessinerPoints(frame, objectPoints, rotVec, tvecs, cameraMatrix, distCoeffs); imagePoints = dessinerPoints(frame, pointsVisage3D, rotVec, tvecs, cameraMatrix, distCoeffs); double r11 = rotVec.at<double>(0, 0); double r21 = rotVec.at<double>(1, 0); double r31 = rotVec.at<double>(2, 0); double r32 = rotVec.at<double>(2, 1); double r33 = rotVec.at<double>(2, 2); osg::Matrixd matrixR; matrixR.makeRotate( atan2(r32, r33), osg::Vec3d(1.0, 0.0, 0.0), -atan2(-r31, sqrt((r32 * r32) + (r33 * r33))), osg::Vec3d(0.0, 0.0, 1.0), atan2(r21, r11), osg::Vec3d(0.0, 1.0, 0.0)); mat->setMatrix(matrixR); pat->setPosition(osg::Vec3d(tvecs.at<double>(0, 0), tvecs.at<double>(2, 0), -tvecs.at<double>(1, 0))); //std::cout << "x = " << tvecs.at<double>(0, 0) << " - y = " << tvecs.at<double>(1, 0) << " - z = " << tvecs.at<double>(2, 0) << std::endl; // Calcul d'erreur de reprojection double moy = 0; for(int j = 0; j < pointsVisageInit[1].size() ; j++) { double d = sqrt(pow(pointsVisageInit[0][j].y - imagePoints[j].y, 2) + pow(pointsVisageInit[0][j].x - imagePoints[j].x, 2)); distances.push_back(d); moy += d; } moyDistances = moy / pointsVisageInit[1].size(); if(moyDistances > 1) // si l'ecart de reproj est trop grand, reset resetAuto = true; double duree = (clock() - start)/(double) CLOCKS_PER_SEC; std::cout << (int)(1/duree) << " fps" << std::endl; moyCpt += (int)(1/duree); duree = 0; i++; backgroundImage->dirty(); compositeViewer.frame(); }while(!compositeViewer.done() && !resetAuto); }while(!compositeViewer.done()); std::cout << std::endl << "Moyenne des fps : " << moyCpt/i << std::endl; std::system("PAUSE"); }
// Extent the velocities to "air" void FluidSolver::VelocityExtension() { int iterations = 0; float narrowbandWidth = 0; float dt = 0.7; std::set<LevelSet *>::const_iterator iter = mFluids.begin(); std::set<LevelSet *>::const_iterator iend = mFluids.end(); while (iter != iend) { iterations = std::max(iterations, (int)((*iter)->GetNarrowBandWidth()*0.5f/dt)); narrowbandWidth = std::max(narrowbandWidth, (*iter)->GetNarrowBandWidth()*(*iter)->GetDx()*0.5f); iter++; } iterations = std::min(iterations, (int)(mVoxels.GetDimX()/dt)); iterations = std::min(iterations, (int)(mVoxels.GetDimY()/dt)); iterations = std::min(iterations, (int)(mVoxels.GetDimZ()/dt)); std::cerr << "Velocity extension (" << iterations << " iterations)..." << std::endl; for (int iter = 0; iter < iterations; iter++) { Volume<Vector3<float> > velocities = mVelocityField; for (int i = 0; i < mVoxels.GetDimX(); i++) { for (int j = 0; j < mVoxels.GetDimY(); j++) { for (int k = 0; k < mVoxels.GetDimZ(); k++) { const float val = mVoxels.GetValue(i,j,k); // Extend only in air or solid (don't extend beyond narrowband for efficiency) if (IsFluid(i,j,k) || val >= narrowbandWidth) continue; Vector3<float> normal(mVoxels.GetValue(i+1,j,k) - mVoxels.GetValue(i-1,j,k), mVoxels.GetValue(i,j+1,k) - mVoxels.GetValue(i,j-1,k), mVoxels.GetValue(i,j,k+1) - mVoxels.GetValue(i,j,k-1)); if (normal.Length() > 0) normal.Normalize(); Vector3<float> v = mVelocityField.GetValue(i,j,k); Vector3<float> ip = mVelocityField.GetValue(i+1,j,k); Vector3<float> im = mVelocityField.GetValue(i-1,j,k); Vector3<float> jp = mVelocityField.GetValue(i,j+1,k); Vector3<float> jm = mVelocityField.GetValue(i,j-1,k); Vector3<float> kp = mVelocityField.GetValue(i,j,k+1); Vector3<float> km = mVelocityField.GetValue(i,j,k-1); Vector3<float> v0 = v - dt*(std::max(0.0f, normal[0])*(v - im) + std::min(0.0f, normal[0])*(ip - v) + std::max(0.0f, normal[1])*(v - jm) + std::min(0.0f, normal[1])*(jp - v) + std::max(0.0f, normal[2])*(v - km) + std::min(0.0f, normal[2])*(kp - v)); velocities.SetValue(i,j,k, v0); } } } mVelocityField = velocities; } }
// play CA2 event (us vs them game for ng no. of groups) void event_CA2(int ng) { int i, j, k, *g, *sg, preexists; double wgx, xb; // sum of groups contribution with Beta exponent g = malloc(ng*sizeof(int)); // array for group index of selected group in its polity sg = malloc(ng*sizeof(int)); // array for selected unique group indices // choose ng no. of groups randomly for(k = 0; k < ng; k++){ do{ g[k] = rnd(G); // selection from uniform distribution; // generate random no. less than total no. of groups preexists = 0; for(j = 0; j < k; j++){ // check if it already exists in the selected group array 'sg' for playing game if(g[k] == sg[j]){ preexists = 1; break; } } }while(preexists == 1); sg[k] = g[k]; } // play us vs them game B = normal(Bo, Sigma_B); // randomly choose Benefit B if(B < 0) B = 0; // check if B is less than zero update_XP_CA2(g, ng); // caculate probability of success of ng groups in array pbs for(k = 0; k < ng; k++){ updateIndividualPayoffAfterProduction(g[k], ng, P[g[k]]); // update payoff of individuals in each group and of each group #if PUNISH punish(g[k]); #endif updateGroupPayoff(g[k]); } for(wgx = 0, k = 0; k < ng; k++) wgx += pow(X[g[k]], Beta); // sum of group contribution with Beta exponent for(k = 0; k < ng; k++){ Tx = malloc( GS[g[k]]*sizeof(double [TRAITS])); Xmax = malloc(GS[g[k]]*sizeof(double)); // get state of strategies before update for(i = 0; i < GS[g[k]]; i++){ Tx[i][0] = x[g[k]][i]; Tx[i][1] = dxi[g[k]][i]; Tx[i][2] = dsi[g[k]][i]; #if Accumulatedpayoff Xmax[i] = pow( ( (1-Discount)*Api[g[k]][i] + (1.0 + B*V[g[k]][i]) )/C, I_Alpha ); // upper bound of x at each role #else Xmax[i] = pow( (1.0 + B*V[g[k]][i])/C, I_Alpha); #endif } #if PUNISH Lookup = malloc(2*(GS[g[k]]-1)*sizeof(int[3])); // allocate memory for lookup table Lidx = malloc(2*(GS[g[k]]-1)*sizeof(int)); #if FORESIGHT Strat = malloc(GS[g[k]]*sizeof(double[TRAITS])); FS = malloc(GS[g[k]]*sizeof(double[TRAITS])); Lt = malloc(2*(GS[g[k]]-1)*sizeof(int[3])); Idx = malloc(2*(GS[g[k]]-1)*sizeof(int)); #endif #endif // update strategies of individual with probability Mu for(i = 0; i < GS[g[k]]; i++){ // through every individual in group if( U01() < Mu){ // if random number is less than Mu, then update strategy xb = wgx - pow( X[g[k]], Beta ); xb = MAX(xb, 0.001); updateStrategy(g[k], i, ng, xb); // use quantal response approach to update strategy (each individual strategy is updated believing other individuals do not change) } } free(Tx); #if PUNISH free(Lookup); free(Lidx); #if FORESIGHT free(Strat); free(FS); free(Lt); free(Idx); #endif #endif } free(g); free(sg); free(Xmax); }
float Random::getRealNormal(float p_min, float p_max, Generator p_generator/*= Generator::DETERMINISTIC*/) { std::normal_distribution<float> normal(p_min, p_max); float res = normal(*getEnginebyType(p_generator)); return res; }
void Smooth::operator() (const Mesh& in, Mesh& out) const { std::unique_ptr<ProgressBar> progress; if (message.size()) progress.reset (new ProgressBar (message, 8)); out.clear(); const size_t V = in.num_vertices(); if (!V) return; if (in.num_quads()) throw Exception ("For now, mesh smoothing is only supported for triangular meshes"); const size_t T = in.num_triangles(); if (V == 3*T) throw Exception ("Cannot perform smoothing on this mesh: no triangulation information"); // Pre-compute polygon centroids and areas VertexList centroids; vector<default_type> areas; for (TriangleList::const_iterator p = in.triangles.begin(); p != in.triangles.end(); ++p) { centroids.push_back ((in.vertices[(*p)[0]] + in.vertices[(*p)[1]] + in.vertices[(*p)[2]]) * (1.0/3.0)); areas.push_back (area (in, *p)); } if (progress) ++(*progress); // Perform pre-calculation of an appropriate mesh neighbourhood for each vertex // Use knowledge of the connections between vertices provided by the triangles/quads to // perform an iterative search outward from each vertex, selecting a subset of // polygons for each vertex // Extent of window should be approximately the value of spatial_factor, though only an // approximate windowing is likely to be used (i.e. number of iterations) // // Initialisation is different to iterations: Need a single pass to find those // polygons that actually use the vertex vector< std::set<uint32_t> > vert_polys (V, std::set<uint32_t>()); // For each vertex, don't just want to store the polygons within the neighbourhood; // also want to store those that will be expanded from in the next iteration vector< vector<uint32_t> > vert_polys_to_expand (V, vector<uint32_t>()); for (uint32_t t = 0; t != T; ++t) { for (uint32_t i = 0; i != 3; ++i) { vert_polys[(in.triangles[t])[i]].insert (t); vert_polys_to_expand[(in.triangles[t])[i]].push_back (t); } } if (progress) ++(*progress); // Now, we want to expand this selection outwards for each vertex // To do this, also want to produce a list for each polygon: containing those polygons // that share a common edge (i.e. two vertices) vector< vector<uint32_t> > poly_neighbours (T, vector<uint32_t>()); for (uint32_t i = 0; i != T; ++i) { for (uint32_t j = i+1; j != T; ++j) { if (in.triangles[i].shares_edge (in.triangles[j])) { poly_neighbours[i].push_back (j); poly_neighbours[j].push_back (i); } } } if (progress) ++(*progress); // TODO Will want to develop a better heuristic for this for (size_t iter = 0; iter != 8; ++iter) { for (uint32_t v = 0; v != V; ++v) { // Find polygons at the outer edge of this expanding front, and add them to the neighbourhood for this vertex vector<uint32_t> next_front; for (vector<uint32_t>::const_iterator front = vert_polys_to_expand[v].begin(); front != vert_polys_to_expand[v].end(); ++front) { for (vector<uint32_t>::const_iterator expansion = poly_neighbours[*front].begin(); expansion != poly_neighbours[*front].end(); ++expansion) { const std::set<uint32_t>::const_iterator existing = vert_polys[v].find (*expansion); if (existing == vert_polys[v].end()) { vert_polys[v].insert (*expansion); next_front.push_back (*expansion); } } } vert_polys_to_expand[v] = std::move (next_front); } } if (progress) ++(*progress); // Need to perform a first mollification pass, where the polygon normals are // smoothed but the vertices are not perturbed // However, in order to calculate these new normals, we need to calculate new vertex positions! VertexList mollified_vertices; // Use half standard spatial factor for mollification // Denominator = 2(SF/2)^2 const default_type spatial_mollification_power_multiplier = -2.0 / Math::pow2 (spatial); // No need to normalise the Gaussian; have to explicitly normalise afterwards for (uint32_t v = 0; v != V; ++v) { Vertex new_pos (0.0, 0.0, 0.0); default_type sum_weights = 0.0; // For now, just use every polygon as part of the estimate // Eventually, restrict this to some form of mesh neighbourhood //for (size_t i = 0; i != centroids.size(); ++i) { for (std::set<uint32_t>::const_iterator it = vert_polys[v].begin(); it != vert_polys[v].end(); ++it) { const uint32_t i = *it; default_type this_weight = areas[i]; const default_type distance_sq = (centroids[i] - in.vertices[v]).squaredNorm(); this_weight *= std::exp (distance_sq * spatial_mollification_power_multiplier); const Vertex prediction = centroids[i]; new_pos += this_weight * prediction; sum_weights += this_weight; } new_pos *= (1.0 / sum_weights); mollified_vertices.push_back (new_pos); } if (progress) ++(*progress); // Have new vertices; compute polygon normals based on these vertices Mesh mollified_mesh; mollified_mesh.load (mollified_vertices, in.triangles); VertexList tangents; for (TriangleList::const_iterator p = mollified_mesh.triangles.begin(); p != mollified_mesh.triangles.end(); ++p) tangents.push_back (normal (mollified_mesh, *p)); if (progress) ++(*progress); // Now perform the actual smoothing const default_type spatial_power_multiplier = -0.5 / Math::pow2 (spatial); const default_type influence_power_multiplier = -0.5 / Math::pow2 (influence); for (size_t v = 0; v != V; ++v) { Vertex new_pos (0.0, 0.0, 0.0); default_type sum_weights = 0.0; //for (size_t i = 0; i != centroids.size(); ++i) { for (std::set<uint32_t>::const_iterator it = vert_polys[v].begin(); it != vert_polys[v].end(); ++it) { const uint32_t i = *it; default_type this_weight = areas[i]; const default_type distance_sq = (centroids[i] - in.vertices[v]).squaredNorm(); this_weight *= std::exp (distance_sq * spatial_power_multiplier); const default_type prediction_distance = (centroids[i] - in.vertices[v]).dot (tangents[i]); const Vertex prediction = in.vertices[v] + (tangents[i] * prediction_distance); this_weight *= std::exp (Math::pow2 (prediction_distance) * influence_power_multiplier); new_pos += this_weight * prediction; sum_weights += this_weight; } new_pos *= (1.0 / sum_weights); out.vertices.push_back (new_pos); } if (progress) ++(*progress); out.triangles = in.triangles; // If the vertex normals were calculated previously, re-calculate them if (in.have_normals()) out.calculate_normals(); }
VECT surface_height::height_normal_get(int x, int y, MASK_INTERFACE *p_mask, int mask_x, int mask_y) { VECT2DI list[] = { VECT2DI( 0, 1), VECT2DI( 1, 1), VECT2DI( 1, 0), VECT2DI( 1,-1), VECT2DI( 0,-1), VECT2DI(-1,-1), VECT2DI(-1, 0), VECT2DI(-1, 1), VECT2DI( 0, 1) }; VECT vects[] = { VECT3DF( 0, 0, 1), VECT3DF( 1, 0, 1), VECT3DF( 1, 0, 0), VECT3DF( 1, 0,-1), VECT3DF( 0, 0,-1), VECT3DF(-1, 0,-1), VECT3DF(-1, 0, 0), VECT3DF(-1, 0, 1), VECT3DF( 0, 0, 1) }; int points = (int)(sizeof(list)/sizeof(list[0])); float height_current = height_get(x, y); int i; for(i = 0; i < points; i++) { tpos ax = x+list[i].x; tpos ay = y+list[i].y; if(pixel_valid(ax, ay) && (!p_mask || p_mask->mask_get(mask_x+ax,mask_y+ay))) { float height = height_get(ax, ay); vects[i].y = (height - height_current)*pixel_height_range; vects[i].norm(); } else { vects[i].set(FLOAT_UNDEFINED); } } VECT last(0); for(i = 0; i < points; i++) { if(vects[i].x != FLOAT_UNDEFINED) { last = vects[i]; break; } } if(i == points) { // return some default normal return(VECT3DF(0,-1,0)); } VECT normal(0); for(i = i+1; i < points; i++) { if(vects[i].x != FLOAT_UNDEFINED) { VECT3DF tmp = vect_mult(last, vects[i]); tmp.norm(); normal += tmp; last = vects[i]; } } normal.norm(); return(normal); }
int main() { ofstream output; ifstream input_train ; ifstream input_test ; //ofstream save ; output.open("ex.txt"); input_train.open("trainSet"); //save.open("Net.ini"); YVector normal(2); YVector normal_out(1); normal = 1; normal_out = 1; // e deve avere la dimensione di una riga di Y YVector Yp(1),e(1),X(2),Y(1); double vecw; int i; RFNet prima; // rfwr('Init',ID,2,1,0,0,0,1e-7,50,[1;1],[1],'rfwr_test'); prima.Init(2,1,0,0,0,0.0000001,50,normal,normal_out,"rfwr_test"); //rfwr('Change',ID,'init_D',[25 0.01; 0.01 25]); double el[] = {25, 0.01, 0.01,25}; prima.SetInitD (2,2,el); //rfwr('Change',ID,'init_alpha',ones(2)*250); YMatrix a(2,2); a = 250 * ones(2); prima.SetInitAlpha (a); //rfwr('Change',ID,'w_gen',0.2); prima.SetWGen (0.2); //rfwr('Change',ID,'meta',1); // prima.SetMeta (1); //rfwr('Change',ID,'meta_rate',250); // prima.SetMetaRate (250); for (i = 1; i <=N*20; i++) { input_train >> X(1); input_train >> X(2); input_train >> Y(1); vecw = prima.Train (X,Y,Yp,e); // Per stampare il risulato del train // cout << vecw << endl; // output << vecw << endl; // cout << Yp << endl; // output << Yp << endl; // cout << i << endl; } input_train.close(); input_test.open("testSet"); for (i = 1; i <=1681; i++) { input_test >> X(1); input_test >> X(2); input_test >> Y(1); //[yp,w]=rfwr('Predict',ID,Xt(i,:)',0.001); prima.Simulate (X,0.001,Yp); // Per stampare il risultato del predict // cout << Yp << endl; // output << Yp << endl; outIni(Yp,output); } // prima.print(output); //**** // prima.SaveNet (save); output.close(); input_test.close(); // save.close(); return 0; }
static void makeFastFace(TileSpec tile, u16 li0, u16 li1, u16 li2, u16 li3, v3f p, v3s16 dir, v3f scale, u8 light_source, std::vector<FastFace> &dest) { // Position is at the center of the cube. v3f pos = p * BS; float x0 = 0.0; float y0 = 0.0; float w = 1.0; float h = 1.0; v3f vertex_pos[4]; v3s16 vertex_dirs[4]; getNodeVertexDirs(dir, vertex_dirs); v3s16 t; u16 t1; switch (tile.rotation) { case 0: break; case 1: //R90 t = vertex_dirs[0]; vertex_dirs[0] = vertex_dirs[3]; vertex_dirs[3] = vertex_dirs[2]; vertex_dirs[2] = vertex_dirs[1]; vertex_dirs[1] = t; t1=li0; li0=li3; li3=li2; li2=li1; li1=t1; break; case 2: //R180 t = vertex_dirs[0]; vertex_dirs[0] = vertex_dirs[2]; vertex_dirs[2] = t; t = vertex_dirs[1]; vertex_dirs[1] = vertex_dirs[3]; vertex_dirs[3] = t; t1 = li0; li0 = li2; li2 = t1; t1 = li1; li1 = li3; li3 = t1; break; case 3: //R270 t = vertex_dirs[0]; vertex_dirs[0] = vertex_dirs[1]; vertex_dirs[1] = vertex_dirs[2]; vertex_dirs[2] = vertex_dirs[3]; vertex_dirs[3] = t; t1 = li0; li0 = li1; li1 = li2; li2 = li3; li3 = t1; break; case 4: //FXR90 t = vertex_dirs[0]; vertex_dirs[0] = vertex_dirs[3]; vertex_dirs[3] = vertex_dirs[2]; vertex_dirs[2] = vertex_dirs[1]; vertex_dirs[1] = t; t1 = li0; li0 = li3; li3 = li2; li2 = li1; li1 = t1; y0 += h; h *= -1; break; case 5: //FXR270 t = vertex_dirs[0]; vertex_dirs[0] = vertex_dirs[1]; vertex_dirs[1] = vertex_dirs[2]; vertex_dirs[2] = vertex_dirs[3]; vertex_dirs[3] = t; t1 = li0; li0 = li1; li1 = li2; li2 = li3; li3 = t1; y0 += h; h *= -1; break; case 6: //FYR90 t = vertex_dirs[0]; vertex_dirs[0] = vertex_dirs[3]; vertex_dirs[3] = vertex_dirs[2]; vertex_dirs[2] = vertex_dirs[1]; vertex_dirs[1] = t; t1 = li0; li0 = li3; li3 = li2; li2 = li1; li1 = t1; x0 += w; w *= -1; break; case 7: //FYR270 t = vertex_dirs[0]; vertex_dirs[0] = vertex_dirs[1]; vertex_dirs[1] = vertex_dirs[2]; vertex_dirs[2] = vertex_dirs[3]; vertex_dirs[3] = t; t1 = li0; li0 = li1; li1 = li2; li2 = li3; li3 = t1; x0 += w; w *= -1; break; case 8: //FX y0 += h; h *= -1; break; case 9: //FY x0 += w; w *= -1; break; default: break; } for(u16 i=0; i<4; i++) { vertex_pos[i] = v3f( BS/2*vertex_dirs[i].X, BS/2*vertex_dirs[i].Y, BS/2*vertex_dirs[i].Z ); } for(u16 i=0; i<4; i++) { vertex_pos[i].X *= scale.X; vertex_pos[i].Y *= scale.Y; vertex_pos[i].Z *= scale.Z; vertex_pos[i] += pos; } f32 abs_scale = 1.0; if (scale.X < 0.999 || scale.X > 1.001) abs_scale = scale.X; else if(scale.Y < 0.999 || scale.Y > 1.001) abs_scale = scale.Y; else if(scale.Z < 0.999 || scale.Z > 1.001) abs_scale = scale.Z; v3f normal(dir.X, dir.Y, dir.Z); u8 alpha = tile.alpha; dest.push_back(FastFace()); FastFace& face = *dest.rbegin(); face.vertices[0] = video::S3DVertex(vertex_pos[0], normal, MapBlock_LightColor(alpha, li0, light_source), core::vector2d<f32>(x0+w*abs_scale, y0+h)); face.vertices[1] = video::S3DVertex(vertex_pos[1], normal, MapBlock_LightColor(alpha, li1, light_source), core::vector2d<f32>(x0, y0+h)); face.vertices[2] = video::S3DVertex(vertex_pos[2], normal, MapBlock_LightColor(alpha, li2, light_source), core::vector2d<f32>(x0, y0)); face.vertices[3] = video::S3DVertex(vertex_pos[3], normal, MapBlock_LightColor(alpha, li3, light_source), core::vector2d<f32>(x0+w*abs_scale, y0)); face.tile = tile; }
void YPlus::execute() { Mesh& mesh = this->mesh(); // Geometry data const Field& coords = mesh.geometry_fields().coordinates(); const Uint nb_nodes = coords.size(); const Uint dim = coords.row_size(); // Velocity data const Field& velocity_field = common::find_component_recursively_with_tag<Field>(mesh, options().value<std::string>("velocity_tag")); const auto velocity_dict_handle = Handle<Dictionary const>(velocity_field.parent()); cf3_assert(velocity_dict_handle != nullptr); const Dictionary& velocity_dict = *velocity_dict_handle; const Uint vel_offset = velocity_field.descriptor().offset("Velocity"); // initialize if needed auto volume_node_connectivity = Handle<NodeConnectivity>(mesh.get_child("volume_node_connectivity")); if(m_normals.empty()) { // Node-to-element connectivity for the volume elements volume_node_connectivity = mesh.create_component<NodeConnectivity>("volume_node_connectivity"); std::vector< Handle<Entities const> > volume_entities; for(const mesh::Elements& elements : common::find_components_recursively_with_filter<mesh::Elements>(mesh, IsElementsVolume())) { volume_entities.push_back(elements.handle<Entities const>()); } volume_node_connectivity->initialize(nb_nodes, volume_entities); mesh.geometry_fields().create_field("wall_velocity_gradient_nodal").add_tag("wall_velocity_gradient_nodal"); Dictionary& wall_P0 = *mesh.create_component<DiscontinuousDictionary>("wall_P0"); m_normals.clear(); for(const Handle<Region>& region : regions()) { for(mesh::Elements& wall_entity : common::find_components_recursively_with_filter<mesh::Elements>(*region, IsElementsSurface())) { const Uint nb_elems = wall_entity.size(); const auto& geom_conn = wall_entity.geometry_space().connectivity(); const ElementType& etype = wall_entity.element_type(); const Uint element_nb_nodes = etype.nb_nodes(); m_normals.push_back(std::vector<RealVector>(nb_elems, RealVector(dim))); RealMatrix elem_coords(element_nb_nodes, dim); for(Uint elem_idx = 0; elem_idx != nb_elems; ++elem_idx) { const Connectivity::ConstRow conn_row = geom_conn[elem_idx]; fill(elem_coords, coords, conn_row); RealVector normal(dim); etype.compute_normal(elem_coords, m_normals.back()[elem_idx]); m_normals.back()[elem_idx] /= m_normals.back()[elem_idx].norm(); } wall_entity.create_component<FaceConnectivity>("wall_face_connectivity")->initialize(*volume_node_connectivity); wall_entity.create_space("cf3.mesh.LagrangeP0."+wall_entity.element_type().shape_name(),wall_P0); } } wall_P0.build(); // to tell the dictionary that all spaces have been added mesh.update_structures(); // to tell the mesh there is a new dictionary added manually wall_P0.create_field("wall_velocity_gradient").add_tag("wall_velocity_gradient"); } // Create the y+ field in the geometry dictionary if(common::find_component_ptr_with_tag(mesh.geometry_fields(), "yplus") == nullptr) { mesh.geometry_fields().create_field("yplus").add_tag("yplus"); } // Compute shear stress Uint surface_idx = 0; Dictionary& wall_P0 = *Handle<mesh::Dictionary>(mesh.get_child_checked("wall_P0")); Field& wall_velocity_gradient_field = *Handle<Field>(wall_P0.get_child_checked("wall_velocity_gradient")); for(const Handle<Region>& region : regions()) { for(const mesh::Elements& elements : common::find_components_recursively_with_filter<mesh::Elements>(*region, IsElementsSurface())) { const Uint nb_elements = elements.geometry_space().connectivity().size(); cf3_assert(elements.element_type().nb_faces() == 1); const auto& face_connectivity = *Handle<FaceConnectivity const>(elements.get_child_checked("wall_face_connectivity")); const auto& wall_conn = elements.space(wall_P0).connectivity(); for(Uint surface_elm_idx = 0; surface_elm_idx != nb_elements; ++surface_elm_idx) { if(face_connectivity.has_adjacent_element(surface_elm_idx, 0)) { const Uint wall_field_idx = wall_conn[surface_elm_idx][0]; // Get the wall normal vector const RealVector& normal = m_normals[surface_idx][surface_elm_idx]; RealVector3 normal3; normal3[0] = normal[0]; normal3[1] = normal[1]; normal3[2] = dim == 3 ? normal[2] : 0.; // The connected volume element NodeConnectivity::ElementReferenceT connected = face_connectivity.adjacent_element(surface_elm_idx, 0); const mesh::Entities& volume_entities = *face_connectivity.node_connectivity().entities()[connected.first]; const Uint volume_elem_idx = connected.second; const auto& velocity_conn = volume_entities.space(velocity_dict).connectivity(); const auto& velocity_sf = volume_entities.space(velocity_dict).shape_function(); const auto& geom_conn = volume_entities.geometry_space().connectivity(); const ElementType& volume_etype = volume_entities.element_type(); const Uint nb_vel_nodes = velocity_sf.nb_nodes(); const RealVector centroid_mapped_coord = 0.5*(velocity_sf.local_coordinates().colwise().minCoeff() + velocity_sf.local_coordinates().colwise().maxCoeff()); RealMatrix elem_coords(geom_conn.row_size(), dim); fill(elem_coords, coords, geom_conn[volume_elem_idx]); RealVector tangential_velocity(nb_vel_nodes); // For every node, the component of the velocity tangential to the wall RealVector3 v3; for(Uint i = 0; i != nb_vel_nodes; ++i) { Eigen::Map<RealVector const> v(&velocity_field[velocity_conn[volume_elem_idx][i]][vel_offset], dim); v3[0] = v[0]; v3[1] = v[1]; v3[2] = dim == 3 ? v[2] : 0.; tangential_velocity[i] = v3.cross(normal3).norm(); } wall_velocity_gradient_field[wall_field_idx][0] = fabs((volume_etype.jacobian(centroid_mapped_coord, elem_coords).inverse() * velocity_sf.gradient(centroid_mapped_coord) * tangential_velocity).dot(-normal)); } } ++surface_idx; } } wall_velocity_gradient_field.synchronize(); // Compute a nodal version of the wall velocity gradient const auto& wall_node_connectivity = *Handle<NodeConnectivity>(mesh.get_child_checked("wall_node_connectivity")); Field& wall_velocity_gradient_field_nodal = *Handle<Field>(mesh.geometry_fields().get_child_checked("wall_velocity_gradient_nodal")); for(Uint node_idx = 0; node_idx != nb_nodes; ++node_idx) { Uint nb_connected_elems = 0; wall_velocity_gradient_field_nodal[node_idx][0] = 0; for(const NodeConnectivity::ElementReferenceT elref : wall_node_connectivity.node_element_range(node_idx)) { const Uint wall_field_idx = wall_node_connectivity.entities()[elref.first]->space(wall_P0).connectivity()[elref.second][0]; wall_velocity_gradient_field_nodal[node_idx][0] += wall_velocity_gradient_field[wall_field_idx][0]; ++nb_connected_elems; } if(nb_connected_elems != 0) { wall_velocity_gradient_field_nodal[node_idx][0] /= static_cast<Real>(nb_connected_elems); } } // Set Yplus Field& yplus_field = *Handle<Field>(mesh.geometry_fields().get_child_checked("yplus")); const Field& wall_distance_field = *Handle<Field>(mesh.geometry_fields().get_child_checked("wall_distance")); const auto& node_to_wall_element = *Handle<common::Table<Uint>>(mesh.get_child_checked("node_to_wall_element")); const Real nu = physical_model().options().value<Real>("kinematic_viscosity"); for(Uint node_idx = 0; node_idx != nb_nodes; ++node_idx) { yplus_field[node_idx][0] = 0; if(node_to_wall_element[node_idx][0] != 0) { const Entities& wall_entities = *wall_node_connectivity.entities()[node_to_wall_element[node_idx][1]]; const Uint wall_field_idx = wall_entities.space(wall_P0).connectivity()[node_to_wall_element[node_idx][2]][0]; yplus_field[node_idx][0] = wall_distance_field[node_idx][0] * sqrt(nu*wall_velocity_gradient_field[wall_field_idx][0]) / nu; } else { yplus_field[node_idx][0] = wall_distance_field[node_idx][0] * sqrt(nu*wall_velocity_gradient_field_nodal[node_to_wall_element[node_idx][1]][0]) / nu; } } }
void GenericContactProcess (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex) { #if 0 dFloat speed0; dFloat speed1; SpecialEffectStruct* currectEffect; // get the pointer to the special effect structure currectEffect = (SpecialEffectStruct *)NewtonMaterialGetMaterialPairUserData (material); // save the contact information NewtonMaterialGetContactPositionAndNormal (material, &currectEffect->m_position.m_x, &currectEffect->m_normal.m_x); NewtonMaterialGetContactTangentDirections (material, &currectEffect->m_tangentDir0.m_x, &currectEffect->m_tangentDir1.m_x); // Get the maximum normal speed of this impact. this can be used for positioning collision sound speed0 = NewtonMaterialGetContactNormalSpeed (material); if (speed0 > currectEffect->m_contactMaxNormalSpeed) { // save the position of the contact (for 3d sound of particles effects) currectEffect->m_contactMaxNormalSpeed = speed0; } // get the maximum of the two sliding contact speed speed0 = NewtonMaterialGetContactTangentSpeed (material, 0); speed1 = NewtonMaterialGetContactTangentSpeed (material, 1); if (speed1 > speed0) { speed0 = speed1; } // Get the maximum tangent speed of this contact. this can be used for particles(sparks) of playing scratch sounds if (speed0 > currectEffect->m_contactMaxTangentSpeed) { // save the position of the contact (for 3d sound of particles effects) currectEffect->m_contactMaxTangentSpeed = speed0; } #endif // read the table direction // dVector dir (tableDir); // dVector updir (TableDir); // NewtonBody* const body = NewtonJointGetBody0(contactJoint); // for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) { // dFloat speed; // dVector point; // dVector normal; // dVector dir0; // dVector dir1; // dVector force; // NewtonMaterial* material; // // material = NewtonContactGetMaterial (contact); // NewtonMaterialGetContactPositionAndNormal (material, body, &point.m_x, &normal.m_x); // // // if the normal is vertical is large the say 40 degrees // if (fabsf (normal % upDir) > 0.7f) { // // rotate the normal to be aligned with the table direction // NewtonMaterialContactRotateTangentDirections (material, dir); // } // } NewtonBody* const body = NewtonJointGetBody0(contactJoint); for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) { dVector point(0.0f); dVector normal(0.0f); dVector dir0(0.0f); dVector dir1(0.0f); dVector force(0.0f); NewtonMaterial* const material = NewtonContactGetMaterial (contact); NewtonMaterialGetContactForce (material, body, &force.m_x); NewtonMaterialGetContactPositionAndNormal (material, body, &point.m_x, &normal.m_x); NewtonMaterialGetContactTangentDirections (material, body, &dir0.m_x, &dir1.m_x); //dFloat speed = NewtonMaterialGetContactNormalSpeed(material); //speed = NewtonMaterialGetContactNormalSpeed(material); // play sound base of the contact speed. // } }
std::unique_ptr<Occluder> Occluder::bake(const std::vector<__m128>& vertices, __m128 refMin, __m128 refMax) { assert(vertices.size() % 16 == 0); // Simple k-means clustering by normal direction to improve backface culling efficiency std::vector<__m128> quadNormals; for (auto i = 0; i < vertices.size(); i += 4) { auto v0 = vertices[i + 0]; auto v1 = vertices[i + 1]; auto v2 = vertices[i + 2]; auto v3 = vertices[i + 3]; quadNormals.push_back(normalize(_mm_add_ps(normal(v0, v1, v2), normal(v0, v2, v3)))); } std::vector<__m128> centroids; std::vector<uint32_t> centroidAssignment; centroids.push_back(_mm_setr_ps(+1.0f, 0.0f, 0.0f, 0.0f)); centroids.push_back(_mm_setr_ps(0.0f, +1.0f, 0.0f, 0.0f)); centroids.push_back(_mm_setr_ps(0.0f, 0.0f, +1.0f, 0.0f)); centroids.push_back(_mm_setr_ps(0.0f, -1.0f, 0.0f, 0.0f)); centroids.push_back(_mm_setr_ps(0.0f, 0.0f, -1.0f, 0.0f)); centroids.push_back(_mm_setr_ps(-1.0f, 0.0f, 0.0f, 0.0f)); centroidAssignment.resize(vertices.size() / 4); bool anyChanged = true; for (int iter = 0; iter < 10 && anyChanged; ++iter) { anyChanged = false; for (auto j = 0; j < quadNormals.size(); ++j) { __m128 normal = quadNormals[j]; __m128 bestDistance = _mm_set1_ps(-std::numeric_limits<float>::infinity()); int bestCentroid = -1; for (int k = 0; k < centroids.size(); ++k) { __m128 distance = _mm_dp_ps(centroids[k], normal, 0x7F); if (_mm_comige_ss(distance, bestDistance)) { bestDistance = distance; bestCentroid = k; } } if (centroidAssignment[j] != bestCentroid) { centroidAssignment[j] = bestCentroid; anyChanged = true; } } for (int k = 0; k < centroids.size(); ++k) { centroids[k] = _mm_setzero_ps(); } for (int j = 0; j < quadNormals.size(); ++j) { int k = centroidAssignment[j]; centroids[k] = _mm_add_ps(centroids[k], quadNormals[j]); } for (int k = 0; k < centroids.size(); ++k) { centroids[k] = normalize(centroids[k]); } } std::vector<__m128> orderedVertices; for (int k = 0; k < centroids.size(); ++k) { for (int j = 0; j < vertices.size() / 4; ++j) { if (centroidAssignment[j] == k) { orderedVertices.push_back(vertices[4 * j + 0]); orderedVertices.push_back(vertices[4 * j + 1]); orderedVertices.push_back(vertices[4 * j + 2]); orderedVertices.push_back(vertices[4 * j + 3]); } } } auto occluder = std::make_unique<Occluder>(); __m128 invExtents = _mm_div_ps(_mm_set1_ps(1.0f), _mm_sub_ps(refMax, refMin)); __m128 scalingX = _mm_set1_ps(2047.0f); __m128 scalingY = _mm_set1_ps(2047.0f); __m128 scalingZ = _mm_set1_ps(1023.0f); __m128 half = _mm_set1_ps(0.5f); for (size_t i = 0; i < orderedVertices.size(); i += 16) { for (auto j = 0; j < 4; ++j) { // Transform into [0,1] space relative to bounding box __m128 v0 = _mm_mul_ps(_mm_sub_ps(orderedVertices[i + j + 0], refMin), invExtents); __m128 v1 = _mm_mul_ps(_mm_sub_ps(orderedVertices[i + j + 4], refMin), invExtents); __m128 v2 = _mm_mul_ps(_mm_sub_ps(orderedVertices[i + j + 8], refMin), invExtents); __m128 v3 = _mm_mul_ps(_mm_sub_ps(orderedVertices[i + j + 12], refMin), invExtents); // Transpose into [xxxx][yyyy][zzzz][wwww] _MM_TRANSPOSE4_PS(v0, v1, v2, v3); // Scale and truncate to int v0 = _mm_fmadd_ps(v0, scalingX, half); v1 = _mm_fmadd_ps(v1, scalingY, half); v2 = _mm_fmadd_ps(v2, scalingZ, half); __m128i X = _mm_cvttps_epi32(v0); __m128i Y = _mm_cvttps_epi32(v1); __m128i Z = _mm_cvttps_epi32(v2); // Pack to 11/11/10 format __m128i XYZ = _mm_or_si128(_mm_slli_epi32(X, 21), _mm_or_si128(_mm_slli_epi32(Y, 10), Z)); occluder->m_vertexData.push_back(XYZ); } } occluder->m_refMin = refMin; occluder->m_refMax = refMax; __m128 min = _mm_set1_ps(+std::numeric_limits<float>::infinity()); __m128 max = _mm_set1_ps(-std::numeric_limits<float>::infinity()); for (size_t i = 0; i < orderedVertices.size(); ++i) { min = _mm_min_ps(vertices[i], min); max = _mm_max_ps(vertices[i], max); } // Set W = 1 - this is expected by frustum culling code min = _mm_blend_ps(min, _mm_set1_ps(1.0f), 0b1000); max = _mm_blend_ps(max, _mm_set1_ps(1.0f), 0b1000); occluder->m_boundsMin = min; occluder->m_boundsMax = max; occluder->m_center = _mm_mul_ps(_mm_add_ps(max, min), _mm_set1_ps(0.5f)); return occluder; }
void Game::paintEvent(QPaintEvent *event) { if(gameStarted) { ///*** Setup Painter ***/// QPainter painter(this); QBrush normal(painter.brush()); QBrush manaBar(Qt::blue,Qt::SolidPattern); QBrush healthBar(Qt::green,Qt::SolidPattern); ///*** Prevent painter from drawing if the game is over ***/// if (gameOver) { painter.end(); QPainter painter(this); QFont font("Courier", 15, QFont::DemiBold); QFontMetrics fm(font); int textWidth = fm.width("Game Over!"); int textWidth2 = fm.width("Press ESC to close window"); painter.setFont(font); painter.translate(QPoint(400, 300)); painter.drawText(-textWidth/2, 0, "Game Over!"); painter.drawText(-textWidth2/2, 15, "Press ESC to close window"); killTimer(timerId); std::cout << "BLAHBLAHBLAH\n"; } ///*** Draw Player ***/// if(player->getType() == TANK) { if(player->getCanAttack()) painter.drawImage(player->getLocation().x()-9,player->getLocation().y()-5,player->getAttackFrame()); if(player->getAttacking()) painter.drawImage(player->getLocation().x()-10,player->getLocation().y()-10,player->getManaAttackFrame()); } else if(player->getType() == ASSN) { if(player->getCanAttack()) { painter.drawImage(player->getLocation().x()-20,player->getLocation().y()-17,player->getAttackFrame()); } if(player->bombSet) painter.drawImage(player->bomb->getLocation().x()-20,player->bomb->getLocation().y()-17,player->getManaAttackFrame()); } ///*** Draw Enemies ***/// for(int i=0;i<drones.size();i++) { painter.setBrush(healthBar); painter.drawRect(drones.at(i)->getLocation().x()-15,drones.at(i)->getLocation().y()-20,drones.at(i)->getHealth(), 4); painter.setBrush(normal); painter.drawImage(drones.at(i)->getLocation().x()-15,drones.at(i)->getLocation().y()-15,drones.at(i)->getAttackFrame()); } if(canSpawnArcher) { for(int i=0;i<archers.size();i++) { painter.setBrush(normal); painter.drawImage(archers.at(i)->getLocation().x()-15,archers.at(i)->getLocation().y()-15,archers.at(i)->getAttackFrame()); if(archers.at(i)->getAttacking()) { painter.setBrush(manaBar); painter.drawEllipse(archers.at(i)->bullet->getLocation().x(),archers.at(i)->bullet->getLocation().y(),10,10); } } } ///*** Draw Status Bars ***/// painter.setBrush(healthBar); painter.drawRect(5,5,player->getHealth(),10); painter.setBrush(normal); painter.drawRect(5,5,player->getMaxHealth(),10); painter.setBrush(manaBar); painter.drawRect(5,20,player->getMana(),10); painter.setBrush(normal); painter.drawRect(5,20,player->getMaxMana(),10); ///*** Give player tips at appropriate levels ***/// if (player->getXP() < 1) { if(player->getType() == TANK) { QFont font("Courier", 15, QFont::DemiBold); QFontMetrics fm(font); int textWidth = fm.width("Left Click to 'Enrage', then run into an enemy!"); painter.setFont(font); painter.translate(QPoint(400, 300)); painter.drawText(-textWidth/2, 0, "Left Click to 'Enrage', then run into an enemy!"); } else if(player->getType() == ASSN) { QFont font("Courier", 15, QFont::DemiBold); QFontMetrics fm(font); int textWidth = fm.width("Left Click to Slash enemies, but don't get too close!"); painter.setFont(font); painter.translate(QPoint(400, 300)); painter.drawText(-textWidth/2, 0, "Left Click to Slash enemies, but don't get too close!"); } } if (player->getXP() == 5) { QFont font("Courier", 15, QFont::DemiBold); QFontMetrics fm(font); int textWidth = fm.width("Level Up!\nRight Click for New Skill"); painter.setFont(font); painter.translate(QPoint(400, 300)); painter.drawText(-textWidth/2, 0, "Level Up! Right Click for New Skill"); level=2; emit levelChanged(level); canSpawnArcher = true; } if (player->getXP() == 10) { QFont font("Courier", 15, QFont::DemiBold); QFontMetrics fm(font); int textWidth = fm.width("Level Up!"); painter.setFont(font); painter.translate(QPoint(400, 300)); painter.drawText(-textWidth/2, 0, "Level Up!"); canSpawnArcher = true; level=3; emit levelChanged(level); } } }
point boundaryLayers::createNewVertex ( const label bpI, const boolList& treatPatches, const List<direction>& patchVertex ) const { const meshSurfaceEngine& mse = surfaceEngine(); const labelList& bPoints = mse.boundaryPoints(); const faceList::subList& bFaces = mse.boundaryFaces(); const vectorField& pNormals = mse.pointNormals(); const VRWGraph& pFaces = mse.pointFaces(); const labelList& boundaryFacePatches = mse.boundaryFacePatches(); const VRWGraph& pointPoints = mse.pointPoints(); const meshSurfacePartitioner& mPart = surfacePartitioner(); const VRWGraph& pPatches = mPart.pointPatches(); const pointFieldPMG& points = mesh_.points(); # ifdef DEBUGLayer Info << "Creating new vertex for boundary vertex " << bpI << endl; Info << "Global vertex label " << bPoints[bpI] << endl; # endif vector normal(vector::zero); scalar dist(VGREAT); const point& p = points[bPoints[bpI]]; if( patchVertex[bpI] & EDGENODE ) { # ifdef DEBUGLayer Info << "Vertex is on the border" << endl; # endif DynList<label> otherPatches; forAllRow(pPatches, bpI, patchI) if( !treatPatches[pPatches(bpI, patchI)] ) otherPatches.appendIfNotIn ( pPatches(bpI, patchI) ); if( otherPatches.size() == 1 ) { //- vertex is on an edge # ifdef DEBUGLayer Info << "Vertex is on an edge" << endl; # endif vector v(vector::zero); forAllRow(pFaces, bpI, pfI) { const face& f = bFaces[pFaces(bpI, pfI)]; const label patchLabel = boundaryFacePatches[pFaces(bpI, pfI)]; if( treatPatches[patchLabel] ) { normal += f.normal(points); } else { v += f.normal(points); } } const scalar magV = mag(v) + VSMALL; v /= magV; normal -= (normal & v) * v; const scalar magN = mag(normal) + VSMALL; normal /= magN; forAllRow(pointPoints, bpI, ppI) { if( patchVertex[pointPoints(bpI, ppI)] ) continue; const vector vec = points[bPoints[pointPoints(bpI, ppI)]] - p; const scalar prod = 0.5 * mag(vec & normal); if( prod < dist ) dist = prod; } }
bool UnitSphere::intersect( Ray3D& ray, const Matrix4x4& worldToModel, const Matrix4x4& modelToWorld ) { // DONE: implement intersection code for UnitSphere, which is centred // on the origin. // // Your goal here is to fill ray.intersection with correct values // should an intersection occur. This includes intersection.point, // intersection.normal, intersection.none, intersection.t_value. // // HINT: Remember to first transform the ray into object space // to simplify the intersection test. // transform the ray into object space Ray3D obj_ray; obj_ray.origin = worldToModel * ray.origin; obj_ray.dir = worldToModel * ray.dir; // referred to the solution at: // http://www.scratchapixel.com/lessons/3d-basic-rendering/minimal-ray-tracer-rendering-simple-shapes/ray-sphere-intersection // analytic solution Vector3D L = obj_ray.origin - Point3D(0, 0, 0); double a = obj_ray.dir.dot(obj_ray.dir); double b = 2 * obj_ray.dir.dot(L); double c = L.dot(L) - 1.0; double discriminant = b * b - 4 * a * c; double x, y, z, t_value, t_value1, t_value2; // no solution if(discriminant < 0) { return false; } // 1 solution if(discriminant == 0) { t_value = - 0.5 * b / a; } // 2 solutions if(discriminant > 0) { float q = (b > 0) ? -0.5 * (b + sqrt(discriminant)) : -0.5 * (b - sqrt(discriminant)); t_value1 = q / a; t_value2 = c / q; } if (t_value1 < t_value2){ t_value = t_value1; } else{ t_value = t_value2; } if(ray.intersection.none || t_value < ray.intersection.t_value) { // 1. x componet of the intersection double x = obj_ray.origin[0] + t_value * obj_ray.dir[0]; // 2. y componet of the intersection double y = obj_ray.origin[1] + t_value * obj_ray.dir[1]; // 3. z componet of the intersection double z = obj_ray.origin[2] + t_value * obj_ray.dir[2]; Point3D intersection = modelToWorld * Point3D(x, y, z); Vector3D normal(x, y, z); normal = transNorm(worldToModel, normal); normal.normalize(); ray.intersection.point = intersection; ray.intersection.normal = normal; ray.intersection.mat = ray.intersection.mat; ray.intersection.t_value = t_value; ray.intersection.none = false; return true; } }
void PhysicalWorld::onGeometryCollision(void* i_world, dGeomID i_geometry1, dGeomID i_geometry2) { // due to organisation i_geometry1 and i_geometry2 are not spaces. InternalMessage("Physic","PhysicalWorld::onGeometryCollision entering") ; if (! Collideable::canCollide(i_geometry1,i_geometry2)) return ; // i_world is in fact a world. PhysicalWorld* world = static_cast<PhysicalWorld*>(i_world) ; if (!world) { return ; } // retreive usefull objects Collideable* collideable1 = static_cast<Collideable*>(dGeomGetData(i_geometry1)) ; Collideable* collideable2 = static_cast<Collideable*>(dGeomGetData(i_geometry2)) ; PhysicalObject* object1 = collideable1 ? collideable1->getControler()->getControler<PhysicalObject>() : NULL ; PhysicalObject* object2 = collideable2 ? collideable2->getControler()->getControler<PhysicalObject>() : NULL ; InternalMessage("Physic","PhysicalWorld::onGeometryCollision " + (object1 ? Kernel::toString(object1->getObject()->getIdentifier()) : "no object1") + " " + (object2 ? Kernel::toString(object2->getObject()->getIdentifier()) : "no object2") + (collideable1->isCollideableWith(collideable2) ? " collideable" : "not collideable") ) ; if (object1 && object2) { // object positions Ogre::Vector3 object1_position ; Ogre::Vector3 object2_position ; const dReal* temp_position ; temp_position = dBodyGetPosition(object1->getBody()->id()) ; object1_position.x = temp_position[0] ; object1_position.y = temp_position[1] ; object1_position.z = temp_position[2] ; temp_position = dBodyGetPosition(object2->getBody()->id()) ; object2_position.x = temp_position[0] ; object2_position.y = temp_position[1] ; object2_position.z = temp_position[2] ; InternalMessage("Physic","PhysicalWorld::onGeometryCollision object positions " + Ogre::StringConverter::toString(object1_position) + ";" + Ogre::StringConverter::toString(object2_position)) ; // calculate contact points int number_of_contacts = dCollide(i_geometry1, i_geometry2, maximum_contact_points, contact_points, sizeof(dContactGeom)) ; InformationMessage("Physic","number of contact points = " + Kernel::toString(number_of_contacts)) ; Ogre::Vector3 result(0,0,0) ; int real_number_of_contact_points = 0 ; Ogre::Vector3 average_contact_point(0,0,0) ; for (int contact_index = 0 ; contact_index < number_of_contacts ; ++contact_index) { // create contact joint dContact contact ; contact.surface.mode = dContactSoftCFM|dContactSoftERP|dContactBounce ; contact.surface.mu = Kernel::Parameters::getValue<float>("Physic","ContactMu") ; contact.surface.mu2 = 0 ; contact.surface.bounce = Kernel::Parameters::getValue<float>("Physic","ContactBounce") ; contact.surface.bounce_vel = Kernel::Parameters::getValue<float>("Physic","ContactBounceVelocity") ; contact.surface.soft_erp = 1 ; contact.surface.soft_cfm = 1 ; contact.surface.motion1 = 0 ; contact.surface.motion2 = 0 ; contact.surface.slip1 = 0 ; contact.surface.slip2 = 0 ; contact.geom = contact_points[contact_index] ; contact.fdir1[0] = 0 ; contact.fdir1[1] = 0 ; contact.fdir1[2] = 0 ; contact.fdir1[3] = 0 ; Ogre::Vector3 point(contact_points[contact_index].pos[0], contact_points[contact_index].pos[1], contact_points[contact_index].pos[2]) ; Ogre::Vector3 v1 = point - object1_position ; Ogre::Vector3 normal(contact_points[contact_index].normal[0], contact_points[contact_index].normal[1], contact_points[contact_index].normal[2]) ; float dot1 = v1.dotProduct(normal) ; if (dot1 < 0) { average_contact_point += point ; dJointID joint_id = dJointCreateContact(world->m_world->id(), world->m_contact_group, &contact) ; dJointAttach(joint_id, object1->getBody()->id(), object2->getBody()->id()) ; ++real_number_of_contact_points ; } } if (real_number_of_contact_points != 0) { average_contact_point = average_contact_point / real_number_of_contact_points ; // create a collision object Kernel::Object* collision_object = world->getObject()->createObject() ; collision_object->addTrait(new Model::Collision( collideable1->getControler()->getObject(), collideable2->getControler()->getObject())) ; collision_object->addTrait(new Model::Positionned( Model::Position::Meter( average_contact_point.x, average_contact_point.y, average_contact_point.z))) ; } } }
//creates a Mesh with the appropriate values and returns its pointer //calculates the rotation Mesh* Surfrev::createMesh() { Mesh* m = new Mesh(); //empty mesh float degrees = 360/slices; float angle = 0; vec3 Y(0,1,0); //inserts all points doing the rotation around the y-axis for(int i=0; i<slices; i++) { for(int j=0; j<points; j++) { vec3 v = rotation3D(Y,angle)*polyline[j]; //cout<<"X = "<<v[0]<<" Y = "<<v[1] <<" Z = "<< v[2]<<endl; m->addVertex(v); } angle += degrees; } for(int i=0; i<(slices*points)-1; i++) { //finds normal for face that is going to be inserted to the mesh if(i<((slices*points)-(points+1))) { int mod = (1+i)%points; if(mod != 0) { /*cout<<"--------\nPoint: "<<i<<endl; cout<<"i+1: "<<i+1<<endl; cout<<"i+1+points: "<<i+1+points<<endl; cout<<"i+points: "<<i+points<<"\n"<<endl; cout<<(mod)<<" = Mod\n\n";*/ } vec4 vert1(m->getVertex(i)[0],m->getVertex(i)[1],m->getVertex(i)[2],1); vec4 vert2(m->getVertex(i+1)[0],m->getVertex(i+1)[1],m->getVertex(i+1)[2],1); vec4 vert3(m->getVertex(i+points+1)[0],m->getVertex(i+points+1)[1],m->getVertex(i+points+1)[2],1); //check for final point that has x = 0: triangle not square if(((i+2)%points) == 0 && m->getVertex(i+1)[0] == 0) { vert3[0] = m->getVertex(i+points)[0]; vert3[1] = m->getVertex(i+points)[1]; vert3[2] = m->getVertex(i+points)[2]; } vec4 n = getNormal(vert1,vert2,vert3); vec3 normal(n[0],n[1],n[2]); if(mod != 0) { /*std::cout<<"Normal x = "<<normal[0]<<std::endl; std::cout<<"Normal y = "<<normal[1]<<std::endl; std::cout<<"Normal z = "<<normal[2]<<std::endl;*/ } m->addNormal(normal); ////creates the faces, normal index is = i, always have 4 vertices for a face vec2 f1((double)i,(double)i); vec2 f2((double)i+1,(double)i); vec2 f3((double)points+i+1,(double)i); vec2 f4((double)points+i,(double)i); std::vector<vec2> face; face.push_back(f2); face.push_back(f1); face.push_back(f4); face.push_back(f3); if(mod != 0) { //cout<<"add face: "<<i<<endl; m->addFace(face); } } //case: do last face else { int mod = (1+i)%points; int k = (points*slices); /*cout<<"Else Point: "<<i<<endl; cout<<"i+1: "<<i+1<<endl; cout<<"i+1+points: "<<i+1+points-k<<endl; cout<<"i+points: "<<i+points-k<<"\n"<<endl; cout<<(mod)<<" = Mod\n\n";*/ vec4 vert1L(m->getVertex(i)[0],m->getVertex(i)[1],m->getVertex(i)[2],1); vec4 vert2L(m->getVertex(i+1)[0],m->getVertex(i+1)[1],m->getVertex(i+1)[2],1); vec4 vert3L(m->getVertex(i+points+1-k)[0],m->getVertex(i+points+1-k)[1],m->getVertex(i+points+1-k)[2],1); if(((i+2)%points) == 0 && m->getVertex(i+1)[0] == 0) { vert3L[0] = m->getVertex(i+points-k)[0]; vert3L[1] = m->getVertex(i+points-k)[1]; vert3L[2] = m->getVertex(i+points-k)[2]; } vec4 nL = getNormal(vert1L,vert2L,vert3L); vec3 normalL(nL[0],nL[1],nL[2]); /*std::cout<<"--------\nNormal x = "<<normal[0]<<std::endl; std::cout<<"Normal y = "<<normal[1]<<std::endl; std::cout<<"Normal z = "<<normal[2]<<std::endl;*/ m->addNormal(normalL); //creates the faces, normal index is = i, always have 4 vertices for a face vec2 f1L((double)i,(double)i); vec2 f2L((double)i+1,(double)i); vec2 f3L((double)points+i+1-k,(double)i); vec2 f4L((double)points+i-k,(double)i); std::vector<vec2> faceL; faceL.push_back(f2L); faceL.push_back(f1L); faceL.push_back(f4L); faceL.push_back(f3L); if(mod != 0) { //cout<<"add face: "<<i<<endl; m->addFace(faceL); } } } //check if there is an opening at the top or bottom to cap or not if(polyline[0][0] != 0) { //cap the bottom /*cout<<"X1 = "<<m->getVertex(0)[0]<<" Y1 = "<<m->getVertex(0)[1]<<" Z1 = "<<m->getVertex(points-1)[2]<<endl; cout<<"X2 = "<<m->getVertex((points))[0]<<" Y2 = "<<m->getVertex((points))[1]<<" Z2 = "<<m->getVertex((points))[2]<<endl; cout<<"X3 = "<<m->getVertex((points*2))[0]<<" Y3 = "<<m->getVertex((points*2))[1]<<" Z3 = "<<m->getVertex((points*2))[2]<<"\n"<<endl;*/ vec4 vert1(m->getVertex(0)[0],m->getVertex(0)[1],m->getVertex(0)[2],1); vec4 vert2(m->getVertex(points)[0],m->getVertex(points)[1],m->getVertex(points)[2],1); vec4 vert3(m->getVertex(points*2)[0],m->getVertex(points*2)[1],m->getVertex(points*2)[2],1); vec4 n = getNormal(vert1,vert2,vert3); vec3 normal(n[0],n[1],n[2]); m->addNormal(normal); std::vector<vec2> face; for(int k=0; k<slices; k++) { vec2 f((double)points*k,(double)m->lastNorm()); face.push_back(f); } m->addFace(face); } if(polyline[points-1][0] != 0) { //cap the top /*cout<<"Top\nX1 = "<<m->getVertex(points-1)[0]<<" Y1 = "<<m->getVertex(points-1)[1]<<" Z1 = "<<m->getVertex(points-1)[2]<<endl; cout<<"X2 = "<<m->getVertex((points*2)-1)[0]<<" Y2 = "<<m->getVertex((points*2)-1)[1]<<" Z2 = "<<m->getVertex((points*2)-1)[2]<<endl; cout<<"X3 = "<<m->getVertex((points*3)-1)[0]<<" Y3 = "<<m->getVertex((points*3)-1)[1]<<" Z3 = "<<m->getVertex((points*3)-1)[2]<<"\n"<<endl;*/ vec4 vert1(m->getVertex(points-1)[0],m->getVertex(points-1)[1],m->getVertex(points-1)[2],1); vec4 vert2(m->getVertex((points*2)-1)[0],m->getVertex((points*2)-1)[1],m->getVertex((points*2)-1)[2],1); vec4 vert3(m->getVertex((points*3)-1)[0],m->getVertex((points*3)-1)[1],m->getVertex((points*3)-1)[2],1); vec4 n = getNormal(vert1,vert2,vert3); n = n * (-1); vec3 normal(n[0],n[1],n[2]); m->addNormal(normal); std::vector<vec2> face; for(int k=1; k<slices+1; k++) { vec2 f((double)(points*k)-1,(double)m->lastNorm()); face.push_back(f); } m->addFace(face); } return m; }
void Ball::animateBall(float interval) { bool collision = false; Vector3 normal(0.0f, 0.0f, 0.0f); Vector3 ball; Vector3 step; mVelocity = add(mVelocity, scale(MOVE_FORCE / mMass * interval, mPushDirection)); mVelocity.z -= GRAVITY * interval; /* collision detection */ step = scale(interval, mVelocity); ball = mPos + step; /* check only fields near by the ball. check field under ball first!!! */ QuadList list = getFieldSphereIntersection(ball, BALL_RADIUS); while (list.next()) { Quad quad = *list; const Vector3* q = quad.mVertices; const Vector3& dir = quad.mNormals[0]; Vector3 a1 = closestPointTriangle(ball, q[0], q[1], q[2]); Vector3 a2 = closestPointTriangle(ball, q[2], q[3], q[0]); float d1 = (ball - a1).len(); float d2 = (ball - a2).len(); Vector3 a = (d1 <= d2) ? a1 : a2; if ((a - ball).len() <= BALL_RADIUS) { /* dist = vector from ball center to quad */ Vector3 dist = a - ball; float l = dist.len(); /* move = vector to move the ball out of quad */ Vector3 move = scale(-((mRadius - l) / l), dist); /* some rotation for a better look */ Vector3 right = norm(cross(dir, step)); Vector3 forward = norm(cross(right, dir)); mAngularRate = scale(dot(sub(ball, mPos), forward) / (2.0f * M_PI * mRadius) * 360.0f / interval, right); ball = add(ball, move); normal = add(normal, move); collision = true; } } mPos = ball; normal = norm(normal); mHasBallHitGoal = false; /* contact to surface? */ if (collision) { float vn = dot(mVelocity, normal); Vector3 rebound = scale(-(1 + ELASTICITY) * vn, normal); if (len(rebound) > 3.0f * JUMP_FORCE * interval) { /* collision was to havy */ explodeBall(); } else { mVelocity = add(mVelocity, rebound); if (mJump) { mVelocity = add(mVelocity, scale(JUMP_FORCE / mMass * interval, normal)); } } int x = (int) floor(ball.x - sgLevel.origin.x); int y = (int) floor(ball.y - sgLevel.origin.y); if (x == sgLevel.finish.x && y == sgLevel.finish.y) { /* hit goal */ mHasBallHitGoal = true; } } /***/ /* damping */ mVelocity = scale(DAMPING, mVelocity); Quaternion rotation = mkQuaternion(len(mAngularRate) * interval, mAngularRate); mOrientation = mulQuaternion(rotation, mOrientation); /* falling to infinity */ if (mPos.z < -1.0f) { explodeBall(); } /* reset through user */ if (wasKeyPressed(KEY_ENTER)) { explodeBall(); } mPushDirection = Vector3(0.0f, 0.0f, 0.0f); mJump = false; }
void AddCollisionTreeMesh (DemoEntityManager* const scene) { // open the level data char fullPathName[2048]; GetWorkingFileName ("playground.ngd", fullPathName); scene->LoadScene (fullPathName); // find the visual mesh and make a collision tree NewtonWorld* const world = scene->GetNewton(); DemoEntity* const entity = scene->GetLast()->GetInfo(); DemoMesh* const mesh = (DemoMesh*)entity->GetMesh(); dAssert (mesh->IsType(DemoMesh::GetRttiType())); NewtonCollision* const tree = NewtonCreateTreeCollision(world, 0); NewtonTreeCollisionBeginBuild(tree); dFloat* const vertex = mesh->m_vertex; for (DemoMesh::dListNode* node = mesh->GetFirst(); node; node = node->GetNext()){ DemoSubMesh* const subMesh = &node->GetInfo(); unsigned int* const indices = subMesh->m_indexes; int trianglesCount = subMesh->m_indexCount; for (int i = 0; i < trianglesCount; i += 3) { dVector face[3]; int index = indices[i + 0] * 3; face[0] = dVector (vertex[index + 0], vertex[index + 1], vertex[index + 2]); index = indices[i + 1] * 3; face[1] = dVector (vertex[index + 0], vertex[index + 1], vertex[index + 2]); index = indices[i + 2] * 3; face[2] = dVector (vertex[index + 0], vertex[index + 1], vertex[index + 2]); int matID = 0; //matID = matID == 2 ? 1 : 2 ; NewtonTreeCollisionAddFace(tree, 3, &face[0].m_x, sizeof (dVector), matID); } } NewtonTreeCollisionEndBuild (tree, 0); // add the collision tree to the collision scene void* const proxy = NewtonSceneCollisionAddSubCollision (m_sceneCollision, tree); // destroy the original tree collision NewtonDestroyCollision (tree); // set the parameter on the added collision share dMatrix matrix (entity->GetCurrentMatrix()); NewtonCollision* const collisionTree = NewtonSceneCollisionGetCollisionFromNode (m_sceneCollision, proxy); NewtonSceneCollisionSetSubCollisionMatrix (m_sceneCollision, proxy, &matrix[0][0]); NewtonCollisionSetUserData(collisionTree, mesh); // set the application level callback #ifdef USE_STATIC_MESHES_DEBUG_COLLISION NewtonStaticCollisionSetDebugCallback (collisionTree, ShowMeshCollidingFaces); #endif mesh->AddRef(); scene->RemoveEntity (entity); #ifdef USE_TEST_ALL_FACE_USER_RAYCAST_CALLBACK // set a ray cast callback for all face ray cast NewtonTreeCollisionSetUserRayCastCallback (collisionTree, AllRayHitCallback); dVector p0 (0, 100, 0, 0); dVector p1 (0, -100, 0, 0); dVector normal(0.0f); dLong id; dFloat parameter; parameter = NewtonCollisionRayCast (collisionTree, &p0[0], &p1[0], &normal[0], &id); #endif }
void Role::decipher(QString command) { QStringList arg=command.split(';'); QStringList cardIDList; int targetID,targetArea; int sourceID,sourceArea; int cardID; int howMany; int hitRate; int i; int team,gem,crystal; Card*card; Player*player; Team *red=dataInterface->getRedTeam(); Team *blue=dataInterface->getBlueTeam(); QList<Card*> cards; QString msg; QString flag; QString cardName; QList<Player*>playerList=dataInterface->getPlayerList(); ShowArea* showArea=gui->getShowArea(); switch (arg[0].toInt()) { //回合开始 case 3: targetID=arg[1].toInt(); gui->logAppend("-----------------------------------"); gui->logAppend(playerList[targetID]->getName()+tr("回合开始")); playerArea->setCurrentPlayerID(targetID); if(targetID!=myID) { isMyTurn=0; gui->setEnable(0); } else{ isMyTurn=true; onceUsed=false; start=false; usedAttack=usedMagic=usedSpecial=false; } break; //应战询问 case 5: hitRate=arg[1].toInt(); cardID=arg[2].toInt(); targetID=arg[3].toInt(); sourceID=arg[4].toInt(); card=dataInterface->getCard(cardID); if(targetID!=myID) { gui->setEnable(0); } else { gui->setEnable(1); msg=playerList[sourceID]->getName()+tr("对")+tr("你")+tr("使用了")+card->getName(); if (hitRate==2) return; if(hitRate==1) msg+=tr(",该攻击无法应战"); sourcePlayerID=sourceID; gui->reset(); tipArea->setMsg(msg); attacked(card->getElement(),hitRate); } break; //弃牌询问 case 7: targetID=arg[1].toInt(); howMany=arg[2].toInt(); flag=arg[3]; msg=playerList[targetID]->getName()+tr("需要弃")+arg[2]+tr("张手牌"); if(flag=="y") gui->logAppend(msg+tr("(明弃)")); else gui->logAppend(msg+tr("(暗弃)")); if(targetID!=myID) { gui->setEnable(0); } else { gui->setEnable(1); gui->reset(); drop(howMany); tipArea->setMsg(tr("你需要弃")+arg[2]+tr("张手牌")); } break; //摸牌 case 9: targetID=arg[1].toInt(); howMany=arg[2].toInt(); gui->logAppend(playerList[targetID]->getName()+tr("摸取")+arg[2]+tr("张手牌")); if(targetID==myID) { cardIDList=arg[3].split(','); for(i=0;i<howMany;i++) { cardID=cardIDList[i].toInt(); card=dataInterface->getCard(cardID); dataInterface->addHandCard(card); } } break; //牌堆重洗 case 10: gui->logAppend(tr("牌堆已重洗")); break; //士气改变 case 11: team=arg[1].toInt(); howMany=arg[2].toInt(); if(team) { red->setMorale(howMany); teamArea->update(); } else { blue->setMorale(howMany); teamArea->update(); } break; //游戏结束 case 12: team=arg[1].toInt(); msg=tr("游戏结束,"); if(team) msg+=tr("红队获胜"); else msg+=tr("蓝队获胜"); gui->logAppend(msg); tipArea->setMsg(msg); break; //弃牌公告 case 13: targetID=arg[1].toInt(); howMany=arg[2].toInt(); flag=arg[3]; msg=playerList[targetID]->getName()+tr("丢弃了")+arg[2]+tr("张手牌"); if(flag=="y") { cardIDList=arg[4].split(','); msg+=":"; cards.clear(); for(i=0;i<howMany;i++) { cardID=cardIDList[i].toInt(); card=dataInterface->getCard(cardID); msg+=card->getName()+"-"+card->getProperty()+" "; cards<<card; dataInterface->removeHandCard(card); } showArea->showCards(cards); } gui->logAppend(msg); break; //命中公告 case 14: flag=arg[1]; targetID=arg[3].toInt(); sourceID=arg[4].toInt(); if(flag=="1") msg=playerList[sourceID]->getName()+tr("未命中")+playerList[targetID]->getName(); else msg=playerList[sourceID]->getName()+tr("命中")+playerList[targetID]->getName(); gui->logAppend(msg); break; //星石改变 case 15: team=arg[1].toInt(); gem=arg[2].toInt(); crystal=arg[3].toInt(); if(team) { red->setGem(gem); red->setCrystal(crystal); teamArea->update(); } else { blue->setGem(gem); blue->setCrystal(crystal); teamArea->update(); } break; //星杯改变 case 17: team=arg[1].toInt(); howMany=arg[2].toInt(); if(team) { red->setGrail(howMany); teamArea->update(); } else { blue->setGrail(howMany); teamArea->update(); } break; //能量改变 case 18: targetID=arg[1].toInt(); gem=arg[2].toInt(); crystal=arg[3].toInt(); player=playerList.at(targetID); player->setGem(gem); player->setCrystal(crystal); playerArea->update(); break; //移牌通告 case 19: howMany=arg[1].toInt(); cardIDList=arg[2].split(","); sourceID=arg[3].toInt(); sourceArea=arg[4].toInt(); targetID=arg[5].toInt(); targetArea=arg[6].toInt(); if(sourceID!=-1) { switch(sourceArea) { case 4: player=playerList.at(sourceID); player->changeHandCardNum(-howMany); break; case 5: cardID=cardIDList[0].toInt(); card=dataInterface->getCard(cardID); playerList[sourceID]->removeStatus(card); break; } } if(targetID!=-1) { switch(targetArea) { case 4: player=playerList.at(targetID); player->changeHandCardNum(howMany); break; case 5: cardID=cardIDList[0].toInt(); card=dataInterface->getCard(cardID); cardName=card->getName(); if(cardName==tr("中毒")) playerList[targetID]->addStatus(0,card); if(cardName==tr("虚弱")) playerList[targetID]->addStatus(1,card); if(cardName==tr("圣盾")||card->getSpecialityList().contains(tr("天使之墙"))) playerList[targetID]->addStatus(2,card); if(card->getType()=="attack"&&card->getProperty()=="幻") playerList[targetID]->addStatus(3,card); break; } } playerArea->update(); break; //物伤通告 case 20: targetID=arg[1].toInt(); sourceID=arg[3].toInt(); gui->logAppend(playerList[sourceID]->getName()+tr("对")+playerList[targetID]->getName()+tr("造成了")+arg[2]+tr("点攻击伤害")); break; //法术通告 case 21: targetID=arg[1].toInt(); sourceID=arg[3].toInt(); gui->logAppend(playerList[sourceID]->getName()+tr("对")+playerList[targetID]->getName()+tr("使用的")+arg[4]+tr("造成了")+arg[2]+tr("点法术伤害")); break; //虚弱询问 case 22: targetID=arg[1].toInt(); gui->logAppend(tr("等待")+playerList[targetID]->getName()+tr("虚弱响应")); if(targetID!=myID) { gui->setEnable(0); } else { gui->setEnable(1); gui->reset(); state=7; decisionArea->enable(0); decisionArea->enable(1); tipArea->setMsg(tr("你被虚弱了,选是跳过行动阶段,选否摸")+arg[2]+tr("张牌")); } break; //虚弱结果 case 24: targetID=arg[1].toInt(); if(arg[2]=="0") gui->logAppend(playerList[targetID]->getName()+tr("选择跳过行动阶段")); else gui->logAppend(playerList[targetID]->getName()+tr("选择摸")+arg[3]+tr("张牌挣脱")); break; //圣盾移除通告 case 25: targetID=arg[1].toInt(); gui->logAppend(playerList[targetID]->getName()+tr("的圣盾被移除")); break; //魔弹询问 case 26: int nextID; targetID=arg[1].toInt(); sourceID=arg[2].toInt(); howMany=arg[3].toInt(); nextID=arg[4].toInt(); gui->logAppend(playerList[sourceID]->getName()+tr("对")+playerList[targetID]->getName()+tr("使用了魔弹,目前伤害为:")+arg[3]+tr("点")); if(targetID!=myID) { gui->setEnable(0); } else { gui->setEnable(1); msg=playerList[sourceID]->getName()+tr("对")+tr("你")+tr("使用了魔弹,目前伤害为:")+arg[3]+tr("点"); gui->reset(); tipArea->setMsg(msg); moDaned(nextID); } break; //卡牌通告 case 28: cardID=arg[1].toInt(); targetID=arg[2].toInt(); sourceID=arg[3].toInt(); flag=arg[4]; card=dataInterface->getCard(cardID); cardName=card->getName(); if(flag=="1"){ cards.clear(); cards<<card; showArea->showCards(cards); } if(card->getElement()!="light") gui->logAppend(playerList[sourceID]->getName()+tr("对")+playerList[targetID]->getName()+tr("使用了")+cardName); else gui->logAppend(playerList[sourceID]->getName()+tr("使用圣光抵御")); break; //行动阶段 flag 0-所有行动,1-攻击行动,2-法术行动,3-特殊行动,4-攻击或法术行动 case 29: targetID=arg[1].toInt(); flag=arg[2]; if(targetID!=myID) { isMyTurn=0; gui->setEnable(0); actionFlag=-1; } else { gui->setEnable(1); if(flag=="0") normal(); else if(flag=="1") attackAction(); else if(flag=="2") magicAction(); else if(flag=="4") attackOrMagic(); if(arg[3]=="1") decisionArea->enable(3); } break; //无法行动公告 case 31: targetID=arg[1].toInt(); msg=playerList[targetID]->getName()+tr("宣告无法行动"); gui->logAppend(msg); break; //治疗改变 case 32: targetID=arg[1].toInt(); howMany=arg[2].toInt(); playerList[targetID]->setCrossNum(howMany); break; //治疗询问 case 33: targetID=arg[1].toInt(); howMany=arg[2].toInt(); flag=arg[3]; gui->reset(); if(targetID==myID) cure(playerList[myID]->getCrossNum(),howMany,flag.toInt()); break; //技能响应询问 case 35: targetID=arg[1].toInt(); flag=arg[2]; msg=tr("等待")+playerList[targetID]->getName()+flag+tr("响应"); gui->logAppend(msg); gui->reset(); gui->setEnable(0); break; //信息通告 case 38: gui->logAppend(arg[1]); break; //角色形态转换通知 case 39: targetID=arg[1].toInt(); flag=arg[2]; if(flag=="0") { playerList[targetID]->setTap(0); msg=playerList[targetID]->getName()+tr("返回")+arg[3]; } else { playerList[targetID]->setTap(1); msg=playerList[targetID]->getName()+tr("进入")+arg[3]; } playerArea->update(); gui->logAppend(msg); gui->reset(); gui->setEnable(0); break; //手牌上限变更通知 case 40: targetID=arg[1].toInt(); howMany=arg[2].toInt(); playerList[targetID]->setHandCardsMax(howMany); playerArea->update(); break; //获得手牌通告 case 41: targetID=arg[1].toInt(); flag=arg[2]; howMany=arg[3].toInt(); gui->logAppend(playerList[targetID]->getName()+tr("获得")+arg[3]+tr("张手牌")); if(targetID==myID) { cardIDList=arg[4].split(','); for(i=0;i<howMany;i++) { cardID=cardIDList[i].toInt(); card=dataInterface->getCard(cardID); dataInterface->addHandCard(card); } } break; //额外行动询问 case 42: targetID=arg[1].toInt(); gui->logAppend(tr("等待")+playerList[targetID]->getName()+tr("额外行动响应")); if(targetID==myID) { gui->setEnable(1); addtionalAction(); } else gui->setEnable(0); break; //专属变更 case 43: targetID=arg[1].toInt(); msg=arg[2]; playerList.at(targetID)->setSpecial(msg.toInt(),arg[3].toInt()); playerArea->update(); break; //标记变更 case 45: targetID=arg[1].toInt(); flag=arg[2]; howMany=arg[3].toInt(); playerList.at(targetID)->setToken(flag.toInt(),howMany); playerArea->update(); break; //天使祝福 case 750: targetID=arg[1].toInt(); howMany=arg[2].toInt(); msg=tr("等待")+playerList[targetID]->getName()+tr("天使祝福(给牌)响应"); gui->logAppend(msg); if(targetID!=myID) { isMyTurn=0; gui->setEnable(0); } else { gui->setEnable(1); TianShiZhuFu(howMany); } break; //魔爆冲击(弃牌) case 850: targetID=arg[1].toInt(); msg=tr("等待")+playerList[targetID]->getName()+tr("魔爆冲击(弃牌)响应"); gui->logAppend(msg); if(targetID!=myID) { isMyTurn=0; gui->setEnable(0); } else { gui->setEnable(1); MoBaoChongJi(); } break; } }
individual_tPtr AddNormalMute(param_tPtr eps, individual_tPtr ind, int gen) { unsigned long i = 1, PLength; if(NULL == eps) panic(A_FATAL, "AddNormalMute", "uninitialized parameter structure : %s : %d", __FILE__, __LINE__); if(NULL == ind) panic(A_FATAL, "AddNormalMute", "uninitialized individual : %s : %d",__FILE__, __LINE__); switch(gen) { case X: if(TRUE == CorrField[X]) return(_AddCorrNormalXMute(eps, ind)); else return(_AddNormalXMute(eps, ind)); case ALPHA: for(i = 1; i <= inGetAlphaLength(ind); i++) { inAssignAlphaComponent(i, AlphaTRANSFORM(eps, ind, inGetAlphaComponent(i, ind), inGetAlphaComponent(i, ind) + eps->Beta * normal(1.0)), ind); } return(ind); case P: if(0 == (PLength = inGetPLength(ind))) panic(A_WARN, "AddNormalMute", "zero dimensional p vector : %s : %d", __FILE__, __LINE__); if(1 == PLength) inAssignPComponent(1, PTRANSFORM(eps, ind, inGetPComponent(1, ind), inGetPComponent(1,ind) + eps->GamOne * normal(1.0)), ind); else { for(i = 1; i <= inGetPLength(ind); i++) { inAssignPComponent(i, PTRANSFORM(eps, ind, inGetPComponent(i, ind), inGetPComponent(i, ind) + eps->GamLcl * normal(1.0)), ind); } } return(ind); case D: case SIGMA: default: panic(A_FATAL, "AddNormalMute", "unallowed gen for the additiv normal mutation scheme : %s : %d", __FILE__, __LINE__); } return(ind); }
void PlayerBehaviourAir::resolve(const sf::Vector3f& manifold, CollisionWorld::Body* other) { switch (other->getType()) { case CollisionWorld::Body::Type::Water: setBehaviour<PlayerBehaviourWater>(); break; case CollisionWorld::Body::Type::Solid: case CollisionWorld::Body::Type::Block: move(sf::Vector2f(manifold.x, manifold.y) * manifold.z); if (manifold.y * manifold.z < 0) //contact is below so must be standing on something { setVelocity({ getVelocity().x, 0.f }); setBehaviour<PlayerBehaviourGround>(); Event playerEvent; playerEvent.type = Event::Player; playerEvent.player.playerId = Category::None; playerEvent.player.action = Event::PlayerEvent::Landed; raiseEvent(playerEvent); } break; case CollisionWorld::Body::Type::Player: { move(sf::Vector2f(manifold.x, manifold.y) * manifold.z); auto vel = getVelocity(); if (manifold.x != 0) //players colliding sideways in air { setVelocity({ -vel.x, vel.y }); } else if (manifold.y * manifold.z < 0) { //bounce off players below setVelocity({ vel.x, -vel.y * getFriction() }); } } break; case CollisionWorld::Body::Type::Npc: { //move(sf::Vector2f(manifold.x, manifold.y) * manifold.z); ////bounce off NPCs //auto vel = getVelocity(); //if (manifold.x != 0) // vel.x = -vel.x; //if (manifold.y != 0) // vel.y = -vel.y; //vel *= getFriction(); //setVelocity(vel); kill(); } break; case CollisionWorld::Body::Item: { Event e; e.node.action = Event::NodeEvent::KilledNode; e.node.type = getParentCategory(); e.node.target = Category::Item; e.type = Event::Node; raiseEvent(e); } break; case CollisionWorld::Body::FreeForm: { if (other->getParentCategory() == Category::HatCarried) break; sf::Vector2f normal(manifold.x, manifold.y); move(normal * manifold.z); setVelocity(Util::Vector::reflect(getVelocity() * getFriction(), normal)); } break; default: break; } }