Пример #1
0
int soltrack_init(double deg_lat, double deg_long)
{
	// Check domain
	if (!(check_lat(deg_lat) && check_long(deg_long))) {
		return SOL_DOMAIN_ERROR;
	}
	// Calculate latitude and longitude in radians
	double rad_lat = deg_to_rad(deg_lat);
	double rad_long = deg_to_rad(deg_long);
	// Create a 3-dimensional vector
	uv_orth_w_spin = gsl_vector_alloc(V_DIM);
	gsl_vector_set(uv_orth_w_spin, X_AXIS, \
		gsl_sf_cos(rad_long) * gsl_sf_cos(rad_lat));
	gsl_vector_set(uv_orth_w_spin, Y_AXIS, \
		gsl_sf_sin(rad_long) * gsl_sf_cos(rad_lat));
	gsl_vector_set(uv_orth_w_spin, Z_AXIS, gsl_sf_sin(rad_lat));
	// Create the rotation matrix for axial tilt of the earth
	rm_ax_tilt = gsl_matrix_calloc(V_DIM, V_DIM); // Initialize 0
	// X-Axis
	gsl_matrix_set(rm_ax_tilt, X_AXIS, X_AXIS, gsl_sf_cos(rad_ax_tilt));
	gsl_matrix_set(rm_ax_tilt, X_AXIS, Z_AXIS, -gsl_sf_sin(rad_ax_tilt));
	// Y-Axis (no rotation)
	gsl_matrix_set(rm_ax_tilt, Y_AXIS, Y_AXIS, 1.0);
	// Z-Axis
	gsl_matrix_set(rm_ax_tilt, Z_AXIS, X_AXIS, gsl_sf_sin(rad_ax_tilt));
	gsl_matrix_set(rm_ax_tilt, Z_AXIS, Z_AXIS, gsl_sf_cos(rad_ax_tilt));
	
	// Initialisation complete
	initialized = 1;	
	return 0;
}
Пример #2
0
void update_pts(){
	for(int i=0; i<N; i++){
		for(int j=0; j<N; j++){
			if(g_map[i][j].occ_flag){ 
				
				((struct OCC_PT*)g_map[i][j].point)->k--; //decrement the confidence
				if(((struct OCC_PT*)g_map[i][j].point)->k < 0){	//replace with free point
					insert_map_pt(i, j, UNSET);
				}
				if(g_map[i][j].occ_flag){	//if there's still a point here
					if(robot.dDis > GRID_UNIT_RES){	//if the robot has moved enough to make a map point shift
						uint8_t i_new = -(robot.dDis * cos(deg_to_rad(robot.heading))) + i;
						uint8_t j_new = -(robot.dDis * sin(deg_to_rad(robot.heading))) + j;

						if((i_new <= N) && (j_new <= N)){	//if the new position is within the map
							insert_map_pt(i_new, j_new, SET);
							//update confidence
							((struct OCC_PT*)g_map[i_new][j_new].point)->k = ((struct OCC_PT*)g_map[i][j].point)->k;
						}
						insert_map_pt(i, j, UNSET);	//get rid of old map point
					}
				}
	
			}
		}
	}
}
Пример #3
0
 /**
  * Calculate distance in meters between two sets of coordinates.
  */
 inline double distance(const osmium::geom::Coordinates& c1, const osmium::geom::Coordinates& c2) {
     double lonh = sin(deg_to_rad(c1.x - c2.x) * 0.5);
     lonh *= lonh;
     double lath = sin(deg_to_rad(c1.y - c2.y) * 0.5);
     lath *= lath;
     const double tmp = cos(deg_to_rad(c1.y)) * cos(deg_to_rad(c2.y));
     return 2.0 * EARTH_RADIUS_IN_METERS * asin(sqrt(lath + tmp*lonh));
 }
