use more BLI math functions.

17 files changed, 114 insertions, 243 deletions

diff --git a/source/blender/blenkernel/intern/DerivedMesh.c b/source/blender/blenkernel/intern/DerivedMesh.c
index 663fbb89b4e..de7b962e38a 100644
--- a/source/blender/blenkernel/intern/DerivedMesh.c
+++ b/source/blender/blenkernel/intern/DerivedMesh.c
@@ -1407,8 +1407,7 @@ static DerivedMesh *getEditMeshDerivedMesh(EditMesh *em, Object *ob,
 			/* following Mesh convention; we use vertex coordinate itself
 			 * for normal in this case */
 			if (normalize_v3(no)==0.0) {
-				VECCOPY(no, vertexCos[i]);
-				normalize_v3(no);
+				normalize_v3_v3(no, vertexCos[i]);
 			}
 		}
 	}
@@ -2590,9 +2589,7 @@ void DM_add_tangent_layer(DerivedMesh *dm)
 		
 		for(j=0; j<len; j++) {
 			vtang= find_vertex_tangent(vtangents[mf_vi[j]], mtface ? tf->uv[j] : uv[j]);
-
-			VECCOPY(tangent[j], vtang);
-			normalize_v3(tangent[j]);
+			normalize_v3_v3(tangent[j], vtang);
 		}
 	}
 	
diff --git a/source/blender/blenkernel/intern/armature.c b/source/blender/blenkernel/intern/armature.c
index b86be371d66..41821f34ba8 100644
--- a/source/blender/blenkernel/intern/armature.c
+++ b/source/blender/blenkernel/intern/armature.c
@@ -1283,9 +1283,8 @@ void vec_roll_to_mat3(float *vec, float roll, float mat[][3])
 	float	nor[3], axis[3], target[3]={0,1,0};
 	float	theta;
 	float	rMatrix[3][3], bMatrix[3][3];
-	
-	VECCOPY (nor, vec);
-	normalize_v3(nor);
+
+	normalize_v3_v3(nor, vec);
 	
 	/*	Find Axis & Amount for bone matrix*/
 	cross_v3_v3v3(axis,target,nor);
