You could probably make this a bit faster with blendvpd on processors with SSE4.  Not necessary, just an idea.