Пример #4
0
/*
* EduRaster drawing routines.
*/
static void draw(){
    clear_buffer();
    er_load_identity();
    mat3 rotx, roty;
    rotatex_mat3(rotx, deg_to_rad(camera_phi));
    rotatey_mat3(roty, deg_to_rad(camera_theta));
    vec3 cam_pos = {0.0f, 0.0f, camera_offset};
    mult_mat3_vec3(rotx, cam_pos, cam_pos);
    mult_mat3_vec3(roty, cam_pos, cam_pos);
    vec3 up_vec = {0.0f, 1.0f, 0.0f};
    mult_mat3_vec3(rotx, up_vec, up_vec);
    mult_mat3_vec3(roty, up_vec, up_vec);
    er_look_at(cam_pos[0], cam_pos[1], cam_pos[2], 0.0f, 0.0f, 0.0f, up_vec[0], up_vec[1], up_vec[2]);
    draw_cube(1.5f);
}
Пример #5
0
ETERM *body_update_position(ETERM *fromp, ETERM *argp) {

    // get the args
    ETERM *space_refp = erl_element(1, argp);
    ETERM *idp = erl_element(2, argp);
    ETERM *deltap = erl_element(3, argp);

    erlmunk_space *s = NULL;
    int space_id = ERL_REF_NUMBER(space_refp);
    HASH_FIND_INT(erlmunk_spaces, &space_id, s);

    int body_id = ERL_INT_VALUE(idp);
    erlmunk_body *b = NULL;
    HASH_FIND_INT(s->bodies, &body_id, b);
    if (b == NULL)
        return NULL;

    cpVect position = cpBodyGetPosition(b->body);
    float angle = deg_to_rad(cpBodyGetAngle(b->body));
    cpVect angleV = cpvforangle(angle);
    cpVect projection = cpvmult(angleV, ERL_FLOAT_VALUE(deltap));
    cpVect new_position = cpvadd(projection, position);
    cpBodySetPosition(b->body, new_position);

    // DEBUGF(("body_update_position(x: %f, y: %f, delta: %f) has succeeded (x: %f, y: %f)",
    //     position.x, position.y, ERL_FLOAT_VALUE(deltap),
    //     new_position.x, new_position.y));
    return NULL;
}
Пример #6
0
float *calc_arc(float center[3],double radius,int divisions, int start, int end)
{
	float *points = (float *)mem_malloc(sizeof(float)*(divisions*3));
	int i;
	int j=0;

	int angle = end - start;
	float phi = deg_to_rad(angle);

	double delta = (double)((double)(phi ) / (divisions -1 ));
	double a = 0;

	for (i=0;i<divisions;i++)
	{
		float r[3];
		float result[3];
		vset(r,cos(a),sin(a),0);
		a += delta;

		vmul(r,(float)radius);
		vadd(result,center,r);
		points[j]=result[0];
		points[j+1]=result[1];
		points[j+2]=result[2];
		j+=3;
	}

	return points;
}
Пример #7
0
ETERM *body_apply_impulse(ETERM *fromp, ETERM *argp) {

    // get the args
    ETERM *space_refp = erl_element(1, argp);
    ETERM *idp = erl_element(2, argp);
    ETERM *impulsep = erl_element(3, argp);

    erlmunk_space *s;
    int space_id = ERL_REF_NUMBER(space_refp);
    HASH_FIND_INT(erlmunk_spaces, &space_id, s);

    int body_id = ERL_INT_VALUE(idp);
    erlmunk_body *b;
    HASH_FIND_INT(s->bodies, &body_id, b);
    if (b == NULL)
        return NULL;

    // apply the impulse at the center of the body and along it's current angle
    float angle = deg_to_rad(cpBodyGetAngle(b->body));
    cpVect angleV = cpvforangle(angle);
    cpVect impulse = cpvmult(angleV, ERL_FLOAT_VALUE(impulsep));
    cpBodyApplyImpulseAtWorldPoint(b->body, impulse, angleV);

    return NULL;
}
Пример #8
0
Matrix make_rotation ( Vector const& v_original, double a )
{
	Vector v(v_original);
	
	a = deg_to_rad(a);
	Vector u   = unify(v);
	double c   = cos(a);
	double s   = sin(a);
	double omc = 1-c;
	double ux  = u[0];
	double uy  = u[1];
	double uz  = u[2];
	double ux2 = ux*ux;
	double uy2 = uy*uy;
	double uz2 = uz*uz;
	double uxy = ux*uy;
	double uxz = ux*uz;
	double uyz = uy*uz;
	double uxs = ux*s;
	double uys = uy*s;
	double uzs = uz*s;
	
	double const values[16] = {
		ux2+(1-ux2)*c, uxy*omc-uzs,   uxz*omc+uys,   0.0,
		uxy*omc+uzs,   uy2+(1-uy2)*c, uyz*omc-uxs,   0.0,
		uxz*omc-uys,   uyz*omc+uxs,   uz2+(1-uz2)*c, 0.0,
		0.0,           0.0,           0.0,           1.0
	};
	
	return values;
}
Пример #9
0
  void SimilarityTransform :: computeMatrix()
  {
    m_matrix.create(3,3);
    const double cost = cos(deg_to_rad(m_rotation));
    const double sint = sin(deg_to_rad(m_rotation));
    m_matrix(0,0) = m_scale * cost;
    m_matrix(0,1) = -sint * m_scale;
    m_matrix(0,2) = m_translation[0];

    m_matrix(1,0) = sint * m_scale;
    m_matrix(1,1) = m_scale*cost;
    m_matrix(1,2) = m_translation[1];

    m_matrix(2,0) = 0;
    m_matrix(2,1) = 0;
    m_matrix(2,2) = 1;
  }
