/* Used when canceling transforms - return rigidbody and object to initial states */
void BKE_rigidbody_aftertrans_update(Object *ob, float loc[3], float rot[3], float quat[4], float rotAxis[3], float rotAngle)
{
	RigidBodyOb *rbo = ob->rigidbody_object;

	/* return rigid body and object to their initial states */
	copy_v3_v3(rbo->pos, ob->loc);
	copy_v3_v3(ob->loc, loc);

	if (ob->rotmode > 0) {
		eulO_to_quat(rbo->orn, ob->rot, ob->rotmode);
		copy_v3_v3(ob->rot, rot);
	}
	else if (ob->rotmode == ROT_MODE_AXISANGLE) {
		axis_angle_to_quat(rbo->orn, ob->rotAxis, ob->rotAngle);
		copy_v3_v3(ob->rotAxis, rotAxis);
		ob->rotAngle = rotAngle;
	}
	else {
		copy_qt_qt(rbo->orn, ob->quat);
		copy_qt_qt(ob->quat, quat);
	}
	if (rbo->physics_object) {
		/* allow passive objects to return to original transform */
		if (rbo->type == RBO_TYPE_PASSIVE)
			RB_body_set_kinematic_state(rbo->physics_object, TRUE);
		RB_body_set_loc_rot(rbo->physics_object, rbo->pos, rbo->orn);
	}
	// RB_TODO update rigid body physics object's loc/rot for dynamic objects here as well (needs to be done outside bullet's update loop)
}

/* Only allowed for Poses with identical channels */
static void game_blend_poses(bPose *dst, bPose *src, float srcweight, short mode)
{
	bPoseChannel *dchan;
	const bPoseChannel *schan;
	bConstraint *dcon, *scon;
	float dstweight;
	int i;

	if (mode == BL_Action::ACT_BLEND_BLEND)
	{
		dstweight = 1.0f - srcweight;
	} else if (mode == BL_Action::ACT_BLEND_ADD)
	{
		dstweight = 1.0f;
	} else {
		dstweight = 1.0f;
	}
	
	schan= (bPoseChannel *)src->chanbase.first;
	for (dchan = (bPoseChannel *)dst->chanbase.first; dchan; dchan=(bPoseChannel *)dchan->next, schan= (bPoseChannel *)schan->next) {
		// always blend on all channels since we don't know which one has been set
		/* quat interpolation done separate */
		if (schan->rotmode == ROT_MODE_QUAT) {
			float dquat[4], squat[4];
			
			copy_qt_qt(dquat, dchan->quat);
			copy_qt_qt(squat, schan->quat);
			if (mode==BL_Action::ACT_BLEND_BLEND)
				interp_qt_qtqt(dchan->quat, dquat, squat, srcweight);
			else {
				mul_fac_qt_fl(squat, srcweight);
				mul_qt_qtqt(dchan->quat, dquat, squat);
			}
			
			normalize_qt(dchan->quat);
		}

		for (i=0; i<3; i++) {
			/* blending for loc and scale are pretty self-explanatory... */
			dchan->loc[i] = (dchan->loc[i]*dstweight) + (schan->loc[i]*srcweight);
			dchan->size[i] = 1.0f + ((dchan->size[i]-1.0f)*dstweight) + ((schan->size[i]-1.0f)*srcweight);
			
			/* euler-rotation interpolation done here instead... */
			// FIXME: are these results decent?
			if (schan->rotmode)
				dchan->eul[i] = (dchan->eul[i]*dstweight) + (schan->eul[i]*srcweight);
		}
		for (dcon= (bConstraint *)dchan->constraints.first, scon= (bConstraint *)schan->constraints.first;
		     dcon && scon;
		     dcon = dcon->next, scon = scon->next)
		{
			/* no 'add' option for constraint blending */
			dcon->enforce= dcon->enforce*(1.0f-srcweight) + scon->enforce*srcweight;
		}
	}
	
	/* this pose is now in src time */
	dst->ctime= src->ctime;
}

/* this makes sure we can extend for non-cyclic.
 *
 * returns OK: 1/0
 */
