コード例 #1
0
ファイル: GLCamera.cpp プロジェクト: MoraxAlvizo/ProyectoADA
// returns a view matrix using the opengl lookAt style. COLUMN ORDER.
void GLCamera::look_at(const vector3& pos, vector3 targ_pos, const vector3& up_v) {

	this->cam_pos = pos; 

	// inverse translation
	matriz4x4 p = identity_mat4();
	p = p.translate(vector3(-pos.v[0], -pos.v[1], -pos.v[2]));
	// distance vector
	vector3 d = targ_pos - pos;
	// forward vector
	vector3 f = d.normalise();
	// right vector
	vector3 r = cross(f, up_v).normalise();
	// real up vector
	vector3 u = cross(r, f).normalise();
	matriz4x4 ori = identity_mat4();
	ori.m[0] = r.v[0];
	ori.m[4] = r.v[1];
	ori.m[8] = r.v[2];
	ori.m[1] = u.v[0];
	ori.m[5] = u.v[1];
	ori.m[9] = u.v[2];
	ori.m[2] = -f.v[0];
	ori.m[6] = -f.v[1];
	ori.m[10] = -f.v[2];
	R = ori;
	T = p;
	view_mat = R * T;

	// recalc axes to suit new orientation
	mat4_to_quat(quaternion, R.m);
	fwd = R * vector4(0.0, 0.0, -1.0, 0.0);
	rgt = R * vector4(1.0, 0.0, 0.0, 0.0);
	up  = R * vector4(0.0, 1.0, 0.0, 0.0);
}
コード例 #2
0
void TransformBase::decompose(float mat[4][4], float *loc, float eul[3], float quat[4], float *size)
{
	mat4_to_size(size, mat);
	if (eul) {
		mat4_to_eul(eul, mat);
	}
	if (quat) {
		mat4_to_quat(quat, mat);
	}
	copy_v3_v3(loc, mat[3]);
}
コード例 #3
0
ファイル: mball.c プロジェクト: mcgrathd/blender
void BKE_mball_transform(MetaBall *mb, float mat[4][4])
{
    MetaElem *me;
    float quat[4];
    const float scale = mat4_to_scale(mat);
    const float scale_sqrt = sqrtf(scale);

    mat4_to_quat(quat, mat);

    for (me = mb->elems.first; me; me = me->next) {
        mul_m4_v3(mat, &me->x);
        mul_qt_qtqt(me->quat, quat, me->quat);
        me->rad *= scale;
        /* hrmf, probably elems shouldn't be
         * treating scale differently - campbell */
        if (!MB_TYPE_SIZE_SQUARED(me->type)) {
            mul_v3_fl(&me->expx, scale);
        }
        else {
            mul_v3_fl(&me->expx, scale_sqrt);
        }
    }
}
コード例 #4
0
ファイル: animation.c プロジェクト: ghosthamlet/Corange
animation* ani_load_file(char* filename) {
  
  int state = STATE_LOAD_EMPTY;
  
  animation* a =  animation_new();
  skeleton* base = skeleton_new();
  frame* f = NULL;
  
  SDL_RWops* file = SDL_RWFromFile(filename, "r");
  
  if(file == NULL) {
    error("Could not load file %s", filename);
  }
  
  char line[1024];
  while(SDL_RWreadline(file, line, 1024)) {
    
    if (state == STATE_LOAD_EMPTY) {
      
      int version;
      if (sscanf(line, "version %i", &version) > 0) {
        if (version != 1) {
          error("Can't load ani file '%s'. Don't know how to load version %i\n", filename, version);
        }
      }
      
      if (strstr(line, "nodes")) {
        state = STATE_LOAD_NODES;
      }
      
      if (strstr(line, "skeleton")) {
        state = STATE_LOAD_SKELETON;
      }
    }
    
    else if (state == STATE_LOAD_NODES) {
      char name[1024];
      int id, parent;
      if (sscanf(line, "%i \"%[^\"]\" %i", &id, name, &parent) == 3) {
        skeleton_joint_add(base, name, parent);
      }
      
      if (strstr(line, "end")) {
        state = STATE_LOAD_EMPTY;
      }
    }
    
    else if (state == STATE_LOAD_SKELETON) {
    
      float time;
      if (sscanf(line, "time %f", &time) == 1) {
        f = animation_add_frame(a, base->rest_pose);
      }
    
      int id;
      float x, y, z, rx, ry, rz;
      if (sscanf(line, "%i %f %f %f %f %f %f", &id, &x, &y, &z, &rx, &ry, &rz) > 0) {
        
        f->joint_positions[id] = vec3_new(x, z, y);
        
        mat4 rotation = mat4_rotation_euler(rx, ry, rz);
        mat4 handedflip = mat4_new(1,0,0,0,
                                   0,0,1,0,
                                   0,1,0,0,
                                   0,0,0,1);
      
        rotation = mat4_mul_mat4(handedflip, rotation);
        rotation = mat4_mul_mat4(rotation, handedflip);
        rotation = mat4_transpose(rotation);
        
        f->joint_rotations[id] = mat4_to_quat(rotation);
        
      }
      
      if (strstr(line, "end")) {
        state = STATE_LOAD_EMPTY;
      }
      
    }
  }
  
  SDL_RWclose(file);
  
  skeleton_delete(base);
  
  return a;
}
コード例 #5
0
/* called from within the core BKE_pose_where_is loop, all animsystems and constraints
 * were executed & assigned. Now as last we do an IK pass */