Пример #10
0
/*
* EduRaster drawing routines.
*/
static void draw(){
    clear_buffer();
    er_load_identity();
    mat3 rotx, roty;
    rotatex_mat3(rotx, deg_to_rad(camera_phi));
    rotatey_mat3(roty, deg_to_rad(camera_theta));
    vec3 cam_pos = {0.0f, 0.0f, camera_offset};
    mult_mat3_vec3(rotx, cam_pos, cam_pos);
    mult_mat3_vec3(roty, cam_pos, cam_pos);
    vec3 up_vec = {0.0f, 1.0f, 0.0f};
    mult_mat3_vec3(rotx, up_vec, up_vec);
    mult_mat3_vec3(roty, up_vec, up_vec);
    er_look_at(cam_pos[0], cam_pos[1], cam_pos[2], 0.0f, 0.0f, 0.0f, up_vec[0], up_vec[1], up_vec[2]);
    er_use_program(program_axis);
    draw_axis();
    er_use_program(current_surface_program);
    er_draw_elements(ER_TRIANGLES, index_size, index_data);
}
Пример #11
0
void readCompassSensor(){
	
	//~ int compass_reading = 0;//getCompassValue();
	//~ sensor_sensorReadings.positionSensor.compass_direction = 0; //deg_to_rad((double)compass_reading);
	//~ return;
	check_analog_sensors();
	int compass_reading = getCompassValue();
	sensor_sensorReadings.positionSensor.compass_direction = deg_to_rad((double)compass_reading);	
}
Пример #12
0
void update_rpt_xyang(){
	recent_ptset[0].angle = -90 + robot.heading;
	recent_ptset[1].angle = robot.heading;
	recent_ptset[2].angle = 90 + robot.heading;
	recent_ptset[3].angle = 45 + robot.heading;
	recent_ptset[4].angle = -45 + robot.heading;

	for(uint8_t itr = 0; itr < 5; itr++){
		recent_ptset[itr].x = recent_ptset[itr].magnitude * cos(deg_to_rad(recent_ptset[itr].angle)) + robot.x;
	//	rprintf("rxu ");
	}

	for(uint8_t itr = 0; itr < 5; itr++){
		recent_ptset[itr].y = recent_ptset[itr].magnitude * sin(deg_to_rad(recent_ptset[itr].angle)) + robot.y;
	//	rprintf("ryu ");
	}
	//rprintfCRLF();
}
Пример #13
0
        mat4& mat4::setRotationMatrix(const vec3& rotation)
        {
                float x = deg_to_rad(rotation.x);
                float y = deg_to_rad(rotation.y);
                float z = deg_to_rad(rotation.z);

                mat4 rx(1.0f);
		rx.setElement(1, 1, (float)cos(x));	rx.setElement(2, 1,-(float)sin(x));
		rx.setElement(1, 2, (float)sin(x));	rx.setElement(2, 2, (float)cos(x));
                mat4 ry(1.0f);
		ry.setElement(0, 0, (float)cos(y));	ry.setElement(2, 0, (float)sin(y));
		ry.setElement(0, 2,-(float)sin(y));	ry.setElement(2, 2, (float)cos(y));
		mat4 rz(1.0f);
		rz.setElement(0, 0, (float)cos(z));	rz.setElement(1, 0,-(float)sin(z));
		rz.setElement(0, 1, (float)sin(z));	rz.setElement(1, 1, (float)cos(z));

                *this = rz * ry * rx;
                return *this;
        }