static bool where_on_path_deform(
    Object *ob, float ctime, float vec[4], float dir[3], float quat[4], float *radius)
{
  BevList *bl;
  float ctime1;
  int cycl = 0;

  /* test for cyclic */
  bl = ob->runtime.curve_cache->bev.first;
  if (!bl->nr) {
    return false;
  }
  if (bl->poly > -1) {
    cycl = 1;
  }

  if (cycl == 0) {
    ctime1 = CLAMPIS(ctime, 0.0f, 1.0f);
  }
  else {
    ctime1 = ctime;
  }

  /* vec needs 4 items */
  if (where_on_path(ob, ctime1, vec, dir, quat, radius, NULL)) {

    if (cycl == 0) {
      Path *path = ob->runtime.curve_cache->path;
      float dvec[3];

      if (ctime < 0.0f) {
        sub_v3_v3v3(dvec, path->data[1].vec, path->data[0].vec);
        mul_v3_fl(dvec, ctime * (float)path->len);
        add_v3_v3(vec, dvec);
        if (quat) {
          copy_qt_qt(quat, path->data[0].quat);
        }
        if (radius) {
          *radius = path->data[0].radius;
        }
      }
      else if (ctime > 1.0f) {
        sub_v3_v3v3(dvec, path->data[path->len - 1].vec, path->data[path->len - 2].vec);
        mul_v3_fl(dvec, (ctime - 1.0f) * (float)path->len);
        add_v3_v3(vec, dvec);
        if (quat) {
          copy_qt_qt(quat, path->data[path->len - 1].quat);
        }
        if (radius) {
          *radius = path->data[path->len - 1].radius;
        }
        /* weight - not used but could be added */
      }
    }
    return true;
  }
  return false;
}

static void copy_pose_channel_data(bPoseChannel *pchan, const bPoseChannel *chan)
{
	bConstraint *pcon, *con;
	
	copy_v3_v3(pchan->loc, chan->loc);
	copy_v3_v3(pchan->size, chan->size);
	copy_v3_v3(pchan->eul, chan->eul);
	copy_v3_v3(pchan->rotAxis, chan->rotAxis);
	pchan->rotAngle = chan->rotAngle;
	copy_qt_qt(pchan->quat, chan->quat);
	pchan->rotmode = chan->rotmode;
	copy_m4_m4(pchan->chan_mat, (float(*)[4])chan->chan_mat);
	copy_m4_m4(pchan->pose_mat, (float(*)[4])chan->pose_mat);
	pchan->flag = chan->flag;
	
	pchan->roll1 = chan->roll1;
	pchan->roll2 = chan->roll2;
	pchan->curveInX = chan->curveInX;
	pchan->curveInY = chan->curveInY;
	pchan->curveOutX = chan->curveOutX;
	pchan->curveOutY = chan->curveOutY;
	pchan->scaleIn = chan->scaleIn;
	pchan->scaleOut = chan->scaleOut;
	
	con = chan->constraints.first;
	for (pcon = pchan->constraints.first; pcon && con; pcon = pcon->next, con = con->next) {
		pcon->enforce = con->enforce;
		pcon->headtail = con->headtail;
	}
}

static void view3d_smooth_view_state_restore(const struct SmoothView3DState *sms_state,
                                             View3D *v3d, RegionView3D *rv3d)
{
	copy_v3_v3(rv3d->ofs,      sms_state->ofs);
	copy_qt_qt(rv3d->viewquat, sms_state->quat);
	rv3d->dist               = sms_state->dist;
	v3d->lens                = sms_state->lens;
}

static void view3d_smooth_view_state_backup(struct SmoothView3DState *sms_state,
                                            const View3D *v3d, const RegionView3D *rv3d)
{
	copy_v3_v3(sms_state->ofs,   rv3d->ofs);
	copy_qt_qt(sms_state->quat,  rv3d->viewquat);
	sms_state->dist            = rv3d->dist;
	sms_state->lens            = v3d->lens;
}

