public static Se3_F64 axisXyz ( double dx , double dy , double dz , double rotX , double rotY , double rotZ , Se3_F64 se ) { if ( se == null ) se = new Se3_F64 ( ) ; double theta = Math . sqrt ( rotX * rotX + rotY + rotY + rotZ * rotZ ) ; if ( theta == 0 ) { CommonOps_DDRM . setIdentity ( se . R ) ; } else { ConvertRotation3D_F64 . rodriguesToMatrix ( rotX / theta , rotY / theta , rotZ / theta , theta , se . getR ( ) ) ; } Vector3D_F64 T = se . getT ( ) ; T . x = dx ; T . y = dy ; T . z = dz ; return se ; }
Create SE3 using axis - angle for rotation and XYZ tanslation