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); }