/* helper for poseAnim_mapping_get() -> get the relevant F-Curves per PoseChannel */
static void fcurves_to_pchan_links_get(ListBase *pfLinks, Object *ob, bAction *act, bPoseChannel *pchan)
{
	ListBase curves = {NULL, NULL};
	int transFlags = action_get_item_transforms(act, ob, pchan, &curves);
	
	pchan->flag &= ~(POSE_LOC | POSE_ROT | POSE_SIZE | POSE_BBONE_SHAPE);
	
	/* check if any transforms found... */
	if (transFlags) {
		/* make new linkage data */
		tPChanFCurveLink *pfl = MEM_callocN(sizeof(tPChanFCurveLink), "tPChanFCurveLink");
		PointerRNA ptr;
		
		pfl->fcurves = curves;
		pfl->pchan = pchan;
		
		/* get the RNA path to this pchan - this needs to be freed! */
		RNA_pointer_create((ID *)ob, &RNA_PoseBone, pchan, &ptr);
		pfl->pchan_path = RNA_path_from_ID_to_struct(&ptr);
		
		/* add linkage data to operator data */
		BLI_addtail(pfLinks, pfl);
		
		/* set pchan's transform flags */
		if (transFlags & ACT_TRANS_LOC)
			pchan->flag |= POSE_LOC;
		if (transFlags & ACT_TRANS_ROT)
			pchan->flag |= POSE_ROT;
		if (transFlags & ACT_TRANS_SCALE)
			pchan->flag |= POSE_SIZE;
		if (transFlags & ACT_TRANS_BBONE)
			pchan->flag |= POSE_BBONE_SHAPE;
			
		/* store current transforms */
		copy_v3_v3(pfl->oldloc, pchan->loc);
		copy_v3_v3(pfl->oldrot, pchan->eul);
		copy_v3_v3(pfl->oldscale, pchan->size);
		copy_qt_qt(pfl->oldquat, pchan->quat);
		copy_v3_v3(pfl->oldaxis, pchan->rotAxis);
		pfl->oldangle = pchan->rotAngle;
		
		/* store current bbone values */
		pfl->roll1 = pchan->roll1;
		pfl->roll2 = pchan->roll2;
		pfl->curveInX = pchan->curveInX;
		pfl->curveInY = pchan->curveInY;
		pfl->curveOutX = pchan->curveOutX;
		pfl->curveOutY = pchan->curveOutY;
		pfl->ease1 = pchan->ease1;
		pfl->ease2 = pchan->ease2;
		pfl->scaleIn = pchan->scaleIn;
		pfl->scaleOut = pchan->scaleOut;
		
		/* make copy of custom properties */
		if (pchan->prop && (transFlags & ACT_TRANS_PROP))
			pfl->oldprops = IDP_CopyProperty(pchan->prop);
	}
} 

bool ED_view3d_quat_from_axis_view(const char view, float quat[4])
{
	if (RV3D_VIEW_IS_AXIS(view)) {
		copy_qt_qt(quat, view3d_quat_axis[view - RV3D_VIEW_FRONT]);
		return true;
	}
	else {
		return false;
	}
}

static void rotateBevelPiece(Curve *cu, BevPoint *bevp, BevPoint *nbevp, DispList *dlb, float bev_blend, float widfac, float fac, float **r_data)
{
	float *fp, *data = *r_data;
	int b;

	fp = dlb->verts;
	for (b = 0; b < dlb->nr; b++, fp += 3, data += 3) {
		if (cu->flag & CU_3D) {
			float vec[3], quat[4];

			vec[0] = fp[1] + widfac;
			vec[1] = fp[2];
			vec[2] = 0.0;

			if (nbevp == NULL) {
				copy_v3_v3(data, bevp->vec);
				copy_qt_qt(quat, bevp->quat);
			}
			else {
				interp_v3_v3v3(data, bevp->vec, nbevp->vec, bev_blend);
				interp_qt_qtqt(quat, bevp->quat, nbevp->quat, bev_blend);
			}

			mul_qt_v3(quat, vec);

			data[0] += fac * vec[0];
			data[1] += fac * vec[1];
			data[2] += fac * vec[2];
		}
		else {
			float sina, cosa;

			if (nbevp == NULL) {
				copy_v3_v3(data, bevp->vec);
				sina = bevp->sina;
				cosa = bevp->cosa;
			}
			else {
				interp_v3_v3v3(data, bevp->vec, nbevp->vec, bev_blend);

				/* perhaps we need to interpolate angles instead. but the thing is
				 * cosa and sina are not actually sine and cosine
				 */
				sina = nbevp->sina * bev_blend + bevp->sina * (1.0f - bev_blend);
				cosa = nbevp->cosa * bev_blend + bevp->cosa * (1.0f - bev_blend);
			}

			data[0] += fac * (widfac + fp[1]) * sina;
			data[1] += fac * (widfac + fp[1]) * cosa;
			data[2] += fac * fp[2];
		}
	}

	*r_data = data;
}