Пример #14
0
Point circle_coords(const Point& center, const double radius, const double angle) {
  int x, y;

  double rad_ang = deg_to_rad(angle);

  x = center.x + radius * cos(rad_ang);
  y = center.y + radius * sin(rad_ang);

  return Point{x, y};
}
Пример #15
0
int main() 
{

 double lat_deg,lon_deg,h,var;
 int yy,mm,dd;

 lat_deg=45.0;
 lon_deg=9.0;

 h=      1; // altitude in km
 mm=     10;
 dd=     1;
 yy=     11;


 var=rad_to_deg(SGMagVar(deg_to_rad(lat_deg),deg_to_rad(lon_deg),h, yymmdd_to_julian_days(yy,mm,dd)));

 fprintf(stdout,"var= %4.2f \n",var);

}
Пример #16
0
void
ball_set_direction(struct ball *b, float angle)
{
    float rad;

    rad = deg_to_rad(angle);

    b->angle = angle;
    b->direction[COORD_X] = cos(rad) * b->speed;
    b->direction[COORD_Y] = sin(rad) * b->speed;
}
Пример #17
0
	void updateUniformBuffers()
	{
		computeUbo.deltaT = frameTimer * 5.0f;
		computeUbo.destX = sin(deg_to_rad(timer*360.0)) * 0.75f;
		computeUbo.destY = 0;
		uint8_t *pData;
		VkResult err = vkMapMemory(device, uniformData.computeShader.ubo.memory, 0, sizeof(computeUbo), 0, (void **)&pData);
		assert(!err);
		memcpy(pData, &computeUbo, sizeof(computeUbo));
		vkUnmapMemory(device, uniformData.computeShader.ubo.memory);
	}
Пример #18
0
	void updateUniformBuffers()
	{
		// Vertex shader
		glm::mat4 viewMatrix = glm::mat4();
		uboVS.projection = glm::perspective(deg_to_rad(60.0f), (float)width / (float)height, 0.001f, 256.0f);
		viewMatrix = glm::translate(viewMatrix, glm::vec3(0.0f, 2.0f, zoom));

		float offset = 0.5f;
		int uboIndex = 1;
		uboVS.model = viewMatrix * glm::translate(glm::mat4(), glm::vec3(0, 0, 0));
		uboVS.model = glm::rotate(uboVS.model, deg_to_rad(rotation.x), glm::vec3(1.0f, 0.0f, 0.0f));
		uboVS.model = glm::rotate(uboVS.model, deg_to_rad(rotation.y), glm::vec3(0.0f, 1.0f, 0.0f));
		uboVS.model = glm::rotate(uboVS.model, deg_to_rad(rotation.z), glm::vec3(0.0f, 0.0f, 1.0f));

		uint8_t *pData;
		VkResult err = vkMapMemory(device, uniformData.vertexShader.memory, 0, sizeof(uboVS), 0, (void **)&pData);
		assert(!err);
		memcpy(pData, &uboVS, sizeof(uboVS));
		vkUnmapMemory(device, uniformData.vertexShader.memory);
	}