static void execute_posetree(struct Scene *scene, Object *ob, PoseTree *tree)
{
	float R_parmat[3][3], identity[3][3];
	float iR_parmat[3][3];
	float R_bonemat[3][3];
	float goalrot[3][3], goalpos[3];
	float rootmat[4][4], imat[4][4];
	float goal[4][4], goalinv[4][4];
	float irest_basis[3][3], full_basis[3][3];
	float end_pose[4][4], world_pose[4][4];
	float length, basis[3][3], rest_basis[3][3], start[3], *ikstretch = NULL;
	float resultinf = 0.0f;
	int a, flag, hasstretch = 0, resultblend = 0;
	bPoseChannel *pchan;
	IK_Segment *seg, *parent, **iktree, *iktarget;
	IK_Solver *solver;
	PoseTarget *target;
	bKinematicConstraint *data, *poleangledata = NULL;
	Bone *bone;

	if (tree->totchannel == 0)
		return;

	iktree = MEM_mallocN(sizeof(void *) * tree->totchannel, "ik tree");

	for (a = 0; a < tree->totchannel; a++) {
		pchan = tree->pchan[a];
		bone = pchan->bone;

		/* set DoF flag */
		flag = 0;
		if (!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP))
			flag |= IK_XDOF;
		if (!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP))
			flag |= IK_YDOF;
		if (!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP))
			flag |= IK_ZDOF;

		if (tree->stretch && (pchan->ikstretch > 0.0f)) {
			flag |= IK_TRANS_YDOF;
			hasstretch = 1;
		}

		seg = iktree[a] = IK_CreateSegment(flag);

		/* find parent */
		if (a == 0)
			parent = NULL;
		else
			parent = iktree[tree->parent[a]];

		IK_SetParent(seg, parent);

		/* get the matrix that transforms from prevbone into this bone */
		copy_m3_m4(R_bonemat, pchan->pose_mat);

		/* gather transformations for this IK segment */

		if (pchan->parent)
			copy_m3_m4(R_parmat, pchan->parent->pose_mat);
		else
			unit_m3(R_parmat);

		/* bone offset */
		if (pchan->parent && (a > 0))
			sub_v3_v3v3(start, pchan->pose_head, pchan->parent->pose_tail);
		else
			/* only root bone (a = 0) has no parent */
			start[0] = start[1] = start[2] = 0.0f;

		/* change length based on bone size */
		length = bone->length * len_v3(R_bonemat[1]);

		/* compute rest basis and its inverse */
		copy_m3_m3(rest_basis, bone->bone_mat);
		copy_m3_m3(irest_basis, bone->bone_mat);
		transpose_m3(irest_basis);

		/* compute basis with rest_basis removed */
		invert_m3_m3(iR_parmat, R_parmat);
		mul_m3_m3m3(full_basis, iR_parmat, R_bonemat);
		mul_m3_m3m3(basis, irest_basis, full_basis);

		/* basis must be pure rotation */
		normalize_m3(basis);

		/* transform offset into local bone space */
		normalize_m3(iR_parmat);
		mul_m3_v3(iR_parmat, start);

		IK_SetTransform(seg, start, rest_basis, basis, length);

		if (pchan->ikflag & BONE_IK_XLIMIT)
			IK_SetLimit(seg, IK_X, pchan->limitmin[0], pchan->limitmax[0]);
		if (pchan->ikflag & BONE_IK_YLIMIT)
			IK_SetLimit(seg, IK_Y, pchan->limitmin[1], pchan->limitmax[1]);
		if (pchan->ikflag & BONE_IK_ZLIMIT)
			IK_SetLimit(seg, IK_Z, pchan->limitmin[2], pchan->limitmax[2]);

		IK_SetStiffness(seg, IK_X, pchan->stiffness[0]);
		IK_SetStiffness(seg, IK_Y, pchan->stiffness[1]);
		IK_SetStiffness(seg, IK_Z, pchan->stiffness[2]);

		if (tree->stretch && (pchan->ikstretch > 0.0f)) {
			const float ikstretch = pchan->ikstretch * pchan->ikstretch;
			/* this function does its own clamping */
			IK_SetStiffness(seg, IK_TRANS_Y, 1.0f - ikstretch);
			IK_SetLimit(seg, IK_TRANS_Y, IK_STRETCH_STIFF_MIN, IK_STRETCH_STIFF_MAX);
		}
	}

	solver = IK_CreateSolver(iktree[0]);

	/* set solver goals */

	/* first set the goal inverse transform, assuming the root of tree was done ok! */
	pchan = tree->pchan[0];
	if (pchan->parent) {
		/* transform goal by parent mat, so this rotation is not part of the
		 * segment's basis. otherwise rotation limits do not work on the
		 * local transform of the segment itself. */
		copy_m4_m4(rootmat, pchan->parent->pose_mat);
		/* However, we do not want to get (i.e. reverse) parent's scale, as it generates [#31008]
		 * kind of nasty bugs... */
		normalize_m4(rootmat);
	}
	else
		unit_m4(rootmat);
	copy_v3_v3(rootmat[3], pchan->pose_head);

	mul_m4_m4m4(imat, ob->obmat, rootmat);
	invert_m4_m4(goalinv, imat);

	for (target = tree->targets.first; target; target = target->next) {
		float polepos[3];
		int poleconstrain = 0;

		data = (bKinematicConstraint *)target->con->data;

		/* 1.0=ctime, we pass on object for auto-ik (owner-type here is object, even though
		 * strictly speaking, it is a posechannel)
		 */
		BKE_constraint_target_matrix_get(scene, target->con, 0, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);

		/* and set and transform goal */
		mul_m4_m4m4(goal, goalinv, rootmat);

		copy_v3_v3(goalpos, goal[3]);
		copy_m3_m4(goalrot, goal);
		normalize_m3(goalrot);

		/* same for pole vector target */
		if (data->poletar) {
			BKE_constraint_target_matrix_get(scene, target->con, 1, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);

			if (data->flag & CONSTRAINT_IK_SETANGLE) {
				/* don't solve IK when we are setting the pole angle */
				break;
			}
			else {
				mul_m4_m4m4(goal, goalinv, rootmat);
				copy_v3_v3(polepos, goal[3]);
				poleconstrain = 1;

				/* for pole targets, we blend the result of the ik solver
				 * instead of the target position, otherwise we can't get
				 * a smooth transition */
				resultblend = 1;
				resultinf = target->con->enforce;

				if (data->flag & CONSTRAINT_IK_GETANGLE) {
					poleangledata = data;
					data->flag &= ~CONSTRAINT_IK_GETANGLE;
				}
			}
		}

		/* do we need blending? */
		if (!resultblend && target->con->enforce != 1.0f) {
			float q1[4], q2[4], q[4];
			float fac = target->con->enforce;
			float mfac = 1.0f - fac;

			pchan = tree->pchan[target->tip];

			/* end effector in world space */
			copy_m4_m4(end_pose, pchan->pose_mat);
			copy_v3_v3(end_pose[3], pchan->pose_tail);
			mul_serie_m4(world_pose, goalinv, ob->obmat, end_pose, NULL, NULL, NULL, NULL, NULL);

			/* blend position */
			goalpos[0] = fac * goalpos[0] + mfac * world_pose[3][0];
			goalpos[1] = fac * goalpos[1] + mfac * world_pose[3][1];
			goalpos[2] = fac * goalpos[2] + mfac * world_pose[3][2];

			/* blend rotation */
			mat3_to_quat(q1, goalrot);
			mat4_to_quat(q2, world_pose);
			interp_qt_qtqt(q, q1, q2, mfac);
			quat_to_mat3(goalrot, q);
		}

		iktarget = iktree[target->tip];

		if ((data->flag & CONSTRAINT_IK_POS) && data->weight != 0.0f) {
			if (poleconstrain)
				IK_SolverSetPoleVectorConstraint(solver, iktarget, goalpos,
				                                 polepos, data->poleangle, (poleangledata == data));
			IK_SolverAddGoal(solver, iktarget, goalpos, data->weight);
		}
		if ((data->flag & CONSTRAINT_IK_ROT) && (data->orientweight != 0.0f))
			if ((data->flag & CONSTRAINT_IK_AUTO) == 0)
				IK_SolverAddGoalOrientation(solver, iktarget, goalrot,
				                            data->orientweight);
	}

	/* solve */
	IK_Solve(solver, 0.0f, tree->iterations);

	if (poleangledata)
		poleangledata->poleangle = IK_SolverGetPoleAngle(solver);

	IK_FreeSolver(solver);

	/* gather basis changes */
	tree->basis_change = MEM_mallocN(sizeof(float[3][3]) * tree->totchannel, "ik basis change");
	if (hasstretch)
		ikstretch = MEM_mallocN(sizeof(float) * tree->totchannel, "ik stretch");

	for (a = 0; a < tree->totchannel; a++) {
		IK_GetBasisChange(iktree[a], tree->basis_change[a]);

		if (hasstretch) {
			/* have to compensate for scaling received from parent */
			float parentstretch, stretch;

			pchan = tree->pchan[a];
			parentstretch = (tree->parent[a] >= 0) ? ikstretch[tree->parent[a]] : 1.0f;

			if (tree->stretch && (pchan->ikstretch > 0.0f)) {
				float trans[3], length;

				IK_GetTranslationChange(iktree[a], trans);
				length = pchan->bone->length * len_v3(pchan->pose_mat[1]);

				ikstretch[a] = (length == 0.0f) ? 1.0f : (trans[1] + length) / length;
			}
			else
				ikstretch[a] = 1.0;

			stretch = (parentstretch == 0.0f) ? 1.0f : ikstretch[a] / parentstretch;

			mul_v3_fl(tree->basis_change[a][0], stretch);
			mul_v3_fl(tree->basis_change[a][1], stretch);
			mul_v3_fl(tree->basis_change[a][2], stretch);
		}

		if (resultblend && resultinf != 1.0f) {
			unit_m3(identity);
			blend_m3_m3m3(tree->basis_change[a], identity,
			              tree->basis_change[a], resultinf);
		}

		IK_FreeSegment(iktree[a]);
	}

	MEM_freeN(iktree);
	if (ikstretch) MEM_freeN(ikstretch);
}