/* both poses should be in sync */
bool BKE_pose_copy_result(bPose *to, bPose *from)
{
	bPoseChannel *pchanto, *pchanfrom;
	
	if (to == NULL || from == NULL) {
		printf("Pose copy error, pose to:%p from:%p\n", (void *)to, (void *)from); /* debug temp */
		return false;
	}

	if (to == from) {
		printf("BKE_pose_copy_result source and target are the same\n");
		return false;
	}


	for (pchanfrom = from->chanbase.first; pchanfrom; pchanfrom = pchanfrom->next) {
		pchanto = BKE_pose_channel_find_name(to, pchanfrom->name);
		if (pchanto) {
			copy_m4_m4(pchanto->pose_mat, pchanfrom->pose_mat);
			copy_m4_m4(pchanto->chan_mat, pchanfrom->chan_mat);
			
			/* used for local constraints */
			copy_v3_v3(pchanto->loc, pchanfrom->loc);
			copy_qt_qt(pchanto->quat, pchanfrom->quat);
			copy_v3_v3(pchanto->eul, pchanfrom->eul);
			copy_v3_v3(pchanto->size, pchanfrom->size);
			
			copy_v3_v3(pchanto->pose_head, pchanfrom->pose_head);
			copy_v3_v3(pchanto->pose_tail, pchanfrom->pose_tail);
			
			pchanto->roll1 = pchanfrom->roll1;
			pchanto->roll2 = pchanfrom->roll2;
			pchanto->curveInX = pchanfrom->curveInX;
			pchanto->curveInY = pchanfrom->curveInY;
			pchanto->curveOutX = pchanfrom->curveOutX;
			pchanto->curveOutY = pchanfrom->curveOutY;
			pchanto->scaleIn = pchanfrom->scaleIn;
			pchanto->scaleOut = pchanfrom->scaleOut;
			
			pchanto->rotmode = pchanfrom->rotmode;
			pchanto->flag = pchanfrom->flag;
			pchanto->protectflag = pchanfrom->protectflag;
			pchanto->bboneflag = pchanfrom->bboneflag;
		}
	}
	return true;
}

static void psys_path_iter_get(ParticlePathIterator *iter, ParticleCacheKey *keys, int totkeys,
                               ParticleCacheKey *parent, int index)
{
	BLI_assert(index >= 0 && index < totkeys);

	iter->key = keys + index;
	iter->index = index;
	iter->time = (float)index / (float)(totkeys - 1);

	if (parent) {
		iter->parent_key = parent + index;
		if (index > 0)
			mul_qt_qtqt(iter->parent_rotation, iter->parent_key->rot, parent->rot);
		else
			copy_qt_qt(iter->parent_rotation, parent->rot);
	}
	else {
		iter->parent_key = NULL;
		unit_qt(iter->parent_rotation);
	}
}

/* axis is using another define!!! */
static int calc_curve_deform(Scene *scene, Object *par, float co[3],
                             const short axis, CurveDeform *cd, float quat_r[4])
{
	Curve *cu = par->data;
	float fac, loc[4], dir[3], new_quat[4], radius;
	short index;
	const int is_neg_axis = (axis > 2);

	/* to be sure, mostly after file load */
	if (cu->path == NULL) {
		BKE_displist_make_curveTypes(scene, par, 0);
		if (cu->path == NULL) return 0;  // happens on append...
	}
	
	/* options */
	if (is_neg_axis) {
		index = axis - 3;
		if (cu->flag & CU_STRETCH)
			fac = (-co[index] - cd->dmax[index]) / (cd->dmax[index] - cd->dmin[index]);
		else
			fac = -(co[index] - cd->dmax[index]) / (cu->path->totdist);
	}
	else {
		index = axis;
		if (cu->flag & CU_STRETCH)
			fac = (co[index] - cd->dmin[index]) / (cd->dmax[index] - cd->dmin[index]);
		else
			fac = +(co[index] - cd->dmin[index]) / (cu->path->totdist);
	}
	
	if (where_on_path_deform(par, fac, loc, dir, new_quat, &radius)) {  /* returns OK */
		float quat[4], cent[3];

		if (cd->no_rot_axis) {  /* set by caller */

			/* this is not exactly the same as 2.4x, since the axis is having rotation removed rather than
			 * changing the axis before calculating the tilt but serves much the same purpose */
			float dir_flat[3] = {0, 0, 0}, q[4];
			copy_v3_v3(dir_flat, dir);
			dir_flat[cd->no_rot_axis - 1] = 0.0f;

			normalize_v3(dir);
			normalize_v3(dir_flat);

			rotation_between_vecs_to_quat(q, dir, dir_flat); /* Could this be done faster? */

			mul_qt_qtqt(new_quat, q, new_quat);
		}


		/* Logic for 'cent' orientation *
		 *
		 * The way 'co' is copied to 'cent' may seem to have no meaning, but it does.
		 *
		 * Use a curve modifier to stretch a cube out, color each side RGB, positive side light, negative dark.
		 * view with X up (default), from the angle that you can see 3 faces RGB colors (light), anti-clockwise
		 * Notice X,Y,Z Up all have light colors and each ordered CCW.
		 *
		 * Now for Neg Up XYZ, the colors are all dark, and ordered clockwise - Campbell
		 *
		 * note: moved functions into quat_apply_track/vec_apply_track
		 * */
		copy_qt_qt(quat, new_quat);
		copy_v3_v3(cent, co);

		/* zero the axis which is not used,
		 * the big block of text above now applies to these 3 lines */
		quat_apply_track(quat, axis, (axis == 0 || axis == 2) ? 1 : 0); /* up flag is a dummy, set so no rotation is done */
		vec_apply_track(cent, axis);
		cent[index] = 0.0f;


		/* scale if enabled */
		if (cu->flag & CU_PATH_RADIUS)
			mul_v3_fl(cent, radius);
		
		/* local rotation */
		normalize_qt(quat);
		mul_qt_v3(quat, cent);

		/* translation */
		add_v3_v3v3(co, cent, loc);

		if (quat_r)
			copy_qt_qt(quat_r, quat);

		return 1;
	}
	return 0;
}