diff --git a/source/blender/blenkernel/intern/boids.c b/source/blender/blenkernel/intern/boids.c
index 82602a6951d..7ae65d0113a 100644
--- a/source/blender/blenkernel/intern/boids.c
+++ b/source/blender/blenkernel/intern/boids.c
@@ -659,9 +659,9 @@ static int rule_fight(BoidRule *rule, BoidBrainData *bbd, BoidValues *val, Parti
 		/* attack if in range */
 		if(closest_dist <= bbd->part->boids->range + pa->size + enemy_pa->size) {
 			float damage = BLI_frand();
-			float enemy_dir[3] = {bbd->wanted_co[0],bbd->wanted_co[1],bbd->wanted_co[2]};
+			float enemy_dir[3];
 
-			normalize_v3(enemy_dir);
+			normalize_v3_v3(enemy_dir, bbd->wanted_co);
 
 			/* fight mode */
 			bbd->wanted_speed = 0.0f;
@@ -786,8 +786,7 @@ static Object *boid_find_ground(BoidBrainData *bbd, ParticleData *pa, float *gro
 		if(hit.index>=0) {
 			t = hit.dist/col.ray_len;
 			interp_v3_v3v3(ground_co, col.co1, col.co2, t);
-			VECCOPY(ground_nor, col.nor);
-			normalize_v3(ground_nor);
+			normalize_v3_v3(ground_nor, col.nor);
 			return col.hit_ob;
 		}
 		else {
@@ -1115,8 +1114,7 @@ void boid_body(BoidBrainData *bbd, ParticleData *pa)
 		}
 
 		VECCOPY(old_dir, pa->prev_state.ave);
-		VECCOPY(wanted_dir, bbd->wanted_co);
-		new_speed = normalize_v3(wanted_dir);
+		new_speed = normalize_v3_v3(wanted_dir, bbd->wanted_co);
 
 		/* first check if we have valid direction we want to go towards */
 		if(new_speed == 0.0f) {
@@ -1356,8 +1354,7 @@ void boid_body(BoidBrainData *bbd, ParticleData *pa)
 	/* 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.1*pa->size) {
-		VECCOPY(pa->state.ave, pa->state.vel);
-		normalize_v3(pa->state.ave);
+		normalize_v3_v3(pa->state.ave, pa->state.vel);
 	}
 
 	/* apply damping */
diff --git a/source/blender/blenkernel/intern/cdderivedmesh.c b/source/blender/blenkernel/intern/cdderivedmesh.c
index fb1e8c00991..ca81e216006 100644
--- a/source/blender/blenkernel/intern/cdderivedmesh.c
+++ b/source/blender/blenkernel/intern/cdderivedmesh.c
@@ -1777,14 +1777,10 @@ void CDDM_calc_normals(DerivedMesh *dm)
 	for(i = 0; i < numVerts; i++, mv++) {
 		float *no = temp_nors[i];
 		
-		if (normalize_v3(no) == 0.0) {
-			VECCOPY(no, mv->co);
-			normalize_v3(no);
-		}
+		if (normalize_v3(no) == 0.0)
+			normalize_v3_v3(no, mv->co);
 
-		mv->no[0] = (short)(no[0] * 32767.0);
-		mv->no[1] = (short)(no[1] * 32767.0);
-		mv->no[2] = (short)(no[2] * 32767.0);
+		normal_float_to_short_v3(mv->no, no);
 	}
 	
 	MEM_freeN(temp_nors);
diff --git a/source/blender/blenkernel/intern/collision.c b/source/blender/blenkernel/intern/collision.c
index 3a9c15f654c..af12d23b2c2 100644
--- a/source/blender/blenkernel/intern/collision.c
+++ b/source/blender/blenkernel/intern/collision.c
@@ -691,8 +691,7 @@ CollPair* cloth_collision ( ModifierData *md1, ModifierData *md2, BVHTreeOverlap
 
 		if ( distance <= ( epsilon1 + epsilon2 + ALMOST_ZERO ) )
 		{
-			VECCOPY ( collpair->normal, collpair->vector );
-			normalize_v3( collpair->normal );
+			normalize_v3_v3( collpair->normal, collpair->vector );
 
 			collpair->distance = distance;
 			collpair->flag = 0;
diff --git a/source/blender/blenkernel/intern/constraint.c b/source/blender/blenkernel/intern/constraint.c
index aae70c055d0..b415484c1c1 100644
--- a/source/blender/blenkernel/intern/constraint.c
+++ b/source/blender/blenkernel/intern/constraint.c
@@ -256,7 +256,7 @@ void constraint_mat_convertspace (Object *ob, bPoseChannel *pchan, float mat[][4
 								
 							/* construct offs_bone the same way it is done in armature.c */
 							copy_m4_m3(offs_bone, pchan->bone->bone_mat);
-							VECCOPY(offs_bone[3], pchan->bone->head);
+							copy_v3_v3(offs_bone[3], pchan->bone->head);
 							offs_bone[3][1]+= pchan->bone->parent->length;
 							
 							if (pchan->bone->flag & BONE_HINGE) {
@@ -267,7 +267,7 @@ void constraint_mat_convertspace (Object *ob, bPoseChannel *pchan, float mat[][4
 								copy_m4_m4(tmat, pchan->bone->parent->arm_mat);
 								
 								/* the location of actual parent transform */
-								VECCOPY(tmat[3], offs_bone[3]);
+								copy_v3_v3(tmat[3], offs_bone[3]);
 								offs_bone[3][0]= offs_bone[3][1]= offs_bone[3][2]= 0.0f;
 								mul_m4_v3(pchan->parent->pose_mat, tmat[3]);
 								
@@ -309,7 +309,7 @@ void constraint_mat_convertspace (Object *ob, bPoseChannel *pchan, float mat[][4
 						
 						/* construct offs_bone the same way it is done in armature.c */
 						copy_m4_m3(offs_bone, pchan->bone->bone_mat);
-						VECCOPY(offs_bone[3], pchan->bone->head);
+						copy_v3_v3(offs_bone[3], pchan->bone->head);
 						offs_bone[3][1]+= pchan->bone->parent->length;
 						
 						if (pchan->bone->flag & BONE_HINGE) {
@@ -320,8 +320,8 @@ void constraint_mat_convertspace (Object *ob, bPoseChannel *pchan, float mat[][4
 							copy_m4_m4(tmat, pchan->bone->parent->arm_mat);
 							
 							/* the location of actual parent transform */
-							VECCOPY(tmat[3], offs_bone[3]);
-							offs_bone[3][0]= offs_bone[3][1]= offs_bone[3][2]= 0.0f;
+							copy_v3_v3(tmat[3], offs_bone[3]);
+							zero_v3(offs_bone[3]);
 							mul_m4_v3(pchan->parent->pose_mat, tmat[3]);
 							
 							mul_m4_m4m4(diff_mat, offs_bone, tmat);
@@ -400,7 +400,7 @@ static void contarget_get_mesh_mat (Scene *scene, Object *ob, char *substring, f
 	DerivedMesh *dm = NULL;
 	Mesh *me= ob->data;
 	EditMesh *em = BKE_mesh_get_editmesh(me);
-	float vec[3] = {0.0f, 0.0f, 0.0f}, tvec[3];
+	float vec[3] = {0.0f, 0.0f, 0.0f};
 	float normal[3] = {0.0f, 0.0f, 0.0f}, plane[3];
 	float imat[3][3], tmat[3][3];
 	int dgroup;
@@ -477,9 +477,9 @@ static void contarget_get_mesh_mat (Scene *scene, Object *ob, char *substring, f
 			mul_m3_v3(tmat, normal);
 			
 			normalize_v3(normal);
-			VECCOPY(plane, tmat[1]);
+			copy_v3_v3(plane, tmat[1]);
 			
-			VECCOPY(tmat[2], normal);
+			copy_v3_v3(tmat[2], normal);
 			cross_v3_v3v3(tmat[0], normal, plane);
 			cross_v3_v3v3(tmat[1], tmat[2], tmat[0]);
 			
@@ -488,8 +488,7 @@ static void contarget_get_mesh_mat (Scene *scene, Object *ob, char *substring, f
 			
 			
 			/* apply the average coordinate as the new location */
-			mul_v3_m4v3(tvec, ob->obmat, vec);
-			VECCOPY(mat[3], tvec);
+			mul_v3_m4v3(mat[3], ob->obmat, vec);
 		}
 	}
 	
@@ -554,7 +553,7 @@ static void contarget_get_lattice_mat (Object *ob, char *substring, float mat[][
 	mul_v3_m4v3(tvec, ob->obmat, vec);
 	
 	/* copy new location to matrix */
-	VECCOPY(mat[3], tvec);
+	copy_v3_v3(mat[3], tvec);
 }
 
 /* generic function to get the appropriate matrix for most target cases */
@@ -819,11 +818,11 @@ static void childof_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *ta
 		copy_m4_m4(invmat, data->invmat);
 		
 		/* extract components of both matrices */
-		VECCOPY(loc, ct->matrix[3]);
+		copy_v3_v3(loc, ct->matrix[3]);
 		mat4_to_eulO(eul, ct->rotOrder, ct->matrix);
 		mat4_to_size(size, ct->matrix);
 		
-		VECCOPY(loco, invmat[3]);
+		copy_v3_v3(loco, invmat[3]);
 		mat4_to_eulO(eulo, cob->rotOrder, invmat);
 		mat4_to_size(sizo, invmat);
 		
@@ -940,9 +939,8 @@ static void vectomat (float *vec, float *target_up, short axis, short upflag, sh
 	float right[3];
 	float neg = -1;
 	int right_index;
-	
-	copy_v3_v3(n, vec);
-	if (normalize_v3(n) == 0.0) { 
+
+	if (normalize_v3_v3(n, vec) == 0.0) { 
 		n[0] = 0.0;
 		n[1] = 0.0;
 		n[2] = 1.0;
@@ -953,9 +951,7 @@ static void vectomat (float *vec, float *target_up, short axis, short upflag, sh
 	/* n specifies the transformation of the track axis */
 	if (flags & TARGET_Z_UP) { 
 		/* target Z axis is the global up axis */
-		u[0] = target_up[0];
-		u[1] = target_up[1];
-		u[2] = target_up[2];
+		copy_v3_v3(u, target_up);
 	}
 	else { 
 		/* world Z axis is the global up axis */
@@ -988,20 +984,13 @@ static void vectomat (float *vec, float *target_up, short axis, short upflag, sh
 		m[right_index][1] = neg * right[1];
 		m[right_index][2] = neg * right[2];
 		
-		m[upflag][0] = proj[0];
-		m[upflag][1] = proj[1];
-		m[upflag][2] = proj[2];
+		copy_v3_v3(m[upflag], proj);
 		
-		m[axis][0] = n[0];
-		m[axis][1] = n[1];
-		m[axis][2] = n[2];
+		copy_v3_v3(m[axis], n);
 	}
 	/* identity matrix - don't do anything if the two axes are the same */
 	else {
-		m[0][0]= m[1][1]= m[2][2]= 1.0;
-		m[0][1]= m[0][2]= 0.0;
-		m[1][0]= m[1][2]= 0.0;
-		m[2][0]= m[2][1]= 0.0;
+		unit_m3(m);
 	}
 }
 
@@ -1264,7 +1253,7 @@ static void followpath_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstr
 					copy_m4_m4(totmat, rmat);
 				}
 				
-				VECCOPY(totmat[3], vec);
+				copy_v3_v3(totmat[3], vec);
 				
 				mul_serie_m4(ct->matrix, ct->tar->obmat, totmat, NULL, NULL, NULL, NULL, NULL, NULL);
 			}
@@ -1383,7 +1372,7 @@ static void rotlimit_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *t
 	float eul[3];
 	float size[3];
 	
-	VECCOPY(loc, cob->matrix[3]);
+	copy_v3_v3(loc, cob->matrix[3]);
 	mat4_to_size(size, cob->matrix);
 	
 	mat4_to_eulO(eul, cob->rotOrder, cob->matrix);
@@ -1544,7 +1533,7 @@ static void loclike_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *ta
 		float offset[3] = {0.0f, 0.0f, 0.0f};
 		
 		if (data->flag & LOCLIKE_OFFSET)
-			VECCOPY(offset, cob->matrix[3]);
+			copy_v3_v3(offset, cob->matrix[3]);
 			
 		if (data->flag & LOCLIKE_X) {
 			cob->matrix[3][0] = ct->matrix[3][0];
@@ -1636,7 +1625,7 @@ static void rotlike_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *ta
 		float	eul[3], obeul[3];
 		float	size[3];
 		
-		VECCOPY(loc, cob->matrix[3]);
+		copy_v3_v3(loc, cob->matrix[3]);
 		mat4_to_size(size, cob->matrix);
 		
 		/* to allow compatible rotations, must get both rotations in the order of the owner... */
@@ -2138,7 +2127,7 @@ static void actcon_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraint
 		}
 		else {
 			/* extract location */
-			VECCOPY(vec, tempmat[3]);
+			copy_v3_v3(vec, tempmat[3]);
 			axis= data->type - 20;
 		}
 		
@@ -2294,10 +2283,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 					normalize_v3(totmat[1]);
 					
 					/* the x axis is fixed */
-					totmat[0][0] = cob->matrix[0][0];
-					totmat[0][1] = cob->matrix[0][1];
-					totmat[0][2] = cob->matrix[0][2];
-					normalize_v3(totmat[0]);
+					normalize_v3_v3(totmat[0], cob->matrix[0]);
 					
 					/* the z axis gets mapped onto a third orthogonal vector */
 					cross_v3_v3v3(totmat[2], totmat[0], totmat[1]);
@@ -2311,10 +2297,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 					normalize_v3(totmat[2]);
 					
 					/* the x axis is fixed */
-					totmat[0][0] = cob->matrix[0][0];
-					totmat[0][1] = cob->matrix[0][1];
-					totmat[0][2] = cob->matrix[0][2];
-					normalize_v3(totmat[0]);
+					normalize_v3_v3(totmat[0], cob->matrix[0]);
 					
 					/* the z axis gets mapped onto a third orthogonal vector */
 					cross_v3_v3v3(totmat[1], totmat[2], totmat[0]);
@@ -2329,10 +2312,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 					negate_v3(totmat[1]);
 					
 					/* the x axis is fixed */
-					totmat[0][0] = cob->matrix[0][0];
-					totmat[0][1] = cob->matrix[0][1];
-					totmat[0][2] = cob->matrix[0][2];
-					normalize_v3(totmat[0]);
+					normalize_v3_v3(totmat[0], cob->matrix[0]);
 					
 					/* the z axis gets mapped onto a third orthogonal vector */
 					cross_v3_v3v3(totmat[2], totmat[0], totmat[1]);
@@ -2347,10 +2327,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 					negate_v3(totmat[2]);
 						
 					/* the x axis is fixed */
-					totmat[0][0] = cob->matrix[0][0];
-					totmat[0][1] = cob->matrix[0][1];
-					totmat[0][2] = cob->matrix[0][2];
-					normalize_v3(totmat[0]);
+					normalize_v3_v3(totmat[0], cob->matrix[0]);
 						
 					/* the z axis gets mapped onto a third orthogonal vector */
 					cross_v3_v3v3(totmat[1], totmat[2], totmat[0]);
@@ -2358,9 +2335,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 					break;
 				default:
 				{
-					totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
-					totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
-					totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
+					unit_m3(totmat);
 				}
 					break;
 			}
@@ -2377,11 +2352,8 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 					normalize_v3(totmat[0]);
 					
 					/* the y axis is fixed */
-					totmat[1][0] = cob->matrix[1][0];
-					totmat[1][1] = cob->matrix[1][1];
-					totmat[1][2] = cob->matrix[1][2];
-					normalize_v3(totmat[1]);
-					
+					normalize_v3_v3(totmat[1], cob->matrix[1]);
+
 					/* the z axis gets mapped onto a third orthogonal vector */
 					cross_v3_v3v3(totmat[2], totmat[0], totmat[1]);
 				}
@@ -2394,10 +2366,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 					normalize_v3(totmat[2]);
 					
 					/* the y axis is fixed */
-					totmat[1][0] = cob->matrix[1][0];
-					totmat[1][1] = cob->matrix[1][1];
-					totmat[1][2] = cob->matrix[1][2];
-					normalize_v3(totmat[1]);
+					normalize_v3_v3(totmat[1], cob->matrix[1]);
 					
 					/* the z axis gets mapped onto a third orthogonal vector */
 					cross_v3_v3v3(totmat[0], totmat[1], totmat[2]);
@@ -2412,10 +2381,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 					negate_v3(totmat[0]);
 					
 					/* the y axis is fixed */
-					totmat[1][0] = cob->matrix[1][0];
-					totmat[1][1] = cob->matrix[1][1];
-					totmat[1][2] = cob->matrix[1][2];
-					normalize_v3(totmat[1]);
+					normalize_v3_v3(totmat[1], cob->matrix[1]);
 					
 					/* the z axis gets mapped onto a third orthogonal vector */
 					cross_v3_v3v3(totmat[2], totmat[0], totmat[1]);
@@ -2430,10 +2396,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 					negate_v3(totmat[2]);
 					
 					/* the y axis is fixed */
-					totmat[1][0] = cob->matrix[1][0];
-					totmat[1][1] = cob->matrix[1][1];
-					totmat[1][2] = cob->matrix[1][2];
-					normalize_v3(totmat[1]);
+					normalize_v3_v3(totmat[1], cob->matrix[1]);
 					
 					/* the z axis gets mapped onto a third orthogonal vector */
 					cross_v3_v3v3(totmat[0], totmat[1], totmat[2]);
@@ -2441,9 +2404,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 					break;
 				default:
 				{
-					totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
-					totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
-					totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
+					unit_m3(totmat);
 				}
 					break;
 			}
@@ -2460,10 +2421,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 					normalize_v3(totmat[0]);
 					
 					/* the z axis is fixed */
-					totmat[2][0] = cob->matrix[2][0];
-					totmat[2][1] = cob->matrix[2][1];
-					totmat[2][2] = cob->matrix[2][2];
-					normalize_v3(totmat[2]);
+					normalize_v3_v3(totmat[2], cob->matrix[2]);
 					
 					/* the x axis gets mapped onto a third orthogonal vector */
 					cross_v3_v3v3(totmat[1], totmat[2], totmat[0]);
@@ -2477,10 +2435,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 					normalize_v3(totmat[1]);
 					
 					/* the z axis is fixed */
-					totmat[2][0] = cob->matrix[2][0];
-					totmat[2][1] = cob->matrix[2][1];
-					totmat[2][2] = cob->matrix[2][2];
-					normalize_v3(totmat[2]);
+					normalize_v3_v3(totmat[2], cob->matrix[2]);
 						
 					/* the x axis gets mapped onto a third orthogonal vector */
 					cross_v3_v3v3(totmat[0], totmat[1], totmat[2]);
@@ -2495,10 +2450,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 					negate_v3(totmat[0]);
 					
 					/* the z axis is fixed */
-					totmat[2][0] = cob->matrix[2][0];
-					totmat[2][1] = cob->matrix[2][1];
-					totmat[2][2] = cob->matrix[2][2];
-					normalize_v3(totmat[2]);
+					normalize_v3_v3(totmat[2], cob->matrix[2]);
 					
 					/* the x axis gets mapped onto a third orthogonal vector */
 					cross_v3_v3v3(totmat[1], totmat[2], totmat[0]);
@@ -2513,10 +2465,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 					negate_v3(totmat[1]);
 					
 					/* the z axis is fixed */
-					totmat[2][0] = cob->matrix[2][0];
-					totmat[2][1] = cob->matrix[2][1];
-					totmat[2][2] = cob->matrix[2][2];
-					normalize_v3(totmat[2]);
+					normalize_v3_v3(totmat[2], cob->matrix[2]);
 						
 					/* the x axis gets mapped onto a third orthogonal vector */
 					cross_v3_v3v3(totmat[0], totmat[1], totmat[2]);
@@ -2524,9 +2473,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 					break;
 				default:
 				{
-					totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
-					totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
-					totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
+					unit_m3(totmat);
 				}
 					break;
 			}
@@ -2534,19 +2481,13 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 			break;
 		default:
 		{
-			totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
-			totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
-			totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
+			unit_m3(totmat);
 		}
 			break;
 		}
 		/* Block to keep matrix heading */
-		tmpmat[0][0] = cob->matrix[0][0];tmpmat[0][1] = cob->matrix[0][1];tmpmat[0][2] = cob->matrix[0][2];
-		tmpmat[1][0] = cob->matrix[1][0];tmpmat[1][1] = cob->matrix[1][1];tmpmat[1][2] = cob->matrix[1][2];
-		tmpmat[2][0] = cob->matrix[2][0];tmpmat[2][1] = cob->matrix[2][1];tmpmat[2][2] = cob->matrix[2][2];
-		normalize_v3(tmpmat[0]);
-		normalize_v3(tmpmat[1]);
-		normalize_v3(tmpmat[2]);
+		copy_m3_m4(tmpmat, cob->matrix);
+		normalize_m3(tmpmat);
 		invert_m3_m3(invmat, tmpmat);
 		mul_m3_m3m3(tmpmat, totmat, invmat);
 		totmat[0][0] = tmpmat[0][0];totmat[0][1] = tmpmat[0][1];totmat[0][2] = tmpmat[0][2];
@@ -2559,9 +2500,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 						totmat[1][0],totmat[1][1],totmat[1][2],
 						totmat[2][0],totmat[2][1],totmat[2][2]);
 		if (mdet==0) {
-			totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
-			totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
-			totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
+			unit_m3(totmat);
 		}
 		
 		/* apply out transformaton to the object */
@@ -2689,7 +2628,7 @@ static void distlimit_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 			interp_v3_v3v3(dvec, ct->matrix[3], cob->matrix[3], sfac);
 			
 			/* copy new vector onto owner */
-			VECCOPY(cob->matrix[3], dvec);
+			copy_v3_v3(cob->matrix[3], dvec);
 		}
 	}
 }
@@ -2772,16 +2711,10 @@ static void stretchto_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 		mat4_to_size(size, cob->matrix);
 		
 		/* store X orientation before destroying obmat */
-		xx[0] = cob->matrix[0][0];
-		xx[1] = cob->matrix[0][1];
-		xx[2] = cob->matrix[0][2];
-		normalize_v3(xx);
+		normalize_v3_v3(xx, cob->matrix[0]);
 		
 		/* store Z orientation before destroying obmat */
-		zz[0] = cob->matrix[2][0];
-		zz[1] = cob->matrix[2][1];
-		zz[2] = cob->matrix[2][2];
-		normalize_v3(zz);
+		normalize_v3_v3(zz, cob->matrix[2]);
 		
 		sub_v3_v3v3(vec, cob->matrix[3], ct->matrix[3]);
 		vec[0] /= size[0];
@@ -2836,9 +2769,7 @@ static void stretchto_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 		normalize_v3(vec);
 		
 		/* new Y aligns  object target connection*/
-		totmat[1][0] = -vec[0];
-		totmat[1][1] = -vec[1];
-		totmat[1][2] = -vec[2];
+		negate_v3_v3(totmat[1], vec);
 		switch (data->plane) {
 		case PLANE_X:
 			/* build new Z vector */
@@ -2847,16 +2778,11 @@ static void stretchto_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 			normalize_v3(orth);
 			
 			/* new Z*/
-			totmat[2][0] = orth[0];
-			totmat[2][1] = orth[1];
-			totmat[2][2] = orth[2];
+			copy_v3_v3(totmat[2], orth);
 			
 			/* we decided to keep X plane*/
 			cross_v3_v3v3(xx, orth, vec);
-			normalize_v3(xx);
-			totmat[0][0] = xx[0];
-			totmat[0][1] = xx[1];
-			totmat[0][2] = xx[2];
+			normalize_v3_v3(totmat[0], xx);
 			break;
 		case PLANE_Z:
 			/* build new X vector */
@@ -2865,16 +2791,11 @@ static void stretchto_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 			normalize_v3(orth);
 			
 			/* new X */
-			totmat[0][0] = -orth[0];
-			totmat[0][1] = -orth[1];
-			totmat[0][2] = -orth[2];
+			negate_v3_v3(totmat[0], orth);
 			
 			/* we decided to keep Z */
 			cross_v3_v3v3(zz, orth, vec);
-			normalize_v3(zz);
-			totmat[2][0] = zz[0];
-			totmat[2][1] = zz[1];
-			totmat[2][2] = zz[2];
+			normalize_v3_v3(totmat[2], zz);
 			break;
 		} /* switch (data->plane) */
 		
@@ -3006,10 +2927,10 @@ static void minmax_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *tar
 			obmat[3][index] = tarmat[3][index] + data->offset;
 			if (data->flag & MINMAX_STICKY) {
 				if (data->flag & MINMAX_STUCK) {
-					VECCOPY(obmat[3], data->cache);
+					copy_v3_v3(obmat[3], data->cache);
 				} 
 				else {
-					VECCOPY(data->cache, obmat[3]);
+					copy_v3_v3(data->cache, obmat[3]);
 					data->flag |= MINMAX_STUCK;
 				}
 			}
@@ -3019,7 +2940,7 @@ static void minmax_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *tar
 				copy_m4_m4(cob->matrix, tmat);
 			} 
 			else {			
-				VECCOPY(cob->matrix[3], obmat[3]);
+				copy_v3_v3(cob->matrix[3], obmat[3]);
 			}
 		} 
 		else {
@@ -3174,7 +3095,7 @@ static void clampto_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *ta
 		
 		copy_m4_m4(obmat, cob->matrix);
 		unit_m4(targetMatrix);
-		VECCOPY(ownLoc, obmat[3]);
+		copy_v3_v3(ownLoc, obmat[3]);
 		
 		INIT_MINMAX(curveMin, curveMax)
 		minmax_object(ct->tar, curveMin, curveMax);
@@ -3263,14 +3184,14 @@ static void clampto_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *ta
 			/* 3. position on curve */
 			if (where_on_path(ct->tar, curvetime, vec, dir, NULL, NULL, NULL) ) {
 				unit_m4(totmat);
-				VECCOPY(totmat[3], vec);
+				copy_v3_v3(totmat[3], vec);
 				
 				mul_serie_m4(targetMatrix, ct->tar->obmat, totmat, NULL, NULL, NULL, NULL, NULL, NULL);
 			}
 		}
 		
 		/* obtain final object position */
-		VECCOPY(cob->matrix[3], targetMatrix[3]);
+		copy_v3_v3(cob->matrix[3], targetMatrix[3]);
 	}
 }
 
@@ -3362,7 +3283,7 @@ static void transform_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 		}
 		
 		/* extract components of owner's matrix */
-		VECCOPY(loc, cob->matrix[3]);
+		copy_v3_v3(loc, cob->matrix[3]);
 		mat4_to_eulO(eul, cob->rotOrder, cob->matrix);
 		mat4_to_size(size, cob->matrix);	
 		
@@ -3556,7 +3477,7 @@ static void shrinkwrap_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstr
 						fail = TRUE;
 						break;
 					}
-					VECCOPY(co, hit.co);
+					copy_v3_v3(co, hit.co);
 				break;
 			}
 			
@@ -3572,7 +3493,7 @@ static void shrinkwrap_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstr
 			
 			/* co is in local object coordinates, change it to global and update target position */
 			mul_m4_v3(cob->matrix, co);
-			VECCOPY(ct->matrix[3], co);
+			copy_v3_v3(ct->matrix[3], co);
 		}
 	}
 }
@@ -3584,7 +3505,7 @@ static void shrinkwrap_evaluate (bConstraint *con, bConstraintOb *cob, ListBase
 	/* only evaluate if there is a target */
 	if (VALID_CONS_TARGET(ct))
 	{
-		VECCOPY(cob->matrix[3], ct->matrix[3]);
+		copy_v3_v3(cob->matrix[3], ct->matrix[3]);
 	}
 }
 
@@ -3668,23 +3589,23 @@ static void damptrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 		 *	- the normalisation step at the end should take care of any unwanted scaling
 		 *	  left over in the 3x3 matrix we used
 		 */
-		VECCOPY(obvec, track_dir_vecs[data->trackflag]);
+		copy_v3_v3(obvec, track_dir_vecs[data->trackflag]);
 		mul_mat3_m4_v3(cob->matrix, obvec);
 		
 		if (normalize_v3(obvec) == 0.0f) {
 			/* exceptional case - just use the track vector as appropriate */
-			VECCOPY(obvec, track_dir_vecs[data->trackflag]);
+			copy_v3_v3(obvec, track_dir_vecs[data->trackflag]);
 		}
 		
 		/* find the (unit) direction vector going from the owner to the target */
-		VECCOPY(obloc, cob->matrix[3]);
+		copy_v3_v3(obloc, cob->matrix[3]);
 		sub_v3_v3v3(tarvec, ct->matrix[3], obloc);
 		
 		if (normalize_v3(tarvec) == 0.0f) {
 			/* the target is sitting on the owner, so just make them use the same direction vectors */
 			// FIXME: or would it be better to use the pure direction vector?
-			VECCOPY(tarvec, obvec);
-			//VECCOPY(tarvec, track_dir_vecs[data->trackflag]);
+			copy_v3_v3(tarvec, obvec);
+			//copy_v3_v3(tarvec, track_dir_vecs[data->trackflag]);
 		}
 		
 		/* determine the axis-angle rotation, which represents the smallest possible rotation
@@ -3712,7 +3633,7 @@ static void damptrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
 		mul_m4_m3m4(tmat, rmat, cob->matrix); // m1, m3, m2
 		
 		copy_m4_m4(cob->matrix, tmat);
-		VECCOPY(cob->matrix[3], obloc);
+		copy_v3_v3(cob->matrix[3], obloc);
 	}
 }
 
@@ -3907,7 +3828,7 @@ static void pivotcon_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *t
 		}
 		else {
 			/* directly use the 'offset' specified as an absolute position instead */
-			VECCOPY(pivot, data->offset);
+			copy_v3_v3(pivot, data->offset);
 		}
 	}
 	
