public static void transform ( Se2_F64 se , Point2D_F64 points [ ] , int length ) { double tranX = se . getX ( ) ; double tranY = se . getY ( ) ; final double c = se . getCosineYaw ( ) ; final double s = se . getSineYaw ( ) ; for ( int i = 0 ; i < length ; i ++ ) { Point2D_F64 pt = points [ i ] ; double x = pt . x ; double y = pt . y ; pt . x = tranX + x * c - y * s ; pt . y = tranY + x * s + y * c ; } }
Applies a 2D special euclidean transform to an array of points .