/* clear rotation of object */
static void object_clear_rot(Object *ob)
{
	/* clear rotations that aren't locked */
	if (ob->protectflag & (OB_LOCK_ROTX|OB_LOCK_ROTY|OB_LOCK_ROTZ|OB_LOCK_ROTW)) {
		if (ob->protectflag & OB_LOCK_ROT4D) {
			/* perform clamping on a component by component basis */
			if (ob->rotmode == ROT_MODE_AXISANGLE) {
				if ((ob->protectflag & OB_LOCK_ROTW) == 0)
					ob->rotAngle= ob->drotAngle= 0.0f;
				if ((ob->protectflag & OB_LOCK_ROTX) == 0)
					ob->rotAxis[0]= ob->drotAxis[0]= 0.0f;
				if ((ob->protectflag & OB_LOCK_ROTY) == 0)
					ob->rotAxis[1]= ob->drotAxis[1]= 0.0f;
				if ((ob->protectflag & OB_LOCK_ROTZ) == 0)
					ob->rotAxis[2]= ob->drotAxis[2]= 0.0f;
					
				/* check validity of axis - axis should never be 0,0,0 (if so, then we make it rotate about y) */
				if (IS_EQF(ob->rotAxis[0], ob->rotAxis[1]) && IS_EQF(ob->rotAxis[1], ob->rotAxis[2]))
					ob->rotAxis[1] = 1.0f;
				if (IS_EQF(ob->drotAxis[0], ob->drotAxis[1]) && IS_EQF(ob->drotAxis[1], ob->drotAxis[2]))
					ob->drotAxis[1]= 1.0f;
			}
			else if (ob->rotmode == ROT_MODE_QUAT) {
				if ((ob->protectflag & OB_LOCK_ROTW) == 0)
					ob->quat[0]= ob->dquat[0]= 1.0f;
				if ((ob->protectflag & OB_LOCK_ROTX) == 0)
					ob->quat[1]= ob->dquat[1]= 0.0f;
				if ((ob->protectflag & OB_LOCK_ROTY) == 0)
					ob->quat[2]= ob->dquat[2]= 0.0f;
				if ((ob->protectflag & OB_LOCK_ROTZ) == 0)
					ob->quat[3]= ob->dquat[3]= 0.0f;
					
				// TODO: does this quat need normalising now?
			}
			else {
				/* the flag may have been set for the other modes, so just ignore the extra flag... */
				if ((ob->protectflag & OB_LOCK_ROTX) == 0)
					ob->rot[0]= ob->drot[0]= 0.0f;
				if ((ob->protectflag & OB_LOCK_ROTY) == 0)
					ob->rot[1]= ob->drot[1]= 0.0f;
				if ((ob->protectflag & OB_LOCK_ROTZ) == 0)
					ob->rot[2]= ob->drot[2]= 0.0f;
			}
		}
		else {
			/* perform clamping using euler form (3-components) */
			// FIXME: deltas are not handled for these cases yet...
			float eul[3], oldeul[3], quat1[4] = {0};
			
			if (ob->rotmode == ROT_MODE_QUAT) {
				copy_qt_qt(quat1, ob->quat);
				quat_to_eul(oldeul, ob->quat);
			}
			else if (ob->rotmode == ROT_MODE_AXISANGLE) {
				axis_angle_to_eulO(oldeul, EULER_ORDER_DEFAULT, ob->rotAxis, ob->rotAngle);
			}
			else {
				copy_v3_v3(oldeul, ob->rot);
			}
			
			eul[0]= eul[1]= eul[2]= 0.0f;
			
			if (ob->protectflag & OB_LOCK_ROTX)
				eul[0]= oldeul[0];
			if (ob->protectflag & OB_LOCK_ROTY)
				eul[1]= oldeul[1];
			if (ob->protectflag & OB_LOCK_ROTZ)
				eul[2]= oldeul[2];
			
			if (ob->rotmode == ROT_MODE_QUAT) {
				eul_to_quat(ob->quat, eul);
				/* quaternions flip w sign to accumulate rotations correctly */
				if ((quat1[0]<0.0f && ob->quat[0]>0.0f) || (quat1[0]>0.0f && ob->quat[0]<0.0f)) {
					mul_qt_fl(ob->quat, -1.0f);
				}
			}
			else if (ob->rotmode == ROT_MODE_AXISANGLE) {
				eulO_to_axis_angle(ob->rotAxis, &ob->rotAngle,eul, EULER_ORDER_DEFAULT);
			}
			else {
				copy_v3_v3(ob->rot, eul);
			}
		}
	}						 // Duplicated in source/blender/editors/armature/editarmature.c
	else { 
		if (ob->rotmode == ROT_MODE_QUAT) {
			unit_qt(ob->quat);
			unit_qt(ob->dquat);
		}
		else if (ob->rotmode == ROT_MODE_AXISANGLE) {
			unit_axis_angle(ob->rotAxis, &ob->rotAngle);
			unit_axis_angle(ob->drotAxis, &ob->drotAngle);
		}
		else {
			zero_v3(ob->rot);
			zero_v3(ob->drot);
		}
	}
}