diff --git a/source/blender/blenkernel/intern/effect.c b/source/blender/blenkernel/intern/effect.c
index 5f55814341b..6f6d405dd90 100644
--- a/source/blender/blenkernel/intern/effect.c
+++ b/source/blender/blenkernel/intern/effect.c
@@ -662,8 +662,7 @@ int get_effector_data(EffectorCache *eff, EffectorData *efd, EffectedPoint *poin
 		where_is_object_time(eff->scene, ob, cfra);
 
 		/* use z-axis as normal*/
-		VECCOPY(efd->nor, ob->obmat[2]);
-		normalize_v3(efd->nor);
+		normalize_v3_v3(efd->nor, ob->obmat[2]);
 
 		/* for vortex the shape chooses between old / new force */
 		if(eff->pd && eff->pd->shape == PFIELD_SHAPE_PLANE) {
@@ -707,8 +706,7 @@ int get_effector_data(EffectorCache *eff, EffectorData *efd, EffectedPoint *poin
 		else {
 			/* for some effectors we need the object center every time */
 			sub_v3_v3v3(efd->vec_to_point2, point->loc, eff->ob->obmat[3]);
-			VECCOPY(efd->nor2, eff->ob->obmat[2]);
-			normalize_v3(efd->nor2);
+			normalize_v3_v3(efd->nor2, eff->ob->obmat[2]);
 		}
 	}
 
diff --git a/source/blender/blenkernel/intern/implicit.c b/source/blender/blenkernel/intern/implicit.c
index d544174b0d4..158f964a846 100644
--- a/source/blender/blenkernel/intern/implicit.c
+++ b/source/blender/blenkernel/intern/implicit.c
@@ -1626,9 +1626,8 @@ static void cloth_calc_force(ClothModifierData *clmd, float frame, lfVector *lF,
 				CalcFloat4(lX[mfaces[i].v1],lX[mfaces[i].v2],lX[mfaces[i].v3],lX[mfaces[i].v4],triunnormal);
 			else
 				CalcFloat(lX[mfaces[i].v1],lX[mfaces[i].v2],lX[mfaces[i].v3],triunnormal);
-			
-			VECCOPY(trinormal, triunnormal);
-			normalize_v3(trinormal);
+
+			normalize_v3_v3(trinormal, triunnormal);
 			
 			// add wind from v1
 			VECCOPY(tmp, trinormal);
diff --git a/source/blender/blenkernel/intern/mesh.c b/source/blender/blenkernel/intern/mesh.c
index a017b7344d7..94131fdbe9d 100644
--- a/source/blender/blenkernel/intern/mesh.c
+++ b/source/blender/blenkernel/intern/mesh.c
@@ -1236,14 +1236,10 @@ void mesh_calc_normals(MVert *mverts, int numVerts, MFace *mfaces, int numFaces,
 		MVert *mv= &mverts[i];
 		float *no= tnorms[i];
 		
-		if (normalize_v3(no)==0.0) {
-			VECCOPY(no, mv->co);
-			normalize_v3(no);
-		}
+		if (normalize_v3(no)==0.0)
+			normalize_v3_v3(no, mv->co);
 
-		mv->no[0]= (short)(no[0]*32767.0);
-		mv->no[1]= (short)(no[1]*32767.0);
-		mv->no[2]= (short)(no[2]*32767.0);
+		normal_float_to_short_v3(mv->no, no);
 	}
 	
 	MEM_freeN(tnorms);
diff --git a/source/blender/blenkernel/intern/particle.c b/source/blender/blenkernel/intern/particle.c
index 7b3638f0e33..681c48b0cf8 100644
--- a/source/blender/blenkernel/intern/particle.c
+++ b/source/blender/blenkernel/intern/particle.c
@@ -624,8 +624,7 @@ static float psys_render_projected_area(ParticleSystem *psys, float *center, flo
 	mul_m4_v4(data->viewmat, co);
 	
 	/* compute two vectors orthogonal to view vector */
-	VECCOPY(view, co);
-	normalize_v3(view);
+	normalize_v3_v3(view, co);
 	ortho_basis_v3v3_v3( ortho1, ortho2,view);
 
 	/* compute on screen minification */
@@ -1923,8 +1922,7 @@ static void do_prekink(ParticleKey *state, ParticleKey *par, float *par_rot, flo
 				mul_qt_v3(q2,z_vec);
 				
 				VECSUB(vec_from_par,state->co,par->co);
-				VECCOPY(vec_one,vec_from_par);
-				radius=normalize_v3(vec_one);
+				radius= normalize_v3_v3(vec_one, vec_from_par);
 
 				inp_y=dot_v3v3(y_vec,vec_one);
 				inp_z=dot_v3v3(z_vec,vec_one);
@@ -2929,8 +2927,7 @@ void psys_cache_paths(ParticleSimulationData *sim, float cfra)
 			if(k == 1) {
 				/* calculate initial tangent for incremental rotations */
 				VECSUB(tangent, ca->co, (ca - 1)->co);
-				VECCOPY(prev_tangent, tangent);
-				normalize_v3(prev_tangent);
+				normalize_v3_v3(prev_tangent, tangent);
 
 				/* First rotation is based on emitting face orientation.		*/
 				/* This is way better than having flipping rotations resulting	*/
@@ -3106,8 +3103,7 @@ void psys_cache_edit_paths(Scene *scene, Object *ob, PTCacheEdit *edit, float cf
 					if(k == 1) {
 						/* calculate initial tangent for incremental rotations */
 						VECSUB(tangent, ca->co, (ca - 1)->co);
-						VECCOPY(prev_tangent, tangent);
-						normalize_v3(prev_tangent);
+						normalize_v3_v3(prev_tangent, tangent);
 
 						/* First rotation is based on emitting face orientation.		*/
 						/* This is way better than having flipping rotations resulting	*/
@@ -4371,20 +4367,14 @@ void psys_make_billboard(ParticleBillboardData *bb, float xvec[3], float yvec[3]
 		onevec[bb->align]=1.0f;
 
 	if(bb->lock && (bb->align == PART_BB_VIEW)) {
-		VECCOPY(xvec, bb->ob->obmat[0]);
-		normalize_v3(xvec);
-
-		VECCOPY(yvec, bb->ob->obmat[1]);
-		normalize_v3(yvec);
-
-		VECCOPY(zvec, bb->ob->obmat[2]);
-		normalize_v3(zvec);
+		normalize_v3_v3(xvec, bb->ob->obmat[0]);
+		normalize_v3_v3(yvec, bb->ob->obmat[1]);
+		normalize_v3_v3(zvec, bb->ob->obmat[2]);
 	}
 	else if(bb->align == PART_BB_VEL) {
 		float temp[3];
 
-		VECCOPY(temp, bb->vel);
-		normalize_v3(temp);
+		normalize_v3_v3(temp, bb->vel);
 
 		VECSUB(zvec, bb->ob->obmat[3], bb->vec);
 
diff --git a/source/blender/blenkernel/intern/particle_system.c b/source/blender/blenkernel/intern/particle_system.c
index 9b9c3ff16b6..cfbab609f37 100644
--- a/source/blender/blenkernel/intern/particle_system.c
+++ b/source/blender/blenkernel/intern/particle_system.c
@@ -1729,8 +1729,7 @@ void reset_particle(ParticleSimulationData *sim, ParticleData *pa, float dtime,
 		mul_qt_v3(rot, vtan);
 		mul_qt_v3(rot, utan);
 
-		VECCOPY(p_vel, state.vel);
-		speed=normalize_v3(p_vel);
+		speed= normalize_v3_v3(p_vel, state.vel);
 		mul_v3_fl(p_vel, dot_v3v3(r_vel, p_vel));
 		VECSUB(p_vel, r_vel, p_vel);
 		normalize_v3(p_vel);
@@ -1871,18 +1870,15 @@ void reset_particle(ParticleSimulationData *sim, ParticleData *pa, float dtime,
 
 		/*		*emitter object orientation		*/
 		if(part->ob_vel[0]!=0.0) {
-			VECCOPY(vec, ob->obmat[0]);
-			normalize_v3(vec);
+			normalize_v3_v3(vec, ob->obmat[0]);
 			VECADDFAC(vel, vel, vec, part->ob_vel[0]);
 		}
 		if(part->ob_vel[1]!=0.0) {
-			VECCOPY(vec, ob->obmat[1]);
-			normalize_v3(vec);
+			normalize_v3_v3(vec, ob->obmat[1]);
 			VECADDFAC(vel, vel, vec, part->ob_vel[1]);
 		}
 		if(part->ob_vel[2]!=0.0) {
-			VECCOPY(vec, ob->obmat[2]);
-			normalize_v3(vec);
+			normalize_v3_v3(vec, ob->obmat[2]);
 			VECADDFAC(vel, vel, vec, part->ob_vel[2]);
 		}
 
diff --git a/source/blender/blenkernel/intern/sketch.c b/source/blender/blenkernel/intern/sketch.c
index 33871f78864..7e39cdd1196 100644
--- a/source/blender/blenkernel/intern/sketch.c
+++ b/source/blender/blenkernel/intern/sketch.c
@@ -72,8 +72,7 @@ void sk_initPoint(SK_Point *pt, SK_DrawData *dd, float *no)
 {
 	if (no)
 	{
-		VECCOPY(pt->no, no);
-		normalize_v3(pt->no);
+		normalize_v3_v3(pt->no, no);
 	}
 	else
 	{
diff --git a/source/blender/blenkernel/intern/softbody.c b/source/blender/blenkernel/intern/softbody.c
index b8e824ce3d5..98a50eee146 100644
--- a/source/blender/blenkernel/intern/softbody.c
+++ b/source/blender/blenkernel/intern/softbody.c
@@ -2032,8 +2032,7 @@ static int sb_detect_vertex_collisionCached(float opco[3], float facenormal[3],
 	VECCOPY(vel,avel);
 	if (ci) *intrusion /= ci;
 	if (deflected){
-		VECCOPY(facenormal,force);
-		normalize_v3(facenormal);
+		normalize_v3_v3(facenormal, force);
 	}
 	return deflected;
 }
diff --git a/source/blender/blenlib/intern/math_geom.c b/source/blender/blenlib/intern/math_geom.c
index fd0829ebe3b..68b1feea632 100644
--- a/source/blender/blenlib/intern/math_geom.c
+++ b/source/blender/blenlib/intern/math_geom.c
@@ -841,10 +841,8 @@ int isect_line_line_v3(float v1[3], float v2[3], float v3[3], float v4[3], float
 	sub_v3_v3v3(a, v2, v1);
 	sub_v3_v3v3(b, v4, v3);
 
-	copy_v3_v3(dir1, a);
-	normalize_v3(dir1);
-	copy_v3_v3(dir2, b);
-	normalize_v3(dir2);
+	normalize_v3_v3(dir1, a);
+	normalize_v3_v3(dir2, b);
 	d = dot_v3v3(dir1, dir2);
 	if (d == 1.0f || d == -1.0f) {
 		/* colinear */
@@ -908,10 +906,8 @@ int isect_line_line_strict_v3(float v1[3], float v2[3], float v3[3], float v4[3]
 	sub_v3_v3v3(a, v2, v1);
 	sub_v3_v3v3(b, v4, v3);
 
-	copy_v3_v3(dir1, a);
-	normalize_v3(dir1);
-	copy_v3_v3(dir2, b);
-	normalize_v3(dir2);
+	normalize_v3_v3(dir1, a);
+	normalize_v3_v3(dir2, b);
 	d = dot_v3v3(dir1, dir2);
 	if (d == 1.0f || d == -1.0f || d == 0) {
 		/* colinear or one vector is zero-length*/
diff --git a/source/blender/blenlib/intern/math_matrix.c b/source/blender/blenlib/intern/math_matrix.c
index 6c06da2e32d..64c3e746982 100644
--- a/source/blender/blenlib/intern/math_matrix.c
+++ b/source/blender/blenlib/intern/math_matrix.c
@@ -595,9 +595,7 @@ void transpose_m4(float mat[][4])
 void orthogonalize_m3(float mat[][3], int axis)
 {
 	float size[3];
-	size[0] = len_v3(mat[0]);
-	size[1] = len_v3(mat[1]);
-	size[2] = len_v3(mat[2]);
+	mat3_to_size(size, mat);
 	normalize_v3(mat[axis]);
 	switch(axis)
 	{
@@ -658,9 +656,7 @@ void orthogonalize_m3(float mat[][3], int axis)
 void orthogonalize_m4(float mat[][4], int axis)
 {
 	float size[3];
-	size[0] = len_v3(mat[0]);
-	size[1] = len_v3(mat[1]);
-	size[2] = len_v3(mat[2]);
+	mat4_to_size(size, mat);
 	normalize_v3(mat[axis]);
 	switch(axis)
 	{
diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c
index 4d015e527a8..f72269f6d7b 100644
--- a/source/blender/blenlib/intern/math_rotation.c
+++ b/source/blender/blenlib/intern/math_rotation.c
@@ -135,10 +135,7 @@ void mul_fac_qt_fl(float *q, const float fac)
 	float si= (float)sin(angle);
 	q[0]= co;
 	normalize_v3(q+1);
-	q[1]*= si;
-	q[2]*= si;
-	q[3]*= si;
-	
+	mul_v3_fl(q+1, si);
 }
 
 void quat_to_mat3(float m[][3], float *q)
@@ -595,9 +592,8 @@ void axis_angle_to_quat(float q[4], float axis[3], float angle)
 {
 	float nor[3];
 	float si;
-	
-	copy_v3_v3(nor, axis);
-	normalize_v3(nor);
+
+	normalize_v3_v3(nor, axis);
 	
 	angle /= 2;
 	si = (float)sin(angle);
@@ -654,8 +650,7 @@ void axis_angle_to_mat3(float mat[3][3],float axis[3], float angle)
 	float nor[3], nsi[3], co, si, ico;
 	
 	/* normalise the axis first (to remove unwanted scaling) */
-	copy_v3_v3(nor, axis);
-	normalize_v3(nor);
+	normalize_v3_v3(nor, axis);
 	
 	/* now convert this to a 3x3 matrix */
 	co= (float)cos(angle);		
diff --git a/source/blender/blenlib/intern/math_vector.c b/source/blender/blenlib/intern/math_vector.c
index 9baf897c830..b1cea9ab3c4 100644
--- a/source/blender/blenlib/intern/math_vector.c
+++ b/source/blender/blenlib/intern/math_vector.c
@@ -120,10 +120,8 @@ float angle_v3v3(float *v1, float *v2)
 {
 	float vec1[3], vec2[3];
 
-	copy_v3_v3(vec1, v1);
-	copy_v3_v3(vec2, v2);
-	normalize_v3(vec1);
-	normalize_v3(vec2);
+	normalize_v3_v3(vec1, v1);
+	normalize_v3_v3(vec2, v2);
 
 	return angle_normalized_v3v3(vec1, vec2);
 }