Пример #19
0
int epos_home(epos_node_p node, double timeout) {
  epos_home_t home;
  epos_home_init(&home,
    config_get_int(&node->config, EPOS_PARAMETER_HOME_METHOD),
    config_get_float(&node->config, EPOS_PARAMETER_HOME_CURRENT),
    deg_to_rad(config_get_float(&node->config, EPOS_PARAMETER_HOME_VELOCITY)),
    deg_to_rad(config_get_float(&node->config, 
      EPOS_PARAMETER_HOME_ACCELERATION)),
    deg_to_rad(config_get_float(&node->config, EPOS_PARAMETER_HOME_POSITION)));
  home.type = config_get_int(&node->config, EPOS_PARAMETER_HOME_TYPE);
  home.offset = deg_to_rad(config_get_float(&node->config, 
    EPOS_PARAMETER_HOME_OFFSET));

  if (!epos_home_start(node, &home)) {
    epos_home_wait(node, timeout);
    return EPOS_ERROR_NONE;
  }
  else
    return EPOS_ERROR_HOME;
}
wxSize CardViewer::DoGetBestSize() const {
	wxSize ws = GetSize(), cs = GetClientSize();
	if (set) {
		if (!stylesheet) stylesheet = set->stylesheet;
		StyleSheetSettings& ss = settings.stylesheetSettingsFor(*stylesheet);
		wxSize size(int(stylesheet->card_width * ss.card_zoom()), int(stylesheet->card_height * ss.card_zoom()));
		if (is_sideways(deg_to_rad(ss.card_angle()))) swap(size.x, size.y);
		return size + ws - cs;
	}
	return cs;
}
Пример #21
0
	void updateUniformBufferMatrices()
	{
		// Only updates the uniform buffer block part containing the global matrices

		// Projection
		uboVS.matrices.projection = glm::perspective(deg_to_rad(60.0f), (float)width / (float)height, 0.001f, 256.0f);

		// View
		uboVS.matrices.view = glm::translate(glm::mat4(), glm::vec3(0.0f, -1.0f, zoom));
		uboVS.matrices.view = glm::rotate(uboVS.matrices.view, deg_to_rad(rotation.x), glm::vec3(1.0f, 0.0f, 0.0f));
		uboVS.matrices.view = glm::rotate(uboVS.matrices.view, deg_to_rad(rotation.y), glm::vec3(0.0f, 1.0f, 0.0f));
		uboVS.matrices.view = glm::rotate(uboVS.matrices.view, deg_to_rad(rotation.z), glm::vec3(0.0f, 0.0f, 1.0f));

		// Only update the matrices part of the uniform buffer
		uint8_t *pData;
		VkResult err = vkMapMemory(device, uniformData.vertexShader.memory, 0, sizeof(uboVS.matrices), 0, (void **)&pData);
		assert(!err);
		memcpy(pData, &uboVS.matrices, sizeof(uboVS.matrices));
		vkUnmapMemory(device, uniformData.vertexShader.memory);
	}
Пример #22
0
	void updateUniformBuffers()
	{
		// Vertex shader
		glm::mat4 viewMatrix = glm::mat4();
		uboVS.projection = glm::perspective(deg_to_rad(60.0f), (float)width / (float)height, 0.1f, 256.0f);
		viewMatrix = glm::translate(viewMatrix, glm::vec3(0.0f, 0.0f, zoom));

		glm::mat4 rotMatrix = glm::mat4();
		rotMatrix = glm::rotate(rotMatrix, deg_to_rad(rotation.x), glm::vec3(1.0f, 0.0f, 0.0f));
		rotMatrix = glm::rotate(rotMatrix, deg_to_rad(rotation.y), glm::vec3(0.0f, 1.0f, 0.0f));
		rotMatrix = glm::rotate(rotMatrix, deg_to_rad(rotation.z), glm::vec3(0.0f, 0.0f, 1.0f));

		uboVS.model = glm::mat4();
		uboVS.model = viewMatrix * rotMatrix;;

		uboVS.visible = 1.0f;

		uint8_t *pData;
		VkResult err = vkMapMemory(device, uniformData.vsScene.memory, 0, sizeof(uboVS), 0, (void **)&pData);
		assert(!err);
		memcpy(pData, &uboVS, sizeof(uboVS));
		vkUnmapMemory(device, uniformData.vsScene.memory);

		// teapot
		// Toggle color depending on visibility
		uboVS.visible = (passedSamples[0] > 0) ? 1.0f : 0.0f;
		uboVS.model = viewMatrix * rotMatrix * glm::translate(glm::mat4(), glm::vec3(0.0f, 0.0f, -10.0f));
		err = vkMapMemory(device, uniformData.teapot.memory, 0, sizeof(uboVS), 0, (void **)&pData);
		assert(!err);
		memcpy(pData, &uboVS, sizeof(uboVS));
		vkUnmapMemory(device, uniformData.teapot.memory);

		// sphere
		// Toggle color depending on visibility
		uboVS.visible = (passedSamples[1] > 0) ? 1.0f : 0.0f;
		uboVS.model = viewMatrix * rotMatrix * glm::translate(glm::mat4(), glm::vec3(0.0f, 0.0f, 10.0f));
		err = vkMapMemory(device, uniformData.sphere.memory, 0, sizeof(uboVS), 0, (void **)&pData);
		assert(!err);
		memcpy(pData, &uboVS, sizeof(uboVS));
		vkUnmapMemory(device, uniformData.sphere.memory);
	}