//.........这里部分代码省略.........
				else if (boids->options & BOID_ALLOW_FLIGHT && bbd->wanted_co[2] > 0.0f)
					bpa->data.mode = eBoidMode_InAir;
			}
			else
				bpa->data.mode = eBoidMode_InAir;
			break;
		}
		case eBoidMode_Climbing:
		{
			boid_climb(boids, pa, ground_co, ground_nor);
			//float nor[3];
			//copy_v3_v3(nor, ground_nor);

			///* gather apparent gravity to r_ve */
			//madd_v3_v3fl(pa->r_ve, ground_nor, -1.0);
			//normalize_v3(pa->r_ve);

			///* raise boid it's size from surface */
			//mul_v3_fl(nor, pa->size * boids->height);
			//add_v3_v3v3(pa->state.co, ground_co, nor);

			///* remove normal component from velocity */
			//project_v3_v3v3(v, pa->state.vel, ground_nor);
			//sub_v3_v3v3(pa->state.vel, pa->state.vel, v);
			break;
		}
		case eBoidMode_OnLand:
		{
			/* stick boid on goal when close enough */
			if (bbd->goal_ob && boid_goal_signed_dist(pa->state.co, bbd->goal_co, bbd->goal_nor) <= pa->size * boids->height) {
				bpa->data.mode = eBoidMode_Climbing;
				bpa->ground = bbd->goal_ob;
				boid_find_ground(bbd, pa, ground_co, ground_nor);
				boid_climb(boids, pa, ground_co, ground_nor);
			}
			/* ground is too far away so boid falls */
			else if (pa->state.co[2]-ground_co[2] > 1.1f * pa->size * boids->height)
				bpa->data.mode = eBoidMode_Falling;
			else {
				/* constrain to surface */
				pa->state.co[2] = ground_co[2] + pa->size * boids->height;
				pa->state.vel[2] = 0.0f;
			}

			if (boids->banking > 0.0f) {
				float grav[3];
				/* Don't take gravity's strength in to account, */
				/* otherwise amount of banking is hard to control. */
				negate_v3_v3(grav, ground_nor);

				project_v3_v3v3(dvec, bpa->data.acc, pa->state.vel);
				sub_v3_v3v3(dvec, bpa->data.acc, dvec);

				/* gather apparent gravity */
				madd_v3_v3v3fl(bpa->gravity, grav, dvec, -boids->banking);
				normalize_v3(bpa->gravity);
			}
			else {
				/* gather negative surface normal */
				madd_v3_v3fl(bpa->gravity, ground_nor, -1.0f);
				normalize_v3(bpa->gravity);
			}
			break;
		}
	}

	/* save direction to state.ave unless the boid is falling */
	/* (boids can't effect their direction when falling) */
	if (bpa->data.mode!=eBoidMode_Falling && len_v3(pa->state.vel) > 0.1f*pa->size) {
		copy_v3_v3(pa->state.ave, pa->state.vel);
		pa->state.ave[2] *= bbd->part->boids->pitch;
		normalize_v3(pa->state.ave);
	}

	/* apply damping */
	if (ELEM(bpa->data.mode, eBoidMode_OnLand, eBoidMode_Climbing))
		mul_v3_fl(pa->state.vel, 1.0f - 0.2f*bbd->part->dampfac);

	/* calculate rotation matrix based on forward & down vectors */
	if (bpa->data.mode == eBoidMode_InAir) {
		copy_v3_v3(mat[0], pa->state.ave);

		project_v3_v3v3(dvec, bpa->gravity, pa->state.ave);
		sub_v3_v3v3(mat[2], bpa->gravity, dvec);
		normalize_v3(mat[2]);
	}
	else {
		project_v3_v3v3(dvec, pa->state.ave, bpa->gravity);
		sub_v3_v3v3(mat[0], pa->state.ave, dvec);
		normalize_v3(mat[0]);

		copy_v3_v3(mat[2], bpa->gravity);
	}
	negate_v3(mat[2]);
	cross_v3_v3v3(mat[1], mat[2], mat[0]);
	
	/* apply rotation */
	mat3_to_quat_is_ok(q, mat);
	copy_qt_qt(pa->state.rot, q);
}

