Example #1
0
void convertToBtTransform(const Vector3d& start_pos, const Vector3d& end_pos, btVector3& origin, btMatrix3x3& basis)
{
	Matrix3d rotation;
	rotation_from_tangent((start_pos - end_pos).normalized(), rotation);
	basis.setValue(rotation(0,0), rotation(0,1), rotation(0,2),
								 rotation(1,0), rotation(1,1), rotation(1,2),
								 rotation(2,0), rotation(2,1), rotation(2,2));
	Vector3d mid_point = (start_pos + end_pos)/2.0;
	origin.setValue(mid_point(0), mid_point(1), mid_point(2));
}
Example #2
0
/* Get grad & const for perpend. line to given points */
static void
line_definition (gdouble  x1,
                 gdouble  y1,
                 gdouble  x2,
                 gdouble  y2,
                 gdouble *lgrad,
                 gdouble *lconst)
{
  double grad1;
  double midx, midy;

  grad1 = line_grad (x1, y1, x2, y2);

  if (grad1 == 0.0)
    {
#ifdef DEBUG
      printf ("Infinite grad....\n");
#endif /* DEBUG */
      return;
    }

  mid_point (x1, y1, x2, y2, &midx, &midy);

  /* Invert grad for perpen gradient */

  *lgrad = -1.0 / grad1;

  *lconst = line_cons (midx, midy,*lgrad);
}
Example #3
0
void Tree::Create(const std::vector<sf::Vector3f>& tile_vertices)
{
    if(tile_vertices.size() != 4)
    {
        throw std::invalid_argument("tile_vertices must have 4 elements");
    }

    sf::Vector3f mid_point((tile_vertices[0].x + tile_vertices[2].x) / 2.0,
                           (tile_vertices[0].y + tile_vertices[2].y) / 2.0,
                           (tile_vertices[0].z + tile_vertices[2].z) / 2.0);
    std::vector<sf::Vector3f> quad_vertices(4);
    this->quads.resize(4);
    quad_vertices[0] = tile_vertices[0];
    quad_vertices[1] = mid_point;
    quad_vertices[2] = mid_point + sf::Vector3f(0.0, 0.0, 1.0);
    quad_vertices[3] = tile_vertices[0] + sf::Vector3f(0.0, 0.0, 1.0);
    this->quads[0].create(quad_vertices);
    quad_vertices[0] = tile_vertices[1];
    quad_vertices[1] = mid_point;
    quad_vertices[2] = mid_point + sf::Vector3f(0.0, 0.0, 1.0);
    quad_vertices[3] = tile_vertices[1] + sf::Vector3f(0.0, 0.0, 1.0);
    this->quads[1].create(quad_vertices);
    quad_vertices[0] = tile_vertices[2];
    quad_vertices[1] = mid_point;
    quad_vertices[2] = mid_point + sf::Vector3f(0.0, 0.0, 1.0);
    quad_vertices[3] = tile_vertices[2] + sf::Vector3f(0.0, 0.0, 1.0);
    this->quads[2].create(quad_vertices);
    quad_vertices[0] = tile_vertices[3];
    quad_vertices[1] = mid_point;
    quad_vertices[2] = mid_point + sf::Vector3f(0.0, 0.0, 1.0);
    quad_vertices[3] = tile_vertices[3] + sf::Vector3f(0.0, 0.0, 1.0);
    this->quads[3].create(quad_vertices);
    this->type = rand() % 2;
}
Example #4
0
double capsuleBoxDistance(const Vector3d& a_start, const Vector3d& a_end, const double a_radius, const Vector3d& b_center, const Vector3d& b_half_length, Vector3d& direction)
{
	btTransform tr[2];

	Matrix3d rotationA;
	rotation_from_tangent((a_start - a_end).normalized(), rotationA);
	btMatrix3x3 basisA;
	basisA.setValue(rotationA(0,1), rotationA(0,0), rotationA(0,2),
									rotationA(1,1), rotationA(1,0), rotationA(1,2),
									rotationA(2,1), rotationA(2,0), rotationA(2,2));
	btMatrix3x3 basisB;
	basisB.setIdentity();
	tr[0].setBasis(basisA);
	tr[1].setBasis(basisB);
	
	Vector3d mid_point = (a_start + a_end)/2.0;
	btVector3 originA(mid_point(0), mid_point(1), mid_point(2));
	btVector3 originB(b_center(0), b_center(1), b_center(2));
	tr[0].setOrigin(originA);
	tr[1].setOrigin(originB);	
	
	btCollisionShape*	shapePtr[2];
	
	btCapsuleShape capsule(btScalar(a_radius), btScalar((a_start-a_end).norm()));
	btBoxShape box(btVector3(b_half_length(0), b_half_length(1), b_half_length(2)));
	shapePtr[0] = &capsule;
	shapePtr[1] = &box;

	btDefaultCollisionConfiguration collisionConfiguration;
	btCollisionDispatcher				dispatcher(&collisionConfiguration);
	btDbvtBroadphase pairCache;
	btCollisionWorld world (&dispatcher,&pairCache,&collisionConfiguration);
	world.getDispatchInfo().m_convexMaxDistanceUseCPT = true;
	MyContactResultCallback result;
	btCollisionObject obA;
	obA.setCollisionShape(shapePtr[0]);
	obA.setWorldTransform(tr[0]);
	btCollisionObject obB;
	obB.setCollisionShape(shapePtr[1]);
	obB.setWorldTransform(tr[1]);
	world.contactPairTest(&obA,&obB,result);

	direction = result.positionWorldOnB - result.positionWorldOnA;
	return result.distance;
}
Example #5
0
static void
calc_section (spline *s,
	      double cminus1x, double cminus1y,
	      double cx, double cy,
	      double cplus1x, double cplus1y,
	      double cplus2x, double cplus2y)
{
  double p0x, p1x, p2x, p3x, tempx,
  p0y, p1y, p2y, p3y, tempy;
  
  third_point (cx, cy, cplus1x, cplus1y, &p1x, &p1y);
  third_point (cplus1x, cplus1y, cx, cy, &p2x, &p2y);
  third_point (cx, cy, cminus1x, cminus1y, &tempx, &tempy);
  mid_point (tempx, tempy, p1x, p1y, &p0x, &p0y);
  third_point (cplus1x, cplus1y, cplus2x, cplus2y, &tempx, &tempy);
  mid_point (tempx, tempy, p2x, p2y, &p3x, &p3y);
  add_bezier_arc (s, p0x, p0y, p1x, p1y, p2x, p2y, p3x, p3y);
}
Example #6
0
CubitVector RefEdge::curve_center()
{
  CubitVector p1 = start_vertex()->coordinates();
  CubitVector p2 = end_vertex()->coordinates();
  if ( start_vertex() == end_vertex() )
    {
      mid_point(p2);
    }
  p1 += p2;
  p1 /= 2.0;
  return p1;
}
Example #7
0
static void 
add_bezier_arc (spline *s,
		double x0, double y0,
		double x1, double y1,
		double x2, double y2,
		double x3, double y3)
{
  double midx01, midx12, midx23, midlsegx, midrsegx, cx,
  midy01, midy12, midy23, midlsegy, midrsegy, cy;
    
  mid_point (x0, y0, x1, y1, &midx01, &midy01);
  mid_point (x1, y1, x2, y2, &midx12, &midy12);
  mid_point (x2, y2, x3, y3, &midx23, &midy23);
  mid_point (midx01, midy01, midx12, midy12, &midlsegx, &midlsegy);
  mid_point (midx12, midy12, midx23, midy23, &midrsegx, &midrsegy);
  mid_point (midlsegx, midlsegy, midrsegx, midrsegy, &cx, &cy);    

  if (can_approx_with_line (x0, y0, midlsegx, midlsegy, cx, cy))
    add_line (s, x0, y0, cx, cy);
  else if ((midx01 != x1) || (midy01 != y1) || (midlsegx != x2)
	   || (midlsegy != y2) || (cx != x3) || (cy != y3))
    add_bezier_arc (s, x0, y0, midx01, midy01, midlsegx, midlsegy, cx, cy);

  if (can_approx_with_line (cx, cy, midx23, midy23, x3, y3))
    add_line (s, cx, cy, x3, y3);
  else if ((cx != x0) || (cy != y0) || (midrsegx != x1) || (midrsegy != y1)
	   || (midx23 != x2) || (midy23 != y2))  
    add_bezier_arc (s, cx, cy, midrsegx, midrsegy, midx23, midy23, x3, y3);
}
Example #8
0
void CombatCamera::ViewportRay(double screen_x, double screen_y,
                               double out_ray_origin[3], double out_ray_direction[3]) const
{
    Matrix projection(4, 4);
    std::copy(m_camera.getProjectionMatrix()[0], m_camera.getProjectionMatrix()[0] + 16,
              projection.data().begin());

    Matrix view(4, 4);
    std::copy(m_camera.getViewMatrix(true)[0], m_camera.getViewMatrix(true)[0] + 16, view.data().begin());

    Matrix inverse_vp = Inverse4(prod(projection, view));

    double nx = (2.0 * screen_x) - 1.0;
    double ny = 1.0 - (2.0 * screen_y);
    Matrix near_point(3, 1);
    near_point(0, 0) = nx;
    near_point(1, 0) = ny;
    near_point(2, 0) = -1.0;
    // Use mid_point rather than far point to avoid issues with infinite projection
    Matrix mid_point(3, 1);
    mid_point(0, 0) = nx;
    mid_point(1, 0) = ny;
    mid_point(2, 0) = 0.0;

    // Get ray origin and ray target on near plane in world space
    Matrix ray_origin = Matrix4xVector3(inverse_vp, near_point);
    Matrix ray_target = Matrix4xVector3(inverse_vp, mid_point);

    Matrix ray_direction = ray_target - ray_origin;
    ray_direction /=
        ray_direction(0, 0) * ray_direction(0, 0) +
        ray_direction(1, 0) * ray_direction(1, 0) +
        ray_direction(2, 0) * ray_direction(2, 0);

    std::copy(ray_origin.data().begin(), ray_origin.data().end(), out_ray_origin);
    std::copy(ray_direction.data().begin(), ray_direction.data().end(), out_ray_direction);
}
Example #9
0
void line_arc_line(double tooth_angle)
{
	double gap = gap_width();
	double radius = gap/3;
	double clearance = gear_for_point->GetClearanceMM();
	double line_length = clearance - radius;
	if(line_length < 0.0)
	{
		line_length = 0.0;
		radius = clearance;
	}

	gp_Pnt p1, p2, p1B, p2B;
	get_clearance_points(p1, p2, tooth_angle, 0.0);
	get_clearance_points(p1B, p2B, tooth_angle, line_length);
	gp_Pnt pm = mid_point(tooth_angle, line_length);
	gp_Circ c(gp_Ax2(pm, gp_Dir(0, 0, -1)), radius);

	if(line_length >= wxGetApp().m_geom_tol)
		sketch_for_gear->Add(new HLine(p1, p1B, &wxGetApp().current_color), NULL);

	double mid_span_line_length = gap - 2*radius;
	if(mid_span_line_length >= wxGetApp().m_geom_tol)
	{
		gp_Vec v(p1, p2);
		v.Normalize();
		gp_Pnt p1C, p2C;
		get_clearance_points(p1C, p2C, tooth_angle, clearance);
		gp_Pnt two_arc_p1 = gp_Pnt(p1C.XYZ() + v.XYZ() * radius);
		gp_Pnt two_arc_p2 = gp_Pnt(two_arc_p1.XYZ() + v.XYZ() * mid_span_line_length);
		gp_Pnt pm1 = gp_Pnt(p1B.XYZ() + v.XYZ() * radius);
		gp_Pnt pm2 = gp_Pnt(pm1.XYZ() + v.XYZ() * mid_span_line_length);
		gp_Circ c1(gp_Ax2(pm1, gp_Dir(0, 0, -1)), radius);
		gp_Circ c2(gp_Ax2(pm2, gp_Dir(0, 0, -1)), radius);
		sketch_for_gear->Add(new HArc(p1B, two_arc_p1, c1, &wxGetApp().current_color), NULL);
		sketch_for_gear->Add(new HLine(two_arc_p1, two_arc_p2, &wxGetApp().current_color), NULL);
		sketch_for_gear->Add(new HArc(two_arc_p2, p2B, c2, &wxGetApp().current_color), NULL);
	}
	else
	{
		sketch_for_gear->Add(new HArc(p1B, p2B, c, &wxGetApp().current_color), NULL);
	}

	if(line_length >= wxGetApp().m_geom_tol)
		sketch_for_gear->Add(new HLine(p2B, p2, &wxGetApp().current_color), NULL);

	spline_points_for_gear.clear();
	spline_points_for_gear.push_back(p2);
}
Example #10
0
// need code that binary searches and figures out where to insert
// need to account for the possibility of the difference being less
// than 0.001
int binary_search(int* ids, int id, double* values, double value, int min_index, int max_index){
	if (max_index < min_index){
		return min_index;
	} else {
		int mid_index = mid_point(min_index , max_index);
		double of_interest = values[ids[mid_index]];
		double diff = abs(of_interest-value);
		int same = FALSE;
		if (diff < 0.001){
			same = TRUE;
		}
		if ((of_interest > value && !same) || (same && id > ids[mid_index])){
			return binary_search(ids, id, values, value, min_index, mid_index-1);
		} else {
			return binary_search(ids, id, values, value, mid_index+1, max_index);
		}
	}
}
Example #11
0
void Points(double *matrix, int *csize, double *tMatrix)
{

	int j;
   for(j=0;j<*csize-1;j++)
   {
      if(matrix[j+1]==matrix[j])
        {
         do
           {
            tMatrix[j]=0.0;
            j++;
           } while(matrix[j+1]==matrix[j]);
         }
         tMatrix[j] = mid_point(matrix[j], matrix[j+1]);
   }
   tMatrix[(*csize)-1]=0.0;

}
void orderPointsCounterClockWise(std::vector<vpImagePoint> &p)
{
  // Now we create a map of VpPoint in order to order the points
  std::map< double, vpImagePoint> poly_verteces;
  vpImagePoint mid_point(0, 0);
  for(unsigned int i=0; i < p.size(); i++)
    mid_point += p[i];
  mid_point /= p.size();

  double theta;
  for(unsigned int i=0; i < p.size(); i++)
  {
    theta = atan2(p[i].get_j() - mid_point.get_j(), p[i].get_i() - mid_point.get_i());
    // Insert the vertexes in the map (ordered)
    poly_verteces.insert ( std::pair<double, vpImagePoint>(theta, p[i]) );
  }
  // Now we create a Vector containing the ordered vertexes
  std::vector<vpImagePoint> p_ordered;
  for( std::map<double, vpImagePoint>::iterator it = poly_verteces.begin(); it!=poly_verteces.end(); ++it )
  {
    p_ordered.push_back( it->second );
  }
  p = p_ordered;
}
Example #13
0
void clearance_point_mid(double tooth_angle)
{
	gp_Pnt m = mid_point(tooth_angle, 1.0);
	point(m.X(), m.Y());
}