Пример #23
0
int era_move_home(era_arm_p arm, double vel_factor) {
  int i;
  era_joint_state_t home_state;
  double* home_state_a = (double*)&home_state;
  config_p config_a = (config_p)&arm->config.joints;

  for (i = 0; i < sizeof(era_joint_state_t)/sizeof(double); ++i)
    home_state_a[i] = deg_to_rad(config_get_float(&config_a[i], 
    EPOS_PARAMETER_HOME_POSITION));

  return era_move_joints(arm, &home_state, vel_factor);
}
Пример #24
0
            Coordinates operator()(osmium::Location location) const {
                Coordinates c {location.lon(), location.lat()};

                if (m_epsg != 4326) {
                    c = transform(m_crs_wgs84, m_crs_user, Coordinates(deg_to_rad(location.lon()), deg_to_rad(location.lat())));
                    if (m_crs_user.is_latlong()) {
                        c.x = rad_to_deg(c.x);
                        c.y = rad_to_deg(c.y);
                    }
                }

                return c;
            }
float coordinate_calculation::bearing(const FixedPointCoordinate &first_coordinate,
                                      const FixedPointCoordinate &second_coordinate)
{
    const float lon_diff =
        second_coordinate.lon / COORDINATE_PRECISION - first_coordinate.lon / COORDINATE_PRECISION;
    const float lon_delta = deg_to_rad(lon_diff);
    const float lat1 = deg_to_rad(first_coordinate.lat / COORDINATE_PRECISION);
    const float lat2 = deg_to_rad(second_coordinate.lat / COORDINATE_PRECISION);
    const float y = std::sin(lon_delta) * std::cos(lat2);
    const float x =
        std::cos(lat1) * std::sin(lat2) - std::sin(lat1) * std::cos(lat2) * std::cos(lon_delta);
    float result = rad_to_deg(std::atan2(y, x));
    while (result < 0.f)
    {
        result += 360.f;
    }

    while (result >= 360.f)
    {
        result -= 360.f;
    }
    return result;
}
Пример #26
0
float great_circle_distance(float lat1,float long1,float lat2,float long2)
{
        float delta_long = 0;
        float delta_lat = 0;
        float temp = 0;
        float distance = 0;

        /* Convert all the degrees to radians */
        lat1 = deg_to_rad(lat1);
        long1 = deg_to_rad(long1);
        lat2 = deg_to_rad(lat2);
        long2 = deg_to_rad(long2);

        /* Find the deltas */
        delta_lat = lat2 - lat1;
        delta_long = long2 - long1;

        /* Find the GC distance */
        temp = pow(sin(delta_lat / 2.0), 2) + cos(lat1) * cos(lat2) * pow(sin(delta_long / 2.0), 2);

        distance = EARTH_RADIUS * 2 * atan2(sqrt(temp),sqrt(1 - temp));

        return (distance);
}
Пример #27
0
        mat4& mat4::setPerspectiveMatrix(float fov, float aspectRatio, float far, float near)
        {
                *this = mat4();

                float ar = aspectRatio;
		float thf = tan(deg_to_rad(fov / 2.0f));
		float range = near - far;

                setElement(0, 0, 1.0f / (ar * thf));
		setElement(1, 1, 1.0f / thf);
		setElement(2, 2, (-near - far) / range);
		setElement(3, 2, (2.0f * near * far) / range);
		setElement(2, 3, 1.0f);

                return *this;
        }
