public PVector rotateAroundArbitraryAxis(PVector offsetParam,PVector controlParam,PVector axisParam,float angle) { PVector control = controlParam.get(); PVector axis = axisParam.get(); PVector offset = offsetParam.get(); control.sub(offset); axis.sub(offset); axis.normalize(axis); float rads = PI/180 * angle; float sinTheta = sin(rads*.5); float cosTheta = cos(rads*.5); Quaternion rotQuat = new Quaternion( cosTheta, sinTheta * axis.x, sinTheta * axis.y, sinTheta * axis.z ); Quaternion pointQuat = new Quaternion( 0, control.x, control.y, control.z ); Quaternion conjQuat = new Quaternion( rotQuat.a, -rotQuat.b, -rotQuat.c, -rotQuat.d ); Quaternion temp = quat_mult(rotQuat,pointQuat); PVector result = quat_pointmult(temp,conjQuat); result.add(offset); return result; } public PVector rotateAroundZAxis(PVector offset, PVector controlParam, float angle) { PVector control = controlParam.get(); control.sub(offset); float rads = PI/180 * angle; float cosTheta = cos(rads); float sinTheta = sin(rads); float x = cosTheta*control.x + sinTheta*control.y; float y = -sinTheta*control.x + cosTheta*control.y; float z = control.z; PVector result = new PVector(x,y,z); result.add(offset); return result; } public PVector rotateAroundYAxis(PVector offset, PVector controlParam, float angle) { PVector control = controlParam.get(); control.sub(offset); float rads = PI/180 * angle; float cosTheta = cos(rads); float sinTheta = sin(rads); float x = cosTheta*control.x - sinTheta*control.z; float y = control.y; float z = sinTheta*control.x + cosTheta*control.z; PVector result = new PVector(x,y,z); result.add(offset); return result; } public PVector rotateAroundXAxis(PVector offset, PVector controlParam, float angle) { PVector control = controlParam.get(); control.sub(offset); float rads = PI/180 * angle; float cosTheta = cos(rads); float sinTheta = sin(rads); float x = control.x; float y = cosTheta*control.y + sinTheta*control.z; float z = -sinTheta*control.y + cosTheta*control.z; PVector result = new PVector(x,y,z); result.add(offset); return result; }