C++ copy_m3_m4函数代码示例

void env_rotate_scene(Render *re, float mat[4][4], int do_rotate)
{
	GroupObject *go;
	ObjectRen *obr;
	ObjectInstanceRen *obi;
	LampRen *lar = NULL;
	HaloRen *har = NULL;
	float imat[3][3], mat_inverse[4][4], smat[4][4], tmat[4][4], cmat[3][3], tmpmat[4][4];
	int a;
	
	if (do_rotate == 0) {
		invert_m4_m4(tmat, mat);
		copy_m3_m4(imat, tmat);
		
		copy_m4_m4(mat_inverse, mat);
	}
	else {
		copy_m4_m4(tmat, mat);
		copy_m3_m4(imat, mat);
		
		invert_m4_m4(mat_inverse, tmat);
	}

	for (obi = re->instancetable.first; obi; obi = obi->next) {
		/* append or set matrix depending on dupli */
		if (obi->flag & R_DUPLI_TRANSFORMED) {
			copy_m4_m4(tmpmat, obi->mat);
			mul_m4_m4m4(obi->mat, tmat, tmpmat);
		}
		else if (do_rotate == 1)
			copy_m4_m4(obi->mat, tmat);
		else
			unit_m4(obi->mat);

		copy_m3_m4(cmat, obi->mat);
		invert_m3_m3(obi->nmat, cmat);
		transpose_m3(obi->nmat);

		/* indicate the renderer has to use transform matrices */
		if (do_rotate == 0)
			obi->flag &= ~R_ENV_TRANSFORMED;
		else {
			obi->flag |= R_ENV_TRANSFORMED;
			copy_m4_m4(obi->imat, mat_inverse);
		}
	}
	

	for (obr = re->objecttable.first; obr; obr = obr->next) {
		for (a = 0; a < obr->tothalo; a++) {
			if ((a & 255) == 0) har = obr->bloha[a >> 8];
			else har++;
		
			mul_m4_v3(tmat, har->co);
		}

		/* imat_ren is needed for correct texture coordinates */
		mul_m4_m4m4(obr->ob->imat_ren, re->viewmat, obr->ob->obmat);
		invert_m4(obr->ob->imat_ren);
	}

/* v3d and rv3d are allowed to be NULL */
void add_primitive_bone(Scene *scene, View3D *v3d, RegionView3D *rv3d)
{
	Object *obedit = scene->obedit; // XXX get from context
	bArmature *arm = obedit->data;
	float obmat[3][3], curs[3], viewmat[3][3], totmat[3][3], imat[3][3];
	EditBone    *bone;

	/* Get inverse point for head and orientation for tail */
	invert_m4_m4(obedit->imat, obedit->obmat);
	mul_v3_m4v3(curs, obedit->imat, give_cursor(scene, v3d));

	if (rv3d && (U.flag & USER_ADD_VIEWALIGNED))
		copy_m3_m4(obmat, rv3d->viewmat);
	else unit_m3(obmat);
	
	copy_m3_m4(viewmat, obedit->obmat);
	mul_m3_m3m3(totmat, obmat, viewmat);
	invert_m3_m3(imat, totmat);
	
	ED_armature_deselect_all(obedit, 0);
	
	/* Create a bone */
	bone = ED_armature_edit_bone_add(arm, "Bone");

	arm->act_edbone = bone;

	copy_v3_v3(bone->head, curs);
	
	if (rv3d && (U.flag & USER_ADD_VIEWALIGNED))
		add_v3_v3v3(bone->tail, bone->head, imat[1]);   // bone with unit length 1
	else
		add_v3_v3v3(bone->tail, bone->head, imat[2]);   // bone with unit length 1, pointing up Z
}

/**
 * \param ray_distance  Distance to the hit point
 * \param r_location  Location of the hit point
 * \param r_normal  Normal of the hit surface, transformed to always face the camera
 */
static bool walk_ray_cast(bContext *C, RegionView3D *rv3d, WalkInfo *walk, float r_location[3], float r_normal[3], float *ray_distance)
{
	float dummy_dist_px = 0;
	float ray_normal[3] = {0, 0, 1}; /* forward */
	float ray_start[3];
	float mat[3][3]; /* 3x3 copy of the view matrix so we can move along the view axis */
	bool ret;

	*ray_distance = TRANSFORM_DIST_MAX_RAY;

	copy_v3_v3(ray_start, rv3d->viewinv[3]);
	copy_m3_m4(mat, rv3d->viewinv);

	mul_m3_v3(mat, ray_normal);

	mul_v3_fl(ray_normal, -1);
	normalize_v3(ray_normal);

	ret = snapObjectsRayEx(CTX_data_scene(C), NULL, NULL, NULL, NULL, SCE_SNAP_MODE_FACE,
	                       NULL, NULL,
	                       ray_start, ray_normal, ray_distance,
	                       NULL, &dummy_dist_px, r_location, r_normal, SNAP_ALL);


	/* dot is positive if both rays are facing the same direction */
	if (dot_v3v3(ray_normal, r_normal) > 0) {
		copy_v3_fl3(r_normal, -r_normal[0], -r_normal[1], -r_normal[2]);
	}

	/* artifically scale the distance to the scene size */
	*ray_distance /= walk->grid;

	return ret;
}

static int object_origin_clear_exec(bContext *C, wmOperator *UNUSED(op))
{
	Main *bmain = CTX_data_main(C);
	float *v1, *v3;
	float mat[3][3];

	CTX_DATA_BEGIN (C, Object *, ob, selected_editable_objects)
	{
		if (ob->parent) {
			/* vectors pointed to by v1 and v3 will get modified */
			v1 = ob->loc;
			v3 = ob->parentinv[3];
			
			copy_m3_m4(mat, ob->parentinv);
			negate_v3_v3(v3, v1);
			mul_m3_v3(mat, v3);
		}

		DAG_id_tag_update(&ob->id, OB_RECALC_OB);
	}
	CTX_DATA_END;

	DAG_ids_flush_update(bmain, 0);
	
	WM_event_add_notifier(C, NC_OBJECT | ND_TRANSFORM, NULL);
	
	return OPERATOR_FINISHED;
}

void setLocalConstraint(TransInfo *t, int mode, const char text[]) {
	if (t->flag & T_EDIT) {
		float obmat[3][3];
		copy_m3_m4(obmat, t->scene->obedit->obmat);
		normalize_m3(obmat);
		setConstraint(t, obmat, mode, text);
	}
	else {
		if (t->total == 1) {
			setConstraint(t, t->data->axismtx, mode, text);
		}
		else {
			strncpy(t->con.text + 1, text, 48);
			copy_m3_m3(t->con.mtx, t->data->axismtx);
			t->con.mode = mode;
			getConstraintMatrix(t);

			startConstraint(t);

			t->con.drawExtra = drawObjectConstraint;
			t->con.applyVec = applyObjectConstraintVec;
			t->con.applySize = applyObjectConstraintSize;
			t->con.applyRot = applyObjectConstraintRot;
			t->redraw = 1;
		}
	}
}

void ED_armature_transform_bones(struct bArmature *arm, float mat[4][4])
{
	EditBone *ebone;
	float scale = mat4_to_scale(mat);   /* store the scale of the matrix here to use on envelopes */
	float mat3[3][3];

	copy_m3_m4(mat3, mat);
	normalize_m3(mat3);
	/* Do the rotations */
	for (ebone = arm->edbo->first; ebone; ebone = ebone->next) {
		float tmat[3][3];
		
		/* find the current bone's roll matrix */
		ED_armature_ebone_to_mat3(ebone, tmat);
		
		/* transform the roll matrix */
		mul_m3_m3m3(tmat, mat3, tmat);
		
		/* transform the bone */
		mul_m4_v3(mat, ebone->head);
		mul_m4_v3(mat, ebone->tail);

		/* apply the transformed roll back */
		mat3_to_vec_roll(tmat, NULL, &ebone->roll);
		
		ebone->rad_head *= scale;
		ebone->rad_tail *= scale;
		ebone->dist     *= scale;
		
		/* we could be smarter and scale by the matrix along the x & z axis */
		ebone->xwidth   *= scale;
		ebone->zwidth   *= scale;
	}
}

bool paintface_minmax(Object *ob, float r_min[3], float r_max[3])
{
	const Mesh *me;
	const MPoly *mp;
	const MLoop *ml;
	const MVert *mvert;
	int a, b;
	bool ok = false;
	float vec[3], bmat[3][3];

	me = BKE_mesh_from_object(ob);
	if (!me || !me->mloopuv) {
		return ok;
	}
	
	copy_m3_m4(bmat, ob->obmat);

	mvert = me->mvert;
	mp = me->mpoly;
	for (a = me->totpoly; a > 0; a--, mp++) {
		if (mp->flag & ME_HIDE || !(mp->flag & ME_FACE_SEL))
			continue;

		ml = me->mloop + mp->totloop;
		for (b = 0; b < mp->totloop; b++, ml++) {
			mul_v3_m3v3(vec, bmat, mvert[ml->v].co);
			add_v3_v3v3(vec, vec, ob->obmat[3]);
			minmax_v3v3_v3(r_min, r_max, vec);
		}

		ok = true;
	}

	return ok;
}

static void rigid_orthogonalize_R(float R[][3])
{
	HMatrix M, Q, S;

	copy_m4_m3(M, R);
	polar_decomp(M, Q, S);
	copy_m3_m4(R, Q);
}

static void init_curve_deform(Object *par, Object *ob, CurveDeform *cd)
{
	invert_m4_m4(ob->imat, ob->obmat);
	mult_m4_m4m4(cd->objectspace, ob->imat, par->obmat);
	invert_m4_m4(cd->curvespace, cd->objectspace);
	copy_m3_m4(cd->objectspace3, cd->objectspace);
	cd->no_rot_axis = 0;
}

static int armature_bone_primitive_add_exec(bContext *C, wmOperator *op) 
{
	RegionView3D *rv3d = CTX_wm_region_view3d(C);
	Object *obedit = CTX_data_edit_object(C);
	EditBone *bone;
	float obmat[3][3], curs[3], viewmat[3][3], totmat[3][3], imat[3][3];
	char name[MAXBONENAME];
	
	RNA_string_get(op->ptr, "name", name);
	
	copy_v3_v3(curs, ED_view3d_cursor3d_get(CTX_data_scene(C), CTX_wm_view3d(C)));

	/* Get inverse point for head and orientation for tail */
	invert_m4_m4(obedit->imat, obedit->obmat);
	mul_m4_v3(obedit->imat, curs);

	if (rv3d && (U.flag & USER_ADD_VIEWALIGNED))
		copy_m3_m4(obmat, rv3d->viewmat);
	else unit_m3(obmat);
	
	copy_m3_m4(viewmat, obedit->obmat);
	mul_m3_m3m3(totmat, obmat, viewmat);
	invert_m3_m3(imat, totmat);
	
	ED_armature_deselect_all(obedit);
	
	/*	Create a bone	*/
	bone = ED_armature_edit_bone_add(obedit->data, name);

	copy_v3_v3(bone->head, curs);
	
	if (rv3d && (U.flag & USER_ADD_VIEWALIGNED))
		add_v3_v3v3(bone->tail, bone->head, imat[1]);   // bone with unit length 1
	else
		add_v3_v3v3(bone->tail, bone->head, imat[2]);   // bone with unit length 1, pointing up Z

	/* note, notifier might evolve */
	WM_event_add_notifier(C, NC_OBJECT | ND_BONE_SELECT, obedit);

	return OPERATOR_FINISHED;
}

void ED_armature_ebone_from_mat4(EditBone *ebone, float mat[4][4])
{
	float mat3[3][3];

	copy_m3_m4(mat3, mat);
	/* We want normalized matrix here, to be consistent with ebone_to_mat. */
	BLI_ASSERT_UNIT_M3(mat3);

	sub_v3_v3(ebone->tail, ebone->head);
	copy_v3_v3(ebone->head, mat[3]);
	add_v3_v3(ebone->tail, mat[3]);
	ED_armature_ebone_from_mat3(ebone, mat3);
}

static float RotationBetween(TransInfo *t, const float p1[3], const float p2[3])
{
	float angle, start[3], end[3];

	sub_v3_v3v3(start, p1, t->center_global);
	sub_v3_v3v3(end,   p2, t->center_global);
		
	// Angle around a constraint axis (error prone, will need debug)
	if (t->con.applyRot != NULL && (t->con.mode & CON_APPLY)) {
		float axis[3], tmp[3];
		
		t->con.applyRot(t, NULL, axis, NULL);

		project_v3_v3v3(tmp, end, axis);
		sub_v3_v3v3(end, end, tmp);
		
		project_v3_v3v3(tmp, start, axis);
		sub_v3_v3v3(start, start, tmp);
		
		normalize_v3(end);
		normalize_v3(start);
		
		cross_v3_v3v3(tmp, start, end);
		
		if (dot_v3v3(tmp, axis) < 0.0f)
			angle = -acosf(dot_v3v3(start, end));
		else
			angle = acosf(dot_v3v3(start, end));
	}
	else {
		float mtx[3][3];
		
		copy_m3_m4(mtx, t->viewmat);

		mul_m3_v3(mtx, end);
		mul_m3_v3(mtx, start);
		
		angle = atan2f(start[1], start[0]) - atan2f(end[1], end[0]);
	}
	
	if (angle > (float)M_PI) {
		angle = angle - 2 * (float)M_PI;
	}
	else if (angle < -((float)M_PI)) {
		angle = 2.0f * (float)M_PI + angle;
	}
	
	return angle;
}

static void camera_frame_fit_data_init(
        const Scene *scene, const Object *ob,
        CameraParams *params, CameraViewFrameData *data)
{
	float camera_rotmat_transposed_inversed[4][4];
	unsigned int i;

	/* setup parameters */
	BKE_camera_params_init(params);
	BKE_camera_params_from_object(params, ob);

	/* compute matrix, viewplane, .. */
	if (scene) {
		BKE_camera_params_compute_viewplane(params, scene->r.xsch, scene->r.ysch, scene->r.xasp, scene->r.yasp);
	}
	else {
		BKE_camera_params_compute_viewplane(params, 1, 1, 1.0f, 1.0f);
	}
	BKE_camera_params_compute_matrix(params);

	/* initialize callback data */
	copy_m3_m4(data->camera_rotmat, (float (*)[4])ob->obmat);
	normalize_m3(data->camera_rotmat);
	/* To transform a plane which is in its homogeneous representation (4d vector),
	 * we need the inverse of the transpose of the transform matrix... */
	copy_m4_m3(camera_rotmat_transposed_inversed, data->camera_rotmat);
	transpose_m4(camera_rotmat_transposed_inversed);
	invert_m4(camera_rotmat_transposed_inversed);

	/* Extract frustum planes from projection matrix. */
	planes_from_projmat(params->winmat,
	                    /*   left              right                 top              bottom        near  far */
	                    data->plane_tx[2], data->plane_tx[0], data->plane_tx[3], data->plane_tx[1], NULL, NULL);

	/* Rotate planes and get normals from them */
	for (i = 0; i < CAMERA_VIEWFRAME_NUM_PLANES; i++) {
		mul_m4_v4(camera_rotmat_transposed_inversed, data->plane_tx[i]);
		normalize_v3_v3(data->normal_tx[i], data->plane_tx[i]);
	}

	copy_v4_fl(data->dist_vals_sq, FLT_MAX);
	data->tot = 0;
	data->is_ortho = params->is_ortho;
	if (params->is_ortho) {
		/* we want (0, 0, -1) transformed by camera_rotmat, this is a quicker shortcut. */
		negate_v3_v3(data->camera_no, data->camera_rotmat[2]);
		data->dist_to_cam = FLT_MAX;
	}
}

static void obmat_to_viewmat(RegionView3D *rv3d, Object *ob)
{
	float bmat[4][4];
	float tmat[3][3];
	
	rv3d->view = RV3D_VIEW_USER; /* don't show the grid */
	
	copy_m4_m4(bmat, ob->obmat);
	normalize_m4(bmat);
	invert_m4_m4(rv3d->viewmat, bmat);
	
	/* view quat calculation, needed for add object */
	copy_m3_m4(tmat, rv3d->viewmat);
	mat3_to_quat(rv3d->viewquat, tmat);
}

void ED_armature_apply_transform(Object *ob, float mat[4][4])
{
	EditBone *ebone;
	bArmature *arm = ob->data;
	float scale = mat4_to_scale(mat);   /* store the scale of the matrix here to use on envelopes */
	float mat3[3][3];
	
	copy_m3_m4(mat3, mat);
	normalize_m3(mat3);
	
	/* Put the armature into editmode */
	ED_armature_to_edit(ob);
	
	/* Do the rotations */
	for (ebone = arm->edbo->first; ebone; ebone = ebone->next) {
		float delta[3], tmat[3][3];
		
		/* find the current bone's roll matrix */
		sub_v3_v3v3(delta, ebone->tail, ebone->head);
		vec_roll_to_mat3(delta, ebone->roll, tmat);
		
		/* transform the roll matrix */
		mul_m3_m3m3(tmat, mat3, tmat);
		
		/* transform the bone */
		mul_m4_v3(mat, ebone->head);
		mul_m4_v3(mat, ebone->tail);
		
		/* apply the transfiormed roll back */
		mat3_to_vec_roll(tmat, NULL, &ebone->roll);
		
		ebone->rad_head *= scale;
		ebone->rad_tail *= scale;
		ebone->dist     *= scale;
		
		/* we could be smarter and scale by the matrix along the x & z axis */
		ebone->xwidth   *= scale;
		ebone->zwidth   *= scale;
	}
	
	/* Turn the list into an armature */
	ED_armature_from_edit(ob);
	ED_armature_edit_free(ob);
}