Пример #28
0
PosAndOrient Cal3dModel::getPositionForSubmodel(const std::string &bone) const {
  PosAndOrient po;
  po.orient.identity();
  po.pos = WFMath::Vector<3>(0, 0, 0);

  const std::string &mapped_bone = m_core_model->mapBoneName(bone);
  if (mapped_bone.empty()) return po;

  // Get a pointer to the bone we need from cal3d
  CalSkeleton * cs = m_calModel->getSkeleton();
  if (cs == 0) {
    return po;
  }
  CalCoreSkeleton * ccs = cs->getCoreSkeleton();
  if (ccs == 0) {
    return po;
  }
  int boneId = ccs->getCoreBoneId(mapped_bone);
  if (boneId == -1) {
    return po;
  }
  CalBone * cb1 = cs->getBone(boneId);
  if (cb1 == 0) {
    return po;
  }

  // Get the position and orientation of the bone in cal3d coordinates
  const CalQuaternion & cq = cb1->getRotationAbsolute();
  const CalVector & cv = cb1->getTranslationAbsolute();

  // Rotate the orienation into out coordinate system
  WFMath::Quaternion model_rotation(2, deg_to_rad(m_rotate));
  // The second rotation translates object coords to world coords
  // the first rotation makes the coord system compatible
  // The third rotation takes into account the model rotation to make it
  // face the right way. 
  po.orient = WFMath::Quaternion(1, WFMath::Pi / 2.0) * 
    m_core_model->getBoneRotation(bone) *
    WFMath::Quaternion(cq.w, cq.x, cq.y, cq.z).inverse() * 
    model_rotation;
    
  // Rotate the vector into our coordinate system
  po.pos = WFMath::Vector<3>(cv.x, cv.y, cv.z).rotate(model_rotation);
  return po;
}
Пример #29
0
void Space_Object::draw_orbit() {
	
	glPushMatrix();

	float t_x;
	float t_y;

	this->object_orbit.focus_translate(t_x, t_y);;

	glColor3f(0, 1, 0);
	glBegin(GL_LINE_LOOP);

	int i;
	for (i = 0; i<360; i++)
	{
		float rad = deg_to_rad(i);
		// Calculate rotation such that point will lie on new plane defined by orbital normal and the point of the parent.
		GLfloat trans_norm[3];
		GLfloat trans_non_normalizes[3];


		trans_non_normalizes[0] = trans_norm[0] =cos(rad)*this->object_orbit.ellipse_a + t_x;
		trans_non_normalizes[1] = trans_norm[1] = sin(rad)*this->object_orbit.ellipse_b + t_y;
		trans_non_normalizes[2] = trans_norm[2] =  0.0f;
		normalize_vector(trans_norm);

		GLfloat up_norm[] = { 0, 0, 1 };
		GLfloat rot_axis[3];
		vector_cross(up_norm, this->orbit_plane.planeNormal, rot_axis);


		GLfloat rot_angle = asin(vector_length(rot_axis));
		rot_vector((rot_angle), rot_axis[0], rot_axis[1], rot_axis[2], trans_non_normalizes);

		trans_non_normalizes[0] += this->parent_pos[0];
		trans_non_normalizes[1] += this->parent_pos[1];
		trans_non_normalizes[2] += this->parent_pos[2];
		glVertex3fv(trans_non_normalizes);
	}

	glEnd();

	glPopMatrix();
}
Пример #30
0
void rotate_z(double deg){ // rotate z direction
	matrix *R = new matrix();
    double theta = deg_to_rad(deg);
	//init T according to input
    for(int i=0; i<4; i++){
        for(int j=0; j<4; j++){
            R->ele[i][j] = 0;
        }
    }
    R->ele[0][0] = cos(theta);
    R->ele[1][1] = cos(theta);
    R->ele[0][1] = -sin(theta);
    R->ele[1][0] = sin(theta);
    R->ele[2][2] = 1;
    R->ele[3][3] = 1;
	//multipy 
	multiply_top_stack(R);
	delete(R);
}