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