/* the arguments are the desired situation */
void ED_view3d_smooth_view_ex(
        /* avoid passing in the context */
        wmWindowManager *wm, wmWindow *win, ScrArea *sa,

        View3D *v3d, ARegion *ar, Object *oldcamera, Object *camera,
        const float *ofs, const float *quat, const float *dist, const float *lens,
        const int smooth_viewtx)
{
	RegionView3D *rv3d = ar->regiondata;
	struct SmoothView3DStore sms = {{0}};
	bool ok = false;
	
	/* initialize sms */
	view3d_smooth_view_state_backup(&sms.dst, v3d, rv3d);
	view3d_smooth_view_state_backup(&sms.src, v3d, rv3d);
	/* if smoothview runs multiple times... */
	if (rv3d->sms == NULL) {
		view3d_smooth_view_state_backup(&sms.org, v3d, rv3d);
		sms.org_view = rv3d->view;
	}
	else {
		sms.org = rv3d->sms->org;
		sms.org_view = rv3d->sms->org_view;
	}
	/* sms.to_camera = false; */  /* initizlized to zero anyway */

	/* note on camera locking, this is a little confusing but works ok.
	 * we may be changing the view 'as if' there is no active camera, but in fact
	 * there is an active camera which is locked to the view.
	 *
	 * In the case where smooth view is moving _to_ a camera we don't want that
	 * camera to be moved or changed, so only when the camera is not being set should
	 * we allow camera option locking to initialize the view settings from the camera.
	 */
	if (camera == NULL && oldcamera == NULL) {
		ED_view3d_camera_lock_init(v3d, rv3d);
	}

	/* store the options we want to end with */
	if (ofs)  copy_v3_v3(sms.dst.ofs, ofs);
	if (quat) copy_qt_qt(sms.dst.quat, quat);
	if (dist) sms.dst.dist = *dist;
	if (lens) sms.dst.lens = *lens;

	if (camera) {
		sms.dst.dist = ED_view3d_offset_distance(camera->obmat, ofs, VIEW3D_DIST_FALLBACK);
		ED_view3d_from_object(camera, sms.dst.ofs, sms.dst.quat, &sms.dst.dist, &sms.dst.lens);
		sms.to_camera = true; /* restore view3d values in end */
	}
	
	/* skip smooth viewing for render engine draw */
	if (smooth_viewtx && v3d->drawtype != OB_RENDER) {
		bool changed = false; /* zero means no difference */
		
		if (oldcamera != camera)
			changed = true;
		else if (sms.dst.dist != rv3d->dist)
			changed = true;
		else if (sms.dst.lens != v3d->lens)
			changed = true;
		else if (!equals_v3v3(sms.dst.ofs, rv3d->ofs))
			changed = true;
		else if (!equals_v4v4(sms.dst.quat, rv3d->viewquat))
			changed = true;
		
		/* The new view is different from the old one
		 * so animate the view */
		if (changed) {
			/* original values */
			if (oldcamera) {
				sms.src.dist = ED_view3d_offset_distance(oldcamera->obmat, rv3d->ofs, 0.0f);
				/* this */
				ED_view3d_from_object(oldcamera, sms.src.ofs, sms.src.quat, &sms.src.dist, &sms.src.lens);
			}
			/* grid draw as floor */
			if ((rv3d->viewlock & RV3D_LOCKED) == 0) {
				/* use existing if exists, means multiple calls to smooth view wont loose the original 'view' setting */
				rv3d->view = RV3D_VIEW_USER;
			}

			sms.time_allowed = (double)smooth_viewtx / 1000.0;
			
			/* if this is view rotation only
			 * we can decrease the time allowed by
			 * the angle between quats 
			 * this means small rotations wont lag */
			if (quat && !ofs && !dist) {
				float vec1[3] = {0, 0, 1}, vec2[3] = {0, 0, 1};
				float q1[4], q2[4];

				invert_qt_qt(q1, sms.dst.quat);
				invert_qt_qt(q2, sms.src.quat);

				mul_qt_v3(q1, vec1);
				mul_qt_v3(q2, vec2);

				/* scale the time allowed by the rotation */
				sms.time_allowed *= (double)angle_v3v3(vec1, vec2) / M_PI; /* 180deg == 1.0 */
			}
//.........这里部分代码省略.........

//.........这里部分代码省略.........

				/* Incrementally Rotating Frame (Bishop Frame) */
				if (k == 0) {
					float hairmat[4][4];
					float mat[3][3];
					
					if (first_particle + p < psys->totpart)
						pa = psys->particles + first_particle + p;
					else {
						ChildParticle *cpa = psys->child + (p - psys->totpart);
						pa = psys->particles + cpa->parent;
					}
					psys_mat_hair_to_global(sim.ob, sim.psmd->dm, sim.psys->part->from, pa, hairmat);
					copy_m3_m4(mat, hairmat);
					/* to quaternion */
					mat3_to_quat(frame, mat);
					
					/* note: direction is same as normal vector currently,
					 * but best to keep this separate so the frame can be
					 * rotated later if necessary
					 */
					copy_v3_v3(prev_dir, state.vel);
				}
				else {
					float rot[4];
					
					/* incrementally rotate along bend direction */
					rotation_between_vecs_to_quat(rot, prev_dir, state.vel);
					mul_qt_qtqt(frame, rot, frame);
					
					copy_v3_v3(prev_dir, state.vel);
				}
				
				copy_qt_qt(state.rot, frame);
#if 0
				/* Absolute Frame (Frenet Frame) */
				if (state.vel[axis] < -0.9999f || state.vel[axis] > 0.9999f) {
					unit_qt(state.rot);
				}
				else {
					float cross[3];
					float temp[3] = {0.0f, 0.0f, 0.0f};
					temp[axis] = 1.0f;
					
					cross_v3_v3v3(cross, temp, state.vel);
					
					/* state.vel[axis] is the only component surviving from a dot product with the axis */
					axis_angle_to_quat(state.rot, cross, saacos(state.vel[axis]));
				}
#endif
			}
			else {
				state.time = -1.0;
				psys_get_particle_state(&sim, first_particle + p, &state, 1);
			}

			mul_qt_v3(state.rot, mv->co);
			if (pimd->flag & eParticleInstanceFlag_UseSize)
				mul_v3_fl(mv->co, size[p]);
			add_v3_v3(mv->co, state.co);
		}

		/* create polys and loops */
		for (k = 0; k < totpoly; k++) {
			MPoly *inMP = orig_mpoly + k;
			MPoly *mp = mpoly + p_skip * totpoly + k;