void uav3D_init(void) {
uint8_t i = 0;
cam3D_init();
uav3D.state = 3;
uav3D.attr = 0;
VECTOR4D_INITXYZ(&(uav3D.world_pos), 0.0, 0.0, 100.0);
VECTOR4D_ZERO(&(uav3D.dir));
//////////////////////////////////////////////////////////
uav3D.num_vertices = 8;
VECTOR4D_INITXYZ(&(uav3D.vlist_local[0]), 0.0, 12.0, 0.0);
VECTOR4D_INITXYZ(&(uav3D.vlist_local[1]), 5.0, -12.0, 0.0);
VECTOR4D_INITXYZ(&(uav3D.vlist_local[2]), 0.0, -5.0, -5.0);
VECTOR4D_INITXYZ(&(uav3D.vlist_local[3]), -5.0, -12.0, 0.0);
VECTOR4D_INITXYZ(&(uav3D.vlist_local[4]), 10.0, -12.0, 0.0);
VECTOR4D_INITXYZ(&(uav3D.vlist_local[5]), -10.0, -12.0, 0.0);
VECTOR4D_INITXYZ(&(uav3D.vlist_local[6]), 2.0, 0.0, 0.0);
VECTOR4D_INITXYZ(&(uav3D.vlist_local[7]), -2.0, 0.0, 0.0);
for (i = 0; i < uav3D.num_vertices; i++)
{
VECTOR4D_ZERO(&(uav3D.vlist_trans[i]));
}
uav3D.num_polys = 5;
for (i = 0; i < uav3D.num_polys; i++)
{
uav3D.plist[i].state = 1;
uav3D.plist[i].attr = 137;
uav3D.plist[i].color = 1920;
VECTOR4D_COPY(uav3D.plist[i].vlist, uav3D.vlist_local);
}
uav3D.plist[0].vert[0] = 0;
uav3D.plist[0].vert[1] = 1;
uav3D.plist[0].vert[2] = 2;
uav3D.plist[1].vert[0] = 6;
uav3D.plist[1].vert[1] = 4;
uav3D.plist[1].vert[2] = 1;
uav3D.plist[2].vert[0] = 0;
uav3D.plist[2].vert[1] = 2;
uav3D.plist[2].vert[2] = 3;
uav3D.plist[3].vert[0] = 7;
uav3D.plist[3].vert[1] = 3;
uav3D.plist[3].vert[2] = 5;
uav3D.plist[4].vert[0] = 2;
uav3D.plist[4].vert[1] = 1;
uav3D.plist[4].vert[2] = 3;
VECTOR4D v;
//translate the obj
VECTOR4D_INITXYZ(&v, eeprom_buffer.params.Atti_3D_posX - GRAPHICS_X_MIDDLE,
-eeprom_buffer.params.Atti_3D_posY + GRAPHICS_Y_MIDDLE, 0);
Translate_OBJECT4DV1(&uav3D, &v);
//scale the mode
VECTOR4D_INITXYZ(&v, atti_3d_scale, atti_3d_scale, 0);
Scale_OBJECT4DV1(&uav3D, &v);
}
void Init_CAM4D(LPCAM4D cam,
int attr,
LPPOINT4D postion,
LPVECTOR4D direction,
LPPOINT4D target,
float nearClipZ,
float farClipZ,
float fov,
float viewPortWidth,
float viewPortHeight)
{
cam->attr = attr;
VECTOR4D_COPY(&cam->pos, postion);
VECTOR4D_COPY(&cam->dir, direction);
VECTOR4D_INITXYZ(&cam->u, 1, 0, 0);
VECTOR4D_INITXYZ(&cam->v, 0, 1, 0);
VECTOR4D_INITXYZ(&cam->n, 0, 0, 1);
if (target != NULL)
VECTOR4D_COPY(&cam->target, target);
else
VECTOR4D_ZERO(&cam->target);
cam->nearClipZ = nearClipZ;
cam->farClipZ = farClipZ;
cam->viewPortWidth = viewPortWidth;
cam->viewPortHeight = viewPortHeight;
cam->viewPortCenterX = (viewPortWidth - 1) / 2;
cam->viewPortCenterY = (viewPortHeight - 1) / 2;
cam->aspectRatio = (float)viewPortWidth / (float)viewPortHeight;
MATRIX_IDENTITY_4X4(&cam->mWordToCam);
MATRIX_IDENTITY_4X4(&cam->mCamToPer);
MATRIX_IDENTITY_4X4(&cam->mPerToScreen);
cam->fov = fov;
cam->viewPlaneWidth = 2.0;
cam->viewPlaneHeight = 2.0 / cam->aspectRatio;
cam->viewDistance = 0.5 * (cam->viewPlaneWidth) / tan(DEG_TO_RAD(fov / 2));
if (fov == 90)
{
VECTOR3D vn;
POINT3D ptOrigin;
VECTOR3D_INITXYZ(&ptOrigin, 0, 0, 0);
VECTOR3D_INITXYZ(&vn, 1, 0, -1);
Plane3D_Init(&cam->rightClipPlane, &ptOrigin, &vn, 1); //x=z plane
VECTOR3D_INITXYZ(&vn, -1, 0, -1);
Plane3D_Init(&cam->leftClipPlane, &ptOrigin, &vn ,1); //-x=z plane
VECTOR3D_INITXYZ(&vn, 0, 1, -1);
Plane3D_Init(&cam->topClipPlane, &ptOrigin, &vn ,1); //y=z plane
VECTOR3D_INITXYZ(&vn, 0, -1, -1);
Plane3D_Init(&cam->bottomClipPlane, &ptOrigin, &vn ,1); //-y=z plane
}
else
{
VECTOR3D vn;
POINT3D ptOrigin;
VECTOR3D_INITXYZ(&ptOrigin, 0, 0, 0);
VECTOR3D_INITXYZ(&vn, cam->viewDistance, 0, -cam->viewPlaneWidth / 2);
Plane3D_Init(&cam->rightClipPlane, &ptOrigin, &vn, 1);
VECTOR3D_INITXYZ(&vn, -cam->viewDistance, 0, -cam->viewPlaneWidth / 2);
Plane3D_Init(&cam->leftClipPlane, &ptOrigin, &vn, 1);
VECTOR3D_INITXYZ(&vn, 0, cam->viewDistance, -cam->viewPlaneWidth / 2);
Plane3D_Init(&cam->topClipPlane, &ptOrigin, &vn, 1);
VECTOR3D_INITXYZ(&vn, 0, -cam->viewDistance, -cam->viewPlaneWidth / 2);
Plane3D_Init(&cam->bottomClipPlane, &ptOrigin, &vn, 1);
}
}
void Init_CAM4DV1(CAM4DV1_PTR cam, int cam_attr, POINT4D_PTR cam_pos, VECTOR4D_PTR cam_dir
, POINT4D_PTR cam_target, float near_clip_z, float far_clip_z, float fov, float viewport_width, float viewport_height)
{
cam->attr = cam_attr;
VECTOR4D_COPY(&cam->pos, cam_pos);
VECTOR4D_COPY(&cam->dir, cam_dir);
VECTOR4D_INITXYZ(&cam->u, 1.f, .0f, .0f);
VECTOR4D_INITXYZ(&cam->v, 0.f, 1.f, .0f);
VECTOR4D_INITXYZ(&cam->n, .0f, .0f, 1.f);
if (cam_pos != NULL)
VECTOR4D_COPY(&cam->target, cam_target);
else
VECTOR4D_ZERO(&cam->target);
cam->near_clip_z = near_clip_z;
cam->far_clip_z = far_clip_z;
cam->viewport_width = viewport_width;
cam->viewport_height = viewport_height;
cam->viewport_center_x = (viewport_width - 1) / 2;
cam->viewport_center_y = (viewport_height - 1) / 2;
cam->aspect_ratio = (float)viewport_width / (float)viewport_height;
MAT_IDENTITY_4X4(&cam->mcam);
MAT_IDENTITY_4X4(&cam->mper);
MAT_IDENTITY_4X4(&cam->mscr);
cam->fov = fov;
cam->viewplane_width = 2;
cam->viewplane_height = 2 / cam->aspect_ratio;
float tan_fov_div2 = tan(DEG_TO_RAD(fov / 2));
//cam->view_dist = 0.5f * cam->viewplane_width * tan_fov_div2;
cam->view_dist = 0.5f * cam->viewplane_width / tan_fov_div2;
cam->viewport_dist = 0.5f * cam->viewport_width / tan_fov_div2;
if (fov == 90.0)
{
POINT3D pt_origin;
VECTOR3D_INITXYZ(&pt_origin, 0, 0, 0);
VECTOR3D vn;
//ÓҲüôÃæ
VECTOR3D_INITXYZ(&vn, 1, 0, 11);
PLANE3D_Init(&cam->rt_clip_plane, &pt_origin, &vn, 1);
//×ó²Ã¼ôÃæ
VECTOR3D_INITXYZ(&vn, -1, 0, 1);
PLANE3D_Init(&cam->lt_clip_plane, &pt_origin, &vn, 1);
//ÉϲüôÃæ
VECTOR3D_INITXYZ(&vn, 0, 1, 1);
PLANE3D_Init(&cam->tp_clip_plane, &pt_origin, &vn, 1);
//ϲüôÃæ
VECTOR3D_INITXYZ(&vn, 0, -1, 1);
PLANE3D_Init(&cam->bt_clip_plane, &pt_origin, &vn, 1);
}
else
{
POINT3D pt_origin;
VECTOR3D_INITXYZ(&pt_origin, 0, 0, 0);
VECTOR3D vn;
VECTOR3D_INITXYZ(&vn, cam->view_dist, 0, cam->viewplane_width / 2.0f);
PLANE3D_Init(&cam->rt_clip_plane, &pt_origin, &vn, 1);
VECTOR3D_INITXYZ(&vn, -cam->view_dist, 0, cam->viewplane_width / 2.0f);
PLANE3D_Init(&cam->lt_clip_plane, &pt_origin, &vn, 1);
VECTOR3D_INITXYZ(&vn, 0, cam->view_dist, cam->viewplane_width / 2.0f);
PLANE3D_Init(&cam->rt_clip_plane, &pt_origin, &vn, 1);
VECTOR3D_INITXYZ(&vn, 0, -cam->view_dist, cam->viewplane_width / 2.0f);
PLANE3D_Init(&cam->rt_clip_plane, &pt_origin, &vn, 1);
}
}
